From e0e6751d0fad6651514ad8d3db9949116330da3b Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 19 Oct 2023 13:52:04 -0700 Subject: [PATCH] Plane: mode LoiterAltQLand to reuse loiter point if available --- ArduPlane/mode_LoiterAltQLand.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 0cc8b9457c11e7..b8dbc3dc69571a 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -10,17 +10,17 @@ bool ModeLoiterAltQLand::_enter() return true; } + const Location previous_loiter_wp = plane.next_WP_loc; + const bool already_in_a_loiter = plane.previous_mode->does_auto_navigation() && 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_loiter_wp : plane.next_WP_loc; + + handle_guided_request(loiter_wp); switch_qland();