commander: report GNSS yaw fault to user

Author: bresch

msg/estimator_event_flags.msg

@@ -30,3 +30,4 @@ bool bad_yaw_using_gps_course           #  7 - true when the fiter has detected
 bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
 bool vision_data_stopped                #  9 - true when the vision system data has stopped for a significant time period
 bool emergency_yaw_reset_mag_stopped    # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data

msg/estimator_status.msg

@@ -46,6 +46,10 @@ uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect ind
 uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
 uint8 CS_GPS_YAW = 22		# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 uint8 CS_MAG_ALIGNED = 23	# 23 - true when the in-flight mag field alignment has been completed
+uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from external vision measurements is intended
+uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
+uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
+uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
 # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error

msg/estimator_status_flags.msg

@@ -31,7 +31,7 @@ bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
-
+bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declared faulty and is no longer being used
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/commander/Commander.cpp

@@ -3786,6 +3786,8 @@ void Commander::estimator_check()
 	}
 
 	const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
+	const bool gnss_heading_fault_prev = (_estimator_status_sub.get().control_mode_flags &
+					      (1 << estimator_status_s::CS_GPS_YAW_FAULT));
 
 	// use primary estimator_status
 	if (_estimator_selector_status_sub.updated()) {
@@ -3803,12 +3805,18 @@ void Commander::estimator_check()
 
 		// Check for a magnetomer fault and notify the user
 		const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
+		const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT));
 
 		if (!mag_fault_prev && mag_fault) {
 			mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing");
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
 		}
 
+		if (!gnss_heading_fault_prev && gnss_heading_fault) {
+			mavlink_log_critical(&_mavlink_log_pub, "Stopping GNSS heading use! Check configuration on landing");
+			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status);
+		}
+
 		/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
 		 * for a short time interval after takeoff.
 		 * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial

src/modules/ekf2/EKF/common.h

@@ -487,6 +487,7 @@ union filter_control_status_u {
 		uint32_t ev_vel      : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
 		uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
 		uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
+		uint32_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -751,7 +751,7 @@ void Ekf::controlGpsFusion()
 void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 {
 	if (!(_params.fusion_mode & MASK_USE_GPSYAW)
-	    || _is_gps_yaw_faulty) {
+	    || _control_status.flags.gps_yaw_fault) {
 
 		stopGpsYawFusion();
 		return;
@@ -793,7 +793,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 					} else if (starting_conditions_passing) {
 						// Data seems good, but previous reset did not fix the issue
 						// something else must be wrong, declare the sensor faulty and stop the fusion
-						_is_gps_yaw_faulty = true;
+						_control_status.flags.gps_yaw_fault = true;
 						stopGpsYawFusion();
 
 					} else {

src/modules/ekf2/EKF/ekf.h

@@ -536,7 +536,6 @@ class Ekf final : public EstimatorInterface
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
 	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
-	bool _is_gps_yaw_faulty{false};		///< true if gps yaw data is rejected by the filter for too long
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1698,7 +1698,7 @@ bool Ekf::resetYawToEKFGSF()
 			_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
 
 		} else if (_control_status.flags.gps_yaw) {
-			_is_gps_yaw_faulty = true;
+			_control_status.flags.gps_yaw_fault = true;
 			_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
 
 		} else if (_control_status.flags.ev_yaw) {

src/modules/ekf2/EKF2.cpp

@@ -641,6 +641,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		event_flags.stopping_mag_use                    = _ekf.warning_event_flags().stopping_mag_use;
 		event_flags.vision_data_stopped                 = _ekf.warning_event_flags().vision_data_stopped;
 		event_flags.emergency_yaw_reset_mag_stopped     = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
+		event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;
 
 		event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_event_flags_pub.publish(event_flags);
@@ -1172,6 +1173,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
+		status_flags.cs_gps_yaw_fault         = _ekf.control_status_flags().gps_yaw_fault;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;