From d820a538d5f2974a538348ae5406b06a8217db12 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Nov 2015 19:10:09 +1100 Subject: [PATCH] AP_NavEKF2: Make magnetometer data available status global This assists with debugging and makes it consistent with other measurements that are buffered --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 6 +++--- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index d3a7332e48..48569dad14 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -139,16 +139,16 @@ void NavEKF2_core::SelectMagFusion() } // check for availability of magnetometer data to fuse - bool newMagDataAvailable = RecallMag(); + magDataToFuse = RecallMag(); - if (newMagDataAvailable) { + if (magDataToFuse) { // Control reset of yaw and magnetic field states controlMagYawReset(); } // determine if conditions are right to start a new fusion cycle // wait until the EKF time horizon catches up with the measurement - bool dataReady = (newMagDataAvailable && statesInitialised && use_compass() && yawAlignComplete); + bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete); if (dataReady) { // If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading if(inhibitMagStates) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 3b895ddaba..7060a68341 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -864,6 +864,7 @@ private: bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon. bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon. bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon. + bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon Vector2f heldVelNE; // velocity held when no aiding is available enum AidingMode {AID_ABSOLUTE=0, // GPS aiding is being used (optical flow may also be used) so position estimates are absolute. AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.