From 17aeaa3ee0c60b86990add14b9f9ec00ff9b553c Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Mon, 12 Feb 2018 10:17:47 -0500
Subject: [PATCH] Runaway Takeoff Prevention - temporarily disable during
 configurator bench testing

Temporarily disables Runaway Takeoff Prevention when the `ARMING_DISABLED_MSP` flag is cleared in the configurator by switching the safety switch on the motors tab.  Allows bench testing the motors and flight controller response without triggering a runaway takeoff auto-disarm.

Requires coordination with the configurator to pass an extra runaway takeoff temporary disable flag with the MSP_ARMING_DISABLE msp command.
---
 src/main/fc/fc_core.c    | 11 +++++++++++
 src/main/fc/fc_core.h    |  2 ++
 src/main/interface/msp.c | 10 ++++++++++
 3 files changed, 23 insertions(+)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 34476e89db..338f0e9f41 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -127,6 +127,7 @@ static timeUs_t runawayTakeoffDeactivateUs = 0;
 static timeUs_t runawayTakeoffAccumulatedUs = 0;
 static bool runawayTakeoffCheckDisabled = false;
 static timeUs_t runawayTakeoffTriggerUs = 0;
+static bool runawayTakeoffTemporarilyDisabled = false;
 #endif
 
 
@@ -444,6 +445,14 @@ bool areSticksActive(uint8_t stickPercentLimit)
     }
     return false;
 }
+
+
+// allow temporarily disabling runaway takeoff prevention if we are connected
+// to the configurator and the ARMING_DISABLED_MSP flag is cleared.
+void runawayTakeoffTemporaryDisable(uint8_t disableFlag)
+{
+    runawayTakeoffTemporarilyDisabled = disableFlag;
+}
 #endif
 
 
@@ -530,6 +539,7 @@ bool processRx(timeUs_t currentTimeUs)
         && pidConfig()->runaway_takeoff_prevention
         && !runawayTakeoffCheckDisabled
         && !flipOverAfterCrashMode
+        && !runawayTakeoffTemporarilyDisabled
         && !STATE(FIXED_WING)) {
 
         // Determine if we're in "flight"
@@ -767,6 +777,7 @@ static void subTaskPidController(timeUs_t currentTimeUs)
         && pidConfig()->runaway_takeoff_prevention
         && !runawayTakeoffCheckDisabled
         && !flipOverAfterCrashMode
+        && !runawayTakeoffTemporarilyDisabled
         && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
 
         const float runawayTakeoffThreshold = pidConfig()->runaway_takeoff_threshold * 10.0f;
diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h
index 6d215beab4..1482b7687c 100644
--- a/src/main/fc/fc_core.h
+++ b/src/main/fc/fc_core.h
@@ -48,3 +48,5 @@ void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
 bool isFlipOverAfterCrashMode(void);
+
+void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index b4754ce6a7..af7ea50bcd 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1777,13 +1777,23 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
     case MSP_SET_ARMING_DISABLED:
         {
             const uint8_t command = sbufReadU8(src);
+            uint8_t disableRunawayTakeoff = 0;
+            if (sbufBytesRemaining(src)) {
+                disableRunawayTakeoff = sbufReadU8(src);
+            }
             if (command) {
                 setArmingDisabled(ARMING_DISABLED_MSP);
                 if (ARMING_FLAG(ARMED)) {
                     disarm();
                 }
+#ifdef USE_RUNAWAY_TAKEOFF
+                runawayTakeoffTemporaryDisable(false);
+#endif
             } else {
                 unsetArmingDisabled(ARMING_DISABLED_MSP);
+#ifdef USE_RUNAWAY_TAKEOFF
+                runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
+#endif
             }
         }
         break;