// acro_run - runs the acro controller
// should be called at 100hz or more
void Sub::acro_run()
{
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
// if motors not running reset angle targets
if(!motors.armed()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
// run attitude controller
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// output pilot's throttle without angle boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
//control_in is range 0-1000
//radio_in is raw pwm value
motors.set_forward(channel_forward->radio_in);
motors.set_strafe(channel_strafe->radio_in);
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
if (old_control_mode == THROW) {
throw_exit();
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}
// cancel any takeoffs in progress
takeoff_stop();
}
void Copter::ModeAcro::run()
{
float target_roll, target_pitch, target_yaw;
float pilot_throttle_scaled;
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// clear landing flag
set_land_complete(false);
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid);
// run attitude controller
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
}
// drift_run - runs the drift controller
// should be called at 100hz or more
void Copter::drift_run()
{
static float breaker = 0.0f;
static float roll_input = 0.0f;
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// convert pilot input to lean angles
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
// Grab inertial velocity
const Vector3f& vel = inertial_nav.get_velocity();
// rotate roll, pitch input from north facing to vehicle's perspective
float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
// gain sceduling for Yaw
float pitch_vel2 = min(fabsf(pitch_vel), 2000);
target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
roll_input = roll_input * .96f + (float)channel_yaw->control_in * .04f;
//convert user input into desired roll velocity
float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN);
// Roll velocity is feed into roll acceleration to minimize slip
target_roll = roll_vel_error * -DRIFT_SPEEDGAIN;
target_roll = constrain_int16(target_roll, -4500, 4500);
// If we let go of sticks, bring us to a stop
if(is_zero(target_pitch)) {
// .14/ (.03 * 100) = 4.6 seconds till full breaking
breaker += .03f;
breaker = min(breaker, DRIFT_SPEEDGAIN);
target_pitch = pitch_vel * breaker;
} else {
breaker = 0.0f;
}
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle with angle boost
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
}
// stabilize_init - initialise stabilize controller
bool Copter::ModeStabilize::init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
return false;
}
return true;
}
bool Copter::ModeAcro::init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), _copter.g2.acro_thr_mid) > _copter.get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control->set_alt_target(0);
return true;
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
if (old_control_mode == THROW) {
throw_exit();
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in()));
}
// cancel any takeoffs in progress
takeoff_stop();
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control.use_flybar_passthrough(false, false);
motors.set_acro_tail(false);
}
// if we are changing from a mode that did not use manual throttle,
// stab col ramp value should be pre-loaded to the correct value to avoid a twitch
// heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
if (!mode_has_manual_throttle(old_control_mode)){
if (new_control_mode == STABILIZE){
input_manager.set_stab_col_ramp(1.0);
} else if (new_control_mode == ACRO){
input_manager.set_stab_col_ramp(0.0);
}
}
#endif //HELI_FRAME
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}
// cancel any takeoffs in progress
takeoff_stop();
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control.use_flybar_passthrough(false, false);
motors.set_acro_tail(false);
}
// reset RC Passthrough to motors
motors.reset_radio_passthrough();
#endif //HELI_FRAME
}
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::ModeStabilize::run()
{
float target_roll, target_pitch;
float target_yaw_rate;
float pilot_throttle_scaled;
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
return;
}
// clear landing flag
set_land_complete(false);
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// body-frame rate controller is run directly from 100hz loop
// output pilot's throttle
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || ap.throttle_zero) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop
// output pilot's throttle
attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}
// flip_run - runs the flip controller
// should be called at 100hz or more
void Sub::flip_run()
{
float throttle_out;
float recovery_angle;
// if pilot inputs roll > 40deg or timeout occurs abandon flip
if (!motors.armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
flip_state = Flip_Abandon;
}
// get pilot's desired throttle
throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in());
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition
int32_t flip_angle;
if (flip_roll_dir != 0) {
flip_angle = ahrs.roll_sensor * flip_roll_dir;
} else {
flip_angle = ahrs.pitch_sensor * flip_pitch_dir;
}
// state machine
switch (flip_state) {
case Flip_Start:
// under 45 degrees request 400deg/sec roll or pitch
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// increase throttle
throttle_out += FLIP_THR_INC;
// beyond 45deg lean angle move to next stage
if (flip_angle >= 4500) {
if (flip_roll_dir != 0) {
// we are rolling
flip_state = Flip_Roll;
} else {
// we are pitching
flip_state = Flip_Pitch_A;
}
}
break;
case Flip_Roll:
// between 45deg ~ -90deg request 400deg/sec roll
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
// decrease throttle
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// beyond -90deg move on to recovery
if ((flip_angle < 4500) && (flip_angle > -9000)) {
flip_state = Flip_Recover;
}
break;
case Flip_Pitch_A:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// check roll for inversion
if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
flip_state = Flip_Pitch_B;
}
break;
case Flip_Pitch_B:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// check roll for inversion
if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
flip_state = Flip_Recover;
}
break;
case Flip_Recover:
// use originally captured earth-frame angle targets to recover
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain());
// increase throttle to gain any lost altitude
throttle_out += FLIP_THR_INC;
if (flip_roll_dir != 0) {
// we are rolling
recovery_angle = fabsf(flip_orig_attitude.x - (float)ahrs.roll_sensor);
} else {
// we are pitching
recovery_angle = fabsf(flip_orig_attitude.y - (float)ahrs.pitch_sensor);
}
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log successful completion
Log_Write_Event(DATA_FLIP_END);
}
break;
case Flip_Abandon:
// restore original flight mode
if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log abandoning flip
Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
break;
}
// output pilot's throttle without angle boost
attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
}
// manual_init - initialise manual controller
bool Sub::manual_init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);
return true;
}
// drift_run - runs the drift controller
// should be called at 100hz or more
void Copter::ModeDrift::run()
{
static float braker = 0.0f;
static float roll_input = 0.0f;
float target_roll, target_pitch;
float target_yaw_rate;
float pilot_throttle_scaled;
// if landed and throttle at zero, set throttle to zero and exit immediately
if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) {
zero_throttle_and_relax_ac();
return;
}
// clear landing flag above zero throttle
if (!ap.throttle_zero) {
set_land_complete(false);
}
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
// Grab inertial velocity
const Vector3f& vel = inertial_nav.get_velocity();
// rotate roll, pitch input from north facing to vehicle's perspective
float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
// gain sceduling for Yaw
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000);
target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
roll_input = roll_input * .96f + (float)channel_yaw->get_control_in() * .04f;
//convert user input into desired roll velocity
float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN);
// Roll velocity is feed into roll acceleration to minimize slip
target_roll = roll_vel_error * -DRIFT_SPEEDGAIN;
target_roll = constrain_float(target_roll, -4500.0f, 4500.0f);
// If we let go of sticks, bring us to a stop
if(is_zero(target_pitch)){
// .14/ (.03 * 100) = 4.6 seconds till full braking
braker += .03f;
braker = MIN(braker, DRIFT_SPEEDGAIN);
target_pitch = pitch_vel * braker;
}else{
braker = 0.0f;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// output pilot's throttle with angle boost
attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
}
// run - runs the flip controller
// should be called at 100hz or more
void Copter::ModeFlip::run()
{
// if pilot inputs roll > 40deg or timeout occurs abandon flip
if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) {
_state = FlipState::Abandon;
}
// get pilot's desired throttle
float throttle_out = get_pilot_desired_throttle();
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition
int32_t flip_angle;
if (roll_dir != 0) {
flip_angle = ahrs.roll_sensor * roll_dir;
} else {
flip_angle = ahrs.pitch_sensor * pitch_dir;
}
// state machine
switch (_state) {
case FlipState::Start:
// under 45 degrees request 400deg/sec roll or pitch
attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, FLIP_ROTATION_RATE * pitch_dir, 0.0);
// increase throttle
throttle_out += FLIP_THR_INC;
// beyond 45deg lean angle move to next stage
if (flip_angle >= 4500) {
if (roll_dir != 0) {
// we are rolling
_state = FlipState::Roll;
} else {
// we are pitching
_state = FlipState::Pitch_A;
}
}
break;
case FlipState::Roll:
// between 45deg ~ -90deg request 400deg/sec roll
attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, 0.0, 0.0);
// decrease throttle
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// beyond -90deg move on to recovery
if ((flip_angle < 4500) && (flip_angle > -9000)) {
_state = FlipState::Recover;
}
break;
case FlipState::Pitch_A:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * pitch_dir, 0.0);
// decrease throttle
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// check roll for inversion
if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
_state = FlipState::Pitch_B;
}
break;
case FlipState::Pitch_B:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control->input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * pitch_dir, 0.0);
// decrease throttle
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// check roll for inversion
if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
_state = FlipState::Recover;
}
break;
case FlipState::Recover: {
// use originally captured earth-frame angle targets to recover
attitude_control->input_euler_angle_roll_pitch_yaw(orig_attitude.x, orig_attitude.y, orig_attitude.z, false);
// increase throttle to gain any lost altitude
throttle_out += FLIP_THR_INC;
float recovery_angle;
if (roll_dir != 0) {
// we are rolling
recovery_angle = fabsf(orig_attitude.x - (float)ahrs.roll_sensor);
} else {
// we are pitching
recovery_angle = fabsf(orig_attitude.y - (float)ahrs.pitch_sensor);
}
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log successful completion
Log_Write_Event(DATA_FLIP_END);
}
break;
}
case FlipState::Abandon:
// restore original flight mode
if (!copter.set_mode(orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log abandoning flip
AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED);
break;
}
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt);
}