AP_GPS: added GPS_DRV_OPTIONS for fully parsing RTCMv3 injection

Tools/AP_Periph/gps.cpp

@@ -249,27 +249,41 @@ void AP_Periph_FW::send_moving_baseline_msg()
     if (len == 0 || data == nullptr) {
         return;
     }
-    // send the packet from Moving Base to be used RelPosHeading calc by GPS module
-    ardupilot_gnss_MovingBaselineData mbldata {};
-    // get the data from the moving base
-    static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong");
-    mbldata.data.len = len;
-    memcpy(mbldata.data.data, data, len);
-    uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {};
-    const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout());
+
+    /*
+      send the packet from Moving Base to be used RelPosHeading calc by GPS module
+      for long RTCMv3 packets we may need to split it up
+    */
 
     uint8_t iface_mask = 0;
 #if HAL_NUM_CAN_IFACES >= 2 && CANARD_MULTI_IFACE
     if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) {
         iface_mask = 1U<<gps_mb_can_port;
     }
 #endif
-    canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
-                     ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
-                     CANARD_TRANSFER_PRIORITY_LOW,
-                     &buffer[0],
-                     total_size,
-                     iface_mask);
+
+    // get the data from the moving base and send as MovingBaselineData message
+    while (len > 0) {
+        ardupilot_gnss_MovingBaselineData mbldata {};
+
+        const uint16_t n = MIN(len, sizeof(mbldata.data.data));
+
+        mbldata.data.len = n;
+        memcpy(mbldata.data.data, data, n);
+
+        uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {};
+        const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout());
+
+        canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
+                         ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
+                         CANARD_TRANSFER_PRIORITY_LOW,
+                         &buffer[0],
+                         total_size,
+                         iface_mask);
+        len -= n;
+        data += n;
+    }
+
     gps.clear_RTCMV3();
 #endif // GPS_MOVING_BASELINE
 }

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:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 
@@ -1338,12 +1342,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);
@@ -1676,7 +1680,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);
@@ -1686,9 +1690,89 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
         return;
     }
 
+#if AP_GPS_RTCM_DECODE_ENABLED
+    if (!option_set(DriverOptions::DisableRTCMDecode)) {
+        const uint16_t mask = (1U << unsigned(chan));
+        rtcm.seen_mav_channels |= mask;
+        if (option_set(DriverOptions::AlwaysRTCMDecode) ||
+            (rtcm.seen_mav_channels & ~mask) != 0) {
+            /*
+              we are seeing RTCM on multiple mavlink channels. 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;
+        }
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: RTCM parsing for chan %u", unsigned(chan));
+    }
+    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,9 @@ class AP_GPS
         UBX_Use115200     = (1U << 2U),
         UAVCAN_MBUseDedicatedBus  = (1 << 3U),
         HeightEllipsoid   = (1U << 4),
-        GPSL5HealthOverride = (1U << 5)
+        GPSL5HealthOverride = (1U << 5),
+        AlwaysRTCMDecode = (1U << 6),
+        DisableRTCMDecode = (1U << 7),
     };
 
     // check if an option is set
@@ -729,7 +732,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
@@ -787,6 +790,20 @@ 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;
+        uint16_t seen_mav_channels;
+    } 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[]

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4107,7 +4107,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_GPS_INPUT:
     case MAVLINK_MSG_ID_HIL_GPS:
     case MAVLINK_MSG_ID_GPS_INJECT_DATA:
-        AP::gps().handle_msg(msg);
+        AP::gps().handle_msg(chan, msg);
         break;
 #endif