From e9d43015ce6744d9516a5a53215349fd6a307596 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 15 Feb 2024 12:59:21 +0100 Subject: [PATCH] ekf2: fix unit tests failing due to mag fusion changes --- src/modules/ekf2/test/test_EKF_basics.cpp | 6 ++++-- src/modules/ekf2/test/test_EKF_drag_fusion.cpp | 6 ++++-- src/modules/ekf2/test/test_EKF_flow.cpp | 4 ++-- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 928cb24e03..c7a8ffa836 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -107,7 +107,8 @@ TEST_F(EkfBasicsTest, initialControlMode) EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); - EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec); EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt); @@ -161,7 +162,8 @@ TEST_F(EkfBasicsTest, gpsFusion) EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); - EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec); EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt); diff --git a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp index ca03a0f5e7..edd81e640c 100644 --- a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp @@ -144,8 +144,10 @@ TEST_F(EkfDragFusionTest, testLateralMomentumDrag) predicted_accel(0) = CONSTANTS_ONE_G * sinf(pitch); predicted_accel(1) = - CONSTANTS_ONE_G * sinf(roll); Vector2f wind_speed = predicted_accel / mcoef; - EXPECT_NEAR(vel_wind_earth(0), wind_speed(0), fmaxf(1.0f, 0.1f * fabsf(wind_speed(0)))); - EXPECT_NEAR(vel_wind_earth(1), wind_speed(1), fmaxf(1.0f, 0.1f * fabsf(wind_speed(1)))); + // Note that the wind direction is stightly incorrect heading estimate due to a mismatch between + // the simulated mag field and assumed dectination from the WMM + EXPECT_NEAR(vel_wind_earth(0), wind_speed(0), fmaxf(1.0f, 0.15f * fabsf(wind_speed.norm()))); + EXPECT_NEAR(vel_wind_earth(1), wind_speed(1), fmaxf(1.0f, 0.15f * fabsf(wind_speed.norm()))); }; TEST_F(EkfDragFusionTest, testForwardBluffBodyDrag) diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 645a080743..9d3df6cbd8 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -241,7 +241,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData) _sensor_simulator.runSeconds(5.f); // AND WHEN: there is a pure yaw rotation - const Vector3f body_rate(0.f, 0.f, 3.14159f); + const Vector3f body_rate(0.f, 0.f, 2.9f); const Vector3f flow_offset(0.15, -0.05f, 0.2f); _ekf_wrapper.setFlowOffset(flow_offset); @@ -278,7 +278,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData) _sensor_simulator.runSeconds(5.f); // AND WHEN: there is a pure yaw rotation - const Vector3f body_rate(0.f, 0.f, 3.14159f); + const Vector3f body_rate(0.f, 0.f, 2.9f); const Vector3f flow_offset(-0.15, 0.05f, 0.2f); _ekf_wrapper.setFlowOffset(flow_offset);