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) {