From e5a9ac84e75e8627e984d0765f23356699596336 Mon Sep 17 00:00:00 2001 From: Przemek Lekston Date: Wed, 7 Feb 2018 23:16:47 +0100 Subject: [PATCH] [Plane] Fix verify_loiter_heading acceptance criteria. --- ArduPlane/commands_logic.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a43ad58691..04383112df 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1065,20 +1065,23 @@ bool Plane::verify_loiter_heading(bool init) int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); if (init) { - loiter.total_cd = wrap_360_cd(bearing_cd - heading_cd); 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. Allow turn count - to stop it too to ensure we don't loop around forever in - case high winds are forcing us beyond 200 deg/sec at this - particular moment. + 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 */ - if (labs(heading_err_cd) <= 1000 || - loiter.sum_cd > loiter.total_cd) { + + // 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