Rover: Implemented MAV_CMD_NAV_LOITER_UNLIM

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);

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
 /********************************************************************************/