From 8b4757a883426c4dc820ee5aad5f17ea31bc504a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Mar 2024 17:44:23 +1100 Subject: [PATCH] Plane: call update_loiter before determining whether to fly home or not we are calling "reached_loiter_target" as part of our checks as to whether to fly home or not. We need to call update_loiter so the L1 controller can update its internal state for the new waypoint which do_RTL has set. Depending on location (but typically), that will mean that L1's reached_loiter_target() will then return false, so we fly home. This bug was affected by f8d7be5e43686e5a20a1a59420c308ac1fab4c70 . Any sort of altitude error greater than 10m would delay us entering the landing sequence, allowing the L1 controller to update its state. --- ArduPlane/mode_rtl.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index ee851089e2..ae0283c98a 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -95,6 +95,13 @@ void ModeRTL::navigate() } #endif + uint16_t radius = abs(plane.g.rtl_radius); + if (radius > 0) { + plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1; + } + + plane.update_loiter(radius); + if (!plane.auto_state.checked_for_autoland) { if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) || (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START && @@ -116,13 +123,6 @@ void ModeRTL::navigate() plane.auto_state.checked_for_autoland = true; } } - - uint16_t radius = abs(plane.g.rtl_radius); - if (radius > 0) { - plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1; - } - - plane.update_loiter(radius); } #if HAL_QUADPLANE_ENABLED