From a95c11d48a7a0c0ebc572d606acefcea3289361f Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 24 Jul 2023 11:43:24 +0200 Subject: [PATCH] ekf2-test: add test for inclination of mag earth field --- src/modules/ekf2/EKF/estimator_interface.h | 11 +++++++++++ src/modules/ekf2/EKF/mag_control.cpp | 2 +- src/modules/ekf2/test/test_EKF_mag.cpp | 5 +++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 9c358e66cd..9ac957292a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -227,6 +227,17 @@ public: } } + bool get_mag_inc_deg(float &val) const + { + if (_NED_origin_initialised) { + val = math::degrees(_mag_inclination_gps); + return true; + + } else { + return false; + } + } + void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const { inc_deg = math::degrees(_mag_inclination); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index b678189674..8e95388a4b 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -472,7 +472,7 @@ bool Ekf::checkMagField(const Vector3f &mag_sample) } const Vector3f mag_earth = _R_to_earth * mag_sample; - _mag_inclination = asin(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); + _mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); if (_params.mag_check & static_cast(MagCheckMask::INCLINATION)) { if (PX4_ISFINITE(_mag_inclination_gps)) { diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index ec55eeafb9..b166aea45f 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -97,6 +97,11 @@ TEST_F(EkfMagTest, fusionStartWithReset) 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); + + float mag_incl = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); + float mag_incl_wmm_deg = 0.f; + _ekf->get_mag_inc_deg(mag_incl_wmm_deg); + EXPECT_NEAR(degrees(mag_incl), mag_incl_wmm_deg, 1e-6f); } TEST_F(EkfMagTest, noInitLargeStrength)