rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Flight/Actuator Go to failsafe actuator outputs when a command is not received

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@763 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-06-14 02:00:16 +00:00 committed by vassilis
parent 0731747d84
commit 106bdca20e
2 changed files with 43 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/OpenPilot/Makefile
View File

@ -45,9 +45,9 @@ FLASH_TOOL = OPENOCD
USE_THUMB_MODE = YES USE_THUMB_MODE = YES
# List of modules to include # List of modules to include
#MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude
#MODULES = Telemetry Example #MODULES = Telemetry Example
MODULES = Telemetry MK/MKSerial #MODULES = Telemetry MK/MKSerial
# MCU name, submodel and board # MCU name, submodel and board

43
flight/OpenPilot/Modules/Actuator/actuator.c
View File

@ -35,6 +35,7 @@
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
#define STACK_SIZE configMINIMAL_STACK_SIZE #define STACK_SIZE configMINIMAL_STACK_SIZE
#define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define FAILSAFE_TIMEOUT_MS 100
// Private types // Private types
@ -48,6 +49,7 @@ static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const Actuat
static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd); static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd); static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
static void setFailsafe();
/** /**
* Module initialization * Module initialization
@ -81,11 +83,18 @@ static void actuatorTask(void* parameters)
ActuatorSettingsGet(&settings); ActuatorSettingsGet(&settings);
PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
setFailsafe();
// Main task loop // Main task loop
while (1) while (1)
{ {
// Wait until the ActuatorDesired object is updated // Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
while ( xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE ); if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
setFailsafe();
continue;
}
// Read settings // Read settings
ActuatorSettingsGet(&settings); ActuatorSettingsGet(&settings);
@ -236,3 +245,33 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
} }
return valueScaled; return valueScaled;
} }
/**
* Set actuator output to the neutral values (failsafe)
*/
static void setFailsafe()
{
ActuatorSettingsData settings;
ActuatorCommandData cmd;
// Read settings
ActuatorSettingsGet(&settings);
// Reset ActuatorCommand to neutral values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
cmd.Channel[n] = settings.ChannelNeutral[n];
}
// Set alarm
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
// Update servo outputs
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
PIOS_Servo_Set( n+1, cmd.Channel[n] );
}
// Update output object
ActuatorCommandSet(&cmd);
}