* add gnss-fault flags to estimator-status msg

* react to comments

msg/EstimatorStatusFlags.msg

@@ -49,8 +49,9 @@ bool cs_valid_fake_pos          # 41 - true if a valid constant position is bein
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 bool cs_gnss_vel                # 44 - true if GNSS velocity measurement fusion is intended
-bool cs_gnss_fault              # 45 - true if GNSS measurements have been declared faulty and are no longer used
+bool cs_gnss_fault              # 45 - true if GNSS true if GNSS measurements (lat, lon, vel) have been declared faulty
 bool cs_yaw_manual              # 46 - true if yaw has been set manually
+bool cs_gnss_hgt_fault          # 47 - true if GNSS true if GNSS measurements (alt) have been declared faulty
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -120,12 +120,17 @@ 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(&aid_src);
+					stopGpsHgtFusion();
+
+					if (!isGnssHgtResetAllowed()) {
+						_control_status.flags.gnss_hgt_fault = true;
+						_time_last_gnss_hgt_rejected = _time_delayed_us;
+					}
 				}
 
 			} else {
 				ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
-				stopGpsHgtFusion(&aid_src);
+				stopGpsHgtFusion();
 			}
 
 		} else {
@@ -146,16 +151,12 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 				bool is_gnss_hgt_consistent = true;
 
 				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;
+					if (aid_src.innovation_rejected) {
+						_time_last_gnss_hgt_rejected = _time_delayed_us;
 					}
 
-					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);
-
+					is_gnss_hgt_consistent = isTimedOut(_time_last_gnss_hgt_rejected, _params.hgt_fusion_timeout_max);
 				}
 
 				if (is_gnss_hgt_consistent) {
@@ -185,11 +186,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(&aid_src);
+		stopGpsHgtFusion();
 	}
 }
 
-void Ekf::stopGpsHgtFusion(estimator_aid_source1d_s *aid_src)
+void Ekf::stopGpsHgtFusion()
 {
 	if (_control_status.flags.gps_hgt) {
 
@@ -200,12 +201,6 @@ void Ekf::stopGpsHgtFusion(estimator_aid_source1d_s *aid_src)
 		_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;
-		}
-
 	}
 }
 

src/modules/ekf2/EKF/ekf.h

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

src/modules/ekf2/EKF2.cpp

@@ -1955,6 +1955,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
 		status_flags.cs_gnss_fault          = _ekf.control_status_flags().gnss_fault;
 		status_flags.cs_yaw_manual          = _ekf.control_status_flags().yaw_manual;
+		status_flags.cs_gnss_hgt_fault      = _ekf.control_status_flags().gnss_hgt_fault;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;