From 378bd84b2984b02f0353f39fdaea7c989e7d7059 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Tue, 20 Dec 2016 14:31:42 +1100 Subject: [PATCH] Rover: Fixed a bug in the wp loiter functionality Had a bug where the rover would loiter for ever and not resume even though its loiter time had expired. This resolves that. --- APMrover2/Steering.cpp | 7 ++++--- APMrover2/commands_logic.cpp | 5 ++++- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 191c5bf388..2f1e16a122 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -83,10 +83,11 @@ bool Rover::in_stationary_loiter() // Confirm we are in AUTO mode and need to loiter for a time period if ((loiter_start_time > 0) && (control_mode == AUTO)) { // Check if active loiter is enabled AND we are outside the waypoint loiter radius - // then NOT the result for the if logic - if (!(active_loiter && (wp_distance > g.waypoint_radius))) { - return true; + // then the vehicle still needs to move so return false + if (active_loiter && (wp_distance > g.waypoint_radius)) { + return false; } + return true; } return false; diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index fe17429b5c..7e2fb54e38 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -288,7 +288,10 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // have we gone past the waypoint? // We should always go through the waypoint i.e. the above code // first before we go past it but sometimes we don't. - if (location_passed_point(current_loc, prev_WP, next_WP)) { + // OR have we reached the waypoint previously be we aren't actively loitering + // This second check is required for when we roll past the waypoint radius + if (location_passed_point(current_loc, prev_WP, next_WP) || + (!active_loiter && previously_reached_wp)) { // As we have passed the waypoint navigation needs to be done from current location prev_WP = current_loc; // Check if this is the first time we have reached the waypoint even though we have gone past it