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

ekf2: refactor output predictor to class

Browse Source 
- refactor all EKF backend output predictor pieces into new OutputPredictor class
 - output states are now calculated immediately with new high rate IMU rather than after EKF update
 - IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear
This commit is contained in:
Daniel Agar
2023-01-18 10:59:34 -05:00
committed by GitHub
parent c054ca20cc
commit 95300d5637
39 changed files with 855 additions and 618 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
@@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion()
baroSample baro_sample;
if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
const float measurement_var = sq(_params.baro_noise);
@@ -135,7 +135,7 @@ void Ekf::controlBaroHeightFusion()
// reset vertical velocity
resetVerticalVelocityToZero();
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
aid_src.time_last_fuse = _time_delayed_us;
} else if (is_fusion_failing) {
// Some other height source is still working
@@ -164,7 +164,7 @@ void Ekf::controlBaroHeightFusion()
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
aid_src.time_last_fuse = _time_delayed_us;
bias_est.setFusionActive();
_control_status.flags.baro_hgt = true;
}