diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c604e416b8..8481ed8b0c 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -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(HeightSensor::GNSS)) { + + if (_params.ekf2_hgt_ref == static_cast(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(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(_params.ekf2_gps_mode) == GnssMode::kDeadReckoning + && isOtherSourceOfVerticalPositionAidingThan(_control_status.flags.gps_hgt)); + + return allowed; +} diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 76936556a4..60ed3f74ce 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -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(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(); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 63655750f5..c8d61754aa 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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; }; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 3c5f7ba3f5..099827f6dc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -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);