dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2-test: add test for inclination of mag earth field

This commit is contained in:
bresch 2023-07-24 11:43:24 +02:00 committed by Daniel Agar
parent 72be724b86
commit a95c11d48a
3 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -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);

2
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -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<int32_t>(MagCheckMask::INCLINATION)) {
if (PX4_ISFINITE(_mag_inclination_gps)) {

5
src/modules/ekf2/test/test_EKF_mag.cpp
View File

@ -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)