1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00

Merge branch 'corvuscorax/OP699' into revo-fixes

This commit is contained in:
Corvus Corax 2012-11-22 10:57:11 +01:00
commit 34210ef9ae
8 changed files with 100 additions and 317 deletions

View File

@ -556,6 +556,95 @@ static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const Mix
ActuatorCommandChannelSet(Channel);
}
/**
* determine buzzer or blink sequence
**/
typedef enum {BUZZ_BUZZER=0,BUZZ_ARMING=1,BUZZ_INFO=2,BUZZ_MAX=3} buzzertype;
static inline bool buzzerState(buzzertype type)
{
// This is for buzzers that take a PWM input
static uint32_t tune[BUZZ_MAX]={0};
static uint32_t tunestate[BUZZ_MAX]={0};
uint32_t newTune = 0;
if(type==BUZZ_BUZZER)
{
// Decide what tune to play
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
newTune = 0b11110110110000; // pause, short, short, short, long
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
newTune = 0x80000000; // pause, short
} else {
newTune = 0;
}
} else { // BUZZ_ARMING || BUZZ_INFO
uint8_t arming;
FlightStatusArmedGet(&arming);
//base idle tune
newTune = 0x80000000; // 0b1000...
// Merge the error pattern for InfoLed
if(type==BUZZ_INFO)
{
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000001111111011111110000000;
}
else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000000000110110110000000000;
}
}
// fast double blink pattern if armed
if (arming == FLIGHTSTATUS_ARMED_ARMED)
newTune |= 0xA0000000; // 0b101000...
}
// Do we need to change tune?
if (newTune != tune[type]) {
tune[type] = newTune;
// resynchronize all tunes on change, so they stay in sync
for (int i=0;i<BUZZ_MAX;i++) {
tunestate[i] = tune[i];
}
}
// Play tune
bool buzzOn = false;
static portTickType lastSysTime = 0;
portTickType thisSysTime = xTaskGetTickCount();
portTickType dT = 0;
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
if (tune[type]) {
if(thisSysTime > lastSysTime) {
dT = thisSysTime - lastSysTime;
} else {
lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80
}
buzzOn = (tunestate[type]&1);
if (dT > 80) {
// Go to next bit in alarm_seq_state
for (int i=0;i<BUZZ_MAX;i++) {
tunestate[i] >>=1;
if (tunestate[i]==0) // All done, re-start the tune
tunestate[i]=tune[i];
}
lastSysTime = thisSysTime;
}
}
return buzzOn;
}
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings)
{
@ -566,118 +655,17 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
{
switch(actuatorSettings->ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
{
// This is for buzzers that take a PWM input
static uint32_t currBuzzTune = 0;
static uint32_t currBuzzTuneState;
static uint32_t currArmingTune = 0;
static uint32_t currArmingTuneState;
static uint32_t currInfoTune = 0;
static uint32_t currInfoTuneState;
uint32_t newTune = 0;
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
{
// Decide what tune to play
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
newTune = 0b11110110110000; // pause, short, short, short, long
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
newTune = 0x80000000; // pause, short
} else {
newTune = 0;
}
// Do we need to change tune?
if (newTune != currBuzzTune) {
currBuzzTune = newTune;
currBuzzTuneState = currBuzzTune;
}
}
else // ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED || ACTUATORSETTINGS_CHANNELTYPE_INFOLED
{
uint8_t arming;
FlightStatusArmedGet(&arming);
//base idle tune
newTune = 0x80000000; // 0b1000...
// Merge the error pattern for InfoLed
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
{
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000001111111011111110000000;
}
else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000000000110110110000000000;
}
}
// fast double blink pattern if armed
if (arming == FLIGHTSTATUS_ARMED_ARMED)
newTune |= 0xA0000000; // 0b101000...
// Do we need to change tune?
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
{
if (newTune != currArmingTune) {
currArmingTune = newTune;
// note: those are both updated so that Info and Arming are in sync if used simultaneously
currArmingTuneState = currArmingTune;
currInfoTuneState = currInfoTune;
}
}
else
{
if (newTune != currInfoTune) {
currInfoTune = newTune;
currArmingTuneState = currArmingTune;
currInfoTuneState = currInfoTune;
}
}
}
// Play tune
bool buzzOn = false;
static portTickType lastSysTime = 0;
portTickType thisSysTime = xTaskGetTickCount();
portTickType dT = 0;
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
if (currBuzzTune||currArmingTune||currInfoTune) {
if(thisSysTime > lastSysTime)
dT = thisSysTime - lastSysTime;
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
buzzOn = (currBuzzTuneState&1); // Buzz when the LS bit is 1
else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
buzzOn = (currArmingTuneState&1);
else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
buzzOn = (currInfoTuneState&1);
if (dT > 80) {
// Go to next bit in alarm_seq_state
currArmingTuneState >>=1;
currInfoTuneState >>= 1;
currBuzzTuneState >>= 1;
if (currBuzzTuneState == 0)
currBuzzTuneState = currBuzzTune; // All done, re-start the tune
if (currArmingTuneState == 0)
currArmingTuneState = currArmingTune;
if (currInfoTuneState == 0)
currInfoTuneState = currInfoTune;
lastSysTime = thisSysTime;
}
}
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzOn?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
buzzerState(BUZZ_BUZZER)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_ARMING)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_INFO)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
return true;
}
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
return true;
@ -686,7 +674,6 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value);
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value);
break;
#endif
default:
return false;

View File

@ -384,7 +384,6 @@ static uint8_t updateFixedDesiredAttitude()
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
VelocityActualGet(&velocityActual);
// VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);

View File

@ -37,7 +37,6 @@
#include "pathaction.h"
#include "pathdesired.h"
#include "pathstatus.h"
#include "pathplannersettings.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "waypoint.h"
@ -49,17 +48,14 @@
#define MAX_QUEUE_SIZE 2
#define F_PI 3.141526535897932f
#define RAD2DEG (180.0f/F_PI)
#define PATH_PLANNER_UPDATE_RATE_MS 20
// Private types
// Private functions
static void pathPlannerTask(void *parameters);
static void settingsUpdated(UAVObjEvent * ev);
static void updatePathDesired(UAVObjEvent * ev);
static void setWaypoint(uint16_t num);
static void WaypointCreateInstances(uint16_t num);
static void createPathBox();
static void createPathLogo();
static uint8_t pathConditionCheck();
static uint8_t conditionNone();
@ -76,8 +72,6 @@ static uint8_t conditionImmediate();
// Private variables
static xTaskHandle taskHandle;
static xQueueHandle queue;
static PathPlannerSettingsData pathPlannerSettings;
static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
@ -105,7 +99,6 @@ int32_t PathPlannerInitialize()
{
taskHandle = NULL;
PathPlannerSettingsInitialize();
PathActionInitialize();
PathStatusInitialize();
PathDesiredInitialize();
@ -115,9 +108,6 @@ int32_t PathPlannerInitialize()
WaypointInitialize();
WaypointActiveInitialize();
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
return 0;
}
@ -128,10 +118,6 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
*/
static void pathPlannerTask(void *parameters)
{
// update settings on change
PathPlannerSettingsConnectCallback(settingsUpdated);
settingsUpdated(PathPlannerSettingsHandle());
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
WaypointActiveConnectCallback(updatePathDesired);
@ -146,7 +132,7 @@ static void pathPlannerTask(void *parameters)
while (1)
{
vTaskDelay(20);
vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
@ -221,25 +207,6 @@ static void pathPlannerTask(void *parameters)
}
}
// standard settings updated callback
void settingsUpdated(UAVObjEvent * ev) {
uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath;
PathPlannerSettingsGet(&pathPlannerSettings);
if (pathPlannerSettings.PreprogrammedPath != preprogrammedPath) {
switch(pathPlannerSettings.PreprogrammedPath) {
case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_NONE:
break;
case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_10M_BOX:
createPathBox();
break;
case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_LOGO:
createPathLogo();
break;
}
}
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(UAVObjEvent * ev) {
@ -303,17 +270,6 @@ static void setWaypoint(uint16_t num) {
}
// helper function to make sure there are enough waypoint instances
static void WaypointCreateInstances(uint16_t num) {
uint16_t t;
for (t=UAVObjGetNumInstances(WaypointHandle());t<num;t++) {
WaypointCreateInstance();
}
}
// execute the apropriate condition and report result
static uint8_t pathConditionCheck() {
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
@ -544,150 +500,6 @@ static uint8_t conditionImmediate() {
return true;
}
// demo path - box
static void createPathBox()
{
uint16_t t;
for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
PathActionCreateInstance();
}
PathActionData action;
PathActionInstGet(0,&action);
action.Mode = PATHACTION_MODE_FLYVECTOR;
action.ModeParameters[0]=0;
action.ModeParameters[1]=0;
action.ModeParameters[2]=0;
action.ModeParameters[3]=0;
action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
action.ConditionParameters[0] = 0;
action.ConditionParameters[1] = 0;
action.ConditionParameters[2] = 0;
action.ConditionParameters[3] = 0;
action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
action.JumpDestination = 0;
action.ErrorDestination = 0;
PathActionInstSet(0,&action);
PathActionInstSet(1,&action);
PathActionInstSet(2,&action);
WaypointCreateInstances(6);
// Draw O
WaypointData waypoint;
waypoint.Action = 0;
waypoint.Velocity = 2;
waypoint.Position[0] = 5;
waypoint.Position[1] = 5;
waypoint.Position[2] = -10;
WaypointInstSet(0, &waypoint);
waypoint.Position[0] = -5;
waypoint.Position[1] = 5;
WaypointInstSet(1, &waypoint);
waypoint.Position[0] = -5;
waypoint.Position[1] = -5;
WaypointInstSet(2, &waypoint);
waypoint.Position[0] = 5;
waypoint.Position[1] = -5;
WaypointInstSet(3, &waypoint);
waypoint.Position[0] = 5;
waypoint.Position[1] = 5;
WaypointInstSet(4, &waypoint);
waypoint.Position[0] = 0;
waypoint.Position[1] = 0;
WaypointInstSet(5, &waypoint);
}
// demo path - logo
static void createPathLogo()
{
#define SIZE 10.0f
PathActionData action;
PathActionInstGet(0,&action);
action.Mode = PATHACTION_MODE_FLYVECTOR;
action.ModeParameters[0]=0;
action.ModeParameters[1]=0;
action.ModeParameters[2]=0;
action.ModeParameters[3]=0;
action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
action.ConditionParameters[0] = 0;
action.ConditionParameters[1] = 0;
action.ConditionParameters[2] = 0;
action.ConditionParameters[3] = 0;
action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
action.JumpDestination = 0;
action.ErrorDestination = 0;
PathActionInstSet(0,&action);
uint16_t t;
for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
PathActionCreateInstance();
}
WaypointCreateInstances(42);
// Draw O
WaypointData waypoint;
waypoint.Velocity = 2; // Since for now this isn't directional just set a mag
waypoint.Action = 0;
for(uint32_t i = 0; i < 20; i++) {
waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50;
WaypointInstSet(i, &waypoint);
}
// Draw P
for(uint32_t i = 20; i < 35; i++) {
waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[2] = -50;
WaypointInstSet(i, &waypoint);
}
waypoint.Position[1] = SIZE * 35;
waypoint.Position[0] = SIZE * -50;
waypoint.Position[2] = -50;
WaypointInstSet(35, &waypoint);
// Draw Box
waypoint.Position[1] = SIZE * 35;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
WaypointInstSet(36, &waypoint);
waypoint.Position[1] = SIZE * 85;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
WaypointInstSet(37, &waypoint);
waypoint.Position[1] = SIZE * 85;
waypoint.Position[0] = SIZE * 60;
waypoint.Position[2] = -30;
WaypointInstSet(38, &waypoint);
waypoint.Position[1] = SIZE * -40;
waypoint.Position[0] = SIZE * 60;
waypoint.Position[2] = -30;
WaypointInstSet(39, &waypoint);
waypoint.Position[1] = SIZE * -40;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
WaypointInstSet(40, &waypoint);
waypoint.Position[1] = SIZE * 35;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
WaypointInstSet(41, &waypoint);
}
/**
* @}
* @}

View File

@ -69,7 +69,6 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired

View File

@ -69,7 +69,6 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired

View File

@ -66,7 +66,6 @@ UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired

View File

@ -61,7 +61,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/gpssatellites.h \
$$UAVOBJECT_SYNTHETICS/pathaction.h \
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
$$UAVOBJECT_SYNTHETICS/pathplannersettings.h \
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
$$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
$$UAVOBJECT_SYNTHETICS/positionactual.h \
@ -141,7 +140,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
$$UAVOBJECT_SYNTHETICS/pathaction.cpp \
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
$$UAVOBJECT_SYNTHETICS/pathplannersettings.cpp \
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
$$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
$$UAVOBJECT_SYNTHETICS/positionactual.cpp \

View File

@ -1,10 +0,0 @@
<xml>
<object name="PathPlannerSettings" singleinstance="true" settings="true">
<description>Settings for the @ref PathPlanner Module</description>
<field name="PreprogrammedPath" units="" type="enum" elements="1" options="NONE,10M_BOX,LOGO" defaultvalue="NONE"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>