From b60cb536abafd92673273c24c58451539edc0806 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 22 Nov 2017 21:38:57 +0900
Subject: [PATCH] Rover: RTL within auto mode

---
 APMrover2/Rover.cpp          |  3 ++-
 APMrover2/commands_logic.cpp |  5 +++--
 APMrover2/mode.h             | 15 ++++++++++++++-
 APMrover2/mode_auto.cpp      | 21 +++++++++++++++++++++
 4 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp
index 3c35e091f4..02402a6c98 100644
--- a/APMrover2/Rover.cpp
+++ b/APMrover2/Rover.cpp
@@ -32,6 +32,7 @@ Rover::Rover(void) :
     nav_controller(&L1_controller),
     control_mode(&mode_initializing),
     home(ahrs.get_home()),
-    G_Dt(0.02f)
+    G_Dt(0.02f),
+    mode_auto(mode_rtl)
 {
 }
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index e169a269b2..bf34cce640 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -211,7 +211,8 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 
 void Rover::do_RTL(void)
 {
-    set_mode(mode_rtl, MODE_REASON_MISSION_COMMAND);
+    // start rtl in auto mode
+    mode_auto.start_RTL();
 }
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest)
@@ -307,7 +308,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 
 bool Rover::verify_RTL()
 {
-    return mode_rtl.reached_destination();
+    return mode_auto.reached_destination();
 }
 
 bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index df019a9f99..76b3914d33 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -6,6 +6,9 @@
 
 #define MODE_NEXT_HEADING_UNKNOWN   99999.0f    // used to indicate to set_desired_location method that next leg's heading is unknown
 
+// pre-define ModeRTL so Auto can appear higher in this file
+class ModeRTL;
+
 class Mode
 {
 public:
@@ -142,6 +145,9 @@ class ModeAuto : public Mode
 {
 public:
 
+    // constructor
+    ModeAuto(ModeRTL& mode_rtl);
+
     uint32_t mode_number() const override { return AUTO; }
 
     // methods that affect movement of the vehicle in this mode
@@ -164,6 +170,9 @@ public:
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
     bool reached_heading();
 
+    // start RTL (within auto)
+    void start_RTL();
+
     // execute the mission in reverse (i.e. backing up)
     void set_reversed(bool value);
 
@@ -174,13 +183,17 @@ protected:
 
     enum AutoSubMode {
         Auto_WP,                // drive to a given location
-        Auto_HeadingAndSpeed    // turn to a given heading
+        Auto_HeadingAndSpeed,   // turn to a given heading
+        Auto_RTL                // perform RTL 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;
 
diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp
index b3351104c0..696eda7aa0 100644
--- a/APMrover2/mode_auto.cpp
+++ b/APMrover2/mode_auto.cpp
@@ -1,6 +1,12 @@
 #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
@@ -75,6 +81,10 @@ void ModeAuto::update()
             }
             break;
         }
+
+        case Auto_RTL:
+            _mode_rtl.update();
+            break;
     }
 }
 
@@ -94,6 +104,9 @@ bool ModeAuto::reached_destination()
     if (_submode == Auto_WP) {
         return _reached_destination;
     }
+    if (_submode == Auto_RTL) {
+        return _mode_rtl.reached_destination();
+    }
     // we should never reach here but just in case, return true to allow missions to continue
     return true;
 }
@@ -118,6 +131,14 @@ bool ModeAuto::reached_heading()
     return true;
 }
 
+// start RTL (within auto)
+void ModeAuto::start_RTL()
+{
+    if (_mode_rtl.enter()) {
+        _submode = Auto_RTL;
+    }
+}
+
 // execute the mission in reverse (i.e. backing up)
 void ModeAuto::set_reversed(bool value)
 {