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

ekf2: allow initialization without baro data when fusion is disabled

Browse Source
This commit is contained in:
bresch
2022-09-10 12:29:29 -04:00
committed by Daniel Agar
parent a54fa7b9b1
commit fe4a6ce8de
1 changed files with 18 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+18 -15
View File
@@ -161,29 +161,32 @@ bool Ekf::initialiseFilter()
} }
// accumulate enough height measurements to be confident in the quality of the data // accumulate enough height measurements to be confident in the quality of the data
if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_baro_buffer) {
if (_baro_sample_delayed.time_us != 0) { baroSample baro_sample;
if (_baro_counter == 0) {
_baro_lpf.reset(_baro_sample_delayed.hgt);
} else { if (_baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
_baro_lpf.update(_baro_sample_delayed.hgt); if (baro_sample.time_us != 0) {
if (_baro_counter == 0) {
_baro_lpf.reset(baro_sample.hgt);
_baro_b_est.setBias(_baro_lpf.getState()); } else {
_baro_lpf.update(baro_sample.hgt);
_baro_b_est.setBias(_baro_lpf.getState());
}
_baro_counter++;
} }
_baro_counter++;
} }
} }
if (_baro_counter < _obs_buffer_length) { if (_params.baro_ctrl != 0) {
// not enough baro samples accumulated if (_baro_counter < _obs_buffer_length) {
return false; // not enough baro samples accumulated
return false;
}
} }
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
_control_status.flags.baro_hgt = false;
if (!initialiseTilt()) { if (!initialiseTilt()) {
return false; return false;
} }