Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

[Plane] Fix verify_loiter_heading acceptance criteria.

This commit is contained in:
Przemek Lekston 2018-02-07 23:16:47 +01:00 committed by Tom Pittenger
parent 032ad9a67b
commit e5a9ac84e7
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/commands_logic.cpp
View File

@ -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