Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 06:45:16 +03:00
Code Issues Wiki Activity

Crash recovery - Fixes #4379

Browse source
This commit is contained in:
Vidalcris 2017-10-19 23:35:06 +02:00 • committed by GitHub
parent 3ff074dfbe
commit 70973bad04
1 changed files with 15 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

26
src/main/flight/pid.c
View file

@ -515,20 +515,24 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
previousRateError[axis] = rD; previousRateError[axis] = rD;
// if crash recovery is on and accelerometer enabled then check for a crash // if crash recovery is on and accelerometer enabled then check for a crash
if (pidProfile->crash_recovery && ARMING_FLAG(ARMED)) { if (pidProfile->crash_recovery) {
if (motorMixRange >= 1.0f && inCrashRecoveryMode == false if (ARMING_FLAG(ARMED)) {
&& ABS(delta) > crashDtermThreshold if (motorMixRange >= 1.0f && inCrashRecoveryMode == false
&& ABS(errorRate) > crashGyroThreshold && ABS(delta) > crashDtermThreshold
&& ABS(getSetpointRate(axis)) < crashSetpointThreshold) { && ABS(errorRate) > crashGyroThreshold
inCrashRecoveryMode = true; && ABS(getSetpointRate(axis)) < crashSetpointThreshold) {
crashDetectedAtUs = currentTimeUs; inCrashRecoveryMode = true;
} crashDetectedAtUs = currentTimeUs;
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold }
|| ABS(getSetpointRate(axis)) > crashSetpointThreshold)) { if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold
|| ABS(getSetpointRate(axis)) > crashSetpointThreshold)) {
inCrashRecoveryMode = false;
}
} else {
inCrashRecoveryMode = false; inCrashRecoveryMode = false;
BEEP_OFF;
} }
} }
axisPID_D[axis] = Kd[axis] * delta * tpaFactor; axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
} }