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

ekf2-test-gnss-yaw: add unit test for fix type requirement

Browse Source
This commit is contained in:
bresch
2023-07-13 14:44:00 +02:00
parent 141c6d77b7
commit cce7ee2c6a
1 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_gps_yaw.cpp
+25
View File
@@ -69,6 +69,7 @@ public:
_sensor_simulator.runSeconds(_init_duration_s);
_sensor_simulator._gps.setYaw(NAN);
_sensor_simulator._gps.setFixType(6);
_sensor_simulator.runSeconds(2);
_ekf_wrapper.enableGpsFusion();
_ekf_wrapper.enableGpsHeadingFusion();
@@ -218,6 +219,30 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}
TEST_F(EkfGpsHeadingTest, fixTypeDrop)
{
// GIVEN: valid yaw data with a good fix
float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle());
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator._gps.setFixType(6); // "RTk fixed"
// WHEN: the GPS yaw fusion is activated
_sensor_simulator.runSeconds(1);
// THEN: GPS heading fusion should have started, and mag
// fusion should be disabled
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
// BUT WHEN: the GPS fix drops to "RTK float"
_sensor_simulator._gps.setFixType(5);
_sensor_simulator.runSeconds(1);
// THEN: the fusion should stop and
// the estimator should fall back to mag fusion
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
}
TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator)
{
// GIVEN: an initial GPS yaw, not aligned with the current one (e.g.: wrong orientation of the antenna array) and no mag.