From 6a62b115365618e6e0e2a743ebc46d2d2825f9e2 Mon Sep 17 00:00:00 2001 From: Przemek Lekston Date: Tue, 21 Jul 2015 00:28:25 +0200 Subject: [PATCH] Plane: fix LOITER_TO_ALT to verify headings towards waypoints within the loiter radius. 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. --- ArduPlane/commands_logic.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index cf921b8df2..4aac5ecfcb 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -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);