diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 366228cbac..0198db443e 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -131,6 +131,17 @@ void ModeLoiter::run() attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); break; + case AltHold_Landed_Ground_Idle: + attitude_control->reset_yaw_target_and_rate(); + FALLTHROUGH; + + case AltHold_Landed_Pre_Takeoff: + attitude_control->reset_rate_controller_I_terms_smoothly(); + loiter_nav->init_target(); + attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero + break; + case AltHold_Takeoff: // initiate take-off if (!takeoff.running()) { @@ -150,17 +161,6 @@ void ModeLoiter::run() attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); break; - case AltHold_Landed_Ground_Idle: - attitude_control->reset_yaw_target_and_rate(); - FALLTHROUGH; - - case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms_smoothly(); - loiter_nav->init_target(); - attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); - pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero - break; - case AltHold_Flying: // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 9ad5133836..7b47eeb7c3 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -115,6 +115,22 @@ void ModePosHold::run() init_wind_comp_estimate(); break; + case AltHold_Landed_Ground_Idle: + loiter_nav->clear_pilot_desired_acceleration(); + loiter_nav->init_target(); + attitude_control->reset_yaw_target_and_rate(); + init_wind_comp_estimate(); + FALLTHROUGH; + + case AltHold_Landed_Pre_Takeoff: + attitude_control->reset_rate_controller_I_terms_smoothly(); + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero + + // set poshold state to pilot override + roll_mode = RPMode::PILOT_OVERRIDE; + pitch_mode = RPMode::PILOT_OVERRIDE; + break; + case AltHold_Takeoff: // initiate take-off if (!takeoff.running()) { @@ -136,22 +152,6 @@ void ModePosHold::run() pitch_mode = RPMode::PILOT_OVERRIDE; break; - case AltHold_Landed_Ground_Idle: - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); - attitude_control->reset_yaw_target_and_rate(); - init_wind_comp_estimate(); - FALLTHROUGH; - - case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero - - // set poshold state to pilot override - roll_mode = RPMode::PILOT_OVERRIDE; - pitch_mode = RPMode::PILOT_OVERRIDE; - break; - case AltHold_Flying: motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 0c2cbb4d6f..954a5a2b98 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -80,6 +80,15 @@ void ModeSport::run() pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; + case AltHold_Landed_Ground_Idle: + attitude_control->reset_yaw_target_and_rate(); + FALLTHROUGH; + + case AltHold_Landed_Pre_Takeoff: + attitude_control->reset_rate_controller_I_terms_smoothly(); + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero + break; + case AltHold_Takeoff: // initiate take-off if (!takeoff.running()) { @@ -93,15 +102,6 @@ void ModeSport::run() takeoff.do_pilot_takeoff(target_climb_rate); break; - case AltHold_Landed_Ground_Idle: - attitude_control->reset_yaw_target_and_rate(); - FALLTHROUGH; - - case AltHold_Landed_Pre_Takeoff: - attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero - break; - case AltHold_Flying: motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);