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

improve gnss altitude fusion starting logic

Browse Source
This commit is contained in:
Marco Hauswirth 2025-10-01 13:42:43 +02:00 committed by Marco Hauswirth
parent c2c721a2d6
commit 6e579cb75a
1 changed files with 40 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@ -134,52 +134,58 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
}
} else {
if (starting_conditions_passing) {
if (altitude_initialisation_conditions_passing) {
// Altitude not initialized, GNSS is the configured height reference
_information_events.flags.reset_hgt_to_gps = true;
initialiseAltitudeTo(measurement, measurement_var);
bias_est.reset();
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::GNSS) && isGnssHgtResetAllowed()) {
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
// Start fusion if GPS vertical position control is also enabled
if (starting_conditions_passing) {
_height_sensor_ref = HeightSensor::GNSS;
resetAidSourceStatusZeroInnovation(aid_src);
aid_src.time_last_fuse = _time_delayed_us;
bias_est.setFusionActive();
_control_status.flags.gps_hgt = true;
}
} else if (starting_conditions_passing) {
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::GNSS) && isGnssHgtResetAllowed()) {
_height_sensor_ref = HeightSensor::GNSS;
_information_events.flags.reset_hgt_to_gps = true;
initialiseAltitudeTo(measurement, measurement_var);
resetAltitudeTo(measurement, measurement_var);
bias_est.reset();
resetAidSourceStatusZeroInnovation(aid_src);
}
bool is_gnss_hgt_consistent = true;
if (_control_status.flags.gnss_hgt_fault) {
if (aid_src.innovation_rejected) {
_time_last_gnss_hgt_rejected = _time_delayed_us;
}
is_gnss_hgt_consistent = isTimedOut(_time_last_gnss_hgt_rejected, _params.hgt_fusion_timeout_max);
}
if (is_gnss_hgt_consistent) {
if (_params.ekf2_hgt_ref != static_cast<int32_t>(HeightSensor::GNSS)) {
bias_est.setBias(-_gpos.altitude() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;
bias_est.setFusionActive();
_control_status.flags.gps_hgt = true;
_control_status.flags.gnss_hgt_fault = false;
} else {
bool is_gnss_hgt_consistent = true;
if (_control_status.flags.gnss_hgt_fault) {
if (aid_src.innovation_rejected) {
_time_last_gnss_hgt_rejected = _time_delayed_us;
}
is_gnss_hgt_consistent = isTimedOut(_time_last_gnss_hgt_rejected, _params.hgt_fusion_timeout_max);
}
if (is_gnss_hgt_consistent) {
if (_params.ekf2_hgt_ref != static_cast<int32_t>(HeightSensor::GNSS)) {
bias_est.setBias(-_gpos.altitude() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;
bias_est.setFusionActive();
_control_status.flags.gps_hgt = true;
_control_status.flags.gnss_hgt_fault = false;
}
}
}
if (altitude_initialisation_conditions_passing && !_control_status.flags.gnss_hgt_fault) {
// Do not start GNSS altitude aiding, but use measurement
// to initialize altitude and bias of other height sensors
_information_events.flags.reset_hgt_to_gps = true;
initialiseAltitudeTo(measurement, measurement_var);
bias_est.reset();
}
}
} else if (_control_status.flags.gps_hgt
@ -207,7 +213,8 @@ void Ekf::stopGpsHgtFusion()
bool Ekf::isGnssHgtResetAllowed()
{
const bool allowed = !(static_cast<GnssMode>(_params.ekf2_gps_mode) == GnssMode::kDeadReckoning
&& isOtherSourceOfVerticalPositionAidingThan(_control_status.flags.gps_hgt));
&& isOtherSourceOfVerticalPositionAidingThan(_control_status.flags.gps_hgt))
|| !PX4_ISFINITE(_local_origin_alt);
return allowed;
}