Plane: gracefully-ish fail set_mode(qmode)

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;
 

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;
     }