EKF2: Spoofing GPS check

msg/EstimatorGpsStatus.msg

@@ -13,6 +13,7 @@ bool check_fail_max_horz_drift   # 6 : maximum allowed horizontal position drift
 bool check_fail_max_vert_drift   # 7 : maximum allowed vertical position drift fail - requires stationary vehicle
 bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle
 bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail
+bool check_fail_spoofed_gps      # 10 : GPS signal is spoofed
 
 float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s)
 float32 position_drift_rate_vertical_m_s   # Vertical position rate magnitude (m/s)

msg/EstimatorStatus.msg

@@ -15,6 +15,7 @@ uint8 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6		# 6 : maximum allowed horizontal positi
 uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7		# 7 : maximum allowed vertical position drift fail - requires stationary vehicle
 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
 
 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

@@ -318,6 +318,22 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 
 		_gps_was_fused = ekf_gps_fusion;
 
+		if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) {
+			if (!_gnss_spoofed) {
+				_gnss_spoofed = true;
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal spoofed\t");
+				}
+
+				events::send(events::ID("check_estimator_gnss_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info},
+					     "GNSS signal spoofed");
+			}
+
+		} else {
+			_gnss_spoofed = false;
+		}
+
 		if (!context.isArmed() && ekf_gps_check_fail) {
 			NavModes required_groups_gps = required_groups;
 			events::Log log_level = events::Log::Error;
@@ -450,6 +466,18 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
 							    log_level, "GPS Vertical Speed Drift too high");
 
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) {
+				message = "Preflight%s: GPS signal spoofed";
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_spoofed"),
+							    log_level, "GPS signal spoofed");
+
 			} else {
 				if (!ekf_gps_fusion) {
 					// Likely cause unknown

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

@@ -64,7 +64,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
 	void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
 				       const vehicle_local_position_s &lpos);
-
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
 	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
@@ -102,6 +101,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 	bool _position_reliant_on_optical_flow{false};
 
 	bool _gps_was_fused{false};
+	bool _gnss_spoofed{false};
 
 	bool _nav_failure_imminent_warned{false};
 

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

@@ -48,15 +48,16 @@
 #include <mathlib/mathlib.h>
 
 // GPS pre-flight check bit locations
-#define MASK_GPS_NSATS  (1<<0)
-#define MASK_GPS_PDOP   (1<<1)
-#define MASK_GPS_HACC   (1<<2)
-#define MASK_GPS_VACC   (1<<3)
-#define MASK_GPS_SACC   (1<<4)
-#define MASK_GPS_HDRIFT (1<<5)
-#define MASK_GPS_VDRIFT (1<<6)
-#define MASK_GPS_HSPD   (1<<7)
-#define MASK_GPS_VSPD   (1<<8)
+#define MASK_GPS_NSATS   (1<<0)
+#define MASK_GPS_PDOP    (1<<1)
+#define MASK_GPS_HACC    (1<<2)
+#define MASK_GPS_VACC    (1<<3)
+#define MASK_GPS_SACC    (1<<4)
+#define MASK_GPS_HDRIFT  (1<<5)
+#define MASK_GPS_VDRIFT  (1<<6)
+#define MASK_GPS_HSPD    (1<<7)
+#define MASK_GPS_VSPD    (1<<8)
+#define MASK_GPS_SPOOFED (1<<9)
 
 void Ekf::collect_gps(const gnssSample &gps)
 {
@@ -136,6 +137,8 @@ void Ekf::collect_gps(const gnssSample &gps)
 
 bool Ekf::runGnssChecks(const gnssSample &gps)
 {
+	_gps_check_fail_status.flags.spoofed = gps.spoofed;
+
 	// Check the fix type
 	_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
 
@@ -240,7 +243,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 		(_gps_check_fail_status.flags.hdrift  && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
 		(_gps_check_fail_status.flags.vdrift  && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
 		(_gps_check_fail_status.flags.hspeed  && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
-		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
+		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD)) ||
+		(_gps_check_fail_status.flags.spoofed && (_params.gps_check_mask & MASK_GPS_SPOOFED))
 	) {
 		_last_gps_fail_us = _time_delayed_us;
 		return false;

src/modules/ekf2/EKF/common.h

@@ -183,6 +183,7 @@ struct gnssSample {
 	float       yaw{};        ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
 	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
 };
 
 struct magSample {
@@ -536,6 +537,7 @@ union gps_check_fail_status_u {
 		uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
 		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
 	} flags;
 	uint16_t value;
 };

src/modules/ekf2/EKF2.cpp

@@ -1226,6 +1226,7 @@ void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
 	estimator_gps_status.check_fail_max_vert_drift   = _ekf.gps_check_fail_status_flags().vdrift;
 	estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed;
 	estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed;
+	estimator_gps_status.check_fail_spoofed_gps      = _ekf.gps_check_fail_status_flags().spoofed;
 
 	estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_gps_status_pub.publish(estimator_gps_status);
@@ -2420,6 +2421,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.yaw = vehicle_gps_position.heading, //TODO: move to different message
 			.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_MULTIPLE,
 		};
 
 		_ekf.setGpsData(gnss_sample);

src/modules/ekf2/module.yaml

@@ -136,7 +136,7 @@ parameters:
           run when the vehicle is on ground and stationary. 7 : Maximum allowed horizontal
           speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle
           is on ground and stationary. 8 : Maximum allowed vertical velocity discrepancy
-          set by EKF2_REQ_VDRIFT'
+          set by EKF2_REQ_VDRIFT. 9: Fails if GPS driver detects consistent spoofing'
       type: bitmask
       bit:
         0: Min sat count (EKF2_REQ_NSATS)
@@ -148,9 +148,10 @@ parameters:
         6: Max vertical position rate (EKF2_REQ_VDRIFT)
         7: Max horizontal speed (EKF2_REQ_HDRIFT)
         8: Max vertical velocity discrepancy (EKF2_REQ_VDRIFT)
+        9: Spoofing check
       default: 245
       min: 0
-      max: 511
+      max: 1023
     EKF2_REQ_EPH:
       description:
         short: Required EPH to use GPS