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

AP_NavEKF3: simplify alignYawAngle

Browse Source
This commit is contained in:
Randy Mackay 2020-10-28 11:33:58 +09:00 committed by Andrew Tridgell
parent 024d2b7435
commit 2681d3afe2
1 changed files with 4 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -180,30 +180,14 @@ void NavEKF3_core::realignYawGPS()
void NavEKF3_core::alignYawAngle()
{
if (yawAngDataDelayed.type == 2) {
Vector3f euler321;
stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
stateStruct.quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng);
} else if (yawAngDataDelayed.type == 1) {
Vector3f euler312 = stateStruct.quat.to_vector312();
stateStruct.quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng);
}
// update quaternion states and covariances
const rotationOrder order = (yawAngDataDelayed.type == 1) ? rotationOrder::TAIT_BRYAN_312 : rotationOrder::TAIT_BRYAN_321;
// set the yaw angle variance reflect the yaw sensor single sample uncertainty in yaw
// assume tilt uncertainty split equally between roll and pitch
Vector3f angleErrVarVec = Vector3f(0.5f * tiltErrorVariance, 0.5f * tiltErrorVariance, sq(yawAngDataDelayed.yawAngErr));
CovariancePrediction(&angleErrVarVec);
// update quaternion states and covariances
resetQuatStateYawOnly(yawAngDataDelayed.yawAng, sq(MAX(yawAngDataDelayed.yawAngErr, 1.0e-2f)), order);
// send yaw alignment information to console
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
// record the yaw reset event
recordYawReset();
// clear any pending yaw reset requests
gpsYawResetRequest = false;
magYawResetRequest = false;
}
/********************************************************