From 6d34ac5ceb8bfdcefe8d7bf8343f3946206a0ae5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 May 2016 10:23:49 +1000 Subject: [PATCH] AP_NavEKF2: improve switch-over to use of mag field states Set initial variances to measurement uncertainties Always perform a field and yaw reset --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index f929eb7f97..10a2524d75 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -67,7 +67,18 @@ void NavEKF2_core::setWindMagStateLearningMode() bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4); // Inhibit the magnetic field calibration if not requested or denied - inhibitMagStates = (!magCalRequested || magCalDenied); + bool setMagInhibit = !magCalRequested || magCalDenied; + if (!inhibitMagStates && setMagInhibit) { + inhibitMagStates = true; + } else if (inhibitMagStates && !setMagInhibit) { + inhibitMagStates = false; + // when commencing use of magnetic field states, set the variances equal to the observation uncertainty + for (uint8_t index=16; index<=21; index++) { + P[index][index] = sq(frontend->_magNoise); + } + // let the magnetometer fusion know it needs to reset the yaw and field states + firstMagYawInit = false; + } // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed // because we want it re-done for each takeoff