From a9781686b17c925b504cc6b5592c3f56fd2d431e Mon Sep 17 00:00:00 2001
From: TsuyoshiKawamura <kawamura@tajisoft.jp>
Date: Tue, 4 Dec 2018 04:25:52 +0900
Subject: [PATCH] Rover: Vehicle will stop with loiter in Guided mode for boat.

---
 APMrover2/mode.h          |  6 +++++-
 APMrover2/mode_guided.cpp | 35 +++++++++++++++++++++++++++++++----
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index 53daff00e2..03b6f9e271 100644
--- a/APMrover2/mode.h
+++ b/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;
diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp
index 0d8fe86def..2a5591b157 100644
--- a/APMrover2/mode_guided.cpp
+++ b/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 {
-                stop_vehicle();
+                if (rover.is_boat() && !start_loiter()) {
+                    stop_vehicle();
+                } else {
+                    stop_vehicle();
+                }
             }
             break;
         }
@@ -48,7 +52,11 @@ void ModeGuided::update()
                 calc_steering_to_heading(_desired_yaw_cd);
                 calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
             } else {
-                stop_vehicle();
+                if (rover.is_boat() && !start_loiter()) {
+                    stop_vehicle();
+                } else {
+                    stop_vehicle();
+                }
             }
             break;
         }
@@ -69,11 +77,21 @@ void ModeGuided::update()
                 g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed, true, true);
             } else {
-                stop_vehicle();
+                if (rover.is_boat() && !start_loiter()) {
+                    stop_vehicle();
+                } else {
+                    stop_vehicle();
+                }
             }
             break;
         }
 
+        case Guided_Loiter:
+        {
+            rover.mode_loiter.update();
+            break;
+        }
+
         default:
             gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
             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;
+}