diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a096d6eccac8f..03e900eb93d39 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -514,6 +514,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, // MISSION_CURRENT MSG_GPS_RAW, MSG_GPS_RTK, + MSG_GNSS_INTEGRITY, #if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 887599d6b985d..20041f3c58ca6 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -625,6 +625,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, + MSG_GNSS_INTEGRITY, #if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, diff --git a/Tools/AP_Periph/GCS_MAVLink.cpp b/Tools/AP_Periph/GCS_MAVLink.cpp index a3df4631fef87..5a821608fe708 100644 --- a/Tools/AP_Periph/GCS_MAVLink.cpp +++ b/Tools/AP_Periph/GCS_MAVLink.cpp @@ -35,6 +35,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #if AP_GPS_ENABLED MSG_GPS_RAW, MSG_GPS_RTK, + MSG_GNSS_INTEGRITY, #endif }; diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6ca80fccc82fe..7ff6c4979546d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -111,6 +111,31 @@ static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D == (uint32_t)GPS_FIX_T static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS == (uint32_t)GPS_FIX_TYPE_DGPS, "FIX_DGPS incorrect"); static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT == (uint32_t)GPS_FIX_TYPE_RTK_FLOAT, "FIX_RTK_FLOAT incorrect"); static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED == (uint32_t)GPS_FIX_TYPE_RTK_FIXED, "FIX_RTK_FIXED incorrect"); + +static_assert((uint32_t)AP_GPS::GPS_Errors::GPS_SYSTEM_ERROR_NONE == (uint32_t)0x00, "GPS_SYSTEM_ERROR_NONE incorrect"); //not ideal but state doesn't exist in mavlink +static_assert((uint32_t)AP_GPS::GPS_Errors::INCOMING_CORRECTIONS == (uint32_t)GPS_SYSTEM_ERROR_INCOMING_CORRECTIONS, "INCOMING_CORRECTIONS incorrect"); +static_assert((uint32_t)AP_GPS::GPS_Errors::CONFIGURATION == (uint32_t)GPS_SYSTEM_ERROR_CONFIGURATION, "CONFIGURATION incorrect"); +static_assert((uint32_t)AP_GPS::GPS_Errors::SOFTWARE == (uint32_t)GPS_SYSTEM_ERROR_SOFTWARE, "SOFTWARE incorrect"); +static_assert((uint32_t)AP_GPS::GPS_Errors::ANTENNA == (uint32_t)GPS_SYSTEM_ERROR_ANTENNA, "ANTENNA incorrect"); +static_assert((uint32_t)AP_GPS::GPS_Errors::EVENT_CONGESTION == (uint32_t)GPS_SYSTEM_ERROR_EVENT_CONGESTION, "EVENT_CONGESTION incorrect"); +static_assert((uint32_t)AP_GPS::GPS_Errors::CPU_OVERLOAD == (uint32_t)GPS_SYSTEM_ERROR_CPU_OVERLOAD, "CPU_OVERLOAD incorrect"); +static_assert((uint32_t)AP_GPS::GPS_Errors::OUTPUT_CONGESTION == (uint32_t)GPS_SYSTEM_ERROR_OUTPUT_CONGESTION, "OUTPUT_CONGESTION inccorect"); + +static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_UNKNOWN == (uint8_t)GPS_AUTHENTICATION_STATE_UNKNOWN, "AUTHENTICATION_UNKNOWN incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_INITIALIZING == (uint8_t)GPS_AUTHENTICATION_STATE_INITIALIZING, "AUTHENTICATION_INITIALIZING incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_ERROR == (uint8_t)GPS_AUTHENTICATION_STATE_ERROR, "AUTHENTICATION_ERROR incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_OK == (uint8_t)GPS_AUTHENTICATION_STATE_OK, "GPS_AUTHENTICATION_STATE_OK incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Authentication::AUTHENTICATION_DISABLED == (uint8_t)GPS_AUTHENTICATION_STATE_DISABLED, "AUTHENTICATION_DISABLED incorrect"); + +static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_UNKNOWN == (uint8_t)GPS_JAMMING_STATE_UNKNOWN, "JAMMING_UNKNOWN incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_OK == (uint8_t)GPS_JAMMING_STATE_OK, "JAMMING_OK incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_DETECTED == (uint8_t)GPS_JAMMING_STATE_DETECTED, "JAMMING_DETECTED incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Jamming::JAMMING_MITIGATED == (uint8_t)GPS_JAMMING_STATE_MITIGATED, "JAMMING_MITIGATED incorrect"); + +static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_UNKNOWN == (uint8_t)GPS_SPOOFING_STATE_UNKNOWN, "SPOOFING_UNKNOWN incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_OK == (uint8_t)GPS_SPOOFING_STATE_OK, "SPOOFING_OK incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_DETECTED == (uint8_t)GPS_SPOOFING_STATE_DETECTED, "SPOOFING_DETECTED incorrect"); +static_assert((uint8_t)AP_GPS::GPS_Spoofing::SPOOFING_MITIGATED == (uint8_t)GPS_SPOOFING_STATE_MITIGATED, "SPOOFING_MITIGATED incorrect"); #endif // ensure that our own enum-class status is equivalent to the @@ -1456,6 +1481,35 @@ void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) } #endif +void AP_GPS::send_mavlink_gnss_integrity(mavlink_channel_t chan, uint8_t instance) { + uint8_t id = instance; + uint32_t system_error = get_system_errors(instance); + uint8_t authentication = get_authentication_state(instance); + uint8_t jamming = get_jamming_state(instance); + uint8_t spoofing = get_spoofing_state(instance); + uint8_t raim_state = 255; //not implemented yet + uint16_t raim_hfom = 255; //not implemented yet + uint16_t raim_vfom = 255; //not implemented yet + uint8_t corrections_quality = 255; //not implemented yet + uint8_t systems_status_summary = 255; //not implemented yet + uint8_t gnss_signal_quality = 255; //not implemented yet + uint8_t post_processing_quality = 255; //not implemented yet + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "gps %u with auth %u and spooof %u", id, authentication, spoofing); + mavlink_msg_gnss_integrity_send(chan, + id, + system_error, + authentication, + jamming, + spoofing, + raim_state, + raim_hfom, + raim_vfom, + corrections_quality, + systems_status_summary, + gnss_signal_quality, + post_processing_quality); +} + bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const { for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a3c814ffc241f..ec23c4ff3680c 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -156,6 +156,49 @@ class AP_GPS GPS_OK_FIX_3D_RTK_FIXED = 6, ///< Receiving valid messages and 3D RTK Fixed }; + /// GPS error bits. These are kept aligned with MAVLink by static_assert + /// in AP_GPS.cpp + enum GPS_Errors { + GPS_SYSTEM_ERROR_NONE = 0, + INCOMING_CORRECTIONS = 1 << 0, + CONFIGURATION = 1 << 1, + SOFTWARE = 1 << 2, + ANTENNA = 1 << 3, + EVENT_CONGESTION = 1 << 4, + CPU_OVERLOAD = 1 << 5, + OUTPUT_CONGESTION = 1 << 6, + }; + + /// GPS authentication status. These are kept aligned with MAVLink by static_assert + /// in AP_GPS.cpp + enum GPS_Authentication { + AUTHENTICATION_UNKNOWN = 0, + AUTHENTICATION_INITIALIZING = 1, + AUTHENTICATION_ERROR = 2, + AUTHENTICATION_OK = 3, + AUTHENTICATION_DISABLED = 4, + }; + + /// GPS jamming status. These are kept aligned with MAVLink by static_assert + /// in AP_GPS.cpp + enum GPS_Jamming { + JAMMING_UNKNOWN = 0, + JAMMING_OK = 1, + JAMMING_MITIGATED = 2, + JAMMING_DETECTED = 3, + + }; + + /// GPS spoofing status. These are kept aligned with MAVLink by static_assert + /// in AP_GPS.cpp + enum GPS_Spoofing { + SPOOFING_UNKNOWN = 0, + SPOOFING_OK = 1, + SPOOFING_MITIGATED = 2, + SPOOFING_DETECTED = 3, + + }; + // GPS navigation engine settings. Not all GPS receivers support // this enum GPS_Engine_Setting { @@ -223,6 +266,10 @@ class AP_GPS uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds bool corrected_timestamp_updated; ///< true if the corrected timestamp has been updated uint32_t lagged_sample_count; ///< number of samples with 50ms more lag than expected + uint32_t system_errors; ///< system errors + uint8_t authentication_state; ///< authentication state of GNSS signals + uint8_t jamming_state; ///< jamming state of GNSS signals + uint8_t spoofing_state; ///< spoofing state of GNSS signals // all the following fields must only all be filled by RTK capable backend drivers uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds @@ -430,6 +477,42 @@ class AP_GPS return get_hdop(primary_instance); } + // general errors in the GPS system + uint32_t get_system_errors(uint8_t instance) const { + return state[instance].system_errors; + } + + uint32_t get_system_errors() const { + return get_system_errors(primary_instance); + } + + // authentication state of GNSS signals + uint8_t get_authentication_state(uint8_t instance) const { + return state[instance].authentication_state; + } + + uint8_t get_authentication_state() const { + return get_authentication_state(primary_instance); + } + + // jamming state of GNSS signals + uint8_t get_jamming_state(uint8_t instance) const { + return state[instance].jamming_state; + } + + uint8_t get_jamming_state() const { + return get_jamming_state(primary_instance); + } + + // spoofing state of GNSS signals + uint8_t get_spoofing_state(uint8_t instance) const { + return state[instance].spoofing_state; + } + + uint8_t get_spoofing_state() const { + return get_spoofing_state(primary_instance); + } + // vertical dilution of precision uint16_t get_vdop(uint8_t instance) const { return state[instance].vdop; @@ -505,6 +588,7 @@ class AP_GPS void send_mavlink_gps2_raw(mavlink_channel_t chan); void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst); + void send_mavlink_gnss_integrity(mavlink_channel_t chan, uint8_t inst); // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index d771e439b338a..c888cff2e4073 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -45,6 +45,9 @@ do { \ #ifndef GPS_SBF_STREAM_NUMBER #define GPS_SBF_STREAM_NUMBER 1 #endif +#ifndef GPS_SBF_STATUS_STREAM_NUMBER + #define GPS_SBF_STATUS_STREAM_NUMBER 3 +#endif #define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte @@ -91,9 +94,9 @@ AP_GPS_SBF::read(void) uint32_t available_bytes = port->available(); for (uint32_t i = 0; i < available_bytes; i++) { uint8_t temp = port->read(); -#if AP_GPS_DEBUG_LOGGING_ENABLED + #if AP_GPS_DEBUG_LOGGING_ENABLED log_data(&temp, 1); -#endif + #endif ret |= parse(temp); } @@ -139,16 +142,20 @@ AP_GPS_SBF::read(void) } break; case Config_State::Constellation: - if ((params.gnss_mode&0x6F)!=0) { - //IMES not taken into account by Septentrio receivers - if (asprintf(&config_string, "sst, %s%s%s%s%s%s\n", (params.gnss_mode&(1U<<0))!=0 ? "GPS" : "", - (params.gnss_mode&(1U<<1))!=0 ? ((params.gnss_mode&0x01)==0 ? "SBAS" : "+SBAS") : "", - (params.gnss_mode&(1U<<2))!=0 ? ((params.gnss_mode&0x03)==0 ? "GALILEO" : "+GALILEO") : "", - (params.gnss_mode&(1U<<3))!=0 ? ((params.gnss_mode&0x07)==0 ? "BEIDOU" : "+BEIDOU") : "", - (params.gnss_mode&(1U<<5))!=0 ? ((params.gnss_mode&0x0F)==0 ? "QZSS" : "+QZSS") : "", - (params.gnss_mode&(1U<<6))!=0 ? ((params.gnss_mode&0x2F)==0 ? "GLONASS" : "+GLONASS") : "") == -1) { - config_string=nullptr; - } + //IMES not taken into account by Septentrio receivers + /*if (asprintf(&config_string, "sst, %s%s%s%s%s%s\n", (params.gnss_mode&(1U<<0))!=0 ? "GPS" : "", + (params.gnss_mode&(1U<<1))!=0 ? ((params.gnss_mode&0x01)==0 ? "SBAS" : "+SBAS") : "", + (params.gnss_mode&(1U<<2))!=0 ? ((params.gnss_mode&0x03)==0 ? "GALILEO" : "+GALILEO") : "", + (params.gnss_mode&(1U<<3))!=0 ? ((params.gnss_mode&0x07)==0 ? "BEIDOU" : "+BEIDOU") : "", + (params.gnss_mode&(1U<<5))!=0 ? ((params.gnss_mode&0x0F)==0 ? "QZSS" : "+QZSS") : "", + (params.gnss_mode&(1U<<6))!=0 ? ((params.gnss_mode&0x2F)==0 ? "GLONASS" : "+GLONASS") : "") == -1) { + config_string=nullptr; + }*/ + case Config_State::SSO_Status: + if (asprintf(&config_string, "sso,Stream%d,COM%d,GalAuthStatus+RFStatus+QualityInd+ReceiverStatus,sec1\n", + (int)GPS_SBF_STATUS_STREAM_NUMBER, + (int)params.com_port) == -1) { + config_string = nullptr; } break; case Config_State::Blob: @@ -377,9 +384,17 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::SSO; break; case Config_State::SSO: - config_step = Config_State::Constellation; + if ((params.gnss_mode&0x6F)!=0) { + config_step = Config_State::Constellation; + } + else { + config_step = Config_State::SSO_Status; + } break; case Config_State::Constellation: + config_step = Config_State::SSO_Status; + break; + case Config_State::SSO_Status: config_step = Config_State::Blob; break; case Config_State::Blob: @@ -548,6 +563,85 @@ AP_GPS_SBF::process_message(void) (unsigned int)(RxError & RX_ERROR_MASK), (unsigned int)(temp.RxError & RX_ERROR_MASK)); } RxError = temp.RxError; + ExtError = temp.ExtError; + state.system_errors = AP_GPS::GPS_Errors::GPS_SYSTEM_ERROR_NONE; + if (ExtError & CORRECTION) { + state.system_errors |= AP_GPS::GPS_Errors::INCOMING_CORRECTIONS; + } + if (RxError & INVALIDCONFIG) { + state.system_errors |= AP_GPS::GPS_Errors::CONFIGURATION; + } + if (RxError & SOFTWARE) { + state.system_errors |= AP_GPS::GPS_Errors::SOFTWARE; + } + if (RxError & ANTENNA) { + state.system_errors |= AP_GPS::GPS_Errors::ANTENNA; + } + if (RxError & MISSEDEVENT) { + state.system_errors |= AP_GPS::GPS_Errors::EVENT_CONGESTION; + } + if (RxError & CPUOVERLOAD) { + state.system_errors |= AP_GPS::GPS_Errors::CPU_OVERLOAD; + } + if (RxError & CONGESTION) { + state.system_errors |= AP_GPS::GPS_Errors::OUTPUT_CONGESTION; + } + break; + } + case RFStatus: + { + const msg4092 &temp = sbf_msg.data.msg4092u; + check_new_itow(temp.TOW, sbf_msg.length); + #if HAL_GCS_ENABLED + if (temp.Flags==0) { + state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_OK; + } + else { + state.spoofing_state = AP_GPS::GPS_Spoofing::SPOOFING_DETECTED; + } + state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_OK; + for (int i = 0; i < temp.N; i++) { + RFStatusRFBandSubBlock* rf_band_data = (RFStatusRFBandSubBlock*)(&temp.RFBand + i * temp.SBLength); + switch (rf_band_data->Info & (uint8_t)0b111) { + case 1: + case 2: + // As long as there is indicated but unmitigated spoofing in one band, don't report the overall state as mitigated + if (state.jamming_state == AP_GPS::GPS_Jamming::JAMMING_OK) { + state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_MITIGATED; + } + break; + case 8: + state.jamming_state = AP_GPS::GPS_Jamming::JAMMING_DETECTED; + break; + } + } + break; + #endif + } + case GALAuthStatus: + { + const msg4245 &temp = sbf_msg.data.msg4245u; + check_new_itow(temp.TOW, sbf_msg.length); + switch (temp.OSNMAStatus & (uint16_t)0b111) { + case 0: + state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_DISABLED; + break; + case 1: + case 2: + state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_INITIALIZING; + break; + case 3: + case 4: + case 5: + state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_ERROR; + break; + case 6: + state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_OK; + break; + default: + state.authentication_state = AP_GPS::GPS_Authentication::AUTHENTICATION_UNKNOWN; + break; + } break; } case VelCovGeodetic: diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index b4e1bb28c62b1..c39ee5e07e16f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -73,6 +73,7 @@ class AP_GPS_SBF : public AP_GPS_Backend enum class Config_State { Baud_Rate, SSO, + SSO_Status, Blob, SBAS, SGA, @@ -103,6 +104,7 @@ class AP_GPS_SBF : public AP_GPS_Backend uint32_t crc_error_counter = 0; uint32_t RxState; uint32_t RxError; + uint8_t ExtError; void mount_disk(void) const; void unmount_disk(void) const; @@ -113,6 +115,8 @@ class AP_GPS_SBF : public AP_GPS_Backend PVTGeodetic = 4007, ReceiverStatus = 4014, BaseVectorGeod = 4028, + RFStatus = 4092, + GALAuthStatus = 4245, VelCovGeodetic = 5908, AttEulerCov = 5939, AuxAntPositions = 5942, @@ -178,6 +182,36 @@ class AP_GPS_SBF : public AP_GPS_Backend // remaining data is AGCData, which we don't have a use for, don't extract the data }; + struct PACKED RFStatusRFBandSubBlock + { + uint32_t Frequency; + uint16_t Bandwidth; + uint8_t Info; + }; + + struct PACKED msg4092 // RFStatus + { + uint32_t TOW; + uint16_t WNc; + uint8_t N; + uint8_t SBLength; + uint8_t Flags; + uint8_t Reserved[3]; + uint8_t RFBand; // So we can use address and parse sub-blocks manually + }; + + struct PACKED msg4245 // GALAuthStatus + { + uint32_t TOW; + uint16_t WNc; + uint16_t OSNMAStatus; + float TrustedTimeDelta; + uint64_t GalActiveMask; + uint64_t GalAuthenticMask; + uint64_t GpsActiveMask; + uint64_t GpsAuthenticMask; + }; + struct PACKED VectorInfoGeod { uint8_t NrSV; uint8_t Error; @@ -265,6 +299,8 @@ class AP_GPS_SBF : public AP_GPS_Backend msg4001 msg4001u; msg4014 msg4014u; msg4028 msg4028u; + msg4092 msg4092u; + msg4245 msg4245u; msg5908 msg5908u; msg5939 msg5939u; msg5942 msg5942u; @@ -297,6 +333,7 @@ class AP_GPS_SBF : public AP_GPS_Backend enum { SOFTWARE = (1 << 3), // set upon detection of a software warning or error. This bit is reset by the command lif, error WATCHDOG = (1 << 4), // set when the watch-dog expired at least once since the last power-on. + ANTENNA = (1 << 5), // set when antenna overcurrent condition is detected CONGESTION = (1 << 6), // set when an output data congestion has been detected on at least one of the communication ports of the receiver during the last second. MISSEDEVENT = (1 << 8), // set when an external event congestion has been detected during the last second. It indicates that the receiver is receiving too many events on its EVENTx pins. CPUOVERLOAD = (1 << 9), // set when the CPU load is larger than 90%. If this bit is set, receiver operation may be unreliable and the user must decrease the processing load by following the recommendations in the User Manual. @@ -304,6 +341,10 @@ class AP_GPS_SBF : public AP_GPS_Backend OUTOFGEOFENCE = (1 << 11), // set if the receiver is currently out of its permitted region of operation (geo-fencing). }; + enum { + CORRECTION = (1 << 1), // : set when an anomaly has been detected in an incoming differential correction stream + }; + static constexpr const char *portIdentifiers[] = { "COM", "USB", "IP1", "NTR", "IPS", "IPR" }; char portIdentifier[5]; uint8_t portLength; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e6318c9e2a15f..9b89b5f72c336 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1019,6 +1019,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_GPS_ENABLED { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW}, { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, + { MAVLINK_MSG_ID_GNSS_INTEGRITY, MSG_GNSS_INTEGRITY}, #if GPS_MAX_RECEIVERS > 1 { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, @@ -6150,6 +6151,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(GPS_RTK); AP::gps().send_mavlink_gps_rtk(chan, 0); break; + case MSG_GNSS_INTEGRITY: + CHECK_PAYLOAD_SIZE(GNSS_INTEGRITY); + AP::gps().send_mavlink_gnss_integrity(chan, 0); + break; #if GPS_MAX_RECEIVERS > 1 case MSG_GPS2_RAW: CHECK_PAYLOAD_SIZE(GPS2_RAW); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 4923317266025..b2baf433ca23d 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -35,6 +35,7 @@ enum ap_message : uint8_t { MSG_SCALED_PRESSURE3, MSG_GPS_RAW, MSG_GPS_RTK, + MSG_GNSS_INTEGRITY, MSG_GPS2_RAW, MSG_GPS2_RTK, MSG_SYSTEM_TIME,