rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

OP-999 trigger a failsafe condition in AH module when throttle is below 0 for more than 200ms

Browse Source 
+review OPReview
This commit is contained in:
Alessio Morale 2013-06-10 23:48:08 +02:00
parent 6f59c49506
commit c1a1c04ad8
2 changed files with 28 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
flight/modules/AltitudeHold/altitudehold.c
View File

@ -63,6 +63,7 @@
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 4 #define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000
// Private types // Private types
// Private variables // Private variables
@ -130,7 +131,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Force update of the settings // Force update of the settings
SettingsUpdatedCb(&ev); SettingsUpdatedCb(&ev);
// Failsafe handling
uint32_t lastAltitudeHoldDesiredUpdate = 0;
bool enterFailSafe = false;
// Listen for updates. // Listen for updates.
AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldDesiredConnectQueue(queue);
BaroAltitudeConnectQueue(queue); BaroAltitudeConnectQueue(queue);
@ -144,6 +147,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Main task loop // Main task loop
bool baro_updated = false; bool baro_updated = false;
while (1) { while (1) {
enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) { if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
if (!running) { if (!running) {
@ -173,6 +177,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
starting_altitude = altHold.Altitude; starting_altitude = altHold.Altitude;
} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { } else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
running = false; running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
} }
} else if (ev.obj == AccelsHandle()) { } else if (ev.obj == AccelsHandle()) {
static uint32_t timeval; static uint32_t timeval;
@ -352,6 +357,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
} }
if (!running) { if (!running) {
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
continue; continue;
} }
@ -370,24 +376,31 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Instead of explicit limit on integral you output limit feedback // Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral - if (!enterFailSafe) {
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka; stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
if (stabilizationDesired.Throttle > 1) { altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
throttleIntegral -= (stabilizationDesired.Throttle - 1); if (stabilizationDesired.Throttle > 1) {
stabilizationDesired.Throttle = 1; throttleIntegral -= (stabilizationDesired.Throttle - 1);
} else if (stabilizationDesired.Throttle < 0) { stabilizationDesired.Throttle = 1;
throttleIntegral -= stabilizationDesired.Throttle; } else if (stabilizationDesired.Throttle < 0) {
stabilizationDesired.Throttle = 0; throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
} else {
// shutdown motors
stabilizationDesired.Throttle = -1;
} }
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired); StabilizationDesiredSet(&stabilizationDesired);
} else if (ev.obj == AltitudeHoldDesiredHandle()) { } else if (ev.obj == AltitudeHoldDesiredHandle()) {
// reset the failsafe timer
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
AltitudeHoldDesiredGet(&altitudeHoldDesired); AltitudeHoldDesiredGet(&altitudeHoldDesired);
} }
} }

5
flight/modules/ManualControl/manualcontrol.c
View File

@ -811,6 +811,11 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// Stop updating AltitudeHoldDesired triggering a failsafe condition.
if (cmd->Throttle < 0) {
return;
}
// this is the max speed in m/s at the extents of throttle // this is the max speed in m/s at the extents of throttle
uint8_t throttleRate; uint8_t throttleRate;
uint8_t throttleExp; uint8_t throttleExp;