From d785a19c0a84d7a89621fe4422f94733e732bde6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 16 Nov 2017 08:53:30 +1100 Subject: [PATCH] EKF: delay final allocation of observation buffers until required --- EKF/RingBuffer.h | 5 ++ EKF/estimator_interface.cpp | 113 ++++++++++++++++++++++++++++++------ EKF/estimator_interface.h | 20 +++++++ 3 files changed, 120 insertions(+), 18 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 0bfedef179..aeaa1564cd 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -151,6 +151,11 @@ public: + // return ttrue if allocated + unsigned is_allocated() + { + return (_buffer != NULL); + } private: data_type *_buffer{nullptr}; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a7969b4e27..0b155c72bd 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -94,9 +94,16 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u _imu_ticks = 0; _imu_updated = true; + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_drag_buffer_pass && !_drag_buffer_fail) { + _drag_buffer_pass = _drag_buffer.allocate(_obs_buffer_length); + _drag_buffer_fail = !_drag_buffer_pass; + } + // down-sample the drag specific force data by accumulating and calculating the mean when // sufficient samples have been collected - if (_params.fusion_mode & MASK_USE_DRAG) { + if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer_pass) { _drag_sample_count ++; // note acceleration is accumulated as a delta velocity _drag_down_sampled.accelXY(0) += imu_sample_new.delta_vel(0); @@ -119,7 +126,6 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u _drag_down_sampled.accelXY(1) /= _drag_sample_time_dt; _drag_down_sampled.time_us /= _drag_sample_count; - // write to buffer _drag_buffer.push(_drag_down_sampled); // reset accumulators @@ -146,8 +152,24 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) { + if (!_initialised) { + return; + } + + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_mag_buffer_pass && !_mag_buffer_fail) { + _mag_buffer_pass = _mag_buffer.allocate(_obs_buffer_length); + _mag_buffer_fail = !_mag_buffer_pass; + + if (_mag_buffer_fail) { + ECL_ERR("EKF mag buffer allocation failed"); + + } + } + // limit data rate to prevent data being lost - if (time_usec - _time_last_mag > _min_obs_interval_us) { + if ((time_usec - _time_last_mag > _min_obs_interval_us) && _mag_buffer_pass) { magSample mag_sample_new; mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000; @@ -158,6 +180,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) mag_sample_new.mag = Vector3f(data); _mag_buffer.push(mag_sample_new); + } } @@ -167,6 +190,17 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) return; } + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_gps_buffer_pass && !_gps_buffer_fail) { + _gps_buffer_pass = _gps_buffer.allocate(_obs_buffer_length); + _gps_buffer_fail = !_gps_buffer_pass; + } + + if (!_gps_buffer_pass) { + 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); @@ -198,6 +232,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) } else { gps_sample_new.pos(0) = 0.0f; gps_sample_new.pos(1) = 0.0f; + } _gps_buffer.push(gps_sample_new); @@ -210,8 +245,15 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data) return; } + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_baro_buffer_pass && !_baro_buffer_fail) { + _baro_buffer_pass = _baro_buffer.allocate(_obs_buffer_length); + _baro_buffer_fail = !_baro_buffer_pass; + } + // limit data rate to prevent data being lost - if (time_usec - _time_last_baro > _min_obs_interval_us) { + if ((time_usec - _time_last_baro > _min_obs_interval_us) && _baro_buffer_pass) { baroSample baro_sample_new; baro_sample_new.hgt = data; @@ -232,8 +274,15 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed return; } + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_airspeed_buffer_pass && !_airspeed_buffer_fail) { + _airspeed_buffer_pass = _airspeed_buffer.allocate(_obs_buffer_length); + _airspeed_buffer_fail = !_airspeed_buffer_pass; + } + // limit data rate to prevent data being lost - if (time_usec - _time_last_airspeed > _min_obs_interval_us) { + if ((time_usec - _time_last_airspeed > _min_obs_interval_us) && _airspeed_buffer_pass) { airspeedSample airspeed_sample_new; airspeed_sample_new.true_airspeed = true_airspeed; airspeed_sample_new.eas2tas = eas2tas; @@ -251,8 +300,15 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data) return; } + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_range_buffer_pass && !_range_buffer_fail) { + _range_buffer_pass = _range_buffer.allocate(_obs_buffer_length); + _range_buffer_fail = !_range_buffer_pass; + } + // limit data rate to prevent data being lost - if (time_usec - _time_last_range > _min_obs_interval_us) { + if ((time_usec - _time_last_range > _min_obs_interval_us) && _range_buffer_pass) { rangeSample range_sample_new; range_sample_new.rng = data; range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000; @@ -269,8 +325,15 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl return; } + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_flow_buffer_pass && !_flow_buffer_fail) { + _flow_buffer_pass = _flow_buffer.allocate(_obs_buffer_length); + _flow_buffer_fail = !_flow_buffer_pass; + } + // limit data rate to prevent data being lost - if (time_usec - _time_last_optflow > _min_obs_interval_us) { + if ((time_usec - _time_last_optflow > _min_obs_interval_us) && _flow_buffer_pass) { // check if enough integration time and fail if integration time is less than 50% // of min arrival interval because too much data is being lost float delta_time = 1e-6f * (float)flow->dt; @@ -300,8 +363,10 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl flowSample optflow_sample_new; // calculate the system time-stamp for the mid point of the integration period optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2; + // copy the quality metric returned by the PX4Flow sensor optflow_sample_new.quality = flow->quality; + // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis. // copy the optical and gyro measured delta angles optflow_sample_new.gyroXYZ = - flow->gyrodata; @@ -319,10 +384,11 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // compensate for body motion to give a LOS rate optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0); optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1); + // convert integration interval to seconds optflow_sample_new.dt = delta_time; _time_last_optflow = time_usec; - // push to buffer + _flow_buffer.push(optflow_sample_new); } } @@ -335,20 +401,30 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message return; } + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (!_ev_buffer_pass && !_ev_buffer_fail) { + _ev_buffer_pass = _ext_vision_buffer.allocate(_obs_buffer_length); + _ev_buffer_fail = !_ev_buffer_pass; + } + // limit data rate to prevent data being lost - if (time_usec - _time_last_ext_vision > _min_obs_interval_us) { + if ((time_usec - _time_last_ext_vision > _min_obs_interval_us) && _ev_buffer_pass) { 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; + // copy required data ev_sample_new.angErr = evdata->angErr; ev_sample_new.posErr = evdata->posErr; ev_sample_new.quat = evdata->quat; ev_sample_new.posNED = evdata->posNED; + // record time for comparison next measurement _time_last_ext_vision = time_usec; - // push to buffer + _ext_vision_buffer.push(ev_sample_new); + } } @@ -376,14 +452,14 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length); if (!(_imu_buffer.allocate(_imu_buffer_length) && - _gps_buffer.allocate(_obs_buffer_length) && - _mag_buffer.allocate(_obs_buffer_length) && - _baro_buffer.allocate(_obs_buffer_length) && - _range_buffer.allocate(_obs_buffer_length) && - _airspeed_buffer.allocate(_obs_buffer_length) && - _flow_buffer.allocate(_obs_buffer_length) && - _ext_vision_buffer.allocate(_obs_buffer_length) && - _drag_buffer.allocate(_obs_buffer_length) && + _gps_buffer.allocate(1) && + _mag_buffer.allocate(1) && + _baro_buffer.allocate(1) && + _range_buffer.allocate(1) && + _airspeed_buffer.allocate(1) && + _flow_buffer.allocate(1) && + _ext_vision_buffer.allocate(1) && + _drag_buffer.allocate(1) && _output_buffer.allocate(_imu_buffer_length) && _output_vert_buffer.allocate(_imu_buffer_length))) { ECL_ERR("EKF buffer allocation failed!"); @@ -455,6 +531,7 @@ void EstimatorInterface::unallocate_buffers() _ext_vision_buffer.unallocate(); _output_buffer.unallocate(); _output_vert_buffer.unallocate(); + _drag_buffer.unallocate(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 7ddc8bdc8a..3e80647355 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -453,6 +453,26 @@ protected: RingBuffer _output_vert_buffer; RingBuffer _drag_buffer; + // observation buffer final allocation failed + bool _gps_buffer_fail{false}; + bool _mag_buffer_fail{false}; + bool _baro_buffer_fail{false}; + bool _range_buffer_fail{false}; + bool _airspeed_buffer_fail{false}; + bool _flow_buffer_fail{false}; + bool _ev_buffer_fail{false}; + bool _drag_buffer_fail{false}; + + // observation buffer final allocation succeeded + bool _gps_buffer_pass{false}; + bool _mag_buffer_pass{false}; + bool _baro_buffer_pass{false}; + bool _range_buffer_pass{false}; + bool _airspeed_buffer_pass{false}; + bool _flow_buffer_pass{false}; + bool _ev_buffer_pass{false}; + bool _drag_buffer_pass{false}; + uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds uint64_t _time_last_gps{0}; // timestamp of last gps measurement in microseconds uint64_t _time_last_mag{0}; // timestamp of last magnetometer measurement in microseconds