diff --git a/EKF/common.h b/EKF/common.h index 6d6537e7c4..a157296f56 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -186,6 +186,7 @@ struct parameters { int32_t sensor_interval_min_ms{20}; // minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. // measurement time delays + float min_delay_ms{0.0f}; // used to a set a minimum buffer length independent of specified sensor delays float mag_delay_ms{0.0f}; // magnetometer measurement delay relative to the IMU (msec) float baro_delay_ms{0.0f}; // barometer height measurement delay relative to the IMU (msec) float gps_delay_ms{110.0f}; // GPS measurement delay relative to the IMU (msec) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 94a51b9cdc..5bfe38f8e8 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -350,13 +350,14 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message bool EstimatorInterface::initialise_interface(uint64_t timestamp) { - // find the maximum time delay required to compensate for + // find the maximum time delay the buffers are required to handle uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms, math::max(_params.range_delay_ms, math::max(_params.gps_delay_ms, math::max(_params.flow_delay_ms, math::max(_params.ev_delay_ms, - math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))); + math::max(_params.min_delay_ms, + math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))))); // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;