diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
index 2e52f24ac1d4..e5e5e083437f 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
+++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
@@ -7,6 +7,7 @@
#
# @maintainer
# @board px4_fmu-v2 exclude
+# @board cuav_x7pro exclude
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5x exclude
# @board px4_fmu-v6x exclude
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..7441e76f08c2 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+++ b/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
+ *
+ * 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");
+
} else {
if (!ekf_gps_fusion) {
// Likely cause unknown
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
index 7f41a9c86dbb..c900e95b8a0d 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+++ b/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};
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..a598f1078b66 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,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;
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index ae1d185860d9..f70918409edf 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 GNSS data is spoofed
} flags;
uint16_t value;
};
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index c42a499e0961..b9109c8b30ae 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1253,6 +1253,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);
@@ -2447,6 +2448,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