EKF: improvements to observation collection

Remove unnecessary function calls
Remove un-used functions
Apply a consistent maximum safe data rate check for all observation inputs
This commit is contained in:
Paul Riseborough
2016-05-26 13:27:08 +10:00
parent e2d9e19a5d
commit d5b5cb5899
2 changed files with 35 additions and 30 deletions
+3 -10
View File
@@ -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