From 023faa9fc4839edbf30ba77f01292f7214e00ff8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 25 Feb 2024 10:50:30 +1100
Subject: [PATCH] AP_Periph: allow for RTCMv3 packets larger than 300 bytes

with newer receivers, RTCMv3 packets can be larger than 300
---
 Tools/AP_Periph/gps.cpp | 42 +++++++++++++++++++++++++++--------------
 1 file changed, 28 insertions(+), 14 deletions(-)

diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp
index 029676f26d..f185f709a0 100644
--- a/Tools/AP_Periph/gps.cpp
+++ b/Tools/AP_Periph/gps.cpp
@@ -249,14 +249,11 @@ 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
@@ -264,12 +261,29 @@ void AP_Periph_FW::send_moving_baseline_msg()
         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
 }