From 4e8c234a55905b6cf6cd46b0cd9866fa7e86eabe Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Nov 2021 17:10:53 -0500 Subject: [PATCH] ekf2: only save mag declination if changed --- src/modules/ekf2/EKF2.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 5ad3f734af..196f13fa46 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1873,12 +1873,14 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) float declination_deg; if (_ekf.get_mag_decl_deg(&declination_deg)) { - _param_ekf2_mag_decl.set(declination_deg); - _mag_decl_saved = true; + _param_ekf2_mag_decl.update(); - if (!_multi_mode || (_multi_mode && _instance == 0)) { + if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) { + _param_ekf2_mag_decl.set(declination_deg); _param_ekf2_mag_decl.commit_no_notification(); } + + _mag_decl_saved = true; } } }