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

6
src/imu.c
View File

@ -267,10 +267,12 @@ static void getEstimatedAttitude(void)
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G); accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle); rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG)) if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle); rotateV(&EstM.V, deltaGyroAngle);
else } else {
rotateV(&EstN.V, deltaGyroAngle); rotateV(&EstN.V, deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
}
// Apply complimentary filter (Gyro drift correction) // 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. // 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.