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:
parent
06f682757d
commit
381b38ca52
2 changed files with 22 additions and 12 deletions
|
@ -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) },
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue