Rover: Implemented loitering at a waypoint if Param1 is non-zero

Rover now honours the Param1 setting of a time in seconds for a
NAV_WAYPOINT and the Rover will loiter at that waypoint for that
period of time.
Note that as soon as the Rover reaches that waypoint the loiter timer
will start. If you enter a different mode during this time (HOLD for
instance) the timer resets. If you then switch back to AUTO
mode and the Rover returns to that waypoint it will wait for the
loiter time configured in param1.

APMrover2/Rover.h

@@ -358,6 +358,12 @@ private:
     static const AP_Param::Info var_info[];
     static const LogStructure log_structure[];
 
+    // Loiter control
+    uint16_t loiter_time_max; // How long we should loiter at the nav_waypoint (time in seconds)
+    uint32_t loiter_time;     // How long have we been loitering - The start time in millis
+
+    float distance_past_wp; // record the distance we have gone past the wp
+
 private:
     // private member functions
     void ahrs_update();

APMrover2/Steering.cpp

@@ -85,8 +85,10 @@ bool Rover::use_pivot_steering(void)
   calculate the throtte for auto-throttle modes
  */
 void Rover::calc_throttle(float target_speed)
-{  
-    if (!auto_check_trigger()) {
+{
+    // If not autostarting OR we are loitering at a waypoint
+    // then set the throttle to minimum
+    if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) {
         channel_throttle->servo_out = g.throttle_min.get();
         return;
     }
@@ -196,6 +198,12 @@ void Rover::calc_lateral_acceleration()
  */
 void Rover::calc_nav_steer()
 {
+    // check to see if the rover is loitering
+    if ((loiter_time > 0) && (control_mode == AUTO)) {
+        channel_steer->servo_out = 0;
+        return;
+    }
+
     // add in obstacle avoidance
     lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
 

APMrover2/commands_logic.cpp

@@ -176,6 +176,14 @@ void Rover::do_RTL(void)
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
+    // this will be used to remember the time in millis after we reach or pass the WP.
+    loiter_time = 0;
+    // this is the delay, stored in seconds
+    loiter_time_max = abs(cmd.p1);
+
+    // 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);
 }
 
@@ -185,17 +193,45 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-        gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
-                          (unsigned)cmd.index,
-                          (unsigned)get_distance(current_loc, next_WP));
+        // Check if we need to loiter at this waypoint
+        if (loiter_time_max > 0) {
+            if (loiter_time == 0) {  // check if we are just starting loiter
+                gcs_send_text_fmt(PSTR("Reached Waypoint #%i - Loiter for %i seconds"),
+                                  (unsigned)cmd.index,
+                                  (unsigned)loiter_time_max);
+                // record the current time i.e. start timer
+                loiter_time = millis();
+            }
+            // Check if we have loiter long enough
+            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+                return false;
+            }
+        } else {
+            gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
+                              (unsigned)cmd.index,
+                              (unsigned)get_distance(current_loc, next_WP));
+        }
         return true;
     }
 
     // have we gone past the waypoint?
+    // We should always go through the waypoint i.e. the above code
+    // first before we go past it.
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
-        gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
-                          (unsigned)cmd.index,
-                          (unsigned)get_distance(current_loc, next_WP));
+        // check if we have gone futher past the wp then last time and output new message if we have
+        if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) {
+            distance_past_wp = get_distance(current_loc, next_WP);
+            gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
+                              (unsigned)cmd.index,
+                              (unsigned)distance_past_wp);
+        }
+        // Check if we need to loiter at this waypoint
+        if (loiter_time_max > 0) {
+            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+                return false;
+            }
+        }
+        distance_past_wp = 0;
         return true;
     }
 

APMrover2/system.cpp

@@ -277,6 +277,11 @@ void Rover::set_mode(enum mode mode)
 		// don't switch modes if we are already in the correct mode.
 		return;
 	}
+
+    // If we are changing out of AUTO mode reset the loiter timer
+    if (control_mode == AUTO)
+        loiter_time = 0;
+
 	control_mode = mode;
     throttle_last = 0;
     throttle = 500;