From 209a32b8b9c426bd4338045593cb5fcf243a5584 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 15 Nov 2017 07:44:07 +1100 Subject: [PATCH] AP_NavEKF3: Fix bug in use of external 321 yaw to align --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index d6a2430606..a02d2cbeba 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -218,7 +218,7 @@ void NavEKF3_core::alignYawAngle() if (yawAngDataDelayed.type == 2) { Vector3f euler321; stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); - stateStruct.quat.to_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng); + 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); @@ -925,10 +925,10 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) float innovation; if (!usePredictedYaw) { if (!useExternalYawSensor) { - // Use the difference between the horizontal projection and declination to give the measured yaw - // rotate measured mag components into earth frame - Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; - float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + // Use the difference between the horizontal projection and declination to give the measured yaw + // rotate measured mag components into earth frame + Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); innovation = wrap_PI(yawAngPredicted - yawAngMeasured); } else { // use the external yaw sensor data