Plane: Fix bug that can cause early exit from loiter to alt and time …

…wp's

The check for the aircraft being lined up for a tangent exit has an early breakout condition if the next waypoint is too close to the loiter circle which can prevent the required ground course to waypoint ever being achieved. This check was using the WP_LOITER_RAD parameter value, not the actual radius being used which can be set differently by the mission plan. If a large value for WP_LOITER_RAD was set and being over-written by the mission plan with a smaller value compatible with the distance to the next waypoint, the aircraft would still exit early.

Cherry-picked from ArduPilot/ardupilot#23801

ArduPlane/Plane.h

@@ -687,6 +687,9 @@ class Plane : public AP_Vehicle {
 
         // The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
         uint32_t time_max_ms;
+
+        // current value of loiter radius in metres used by the controller
+        float radius;
     } loiter;
 
     // Conditional command

ArduPlane/mode_loiter.cpp

@@ -34,8 +34,7 @@ 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.
 
-    const uint16_t loiterRadius = abs(plane.aparm.loiter_radius);
-    if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) {
+    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

ArduPlane/navigation.cpp

@@ -344,6 +344,9 @@ void Plane::update_loiter(uint16_t radius)
         }
     }
 
+    // the radius actually being used by the controller is required by other functions
+    loiter.radius = (float)radius;
+
     update_loiter_update_nav(radius);
 
     if (loiter.start_time_ms == 0) {