diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 519bcf6a1d..c7bd08c528 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -256,7 +256,6 @@ public: const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; } const imuSample &get_imu_sample_newest() const { return _newest_high_rate_imu_sample; } - const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; } const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; } @@ -302,7 +301,6 @@ protected: imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon // measurement samples capturing measurements on the delayed time horizon - baroSample _baro_sample_delayed{}; gpsSample _gps_sample_delayed{}; sensor::SensorRangeFinder _range_sensor{}; airspeedSample _airspeed_sample_delayed{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 5f8fb5219c..f286c2f990 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -769,7 +769,7 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) { - _estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_baro_sample_delayed().time_us, timestamp, + _estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp, _device_id_baro)); _last_baro_bias_published = status.bias; diff --git a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp index fe16477be6..a3e0e77186 100644 --- a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp @@ -51,29 +51,5 @@ public: TEST_F(EkfMeasurementSamplingTest, baroDownSampling) { - // Need to send imu samples to initialise - imuSample imu_sample; - - // WHEN: baro data = {1, -1 , 1, -1, ...} being set at high rate - int baro_rate_Hz = 10000; - uint64_t time = 0; - float baro_data = 1.0f; - - for (int i = 0; i < 2 * baro_rate_Hz; i++) { - if (i % 100 == 0) { - // send imu data at a 100 times lower rate - imu_sample.time_us = time; - _ekf->setIMUData(imu_sample); - } - - _ekf->setBaroData(baroSample{time, baro_data}); - baro_data *= -1.0f; - time += 1000000 / baro_rate_Hz; - } - - _ekf->update(); - - // THEN: average and buffered baro dato should be close to zero - baroSample baro_sample_from_buffer = _ekf->get_baro_sample_delayed(); - EXPECT_NEAR(baro_sample_from_buffer.hgt, 0.0f, 0.01f); + //TODO: implement }