fix(GPS): correct GPS_RTCM_DATA fragmentation for MAVLink reassembly

src/GPS/RTCM/RTCMMavlink.cc

@@ -3,6 +3,8 @@
 #include <QtCore/QByteArray>
 #include <QtCore/QSet>
 #include <QtCore/QThread>
+#include <algorithm>
+#include <cstring>
 
 #include "LinkInterface.h"
 #include "MAVLinkProtocol.h"
@@ -14,6 +16,9 @@
 
 QGC_LOGGING_CATEGORY(RTCMMavlinkLog, "GPS.RTCMMavlink")
 
+// Compile-time check that our constants match the MAVLink message definition.
+static_assert(RTCMMavlink::kFragmentLen == MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN);
+
 RTCMMavlink::RTCMMavlink(QObject* parent) : QObject(parent)
 {
     qCDebug(RTCMMavlinkLog) << this;
@@ -24,41 +29,103 @@ RTCMMavlink::~RTCMMavlink()
     qCDebug(RTCMMavlinkLog) << this;
 }
 
-void RTCMMavlink::RTCMDataUpdate(QByteArrayView data)
+uint8_t RTCMMavlink::_makeFlags(bool fragmented, uint8_t fragmentId, uint8_t sequenceId)
 {
-    _rateTracker.recordBytes(data.size());
-    if (_rateTracker.rateUpdated()) {
-        qCDebug(RTCMMavlinkLog) << QStringLiteral("RTCM bandwidth: %1 kB/s").arg(_rateTracker.kBps(), 0, 'f', 3);
-        emit bandwidthChanged();
+    uint8_t flags = static_cast<uint8_t>((sequenceId & 0x1FU) << 3);
+    if (fragmented) {
+        flags |= 0x01U;
+        flags |= static_cast<uint8_t>((fragmentId & 0x03U) << 1);
     }
+    return flags;
+}
 
-    mavlink_gps_rtcm_data_t gpsRtcmData{};
+RTCMMavlink::PackResult RTCMMavlink::pack(QByteArrayView data, uint8_t sequenceId)
+{
+    PackResult result;
+    result.nextSequenceId = sequenceId;
 
-    static constexpr qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
-    if (data.size() < maxMessageLength) {
-        gpsRtcmData.len = data.size();
-        gpsRtcmData.flags = (_sequenceId & 0x1FU) << 3;
-        (void) memcpy(&gpsRtcmData.data, data.data(), data.size());
-        _sendMessageOnAllLinks(gpsRtcmData);
-    } else {
-        uint8_t fragmentId = 0;
+    if (data.isEmpty()) {
+        return result;
+    }
+
+    // Larger than the 4-fragment reassembly window: stream unfragmented chunks so
+    // the vehicle's RTCM framer can rebuild frames from the inject stream. Do not
+    // invent fragment IDs beyond 0..3 (would clobber the sequence field).
+    if (data.size() > kMaxAssembledLen) {
         qsizetype start = 0;
         while (start < data.size()) {
-            gpsRtcmData.flags = 0x01U;                        // LSB set indicates message is fragmented
-            gpsRtcmData.flags |= fragmentId++ << 1;           // Next 2 bits are fragment id
-            gpsRtcmData.flags |= (_sequenceId & 0x1FU) << 3;  // Next 5 bits are sequence id
+            const qsizetype length = std::min(data.size() - start, kFragmentLen);
+            GpsRtcmPacket packet;
+            packet.flags = _makeFlags(false, 0, result.nextSequenceId);
+            packet.data = data.mid(start, length).toByteArray();
+            result.packets.append(std::move(packet));
+            ++result.nextSequenceId;
+            start += length;
+        }
+        return result;
+    }
 
-            const qsizetype length = std::min(data.size() - start, maxMessageLength);
-            gpsRtcmData.len = length;
+    if (data.size() <= kFragmentLen) {
+        GpsRtcmPacket packet;
+        packet.flags = _makeFlags(false, 0, sequenceId);
+        packet.data = data.toByteArray();
+        result.packets.append(std::move(packet));
+        ++result.nextSequenceId;
+        return result;
+    }
 
-            (void) memcpy(gpsRtcmData.data, data.constData() + start, length);
-            _sendMessageOnAllLinks(gpsRtcmData);
+    // Fragmented: 181..720 bytes. Fragment ID is only 2 bits (0..3).
+    uint8_t fragmentId = 0;
+    qsizetype start = 0;
+    while (start < data.size()) {
+        const qsizetype length = std::min(data.size() - start, kFragmentLen);
+        GpsRtcmPacket packet;
+        packet.flags = _makeFlags(true, fragmentId, sequenceId);
+        packet.data = data.mid(start, length).toByteArray();
+        result.packets.append(std::move(packet));
+        ++fragmentId;
+        start += length;
+    }
 
-            start += length;
-        }
+    // Exact multiple of 180 with fewer than 4 fragments: MAVLink requires a final
+    // zero-length fragment so receivers know the message is complete. (All four
+    // full fragments complete by the "all fragments present" rule without this.)
+    // See ArduPilot AP_GPS::handle_gps_rtcm_fragment and PX4 GpsRtcmMessageAssembler.
+    if ((data.size() % kFragmentLen) == 0 && fragmentId < kMaxFragments) {
+        GpsRtcmPacket terminator;
+        terminator.flags = _makeFlags(true, fragmentId, sequenceId);
+        terminator.data.clear();
+        result.packets.append(std::move(terminator));
     }
 
-    ++_sequenceId;
+    ++result.nextSequenceId;
+    return result;
+}
+
+void RTCMMavlink::RTCMDataUpdate(QByteArrayView data)
+{
+    if (data.isEmpty()) {
+        return;
+    }
+
+    _rateTracker.recordBytes(data.size());
+    if (_rateTracker.rateUpdated()) {
+        qCDebug(RTCMMavlinkLog) << QStringLiteral("RTCM bandwidth: %1 kB/s").arg(_rateTracker.kBps(), 0, 'f', 3);
+        emit bandwidthChanged();
+    }
+
+    const PackResult packed = pack(data, _sequenceId);
+    _sequenceId = packed.nextSequenceId;
+
+    for (const GpsRtcmPacket& packet : packed.packets) {
+        mavlink_gps_rtcm_data_t gpsRtcmData{};
+        gpsRtcmData.flags = packet.flags;
+        gpsRtcmData.len = static_cast<uint8_t>(packet.data.size());
+        if (!packet.data.isEmpty()) {
+            (void) memcpy(gpsRtcmData.data, packet.data.constData(), static_cast<size_t>(packet.data.size()));
+        }
+        _sendMessageOnAllLinks(gpsRtcmData);
+    }
 }
 
 void RTCMMavlink::sendSimulatedData(const std::atomic_bool& requestStop)
@@ -99,8 +166,8 @@ void RTCMMavlink::_sendMessageOnAllLinks(const mavlink_gps_rtcm_data_t& data)
 
         mavlink_message_t message{};
         (void) mavlink_msg_gps_rtcm_data_encode_chan(MAVLinkProtocol::instance()->getSystemId(),
-                                                     MAVLinkProtocol::getComponentId(),
-                                                     sharedLink->mavlinkChannel(), &message, &data);
+                                                     MAVLinkProtocol::getComponentId(), sharedLink->mavlinkChannel(),
+                                                     &message, &data);
         sharedLink->sendMessageThreadSafe(message);
     }
 }

src/GPS/RTCM/RTCMMavlink.h

@@ -1,26 +1,63 @@
 #pragma once
 
+#include <QtCore/QByteArray>
+#include <QtCore/QList>
 #include <QtCore/QObject>
 #include <atomic>
+#include <cstdint>
 
 #include "DataRateTracker.h"
 
 typedef struct __mavlink_gps_rtcm_data_t mavlink_gps_rtcm_data_t;
 
+/// One GPS_RTCM_DATA payload ready to encode. flags layout matches MAVLink:
+/// bit0 = fragmented, bits1-2 = fragment ID, bits3-7 = sequence ID.
+struct GpsRtcmPacket
+{
+    uint8_t flags = 0;
+    QByteArray data;  // 0..kFragmentLen bytes
+};
+
 class RTCMMavlink : public QObject
 {
     Q_OBJECT
     Q_PROPERTY(quint64 totalBytesSent READ totalBytesSent NOTIFY bandwidthChanged)
     Q_PROPERTY(double bandwidthKBps READ bandwidthKBps NOTIFY bandwidthChanged)
 
 public:
+    /// MAVLink GPS_RTCM_DATA data[] field length.
+    static constexpr qsizetype kFragmentLen = 180;
+    /// Fragment ID is 2 bits — at most 4 fragments per reassembled message.
+    static constexpr qsizetype kMaxFragments = 4;
+    /// Max payload that fits one fragmented sequence (4 * 180).
+    static constexpr qsizetype kMaxAssembledLen = kFragmentLen * kMaxFragments;
+
     RTCMMavlink(QObject* parent = nullptr);
     ~RTCMMavlink();
 
     quint64 totalBytesSent() const { return _rateTracker.totalBytes(); }
 
     double bandwidthKBps() const { return _rateTracker.kBps(); }
 
+    /// Pack one RTCM blob into GPS_RTCM_DATA packets per MAVLink rules.
+    ///
+    /// - size 0: no packets
+    /// - size <= 180: one unfragmented packet
+    /// - 181..720: fragmented; exact multiples of 180 with fewer than 4 fragments
+    ///   get a final zero-length fragment (required by MAVLink / ArduPilot / PX4)
+    /// - size > 720: stream as successive unfragmented chunks (protocol cannot
+    ///   reassemble more than 720 bytes in one sequence)
+    ///
+    /// @param sequenceId starting sequence id (0..31); advanced for each logical message
+    /// @return packets plus the next sequence id to use
+    struct PackResult
+    {
+        QList<GpsRtcmPacket> packets;
+        uint8_t nextSequenceId = 0;
+    };
+
+    static PackResult pack(QByteArrayView data, uint8_t sequenceId);
+
 public slots:
     void RTCMDataUpdate(QByteArrayView data);
 
@@ -34,6 +71,7 @@ public slots:
 
 private:
     static void _sendMessageOnAllLinks(const mavlink_gps_rtcm_data_t& data);
+    static uint8_t _makeFlags(bool fragmented, uint8_t fragmentId, uint8_t sequenceId);
 
     uint8_t _sequenceId = 0;
     DataRateTracker _rateTracker;

src/GPS/RTCM/RTCMParser.cc

@@ -106,31 +106,3 @@ QByteArray RTCMParser::currentFrame() const
     frame.append(reinterpret_cast<const char*>(_crcBytes), kCrcSize);
     return frame;
 }
-
-QByteArray RTCMParser::extractValidFrames(const QByteArray& in, int* framesFound, int* framesDropped)
-{
-    QByteArray out;
-    int found = 0;
-    int dropped = 0;
-
-    for (char ch : in) {
-        if (!addByte(static_cast<uint8_t>(ch))) {
-            continue;
-        }
-        if (validateCrc()) {
-            out.append(currentFrame());
-            ++found;
-        } else {
-            ++dropped;
-        }
-        reset();
-    }
-
-    if (framesFound) {
-        *framesFound = found;
-    }
-    if (framesDropped) {
-        *framesDropped = dropped;
-    }
-    return out;
-}

src/GPS/RTCM/RTCMParser.h

@@ -38,11 +38,6 @@ class RTCMParser
     /// immediately after addByte() returned true, before the next reset().
     QByteArray currentFrame() const;
 
-    /// Feed a buffer through the parser, carrying state across calls, and return
-    /// the concatenation of every complete CRC-valid frame found. Whitelist
-    /// filtering is NOT applied. Optionally reports frame counts for caller logging.
-    QByteArray extractValidFrames(const QByteArray& in, int* framesFound = nullptr, int* framesDropped = nullptr);
-
 private:
     enum class State
     {

src/GPS/RTCM/RTCMUdpInput.cc

@@ -28,6 +28,11 @@ bool RTCMUdpInput::start()
     }
     connect(_socket, &QUdpSocket::readyRead, this, &RTCMUdpInput::_readDatagrams);
 
+    if (_port == 0) {
+        _port = _socket->localPort();
+        emit portChanged();
+    }
+
     _running = true;
     emit runningChanged();
     qCDebug(RTCMUdpInputLog) << "Listening for RTCM data on UDP port" << _port;
@@ -82,23 +87,31 @@ void RTCMUdpInput::_readDatagrams()
             continue;
         }
 
+        // Emit one complete RTCM3 frame per signal so RTCMMavlink assigns a distinct
+        // GPS_RTCM_DATA sequence per frame (required for correct MAVLink reassembly).
         int framesFound = 0;
         int framesDropped = 0;
-        const QByteArray validData = _rtcmParser.extractValidFrames(data, &framesFound, &framesDropped);
+        for (const char ch : data) {
+            if (!_rtcmParser.addByte(static_cast<uint8_t>(static_cast<unsigned char>(ch)))) {
+                continue;
+            }
+            if (_rtcmParser.validateCrc()) {
+                ++framesFound;
+                ++_validFrames;
+                emit rtcmDataReceived(_rtcmParser.currentFrame());
+            } else {
+                ++framesDropped;
+                ++_invalidFrames;
+            }
+            _rtcmParser.reset();
+        }
 
-        _validFrames += static_cast<quint64>(framesFound);
-        _invalidFrames += static_cast<quint64>(framesDropped);
         if (framesDropped > 0) {
             qCWarning(RTCMUdpInputLog) << "Dropped" << framesDropped << "RTCM frame(s) - CRC mismatch";
         }
 
-        qCDebug(RTCMUdpInputLog) << "Datagram" << data.size() << "bytes -"
-                                 << "framesFound:" << framesFound << "framesDropped:" << framesDropped
-                                 << "validData:" << validData.size() << "bytes";
-
-        if (!validData.isEmpty()) {
-            emit rtcmDataReceived(validData);
-        }
+        qCDebug(RTCMUdpInputLog) << "Datagram" << data.size() << "bytes -" << "framesFound:" << framesFound
+                                 << "framesDropped:" << framesDropped;
 
         const quint64 totalFrames = _validFrames + _invalidFrames;
         if (totalFrames > 0) {

src/GPS/RTCM/RTCMUdpInput.h

@@ -25,7 +25,8 @@ class QUdpSocket;
  * The class accepts datagrams from any sender on the bound port. With validation
  * disabled each datagram is emitted as-is; with validation enabled (see
  * setValidation) datagrams are reframed through RTCMParser and only CRC-valid
- * RTCM3 frames are forwarded. Downstream (RTCMMavlink) fragments as needed.
+ * RTCM3 frames are forwarded — one signal per frame so each gets its own
+ * GPS_RTCM_DATA sequence. Downstream (RTCMMavlink) fragments as needed.
  */
 class RTCMUdpInput : public QObject
 {
@@ -39,6 +40,7 @@ class RTCMUdpInput : public QObject
 
     /// Bind the socket and begin accepting datagrams.
     /// Safe to call on an already-running instance — restarts with the current port.
+    /// Port 0 binds an ephemeral port; port() then reports the bound port.
     bool start();
 
     /// Unbind the socket and stop accepting datagrams.
@@ -56,7 +58,8 @@ class RTCMUdpInput : public QObject
     void setValidation(const bool validate) { _validateRtcm = validate; }
 
 signals:
-    /// Emitted once per received datagram with the raw RTCM payload.
+    /// Emitted with RTCM payload to forward. With validation off: once per
+    /// datagram. With validation on: once per CRC-valid RTCM3 frame.
     /// Connect directly to RTCMMavlink::RTCMDataUpdate (same thread).
     void rtcmDataReceived(const QByteArray& data);
 

test/GPS/CMakeLists.txt

@@ -23,11 +23,17 @@ target_sources(${CMAKE_PROJECT_NAME}
         UdpForwarderTest.h
         RTCMParserTest.cc
         RTCMParserTest.h
+        RTCMMavlinkTest.cc
+        RTCMMavlinkTest.h
+        RTCMUdpInputTest.cc
+        RTCMUdpInputTest.h
 )
 
 target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
 
 add_qgc_test(RTCMParserTest LABELS Unit)
+add_qgc_test(RTCMMavlinkTest LABELS Unit)
+add_qgc_test(RTCMUdpInputTest LABELS Unit)
 add_qgc_test(NTRIPManagerTest LABELS Unit)
 add_qgc_test(NTRIPHttpTransportTest LABELS Unit)
 add_qgc_test(NTRIPSourceTableTest LABELS Unit)