Plane: Patch heading check for turn flexibility

ArduPlane/mode.h

@@ -333,7 +333,7 @@ class ModeLoiter : public Mode
     void navigate() override;
 
     bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
-    bool isHeadingLinedUp_cd(const int32_t bearing_cd);
+    bool isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t initial_bearing_margin_cd = 1000);
 
     bool allows_throttle_nudging() const override { return true; }
 

ArduPlane/mode_loiter.cpp

@@ -40,46 +40,54 @@ void ModeLoiter::update()
 bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
 {
     // Return true if current heading is aligned to vector to targetLoc.
-    // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
 
-    if (loiterCenterLoc.get_distance(targetLoc) < 1.05f * fabsf(plane.loiter.radius)) {
-        /* Whenever next waypoint is within the loiter radius plus 5%,
+    const float targetDistanceFromCenter = loiterCenterLoc.get_distance(targetLoc);
+    if (targetDistanceFromCenter < 0.99f * fabsf(plane.loiter.radius)) {
+        /* Whenever next waypoint is within the loiter radius (damped slightly for rounding error),
            maintaining loiter would prevent us from ever pointing toward the next waypoint.
            Hence break out of loiter immediately
          */
         return true;
     }
-
     // Bearing in centi-degrees
     const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc);
+    if (targetDistanceFromCenter < 1.05f * fabsf(plane.loiter.radius)) {
+        /*  Target Waypoint is between [radius, 1.05*radius) to center so we can
+            point toward the next waypoint, but there may be overshooting. Hence,
+            tolerance increased to 20 degrees on initial check to reduce loop-backs.
+        */
+        return isHeadingLinedUp_cd(bearing_cd, 2000);
+    }
+    // Target waypoint is far enough from center, base margin tolerance is acceptable.
     return isHeadingLinedUp_cd(bearing_cd);
+
 }
 
 
-bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd)
+bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t initial_bearing_margin_cd)
 {
     // Return true if current heading is aligned to bearing_cd.
-    // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
+    // Tolerance is initially "initial_bearing_margin_cd" and grows at 10 degrees for each loiter circle completed.
 
     // get current heading.
     const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100;
 
     const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
 
     /*
-      Check to see if the the plane is heading toward the land
-      waypoint. We use 20 degrees (+/-10 deg) of margin so that
-      we can handle 200 degrees/second of yaw.
+      Check to see if the the plane is heading toward the land waypoint.
+      We use 2*initial_bearing_margin_cd (+/- initial_bearing_margin_cd) so
+      that we can handle 20*initial_bearing_margin_cd degrees/second of yaw.
 
-      After every full circle, extend acceptance criteria to ensure
-      aircraft will not loop forever in case high winds are forcing
-      it beyond 200 deg/sec when passing the desired exit course
+      After every full circle, extend acceptance criteria to ensure aircraft
+      will not loop forever in case high winds are forcing it beyond
+      20*initial_bearing_margin_cd deg/sec when passing the desired exit course
     */
 
     // Use integer division to get discrete steps
     const int32_t expanded_acceptance = 1000 * (labs(plane.loiter.sum_cd) / 36000);
 
-    if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {
+    if (labs(heading_err_cd) <= initial_bearing_margin_cd + expanded_acceptance) {
         // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
 
         // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location