Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: minor format fix

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

28
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -205,20 +205,20 @@ void NavEKF3_core::SelectMagFusion()
// Store yaw angle when moving for use as a static reference when not moving
if (!onGroundNotMoving) {
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
yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321;
} else {
// 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;
}
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.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
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
yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321;
} else {
// 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;
}
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.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
}
// Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in