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

@@ -450,6 +450,22 @@ 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");
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "GPS signal spoofed, disabled GPS fusion\t");
+				}
+
 			} else {
 				if (!ekf_gps_fusion) {
 					// Likely cause unknown
@@ -671,7 +687,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 	}
 }
 
-void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const
+void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position)
 {
 	if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) {
 		/* EVENT
@@ -703,6 +719,12 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s
 					    events::ID("check_estimator_gps_multiple_spoofing_indicated"),
 					    events::Log::Critical, "GPS reports multiple spoofing indicated");
 
+		if (!_gps_spoofed) {
+			events::send(events::ID("check_estimator_gps_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info},
+				     "GPS signal spoofed");
+			_gps_spoofed = true;
+		}
+
 		if (reporter.mavlink_log_pub()) {
 			mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports multiple spoofing indicated\t");
 		}

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

@@ -64,8 +64,7 @@ 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 checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position);
 	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,
 				     const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
@@ -102,6 +101,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 	bool _position_reliant_on_optical_flow{false};
 
 	bool _gps_was_fused{false};
+	bool _gps_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,29 @@ void Ekf::collect_gps(const gnssSample &gps)
 
 bool Ekf::runGnssChecks(const gnssSample &gps)
 {
+	// Check GPS spoofing
+	// hysterisis: spoofing needs to be consistent for 1 second before it is declared
+	constexpr uint64_t hysteresis_gps_spoofing_us = 1e6; // 1 second, hardcoded
+
+	if (gps.spoofed) {
+		if (_time_last_non_spoofed_gps == 0) {
+			_time_last_non_spoofed_gps = gps.time_us;
+		}
+
+		if (gps.time_us - _time_last_non_spoofed_gps > hysteresis_gps_spoofing_us) {
+			_gps_check_fail_status.flags.spoofed = true;
+		}
+
+		_time_last_spoofed_gps = gps.time_us;
+
+	} else {
+		_time_last_non_spoofed_gps = gps.time_us;
+	}
+
+	if (gps.time_us - _time_last_spoofed_gps > hysteresis_gps_spoofing_us) {
+		_gps_check_fail_status.flags.spoofed = false;
+	}
+
 	// Check the fix type
 	_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
 
@@ -240,7 +264,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 GPS data is spoofed
 	} flags;
 	uint16_t value;
 };

src/modules/ekf2/EKF/ekf.h

@@ -640,6 +640,8 @@ class Ekf final : public EstimatorInterface
 	uint64_t _last_gps_pass_us{0};		///< last system time in usec that the GPS passed it's checks
 	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
 	bool _gps_checks_passed{false};		///> true when all active GPS checks have passed
+	uint64_t _time_last_non_spoofed_gps{0};
+	uint64_t _time_last_spoofed_gps{0};
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 	// height sensor status

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