diff --git a/msg/EstimatorGpsStatus.msg b/msg/EstimatorGpsStatus.msg
index 2d2462ee5c30..b2a9c883944e 100644
--- a/msg/EstimatorGpsStatus.msg
+++ b/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)
diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg
index ac13b59ea7ee..a603a0f5ad86 100644
--- a/msg/EstimatorStatus.msg
+++ b/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
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
index c67fdfd21343..cc55336300eb 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+++ b/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
+ *
+ * This check can be configured via EKF2_GPS_CHECK parameter.
+ *
+ */
+ 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");
}
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
index 7f41a9c86dbb..658b2eef4230 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+++ b/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};
diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
index abdbec64fc1a..e99bb151cd5c 100644
--- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
@@ -48,15 +48,16 @@
#include
// 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;
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index ae1d185860d9..1cac0c678423 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/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;
};
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index f7c4ee2dcfc6..f1bb364ba273 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/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
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 17d55ab740b4..2eb09247c790 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1226,6 +1226,7 @@ void EKF2::PublishGpsStatus(const hrt_abstime ×tamp)
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);
diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml
index be11bd97b353..848ca02a9bf6 100644
--- a/src/modules/ekf2/module.yaml
+++ b/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