diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 0cc8b9457c11e7..62bb345f4453a4 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -10,17 +10,17 @@ bool ModeLoiterAltQLand::_enter() return true; } + const Location previous_wp = plane.next_WP_loc; + const bool already_in_a_loiter = plane.reached_loiter_target(); + ModeLoiter::_enter(); -#if AP_TERRAIN_AVAILABLE - if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); - } else { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); - } -#else - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); -#endif + // at this point plane.next_WP_loc == current_loc which means the instant we change + // modes we set that point as the center of the loiter-down circle. If we were already loitering + // then we don't want to change the center of the loiter circle + const Location loiter_wp = already_in_a_loiter ? previous_wp : plane.next_WP_loc; + + handle_guided_request(loiter_wp); switch_qland();