From fbe483ccac48e5248d392a2c700a45680e5216de Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Mon, 1 Apr 2019 10:54:34 -0700
Subject: [PATCH] Plane: gracefully-ish fail set_mode(qmode)

---
 ArduPlane/mode.h     | 17 +++++++++++++++++
 ArduPlane/system.cpp | 14 +++++++++++++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 0495a30193..464616dbcd 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -62,6 +62,9 @@ public:
     // convert user input to targets, implement high level control for this mode
     virtual void update() = 0;
 
+    // true for all q modes
+    virtual bool is_vtol_mode() const { return false; }
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -326,6 +329,8 @@ public:
     const char *name() const override { return "QSTABILIZE"; }
     const char *name4() const override { return "QSTB"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -344,6 +349,8 @@ public:
     const char *name() const override { return "QHOVER"; }
     const char *name4() const override { return "QHOV"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -360,6 +367,8 @@ public:
     const char *name() const override { return "QLOITER"; }
     const char *name4() const override { return "QLOT"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -376,6 +385,8 @@ public:
     const char *name() const override { return "QLAND"; }
     const char *name4() const override { return "QLND"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -392,6 +403,8 @@ public:
     const char *name() const override { return "QRTL"; }
     const char *name4() const override { return "QRTL"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -408,6 +421,8 @@ public:
     const char *name() const override { return "QACO"; }
     const char *name4() const override { return "QACRO"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -424,6 +439,8 @@ public:
     const char *name() const override { return "QAUTOTUNE"; }
     const char *name4() const override { return "QATN"; }
 
+    bool is_vtol_mode() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 15cb97c425..97ece4d2eb 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -245,13 +245,15 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
 #if !QAUTOTUNE_ENABLED
     if (&new_mode == plane.mode_qautotune) {
         gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled");
-        new_mode = plane.mode_qhover;
+        set_mode(plane.mode_qhover);
+        return;
     }
 #endif
 
     // backup current control_mode and previous_mode
     Mode &old_previous_mode = *previous_mode;
     Mode &old_mode = *control_mode;
+    const mode_reason_t previous_mode_reason_backup = previous_mode_reason;
 
     // update control_mode assuming success
     // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
@@ -268,6 +270,16 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
         // we failed entering new mode, roll back to old
         previous_mode = &old_previous_mode;
         control_mode = &old_mode;
+
+        control_mode_reason = previous_mode_reason;
+        previous_mode_reason = previous_mode_reason_backup;
+
+        // currently, only Q modes can fail enter(). This will likely change in the future and all modes
+        // should be changed to check dependencies and fail early before depending on changes in Mode::set_mode()
+        if (control_mode->is_vtol_mode()) {
+            // ignore result because if we fail we risk looping at the qautotune check above
+            control_mode->enter();
+        }
         return false;
     }