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

fix missing blocks

This commit is contained in:
s0up 2018-04-23 15:02:26 -07:00
parent 4e3a21d2b3
commit ff247c3817
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/flight/imu.c
View File

@ -262,8 +262,13 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// Use raw heading error (from GPS or whatever else)
float ex = 0, ey = 0, ez = 0;
if (useCOG) {
while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf);
while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);
while (courseOverGround > M_PIf) {
courseOverGround -= (2.0f * M_PIf);
}
while (courseOverGround < -M_PIf) {
courseOverGround += (2.0f * M_PIf);
}
const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);