From 381b38ca52d213a390a3d1258c54cb97e0301c25 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 12 Sep 2019 10:39:36 -0400 Subject: [PATCH] Fix crash recovery timeout logic and make settings limits rational Timeout logic was ignored if the user had the accelerometer enabled and the quad wasn't within the angle limit. This was made doubly troublesome as the angle limit could be set to zero preventing crash recovery from ever deactivating. Also adjusted the ranges of the parameters to be more rational to avoid invalid configurations. --- src/main/cli/settings.c | 12 ++++++------ src/main/flight/pid.c | 22 ++++++++++++++++------ 2 files changed, 22 insertions(+), 12 deletions(-) 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