From 5ba2bd675a1de20641130530ff74714770b6b634 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 6 Nov 2021 16:42:40 +0000
Subject: [PATCH] Plane: move quadplane arming checks to own function and add
 angle max and tiltrotor / tailsitter checks

---
 ArduPlane/AP_Arming.cpp | 104 ++++++++++++++++++++++++++--------------
 ArduPlane/AP_Arming.h   |   2 +
 2 files changed, 71 insertions(+), 35 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index 74f969ffd8..97bb652265 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -75,33 +75,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
     }
 
 #if HAL_QUADPLANE_ENABLED
-    if (plane.quadplane.enabled() && !plane.quadplane.available()) {
-        check_failed(display_failure, "Quadplane enabled but not running");
-        ret = false;
-    }
-
-    if (plane.quadplane.available() && plane.scheduler.get_loop_rate_hz() < 100) {
-        check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");
-        ret = false;
-    }
-
-    if (plane.quadplane.available() && !plane.quadplane.motors->initialised_ok()) {
-        check_failed(display_failure, "Quadplane: check motor setup");
-        ret = false;
-    }
-
-    if (plane.quadplane.enabled() && plane.quadplane.available()) {
-        // ensure controllers are OK with us arming:
-        char failure_msg[50];
-        if (!plane.quadplane.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
-            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
-            return false;
-        }
-        if (!plane.quadplane.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
-            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
-            return false;
-        }
-    }
+    ret &= quadplane_checks(display_failure);
 #endif
 
     if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
@@ -120,14 +94,6 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
         ret = false;
     }
 
-#if HAL_QUADPLANE_ENABLED
-    if (plane.quadplane.enabled() && ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) &&
-            !plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) {
-        check_failed(display_failure,"not in Q mode");
-        ret = false;
-    }
-#endif
-
     if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){
        int16_t trim = plane.channel_throttle->get_radio_trim();
        if (trim < 1250 || trim > 1750) {
@@ -139,6 +105,74 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
     return ret;
 }
 
+#if HAL_QUADPLANE_ENABLED
+bool AP_Arming_Plane::quadplane_checks(bool display_failure)
+{
+    if (!plane.quadplane.enabled()) {
+        return true;
+    }
+
+    if (!plane.quadplane.available()) {
+        check_failed(display_failure, "Quadplane enabled but not running");
+        return false;
+    }
+
+    bool ret = true;
+
+    if (plane.scheduler.get_loop_rate_hz() < 100) {
+        check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");
+        ret = false;
+    }
+
+    if (!plane.quadplane.motors->initialised_ok()) {
+        check_failed(display_failure, "Quadplane: check motor setup");
+        ret = false;
+    }
+
+    // lean angle parameter check
+    if (plane.quadplane.aparm.angle_max < 1000 || plane.quadplane.aparm.angle_max > 8000) {
+        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check Q_ANGLE_MAX");
+        ret = false;
+    }
+
+    if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.tiltrotor.enable > 0)) {
+        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set TAILSIT_ENABLE 0 or TILT_ENABLE 0");
+        ret = false;
+
+    } else {
+
+        if ((plane.quadplane.tailsitter.enable > 0) && !plane.quadplane.tailsitter.enabled()) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tailsitter setup not complete, reboot");
+            ret = false;
+        }
+
+        if ((plane.quadplane.tiltrotor.enable > 0) && !plane.quadplane.tiltrotor.enabled()) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tiltrotor setup not complete, reboot");
+            ret = false;
+        }
+    }
+
+    // ensure controllers are OK with us arming:
+    char failure_msg[50];
+    if (!plane.quadplane.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
+        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
+        ret = false;
+    }
+    if (!plane.quadplane.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
+        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
+        ret = false;
+    }
+
+    if (((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) &&
+            !plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) {
+        check_failed(display_failure,"not in Q mode");
+        ret = false;
+    }
+
+    return ret;
+}
+#endif // HAL_QUADPLANE_ENABLED
+
 bool AP_Arming_Plane::ins_checks(bool display_failure)
 {
     // call parent class checks
diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h
index 1a4d4f3628..02239458d3 100644
--- a/ArduPlane/AP_Arming.h
+++ b/ArduPlane/AP_Arming.h
@@ -33,6 +33,8 @@ public:
 protected:
     bool ins_checks(bool report) override;
 
+    bool quadplane_checks(bool display_failure);
+
 private:
     void change_arm_state(void);