fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add test for external wind reset

Browse Source
This commit is contained in:
bresch 2025-12-15 16:44:39 +01:00 committed by Mathieu Bresciani
parent 2c044b327e
commit 6195629373
1 changed files with 50 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/modules/ekf2/test/test_EKF_airspeed.cpp
View File

@ -296,3 +296,53 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2));
}
TEST_F(EkfAirspeedTest, testExternalWindResetOnGround)
{
// WHEN: an external wind reset is performed before flight
const float wind_speed = 4.5f;
const float wind_speed_acc = 2.f;
const float wind_direction = math::radians(-90.f);
const float wind_direction_acc = math::radians(20.f);
_ekf->resetWindToExternalObservation(wind_speed, wind_direction, wind_speed_acc, wind_direction_acc);
Vector2f vel_wind_earth = _ekf->getWindVelocity();
EXPECT_EQ(wind_speed, vel_wind_earth.norm());
EXPECT_EQ(wind_direction, atan2f(vel_wind_earth(1), vel_wind_earth(0)));
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_ekf->set_is_fixed_wing(true);
_ekf_wrapper.enableBetaFusion();
_sensor_simulator.startAirspeedSensor();
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
const float measured_airspeed = 25.f;
_sensor_simulator._airspeed.setData(measured_airspeed, measured_airspeed);
// Since we are doing inertial dead-reckoning
// and that the measurement is inconsistent with the filter
EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
_sensor_simulator.runSeconds(1);
// THEN: the velocity is reset to the airspeed measurement
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
const Vector3f vel = _ekf->getVelocity();
const Vector2f air_relative_vel = vel.xy() - vel_wind_earth;
EXPECT_NEAR(air_relative_vel.norm(), measured_airspeed, 1e-3f);
// AND: wind estimate stayed close to its manually initialized value
vel_wind_earth = _ekf->getWindVelocity();
EXPECT_NEAR(wind_speed, vel_wind_earth.norm(), 0.1f);
EXPECT_NEAR(wind_direction, atan2f(vel_wind_earth(1), vel_wind_earth(0)), 0.1f);
EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
}