// 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();
}
// returns pilot and takeoff climb rates
// pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired
void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
{
// return pilot_climb_rate if take-off inactive
if (!takeoff_state.running) {
takeoff_climb_rate = 0.0f;
return;
}
// acceleration of 50cm/s/s
static const float takeoff_accel = 50.0f;
float takeoff_minspeed = MIN(50.0f,takeoff_state.max_speed);
float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f;
float speed = MIN(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed);
float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel;
float height_gained;
if (time_elapsed <= time_to_max_speed) {
height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed;
} else {
height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed +
(time_elapsed-time_to_max_speed)*takeoff_state.max_speed;
}
// check if the takeoff is over
if (height_gained >= takeoff_state.alt_delta) {
takeoff_stop();
}
// if takeoff climb rate is zero return
if (speed <= 0.0f) {
takeoff_climb_rate = 0.0f;
return;
}
// default take-off climb rate to maximum speed
takeoff_climb_rate = speed;
// if pilot's commands descent
if (pilot_climb_rate < 0.0f) {
// if overall climb rate is still positive, move to take-off climb rate
if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
pilot_climb_rate = 0;
} else {
// if overall is negative, move to pilot climb rate
pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
takeoff_climb_rate = 0.0f;
}
} else { // pilot commands climb
// pilot climb rate is zero until it surpasses the take-off climb rate
if (pilot_climb_rate > takeoff_climb_rate) {
pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
} else {
pilot_climb_rate = 0.0f;
}
}
}
// 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
}
// althold_init - initialise althold controller
bool Copter::althold_init(bool ignore_checks)
{
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// stop takeoff if running
takeoff_stop();
return true;
}
// returns pilot and takeoff climb rates
// pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired
void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
{
// return pilot_climb_rate if take-off inactive
if (!takeoff_state.running) {
takeoff_climb_rate = 0.0f;
return;
}
// check if timeout as expired
if ((millis()-takeoff_state.start_ms) >= takeoff_state.time_ms) {
takeoff_stop();
takeoff_climb_rate = 0.0f;
return;
}
// if takeoff climb rate is zero return
if (takeoff_state.speed <= 0.0f) {
takeoff_climb_rate = 0.0f;
return;
}
// default take-off climb rate to maximum speed
takeoff_climb_rate = takeoff_state.speed;
// if pilot's commands descent
if (pilot_climb_rate < 0.0f) {
// if overall climb rate is still positive, move to take-off climb rate
if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
pilot_climb_rate = 0;
} else {
// if overall is negative, move to pilot climb rate
pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
takeoff_climb_rate = 0.0f;
}
} else { // pilot commands climb
// pilot climb rate is zero until it surpasses the take-off climb rate
if (pilot_climb_rate > takeoff_climb_rate) {
pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
} else {
pilot_climb_rate = 0.0f;
}
}
}
// althold_init - initialise althold controller
bool Copter::althold_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// stop takeoff if running
takeoff_stop();
return true;
}
// 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
}
