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

@@ -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;
+
+        /*
+          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;
 

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;