From 3fe04a91f6758863e8225a76150a7d4ef77b2f48 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Aug 2021 14:24:08 +0200 Subject: [PATCH] GNSS yaw: add observation jump on ground 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 --- src/modules/ekf2/test/test_EKF_gps_yaw.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index c67200170d..c7623f5795 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -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();