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

* avoid gnss-based altitude reset in DR-mode

Browse Source 
* add hysteresis for re-enabling fusion
* disable lat/lon/vel fusion on gnss_hgt_fault
This commit is contained in:
Marco Hauswirth 2025-09-11 16:58:06 +02:00 committed by Marco Hauswirth
parent 86f2fdfd7d
commit 3712af8b7f
4 changed files with 59 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -106,7 +106,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::GNSS)) {
if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::GNSS) && isGnssHgtResetAllowed()) {
// All height sources are failing
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
@ -120,17 +120,18 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
} else if (is_fusion_failing) {
// Some other height source is still working
ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
stopGpsHgtFusion();
stopGpsHgtFusion(&aid_src);
}
} else {
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
stopGpsHgtFusion();
stopGpsHgtFusion(&aid_src);
}
} else {
if (starting_conditions_passing) {
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::GNSS)) {
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::GNSS) && isGnssHgtResetAllowed()) {
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::GNSS;
@ -140,16 +141,36 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
bias_est.reset();
resetAidSourceStatusZeroInnovation(aid_src);
} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
bias_est.setBias(-_gpos.altitude() + measurement);
}
aid_src.time_last_fuse = _time_delayed_us;
bias_est.setFusionActive();
_control_status.flags.gps_hgt = true;
bool is_gnss_hgt_consistent = true;
} if (altitude_initialisation_conditions_passing) {
if (_control_status.flags.gnss_hgt_fault) {
if (aid_src.innovation_rejected) {
_gnss_hgt_hysteresis_time = 0;
} else if (_gnss_hgt_hysteresis_time == 0) {
_gnss_hgt_hysteresis_time = aid_src.timestamp_sample;
}
is_gnss_hgt_consistent = isTimedOut(_gnss_hgt_hysteresis_time, _params.hgt_fusion_timeout_max)
&& isRecent(_gnss_hgt_hysteresis_time, 2 * _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
@ -164,11 +185,11 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
&& !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopGpsHgtFusion();
stopGpsHgtFusion(&aid_src);
}
}
void Ekf::stopGpsHgtFusion()
void Ekf::stopGpsHgtFusion(estimator_aid_source1d_s *aid_src)
{
if (_control_status.flags.gps_hgt) {
@ -179,5 +200,19 @@ void Ekf::stopGpsHgtFusion()
_gps_hgt_b_est.setFusionInactive();
_control_status.flags.gps_hgt = false;
if (aid_src != nullptr && aid_src->innovation_rejected && !isGnssHgtResetAllowed()) {
_control_status.flags.gnss_hgt_fault = true;
_gnss_hgt_hysteresis_time = 0;
}
}
}
bool Ekf::isGnssHgtResetAllowed()
{
const bool allowed = !(static_cast<GnssMode>(_params.ekf2_gps_mode) == GnssMode::kDeadReckoning
&& isOtherSourceOfVerticalPositionAidingThan(_control_status.flags.gps_hgt));
return allowed;
}

6
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@ -124,7 +124,8 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align
&& !_control_status.flags.gnss_fault;
&& !_control_status.flags.gnss_fault
&& !_control_status.flags.gnss_hgt_fault;
const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed();
if (_control_status.flags.gnss_vel) {
@ -180,7 +181,8 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for
const bool continuing_conditions_passing = gnss_pos_enabled
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align;
&& _control_status.flags.yaw_align
&& !_control_status.flags.gnss_hgt_fault;
const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed();
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gnss_checks.passed();

4
src/modules/ekf2/EKF/common.h
View File

@ -607,8 +607,10 @@ uint64_t mag_heading_consistent :
uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
uint64_t gnss_fault :
1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used
1; ///< 45 - true if GNSS measurements (lat, lon, vel) have been declared faulty and are no longer used
uint64_t yaw_manual : 1; ///< 46 - true if yaw has been reset manually
uint64_t gnss_hgt_fault :
1; ///< 47 - true if GNSS measurements (alt) have been declared faulty and are no longer used
} flags;
uint64_t value;
};

5
src/modules/ekf2/EKF/ekf.h
View File

@ -581,6 +581,8 @@ private:
estimator_aid_source2d_s _aid_src_gnss_pos{};
estimator_aid_source3d_s _aid_src_gnss_vel{};
uint64_t _gnss_hgt_hysteresis_time{0};
# if defined(CONFIG_EKF2_GNSS_YAW)
estimator_aid_source1d_s _aid_src_gnss_yaw {};
# endif // CONFIG_EKF2_GNSS_YAW
@ -885,7 +887,8 @@ private:
void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src);
void controlGnssHeightFusion(const gnssSample &gps_sample);
void stopGpsHgtFusion();
void stopGpsHgtFusion(estimator_aid_source1d_s *aid_src=nullptr);
bool isGnssHgtResetAllowed();
# if defined(CONFIG_EKF2_GNSS_YAW)
void controlGnssYawFusion(const gnssSample &gps_sample);