aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: fix LOITER_TO_ALT to verify headings towards waypoints within the loiter radius.

Browse Source 
Whenever next waypoint is within the loiter radius, maintaining loiter would prevent us from ever pointing toward the next waypoint. Hence for very close waypoints loiter_to_alt becomes verified by the altitude only.
This commit is contained in:
Przemek Lekston 2015-07-21 00:28:25 +02:00 committed by Andrew Tridgell
parent d51e6d466b
commit 6a62b11536
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/commands_logic.cpp
View File

@ -588,7 +588,15 @@ bool Plane::verify_loiter_to_alt()
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
}
if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) {
/* Whenever next waypoint is within the loiter radius,
maintaining loiter would prevent us from ever pointing toward the next waypoint.
Hence break out of loiter immediately
*/
return true;
}
// Bearing in radians
int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);