From d0fe1f032a30426a53be56098ca035d72822317a Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 19 Dec 2019 11:17:41 +0100 Subject: [PATCH] mag_fusion: re-introduce delay between mag reset and fusion, this was dropped during the mag fusion refactor PR #662 --- EKF/ekf.h | 1 - EKF/mag_fusion.cpp | 4 +++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 52548f9293..6366beeef8 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -465,7 +465,6 @@ private: // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) - bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 8d8bf90d12..b89dcc7571 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -170,7 +170,9 @@ void Ekf::fuseMag() return; } - bool update_all_states = !_flt_mag_align_converging; + // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise + // before they are used to constrain heading drift + const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) {