rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Attempt to deal with gyro overflow after crash

This commit is contained in:
Martin Budden 2017-08-25 07:44:21 +01:00
parent 2f0a678cbe
commit 879778aeef
1 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/main/flight/pid.c
View File

@ -415,7 +415,16 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
if (axis == FD_YAW) {
// on yaw axis, prevent "yaw spin to the moon" after crash by constraining errorRate
errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
if (gyroRate > 1990.0f || gyroRate < -1990.0f) {
// ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
// This can cause an overflow and sign reversal in the output.
// Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
// If there is a sign reversal we will actually increase crash-induced yaw spin
// so best thing to do is set error to zero.
errorRate = 0.0f;
} else {
errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
}
} else {
// on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
if (sensors(SENSOR_ACC)) {