* avoid gnss-based altitude reset in DR-mode

* add hysteresis for re-enabling fusion
* disable lat/lon/vel fusion on gnss_hgt_fault

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<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;
+				bool is_gnss_hgt_consistent = true;
-				bias_est.setFusionActive();
-				_control_status.flags.gps_hgt = 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;
+}

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<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();
 

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;
 };

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);