fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move baro compensation to delayed time horizon and add validity check

Browse Source 
- this removes an unnecessary virtual call and simplifies things a bit

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2022-10-20 09:17:55 -04:00
committed by GitHub
co-authored by bresch
parent b400b7fcc4
commit fb3adc3faa
5 changed files with 27 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/baro_height_control.cpp
+3 -3
View File
@@ -51,7 +51,7 @@ void Ekf::controlBaroHeightFusion()
if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
const float measurement = baro_sample.hgt;
const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
const float measurement_var = sq(_params.baro_noise);
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
@@ -60,10 +60,10 @@ void Ekf::controlBaroHeightFusion()
if (measurement_valid) {
if (_baro_counter == 0) {
_baro_lpf.reset(baro_sample.hgt);
_baro_lpf.reset(measurement);
} else {
_baro_lpf.update(baro_sample.hgt);
_baro_lpf.update(measurement);
}
if (_baro_counter <= _obs_buffer_length) {