aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix error in calculation of static 312 order yaw reference

Browse Source
This commit is contained in:
Paul Riseborough 2020-12-15 07:54:47 +11:00 committed by Peter Barker
parent 1307a2fea8
commit ccfd89240b
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -246,7 +246,7 @@ void NavEKF3_core::SelectMagFusion()
if (yawAngDataDelayed.type == 2) {
yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
} else if (yawAngDataDelayed.type == 1) {
yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
yawAngDataDelayed.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
}
}
if (onGroundNotMoving) {
@ -290,7 +290,7 @@ void NavEKF3_core::SelectMagFusion()
} else {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
if (!onGroundNotMoving) {
yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
yawAngDataDelayed.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
}
yawAngDataDelayed.type = 1;
}