/**
* Select and use failsafe control
* @param [in] reset_controller True if previously another controller was used
*/
int32_t failsafe_control_select(bool reset_controller)
{
if (reset_controller) {
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
armed_when_enabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
}
uint8_t flight_status;
FlightStatusFlightModeGet(&flight_status);
if (flight_status != FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 || reset_controller) {
flight_status = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
FlightStatusFlightModeSet(&flight_status);
}
#ifdef GIMBAL
// Gimbals do not need failsafe
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabilization_desired);
#else
// Pick default values that will roughly cause a plane to circle down
// and a quad to fall straight down
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
if (!armed_when_enabled) {
/* disable stabilization so outputs do not move when system was not armed */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
} else {
/* Pick default values that will roughly cause a plane to circle down and */
/* a quad to fall straight down */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = -10;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = -5;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
StabilizationDesiredSet(&stabilization_desired);
#endif
return 0;
}
/**
* Select and use failsafe control
* @param [in] reset_controller True if previously another controller was used
*/
int32_t failsafe_control_select(bool reset_controller)
{
if (reset_controller) {
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
armed_when_enabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
rth_active = false;
/* Get configured failsafe behavior */
uint8_t failsafe_behavior;
ManualControlSettingsFailsafeBehaviorGet(&failsafe_behavior);
/* check whether RTH failsafe is configured and can be used */
if (failsafe_behavior == MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_RTH
&& check_rth_preconditions_met())
rth_active = true;
}
uint8_t flight_mode_desired = rth_active ? FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME : FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
uint8_t flight_mode_actual;
FlightStatusFlightModeGet(&flight_mode_actual);
if (flight_mode_actual != flight_mode_desired || reset_controller) {
FlightStatusFlightModeSet(&flight_mode_desired);
}
#ifdef GIMBAL
// Gimbals do not need failsafe
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabilization_desired);
#else
if (!rth_active) {
// Pick default values that will roughly cause a plane to circle down
// and a quad to fall straight down
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
if (!armed_when_enabled) {
/* disable stabilization so outputs do not move when system was not armed */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
} else {
/* Pick default values that will roughly cause a plane to circle down and */
/* a quad to fall straight down */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = -10;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = -5;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
StabilizationDesiredSet(&stabilization_desired);
}
#endif
return 0;
}
