From dd6ce1fa739f6ea253aec6ca12b692c3c8517480 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 2 Dec 2021 11:34:51 +0900
Subject: [PATCH] Rover: guided starts in stop submode

---
 Rover/mode.h          |  6 +++++-
 Rover/mode_guided.cpp | 26 ++++++++++++++++++++++----
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/Rover/mode.h b/Rover/mode.h
index 605b1c65fe..10bcb0c607 100644
--- a/Rover/mode.h
+++ b/Rover/mode.h
@@ -407,6 +407,9 @@ public:
     // vehicle start loiter
     bool start_loiter();
 
+    // start stopping
+    void start_stop();
+
     // guided limits
     void limit_set(uint32_t timeout_ms, float horiz_max);
     void limit_clear();
@@ -420,7 +423,8 @@ protected:
         Guided_HeadingAndSpeed,
         Guided_TurnRateAndSpeed,
         Guided_Loiter,
-        Guided_SteeringAndThrottle
+        Guided_SteeringAndThrottle,
+        Guided_Stop
     };
 
     bool _enter() override;
diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp
index cffe91b996..83c45599e9 100644
--- a/Rover/mode_guided.cpp
+++ b/Rover/mode_guided.cpp
@@ -3,11 +3,14 @@
 
 bool ModeGuided::_enter()
 {
-    // set desired location to reasonable stopping point
-    if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
-        return false;
+    // initialise submode to stop or loiter
+    if (rover.is_boat()) {
+        if (!start_loiter()) {
+            start_stop();
+        }
+    } else {
+        start_stop();
     }
-    _guided_mode = Guided_WP;
 
     // initialise waypoint speed
     g2.wp_nav.set_desired_speed_to_default();
@@ -129,6 +132,10 @@ void ModeGuided::update()
             break;
         }
 
+        case Guided_Stop:
+            stop_vehicle();
+            break;
+
         default:
             gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
             break;
@@ -147,6 +154,7 @@ float ModeGuided::get_distance_to_destination() const
     case Guided_Loiter:
         return rover.mode_loiter.get_distance_to_destination();
     case Guided_SteeringAndThrottle:
+    case Guided_Stop:
         return 0.0f;
     }
 
@@ -164,6 +172,7 @@ bool ModeGuided::reached_destination() const
     case Guided_TurnRateAndSpeed:
     case Guided_Loiter:
     case Guided_SteeringAndThrottle:
+    case Guided_Stop:
         return true;
     }
 
@@ -188,6 +197,7 @@ bool ModeGuided::set_desired_speed(float speed)
     case Guided_Loiter:
         return rover.mode_loiter.set_desired_speed(speed);
     case Guided_SteeringAndThrottle:
+    case Guided_Stop:
         // no speed control
         return false;
     }
@@ -212,6 +222,7 @@ bool ModeGuided::get_desired_location(Location& destination) const
         // get destination from loiter
         return rover.mode_loiter.get_desired_location(destination);
     case Guided_SteeringAndThrottle:
+    case Guided_Stop:
         // no desired location in this submode
         break;
     }
@@ -298,6 +309,13 @@ bool ModeGuided::start_loiter()
     return false;
 }
 
+
+// start stopping vehicle as quickly as possible
+void ModeGuided::start_stop()
+{
+    _guided_mode = Guided_Stop;
+}
+
 // set guided timeout and movement limits
 void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max)
 {