From 3aa742e1b905b1cd4ecdb729e46993e39f391b5b Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 28 Jan 2024 00:09:15 +0000
Subject: [PATCH] Plane: rework `isHeadingLinedUp` function for loiter breakout

---
 ArduPlane/mode.h          |  1 +
 ArduPlane/mode_loiter.cpp | 67 ++++++++++++++++++++++++++++++++-------
 2 files changed, 56 insertions(+), 12 deletions(-)

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index b7f4732e92..83da193de0 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -343,6 +343,7 @@ public:
     void navigate() override;
 
     bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
+    bool isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t heading_cd);
     bool isHeadingLinedUp_cd(const int32_t bearing_cd);
 
     bool allows_throttle_nudging() const override { return true; }
diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp
index 05ce3a6956..b37f3bdcb0 100644
--- a/ArduPlane/mode_loiter.cpp
+++ b/ArduPlane/mode_loiter.cpp
@@ -42,28 +42,71 @@ bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location
     // 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%,
-           maintaining loiter would prevent us from ever pointing toward the next waypoint.
-           Hence break out of loiter immediately
-         */
+    // Corrected radius for altitude
+    const float loiter_radius = plane.nav_controller->loiter_radius(fabsf(plane.loiter.radius));
+    if (!is_positive(loiter_radius)) {
+        // Zero is invalid, protect against divide by zero for destination inside loiter radius case
         return true;
     }
 
-    // Bearing in centi-degrees
-    const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc);
-    return isHeadingLinedUp_cd(bearing_cd);
+    // Calculate relative position of the vehicle relative to loiter center projected onto the closest point of the loiter circle
+    // This removes error due to radial position as the nav controller attempts to track the circle
+    const Vector2f projected_pos = loiterCenterLoc.get_distance_NE(plane.current_loc).normalized() * loiter_radius;
+
+    // Target position relative to loiter center
+    const Vector2f target_pos = loiterCenterLoc.get_distance_NE(targetLoc);
+
+    // Distance between loiter circle and target
+    const float target_dist = target_pos.length();
+    if (!is_positive(target_dist)) {
+        // Target is coincident with loiter center, no heading will be closer than any other
+        return true;
+    }
+
+    // Target bearing in centi-degrees
+    int32_t target_bearing_cd;
+
+    if (target_dist >= loiter_radius) {
+        // Destination outside loiter radius, heading will always line up with destination
+
+        // Vector from between projected vehicle position and target postion
+        const Vector2f pos_to_target = target_pos - projected_pos;
+        target_bearing_cd = degrees(pos_to_target.angle()) * 100;
+
+    } else {
+        // Destination is inside loiter, heading will never line up with destination
+
+        // Advance turn point by the angle of a segment with chord "a"
+        // This results in turning earlier as the target point approaches the center
+        // If target is on radius angle of 0 and angle of 60 deg if target is on center 
+        const float a = loiter_radius - target_dist;
+        const float segment_angle = 2.0 * asinf(a / (2.0 * loiter_radius));
+
+        // Pick the intersection point that will be hit first for the current loiter direction, add 90 deg to get the tangent angle
+        target_bearing_cd = degrees(wrap_PI(target_pos.angle() + (M_PI_2 - segment_angle) * plane.loiter.direction)) * 100;
+
+    }
+
+    // Ideal heading in centi-degrees, +- 90 to get tangent to loiter circle at closest point
+    const int32_t current_heading_cd = degrees(wrap_PI(projected_pos.angle() + M_PI_2 * plane.loiter.direction)) * 100;
+
+    return isHeadingLinedUp_cd(target_bearing_cd, current_heading_cd);
 }
 
 
-bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_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.
+bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) {
 
     // get current heading.
     const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100;
 
+    return isHeadingLinedUp_cd(bearing_cd, heading_cd);
+}
+
+
+bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t heading_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.
     const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
 
     /*