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

LP-446 - Implement battery alarm failsafe

Browse Source
This commit is contained in:
Alessio Morale 2016-11-04 00:28:21 +01:00 committed by Philippe Renon
parent 7a7b5c6f68
commit 45eb52fcd8
2 changed files with 91 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -46,6 +46,7 @@
#include <stabilizationdesired.h>
#include <callbackinfo.h>
#include <stabilizationsettings.h>
#include <systemalarms.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <vtolpathfollowersettings.h>
#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
*/

1
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -95,6 +95,7 @@
<field name="AutoTakeOffHeight" units="m" type="float" elements="1" defaultvalue="2.5" description="height in meters above arming altitude to climb to during autotakeoff"/>
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
<field name="BatteryFailsafeSwitchPositions" units="" type="int8" elementnames="Warning,Critical" defaultvalue="-1" />
<field name="FlightModeChangeRestartsPathPlan" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" description="wether a path plan should continue when interrupted by flight mode change (False), or restart from waypoint 0 (True)"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>