From 99824c445c00580b60995fc4c8ecfc848c1bd0d5 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 21 Jul 2023 10:54:46 +0200 Subject: [PATCH] ekf2: add unit test for earth mag field reset to WMM --- src/modules/ekf2/EKF/ekf.h | 2 ++ src/modules/ekf2/test/test_EKF_mag.cpp | 14 ++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 728c46f3de..e4aad32fb5 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -372,6 +372,8 @@ public: Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2 float getAccelBiasLimit() const { return _params.acc_bias_lim; } + const Vector3f &getMagEarthField() const { return _state.mag_I; } + // mag bias (states 19, 20, 21) const Vector3f &getMagBias() const { return _state.mag_B; } Vector3f getMagBiasVariance() const diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 2add65b786..9970f53cc6 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -83,6 +83,20 @@ TEST_F(EkfMagTest, fusionStartWithReset) EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); + + // AND WHEN: GNSS fusion starts + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + + // THEN: the earth mag field is reset to the WMM + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); + + Vector3f mag_earth = _ekf->getMagEarthField(); + float mag_decl = atan2f(mag_earth(1), mag_earth(0)); + float mag_decl_wmm_deg = 0.f; + _ekf->get_mag_decl_deg(&mag_decl_wmm_deg); + EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-6f); } TEST_F(EkfMagTest, noInitLargeStrength)