From 09eb8169fac6951a632e6da36b9abb6eaffeeffb Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Fri, 21 Jun 2024 16:16:07 +0200 Subject: [PATCH] gnss(septentrio): implement new MAVLink `GNSS_INTEGRITY` message Implement support for the new MAVLink `GNSS_INTEGRITY` message and add support to the Septentrio driver. --- .../src/drivers/dsp_hitl/dsp_hitl.cpp | 4 + msg/SensorGps.msg | 33 ++- src/drivers/gnss/septentrio/CMakeLists.txt | 2 +- src/drivers/gnss/septentrio/sbf/messages.h | 18 +- src/drivers/gnss/septentrio/septentrio.cpp | 104 +++++++++- src/drivers/gps/gps.cpp | 13 ++ src/drivers/ins/vectornav/VectorNav.cpp | 7 + src/drivers/uavcan/sensors/gnss.cpp | 4 + src/examples/fake_gps/FakeGps.cpp | 4 + src/modules/mavlink/mavlink_main.cpp | 4 + src/modules/mavlink/mavlink_messages.cpp | 6 + src/modules/mavlink/mavlink_receiver.cpp | 4 + .../mavlink/streams/GNSS_INTEGRITY.hpp | 192 ++++++++++++++++++ .../gps_blending_test.cpp | 4 + .../sensor_gps_sim/SensorGpsSim.cpp | 4 + .../simulator_mavlink/SimulatorMavlink.cpp | 4 + 16 files changed, 398 insertions(+), 9 deletions(-) create mode 100644 src/modules/mavlink/streams/GNSS_INTEGRITY.hpp diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp index 380ebc04d004..7dde203f5b9c 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -1235,6 +1235,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg) gps.heading = NAN; gps.heading_offset = NAN; + gps.quality_corrections = UINT8_MAX; + gps.quality_receiver = UINT8_MAX; + gps.quality_gnss_signals = UINT8_MAX; + gps.quality_post_processing = UINT8_MAX; gps.timestamp = hrt_absolute_time(); diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index ce2bfad4fd8e..1359432a6eec 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -30,10 +30,11 @@ 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_UNKNOWN = 0 +uint8 JAMMING_STATE_OK = 1 +uint8 JAMMING_STATE_WARNING = 2 +uint8 JAMMING_STATE_CRITICAL = 3 +uint8 JAMMING_STATE_MITIGATED = 4 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 @@ -41,8 +42,17 @@ uint8 SPOOFING_STATE_UNKNOWN = 0 uint8 SPOOFING_STATE_NONE = 1 uint8 SPOOFING_STATE_INDICATED = 2 uint8 SPOOFING_STATE_MULTIPLE = 3 +uint8 SPOOFING_STATE_MITIGATED = 4 uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical +# Combined authentication state (e.g. Galileo OSNMA) +uint8 AUTHENTICATION_STATE_UNKNOWN = 0 +uint8 AUTHENTICATION_STATE_DISABLED = 1 +uint8 AUTHENTICATION_STATE_INITIALIZING = 2 +uint8 AUTHENTICATION_STATE_FAILED = 3 +uint8 AUTHENTICATION_STATE_OK = 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) float32 vel_e_m_s # GPS East velocity, (metres/sec) @@ -55,6 +65,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 +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]) @@ -69,4 +89,9 @@ uint8 RTCM_MSG_USED_NOT_USED = 1 uint8 RTCM_MSG_USED_USED = 2 uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver +uint8 quality_corrections # Abstract corrections quality from 0 to 10, or 255 if not available +uint8 quality_receiver # Abstract overall receiver operating status from 0 to 10, or 255 if not available +uint8 quality_gnss_signals # Abstract quality of GNSS signals from 0 to 10, or 255 if not available +uint8 quality_post_processing # Abstract expected post processing quality from 0 to 10, or 255 if not available + # TOPICS sensor_gps vehicle_gps_position diff --git a/src/drivers/gnss/septentrio/CMakeLists.txt b/src/drivers/gnss/septentrio/CMakeLists.txt index 62451fc9b1f3..6ae46450cb63 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/messages.h b/src/drivers/gnss/septentrio/sbf/messages.h index 487a40054c64..3452a851d0b1 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; + InfoMode info_mode: 4; uint8_t info_reserved: 2; uint8_t info_antenna_id: 2; }; @@ -261,7 +266,16 @@ struct RFStatus { }; struct GALAuthStatus { - uint16_t osnma_status_status: 3; + enum class OSNMAStatus : uint16_t { + Disabled = 0, + Initializing = 1, + AwaitingTrustedTimeInfo = 2, + InitFailedInconsistentTime = 3, + InitFailedKROOTInvalid = 4, + InitFailedInvalidParam = 5, + Authenticating = 6, + }; + OSNMAStatus osnma_status_status: 3; uint16_t osnma_status_initialization_progress: 8; uint16_t osnma_status_trusted_time_source: 3; uint16_t osnma_status_merkle_tree_busy: 1; diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 5c53db6098d5..96eaa881e2f3 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%lu,%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%lu,%s,%s%s,%s\n"; constexpr const char *k_command_clear_sbf = "sso,Stream%lu,%s,none,off\n"; @@ -1199,20 +1199,120 @@ int SeptentrioDriver::process_message() if (_sbf_decoder.parse(&receiver_status) == PX4_OK) { _message_gps_state.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; + + _message_gps_state.system_error = sensor_gps_s::SYSTEM_ERROR_OK; + + if (receiver_status.rx_error_cpu_overload) + _message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_CPU_OVERLOAD; + if (receiver_status.rx_error_antenna) + _message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_ANTENNA; + if (receiver_status.ext_error_diff_corr_error) + _message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_INCOMING_CORRECTIONS; + if (receiver_status.ext_error_setup_error) + _message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_CONFIGURATION; + if (receiver_status.rx_error_software) + _message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_SOFTWARE; + if (receiver_status.rx_error_congestion) + _message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_OUTPUT_CONGESTION; + if (receiver_status.rx_error_missed_event) + _message_gps_state.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_gps_state.quality_gnss_signals = 0; + for (int i = 0; i < math::min(quality_ind.n, static_cast(sizeof(quality_ind.indicators) / sizeof(quality_ind.indicators[0]))); i++) { + switch (quality_ind.indicators[i].type) { + case Type::BaseStationMeasurements: + _message_gps_state.quality_corrections = quality_ind.indicators[i].value; + break; + case Type::Overall: + _message_gps_state.quality_receiver = quality_ind.indicators[i].value; + break; + case Type::RTKPostProcessing: + _message_gps_state.quality_post_processing = quality_ind.indicators[i].value; + break; + case Type::GNSSSignalsMainAntenna: + _message_gps_state.quality_gnss_signals += quality_ind.indicators[i].value; + break; + default: + break; + } + } + } + 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) { + _message_gps_state.jamming_state = sensor_gps_s::JAMMING_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++) { + switch (rf_status.rf_band[i].info_mode) { + case InfoMode::Interference: + _message_gps_state.jamming_state = sensor_gps_s::JAMMING_STATE_CRITICAL; + break; + case InfoMode::Suppressed: + case InfoMode::Mitigated: + // Don't report mitigated when there is unmitigated interference in one band. + if (_message_gps_state.spoofing_state != sensor_gps_s::JAMMING_STATE_CRITICAL) { + _message_gps_state.jamming_state = sensor_gps_s::JAMMING_STATE_MITIGATED; + } + break; + default: + break; + } + } + if (rf_status.flags_inauthentic_gnss_signals || rf_status.flags_inauthentic_navigation_message) { + _message_gps_state.spoofing_state = sensor_gps_s::SPOOFING_STATE_INDICATED; + } + else { + _message_gps_state.spoofing_state = sensor_gps_s::SPOOFING_STATE_NONE; + } + } + 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.osnma_status_status) { + case OSNMAStatus::Disabled: + _message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_DISABLED; + break; + case OSNMAStatus::AwaitingTrustedTimeInfo: + case OSNMAStatus::Initializing: + _message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_INITIALIZING; + break; + case OSNMAStatus::InitFailedInconsistentTime: + case OSNMAStatus::InitFailedKROOTInvalid: + case OSNMAStatus::InitFailedInvalidParam: + _message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_FAILED; + break; + case OSNMAStatus::Authenticating: + _message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_OK; + break; + } + } + break; } case BlockID::EndOfPVT: { diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c1dd783be029..6aecfaf2de82 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -46,6 +46,7 @@ #endif #include +#include #include #include @@ -315,6 +316,10 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = NAN; + _report_gps_pos.quality_corrections = UINT8_MAX; + _report_gps_pos.quality_receiver = UINT8_MAX; + _report_gps_pos.quality_gnss_signals = UINT8_MAX; + _report_gps_pos.quality_post_processing = UINT8_MAX; int32_t enable_sat_info = 0; param_get(param_find("GPS_SAT_INFO"), &enable_sat_info); @@ -907,6 +912,10 @@ GPS::run() memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = heading_offset; + _report_gps_pos.quality_corrections = UINT8_MAX; + _report_gps_pos.quality_receiver = UINT8_MAX; + _report_gps_pos.quality_gnss_signals = UINT8_MAX; + _report_gps_pos.quality_post_processing = UINT8_MAX; if (_mode == gps_driver_mode_t::UBX) { @@ -1188,6 +1197,10 @@ GPS::publish() // Heading/yaw data can be updated at a lower rate than the other navigation data. // 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; + _report_gps_pos.quality_corrections = UINT8_MAX; + _report_gps_pos.quality_receiver = UINT8_MAX; + _report_gps_pos.quality_gnss_signals = UINT8_MAX; + _report_gps_pos.quality_post_processing = UINT8_MAX; _is_gps_main_advertised.store(true); if (_report_gps_pos.spoofing_state != _spoofing_state) { diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index cbe7ab535257..b8bd86ab2fde 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -450,6 +450,13 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.hdop = dop.hDOP; sensor_gps.vdop = dop.vDOP; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.quality_corrections = UINT8_MAX; + sensor_gps.quality_receiver = UINT8_MAX; + sensor_gps.quality_gnss_signals = UINT8_MAX; + sensor_gps.quality_post_processing = UINT8_MAX; + sensor_gps.eph = sqrtf(sq(positionUncertaintyGpsNed.c[0]) + sq(positionUncertaintyGpsNed.c[1])); sensor_gps.epv = positionUncertaintyGpsNed.c[2]; diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index db88941a564b..f66037103256 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -466,6 +466,10 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.heading = heading; report.heading_offset = heading_offset; report.heading_accuracy = heading_accuracy; + report.quality_corrections = UINT8_MAX; + report.quality_receiver = UINT8_MAX; + report.quality_gnss_signals = UINT8_MAX; + report.quality_post_processing = UINT8_MAX; report.noise_per_ms = noise_per_ms; report.jamming_indicator = jamming_indicator; diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index e2f966e023ca..85d983440bff 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -80,6 +80,10 @@ void FakeGps::Run() sensor_gps.timestamp_time_relative = 0; sensor_gps.heading = NAN; sensor_gps.heading_offset = 0.0000; + sensor_gps.quality_corrections = UINT8_MAX; + sensor_gps.quality_receiver = UINT8_MAX; + sensor_gps.quality_gnss_signals = UINT8_MAX; + sensor_gps.quality_post_processing = UINT8_MAX; sensor_gps.fix_type = 4; sensor_gps.jamming_state = 0; sensor_gps.spoofing_state = 0; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 54e1a8549eeb..0999ba4b9e39 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1417,6 +1417,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", 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); @@ -1489,6 +1490,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); @@ -1652,6 +1654,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); @@ -1752,6 +1755,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("EXTENDED_SYS_STATE", 1.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 638de9cce5f4..29bfffc30b38 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -77,6 +77,9 @@ #include "streams/EXTENDED_SYS_STATE.hpp" #include "streams/FLIGHT_INFORMATION.hpp" #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" @@ -505,6 +508,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/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06fb97c8a65f..5dd81bead073 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2433,6 +2433,10 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) gps.heading = NAN; gps.heading_offset = NAN; + gps.quality_corrections = UINT8_MAX; + gps.quality_receiver = UINT8_MAX; + gps.quality_gnss_signals = UINT8_MAX; + gps.quality_post_processing = UINT8_MAX; gps.timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/streams/GNSS_INTEGRITY.hpp b/src/modules/mavlink/streams/GNSS_INTEGRITY.hpp new file mode 100644 index 000000000000..e492f832e846 --- /dev/null +++ b/src/modules/mavlink/streams/GNSS_INTEGRITY.hpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 + +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 _sensor_gps_sub.advertised() ? (MAVLINK_MSG_ID_GNSS_INTEGRITY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamGNSSIntegrity(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0}; + hrt_abstime _last_send_ts {}; + static constexpr hrt_abstime kNoGpsSendInterval {1_s}; + + bool send() override + { + sensor_gps_s gps; + mavlink_gnss_integrity_t msg{}; + hrt_abstime now{}; + + if (_sensor_gps_sub.update(&gps)) { + msg.id = gps.device_id; + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_ANTENNA) { + msg.system_errors |= 8; + } + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_SOFTWARE) { + msg.system_errors |= 4; + } + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_CPU_OVERLOAD) { + msg.system_errors |= 32; + } + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_CONFIGURATION) { + msg.system_errors |= 2; + } + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_EVENT_CONGESTION) { + msg.system_errors |= 16; + } + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_OUTPUT_CONGESTION) { + msg.system_errors |= 64; + } + + if (gps.system_error & sensor_gps_s::SYSTEM_ERROR_INCOMING_CORRECTIONS) { + msg.system_errors |= 1; + } + + switch (gps.authentication_state) { + case sensor_gps_s::AUTHENTICATION_STATE_UNKNOWN: + msg.authentication_state = 0; + break; + + case sensor_gps_s::AUTHENTICATION_STATE_DISABLED: + msg.authentication_state = 4; + break; + + case sensor_gps_s::AUTHENTICATION_STATE_INITIALIZING: + msg.authentication_state = 1; + break; + + case sensor_gps_s::AUTHENTICATION_STATE_FAILED: + msg.authentication_state = 2; + break; + + case sensor_gps_s::AUTHENTICATION_STATE_OK: + msg.authentication_state = 3; + break; + } + + switch (gps.jamming_state) { + case sensor_gps_s::JAMMING_STATE_UNKNOWN: + msg.jamming_state = 0; + break; + + case sensor_gps_s::JAMMING_STATE_OK: + msg.jamming_state = 1; + break; + + case sensor_gps_s::JAMMING_STATE_MITIGATED: + msg.jamming_state = 2; + break; + + case sensor_gps_s::JAMMING_STATE_WARNING: + case sensor_gps_s::JAMMING_STATE_CRITICAL: + msg.jamming_state = 3; + break; + } + + switch (gps.spoofing_state) { + case sensor_gps_s::SPOOFING_STATE_UNKNOWN: + msg.spoofing_state = 0; + break; + + case sensor_gps_s::SPOOFING_STATE_NONE: + msg.spoofing_state = 1; + break; + + case sensor_gps_s::SPOOFING_STATE_MITIGATED: + msg.spoofing_state = 2; + break; + + case sensor_gps_s::SPOOFING_STATE_INDICATED: + case sensor_gps_s::SPOOFING_STATE_MULTIPLE: + msg.spoofing_state = 3; + break; + } + + msg.raim_state = 0; + msg.raim_hfom = UINT16_MAX; + msg.raim_vfom = UINT16_MAX; + msg.corrections_quality = gps.quality_corrections; + msg.system_status_summary = gps.quality_receiver; + msg.gnss_signal_quality = gps.quality_gnss_signals; + msg.post_processing_quality = gps.quality_post_processing; + + mavlink_msg_gnss_integrity_send_struct(_mavlink->get_channel(), &msg); + _last_send_ts = gps.timestamp; + + return true; + + } else if (_last_send_ts != 0 && (now = hrt_absolute_time()) > _last_send_ts + kNoGpsSendInterval) { + msg.raim_hfom = UINT16_MAX; + msg.raim_vfom = UINT16_MAX; + msg.corrections_quality = UINT8_MAX; + msg.system_status_summary = UINT8_MAX; + msg.gnss_signal_quality = UINT8_MAX; + msg.post_processing_quality = UINT8_MAX; + + mavlink_msg_gnss_integrity_send_struct(_mavlink->get_channel(), &msg); + _last_send_ts = now; + + return true; + } + + return false; + } +}; + +#endif // GNSS_INTEGRITY_HPP diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index 50352162c08d..6bc1e4c07c12 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -80,6 +80,10 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData() gps_data.timestamp_time_relative = 0; gps_data.heading = NAN; gps_data.heading_offset = 0.f; + gps_data.quality_corrections = UINT8_MAX; + gps_data.quality_receiver = UINT8_MAX; + gps_data.quality_gnss_signals = UINT8_MAX; + gps_data.quality_post_processing = UINT8_MAX; gps_data.fix_type = 4; gps_data.vel_ned_valid = true; gps_data.satellites_used = 8; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 5641706345a0..e759a8ff5932 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -169,6 +169,10 @@ void SensorGpsSim::Run() sensor_gps.heading = NAN; sensor_gps.heading_offset = NAN; sensor_gps.heading_accuracy = 0; + sensor_gps.quality_corrections = UINT8_MAX; + sensor_gps.quality_receiver = UINT8_MAX; + sensor_gps.quality_gnss_signals = UINT8_MAX; + sensor_gps.quality_post_processing = UINT8_MAX; sensor_gps.automatic_gain_control = 0; sensor_gps.jamming_state = 0; sensor_gps.spoofing_state = 0; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 20eed499b5f0..b237b8a4df36 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -450,6 +450,10 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) gps.heading = NAN; gps.heading_offset = NAN; + gps.quality_corrections = UINT8_MAX; + gps.quality_receiver = UINT8_MAX; + gps.quality_gnss_signals = UINT8_MAX; + gps.quality_post_processing = UINT8_MAX; gps.timestamp = hrt_absolute_time();