Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: simplify logic when updating yawAngDataStatic

Browse Source
This commit is contained in:
Randy Mackay 2020-12-15 13:48:17 +09:00 committed by Peter Barker
parent 843ddb4fdc
commit c2edae905f
1 changed files with 1 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -208,13 +208,10 @@ void NavEKF3_core::SelectMagFusion()
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321; yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321;
yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
} else { } else {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_312; yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_312;
}
if (yawAngDataStatic.order == rotationOrder::TAIT_BRYAN_321) {
yawAngDataStatic.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
} else if (yawAngDataStatic.order == rotationOrder::TAIT_BRYAN_312) {
yawAngDataStatic.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]); yawAngDataStatic.yawAng = atan2f(-prevTnb[1][0], prevTnb[1][1]);
} }
yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f); yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f);