diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index c609169ce2..1f39dd0600 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -114,7 +114,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) ECL_INFO("starting airspeed fusion"); // If starting wind state estimation, reset the wind states and covariances before fusing any data - if (!_control_status.flags.wind) { + // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. + const Vector2f wind_var_xy = getWindVelocityVariance(); + + if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) { // activate the wind states _control_status.flags.wind = true; // reset the wind speed states and corresponding covariances diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index aaaea1cafc..900fb15aa3 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -85,6 +85,21 @@ bool EkfWrapper::isIntendingExternalVisionHeightFusion() const return _ekf->control_status_flags().ev_hgt; } +void EkfWrapper::enableBetaFusion() +{ + _ekf_params->beta_fusion_enabled = true; +} + +void EkfWrapper::disableBetaFusion() +{ + _ekf_params->beta_fusion_enabled = false; +} + +bool EkfWrapper::isIntendingBetaFusion() const +{ + return _ekf->control_status_flags().fuse_beta; +} + void EkfWrapper::enableGpsFusion() { _ekf_params->gnss_ctrl |= GnssCtrl::HPOS | GnssCtrl::VEL; diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index bbb7654162..73ea368166 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -69,6 +69,10 @@ public: /* void disableExternalVisionHeightFusion(); */ bool isIntendingExternalVisionHeightFusion() const; + void enableBetaFusion(); + void disableBetaFusion(); + bool isIntendingBetaFusion() const; + void enableGpsFusion(); void disableGpsFusion(); bool isIntendingGpsFusion() const; diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 9ef44b03f0..c92a3170ec 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -131,3 +131,38 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) EXPECT_NEAR(height_after_pressure_correction, expected_height_after_pressure_correction, 1e-2f); } + +TEST_F(EkfAirspeedTest, testResetWindUsingAirspeed) +{ + const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f); + const Vector2f airspeed_body(15.f, 0.0f); + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator._gps.setVelocity(simulated_velocity_earth); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->set_is_fixed_wing(true); + + // Simulate the fact that the sideslip can start immediately, without + // waiting for a measurement sample. + _ekf_wrapper.enableBetaFusion(); + _sensor_simulator.runSeconds(0.1); + EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); + + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0)); + _sensor_simulator.runSeconds(0.1); + + EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); + + const Vector2f vel_wind_earth = _ekf->getWindVelocity(); + + const Dcmf R_to_earth_sim(_quat_sim); + const Vector3f vel_wind_expected = simulated_velocity_earth - R_to_earth_sim * (Vector3f(airspeed_body(0), + airspeed_body(1), 0.0f)); + + EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1.f); + EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1.f); +}