dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: allow initialization without baro data when fusion is disabled

This commit is contained in:
bresch 2022-08-08 17:03:51 +02:00 committed by Daniel Agar
parent a54fa7b9b1
commit fe4a6ce8de
1 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/ekf2/EKF/ekf.cpp
View File

@ -161,29 +161,32 @@ bool Ekf::initialiseFilter()
}
// 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_sample_delayed.time_us != 0) {
if (_baro_counter == 0) {
_baro_lpf.reset(_baro_sample_delayed.hgt);
if (_baro_buffer) {
baroSample baro_sample;
} else {
_baro_lpf.update(_baro_sample_delayed.hgt);
if (_baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
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) {
// not enough baro samples accumulated
return false;
if (_params.baro_ctrl != 0) {
if (_baro_counter < _obs_buffer_length) {
// 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()) {
return false;
}