diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 075401e7a8..ef64838f6b 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -131,7 +131,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) { resetVelUsingAirspeed(airspeed_sample); - } else if (!_external_wind_init + } else if (!_external_wind_init && !_synthetic_airspeed && (!_control_status.flags.wind || getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) { resetWindUsingAirspeed(airspeed_sample); @@ -212,6 +212,10 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour K.slice(State::wind_vel.idx, 0) = K_wind; } + if (_synthetic_airspeed) { + K.slice(State::wind_vel.idx, 0) = 0.f; + } + measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); aid_src.fused = true; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index cee2fd05af..ca01003eae 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1259,16 +1259,6 @@ void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const } #endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_WIND) - - if (!_control_status.flags.wind) { - for (unsigned i = 0; i < State::wind_vel.dof; i++) { - K(State::wind_vel.idx + i) = 0.f; - } - } - -#endif // CONFIG_EKF2_WIND } float Ekf::getHeadingInnov() const diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5ff02ca7ab..592b786a6e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -105,6 +105,7 @@ public: #if defined(CONFIG_EKF2_AIRSPEED) void setAirspeedData(const airspeedSample &airspeed_sample); + void setSyntheticAirspeed(const bool synthetic_airspeed) { _synthetic_airspeed = synthetic_airspeed; } #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -435,6 +436,7 @@ protected: #if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer {nullptr}; + bool _synthetic_airspeed{false}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 2e84439fb2..974e748719 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2066,8 +2066,11 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) if (_airspeed_validated_sub.update(&airspeed_validated)) { if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && (airspeed_validated.selected_airspeed_index > 0) + && (airspeed_validated.airspeed_source > airspeed_validated_s::GROUND_MINUS_WIND) ) { + + _ekf.setSyntheticAirspeed(airspeed_validated.airspeed_source == airspeed_validated_s::SYNTHETIC); + float cas2tas = 1.f; if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d905fb8570..25ff9935ca 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -218,7 +218,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); const Vector3f vel = _ekf->getVelocity(); - EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-2f); + EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 0.05f); const Vector2f vel_wind_earth = _ekf->getWindVelocity(); EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);