Rover: boats say active at destination in auto

previously the user would use the LOITER_UNLIMITED or LOITER_TIME mission commands to specify that the vehicle should remain active at the destination.  This was cumbersome and not the best way to specify this behaviour because these two commands are valid for regular rovers that should not try to remain active at the destination.

APMrover2/Rover.h

@@ -423,9 +423,7 @@ private:
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);
     void do_RTL(void);
-    void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest);
-    void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
-    void do_loiter_time(const AP_Mission::Mission_Command& cmd);
+    void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
     void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_RTL();

APMrover2/commands_logic.cpp

@@ -32,12 +32,9 @@ 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;
-
-    case MAV_CMD_NAV_LOITER_TIME:
-        do_loiter_time(cmd);
+    case MAV_CMD_NAV_LOITER_UNLIM:  // Loiter indefinitely
+    case MAV_CMD_NAV_LOITER_TIME:   // Loiter for specified time
+        do_nav_wp(cmd, true);
         break;
 
     case MAV_CMD_NAV_SET_YAW_SPEED:
@@ -216,7 +213,7 @@ void Rover::do_RTL(void)
     mode_auto.start_RTL();
 }
 
-void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest)
+void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
 {
     // just starting so we haven't previously reached the waypoint
     previously_reached_wp = false;
@@ -230,27 +227,14 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_a
     // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
     // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
     float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN;
-    if (loiter_duration == 0) {
+    if (!always_stop_at_destination && loiter_duration == 0) {
         next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN);
     }
 
     // retrieve and sanitize target location
     Location cmdloc = cmd.content.location;
     location_sanitize(current_loc, cmdloc);
-    mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd, stay_active_at_dest);
-}
-
-void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
-{
-    do_nav_wp(cmd, true);
-}
-
-// 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)
-{
-    do_nav_wp(cmd, true);
+    mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd);
 }
 
 // do_set_yaw_speed - turn to a specified heading and achieve and given speed

APMrover2/mode.h

@@ -200,8 +200,7 @@ public:
     float get_distance_to_destination() const override { return _distance_to_destination; }
 
     // set desired location, heading and speed
-    // set stay_active_at_dest if the vehicle should attempt to maintain it's position at the destination (mostly for boats)
-    void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN, bool stay_active_at_dest = false);
+    void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
     bool reached_destination() override;
 
     // heading and speed control
@@ -236,7 +235,6 @@ private:
     bool auto_triggered;
 
     bool _reached_heading;      // true when vehicle has reached desired heading in TurnToHeading sub mode
-    bool _stay_active_at_dest;  // true when we should actively maintain position even after reaching the destination
     bool _reversed;             // execute the mission by backing up
 };
 

APMrover2/mode_auto.cpp

@@ -53,11 +53,9 @@ void ModeAuto::update()
                     _reached_destination = true;
                 }
             }
-            // stay active at destination if caller requested this behaviour and outside the waypoint radius
-            bool active_at_destination = _reached_destination && _stay_active_at_dest && (_distance_to_destination > rover.g.waypoint_radius);
-            if (!_reached_destination || active_at_destination) {
+            if (!_reached_destination || rover.is_boat()) {
                 // continue driving towards destination
-                calc_steering_to_waypoint(active_at_destination ? rover.current_loc : _origin, _destination, _reversed);
+                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
                 calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true);
             } else {
                 // we have reached the destination so stop
@@ -90,13 +88,12 @@ void ModeAuto::update()
 }
 
 // set desired location to drive to
-void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd, bool stay_active_at_dest)
+void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {
     // call parent
     Mode::set_desired_location(destination, next_leg_bearing_cd);
 
     _submode = Auto_WP;
-    _stay_active_at_dest = stay_active_at_dest;
 }
 
 // return true if vehicle has reached or even passed destination