1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 08:45:36 +03:00

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.
This commit is contained in:
Bruce Luckcuck 2019-09-12 10:39:36 -04:00
parent 06f682757d
commit 381b38ca52
2 changed files with 22 additions and 12 deletions

View file

@ -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) },

View file

@ -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