From 449f5f2f5cefcaa78a96bf7bfdbed780d75f0493 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 27 Feb 2018 10:05:04 -0500 Subject: [PATCH] Runaway Takeoff remove unneeded parameters and enhance deactivate logic Removed parameters runaway_takeoff_threshold (hardcode to 60) and runaway_takeoff_activate_delay (hardcode to 75). The previous default values worked well and required no tuning. Enhance the deactivate logic to remove the R/P/Y stick activity condition once throttle reaches 2X runaway_takeoff_deactivate_throttle_percent. Additionally reduce the runaway_takeoff_deactivate_delay by 50% when throttle exceeds 75%. --- src/main/fc/fc_core.c | 34 ++++++++++++++++++++-------------- src/main/flight/pid.c | 4 +--- src/main/flight/pid.h | 2 -- src/main/interface/settings.c | 4 +--- 4 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 39ff30048..7d74762cb 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -98,10 +98,13 @@ enum { #define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync #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 RUNAWAY_TAKEOFF_GYRO_LIMIT_RP 15 // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props -#define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW 50 // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props +#define RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD 600 // The pidSum threshold required to trigger - corresponds to a pidSum value of 60% (raw 600) in the blackbox viewer +#define RUNAWAY_TAKEOFF_ACTIVATE_DELAY 75000 // (75ms) Time in microseconds where pidSum is above threshold to trigger +#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 RUNAWAY_TAKEOFF_GYRO_LIMIT_RP 15 // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props +#define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW 50 // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props +#define RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT 75 // High throttle limit to accelerate deactivation (halves the deactivation delay) #define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE 0 #define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY 1 @@ -557,8 +560,9 @@ bool processRx(timeUs_t currentTimeUs) // - 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 ((throttlePercent >= pidConfig()->runaway_takeoff_deactivate_throttle) - && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT) + const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; + const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); + if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) && (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)) { @@ -575,7 +579,12 @@ bool processRx(timeUs_t currentTimeUs) if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } - if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > (pidConfig()->runaway_takeoff_deactivate_delay * 1000)) { + uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay; + // at high throttle levels reduce deactivation delay by 50% + if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) { + deactivateDelay = deactivateDelay / 2; + } + if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) { runawayTakeoffCheckDisabled = true; } @@ -789,19 +798,16 @@ static void subTaskPidController(timeUs_t currentTimeUs) && !runawayTakeoffTemporarilyDisabled && (!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 (((fabsf(axisPIDSum[FD_PITCH]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) + || (fabsf(axisPIDSum[FD_ROLL]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) + || (fabsf(axisPIDSum[FD_YAW]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)) && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { if (runawayTakeoffTriggerUs == 0) { - runawayTakeoffTriggerUs = currentTimeUs + (pidConfig()->runaway_takeoff_activate_delay * 1000); + runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY; } else if (currentTimeUs > runawayTakeoffTriggerUs) { - setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF); disarm(); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0d11e8913..5533e3fc3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -60,7 +60,7 @@ FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3]; static FAST_RAM float dT; -PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); #ifdef STM32F10X #define PID_PROCESS_DENOM_DEFAULT 1 @@ -74,8 +74,6 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 1); 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 ); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3161f6d06..15854919e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -114,8 +114,6 @@ 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; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index ec5e7206a..024dbadbd 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -623,9 +623,7 @@ const clivalue_t valueTable[] = { { "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_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 1000 }, 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