Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

GNSS yaw: add observation jump on ground

Browse Source 
Some receivers are initializing to some heading and then resetting to
the correct one after a couple of seconds. EKF2 should detect that and
reset to the new value
This commit is contained in:
bresch
2021-08-02 14:24:08 +02:00
committed by Mathieu Bresciani
parent 30c7a596af
commit 3fe04a91f6
1 changed files with 20 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_gps_yaw.cpp
+20 -1
View File
@@ -213,7 +213,26 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}
TEST_F(EkfGpsHeadingTest, yaw_jump)
TEST_F(EkfGpsHeadingTest, yaw_jump_on_ground)
{
// GIVEN: the GPS yaw fusion activated
float gps_heading = _ekf_wrapper.getYawAngle();
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(1);
_ekf->set_in_air_status(false);
// WHEN: the measurement suddenly changes
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f));
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(2);
// THEN: the fusion should reset
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}
TEST_F(EkfGpsHeadingTest, yaw_jump_in_air)
{
// GIVEN: the GPS yaw fusion activated
float gps_heading = _ekf_wrapper.getYawAngle();