diff --git a/EKF/common.h b/EKF/common.h index 553778ebe2..da6906b413 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -173,6 +173,7 @@ struct parameters { // measurement source control int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used int vdist_sensor_type; // selects the primary source for height data + int sensor_interval_min_ms; // minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. // measurement time delays float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec) @@ -275,6 +276,7 @@ struct parameters { // measurement source control fusion_mode = MASK_USE_GPS; vdist_sensor_type = VDIST_SENSOR_BARO; + sensor_interval_min_ms = 20; // measurement time delays mag_delay_ms = 0.0f; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ada905ccfa..95318c0e36 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -48,6 +48,8 @@ EstimatorInterface::EstimatorInterface(): + OBS_BUFFER_LENGTH(10), + IMU_BUFFER_LENGTH(30), _min_obs_interval_us(0), _dt_imu_avg(0.0f), _imu_ticks(0), @@ -340,6 +342,22 @@ 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 + 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)))))); + + // 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; + + // set the observaton buffer length to handle the minimum time of arrival between observations + OBS_BUFFER_LENGTH = (max_time_delay_ms / _params.sensor_interval_min_ms) + 1; + + ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH); + ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH); if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) && _gps_buffer.allocate(OBS_BUFFER_LENGTH) && diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6265324250..71764332a4 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -273,8 +273,21 @@ protected: parameters _params; // filter parameters - static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer - static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer + /* + OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer + which sets the maximum frequency at which we can process non-IMU measurements. Measurements that + arrive too soon after the previous measurement will not be processed. + max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001) + This can be adjusted to match the max sensor data rate plus some margin for jitter. + */ + uint8_t OBS_BUFFER_LENGTH; + /* + IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the + EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for. + max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS + This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay. + */ + uint8_t IMU_BUFFER_LENGTH; static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)