Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

fix missing blocks

Browse source
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
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) // Use raw heading error (from GPS or whatever else)
float ex = 0, ey = 0, ez = 0; float ex = 0, ey = 0, ez = 0;
if (useCOG) { if (useCOG) {
while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); while (courseOverGround > M_PIf) {
while (courseOverGround < -M_PIf) courseOverGround += (2.0f * 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]); const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);