From 629f2153e096ad00a035f7ecaed5864cc32bfbf3 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Tue, 16 Jun 2020 12:52:26 -0600
Subject: [PATCH] Plane: move arming delay logic into AP_Arming_Plane

---
 ArduPlane/AP_Arming.cpp | 13 ++++++++++++-
 ArduPlane/AP_Arming.h   |  5 +++++
 ArduPlane/quadplane.cpp | 16 ++++++----------
 ArduPlane/quadplane.h   |  1 +
 4 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index 4a528b9735..640501c44b 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -4,6 +4,8 @@
 #include "AP_Arming.h"
 #include "Plane.h"
 
+constexpr uint32_t AP_ARMING_DELAY_MS = 2000; // delay from arming to start of motor spoolup
+
 const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
     // variables from parent vehicle
     AP_NESTEDGROUPINFO(AP_Arming, 0),
@@ -187,7 +189,9 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
     }
 
     change_arm_state();
-    plane.quadplane.delay_arming = true;
+
+    // rising edge of delay_arming oneshot
+    delay_arming = true;
 
     gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed");
 
@@ -246,5 +250,12 @@ void AP_Arming_Plane::update_soft_armed()
     hal.util->set_soft_armed(is_armed() &&
                              hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
     AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
+
+    // update delay_arming oneshot
+    if (delay_arming &&
+        (AP_HAL::millis() - hal.util->get_last_armed_change() >= AP_ARMING_DELAY_MS)) {
+
+        delay_arming = false;
+    }
 }
 
diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h
index 1bb12213cc..298dd9d869 100644
--- a/ArduPlane/AP_Arming.h
+++ b/ArduPlane/AP_Arming.h
@@ -28,10 +28,15 @@ public:
     bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
 
     void update_soft_armed();
+    bool get_delay_arming() { return delay_arming; };
 
 protected:
     bool ins_checks(bool report) override;
 
 private:
     void change_arm_state(void);
+
+    // oneshot with duration AP_ARMING_DELAY_MS used by quadplane to delay spoolup after arming:
+    // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
+    bool delay_arming;
 };
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index c39e6b0e04..5f939aa833 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -483,6 +483,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @Range: 0.1 1
     // @User: Standard
     AP_GROUPINFO("TAILSIT_GSCMIN", 18, QuadPlane, tailsitter.gain_scaling_min, 0.4),
+
     // @Param: ASSIST_DELAY
     // @DisplayName: Quadplane assistance delay
     // @Description: This is delay between the assistance thresholds being met and the assistance starting.
@@ -2010,16 +2011,11 @@ void QuadPlane::motors_output(bool run_rate_controller)
        2) to allow motors to return to vertical (OPTION_DISARMED_TILT)
      */
     if ((options & OPTION_DISARMED_TILT) || (options & OPTION_DELAY_ARMING)) {
-        constexpr uint32_t ARMING_DELAY_MS = 2000;
-        if (delay_arming) {
-            if (AP_HAL::millis() - hal.util->get_last_armed_change() < ARMING_DELAY_MS) {
-                // delay motor start after arming
-                motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
-                motors->output();
-                return;
-            } else {
-                delay_arming = false;
-            }
+        if (plane.arming.get_delay_arming()) {
+            // delay motor start after arming
+            motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
+            motors->output();
+            return;
         }
     }
 
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index e2ed416cb0..1adfc6b9c1 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -575,6 +575,7 @@ private:
 
     float last_land_final_agl;
 
+
     // oneshot with duration ARMING_DELAY_MS used by quadplane to delay spoolup after arming:
     // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
     bool delay_arming;