Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +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;
// if crash recovery is on and accelerometer enabled then check for a crash
if (pidProfile->crash_recovery && ARMING_FLAG(ARMED)) {
if (motorMixRange >= 1.0f && inCrashRecoveryMode == false
&& ABS(delta) > crashDtermThreshold
&& ABS(errorRate) > crashGyroThreshold
&& ABS(getSetpointRate(axis)) < crashSetpointThreshold) {
inCrashRecoveryMode = true;
crashDetectedAtUs = currentTimeUs;
}
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold
|| ABS(getSetpointRate(axis)) > crashSetpointThreshold)) {
if (pidProfile->crash_recovery) {
if (ARMING_FLAG(ARMED)) {
if (motorMixRange >= 1.0f && inCrashRecoveryMode == false
&& ABS(delta) > crashDtermThreshold
&& ABS(errorRate) > crashGyroThreshold
&& ABS(getSetpointRate(axis)) < crashSetpointThreshold) {
inCrashRecoveryMode = true;
crashDetectedAtUs = currentTimeUs;
}
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold
|| ABS(getSetpointRate(axis)) > crashSetpointThreshold)) {
inCrashRecoveryMode = false;
}
} else {
inCrashRecoveryMode = false;
BEEP_OFF;
}
}
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
}