Rover: Loiter in AUTO mode for boat

APMrover2/Rover.cpp

@@ -33,7 +33,6 @@ Rover::Rover(void) :
     nav_controller(&L1_controller),
     control_mode(&mode_initializing),
     home(ahrs.get_home()),
-    G_Dt(0.02f),
-    mode_auto(mode_rtl)
+    G_Dt(0.02f)
 {
 }

APMrover2/mode.h

@@ -244,9 +244,6 @@ class ModeAuto : public Mode
 {
 public:
 
-    // constructor
-    ModeAuto(ModeRTL& mode_rtl);
-
     uint32_t mode_number() const override { return AUTO; }
     const char *name4() const override { return "AUTO"; }
 
@@ -284,16 +281,14 @@ protected:
     enum AutoSubMode {
         Auto_WP,                // drive to a given location
         Auto_HeadingAndSpeed,   // turn to a given heading
-        Auto_RTL                // perform RTL within auto mode
+        Auto_RTL,               // perform RTL within auto mode
+        Auto_Loiter             // perform Loiter within auto mode
     } _submode;
 
 private:
 
     bool check_trigger(void);
 
-    // references
-    ModeRTL& _mode_rtl;
-
     // this is set to true when auto has been triggered to start
     bool auto_triggered;
 
@@ -320,6 +315,8 @@ private:
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_set_reverse(const AP_Mission::Mission_Command& cmd);
 
+    bool start_loiter();
+
     enum Mis_Done_Behave {
         MIS_DONE_BEHAVE_HOLD      = 0,
         MIS_DONE_BEHAVE_LOITER    = 1

APMrover2/mode_auto.cpp

@@ -1,12 +1,6 @@
 #include "mode.h"
 #include "Rover.h"
 
-// constructor
-ModeAuto::ModeAuto(ModeRTL& mode_rtl) :
-    _mode_rtl(mode_rtl)
-{
-}
-
 bool ModeAuto::_enter()
 {
     // fail to enter auto if no mission commands
@@ -50,14 +44,20 @@ void ModeAuto::update()
                 _reached_destination = true;
             }
             // determine if we should keep navigating
-            if (!_reached_destination || (rover.is_boat() && !near_wp)) {
+            if (!_reached_destination) {
                 // continue driving towards destination
                 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, true);
             } else {
-                // we have reached the destination so stop
+                // we have reached the destination so stay here
+                if (rover.is_boat()) {
+                    if (!start_loiter()) {
                         stop_vehicle();
                     }
+                } else {
+                    stop_vehicle();
+                }
+            }
             break;
         }
 
@@ -70,13 +70,24 @@ void ModeAuto::update()
                 // check if we have reached within 5 degrees of target
                 _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
             } else {
+                // we have reached the destination so stay here
+                if (rover.is_boat()) {
+                    if (!start_loiter()) {
                         stop_vehicle();
                     }
+                } else {
+                    stop_vehicle();
+                }
+            }
             break;
         }
 
         case Auto_RTL:
-            _mode_rtl.update();
+            rover.mode_rtl.update();
+            break;
+        
+        case Auto_Loiter:
+            rover.mode_loiter.update();
             break;
     }
 }
@@ -85,7 +96,7 @@ void ModeAuto::update()
 float ModeAuto::get_distance_to_destination() const
 {
     if (_submode == Auto_RTL) {
-        return _mode_rtl.get_distance_to_destination();
+        return rover.mode_rtl.get_distance_to_destination();
     }
     return _distance_to_destination;
 }
@@ -106,7 +117,7 @@ bool ModeAuto::reached_destination()
         return _reached_destination;
     }
     if (_submode == Auto_RTL) {
-        return _mode_rtl.reached_destination();
+        return rover.mode_rtl.reached_destination();
     }
     // we should never reach here but just in case, return true to allow missions to continue
     return true;
@@ -135,7 +146,7 @@ bool ModeAuto::reached_heading()
 // start RTL (within auto)
 void ModeAuto::start_RTL()
 {
-    if (_mode_rtl.enter()) {
+    if (rover.mode_rtl.enter()) {
         _submode = Auto_RTL;
     }
 }
@@ -194,3 +205,12 @@ void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoida
     }
     Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
 }
+
+bool ModeAuto::start_loiter()
+{
+    if (rover.mode_loiter.enter()) {
+        _submode = Auto_Loiter;
+        return true;
+    }
+    return false;
+}