// 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(); }
// exit_mode - high level call to organise cleanup as a flight mode is exited void Copter::exit_mode(Copter::Mode *&old_flightmode, Copter::Mode *&new_flightmode) { #if AUTOTUNE_ENABLED == ENABLED if (old_flightmode == &mode_autotune) { mode_autotune.stop(); } #endif // stop mission when we leave auto mode #if MODE_AUTO_ENABLED == ENABLED if (old_flightmode == &mode_auto) { if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } #if MOUNT == ENABLED camera_mount.set_mode_to_default(); #endif // MOUNT == ENABLED } #endif // smooth throttle transition when switching from manual to automatic flight modes if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && 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(); } // cancel any takeoffs in progress takeoff_stop(); #if MODE_SMARTRTL_ENABLED == ENABLED // call smart_rtl cleanup if (old_flightmode == &mode_smartrtl) { mode_smartrtl.exit(); } #endif #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. if (old_flightmode == &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 (!old_flightmode->has_manual_throttle()){ if (new_flightmode == &mode_stabilize){ input_manager.set_stab_col_ramp(1.0); } else if (new_flightmode == &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(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 }