From 508c6dd7633051bc0e0c3f2bf3b067ff19aedc4e Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 4 Nov 2016 00:28:21 +0100 Subject: [PATCH] LP-446 - Implement battery alarm failsafe --- flight/modules/ManualControl/manualcontrol.c | 92 ++++++++++++++++++- .../flightmodesettings.xml | 1 + 2 files changed, 91 insertions(+), 2 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b18f8d8cb..e5195f5de 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -46,6 +46,7 @@ #include #include #include +#include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ @@ -66,6 +67,8 @@ #define ALWAYSTABILIZEACCESSORY_THRESHOLD 0.05f +// Wait 5 seconds before triggering the battery failsafe mode +#define BATTERY_FAILSAFE_DEBOUNCE_TIMER 5000 // defined handlers static const controlHandler handler_MANUAL = { @@ -119,9 +122,9 @@ static void commandUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings); +static void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings); #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ static void SettingsUpdatedCb(UAVObjEvent *ev); - #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) /** @@ -172,6 +175,7 @@ int32_t ManualControlInitialize() StabilizationSettingsInitialize(); AccessoryDesiredInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + SystemAlarmsInitialize(); VtolSelfTuningStatsInitialize(); VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); @@ -239,6 +243,11 @@ static void manualControlTask(void) uint8_t newFlightModeAssist = flightStatus.FlightModeAssist; uint8_t newAssistedControlState = flightStatus.AssistedControlState; uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState; + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + HandleBatteryFailsafe(&position, &modeSettings); +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ + if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { newMode = modeSettings.FlightModePosition[position]; } @@ -249,7 +258,6 @@ static void manualControlTask(void) newMode = flightStatus.FlightMode; position = lastPosition; } - // if a mode change occurs we default the assist mode and states here // to avoid having to add it to all of the below modes that are // otherwise unrelated @@ -526,6 +534,86 @@ static void manualControlTask(void) } } +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings) +{ + // static uint8_t lastInputPosition = -1; + typedef enum { BATTERYFAILSAFE_NONE = 0, BATTERYFAILSAFE_WARNING = 1, BATTERYFAILSAFE_CRITICAL = 2 } batteryfailsafemode_t; + static batteryfailsafemode_t lastFailsafeStatus = BATTERYFAILSAFE_NONE; + static bool failsafeOverridden; + static uint8_t lastFlightPosition; + static uint32_t changeTimestamp; + SystemAlarmsAlarmData alarms; + batteryfailsafemode_t failsafeStatus; + FlightStatusArmedOptions armed; + FlightStatusArmedGet(&armed); + + // reset the status and do not change anything when not armed + if (armed != FLIGHTSTATUS_ARMED_ARMED) { + lastFailsafeStatus = BATTERYFAILSAFE_NONE; + failsafeOverridden = false; + changeTimestamp = PIOS_DELAY_GetRaw(); + lastFlightPosition = *position; + return; + } + + SystemAlarmsAlarmGet(&alarms); + + switch (alarms.Battery) { + case SYSTEMALARMS_ALARM_WARNING: + failsafeStatus = BATTERYFAILSAFE_WARNING; + break; + case SYSTEMALARMS_ALARM_CRITICAL: + failsafeStatus = BATTERYFAILSAFE_CRITICAL; + break; + default: + failsafeStatus = BATTERYFAILSAFE_NONE; + } + uint32_t debounceTimerms = PIOS_DELAY_DiffuS(changeTimestamp) / 1000; + + if (failsafeStatus == lastFailsafeStatus) { + changeTimestamp = PIOS_DELAY_GetRaw(); + } else if ((debounceTimerms < BATTERY_FAILSAFE_DEBOUNCE_TIMER) || failsafeStatus < lastFailsafeStatus) { + // do not change within the "grace" period and do not "downgrade" the failsafe mode + failsafeStatus = lastFailsafeStatus; + } else { + // a higher failsafe status was met and grace period elapsed. Trigger the new state + lastFailsafeStatus = failsafeStatus; + lastFlightPosition = *position; + failsafeOverridden = false; + } + + if ((failsafeStatus == BATTERYFAILSAFE_NONE) || failsafeOverridden) { + return; + } + + // failsafe has been triggered. Check for override + if (lastFlightPosition != *position) { + // flag the override and reset the grace period + failsafeOverridden = true; + changeTimestamp = PIOS_DELAY_GetRaw(); + return; + } + + switch (failsafeStatus) { + case BATTERYFAILSAFE_CRITICAL: + // if critical is not set, jump to the other case to use the warning setting. + if (modeSettings->BatteryFailsafeSwitchPositions.Critical != -1) { + *position = modeSettings->BatteryFailsafeSwitchPositions.Critical; + break; + } + case BATTERYFAILSAFE_WARNING: + if (modeSettings->BatteryFailsafeSwitchPositions.Warning != -1) { + *position = modeSettings->BatteryFailsafeSwitchPositions.Warning; + } + break; + default: + break; + } +} + +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ + /** * Called whenever a critical configuration component changes */ diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 394d814a6..9f335c66f 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -95,6 +95,7 @@ +