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

ekf2: reinit baro height on sensor or calibration change

Browse Source 
- handle reset on delayed time horizon
This commit is contained in:
Daniel Agar
2022-12-29 16:37:34 -05:00
parent 0e2df5a152
commit 20a1c73cf0
4 changed files with 9 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/baro_height_control.cpp
+4 -3
View File
@@ -59,17 +59,18 @@ void Ekf::controlBaroHeightFusion()
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
if (measurement_valid) {
if (_baro_counter == 0) {
if ((_baro_counter == 0) || baro_sample.reset) {
_baro_lpf.reset(measurement);
_baro_counter = 1;
} else {
_baro_lpf.update(measurement);
_baro_counter++;
}
if (_baro_counter <= _obs_buffer_length) {
// Initialize the pressure offset (included in the baro bias)
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
_baro_counter++;
}
}
@@ -108,10 +109,10 @@ void Ekf::controlBaroHeightFusion()
// determine if we should use height aiding
const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
&& measurement_valid
&& (_baro_counter > _obs_buffer_length)
&& !_baro_hgt_faulty;
const bool starting_conditions_passing = continuing_conditions_passing
&& (_baro_counter > _obs_buffer_length)
&& isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
if (_control_status.flags.baro_hgt) {