Rover: change RTL/SRTL from circling to loitering for boats

APMrover2/mode.h

@@ -518,11 +518,17 @@ public:
     float get_distance_to_destination() const override { return _distance_to_destination; }
     bool reached_destination() const override;
 
+    // vehicle start loiter
+    bool start_loiter();
+
 protected:
 
     bool _enter() override;
 
     bool sent_notification; // used to send one time notification to ground station
+
+    bool _rtl_loiter;       //if loitering at end of RTL
+
 };
 
 class ModeSmartRTL : public Mode
@@ -548,6 +554,9 @@ public:
     // save current position for use by the smart_rtl flight mode
     void save_position();
 
+    // vehicle start loiter
+    bool start_loiter();
+
 protected:
 
     // Safe RTL states
@@ -560,9 +569,11 @@ protected:
 
     bool _enter() override;
     bool _load_point;
+    bool _srtl_loiter;
 };
 
    
+
 class ModeSteering : public Mode
 {
 public:

APMrover2/mode_rtl.cpp

@@ -46,13 +46,20 @@ void ModeRTL::update()
         }
 
         // we have reached the destination
-        // boats keep navigating, rovers stop
-        if (rover.is_boat()) {
-            navigate_to_waypoint();
+        // boats loiters, rovers stop
+        if (!rover.is_boat()) {
+            stop_vehicle();
         } else {
+            // if not loitering yet, start loitering
+            if (!_rtl_loiter) {
+                if (!start_loiter()) {
                     stop_vehicle();
                 }
+            }
 
+            // update the loiter
+            rover.mode_loiter.update();
+        }
         // update distance to destination
         _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
     }
@@ -72,3 +79,13 @@ bool ModeRTL::reached_destination() const
 {
     return g2.wp_nav.reached_destination();
 }
+
+bool ModeRTL::start_loiter()
+{
+    if (rover.mode_loiter.enter()) {
+        _rtl_loiter = true;
+        return true;
+    }
+    _rtl_loiter= false;
+    return false;
+}

APMrover2/mode_smart_rtl.cpp

@@ -75,12 +75,19 @@ void ModeSmartRTL::update()
         case SmartRTL_StopAtHome:
         case SmartRTL_Failure:
             _reached_destination = true;
-            if (rover.is_boat()) {
-                // boats attempt to hold position at home
-                navigate_to_waypoint();
-            } else {
-                // rovers stop
+            // we have reached the destination
+            // boats loiters, rovers stop
+            if (!rover.is_boat()) {
                stop_vehicle();
+            } else {
+               // if not loitering yet, start loitering
+               if (!_srtl_loiter) {
+                    if (!start_loiter()) {
+                        stop_vehicle();
+                    }
+               }
+            // otherwise update the loiter
+            rover.mode_loiter.update();
             }
             break;
     }
@@ -112,3 +119,13 @@ void ModeSmartRTL::save_position()
     const bool save_pos = (rover.control_mode != &rover.mode_smartrtl);
     g2.smart_rtl.update(true, save_pos);
 }
+
+bool ModeSmartRTL::start_loiter()
+{
+    if (rover.mode_loiter.enter()) {
+        _srtl_loiter = true;
+        return true;
+    }
+    _srtl_loiter= false;
+    return false;
+}