diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dcb33a5cbd..7eda1cc316 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -195,6 +195,7 @@ set(msg_files SensorCombined.msg SensorCorrection.msg SensorGnssRelative.msg + SensorGnssStatus.msg SensorGps.msg SensorGyro.msg SensorGyroFft.msg diff --git a/msg/SensorGnssStatus.msg b/msg/SensorGnssStatus.msg new file mode 100644 index 0000000000..1562d47ce7 --- /dev/null +++ b/msg/SensorGnssStatus.msg @@ -0,0 +1,10 @@ +# Gnss quality indicators + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +bool quality_available # Set to true if quality indicators are available +uint8 quality_corrections # Corrections quality from 0 to 10, or 255 if not available +uint8 quality_receiver # Overall receiver operating status from 0 to 10, or 255 if not available +uint8 quality_gnss_signals # Quality of GNSS signals from 0 to 10, or 255 if not available +uint8 quality_post_processing # Expected post processing quality from 0 to 10, or 255 if not available diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index ce2bfad4fd..2e94d174f3 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -30,18 +30,26 @@ float32 vdop # Vertical dilution of precision int32 noise_per_ms # GPS noise per millisecond uint16 automatic_gain_control # Automatic gain control monitor -uint8 JAMMING_STATE_UNKNOWN = 0 -uint8 JAMMING_STATE_OK = 1 -uint8 JAMMING_STATE_WARNING = 2 -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 JAMMING_STATE_UNKNOWN = 0 #default +uint8 JAMMING_STATE_OK = 1 +uint8 JAMMING_STATE_MITIGATED = 2 +uint8 JAMMING_STATE_DETECTED = 3 +uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Mitigated, 3: Detected +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 +uint8 SPOOFING_STATE_UNKNOWN = 0 #default +uint8 SPOOFING_STATE_OK = 1 +uint8 SPOOFING_STATE_MITIGATED = 2 +uint8 SPOOFING_STATE_DETECTED = 3 +uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Mitigated, 3: Detected + +# Combined authentication state (e.g. Galileo OSNMA) +uint8 AUTHENTICATION_STATE_UNKNOWN = 0 #default +uint8 AUTHENTICATION_STATE_INITIALIZING = 1 +uint8 AUTHENTICATION_STATE_ERROR = 2 +uint8 AUTHENTICATION_STATE_OK = 3 +uint8 AUTHENTICATION_STATE_DISABLED = 4 +uint8 authentication_state # GPS signal authentication state float32 vel_m_s # GPS ground speed, (metres/sec) float32 vel_n_m_s # GPS North velocity, (metres/sec) @@ -55,6 +63,16 @@ uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp whi uint8 satellites_used # Number of satellites used +uint32 SYSTEM_ERROR_OK = 0 #default +uint32 SYSTEM_ERROR_INCOMING_CORRECTIONS = 1 +uint32 SYSTEM_ERROR_CONFIGURATION = 2 +uint32 SYSTEM_ERROR_SOFTWARE = 4 +uint32 SYSTEM_ERROR_ANTENNA = 8 +uint32 SYSTEM_ERROR_EVENT_CONGESTION = 16 +uint32 SYSTEM_ERROR_CPU_OVERLOAD = 32 +uint32 SYSTEM_ERROR_OUTPUT_CONGESTION = 64 +uint32 system_error # General errors with the connected GPS receiver + float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) diff --git a/src/drivers/gnss/septentrio/CMakeLists.txt b/src/drivers/gnss/septentrio/CMakeLists.txt index 6971f57e28..3b3cd47ecc 100644 --- a/src/drivers/gnss/septentrio/CMakeLists.txt +++ b/src/drivers/gnss/septentrio/CMakeLists.txt @@ -37,7 +37,7 @@ px4_add_module( COMPILE_FLAGS # -DDEBUG_BUILD # Enable during development of the module -DSEP_LOG_ERROR # Enable module-level error logs - # -DSEP_LOG_WARN # Enable module-level warning logs + -DSEP_LOG_WARN # Enable module-level warning logs # -DSEP_LOG_INFO # Enable module-level info logs # -DSEP_LOG_TRACE_PARSING # Tracing of parsing steps SRCS diff --git a/src/drivers/gnss/septentrio/sbf/decoder.cpp b/src/drivers/gnss/septentrio/sbf/decoder.cpp index 308d26ef9b..3c66555449 100644 --- a/src/drivers/gnss/septentrio/sbf/decoder.cpp +++ b/src/drivers/gnss/septentrio/sbf/decoder.cpp @@ -163,7 +163,7 @@ int Decoder::parse(QualityInd *message) const int Decoder::parse(RFStatus *message) const { - if (can_parse() && id() == BlockID::PVTGeodetic) { + if (can_parse() && id() == BlockID::RFStatus) { static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small"); memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band)); diff --git a/src/drivers/gnss/septentrio/sbf/messages.h b/src/drivers/gnss/septentrio/sbf/messages.h index 8b7ca0c95e..704f4c3b6e 100644 --- a/src/drivers/gnss/septentrio/sbf/messages.h +++ b/src/drivers/gnss/septentrio/sbf/messages.h @@ -243,9 +243,14 @@ struct QualityInd { }; struct RFBand { + enum class InfoMode : uint8_t { + Suppressed = 1, + Mitigated = 2, + Interference = 8 + }; uint32_t frequency; uint16_t bandwidth; - uint8_t info_mode: 4; + uint8_t info_mode: 4; uint8_t info_reserved: 2; uint8_t info_antenna_id: 2; }; @@ -261,6 +266,15 @@ struct RFStatus { }; struct GALAuthStatus { + enum class OSNMAStatus : uint16_t { + Disabled = 0, + Initializing = 1, + AwaitingTrustedTimeInfo = 2, + InitFailedInconsistentTime = 3, + InitFailedKROOTInvalid = 4, + InitFailedInvalidParam = 5, + Authenticating = 6, + }; uint16_t osnma_status_status: 3; uint16_t osnma_status_initialization_progress: 8; uint16_t osnma_status_trusted_time_source: 3; @@ -271,6 +285,8 @@ struct GALAuthStatus { uint64_t gal_authentic_mask; uint64_t gps_active_mask; uint64_t gps_authentic_mask; + + OSNMAStatus osnmaStatus() const { return static_cast(osnma_status_status); } }; struct VelCovGeodetic { diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index d05e0b61e5..f3c182be23 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -92,7 +92,7 @@ constexpr size_t k_min_receiver_read_bytes = 32; */ constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200; -constexpr uint8_t k_max_command_size = 120; +constexpr uint8_t k_max_command_size = 140; constexpr uint16_t k_timeout_5hz = 500; constexpr uint32_t k_read_buffer_size = 150; constexpr time_t k_gps_epoch_secs = 1234567890ULL; // TODO: This seems wrong @@ -112,7 +112,7 @@ constexpr const char *k_command_reset_hot = "erst,soft,none\n"; constexpr const char *k_command_reset_warm = "erst,soft,PVTData\n"; constexpr const char *k_command_reset_cold = "erst,hard,SatData\n"; constexpr const char *k_command_sbf_output_pvt = - "sso,Stream%" PRIu32 ",%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus,%s\n"; + "sso,Stream%lu,%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus+GALAuthStatus+RFStatus+QualityInd,%s\n"; constexpr const char *k_command_set_sbf_output = "sso,Stream%" PRIu32 ",%s,%s%s,%s\n"; constexpr const char *k_command_clear_sbf = "sso,Stream%" PRIu32 ",%s,none,off\n"; @@ -967,7 +967,7 @@ SeptentrioDriver::ConfigureResult SeptentrioDriver::configure() } // Output a set of SBF blocks on a given connection at a regular interval. - snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency); + snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, (long unsigned int) _receiver_stream_main, com_port, sbf_frequency); if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { SEP_WARN("CONFIG: Failed to configure SBF"); return ConfigureResult::FailedCompletely; @@ -1191,20 +1191,138 @@ int SeptentrioDriver::process_message() if (_sbf_decoder.parse(&receiver_status) == PX4_OK) { _sensor_gps.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED; _time_synced = receiver_status.rx_state_wn_set && receiver_status.rx_state_tow_set; + + _sensor_gps.system_error = sensor_gps_s::SYSTEM_ERROR_OK; + + if (receiver_status.rx_error_cpu_overload) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_CPU_OVERLOAD; + } + if (receiver_status.rx_error_antenna) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_ANTENNA; + } + if (receiver_status.ext_error_diff_corr_error) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_INCOMING_CORRECTIONS; + } + if (receiver_status.ext_error_setup_error) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_CONFIGURATION; + } + if (receiver_status.rx_error_software) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_SOFTWARE; + } + if (receiver_status.rx_error_congestion) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_OUTPUT_CONGESTION; + } + if (receiver_status.rx_error_missed_event) { + _sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_EVENT_CONGESTION; + } } break; } case BlockID::QualityInd: { + using Type = QualityIndicator::Type; + SEP_TRACE_PARSING("Processing QualityInd SBF message"); + + QualityInd quality_ind; + + if (_sbf_decoder.parse(&quality_ind) == PX4_OK) { + _message_sensor_gnss_status.quality_available = true; + _message_sensor_gnss_status.device_id = get_device_id(); + + _message_sensor_gnss_status.quality_corrections = 255; + _message_sensor_gnss_status.quality_receiver = 255; + _message_sensor_gnss_status.quality_post_processing = 255; + _message_sensor_gnss_status.quality_gnss_signals = 255; + + for (int i = 0; i < math::min(quality_ind.n, static_cast(sizeof(quality_ind.indicators) / sizeof(quality_ind.indicators[0]))); i++) { + int quality = quality_ind.indicators[i].value; + + switch (quality_ind.indicators[i].type) { + case Type::BaseStationMeasurements: + _message_sensor_gnss_status.quality_corrections = quality; + break; + case Type::Overall: + _message_sensor_gnss_status.quality_receiver = quality; + break; + case Type::RTKPostProcessing: + _message_sensor_gnss_status.quality_post_processing = quality; + break; + case Type::GNSSSignalsMainAntenna: + _message_sensor_gnss_status.quality_gnss_signals = quality; + break; + default: + break; + } + } + + + _message_sensor_gnss_status.timestamp = hrt_absolute_time(); + _time_last_qualityind_received = hrt_absolute_time(); + _sensor_gnss_status_pub.publish(_message_sensor_gnss_status); + } + break; } case BlockID::RFStatus: { + using InfoMode = RFBand::InfoMode; + SEP_TRACE_PARSING("Processing RFStatus SBF message"); + + RFStatus rf_status; + + if (_sbf_decoder.parse(&rf_status) == PX4_OK) { + _sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_OK; + _sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_OK; + + for (int i = 0; i < math::min(rf_status.n, static_cast(sizeof(rf_status.rf_band) / sizeof(rf_status.rf_band[0]))); i++) { + InfoMode status = static_cast(rf_status.rf_band[i].info_mode); + + if(status == InfoMode::Interference){ + _sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_DETECTED; + break; // Worst case, we don't need to check the other bands + } + + if(status == InfoMode::Suppressed || status == InfoMode::Mitigated){ + _sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_MITIGATED; + } + } + + if (rf_status.flags_inauthentic_gnss_signals || rf_status.flags_inauthentic_navigation_message) { + _sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_DETECTED; + } + _time_last_resilience_received = hrt_absolute_time(); + } + break; } case BlockID::GALAuthStatus: { + using OSNMAStatus = GALAuthStatus::OSNMAStatus; + SEP_TRACE_PARSING("Processing GALAuthStatus SBF message"); + + GALAuthStatus gal_auth_status; + + if (_sbf_decoder.parse(&gal_auth_status) == PX4_OK) { + switch (gal_auth_status.osnmaStatus()) { + case OSNMAStatus::Disabled: + _sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_DISABLED; + break; + case OSNMAStatus::AwaitingTrustedTimeInfo: + case OSNMAStatus::Initializing: + _sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_INITIALIZING; + break; + case OSNMAStatus::InitFailedInconsistentTime: + case OSNMAStatus::InitFailedKROOTInvalid: + case OSNMAStatus::InitFailedInvalidParam: + _sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_ERROR; + break; + case OSNMAStatus::Authenticating: + _sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_OK; + break; + } + } + break; } case BlockID::EndOfPVT: { @@ -1277,6 +1395,31 @@ int SeptentrioDriver::process_message() } } + //Check for how recent the resilience data for reciever is, if outdated set to unknown + if ((_time_last_resilience_received != 0) && (hrt_elapsed_time(&_time_last_resilience_received) > 5_s)) { + _sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_UNKNOWN; + _sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_UNKNOWN; + + _time_last_resilience_received = 0; // Reset + } + + // Check for how recent the status data for receiver is, if outdated set to unknown + if ((_time_last_qualityind_received != 0) && (hrt_elapsed_time(&_time_last_qualityind_received) > 5_s)) { + _message_sensor_gnss_status.quality_available = false; + _message_sensor_gnss_status.device_id = get_device_id(); + _message_sensor_gnss_status.timestamp = hrt_absolute_time(); + + _message_sensor_gnss_status.quality_corrections = 255; + _message_sensor_gnss_status.quality_receiver = 255; + _message_sensor_gnss_status.quality_post_processing = 255; + _message_sensor_gnss_status.quality_gnss_signals = 255; + + + _sensor_gnss_status_pub.publish(_message_sensor_gnss_status); + + _time_last_qualityind_received = 0; // Reset + } + break; } case DecodingStatus::RTCMv3: { @@ -1532,6 +1675,7 @@ void SeptentrioDriver::publish() _sensor_gps.device_id = get_device_id(); _sensor_gps.selected_rtcm_instance = _selected_rtcm_instance; _sensor_gps.rtcm_injection_rate = rtcm_injection_frequency(); + _sensor_gps.timestamp = hrt_absolute_time(); _sensor_gps_pub.publish(_sensor_gps); } diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index 052d6ac1cd..cdc21fd092 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -710,12 +711,16 @@ private: char _port[20] {}; ///< The path of the used serial device hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM injection uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections + uint8_t _spoofing_state {0}; ///< Receiver spoofing state + uint8_t _jamming_state {0}; ///< Receiver jamming state bool _time_synced {false}; ///< Receiver time in sync with GPS time const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls uint32_t _chosen_baud_rate {0}; ///< The baud rate requested by the user static px4::atomic _secondary_instance; hrt_abstime _sleep_end {0}; ///< End time for sleeping State _resume_state {State::OpeningSerialPort}; ///< Resume state after sleep + hrt_abstime _time_last_qualityind_received{0}; ///< Time of last quality message reception + hrt_abstime _time_last_resilience_received{0}; ///< Time of last resilience message reception // Module configuration float _heading_offset {0.0f}; ///< The heading offset given by the `SEP_YAW_OFFS` parameter @@ -737,14 +742,16 @@ private: rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder // uORB topics and subscriptions - sensor_gps_s _sensor_gps{}; ///< uORB topic for position - gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver - gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver - satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info - uORB::PublicationMulti _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position - uORB::Publication _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data - uORB::Publication _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver - uORB::PublicationMulti _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info + sensor_gps_s _sensor_gps {}; ///< uORB topic for position + sensor_gnss_status_s _message_sensor_gnss_status {}; ///< uORB topic for gps status + gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver + gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver + satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info + uORB::PublicationMulti _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position + uORB::PublicationMulti _sensor_gnss_status_pub {ORB_ID(sensor_gnss_status)}; ///< uORB publication for gnss status + uORB::Publication _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data + uORB::Publication _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver + uORB::PublicationMulti _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info uORB::SubscriptionMultiArray _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver // Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate... diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index e2f966e023..ad1166e49c 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -87,6 +87,14 @@ void FakeGps::Run() sensor_gps.satellites_used = 14; sensor_gps.timestamp = hrt_absolute_time(); _sensor_gps_pub.publish(sensor_gps); + + sensor_gnss_status_s sensor_gnss_status{}; + sensor_gnss_status.quality_corrections = 0; + sensor_gnss_status.quality_receiver = 9; + sensor_gnss_status.quality_gnss_signals = 10; + sensor_gnss_status.quality_post_processing = 255; + sensor_gnss_status.timestamp = hrt_absolute_time(); + _sensor_gnss_status_pub.publish(sensor_gnss_status); } int FakeGps::task_spawn(int argc, char *argv[]) diff --git a/src/examples/fake_gps/FakeGps.hpp b/src/examples/fake_gps/FakeGps.hpp index eae3b47512..223f372e44 100644 --- a/src/examples/fake_gps/FakeGps.hpp +++ b/src/examples/fake_gps/FakeGps.hpp @@ -41,6 +41,7 @@ #include #include #include +#include class FakeGps : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -66,6 +67,7 @@ private: void Run() override; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_gnss_status_pub{ORB_ID(sensor_gnss_status)}; double _latitude{29.6603018}; // Latitude in degrees double _longitude{-82.3160500}; // Longitude in degrees diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 60b922bd18..add7456b59 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -594,15 +594,15 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const { - if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) { + if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED) { /* EVENT */ reporter.armingCheckFailure(NavModes::None, health_component_t::gps, events::ID("check_estimator_gps_jamming_critical"), - events::Log::Critical, "GPS reports critical jamming state"); + events::Log::Critical, "GPS jamming detected"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports critical jamming state\t"); + mavlink_log_critical(reporter.mavlink_log_pub(), "GPS jamming detected\t"); } } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c409a3ef47..197179dc6d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2467,7 +2467,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, + .spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED, }; _ekf.setGpsData(gnss_sample); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f0a693b085..3b4c39a73e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1441,6 +1441,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); configure_stream_local("GLOBAL_POSITION", 5.0f); configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + configure_stream_local("GNSS_INTEGRITY", 1.0f); configure_stream_local("GPS2_RAW", 1.0f); configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); configure_stream_local("GPS_RAW_INT", 5.0f); @@ -1513,6 +1514,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); configure_stream_local("GLOBAL_POSITION_INT", 50.0f); + configure_stream_local("GNSS_INTEGRITY", 1.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); configure_stream_local("GPS_RAW_INT", unlimited_rate); @@ -1673,6 +1675,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ESTIMATOR_STATUS", 5.0f); configure_stream_local("EXTENDED_SYS_STATE", 2.0f); configure_stream_local("GLOBAL_POSITION_INT", 10.0f); + configure_stream_local("GNSS_INTEGRITY", 1.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); configure_stream_local("GPS_RAW_INT", unlimited_rate); @@ -1775,6 +1778,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GLOBAL_POSITION", 10.0f); configure_stream_local("GLOBAL_POSITION_INT", 10.0f); configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); + configure_stream_local("GNSS_INTEGRITY", 1.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_RAW_INT", unlimited_rate); configure_stream_local("HOME_POSITION", 0.5f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 876d4c5825..a68c138747 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -79,6 +79,9 @@ #include "streams/GLOBAL_POSITION.hpp" #endif //MAVLINK_MSG_ID_GLOBAL_POSITION #include "streams/GLOBAL_POSITION_INT.hpp" +#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY) +#include "streams/GNSS_INTEGRITY.hpp" +#endif #include "streams/GPS_GLOBAL_ORIGIN.hpp" #include "streams/GPS_RAW_INT.hpp" #include "streams/GPS_RTCM_DATA.hpp" @@ -507,6 +510,9 @@ static const StreamListItem streams_list[] = { #if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP) create_stream_list_item(), #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP +#if defined(GNSS_INTEGRITY_HPP) + create_stream_list_item(), +#endif // GNSS_INTEGRITY_HPP #if defined(AVAILABLE_MODES_HPP) create_stream_list_item(), #endif // AVAILABLE_MODES_HPP diff --git a/src/modules/mavlink/streams/GNSS_INTEGRITY.hpp b/src/modules/mavlink/streams/GNSS_INTEGRITY.hpp new file mode 100644 index 0000000000..a7205952e8 --- /dev/null +++ b/src/modules/mavlink/streams/GNSS_INTEGRITY.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef GNSS_INTEGRITY_HPP +#define GNSS_INTEGRITY_HPP + + +#include +#include +#include + +using namespace time_literals; + +class MavlinkStreamGNSSIntegrity : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGNSSIntegrity(mavlink); } + + static constexpr const char *get_name_static() { return "GNSS_INTEGRITY"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GNSS_INTEGRITY; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _vehicle_gps_position_sub.advertised() ? (MAVLINK_MSG_ID_GNSS_INTEGRITY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + static constexpr int GPS_MAX_RECEIVERS = 2; + + explicit MavlinkStreamGNSSIntegrity(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionMultiArray _sensor_gnss_status_sub{ORB_ID::sensor_gnss_status}; + + bool send() override + { + sensor_gps_s vehicle_gps_position{}; + + if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) { + mavlink_gnss_integrity_t msg{}; + + msg.id = vehicle_gps_position.device_id; + msg.system_errors = vehicle_gps_position.system_error; + msg.authentication_state = vehicle_gps_position.authentication_state; + msg.jamming_state = vehicle_gps_position.jamming_state; + msg.spoofing_state = vehicle_gps_position.spoofing_state; + + msg.corrections_quality = UINT8_MAX; + msg.system_status_summary = UINT8_MAX; + msg.gnss_signal_quality = UINT8_MAX; + msg.post_processing_quality = UINT8_MAX; + + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { + sensor_gnss_status_s sensor_gnss_status{}; + + if (_sensor_gnss_status_sub[i].copy(&sensor_gnss_status)) { + if ((hrt_elapsed_time(&sensor_gnss_status.timestamp) < 3_s) + && (sensor_gnss_status.device_id == vehicle_gps_position.device_id) + && (sensor_gnss_status.quality_available)) { + msg.corrections_quality = sensor_gnss_status.quality_corrections; + msg.system_status_summary = sensor_gnss_status.quality_receiver; + msg.gnss_signal_quality = sensor_gnss_status.quality_gnss_signals; + msg.post_processing_quality = sensor_gnss_status.quality_post_processing; + break; + } + } + } + + msg.raim_hfom = UINT16_MAX; + msg.raim_vfom = UINT16_MAX; + + mavlink_msg_gnss_integrity_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } + +}; + +#endif // GNSS_INTEGRITY_HPP