From d1799884b2ddc0a59f6f7f50a5d898a6a1e86470 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Thu, 14 Jul 2016 11:34:22 +1000 Subject: [PATCH] Rover: Implemented MAV_CMD_NAV_LOITER_UNLIM --- APMrover2/Rover.h | 2 ++ APMrover2/commands_logic.cpp | 23 +++++++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index a40614c6b5..64fe5a5921 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -502,7 +502,9 @@ private: bool verify_command(const AP_Mission::Mission_Command& cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); + void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); + bool verify_loiter_unlim(); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); void do_change_speed(const AP_Mission::Mission_Command& cmd); diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index f023a112a7..3581fd1690 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -31,6 +31,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) do_RTL(); break; + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely + do_loiter_unlimited(cmd); + break; + // Conditional commands case MAV_CMD_CONDITION_DELAY: do_wait_delay(cmd); @@ -153,6 +157,9 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_RETURN_TO_LAUNCH: return verify_RTL(); + case MAV_CMD_NAV_LOITER_UNLIM: + return verify_loiter_unlim(); + case MAV_CMD_CONDITION_DELAY: return verify_wait_delay(); @@ -194,6 +201,15 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd) set_next_WP(cmd.content.location); } +void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +{ + Location cmdloc = cmd.content.location; + location_sanitize(current_loc, cmdloc); + set_next_WP(cmdloc); + loiter_time_max = 100; // an arbitrary large loiter time + distance_past_wp = 0; +} + /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ @@ -264,6 +280,13 @@ bool Rover::verify_RTL() return false; } +bool Rover::verify_loiter_unlim() +{ + // Continually increase the loiter time so it never finishes + loiter_time += loiter_time_max; + return false; +} + /********************************************************************************/ // Condition (May) commands /********************************************************************************/