diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 4a224c0aba..d3b98d1ec3 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -476,8 +476,9 @@ void Ekf2::task_main() } // read airspeed data if available - if (airspeed_updated) { - _ekf->setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s); // Only TAS is now fed into the estimator + float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; + if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) { + _ekf->setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas); } if (optical_flow_updated) { @@ -533,6 +534,7 @@ void Ekf2::task_main() ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); + // Airspeed - take airspeed measurement directly here as no wind is estimated if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 && airspeed.timestamp > 0) {