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

ekf2: move baro control logic to file

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent 12e25eba62
commit d87feee5b0
5 changed files with 137 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
-51
View File
@@ -759,57 +759,6 @@ bool Ekf::isConditionalRangeAidSuitable()
return is_range_aid_suitable;
}
void Ekf::controlBaroHeightFusion()
{
if (!(_params.baro_ctrl == 1)) {
stopBaroHgtFusion();
return;
}
_baro_b_est.predict(_dt_ekf_avg);
// check for intermittent data
const bool baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL);
if (_baro_data_ready) {
_baro_lpf.update(_baro_sample_delayed.hgt);
updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
const bool continuing_conditions_passing = !_baro_hgt_faulty && !baro_hgt_intermittent;
const bool starting_conditions_passing = continuing_conditions_passing;
if (_control_status.flags.baro_hgt) {
if (continuing_conditions_passing) {
fuseBaroHgt(_aid_src_baro_hgt);
const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, _params.hgt_fusion_timeout_max);
if (isHeightResetRequired()) {
// All height sources are failing
resetHeightToBaro();
resetVerticalVelocityToZero();
} else if (is_fusion_failing) {
// Some other height source is still working
stopBaroHgtFusion();
_baro_hgt_faulty = true;
}
} else {
stopBaroHgtFusion();
}
} else {
if (starting_conditions_passing) {
startBaroHgtFusion();
}
}
} else if (_control_status.flags.baro_hgt && baro_hgt_intermittent) {
// No baro data anymore. Stop until it comes back.
stopBaroHgtFusion();
}
}
void Ekf::controlGnssHeightFusion()
{
if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) {