Initial implementation of Runaway Takeoff Prevention (anti-taz)

Detects runaway pidSum values on takeoff and auto-disarms to prevent the "Tasmanian Devil" caused by incorrect props, wrong motor order/direction, incorrect flight controller orientation, etc.  After a successful takeoff and normal flight is detected the feature is disabled for the remainder of the battery.

src/main/build/debug.c

@@ -56,4 +56,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RANGEFINDER_QUALITY",
     "LIDAR_TF",
     "CORE_TEMP",
+    "RUNAWAY_TAKEOFF",
 };

src/main/build/debug.h

@@ -74,6 +74,7 @@ typedef enum {
     DEBUG_RANGEFINDER_QUALITY,
     DEBUG_LIDAR_TF,
     DEBUG_CORE_TEMP,
+    DEBUG_RUNAWAY_TAKEOFF,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/fc_core.c

@@ -18,6 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -98,6 +99,20 @@ enum {
 
 #define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
 
+#ifdef USE_RUNAWAY_TAKEOFF
+#define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15   // 15% - minimum stick deflection during deactivation phase
+#define RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT  100  // 10.0% - pidSum limit during deactivation phase
+
+#define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE      0
+#define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY   1
+#define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY 2
+#define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME  3
+
+#define DEBUG_RUNAWAY_TAKEOFF_TRUE  1
+#define DEBUG_RUNAWAY_TAKEOFF_FALSE 0
+#endif
+
+
 #if defined(USE_GPS) || defined(USE_MAG)
 int16_t magHold;
 #endif
@@ -109,6 +124,14 @@ static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the m
 bool isRXDataNew;
 static int lastArmingDisabledReason = 0;
 
+#ifdef USE_RUNAWAY_TAKEOFF
+static timeUs_t runawayTakeoffDeactivateUs = 0;
+static timeUs_t runawayTakeoffAccumulatedUs = 0;
+static bool runawayTakeoffCheckDisabled = false;
+static timeUs_t runawayTakeoffTriggerUs = 0;
+#endif
+
+
 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
 
 PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
@@ -220,6 +243,12 @@ void updateArmingStatus(void)
                              && !isModeActivationConditionPresent(BOX3DONASWITCH)
                              && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE));
 
+#ifdef USE_RUNAWAY_TAKEOFF
+           if (!IS_RC_MODE_ACTIVE(BOXARM)) {
+               unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
+           }
+#endif
+
           // If arming is disabled and the ARM switch is on
           if (isArmingDisabled()
               && !ignoreGyro
@@ -311,6 +340,12 @@ void tryArm(void)
 #else
         beeper(BEEPER_ARMING);
 #endif
+
+#ifdef USE_RUNAWAY_TAKEOFF
+        runawayTakeoffDeactivateUs = 0;
+        runawayTakeoffAccumulatedUs = 0;
+        runawayTakeoffTriggerUs = 0;
+#endif
     } else {
         if (!isFirstArmingGyroCalibrationRunning()) {
             int armingDisabledReason = ffs(getArmingDisableFlags());
@@ -389,6 +424,54 @@ static bool canUpdateVTX(void)
 }
 #endif
 
+#ifdef USE_RUNAWAY_TAKEOFF
+// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
+bool areSticksActive(uint8_t stickPercentLimit)
+{
+    for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) {
+        const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
+        uint8_t stickPercent = 0;
+        if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) {
+            stickPercent = 100;
+        } else {
+            if (rcData[axis] > (rxConfig()->midrc + deadband)) {
+                stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband);
+            } else if (rcData[axis] < (rxConfig()->midrc - deadband)) {
+                stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN);
+            }
+        }
+        if (stickPercent >= stickPercentLimit) {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+// calculate the throttle stick percent - integer math is good enough here.
+uint8_t calculateThrottlePercent(void)
+{
+    uint8_t ret = 0;
+    if (feature(FEATURE_3D)
+        && !IS_RC_MODE_ACTIVE(BOX3DDISABLE)
+        && !isModeActivationConditionPresent(BOX3DONASWITCH)) {
+
+        if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) {
+            ret = 100;
+        } else {
+            if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
+                ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
+            } else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
+                ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
+            }
+        }
+    } else {
+        ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
+    }
+    return ret;
+}
+#endif
+
 /*
  * processRx called from taskUpdateRxMain
  */
@@ -436,6 +519,66 @@ bool processRx(timeUs_t currentTimeUs)
         pidStabilisationState(PID_STABILISATION_ON);
     }
 
+#ifdef USE_RUNAWAY_TAKEOFF
+    // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
+    // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
+    // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
+    // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
+    // prevention for the remainder of the battery.
+
+    if (ARMING_FLAG(ARMED)
+        && pidConfig()->runaway_takeoff_prevention
+        && !runawayTakeoffCheckDisabled
+        && !STATE(FIXED_WING)) {
+
+        // Determine if we're in "flight"
+        //   - motors running
+        //   - throttle over runaway_takeoff_deactivate_throttle_percent
+        //   - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
+        //   - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
+        bool inStableFlight = false;
+        if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
+            if ((calculateThrottlePercent() >= pidConfig()->runaway_takeoff_deactivate_throttle)
+                && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)
+                && (fabsf(axisPIDSum[FD_PITCH]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
+                && (fabsf(axisPIDSum[FD_ROLL]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
+                && (fabsf(axisPIDSum[FD_YAW]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
+                
+                inStableFlight = true;
+                if (runawayTakeoffDeactivateUs == 0) {
+                    runawayTakeoffDeactivateUs = currentTimeUs;
+                }
+            }
+        }
+
+        // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
+        if (inStableFlight) {
+            if (runawayTakeoffDeactivateUs == 0) {
+                runawayTakeoffDeactivateUs = currentTimeUs;
+            }
+            if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > (pidConfig()->runaway_takeoff_deactivate_delay * 1000)) {
+                runawayTakeoffCheckDisabled = true;
+            }
+
+        } else {
+            if (runawayTakeoffDeactivateUs != 0) {
+                runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
+            }
+            runawayTakeoffDeactivateUs = 0;
+        }
+        if (runawayTakeoffDeactivateUs == 0) {
+            DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
+            DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
+        } else {
+            DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
+            DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
+        }
+    } else {
+        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
+        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
+    }
+#endif
+
     // When armed and motors aren't spinning, do beeps and then disarm
     // board after delay so users without buzzer won't lose fingers.
     // mixTable constrains motor commands, so checking  throttleStatus is enough
@@ -611,6 +754,39 @@ static void subTaskPidController(timeUs_t currentTimeUs)
     // PID - note this is function pointer set by setPIDController()
     pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
+
+#ifdef USE_RUNAWAY_TAKEOFF
+    // Check to see if runaway takeoff detection is active (anti-spaz) and the pidSum is over the threshold.
+    // If so, disarm for safety
+    if (ARMING_FLAG(ARMED)
+        && !STATE(FIXED_WING)
+        && pidConfig()->runaway_takeoff_prevention
+        && !runawayTakeoffCheckDisabled
+        && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
+
+        const float runawayTakeoffThreshold = pidConfig()->runaway_takeoff_threshold * 10.0f;
+        if ((fabsf(axisPIDSum[FD_PITCH]) >= runawayTakeoffThreshold)
+            || (fabsf(axisPIDSum[FD_ROLL]) >= runawayTakeoffThreshold)
+            || (fabsf(axisPIDSum[FD_YAW]) >= runawayTakeoffThreshold)) {
+
+            if (runawayTakeoffTriggerUs == 0) {
+                runawayTakeoffTriggerUs = currentTimeUs + (pidConfig()->runaway_takeoff_activate_delay * 1000);
+            } else if (currentTimeUs > runawayTakeoffTriggerUs) {
+                
+                setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
+                disarm();
+            }
+        } else {
+            runawayTakeoffTriggerUs = 0;
+        }
+        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
+        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
+    } else {
+        runawayTakeoffTriggerUs = 0;
+        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
+        DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
+    }
+#endif
 }
 
 static void subTaskMainSubprocesses(timeUs_t currentTimeUs)

src/main/fc/rc_controls.c

@@ -195,6 +195,14 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
             else {
                 beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
                 repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
+
+#ifdef USE_RUNAWAY_TAKEOFF
+                // Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
+                // by a runaway pidSum detection auto-disarm.
+                // This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
+                // before they're able to rearm
+                unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
+#endif
             }
         }
         return;

src/main/fc/runtime_config.c

@@ -32,7 +32,7 @@ static uint32_t enabledSensors = 0;
 const char *armingDisableFlagNames[]= {
     "NOGYRO", "FAILSAFE", "RXLOSS", "BADRX", "BOXFAILSAFE",
     "THROTTLE", "ANGLE", "BOOTGRACE", "NOPREARM", "LOAD",
-    "CALIB", "CLI", "CMS", "OSD", "BST", "MSP", "ARMSWITCH"
+    "CALIB", "CLI", "CMS", "OSD", "BST", "MSP", "RUNAWAY", "ARMSWITCH"
 };
 
 static armingDisableFlags_e armingDisableFlags = 0;

src/main/fc/runtime_config.h

@@ -51,10 +51,12 @@ typedef enum {
     ARMING_DISABLED_OSD_MENU        = (1 << 13),
     ARMING_DISABLED_BST             = (1 << 14),
     ARMING_DISABLED_MSP             = (1 << 15),
-    ARMING_DISABLED_ARM_SWITCH      = (1 << 16), // Needs to be the last element, since it's always activated if one of the others is active when arming
+    ARMING_DISABLED_RUNAWAY_TAKEOFF = (1 << 16),
+    ARMING_DISABLED_ARM_SWITCH      = (1 << 17), // Needs to be the last element, since it's always activated if one of the others is active when arming
 } armingDisableFlags_e;
 
-#define ARMING_DISABLE_FLAGS_COUNT 17
+#define ARMING_DISABLE_FLAGS_COUNT 18
+
 extern const char *armingDisableFlagNames[ARMING_DISABLE_FLAGS_COUNT];
 
 void setArmingDisabled(armingDisableFlags_e flag);

src/main/flight/pid.c

@@ -69,9 +69,21 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 1);
 #else
 #define PID_PROCESS_DENOM_DEFAULT       2
 #endif
+
+#ifdef USE_RUNAWAY_TAKEOFF
+PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
+    .pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
+    .runaway_takeoff_prevention = true,
+    .runaway_takeoff_threshold = 60,            // corresponds to a pidSum value of 60% (raw 600) in the blackbox viewer
+    .runaway_takeoff_activate_delay = 75,       // 75ms delay before activation (pidSum above threshold time)
+    .runaway_takeoff_deactivate_throttle = 25,  // throttle level % needed to accumulate deactivation time
+    .runaway_takeoff_deactivate_delay = 500     // Accumulated time (in milliseconds) before deactivation in successful takeoff
+);
+#else
 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
     .pid_process_denom = PID_PROCESS_DENOM_DEFAULT
 );
+#endif
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2);
 

src/main/flight/pid.h

@@ -113,6 +113,11 @@ PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
 
 typedef struct pidConfig_s {
     uint8_t pid_process_denom;              // Processing denominator for PID controller vs gyro sampling rate
+    uint8_t runaway_takeoff_prevention;          // off, on - enables pidsum runaway disarm logic
+    uint8_t runaway_takeoff_threshold;           // runaway pidsum trigger threshold
+    uint8_t runaway_takeoff_activate_delay;      // delay in ms where pidSum is above threshold before activation
+    uint16_t runaway_takeoff_deactivate_delay;   // delay in ms for "in-flight" conditions before deactivation (successful flight)
+    uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
 } pidConfig_t;
 
 PG_DECLARE(pidConfig_t, pidConfig);

src/main/interface/settings.c

@@ -605,6 +605,13 @@ const clivalue_t valueTable[] = {
 
 // PG_PID_CONFIG
     { "pid_process_denom",          VAR_UINT8  | MASTER_VALUE,  .config.minmax = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) },
+#ifdef USE_RUNAWAY_TAKEOFF
+    { "runaway_takeoff_prevention", VAR_UINT8  | MODE_LOOKUP,  .config.lookup = { TABLE_OFF_ON }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_prevention) },    // enables/disables runaway takeoff prevention
+    { "runaway_takeoff_threshold",  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 30, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_threshold) },          // pidSum limit to trigger prevention
+    { "runaway_takeoff_activate_delay",  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_activate_delay) }, // time in ms where pidSum is above threshold to trigger prevention
+    { "runaway_takeoff_deactivate_delay",  VAR_UINT16  | MASTER_VALUE, .config.minmax = { 100, 5000 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_delay) },           // deactivate time in ms
+    { "runaway_takeoff_deactivate_throttle_percent",  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
+#endif
 
 // PG_PID_PROFILE
     { "dterm_lowpass_type",         VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) },

src/main/target/common_fc_pre.h

@@ -123,6 +123,7 @@
 #define USE_TELEMETRY_SMARTPORT
 #define USE_RESOURCE_MGMT
 #define USE_SERVOS
+#define USE_RUNAWAY_TAKEOFF     // Runaway Takeoff Prevention (anti-taz)
 #endif
 
 #if (FLASH_SIZE > 128)

src/test/unit/arming_prevention_unittest.cc

@@ -64,6 +64,7 @@ extern "C" {
     gpsSolutionData_t gpsSol;
     uint32_t targetPidLooptime;
     bool cmsInMenu = false;
+    float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
     rxRuntimeConfig_t rxRuntimeConfig = {};
 }
 

src/test/unit/rc_controls_unittest.cc

@@ -50,6 +50,7 @@ extern "C" {
     #include "fc/rc_adjustments.h"
 
     #include "fc/rc_controls.h"
+    #include "fc/runtime_config.h"
 
     #include "scheduler/scheduler.h"
 }
@@ -57,6 +58,10 @@ extern "C" {
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
 
+void unsetArmingDisabled(armingDisableFlags_e flag) {
+  UNUSED(flag);
+}
+
 class RcControlsModesTest : public ::testing::Test {
 protected:
     virtual void SetUp() {