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

GNSS yaw: add test case when measurement jumps

Browse Source
This commit is contained in:
bresch
2021-05-04 14:12:19 +02:00
committed by Paul Riseborough
parent 7a11871566
commit 7ee69706e8
1 changed files with 29 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_gps_yaw.cpp
+29 -1
View File
@@ -186,7 +186,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
// GPS yaw is expected to arrive a bit later, first feed some NANs
// to the filter
_sensor_simulator.runSeconds(6);
float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(10.f));
_sensor_simulator._gps.setYaw(gps_heading);
// WHEN: the GPS yaw fusion is activated
@@ -211,3 +211,31 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}
TEST_F(EkfGpsHeadingTest, yaw_jump)
{
// GIVEN: the GPS yaw fusion activated
float gps_heading = _ekf_wrapper.getYawAngle();
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(5);
_ekf->set_in_air_status(true);
// WHEN: the measurement suddenly changes
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(7.5);
// THEN: the fusion should reset
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
// BUT WHEN: the measurement jumps a 2nd time
gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(7.5);
// THEN: after a few seconds, the fusion should stop and
// the estimator should fall back to mag fusion
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
}