Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

keep EstN from diverging to zero

Browse source
This commit is contained in:
luggi 2014-04-16 15:38:53 +02:00
parent 03fab3f915
commit de9f1095fb
1 changed files with 5 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/imu.c
View file

@ -267,10 +267,12 @@ static void getEstimatedAttitude(void)
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG))
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle);
else
} else {
rotateV(&EstN.V, deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
}
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.