From cb924d1363e34db39ae7e650059c13795f628aed 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 | 32 ++++++++++++++++++++++-
 libraries/AP_L1_Control/AP_L1_Control.h   |  8 ++++++
 2 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp
index 2750c23229..ca6cc75b88 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_loiter.reached_loiter_target_ms = 0;
 
     _bearing_error = Nu; // bearing error angle (radians), +ve to left of track
 
@@ -348,6 +349,8 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
 // update L1 control for loitering
 void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_t loiter_direction)
 {
+    const float radius_unscaled = radius;
+
     Location _current_loc;
 
     // scale loiter radius with square of EAS2TAS to allow us to stay
@@ -442,18 +445,43 @@ 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_loiter.reached_loiter_target_ms != 0 &&
+            now_ms - _last_loiter.reached_loiter_target_ms < 200U &&
+            loiter_direction == _last_loiter.direction &&
+            is_equal(radius_unscaled, _last_loiter.radius) &&
+            center_WP.same_loc_as(_last_loiter.center_WP)) {
+            // same location, within 200ms, keep the _WPcircle status as true
+            _last_loiter.reached_loiter_target_ms = now_ms;
+        } else {
+            _WPcircle = false;
+            _last_loiter.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_loiter.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_loiter.radius = radius_unscaled;
+    _last_loiter.direction = loiter_direction;
+    _last_loiter.center_WP = center_WP;
+
     _data_is_stale = false; // status are correctly updated with current waypoint data
 }
 
@@ -487,6 +515,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_loiter.reached_loiter_target_ms = 0;
 
     _crosstrack_error = 0;
 
@@ -510,6 +539,7 @@ void AP_L1_Control::update_level_flight(void)
 
     // Waypoint capture status is always false during heading hold
     _WPcircle = false;
+    _last_loiter.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 e3035a4dd5..20eb6e5777 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.h
+++ b/libraries/AP_L1_Control/AP_L1_Control.h
@@ -124,6 +124,14 @@ private:
 
     AP_Float _loiter_bank_limit;
 
+    // remember reached_loiter_target decision
+    struct {
+        uint32_t reached_loiter_target_ms;
+        float radius;
+        int8_t direction;
+        Location center_WP;
+    } _last_loiter;
+
     bool _reverse = false;
     float get_yaw() const;
     int32_t get_yaw_sensor() const;