diff --git a/EKF/common.h b/EKF/common.h index 67ce472be3..f240060087 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -277,6 +277,7 @@ struct parameters { // airspeed fusion float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) float eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s) + float arsp_thr{0.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion // synthetic sideslip fusion float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index d94cd57dbc..4736569ac3 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -478,9 +478,14 @@ void EstimatorInterface::setDragData(const imuSample &imu) bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle - float max_time_delay_ms = math::max(_params.baro_delay_ms, - math::max(_params.auxvel_delay_ms, - _params.airspeed_delay_ms)); + // it's reasonable to assume that barometer is always used, and its delay is low + // it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used + float max_time_delay_ms = math::max(_params.baro_delay_ms, _params.auxvel_delay_ms); + + // using airspeed + if (_params.arsp_thr > FLT_EPSILON) { + max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms); + } // mag mode if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {