From 5b411aef65236afbf456dbe1d3ad56ffcbe11957 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Sat, 12 Nov 2016 13:55:37 +1100 Subject: [PATCH] Rover: Fixed LOITER_UNLIM to be an active loiter This changes brings the LOITER commands in line so both LOITER_UNLIM and LOITER_TIME are actively loitering. --- APMrover2/Rover.h | 2 +- APMrover2/commands_logic.cpp | 22 ++++++++++++---------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 8ffc2288ad..81926bc3d7 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -526,7 +526,7 @@ private: void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); void do_loiter_time(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); - bool verify_loiter_unlim(); + bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd); bool verify_loiter_time(const AP_Mission::Mission_Command& cmd); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 160df370c8..4935842c42 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -169,7 +169,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) return verify_RTL(); case MAV_CMD_NAV_LOITER_UNLIM: - return verify_loiter_unlim(); + return verify_loiter_unlimited(cmd); case MAV_CMD_NAV_LOITER_TIME: return verify_loiter_time(cmd); @@ -229,17 +229,16 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd) // this is the distance we travel past the waypoint - not there yet so 0 initially distance_past_wp = 0; - set_next_WP(cmd.content.location); + Location cmdloc = cmd.content.location; + location_sanitize(current_loc, cmdloc); + set_next_WP(cmdloc); } void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { - previously_reached_wp = false; - Location cmdloc = cmd.content.location; - location_sanitize(current_loc, cmdloc); - set_next_WP(cmdloc); + active_loiter = true; + do_nav_wp(cmd); loiter_duration = 100; // an arbitrary large loiter time - distance_past_wp = 0; } // do_loiter_time - initiate loitering at a point for a given time period @@ -290,6 +289,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // 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)) { + // 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 if (!previously_reached_wp) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds", @@ -343,10 +344,11 @@ bool Rover::verify_RTL() return false; } -bool Rover::verify_loiter_unlim() +bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { - // Continually increase the loiter time so it never finishes - loiter_start_time += loiter_duration; + // Continually set loiter start time to now so it never finishes + loiter_start_time += millis(); + verify_nav_wp(cmd); return false; }