Rover: Station Keeping is implemented via NAV_LOITER_COMMAND

It is a very simply form of station keeping.  If a boat is
loitering on a waypoint and it gets blown off a distance
past the WAYPOINT_RADIUS it will automaticlly drive back to
the waypoint.

APMrover2/Rover.h

@@ -363,9 +363,11 @@ private:
     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
+    uint16_t loiter_duration; // How long we should loiter at the nav_waypoint (time in seconds)
+    uint32_t loiter_start_time;     // How long have we been loitering - The start time in millis
+    bool active_loiter; // TRUE if we actively return to the loitering waypoint if we drift off
     float distance_past_wp; // record the distance we have gone past the wp
+    bool previously_reached_wp;  // set to true if we have EVER reached the waypoint
 
     // time that rudder/steering arming has been running
     uint32_t rudder_arm_timer;
@@ -522,8 +524,10 @@ private:
     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);
+    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_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);
     void do_change_speed(const AP_Mission::Mission_Command& cmd);
@@ -540,6 +544,9 @@ private:
     void update_home();
     void accel_cal_update(void);
     void nav_set_yaw_speed();
+    bool in_stationary_loiter(void);
+    void set_loiter_active(const AP_Mission::Mission_Command& cmd);
+
 public:
     bool print_log_menu(void);
     int8_t dump_log(uint8_t argc, const Menu::arg *argv);

APMrover2/Steering.cpp

@@ -75,6 +75,22 @@ bool Rover::use_pivot_steering(void) {
     return false;
 }
 
+/*
+  test if we are loitering AND should be stopped at a waypoint
+*/
+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;
+        }
+    }
+
+    return false;
+}
 
 /*
     calculate the throtte for auto-throttle modes
@@ -82,7 +98,7 @@ bool Rover::use_pivot_steering(void) {
 void Rover::calc_throttle(float target_speed) {
     // 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))) {
+    if (!auto_check_trigger() || in_stationary_loiter()) {
         channel_throttle->set_servo_out(g.throttle_min.get());
         // Stop rotation in case of loitering and skid steering
         if (g.skid_steer_out) {
@@ -163,7 +179,13 @@ void Rover::calc_throttle(float target_speed) {
 void Rover::calc_lateral_acceleration() {
     switch (control_mode) {
     case AUTO:
-        nav_controller->update_waypoint(prev_WP, next_WP);
+        // If we have reached the waypoint previously navigate
+        // back to it from our current position
+        if (previously_reached_wp && (loiter_duration > 0)) {
+            nav_controller->update_waypoint(current_loc, next_WP);
+        } else {
+            nav_controller->update_waypoint(prev_WP, next_WP);
+        }
         break;
 
     case RTL:
@@ -195,7 +217,7 @@ 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)) {
+    if (in_stationary_loiter()) {
         channel_steer->set_servo_out(0);
         return;
     }

APMrover2/commands_logic.cpp

@@ -33,6 +33,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
         do_loiter_unlimited(cmd);
         break;
 
+    case MAV_CMD_NAV_LOITER_TIME:
+        do_loiter_time(cmd);
+        break;
+
     // Conditional commands
     case MAV_CMD_CONDITION_DELAY:
         do_wait_delay(cmd);
@@ -167,6 +171,9 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlim();
 
+    case MAV_CMD_NAV_LOITER_TIME:
+        return verify_loiter_time(cmd);
+
     case MAV_CMD_CONDITION_DELAY:
         return verify_wait_delay();
 
@@ -210,10 +217,14 @@ void Rover::do_RTL(void)
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
+    // just starting so we haven't previously reached the waypoint
+    previously_reached_wp = false;
+
     // this will be used to remember the time in millis after we reach or pass the WP.
-    loiter_time = 0;
+    loiter_start_time = 0;
+
     // this is the delay, stored in seconds
-    loiter_time_max = cmd.p1;
+    loiter_duration = cmd.p1;
 
     // this is the distance we travel past the waypoint - not there yet so 0 initially
     distance_past_wp = 0;
@@ -223,30 +234,46 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 
 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);
-    loiter_time_max = 100; // an arbitrary large loiter time
+    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
+// if the vehicle is moved off the loiter point (i.e. a boat in a current)
+// then the vehicle will actively return to the loiter coords.
+void Rover::do_loiter_time(const AP_Mission::Mission_Command& cmd)
+{
+    active_loiter = true;
+    do_nav_wp(cmd);
+}
+
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
 bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
+    // Have we reached the waypoint i.e. are we within waypoint_radius of the waypoint
     if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-        // 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
+        // check if we are loitering at this waypoint - the message sent to the GCS is different
+        if (loiter_duration > 0) {
+            // Check if this is the first time we have reached the waypoint
+            if (!previously_reached_wp) {
                 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
                                   (unsigned)cmd.index,
-                                  (unsigned)loiter_time_max);
+                                  (unsigned)loiter_duration);
                 // record the current time i.e. start timer
-                loiter_time = millis();
+                loiter_start_time = millis();
+                previously_reached_wp = true;
             }
+
+            distance_past_wp = wp_distance;
+
             // Check if we have loiter long enough
-            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+            if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
                 return false;
             }
         } else {
@@ -254,27 +281,43 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
                               (unsigned)cmd.index,
                               (unsigned)get_distance(current_loc, next_WP));
         }
+        // set loiter_duration to 0 so we know we aren't or have finished loitering
+        loiter_duration = 0;
         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.
+    // first before we go past it but sometimes we don't.
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
-        // check if we have gone further 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);
+        // 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",
+                              (unsigned)cmd.index,
+                              (unsigned)loiter_duration);
+            // record the current time i.e. start timer
+            loiter_start_time = millis();
+            previously_reached_wp = true;
+            distance_past_wp = wp_distance;
+        }
+
+        // check if distance to the WP has changed and output new message if it has
+        float dist_to_wp = get_distance(current_loc, next_WP);
+        if ((uint32_t)distance_past_wp != (uint32_t)dist_to_wp) {
+            distance_past_wp = dist_to_wp;
             gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i. Distance %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) {
+        if (loiter_duration > 0) {
+            if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
                 return false;
             }
         }
-        distance_past_wp = 0;
+        // set loiter_duration to 0 so we know we aren't or have finished loitering
+        loiter_duration = 0;
         return true;
     }
 
@@ -303,10 +346,22 @@ bool Rover::verify_RTL()
 bool Rover::verify_loiter_unlim()
 {
     // Continually increase the loiter time so it never finishes
-    loiter_time += loiter_time_max;
+    loiter_start_time += loiter_duration;
     return false;
 }
 
+// verify_loiter_time - check if we have loitered long enough
+bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
+{
+    bool result = verify_nav_wp(cmd);
+    if (result) {
+        gcs_send_text(MAV_SEVERITY_WARNING, "Finished active loiter\n");
+        // if we have finished active loitering - turn it off
+        active_loiter = false;
+    }
+    return result;
+}
+
 void Rover::nav_set_yaw_speed()
 {
     // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt

APMrover2/system.cpp

@@ -271,7 +271,7 @@ void Rover::set_mode(enum mode mode)
 
     // If we are changing out of AUTO mode reset the loiter timer
     if (control_mode == AUTO) {
-        loiter_time = 0;
+        loiter_start_time = 0;
     }
 
     control_mode = mode;