diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index 2b62787fe3..4404dd524f 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -30,6 +30,12 @@ uint8 JAMMING_STATE_CRITICAL = 3 uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical int32 jamming_indicator # indicates jamming is occurring +uint8 SPOOFING_STATE_UNKNOWN = 0 +uint8 SPOOFING_STATE_NONE = 1 +uint8 SPOOFING_STATE_INDICATED = 2 +uint8 SPOOFING_STATE_MULTIPLE = 3 +uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical + float32 vel_m_s # GPS ground speed, (metres/sec) float32 vel_n_m_s # GPS North velocity, (metres/sec) float32 vel_e_m_s # GPS East velocity, (metres/sec) diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 96e517f355..28cc0c82cc 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_module( -Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707 SRCS gps.cpp + devices/src/crc.cpp devices/src/gps_helper.cpp devices/src/mtk.cpp devices/src/ashtech.cpp @@ -48,6 +49,7 @@ px4_add_module( devices/src/emlid_reach.cpp devices/src/femtomes.cpp devices/src/nmea.cpp + devices/src/unicore.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 5882667129..33e447a3ba 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 5882667129f7ba8e6d5933757a77d52ed585d49b +Subproject commit 33e447a3ba24a3950398043e0df45abf37f79d0c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4bfe697a94..8916a94337 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -182,6 +182,8 @@ private: sensor_gps_s _report_gps_pos{}; ///< uORB topic for gps position satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info + uint8_t _spoofing_state{0}; ///< spoofing state + uint8_t _jamming_state{0}; ///< jamming state uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; @@ -1194,6 +1196,25 @@ GPS::publish() // The uORB message definition requires this data to be set to a NAN if no new valid data is available. _report_gps_pos.heading = NAN; _is_gps_main_advertised.store(true); + + if (_report_gps_pos.spoofing_state != _spoofing_state) { + + if (_report_gps_pos.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) { + PX4_WARN("GPS spoofing detected! (state: %d)", _report_gps_pos.spoofing_state); + } + + _spoofing_state = _report_gps_pos.spoofing_state; + } + + if (_report_gps_pos.jamming_state != _jamming_state) { + + if (_report_gps_pos.jamming_state > sensor_gps_s::JAMMING_STATE_OK) { + PX4_WARN("GPS jamming detected! (state: %d) (indicator: %d)", _report_gps_pos.jamming_state, + (uint8_t)_report_gps_pos.jamming_indicator); + } + + _jamming_state = _report_gps_pos.jamming_state; + } } } diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index 1605a2f9b9..6b08b58666 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -82,6 +82,7 @@ void FakeGps::Run() sensor_gps.heading_offset = 0.0000; sensor_gps.fix_type = 4; sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; sensor_gps.vel_ned_valid = true; sensor_gps.satellites_used = 14; sensor_gps.timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 9c524ffc7f..883fec1724 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -670,6 +670,29 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports critical jamming state\t"); } } + + if (vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_INDICATED) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::None, health_component_t::gps, + events::ID("check_estimator_gps_spoofing_indicated"), + events::Log::Critical, "GPS reports spoofing indicated"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports spoofing indicated\t"); + } + + } else if (vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_MULTIPLE) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::None, health_component_t::gps, + events::ID("check_estimator_gps_multiple_spoofing_indicated"), + events::Log::Critical, "GPS reports multiple spoofing indicated"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports multiple spoofing indicated\t"); + } + } } void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 047d84b49f..5137c1035c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2449,6 +2449,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) gps.automatic_gain_control = 0; gps.jamming_indicator = 0; gps.jamming_state = 0; + gps.spoofing_state = 0; gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 92a9f54974..1e1b5be22c 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -171,6 +171,7 @@ void SensorGpsSim::Run() sensor_gps.heading_accuracy = 0; sensor_gps.automatic_gain_control = 0; sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; sensor_gps.vel_ned_valid = true; sensor_gps.satellites_used = _sim_gps_used.get(); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 71970b9490..e3bc981805 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -421,6 +421,7 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) gps.automatic_gain_control = 0; gps.jamming_indicator = 0; gps.jamming_state = 0; + gps.spoofing_state = 0; gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s