diff --git a/src/main/build/debug.c b/src/main/build/debug.c index d32c94f86..4a41f19aa 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -56,4 +56,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RANGEFINDER_QUALITY", "LIDAR_TF", "CORE_TEMP", + "RUNAWAY_TAKEOFF", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 072fdcde0..813b88fb6 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e3a769a83..c00bdfdc2 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -18,6 +18,7 @@ #include #include #include +#include #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) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 2173f4c88..554c33c1b 100644 --- a/src/main/fc/rc_controls.c +++ b/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; diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 77c3a4d42..8e3f61bda 100644 --- a/src/main/fc/runtime_config.c +++ b/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; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 707bfe3ba..ce764e667 100644 --- a/src/main/fc/runtime_config.h +++ b/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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5ab587d1d..a66e78d83 100644 --- a/src/main/flight/pid.c +++ b/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); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7f536139c..3161f6d06 100644 --- a/src/main/flight/pid.h +++ b/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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 4dcfdfcc1..9dcf2bc34 100644 --- a/src/main/interface/settings.c +++ b/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) }, diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 181261282..610b158fc 100644 --- a/src/main/target/common_fc_pre.h +++ b/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) diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 9339fe37b..5d60dbe3d 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/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 = {}; } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 04bad39a8..97862dbce 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/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() {