diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 74485bf904..2d979f46ce 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -57,6 +57,7 @@ void Copter::ModeLand::gps_run() // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { zero_throttle_and_relax_ac(); + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); // disarm when the landing detector says we've landed diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 3058930ed5..8d28c6dffa 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -100,6 +100,7 @@ bool Copter::ModePosHold::init(bool ignore_checks) poshold.pitch_mode = POSHOLD_LOITER; // set target to current position // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); }else{ // if not landed start in pilot override to avoid hard twitch @@ -133,6 +134,7 @@ void Copter::ModePosHold::run() // initialize vertical speeds and acceleration pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); + loiter_nav->clear_pilot_desired_acceleration(); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index b0cc07f1c7..d1c983f35e 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -265,6 +265,7 @@ void Copter::ModeRTL::descent_run() if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); return; } @@ -356,6 +357,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); // disarm when the landing detector says we've landed diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 9d53e22258..6aba9d91b2 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -79,6 +79,7 @@ void Copter::ModeThrow::run() stage = Throw_PosHold; // initialise the loiter target to the curent position and velocity + loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum