diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 9290651b69..b524f77094 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -174,8 +174,7 @@ public: void update() override; bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); - bool isHeadingLinedUp(const float bearing); - bool isHeadingLinedUp(const int32_t bearing_cd); + bool isHeadingLinedUp_cd(const int32_t bearing_cd); protected: diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 5d993f4f3f..820b0ed48d 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -29,6 +29,9 @@ 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. + const uint16_t loiterRadius = abs(plane.aparm.loiter_radius); if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) { /* Whenever next waypoint is within the loiter radius plus 5%, @@ -40,17 +43,15 @@ bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location // Bearing in centi-degrees const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc); - return isHeadingLinedUp(bearing_cd); + return isHeadingLinedUp_cd(bearing_cd); } -bool ModeLoiter::isHeadingLinedUp(const float bearing) -{ - const int32_t bearing_cd = bearing*100; - return isHeadingLinedUp(bearing_cd); -} -bool ModeLoiter::isHeadingLinedUp(const int32_t bearing_cd) +bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_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. + // get current heading. const int32_t heading_cd = plane.gps.ground_course_cd(); diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 992d78ed75..740d608cd7 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -142,7 +142,7 @@ void Plane::update_soaring() { } case Mode::Number::CRUISE: { - const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp(cruise_state.locked_heading_cd); + const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd); if (cruise_state.locked_heading && !headingLinedupToCruise && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) { break; }