diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 14494340a5..1a6ec1ae19 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -991,13 +991,13 @@ const clivalue_t valueTable[] = { { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedForwardTransition) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, - { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, - { "crash_gthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_gthreshold) }, - { "crash_setpoint_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_setpoint_threshold) }, - { "crash_time", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_time) }, + { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, + { "crash_gthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 100, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_gthreshold) }, + { "crash_setpoint_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 50, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_setpoint_threshold) }, + { "crash_time", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 100, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_time) }, { "crash_delay", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_delay) }, - { "crash_recovery_angle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) }, - { "crash_recovery_rate", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) }, + { "crash_recovery_angle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 5, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) }, + { "crash_recovery_rate", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 50, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) }, { "crash_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_limit_yaw) }, { "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 73c3c31d24..66b6af8e7c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -156,7 +156,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .rateAccelLimit = 0, .itermThrottleThreshold = 250, .itermAcceleratorGain = 5000, - .crash_time = 500, // ms + .crash_time = 1000, // ms .crash_delay = 0, // ms .crash_recovery_angle = 10, // degrees .crash_recovery_rate = 100, // degrees/second @@ -873,7 +873,17 @@ static void handleCrashRecovery( const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim, const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate) { + // check to see if we're still in the startup delay if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { + + // check to see if the crash_time limit has been exceeded, if so then + // exit crash recovery regardless of the current attitude or recovery status + if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs) { + inCrashRecoveryMode = false; + BEEP_OFF; + return; + } + if (crash_recovery == PID_CRASH_RECOVERY_BEEP) { BEEP_ON; } @@ -891,11 +901,11 @@ static void handleCrashRecovery( // reset iterm, since accumulated error before crash is now meaningless // and iterm windup during crash recovery can be extreme, especially on yaw axis pidData[axis].I = 0.0f; - if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs - || (getMotorMixRange() < 1.0f - && fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate - && fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate - && fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) { + if (getMotorMixRange() < 1.0f + && fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate + && fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate + && fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate) { + if (sensors(SENSOR_ACC)) { // check aircraft nearly level if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees