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

AP_NavEKF3: Fix bug in calculation of rotation order

Browse Source
This commit is contained in:
Paul Riseborough 2020-04-09 09:57:51 +10:00 committed by Andrew Tridgell
parent eeac0a05b9
commit a3725e2581
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

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