rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #5337 from etracer65/runaway_takeoff_cleanup 

Runaway Takeoff remove unneeded parameters and enhance deactivate logic
This commit is contained in:
Michael Keller 2018-03-01 02:22:22 +13:00 committed by GitHub
parent 997df8615c 449f5f2f5c
commit 1453a348b7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/main/fc/fc_core.c
View File

@ -98,10 +98,13 @@ enum {
#define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync #define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
#ifdef USE_RUNAWAY_TAKEOFF #ifdef USE_RUNAWAY_TAKEOFF
#define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15 // 15% - minimum stick deflection during deactivation phase #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_DEACTIVATE_PIDSUM_LIMIT 100 // 10.0% - pidSum limit during deactivation phase #define RUNAWAY_TAKEOFF_ACTIVATE_DELAY 75000 // (75ms) Time in microseconds where pidSum is above threshold to trigger
#define RUNAWAY_TAKEOFF_GYRO_LIMIT_RP 15 // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props #define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15 // 15% - minimum stick deflection during deactivation phase
#define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW 50 // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props #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_ENABLED_STATE 0
#define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY 1 #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 // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
bool inStableFlight = false; bool inStableFlight = false;
if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
if ((throttlePercent >= pidConfig()->runaway_takeoff_deactivate_throttle) const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
&& areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT) 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_PITCH]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(axisPIDSum[FD_ROLL]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(axisPIDSum[FD_ROLL]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(axisPIDSum[FD_YAW]) < 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) { if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs; 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; runawayTakeoffCheckDisabled = true;
} }
@ -789,19 +798,16 @@ static void subTaskPidController(timeUs_t currentTimeUs)
&& !runawayTakeoffTemporarilyDisabled && !runawayTakeoffTemporarilyDisabled
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
const float runawayTakeoffThreshold = pidConfig()->runaway_takeoff_threshold * 10.0f; if (((fabsf(axisPIDSum[FD_PITCH]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(axisPIDSum[FD_ROLL]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
if (((fabsf(axisPIDSum[FD_PITCH]) >= runawayTakeoffThreshold) || (fabsf(axisPIDSum[FD_YAW]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
|| (fabsf(axisPIDSum[FD_ROLL]) >= runawayTakeoffThreshold)
|| (fabsf(axisPIDSum[FD_YAW]) >= runawayTakeoffThreshold))
&& ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
if (runawayTakeoffTriggerUs == 0) { if (runawayTakeoffTriggerUs == 0) {
runawayTakeoffTriggerUs = currentTimeUs + (pidConfig()->runaway_takeoff_activate_delay * 1000); runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;
} else if (currentTimeUs > runawayTakeoffTriggerUs) { } else if (currentTimeUs > runawayTakeoffTriggerUs) {
setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF); setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
disarm(); disarm();
} }

4
src/main/flight/pid.c
View File

@ -60,7 +60,7 @@ FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
static FAST_RAM float dT; 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 #ifdef STM32F10X
#define PID_PROCESS_DENOM_DEFAULT 1 #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, PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT, .pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
.runaway_takeoff_prevention = true, .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_throttle = 25, // throttle level % needed to accumulate deactivation time
.runaway_takeoff_deactivate_delay = 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff .runaway_takeoff_deactivate_delay = 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
); );

2
src/main/flight/pid.h
View File

@ -114,8 +114,6 @@ PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
typedef struct pidConfig_s { typedef struct pidConfig_s {
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate 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_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) 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 uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
} pidConfig_t; } pidConfig_t;

4
src/main/interface/settings.c
View File

@ -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) }, { "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 #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_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_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_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 { "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 #endif