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

@@ -55,6 +55,10 @@
 #include <AP_Logger/AP_Logger.h>
 #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; i<pkt.len; i++) {
+        if (rtcm.parsers[chan]->read(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<ARRAY_SIZE(rtcm.sent_crc); c++) {
+                if (rtcm.sent_crc[c] == crc) {
+                    // we have already sent this message
+                    already_seen = true;
+                    break;
+                }
+            }
+            if (already_seen) {
+                continue;
+            }
+            rtcm.sent_crc[rtcm.sent_idx] = crc;
+            rtcm.sent_idx = (rtcm.sent_idx+1) % ARRAY_SIZE(rtcm.sent_crc);
+
+            if (buf != nullptr && len > 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<num_instances; instance++) {

libraries/AP_GPS/AP_GPS.h

@@ -68,6 +68,7 @@
 #endif // GPS_MOVING_BASELINE
 
 class AP_GPS_Backend;
+class RTCM3_Parser;
 
 /// @class AP_GPS
 /// GPS driver main class
@@ -246,7 +247,7 @@ class AP_GPS
     void update(void);
 
     // Pass mavlink data to message handlers (for MAV type)
-    void handle_msg(const mavlink_message_t &msg);
+    void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);
 #if HAL_MSP_GPS_ENABLED
     void handle_msp(const MSP::msp_gps_data_message_t &pkt);
 #endif
@@ -624,7 +625,8 @@ class AP_GPS
         UBX_Use115200     = (1U << 2U),
         UAVCAN_MBUseDedicatedBus  = (1 << 3U),
         HeightEllipsoid   = (1U << 4),
-        GPSL5HealthOverride = (1U << 5)
+        GPSL5HealthOverride = (1U << 5),
+        RTCMDecode = (1U << 6)
     };
 
     // check if an option is set
@@ -728,7 +730,7 @@ class AP_GPS
     } rtcm_stats;
 
     // re-assemble GPS_RTCM_DATA message
-    void handle_gps_rtcm_data(const mavlink_message_t &msg);
+    void handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg);
     void handle_gps_inject(const mavlink_message_t &msg);
 
     //Inject a packet of raw binary to a GPS
@@ -786,6 +788,19 @@ class AP_GPS
     // logging support
     void Write_GPS(uint8_t instance);
 
+#if AP_GPS_RTCM_DECODE_ENABLED
+    /*
+      per mavlink channel RTCM decoder, enabled with RTCM decode
+       option in GPS_DRV_OPTIONS
+    */
+    struct {
+        RTCM3_Parser *parsers[MAVLINK_COMM_NUM_BUFFERS];
+        uint32_t sent_crc[32];
+        uint8_t sent_idx;
+    } rtcm;
+    bool parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt);
+#endif
+
 };
 
 namespace AP {

libraries/AP_GPS/AP_GPS_config.h

@@ -62,3 +62,7 @@
 #ifndef AP_GPS_UBLOX_ENABLED
   #define AP_GPS_UBLOX_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED
 #endif
+
+#ifndef AP_GPS_RTCM_DECODE_ENABLED
+  #define AP_GPS_RTCM_DECODE_ENABLED BOARD_FLASH_SIZE > 1024
+#endif

libraries/AP_GPS/RTCM3_Parser.h

@@ -19,7 +19,10 @@
 #pragma once
 #include <stdint.h>
 
-#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[]