Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

ekf2: fix gnss yaw unit test

Browse Source
This commit is contained in:
bresch
2024-03-19 13:55:51 +01:00
committed by Mathieu Bresciani
parent 37caddedbb
commit c9221b91ad
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_gps_yaw.cpp
+4 -3
View File
@@ -81,9 +81,10 @@ public:
void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad) void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad)
{ {
// GIVEN: an initial GPS yaw, not aligned with the current one // GIVEN: an initial GPS yaw, not aligned with the current one
float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad); // The yaw antenna offset has already been corrected in the driver
float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle());
_sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator._gps.setYaw(gps_heading); // used to remove the correction to fuse the real measurement
_sensor_simulator._gps.setYawOffset(antenna_offset_rad); _sensor_simulator._gps.setYawOffset(antenna_offset_rad);
// WHEN: the GPS yaw fusion is activated // WHEN: the GPS yaw fusion is activated
@@ -91,7 +92,7 @@ void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float anten
_sensor_simulator.runSeconds(5); _sensor_simulator.runSeconds(5);
// THEN: the estimate is reset and stays close to the measurement // THEN: the estimate is reset and stays close to the measurement
checkConvergence(gps_heading, 0.05f); checkConvergence(gps_heading, 0.01f);
} }
void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance_deg) void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance_deg)