diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 5798e07aae..4382888887 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -550,6 +550,7 @@ const clivalue_t valueTable[] = { { "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_delay", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_delay) }, { "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) }, { "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9194e87f62..565be6bfd8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -106,6 +106,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .itermThrottleThreshold = 350, .itermAcceleratorGain = 1000, .crash_time = 500, // ms + .crash_delay = 0, // ms .crash_recovery_angle = 10, // degrees .crash_recovery_rate = 100, // degrees/second .crash_dthreshold = 50, // degrees/second/second @@ -240,6 +241,7 @@ static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, ho static float ITermWindupPoint, ITermWindupPointInv; static uint8_t horizonTiltExpertMode; static timeDelta_t crashTimeLimitUs; +static timeDelta_t crashTimeDelayUs; static int32_t crashRecoveryAngleDeciDegrees; static float crashRecoveryRate; static float crashDtermThreshold; @@ -266,6 +268,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); crashTimeLimitUs = pidProfile->crash_time * 1000; + crashTimeDelayUs = pidProfile->crash_delay * 1000; crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10; crashRecoveryRate = pidProfile->crash_recovery_rate; crashGyroThreshold = pidProfile->crash_gthreshold; @@ -395,8 +398,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); } - if (inCrashRecoveryMode && axis != FD_YAW) { + if (inCrashRecoveryMode && axis != FD_YAW && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { // self-level - errorAngle is deviation from horizontal + if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) { + BEEP_ON; + } const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; currentPidSetpoint = errorAngle * levelGain; if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs @@ -451,16 +457,17 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an previousRateError[axis] = rD; // if crash recovery is on and accelerometer enabled then check for a crash - if (pidProfile->crash_recovery && inCrashRecoveryMode == false && sensors(SENSOR_ACC) && ARMING_FLAG(ARMED)) { - if (motorMixRange >= 1.0f + if (pidProfile->crash_recovery && sensors(SENSOR_ACC) && ARMING_FLAG(ARMED)) { + if (motorMixRange >= 1.0f && inCrashRecoveryMode == false && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold && ABS(getSetpointRate(axis)) < crashSetpointThreshold) { inCrashRecoveryMode = true; crashDetectedAtUs = currentTimeUs; - if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) { - BEEP_ON; - } + } + if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold + || ABS(getSetpointRate(axis)) > crashSetpointThreshold)) { + inCrashRecoveryMode = false; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f8421b515b..70cce21129 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -102,6 +102,7 @@ typedef struct pidProfile_s { 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 + uint16_t crash_delay; // ms uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode