fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Spoofing GPS check (#23366)

Browse Source 
* estimator gps check fail flag for spoofing

* warn whenever spoofing state changes to true, use default hysteresis to completely stop fusion

* dont introduce more GPS namings, GNSS instead

* flash: exclude mantis for cuav_x7pro
This commit is contained in:
Marco Hauswirth 2024-07-09 16:31:11 +02:00 committed by GitHub
parent 62ff39a669
commit 419652b9fe
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 53 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
View File

@ -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

1
msg/EstimatorGpsStatus.msg
View File

@ -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)

1
msg/EstimatorStatus.msg
View File

@ -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

28
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View File

@ -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

2
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
View File

@ -64,7 +64,6 @@ private:
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 @@ private:
bool _position_reliant_on_optical_flow{false};
bool _gps_was_fused{false};
bool _gnss_spoofed{false};
bool _nav_failure_imminent_warned{false};

24
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View File

@ -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;

2
src/modules/ekf2/EKF/common.h
View File

@ -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;
};

2
src/modules/ekf2/EKF2.cpp
View File

@ -1253,6 +1253,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);
@ -2460,6 +2461,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);

5
src/modules/ekf2/module.yaml
View File

@ -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