From 5554ad431198f65eb5ef3a6ff136bdc0f3678c28 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 May 2023 10:18:12 +1000 Subject: [PATCH] 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. --- ArduPlane/Plane.h | 3 +++ ArduPlane/mode_loiter.cpp | 3 +-- ArduPlane/navigation.cpp | 3 +++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8d5a08a189..0cdb615814 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -680,6 +680,9 @@ private: // 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 diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index e7b07c9db1..056d1e7155 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -42,8 +42,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 diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 16ceb26035..edb65fc2c5 100644 --- a/ArduPlane/navigation.cpp +++ b/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) {