fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 03:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move baro downsampling and dynamic pressure comp to ECL

Browse Source
This commit is contained in:
kamilritz
2020-01-09 11:46:16 +01:00
committed by Roman Bapst
parent d7d7b56519
commit 29c5954690
5 changed files with 62 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+16 -5
View File
@@ -243,7 +243,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
}
}
void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
{
if (!_initialised || _baro_buffer_fail) {
return;
@@ -260,19 +260,30 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
}
}
// 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;
// limit data rate to prevent data being lost
if ((time_usec - _time_last_baro) > _min_obs_interval_us) {
const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count;
baroSample baro_sample_new;
baro_sample_new.hgt = data;
baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
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 -= _params.baro_delay_ms * 1000;
baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_baro = time_usec;
baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
_baro_buffer.push(baro_sample_new);
_time_last_baro = time_usec;
_baro_sample_count = 0;
_baro_alt_sum = 0.0f;
}
}