From b325569df217062f90049f1c02c709d42ded2f88 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 29 Sep 2024 09:14:42 +1000
Subject: [PATCH] AP_L1_Control: make reached_loiter_target() more reliable

if our target loiter radius is unachievable then we can reach the
loiter target on initial capture but be unable to maintain it. This
ensures that once we capture we return true on reached_loiter_target()

This is critical for any mission type where we take an action on
reached_loiter_target() and another condition (such as being lined up
for a waypoint). Otherwise we may continue loitering forever
---
 libraries/AP_L1_Control/AP_L1_Control.cpp | 26 ++++++++++++++++++++++-
 libraries/AP_L1_Control/AP_L1_Control.h   |  4 ++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp
index 2750c232298d0d..3e9c2b41ca7b68 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.cpp
+++ b/libraries/AP_L1_Control/AP_L1_Control.cpp
@@ -339,6 +339,7 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
 
     // Waypoint capture status is always false during waypoint following
     _WPcircle = false;
+    last_reached_loiter_target_ms = 0;
 
     _bearing_error = Nu; // bearing error angle (radians), +ve to left of track
 
@@ -442,18 +443,39 @@ void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_
     // Perform switchover between 'capture' and 'circle' modes at the
     // point where the commands cross over to achieve a seamless transfer
     // Only fly 'capture' mode if outside the circle
+    const uint32_t now_ms = AP_HAL::millis();
     if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
         _latAccDem = latAccDemCap;
-        _WPcircle = false;
+
+        /*
+          if we were previously on the circle and the target has not
+          changed then keep _WPcircle true. This prevents
+          reached_loiter_target() from going false due to a gust of
+          wind or an unachievable loiter radius
+         */
+        if (_WPcircle &&
+            last_reached_loiter_target_ms != 0 &&
+            now_ms - last_reached_loiter_target_ms < 200U &&
+            center_WP.same_loc_as(last_center_WP)) {
+            // same location, within 200ms, keep the _WPcircle status as true
+            last_reached_loiter_target_ms = now_ms;
+        } else {
+            _WPcircle = false;
+            last_reached_loiter_target_ms = 0;
+        }
+
         _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
         _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
     } else {
         _latAccDem = latAccDemCirc;
         _WPcircle = true;
+        last_reached_loiter_target_ms = now_ms;
         _bearing_error = 0.0f; // bearing error (radians), +ve to left of track
         _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
     }
 
+    last_center_WP = center_WP;
+
     _data_is_stale = false; // status are correctly updated with current waypoint data
 }
 
@@ -487,6 +509,7 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 
     // Waypoint capture status is always false during heading hold
     _WPcircle = false;
+    last_reached_loiter_target_ms = 0;
 
     _crosstrack_error = 0;
 
@@ -510,6 +533,7 @@ void AP_L1_Control::update_level_flight(void)
 
     // Waypoint capture status is always false during heading hold
     _WPcircle = false;
+    last_reached_loiter_target_ms = 0;
 
     _latAccDem = 0;
 
diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h
index e3035a4dd594f3..7cba218cc788fc 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.h
+++ b/libraries/AP_L1_Control/AP_L1_Control.h
@@ -124,6 +124,10 @@ class AP_L1_Control : public AP_Navigation {
 
     AP_Float _loiter_bank_limit;
 
+    // remember reached_loiter_target decision
+    uint32_t last_reached_loiter_target_ms;
+    Location last_center_WP;
+
     bool _reverse = false;
     float get_yaw() const;
     int32_t get_yaw_sensor() const;