From b8a3ed5f09bd00f62e543adc8f790d432d2225b8 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 21 Jan 2020 14:47:43 +0100 Subject: [PATCH] Update baro interface --- EKF/common.h | 4 ++-- EKF/estimator_interface.cpp | 10 +++++----- EKF/estimator_interface.h | 2 +- test/sensor_simulator/baro.cpp | 3 ++- test/test_EKF_measurementSampling.cpp | 3 ++- 5 files changed, 12 insertions(+), 10 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 161c343674..0c08a5c021 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -128,8 +128,8 @@ struct magSample { }; struct baroSample { - float hgt{0.0f}; ///< pressure altitude above sea level (m) - uint64_t time_us{0}; ///< timestamp of the measurement (uSec) + float hgt; ///< pressure altitude above sea level (m) + uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct rangeSample { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 67a54c28b8..1140258ce7 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -245,7 +245,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps) } } -void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) +void EstimatorInterface::setBaroData(const baroSample &baro_sample) { if (!_initialised || _baro_buffer_fail) { return; @@ -265,11 +265,12 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) // downsample to highest possible sensor rate // by baro data by taking the average of incoming sample _baro_sample_count++; - _baro_alt_sum += baro_alt_meter; - _baro_timestamp_sum += time_usec / 1000; // Dividing by 1000 to avoid overflow + _baro_alt_sum += baro_sample.hgt; + _baro_timestamp_sum += baro_sample.time_us / 1000; // Dividing by 1000 to avoid overflow // limit data rate to prevent data being lost - if ((time_usec - _time_last_baro) > _min_obs_interval_us) { + if ((baro_sample.time_us - _time_last_baro) > _min_obs_interval_us) { + _time_last_baro = baro_sample.time_us; const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count; @@ -284,7 +285,6 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter) _baro_buffer.push(baro_sample_new); - _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 0dbf7bbeff..322521d107 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -181,7 +181,7 @@ public: void setGpsData(const gps_message &gps); - void setBaroData(uint64_t time_usec, float data); + void setBaroData(const baroSample &baro_sample); void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas); diff --git a/test/sensor_simulator/baro.cpp b/test/sensor_simulator/baro.cpp index c7f9b75750..a5cdde0b4e 100644 --- a/test/sensor_simulator/baro.cpp +++ b/test/sensor_simulator/baro.cpp @@ -15,7 +15,8 @@ Baro::~Baro() void Baro::send(uint64_t time) { - _ekf->setBaroData(time,_baro_data); + const baroSample baro_sample {_baro_data, time}; + _ekf->setBaroData(baro_sample); } void Baro::setData(float baro) diff --git a/test/test_EKF_measurementSampling.cpp b/test/test_EKF_measurementSampling.cpp index 4e37a0315d..beb78c96a0 100644 --- a/test/test_EKF_measurementSampling.cpp +++ b/test/test_EKF_measurementSampling.cpp @@ -66,7 +66,8 @@ TEST_F(EkfMeasurementSamplingTest, baroDownSampling) imu_sample.time_us = time; _ekf->setIMUData(imu_sample); } - _ekf->setBaroData(time, baro_data); + const baroSample baro_sample {baro_data, time}; + _ekf->setBaroData(baro_sample); baro_data *= -1.0f; time += 1000000 / baro_rate_Hz; }