From 55b8f7c07afeeaab252f077b35d40ad73dc6073f Mon Sep 17 00:00:00 2001 From: "J.R. Bronkar" Date: Sat, 27 Jan 2024 00:05:19 +0000 Subject: [PATCH] Plane: Patch heading check for turn flexibility Removing breakout logic for "next" waypoints within 1->1.05 of loiter turn radius, adding extra heading buffer to compensate for potential overshooting on initial turn. --- ArduPlane/mode.h | 2 +- ArduPlane/mode_loiter.cpp | 34 +++++++++++++++++++++------------- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 2f745074c6f27..eb97b04903c7c 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -333,7 +333,7 @@ class ModeLoiter : public Mode void navigate() override; bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); - bool isHeadingLinedUp_cd(const int32_t bearing_cd); + bool isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t initial_bearing_margin_cd = 1000); bool allows_throttle_nudging() const override { return true; } diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 05ce3a6956857..1c1921ee095ef 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -40,26 +40,34 @@ void ModeLoiter::update() bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc) { // 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%, + const float targetDistanceFromCenter = loiterCenterLoc.get_distance(targetLoc); + if (targetDistanceFromCenter < 0.99f * fabsf(plane.loiter.radius)) { + /* Whenever next waypoint is within the loiter radius (damped slightly for rounding error), maintaining loiter would prevent us from ever pointing toward the next waypoint. Hence break out of loiter immediately */ return true; } - // Bearing in centi-degrees const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc); + if (targetDistanceFromCenter < 1.05f * fabsf(plane.loiter.radius)) { + /* Target Waypoint is between [radius, 1.05*radius) to center so we can + point toward the next waypoint, but there may be overshooting. Hence, + tolerance increased to 20 degrees on initial check to reduce loop-backs. + */ + return isHeadingLinedUp_cd(bearing_cd, 2000); + } + // Target waypoint is far enough from center, base margin tolerance is acceptable. return isHeadingLinedUp_cd(bearing_cd); + } -bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) +bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t initial_bearing_margin_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. + // Tolerance is initially "initial_bearing_margin_cd" and grows at 10 degrees for each loiter circle completed. // get current heading. const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100; @@ -67,19 +75,19 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); /* - 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. + Check to see if the the plane is heading toward the land waypoint. + We use 2*initial_bearing_margin_cd (+/- initial_bearing_margin_cd) so + that we can handle 20*initial_bearing_margin_cd 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 + After every full circle, extend acceptance criteria to ensure aircraft + will not loop forever in case high winds are forcing it beyond + 20*initial_bearing_margin_cd deg/sec when passing the desired exit course */ // Use integer division to get discrete steps const int32_t expanded_acceptance = 1000 * (labs(plane.loiter.sum_cd) / 36000); - if (labs(heading_err_cd) <= 1000 + expanded_acceptance) { + if (labs(heading_err_cd) <= initial_bearing_margin_cd + 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