Ekf2 add jamming to gnss checks (#26085)

* add jamming check to gnss checks

* keep original order of gnss_check params for default backwards compability

Author: haumarco

msg/EstimatorStatus.msg

@@ -16,6 +16,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7		# 7 : maximum allowed vertical position
 uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8	# 8 : maximum allowed horizontal speed fail - requires stationary vehicle
 uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9	# 9 : maximum allowed vertical velocity discrepancy fail
 uint8 GPS_CHECK_FAIL_SPOOFED = 10		# 10 : GPS signal is spoofed
+uint8 GPS_CHECK_FAIL_JAMMED = 11		# 11 : GPS signal is jammed
 
 uint64 control_mode_flags	# Bitmask to indicate EKF logic state
 uint8 CS_TILT_ALIGN = 0		# 0 - true if the filter tilt alignment is complete

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -272,6 +272,22 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			_gnss_spoofed = false;
 		}
 
+		if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) {
+			if (!_gnss_jammed) {
+				_gnss_jammed = true;
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal jammed\t");
+				}
+
+				events::send(events::ID("check_estimator_gnss_warning_jamming"), {events::Log::Alert, events::LogInternal::Info},
+					     "GNSS signal jammed");
+			}
+
+		} else {
+			_gnss_jammed = false;
+		}
+
 		if (!context.isArmed() && ekf_gps_check_fail) {
 			NavModesMessageFail required_modes;
 			events::Log log_level;
@@ -435,6 +451,18 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 							    events::ID("check_estimator_gps_spoofed"),
 							    log_level, "GPS signal spoofed");
 
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) {
+				message = "Preflight%s: GPS signal jammed";
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
+							    events::ID("check_estimator_gps_jammed"),
+							    log_level, "GPS signal jammed");
+
 			} else {
 				if (!ekf_gps_fusion) {
 					// Likely cause unknown

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -103,6 +103,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 
 	bool _gps_was_fused{false};
 	bool _gnss_spoofed{false};
+	bool _gnss_jammed{false};
 
 	bool _nav_failure_imminent_warned{false};
 

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

@@ -91,6 +91,7 @@ bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)
 	_check_fail_status.flags.sacc = (gnss.sacc > 10.f);
 
 	_check_fail_status.flags.spoofed = gnss.spoofed;
+	_check_fail_status.flags.jammed = gnss.jammed;
 
 	bool passed = true;
 
@@ -99,7 +100,8 @@ bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)
 		(_check_fail_status.flags.hacc    && isCheckEnabled(GnssChecksMask::kHacc)) ||
 		(_check_fail_status.flags.vacc    && isCheckEnabled(GnssChecksMask::kVacc)) ||
 		(_check_fail_status.flags.sacc    && isCheckEnabled(GnssChecksMask::kSacc)) ||
-		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
+		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed)) ||
+		(_check_fail_status.flags.jammed  && isCheckEnabled(GnssChecksMask::kJammed))
 	) {
 		passed = false;
 	}
@@ -126,6 +128,7 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 	_check_fail_status.flags.sacc = (gnss.sacc > _params.ekf2_req_sacc);
 
 	_check_fail_status.flags.spoofed = gnss.spoofed;
+	_check_fail_status.flags.jammed = gnss.jammed;
 
 	runOnGroundGnssChecks(gnss);
 
@@ -153,7 +156,8 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 		(_check_fail_status.flags.vdrift  && isCheckEnabled(GnssChecksMask::kVdrift)) ||
 		(_check_fail_status.flags.hspeed  && isCheckEnabled(GnssChecksMask::kHspd)) ||
 		(_check_fail_status.flags.vspeed  && isCheckEnabled(GnssChecksMask::kVspd)) ||
-		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
+		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed)) ||
+		(_check_fail_status.flags.jammed  && isCheckEnabled(GnssChecksMask::kJammed))
 	) {
 		passed = false;
 	}

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp

@@ -63,6 +63,7 @@ class GnssChecks final
 			uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
 			uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
 			uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed
+			uint16_t jammed : 1; ///< 11 - true if the GNSS data is jammed
 		} flags;
 		uint16_t value;
 	};
@@ -108,7 +109,8 @@ class GnssChecks final
 		kHspd    = (1 << 7),
 		kVspd    = (1 << 8),
 		kSpoofed = (1 << 9),
-		kFix     = (1 << 10)
+		kFix     = (1 << 10),
+		kJammed  = (1 << 11)
 	};
 
 	bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast<int32_t>(check)); }

src/modules/ekf2/EKF/common.h

@@ -202,6 +202,7 @@ struct gnssSample {
 	float       yaw_acc{};    ///< 1-std yaw error (rad)
 	float       yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
 	bool        spoofed{};    ///< true if GNSS data is spoofed
+	bool        jammed{};     ///< true if GNSS data is jammed
 };
 
 struct magSample {

src/modules/ekf2/EKF2.cpp

@@ -2473,6 +2473,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.yaw_acc = vehicle_gps_position.heading_accuracy,
 			.yaw_offset = vehicle_gps_position.heading_offset,
 			.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED,
+			.jammed = vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED,
 		};
 
 		_ekf.setGpsData(gnss_sample, pps_compensation);

src/modules/ekf2/params_gnss.yaml

@@ -128,9 +128,10 @@ parameters:
         8: Vertical speed offset (EKF2_REQ_VDRIFT)
         9: Spoofing
         10: GPS fix type (EKF2_REQ_FIX)
+        11: Jamming
       default: 2047
       min: 0
-      max: 2047
+      max: 4095
     EKF2_REQ_EPH:
       description:
         short: Required EPH to use GPS