From 12408dffd9c26effc6434e567cdd7619a36cc9fa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 25 Feb 2024 10:03:25 +1100 Subject: [PATCH] AP_GPS: added option to decode RTCM injected data this allows for redundent RTCM links (eg. WiFi and SiK links for light show drones) without causing corruption into the GPS. If the GPS_DRV_OPTION bit is set then we instantiate a separate RTCM3 decoder per mavlink channel, and only inject when we get a full packet that passes the RTCM 24 bit CRC --- libraries/AP_GPS/AP_GPS.cpp | 83 ++++++++++++++++++++++++++++++-- libraries/AP_GPS/AP_GPS.h | 21 ++++++-- libraries/AP_GPS/AP_GPS_config.h | 4 ++ libraries/AP_GPS/RTCM3_Parser.h | 7 ++- 4 files changed, 106 insertions(+), 9 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 94e98f0ac1d83b..9c77190494213b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -55,6 +55,10 @@ #include #include "AP_GPS_FixType.h" +#if AP_GPS_RTCM_DECODE_ENABLED +#include "RTCM3_Parser.h" +#endif + #define GPS_RTK_INJECT_TO_ALL 127 #ifndef GPS_MAX_RATE_MS #define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms @@ -341,7 +345,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options - // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health + // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Fully parse RTCMv3 data that comes in via the GPS_RTCM_DATA message before injecting to the GPS // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), @@ -1334,12 +1338,12 @@ void AP_GPS::handle_gps_inject(const mavlink_message_t &msg) /* pass along a mavlink message (for MAV type) */ -void AP_GPS::handle_msg(const mavlink_message_t &msg) +void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) { switch (msg.msgid) { case MAVLINK_MSG_ID_GPS_RTCM_DATA: // pass data to de-fragmenter - handle_gps_rtcm_data(msg); + handle_gps_rtcm_data(chan, msg); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg); @@ -1672,7 +1676,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_ /* re-assemble GPS_RTCM_DATA message */ -void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg) +void AP_GPS::handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg) { mavlink_gps_rtcm_data_t packet; mavlink_msg_gps_rtcm_data_decode(&msg, &packet); @@ -1682,9 +1686,80 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg) return; } +#if AP_GPS_RTCM_DECODE_ENABLED + if (option_set(DriverOptions::RTCMDecode)) { + // we will run the data through a full per-channel RTCM decoder + if (parse_rtcm_injection(chan, packet)) { + return; + } + } +#endif + handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len); } +#if AP_GPS_RTCM_DECODE_ENABLED +/* + fully parse RTCM data coming in from a MAVLink channel, when we have + a full message inject it to the GPS. This approach allows for 2 or + more MAVLink channels to be used for the same RTCM data, allowing + for redundent transports for maximum reliability at the cost of some + extra CPU and a bit of re-assembly lag + */ +bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt) +{ + if (chan >= MAVLINK_COMM_NUM_BUFFERS) { + return false; + } + if (rtcm.parsers[chan] == nullptr) { + rtcm.parsers[chan] = new RTCM3_Parser(); + if (rtcm.parsers[chan] == nullptr) { + return false; + } + } + for (uint16_t i=0; iread(pkt.data[i])) { + // we have a full message, inject it + const uint8_t *buf = nullptr; + uint16_t len = rtcm.parsers[chan]->get_len(buf); + + // see if we have already sent it. This prevents + // duplicates from multiple sources + const uint32_t crc = crc_crc32(0, buf, len); + +#if HAL_LOGGING_ENABLED + AP::logger().WriteStreaming("RTCM", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI", + AP_HAL::micros64(), + uint8_t(chan), + rtcm.parsers[chan]->get_id(), + len, + crc); +#endif + + bool already_seen = false; + for (uint8_t c=0; c 0) { + inject_data(buf, len); + } + rtcm.parsers[chan]->reset(); + } + } + return true; +} +#endif // AP_GPS_RTCM_DECODE_ENABLED + void AP_GPS::Write_AP_Logger_Log_Startup_messages() { for (uint8_t instance=0; instance 1024 +#endif diff --git a/libraries/AP_GPS/RTCM3_Parser.h b/libraries/AP_GPS/RTCM3_Parser.h index 4c514abb453966..0b15e5b701b926 100644 --- a/libraries/AP_GPS/RTCM3_Parser.h +++ b/libraries/AP_GPS/RTCM3_Parser.h @@ -19,7 +19,10 @@ #pragma once #include -#define RTCM3_MAX_PACKET_LEN 300 +// maximum packet length with MAVLink GPS_RTCM_DATA is 4*180 as we +// handle up to 4 fragments +#define RTCM3_MAX_PACKET_LEN 720 + class RTCM3_Parser { public: // process one byte, return true if packet found @@ -40,7 +43,7 @@ class RTCM3_Parser { private: const uint8_t RTCMv3_PREAMBLE = 0xD3; - // raw packet, we shouldn't need over 300 bytes for the MB configs we use + // raw packet, we shouldn't need over 600 bytes for the MB configs we use uint8_t pkt[RTCM3_MAX_PACKET_LEN]; // number of bytes in pkt[]