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

ekf2: move GNSS hgt control to file

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent b6f76b5eaa
commit 28b4cd0579
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
-43
View File
@@ -685,49 +685,6 @@ bool Ekf::isConditionalRangeAidSuitable()
return is_range_aid_suitable;
}
void Ekf::controlGnssHeightFusion()
{
if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) {
stopGpsHgtFusion();
return;
}
_gps_hgt_b_est.predict(_dt_ekf_avg);
if (_gps_data_ready) {
const bool continuing_conditions_passing = !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised;
const bool starting_conditions_passing = continuing_conditions_passing;
if (_control_status.flags.gps_hgt) {
if (continuing_conditions_passing) {
/* fuseGpsHgt(); */ // Done in fuseGpsPos
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.hgt_fusion_timeout_max);
if (isHeightResetRequired()) {
// All height sources are failing
resetHeightToGps();
resetVerticalVelocityToGps(_gps_sample_delayed);
} else if (is_fusion_failing) {
// Some other height source is still working
stopGpsHgtFusion();
}
} else {
stopGpsHgtFusion();
}
} else {
if (starting_conditions_passing) {
startGpsHgtFusion();
}
}
} else if (_control_status.flags.gps_hgt && _gps_intermittent) {
stopGpsHgtFusion();
}
}
void Ekf::controlRangeHeightFusion()
{
if (!((_params.rng_ctrl == RngCtrl::CONDITIONAL) || (_params.rng_ctrl == RngCtrl::ENABLED))) {