diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 656b2ea95e..5996fd65dd 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -833,6 +833,7 @@ void ModeAuto::land_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_spool_down(); + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); return; } @@ -928,6 +929,7 @@ void ModeAuto::loiter_to_alt_run() } if (!loiter_to_alt.loiter_start_done) { + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); _mode = Auto_LoiterToAlt; loiter_to_alt.loiter_start_done = true; diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index b8bbcb2227..0b397d0890 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -61,6 +61,7 @@ void ModeLand::gps_run() // Land State Machine Determination if (is_disarmed_or_landed()) { make_safe_spool_down(); + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); } else { // set motors to full range diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 7ba3407745..df13a8c46a 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -111,6 +111,7 @@ void ModePosHold::run() attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); loiter_nav->update(); @@ -133,6 +134,7 @@ void ModePosHold::run() target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // init and update loiter although pilot is controlling lean angles + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); loiter_nav->update(); @@ -147,6 +149,7 @@ void ModePosHold::run() case AltHold_Landed_Ground_Idle: + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); loiter_nav->update(); attitude_control->reset_rate_controller_I_terms(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 0c72a76122..dc5f603d12 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -384,6 +384,7 @@ void ModeRTL::land_run(bool disarm_on_land) // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_spool_down(); + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); return; }