fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 22:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag: add inclination check

Browse Source
This commit is contained in:
bresch
2023-07-24 10:16:37 -04:00
committed by Daniel Agar
parent e4a16bfc80
commit 357bf024f6
9 changed files with 122 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+9
View File
@@ -214,6 +214,15 @@ void EkfWrapper::enableMagStrengthCheck()
{
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::STRENGTH);
}
void EkfWrapper::enableMagInclinationCheck()
{
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::INCLINATION);
}
void EkfWrapper::enableMagCheckForceWMM()
{
_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::FORCE_WMM);
}
bool EkfWrapper::isWindVelocityEstimated() const
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+2
View File
@@ -102,6 +102,8 @@ public:
bool isIntendingMag3DFusion() const;
void setMagFuseTypeNone();
void enableMagStrengthCheck();
void enableMagInclinationCheck();
void enableMagCheckForceWMM();
bool isWindVelocityEstimated() const;
src/modules/ekf2/test/test_EKF_mag.cpp
+68 -1
View File
@@ -95,7 +95,7 @@ TEST_F(EkfMagTest, fusionStartWithReset)
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);
_ekf->get_mag_decl_deg(mag_decl_wmm_deg);
EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-6f);
}
@@ -141,3 +141,70 @@ TEST_F(EkfMagTest, suddenLargeStrength)
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
}
TEST_F(EkfMagTest, noInitLargeInclination)
{
// GIVEN: a really large magnetic field
_ekf_wrapper.enableMagInclinationCheck();
// To prevent an early pass of the inclination check, "force WMM" must be set
_ekf_wrapper.enableMagCheckForceWMM();
_sensor_simulator.startGps();
Vector3f mag_data(0.4f, 0.f, 0.f);
_sensor_simulator._mag.setData(mag_data);
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
_sensor_simulator.runSeconds(_init_duration_s + 10.f); // live some extra time fo GNSS checks to pass
// THEN: the fusion shouldn't start
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align);
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
// BUT then: as soon as there is some meaningful data
const float mag_heading = -M_PI_F / 7.f;
mag_data = Vector3f(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f);
_sensor_simulator._mag.setData(mag_data);
_sensor_simulator.runSeconds(2.f);
float decl_deg = 0.f;
_ekf->get_mag_decl_deg(decl_deg);
// THEN: the fusion initializes using the mag data and runs normally
EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f));
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}
TEST_F(EkfMagTest, suddenInclinationChange)
{
_ekf_wrapper.enableMagInclinationCheck();
_ekf_wrapper.enableMagCheckForceWMM();
_sensor_simulator.startGps();
// GIVEN: some meaningful mag data
const float mag_heading = -M_PI_F / 7.f;
Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f);
_sensor_simulator._mag.setData(mag_data);
_sensor_simulator.runSeconds(_init_duration_s + 10.f);
float decl_deg = 0.f;
_ekf->get_mag_decl_deg(decl_deg);
// THEN: the fusion initializes using the mag data and runs normally
EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f));
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
// BUT WHEN: the mag field inclination suddenly changes
mag_data(2) = -mag_data(2);
_sensor_simulator._mag.setData(mag_data);
_sensor_simulator.runSeconds(6.f);
// THEN: the mag fusion should stop after some time
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
}