fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 05:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: improvements to observation collection

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+32 -20
View File
@@ -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,34 @@ 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 required to guarantee no loss of data
_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 +148,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 +191,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 +213,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 +233,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 +254,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 +305,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;
EKF/estimator_interface.h
+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