Skip to content

Commit

Permalink
Merge pull request #81 from CarbonixUAV/OV3-1021-CherryPick-necessary…
Browse files Browse the repository at this point in the history
…-for-CxPilot5.0.1

Fix bug that can cause early exit from loiter
  • Loading branch information
loki077 authored Nov 28, 2023
2 parents d6b2456 + 36eb6d5 commit 874d6cf
Show file tree
Hide file tree
Showing 3 changed files with 7 additions and 2 deletions.
3 changes: 3 additions & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -687,6 +687,9 @@ class Plane : public AP_Vehicle {

// 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
Expand Down
3 changes: 1 addition & 2 deletions ArduPlane/mode_loiter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,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
Expand Down
3 changes: 3 additions & 0 deletions ArduPlane/navigation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down

0 comments on commit 874d6cf

Please sign in to comment.