From 699edd25357dde0eeef896224ae49983414d87a6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Nov 2016 08:08:17 +1100 Subject: [PATCH] ekf2: Prevent loss of baro data due to buffer time arrival checks Baro data arriving too soon after the last measurement due to a high sampling rate or timing jitter is rejected inside the ecl EKF to prevent the data buffer overflowing. This patch checks the timestamp difference from the last measurement, and if to small, the data is accumulated and the average sent to the EKF when the time delta is acceptable. --- src/modules/ekf2/ekf2_main.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index f261e871ca..6738ae6375 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -145,6 +145,12 @@ private: uint8_t _mag_sample_count = 0; // number of magnetometer measurements summed uint32_t _mag_time_ms_last_used = 0; // time stamp in msec of the last averaged magnetometer measurement used by the EKF + // Used to down sample barometer data + float _balt_data_sum; // summed barometric altitude readings (m) + uint64_t _balt_time_sum_ms; // summed barometric altitude time stamps (msec) + uint8_t _balt_sample_count = 0; // number of barometric altitude measurements summed + uint32_t _balt_time_ms_last_used = 0; // time stamp in msec of the last averaged barometric altitude measurement used by the EKF + int _sensors_sub = -1; int _gps_sub = -1; int _airspeed_sub = -1; @@ -571,9 +577,23 @@ void Ekf2::task_main() } else { if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) { - _timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative; - _ekf.setBaroData(_timestamp_balt_us, &sensors.baro_alt_meter); + _timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative; + // If the time last used by the EKF is less than 50msec, then accumulate the + // data and push the average when the 50msec is reached. + _balt_time_sum_ms += _timestamp_balt_us/1000; + _balt_sample_count++; + _balt_data_sum += sensors.baro_alt_meter; + uint64_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count; + if (balt_time_ms - _balt_time_ms_last_used > 50) { + float balt_data_avg = _balt_data_sum / (float)_balt_sample_count; + _ekf.setBaroData(1000*(uint64_t)balt_time_ms, &balt_data_avg); + _balt_time_ms_last_used = balt_time_ms; + _balt_time_sum_ms = 0; + _balt_sample_count = 0; + _balt_data_sum = 0.0f; + + } } }