diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 398ce39177..f615e4ab86 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -48,6 +48,7 @@ EstimatorInterface::EstimatorInterface(): + _min_obs_interval_us(0), _dt_imu_avg(0.0f), _imu_ticks(0), _imu_updated(false), @@ -103,30 +104,35 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u memcpy(&imu_sample_new.delta_ang._data[0], delta_ang, sizeof(imu_sample_new.delta_ang._data)); memcpy(&imu_sample_new.delta_vel._data[0], delta_vel, sizeof(imu_sample_new.delta_vel._data)); - //convert time from us to secs + // convert time from us to secs imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f; imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f; imu_sample_new.time_us = time_usec; _imu_ticks++; - + // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available if (collect_imu(imu_sample_new)) { _imu_buffer.push(imu_sample_new); _imu_ticks = 0; _imu_updated = true; + // get the oldest data from the buffer + _imu_sample_delayed = _imu_buffer.get_oldest(); + + // calculate the minimum interval between observations required to guarantee no loss of data + // this will occur if data is overwritten before its time stamp falls behind the fusion time horizon + _min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(OBS_BUFFER_LENGTH - 1); + } else { _imu_updated = false; - } - - _imu_sample_delayed = _imu_buffer.get_oldest(); + } } void EstimatorInterface::setMagData(uint64_t time_usec, float *data) { - - if (time_usec - _time_last_mag > 70000) { + // limit data rate to prevent data being lost + if (time_usec - _time_last_mag > _min_obs_interval_us) { magSample mag_sample_new = {}; mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000; @@ -143,9 +149,13 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data) void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) { - // Limit the GPS data rate to a maximum of 14Hz + if (!_initialised) { + return; + } + + // limit data rate to prevent data being lost bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); - if (time_usec - _time_last_gps > 65000 && need_gps) { + if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps) { gpsSample gps_sample_new = {}; gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; @@ -182,11 +192,12 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) void EstimatorInterface::setBaroData(uint64_t time_usec, float *data) { - if (!collect_baro(time_usec, data) || !_initialised) { + if (!_initialised) { return; } - if (time_usec - _time_last_baro > 68000) { + // limit data rate to prevent data being lost + if (time_usec - _time_last_baro > _min_obs_interval_us) { baroSample baro_sample_new; baro_sample_new.hgt = *data; @@ -203,11 +214,12 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data) void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *true_airspeed, float *eas2tas) { - if (!collect_airspeed(time_usec, true_airspeed, eas2tas) || !_initialised) { + if (!_initialised) { return; } - if (time_usec - _time_last_airspeed > 80000) { + // limit data rate to prevent data being lost + if (time_usec - _time_last_airspeed > _min_obs_interval_us) { airspeedSample airspeed_sample_new; airspeed_sample_new.true_airspeed = *true_airspeed; airspeed_sample_new.eas2tas = *eas2tas; @@ -222,11 +234,12 @@ static float rng; // set range data void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) { - if (!collect_range(time_usec, data) || !_initialised) { + if (!_initialised) { return; } - if (time_usec - _time_last_range > 45000) { + // limit data rate to prevent data being lost + if (time_usec - _time_last_range > _min_obs_interval_us) { rangeSample range_sample_new = {}; range_sample_new.rng = *data; rng = *data; @@ -242,12 +255,12 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) // set optical flow data void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow) { - if (!collect_opticalflow(time_usec, flow) || !_initialised) { + if (!_initialised) { return; } - // if data passes checks, push to buffer - if (time_usec - _time_last_optflow > 40000) { + // limit data rate to prevent data being lost + if (time_usec - _time_last_optflow > _min_obs_interval_us) { // check if enough integration time float delta_time = 1e-6f * (float)flow->dt; bool delta_time_good = (delta_time >= 0.05f); @@ -293,8 +306,8 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message return; } - // if data passes checks, push to buffer - if (time_usec > _time_last_ext_vision) { + // limit data rate to prevent data being lost + if (time_usec - _time_last_ext_vision > _min_obs_interval_us) { extVisionSample ev_sample_new; // calculate the system time-stamp for the mid point of the integration period ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index c2694c9b8f..364eb15882 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -111,18 +111,9 @@ public: // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } + // accumulate and downsample IMU data to the EKF prediction rate virtual bool collect_imu(imuSample &imu) { return true; } - virtual bool collect_mag(uint64_t time_usec, float *data) { return true; } - - virtual bool collect_baro(uint64_t time_usec, float *data) { return true; } - - virtual bool collect_airspeed(uint64_t time_usec, float *true_airspeed, float *eas2tas) { return true; } - - virtual bool collect_range(uint64_t time_usec, float *data) { return true; } - - virtual bool collect_opticalflow(uint64_t time_usec, flow_message *flow) { return true; } - // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); @@ -237,6 +228,8 @@ protected: static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer 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) + float _dt_imu_avg; // average imu update period in s imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon