Exemple #1
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);
	}

	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;
}