Plane: refactor loiter heading exit logic

ArduPlane/commands_logic.cpp

@@ -1074,47 +1074,5 @@ bool Plane::verify_loiter_heading(bool init)
         return true;
     }
 
-    if (next_WP_loc.get_distance(next_nav_cmd.content.location) < abs(aparm.loiter_radius)) {
-        /* Whenever next waypoint is within the loiter radius,
-           maintaining loiter would prevent us from ever pointing toward the next waypoint.
-           Hence break out of loiter immediately
-         */
-        return true;
-    }
-
-    // Bearing in degrees
-    int32_t bearing_cd = current_loc.get_bearing_to(next_nav_cmd.content.location);
-
-    // get current heading.
-    int32_t heading_cd = gps.ground_course_cd();
-
-    int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
-
-    if (init) {
-        loiter.sum_cd = 0;
-    }
-
-    /*
-      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.
-
-      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
-    */
-
-    // Use integer division to get discrete steps
-    int32_t expanded_acceptance = 1000 * (loiter.sum_cd / 36000);
-
-    if (labs(heading_err_cd) <= 1000 + 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
-        if (next_WP_loc.loiter_xtrack) {
-            next_WP_loc = current_loc;
-        }
-        return true;
-    }
-    return false;
+    return plane.mode_loiter.headingLinedUp(init, next_WP_loc, next_nav_cmd.content.location);
 }

ArduPlane/mode.h

@@ -3,6 +3,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Common/Location.h>
 #include <stdint.h>
+#include <AP_Common/Location.h>
 
 class Mode
 {
@@ -172,6 +173,8 @@ public:
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
+    bool headingLinedUp(const bool init, const Location loiterCenterLoc, const Location targetLoc);
+
 protected:
 
     bool _enter() override;

ArduPlane/mode_loiter.cpp

@@ -25,3 +25,50 @@ void ModeLoiter::update()
     plane.calc_throttle();
 }
 
+bool ModeLoiter::headingLinedUp(const bool init, const Location loiterCenterLoc, const Location targetLoc)
+{
+    const uint16_t loiterRadius = abs(plane.aparm.loiter_radius);
+    if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) {
+        /* Whenever next waypoint is within the loiter radius plus 5%,
+           maintaining loiter would prevent us from ever pointing toward the next waypoint.
+           Hence break out of loiter immediately
+         */
+        return true;
+    }
+
+    // Bearing in degrees
+    const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc);
+
+    // get current heading.
+    const int32_t heading_cd = plane.gps.ground_course_cd();
+
+    const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
+
+    if (init) {
+        plane.loiter.sum_cd = 0;
+    }
+
+    /*
+      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.
+
+      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
+    */
+
+    // Use integer division to get discrete steps
+    const int32_t expanded_acceptance = 1000 * (plane.loiter.sum_cd / 36000);
+
+    if (labs(heading_err_cd) <= 1000 + 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
+        if (plane.next_WP_loc.loiter_xtrack) {
+            plane.next_WP_loc = plane.current_loc;
+        }
+        return true;
+    }
+    return false;
+}