dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: fix unit tests failing due to mag fusion changes

This commit is contained in:
bresch 2024-02-15 12:59:21 +01:00 committed by Daniel Agar
parent b46fc9a67d
commit e9d43015ce
3 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/test/test_EKF_basics.cpp
View File

@ -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);

6
src/modules/ekf2/test/test_EKF_drag_fusion.cpp
View File

@ -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)

4
src/modules/ekf2/test/test_EKF_flow.cpp
View File

@ -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);