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

Merge pull request #80 from luggi/EstNfix 

keep EstN from diverging to zero
This commit is contained in:
dongie 2014-04-16 22:48:48 +09:00
parent 03fab3f915 de9f1095fb
commit 45b74bf0b6
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
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.
@ -309,7 +311,7 @@ static void getEstimatedAttitude(void)
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
throttleAngleCorrection = 0;
} else {
} else {
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;