Rover: Vehicle will stop with loiter in Guided mode for boat.

APMrover2/mode.h

@@ -361,12 +361,16 @@ public:
     void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
     void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
 
+    // vehicle start loiter
+    bool start_loiter();
+
 protected:
 
     enum GuidedMode {
         Guided_WP,
         Guided_HeadingAndSpeed,
-        Guided_TurnRateAndSpeed
+        Guided_TurnRateAndSpeed,
+        Guided_Loiter
     };
 
     bool _enter() override;

APMrover2/mode_guided.cpp

@@ -26,12 +26,16 @@ void ModeGuided::update()
                 rover.gcs().send_mission_item_reached_message(0);
             }
             // determine if we should keep navigating
-            if (!_reached_destination || (rover.is_boat() && !near_wp)) {
+            if (!_reached_destination) {
                 // drive 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 {
+                if (rover.is_boat() && !start_loiter()) {
                     stop_vehicle();
+                } else {
+                    stop_vehicle();
+                }
             }
             break;
         }
@@ -47,9 +51,13 @@ void ModeGuided::update()
                 // run steering and throttle controllers
                 calc_steering_to_heading(_desired_yaw_cd);
                 calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
+            } else {
+                if (rover.is_boat() && !start_loiter()) {
+                    stop_vehicle();
                 } else {
                     stop_vehicle();
                 }
+            }
             break;
         }
 
@@ -68,9 +76,19 @@ void ModeGuided::update()
                                                                             rover.G_Dt);
                 g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed, true, true);
+            } else {
+                if (rover.is_boat() && !start_loiter()) {
+                    stop_vehicle();
                 } else {
                     stop_vehicle();
                 }
+            }
+            break;
+        }
+
+        case Guided_Loiter:
+        {
+            rover.mode_loiter.update();
             break;
         }
 
@@ -147,3 +165,12 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
     // log new target
     rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
 }
+
+bool ModeGuided::start_loiter()
+{
+    if (rover.mode_loiter.enter()) {
+        _guided_mode = Guided_Loiter;
+        return true;
+    }
+    return false;
+}