From 5c599225741a1385c79dc7b4521a413c0f6a2ec4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 16 Jun 2016 20:41:15 +1000 Subject: [PATCH] AP_NavEKF2: Fix bug that could publish bad compass offsets Magnetometer bias states will subject to larger errors early in flight before flight motion makes the offsets observable and the state variances reduce. Adds a check on state variances. Replaces the parameter check with a check of the actual filter fusion method being used. --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 8804afed00..52b1fba3cc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -349,8 +349,15 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const // return true if offsets are valid bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const { - // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid - if (mag_idx == magSelectIndex && finalInflightMagInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) { + // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited, + // primary compass is valid and state variances have converged + const float maxMagVar = 5E-6f; + bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar); + if ((mag_idx == magSelectIndex) && + finalInflightMagInit && + !inhibitMagStates && + _ahrs->get_compass()->healthy(magSelectIndex) && + variancesConverged) { magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f; return true; } else {