From 42da33593d571e817388a0884a290666e3ead6f6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 10 May 2016 17:47:31 +1000 Subject: [PATCH] AP_NavEKF2: fix yaw fusion numerical health reporting --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 6a124ca5ae..5de93bab06 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -776,20 +776,18 @@ void NavEKF2_core::fuseEulerYaw() float varInnovInv; if (varInnov >= R_YAW) { varInnovInv = 1.0f / varInnov; - // All three magnetometer components are used in this measurement, so we output health status on three axes - faultStatus.bad_xmag = false; - faultStatus.bad_ymag = false; - faultStatus.bad_zmag = false; + // output numerical health status + faultStatus.bad_yaw = false; } else { // the calculation is badly conditioned, so we cannot perform fusion on this step // we reset the covariance matrix and try again next measurement CovarianceInit(); - // All three magnetometer components are used in this measurement, so we output health status on three axes - faultStatus.bad_xmag = true; - faultStatus.bad_ymag = true; - faultStatus.bad_zmag = true; + // output numerical health status + faultStatus.bad_yaw = true; return; } + + // calculate Kalman gain for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) { Kfusion[rowIndex] = 0.0f; for (uint8_t colIndex=0; colIndex<=2; colIndex++) { @@ -867,11 +865,11 @@ void NavEKF2_core::fuseEulerYaw() // is used to correct the estimated quaternion on the current time step stateStruct.quat.rotate(stateStruct.angErr); - // record fusion health status + // record fusion numerical health status faultStatus.bad_yaw = false; } else { - // record fusion health satus + // record fusion numerical health status faultStatus.bad_yaw = true; } }