fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 09:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Compute the timestamp of average sample correctly

Browse Source
This commit is contained in:
kamilritz
2020-01-21 10:09:48 +03:00
committed by Roman Bapst
parent 29c5954690
commit 4be9ae8029
3 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+3 -1
View File
@@ -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;
}
}