diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index f35bcee66..5798e07aa 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -548,6 +548,7 @@ const clivalue_t valueTable[] = { { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, { "crash_gthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_gthreshold) }, + { "crash_setpoint_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_setpoint_threshold) }, { "crash_time", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_time) }, { "crash_recovery_angle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) }, { "crash_recovery_rate", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7d4cfa111..016048a54 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -105,11 +105,12 @@ void resetPidProfile(pidProfile_t *pidProfile) .rateAccelLimit = 0, .itermThrottleThreshold = 350, .itermAcceleratorGain = 1000, - .crash_time = 500, // 500ms - .crash_recovery_angle = 10, // 10 degrees - .crash_recovery_rate = 100, // 100 degrees/second - .crash_dthreshold = 50, // 50 degrees/second/second - .crash_gthreshold = 500, // 500 degrees/second + .crash_time = 500, // ms + .crash_recovery_angle = 10, // degrees + .crash_recovery_rate = 100, // degrees/second + .crash_dthreshold = 50, // degrees/second/second + .crash_gthreshold = 400, // degrees/second + .crash_setpoint_threshold = 350, // degrees/second .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default .horizon_tilt_effect = 75, .horizon_tilt_expert_mode = false @@ -241,6 +242,7 @@ static int32_t crashRecoveryAngleDeciDegrees; static float crashRecoveryRate; static float crashDtermThreshold; static float crashGyroThreshold; +static float crashSetpointThreshold; void pidInitConfig(const pidProfile_t *pidProfile) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -265,6 +267,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { crashRecoveryRate = pidProfile->crash_recovery_rate; crashGyroThreshold = pidProfile->crash_gthreshold; crashDtermThreshold = pidProfile->crash_dthreshold; + crashSetpointThreshold = pidProfile->crash_setpoint_threshold; } void pidInit(const pidProfile_t *pidProfile) @@ -448,7 +451,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // if crash recovery is on and accelerometer enabled then check for a crash if (pidProfile->crash_recovery && sensors(SENSOR_ACC)) { - if (motorMixRange >= 1.0f && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold) { + if (motorMixRange >= 1.0f + && ABS(delta) > crashDtermThreshold + && ABS(errorRate) > crashGyroThreshold + && ABS(getSetpointRate(axis)) < crashSetpointThreshold) { inCrashRecoveryMode = true; crashDetectedAtUs = currentTimeUs; if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f6de2aa12..f8421b515 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -95,11 +95,12 @@ typedef struct pidProfile_s { uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect - uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) + uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t crash_dthreshold; // dterm crash value uint16_t crash_gthreshold; // gyro crash value + uint16_t crash_setpoint_threshold; // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes uint16_t crash_time; // ms uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second