Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

EKF2: Spoofing GPS check #23366

Merged
merged 5 commits into from
Jul 9, 2024
Merged

EKF2: Spoofing GPS check #23366

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a451e4d
estimator gps check fail flag for spoofing
haumarco Jul 5, 2024
ef8696f
warn whenever spoofing state changes to true, use default hysteresis …
haumarco Jul 9, 2024
30211ab
dont introduce more GPS namings, GNSS instead
haumarco Jul 9, 2024
ad7325d
flash: exclude mantis for cuav_x7pro
haumarco Jul 9, 2024
4e26b37
Merge branch 'main' into pr-ekf2_disable_gps_fusion_w_spoofing
haumarco Jul 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions msg/EstimatorGpsStatus.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions msg/EstimatorStatus.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
30 changes: 29 additions & 1 deletion src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 (!_gps_spoofed) {
_gps_spoofed = true;
haumarco marked this conversation as resolved.
Show resolved Hide resolved

if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "GPS signal spoofed\t");
haumarco marked this conversation as resolved.
Show resolved Hide resolved
}

events::send(events::ID("check_estimator_gps_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info},
haumarco marked this conversation as resolved.
Show resolved Hide resolved
"GPS signal spoofed");
}

} else {
_gps_spoofed = false;
}

if (!context.isArmed() && ekf_gps_check_fail) {
NavModes required_groups_gps = required_groups;
events::Log log_level = events::Log::Error;
Expand Down Expand Up @@ -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)) {
bresch marked this conversation as resolved.
Show resolved Hide resolved
message = "Preflight%s: GPS signal spoofed";
bresch marked this conversation as resolved.
Show resolved Hide resolved
haumarco marked this conversation as resolved.
Show resolved Hide resolved
/* 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"),
haumarco marked this conversation as resolved.
Show resolved Hide resolved
log_level, "GPS signal spoofed");
haumarco marked this conversation as resolved.
Show resolved Hide resolved

} else {
if (!ekf_gps_fusion) {
// Likely cause unknown
Expand Down Expand Up @@ -671,7 +699,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)
haumarco marked this conversation as resolved.
Show resolved Hide resolved
{
if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) {
/* EVENT
Expand Down
4 changes: 2 additions & 2 deletions src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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};

Expand Down
25 changes: 15 additions & 10 deletions src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -136,6 +137,9 @@ void Ekf::collect_gps(const gnssSample &gps)

bool Ekf::runGnssChecks(const gnssSample &gps)
{
// Check GPS spoofing
haumarco marked this conversation as resolved.
Show resolved Hide resolved
_gps_check_fail_status.flags.spoofed = gps.spoofed;

// Check the fix type
_gps_check_fail_status.flags.fix = (gps.fix_type < 3);

Expand Down Expand Up @@ -240,7 +244,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;
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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
haumarco marked this conversation as resolved.
Show resolved Hide resolved
} flags;
uint16_t value;
};
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
5 changes: 3 additions & 2 deletions src/modules/ekf2/module.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down
Loading