/**
* If an autonomous mode is available, make sure all the configurations are
* valid for it
*/
static int32_t check_safe_autonomous()
{
// The current filter combinations are safe for navigation
// Attitude | Navigation
// Comp | Raw (not recommended)
// Comp | INS (recommmended)
// Anything | None (unsafe)
// INSOutdoor | INS
// INSIndoor | INS (unsafe)
#if !defined(SMALLF1)
StateEstimationData stateEstimation;
StateEstimationGet(&stateEstimation);
if (stateEstimation.AttitudeFilter == STATEESTIMATION_ATTITUDEFILTER_INSINDOOR)
return SYSTEMALARMS_CONFIGERROR_NAVFILTER;
// Anything not allowed is invalid, safe default
if (stateEstimation.NavigationFilter != STATEESTIMATION_NAVIGATIONFILTER_INS &&
stateEstimation.NavigationFilter != STATEESTIMATION_NAVIGATIONFILTER_RAW)
return SYSTEMALARMS_CONFIGERROR_NAVFILTER;
#endif
return SYSTEMALARMS_CONFIGERROR_NONE;
}
//! Check if RTH is possible and safe
static bool check_rth_preconditions_met()
{
// If transmitter throttle was low before failsafe kicked in
// RTH is not safe, because pilot probably just forgot to disarm
// before powering transmitter off
if (throttle_low_before_engage)
return false;
// No RTH when disarmed
if (!armed_when_enabled)
return false;
// Only VTOLs are currently RTH capable
uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
if (airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_TRI &&
airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL)
return false;
uint8_t module_admin_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_admin_state);
// VTOLPathFollower module must be enabled
if (module_admin_state[MODULESETTINGS_ADMINSTATE_VTOLPATHFOLLOWER] != MODULESETTINGS_ADMINSTATE_ENABLED)
return false;
// GPS module must be enabled
if (module_admin_state[MODULESETTINGS_ADMINSTATE_GPS] != MODULESETTINGS_ADMINSTATE_ENABLED)
return false;
// check path follower alarm
if (AlarmsGet(SYSTEMALARMS_ALARM_PATHFOLLOWER) >= SYSTEMALARMS_ALARM_WARNING)
return false;
// attitude filter must be set to complementary or INSOutdoor and navigation filter must be INS
StateEstimationData stateEstimation;
StateEstimationGet(&stateEstimation);
if (stateEstimation.AttitudeFilter == STATEESTIMATION_ATTITUDEFILTER_INSINDOOR ||
stateEstimation.NavigationFilter != STATEESTIMATION_NAVIGATIONFILTER_INS)
return false;
return true;
}
