From 4be9ae8029e5d13bbbbd971cb25f1364785270e1 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Fri, 10 Jan 2020 08:48:02 +0100 Subject: [PATCH] Compute the timestamp of average sample correctly --- EKF/common.h | 10 +++++----- EKF/estimator_interface.cpp | 4 +++- EKF/estimator_interface.h | 5 +++-- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 04480c5750..718377ba29 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -343,11 +343,11 @@ struct parameters { int32_t valid_timeout_max{5000000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) // static barometer pressure position error coefficient along body axes - float static_pressure_coef_xp {0.0f}; - float static_pressure_coef_xn {0.0f}; - float static_pressure_coef_yp {0.0f}; - float static_pressure_coef_yn {0.0f}; - float static_pressure_coef_z {0.0f}; + float static_pressure_coef_xp {0.0f}; // (-) + float static_pressure_coef_xn {0.0f}; // (-) + float static_pressure_coef_yp {0.0f}; // (-) + float static_pressure_coef_yn {0.0f}; // (-) + float static_pressure_coef_z {0.0f}; // (-) // upper limit on airspeed used for correction (m/s**2) float max_correction_airspeed {20.0f}; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index c10acc0cde..c8c2be9c47 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -264,6 +264,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) // by baro data by taking the average of incoming sample _baro_sample_count++; _baro_alt_sum += baro_alt_meter; + _baro_timestamp_sum += time_usec; // limit data rate to prevent data being lost if ((time_usec - _time_last_baro) > _min_obs_interval_us) { @@ -274,7 +275,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_alt_avg); // Use the time in the middle of the downsampling interval for the sample - baro_sample_new.time_us = _time_last_baro + (time_usec - _time_last_baro) / 2; + baro_sample_new.time_us = _baro_timestamp_sum / _baro_sample_count; baro_sample_new.time_us -= _params.baro_delay_ms * 1000; baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us); @@ -284,6 +285,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) _time_last_baro = time_usec; _baro_sample_count = 0; _baro_alt_sum = 0.0f; + _baro_timestamp_sum = 0; } } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 7960e0089f..19a133cfc1 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -550,8 +550,9 @@ protected: uint64_t _mag_timestamp_sum {0}; // Used to down sample barometer data - float _baro_alt_sum {0.0f}; ///< summed pressure altitude readings (m) - uint8_t _baro_sample_count {0}; ///< number of barometric altitude measurements summed + float _baro_alt_sum {0.0f}; // summed pressure altitude readings (m) + uint8_t _baro_sample_count {0}; // number of barometric altitude measurements summed + uint64_t _baro_timestamp_sum {0}; // summed timestamp to provide the timestamp of the averaged sample fault_status_u _fault_status{};