From 65e4ee9e2b0394871bef7af33d7a35b91c67d48f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 5 Oct 2019 09:20:51 +0900
Subject: [PATCH] Rover: minor restructure of loiter at end of rtl and srtl

---
 APMrover2/mode.h             | 11 ++---------
 APMrover2/mode_rtl.cpp       | 30 +++++++++++-------------------
 APMrover2/mode_smart_rtl.cpp | 26 +++++++++-----------------
 3 files changed, 22 insertions(+), 45 deletions(-)

diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index ae76591bd5..e9ded46aea 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -518,16 +518,12 @@ 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
+    bool _loitering;        // true if loitering at end of RTL
 
 };
 
@@ -554,9 +550,6 @@ 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
@@ -569,7 +562,7 @@ protected:
 
     bool _enter() override;
     bool _load_point;
-    bool _srtl_loiter;
+    bool _loitering;        // true if loitering at end of SRTL
 };
 
    
diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp
index 42c9047bd6..8a48284252 100644
--- a/APMrover2/mode_rtl.cpp
+++ b/APMrover2/mode_rtl.cpp
@@ -28,7 +28,7 @@ bool ModeRTL::_enter()
     }
 
     sent_notification = false;
-
+    _loitering = false;
     return true;
 }
 
@@ -46,20 +46,22 @@ void ModeRTL::update()
         }
 
         // we have reached the destination
-        // boats loiters, rovers stop
+        // boats loiter, rovers stop
         if (!rover.is_boat()) {
             stop_vehicle();
         } else {
             // if not loitering yet, start loitering
-            if (!_rtl_loiter) {
-                if (!start_loiter()) {
-                    stop_vehicle();
-                }
+            if (!_loitering) {
+                _loitering = rover.mode_loiter.enter();
+            }
+            // update stop or loiter
+            if (_loitering) {
+                rover.mode_loiter.update();
+            } else {
+                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());
     }
@@ -79,13 +81,3 @@ 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;
-}
diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp
index 26f6eb87bc..f5e1f719e8 100644
--- a/APMrover2/mode_smart_rtl.cpp
+++ b/APMrover2/mode_smart_rtl.cpp
@@ -28,6 +28,7 @@ bool ModeSmartRTL::_enter()
 
     // init state
     smart_rtl_state = SmartRTL_WaitForPathCleanup;
+    _loitering = false;
 
     return true;
 }
@@ -80,14 +81,15 @@ void ModeSmartRTL::update()
             if (!rover.is_boat()) {
                stop_vehicle();
             } else {
-               // if not loitering yet, start loitering
-               if (!_srtl_loiter) {
-                    if (!start_loiter()) {
-                        stop_vehicle();
-                    }
+                // if not loitering yet, start loitering
+                if (!_loitering) {
+                    _loitering = rover.mode_loiter.enter();
+                }
+                if (_loitering) {
+                    rover.mode_loiter.update();
+                } else {
+                    stop_vehicle();
                }
-            // otherwise update the loiter
-            rover.mode_loiter.update();
             }
             break;
     }
@@ -119,13 +121,3 @@ 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;
-}