From cb910b7825818978bfb24ae82b46a1d7ce273d5f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 6 Aug 2026 20:23:24 -0600 Subject: [PATCH 1/3] fix(GPS): correct GPS_RTCM_DATA fragmentation for MAVLink reassembly Match MAVLink/ArduPilot/PX4 rules: unfragmented packets up to 180 bytes, zero-length terminator for exact 180 multiples under 4 fragments, and stream oversized (>720) payloads as unfragmented chunks instead of overflowing the 2-bit fragment ID. Emit one UDP-validated RTCM frame per sequence. --- src/GPS/RTCM/RTCMMavlink.cc | 119 ++++++++++++++++++------ src/GPS/RTCM/RTCMMavlink.h | 38 ++++++++ src/GPS/RTCM/RTCMUdpInput.cc | 28 ++++-- src/GPS/RTCM/RTCMUdpInput.h | 6 +- test/GPS/CMakeLists.txt | 3 + test/GPS/RTCMMavlinkTest.cc | 172 +++++++++++++++++++++++++++++++++++ test/GPS/RTCMMavlinkTest.h | 20 ++++ 7 files changed, 348 insertions(+), 38 deletions(-) create mode 100644 test/GPS/RTCMMavlinkTest.cc create mode 100644 test/GPS/RTCMMavlinkTest.h diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index decb49e3ec1b..a67f4cbfcb2e 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -3,6 +3,8 @@ #include #include #include +#include +#include #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((sequenceId & 0x1FU) << 3); + if (fragmented) { + flags |= 0x01U; + flags |= static_cast((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(packet.data.size()); + if (!packet.data.isEmpty()) { + (void) memcpy(gpsRtcmData.data, packet.data.constData(), static_cast(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); } } diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h index a6e0451d9154..539fcdfe96e1 100644 --- a/src/GPS/RTCM/RTCMMavlink.h +++ b/src/GPS/RTCM/RTCMMavlink.h @@ -1,12 +1,23 @@ #pragma once +#include +#include #include #include +#include #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 @@ -14,6 +25,13 @@ class RTCMMavlink : public QObject 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(); @@ -21,6 +39,25 @@ class RTCMMavlink : public QObject 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 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; diff --git a/src/GPS/RTCM/RTCMUdpInput.cc b/src/GPS/RTCM/RTCMUdpInput.cc index 3700573e768d..dc4f14bd3796 100644 --- a/src/GPS/RTCM/RTCMUdpInput.cc +++ b/src/GPS/RTCM/RTCMUdpInput.cc @@ -82,23 +82,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(static_cast(ch)))) { + continue; + } + if (_rtcmParser.validateCrc()) { + ++framesFound; + ++_validFrames; + emit rtcmDataReceived(_rtcmParser.currentFrame()); + } else { + ++framesDropped; + ++_invalidFrames; + } + _rtcmParser.reset(); + } - _validFrames += static_cast(framesFound); - _invalidFrames += static_cast(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) { diff --git a/src/GPS/RTCM/RTCMUdpInput.h b/src/GPS/RTCM/RTCMUdpInput.h index ddded3385861..78dae16e44e1 100644 --- a/src/GPS/RTCM/RTCMUdpInput.h +++ b/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 { @@ -56,7 +57,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); diff --git a/test/GPS/CMakeLists.txt b/test/GPS/CMakeLists.txt index 4f836bd8072b..0e45f63416b9 100644 --- a/test/GPS/CMakeLists.txt +++ b/test/GPS/CMakeLists.txt @@ -23,11 +23,14 @@ target_sources(${CMAKE_PROJECT_NAME} UdpForwarderTest.h RTCMParserTest.cc RTCMParserTest.h + RTCMMavlinkTest.cc + RTCMMavlinkTest.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(NTRIPManagerTest LABELS Unit) add_qgc_test(NTRIPHttpTransportTest LABELS Unit) add_qgc_test(NTRIPSourceTableTest LABELS Unit) diff --git a/test/GPS/RTCMMavlinkTest.cc b/test/GPS/RTCMMavlinkTest.cc new file mode 100644 index 000000000000..d1d9978830f0 --- /dev/null +++ b/test/GPS/RTCMMavlinkTest.cc @@ -0,0 +1,172 @@ +#include "RTCMMavlinkTest.h" + +#include "RTCMMavlink.h" + +namespace { + +bool isFragmented(uint8_t flags) +{ + return (flags & 0x01U) != 0; +} + +uint8_t fragmentId(uint8_t flags) +{ + return (flags >> 1) & 0x03U; +} + +uint8_t sequenceId(uint8_t flags) +{ + return (flags >> 3) & 0x1FU; +} + +QByteArray makePayload(qsizetype size, char fill = 'R') +{ + return QByteArray(size, fill); +} + +} // namespace + +void RTCMMavlinkTest::_testEmpty() +{ + const auto packed = RTCMMavlink::pack({}, 3); + QCOMPARE(packed.packets.size(), 0); + QCOMPARE(packed.nextSequenceId, static_cast(3)); +} + +void RTCMMavlinkTest::_testUnfragmentedSmall() +{ + const QByteArray data = makePayload(100); + const auto packed = RTCMMavlink::pack(data, 5); + + QCOMPARE(packed.packets.size(), 1); + QCOMPARE(packed.nextSequenceId, static_cast(6)); + QVERIFY(!isFragmented(packed.packets[0].flags)); + QCOMPARE(sequenceId(packed.packets[0].flags), static_cast(5)); + QCOMPARE(packed.packets[0].data, data); +} + +void RTCMMavlinkTest::_testUnfragmentedExact180() +{ + // Full single fragment fits unfragmented — must not force fragmentation. + const QByteArray data = makePayload(RTCMMavlink::kFragmentLen); + const auto packed = RTCMMavlink::pack(data, 0); + + QCOMPARE(packed.packets.size(), 1); + QVERIFY(!isFragmented(packed.packets[0].flags)); + QCOMPARE(packed.packets[0].data.size(), RTCMMavlink::kFragmentLen); +} + +void RTCMMavlinkTest::_testFragmented181() +{ + const QByteArray data = makePayload(181); + const auto packed = RTCMMavlink::pack(data, 2); + + QCOMPARE(packed.packets.size(), 2); + QVERIFY(isFragmented(packed.packets[0].flags)); + QVERIFY(isFragmented(packed.packets[1].flags)); + QCOMPARE(fragmentId(packed.packets[0].flags), static_cast(0)); + QCOMPARE(fragmentId(packed.packets[1].flags), static_cast(1)); + QCOMPARE(sequenceId(packed.packets[0].flags), static_cast(2)); + QCOMPARE(sequenceId(packed.packets[1].flags), static_cast(2)); + QCOMPARE(packed.packets[0].data.size(), RTCMMavlink::kFragmentLen); + QCOMPARE(packed.packets[1].data.size(), 1); + QCOMPARE(packed.packets[0].data + packed.packets[1].data, data); + QCOMPARE(packed.nextSequenceId, static_cast(3)); +} + +void RTCMMavlinkTest::_testExactMultiple360Terminator() +{ + // 2 * 180: two full fragments plus required zero-length terminator. + const QByteArray data = makePayload(360); + const auto packed = RTCMMavlink::pack(data, 7); + + QCOMPARE(packed.packets.size(), 3); + for (int i = 0; i < 3; ++i) { + QVERIFY(isFragmented(packed.packets[i].flags)); + QCOMPARE(sequenceId(packed.packets[i].flags), static_cast(7)); + QCOMPARE(fragmentId(packed.packets[i].flags), static_cast(i)); + } + QCOMPARE(packed.packets[0].data.size(), 180); + QCOMPARE(packed.packets[1].data.size(), 180); + QCOMPARE(packed.packets[2].data.size(), 0); + QCOMPARE(packed.packets[0].data + packed.packets[1].data, data); +} + +void RTCMMavlinkTest::_testExactMultiple540Terminator() +{ + const QByteArray data = makePayload(540); + const auto packed = RTCMMavlink::pack(data, 1); + + QCOMPARE(packed.packets.size(), 4); // 3 full + terminator + QCOMPARE(fragmentId(packed.packets[3].flags), static_cast(3)); + QCOMPARE(packed.packets[3].data.size(), 0); + QByteArray reassembled; + for (int i = 0; i < 3; ++i) { + reassembled += packed.packets[i].data; + } + QCOMPARE(reassembled, data); +} + +void RTCMMavlinkTest::_testExact720NoTerminator() +{ + // All four full fragments complete by the "all fragments present" rule. + const QByteArray data = makePayload(720); + const auto packed = RTCMMavlink::pack(data, 4); + + QCOMPARE(packed.packets.size(), 4); + for (int i = 0; i < 4; ++i) { + QVERIFY(isFragmented(packed.packets[i].flags)); + QCOMPARE(fragmentId(packed.packets[i].flags), static_cast(i)); + QCOMPARE(packed.packets[i].data.size(), 180); + } +} + +void RTCMMavlinkTest::_testOversizedStreamsUnfragmented() +{ + // >720 cannot use the 4-fragment protocol; stream as unfragmented chunks. + const QByteArray data = makePayload(900); // 5 * 180 + const auto packed = RTCMMavlink::pack(data, 10); + + QCOMPARE(packed.packets.size(), 5); + QCOMPARE(packed.nextSequenceId, static_cast(15)); + + QByteArray reassembled; + for (int i = 0; i < packed.packets.size(); ++i) { + QVERIFY(!isFragmented(packed.packets[i].flags)); + QCOMPARE(sequenceId(packed.packets[i].flags), static_cast(10 + i)); + QCOMPARE(packed.packets[i].data.size(), 180); + reassembled += packed.packets[i].data; + } + QCOMPARE(reassembled, data); + + // Non-multiple oversized: last chunk short, still unfragmented. + const QByteArray odd = makePayload(721); + const auto packedOdd = RTCMMavlink::pack(odd, 0); + QCOMPARE(packedOdd.packets.size(), 5); // 180*4 + 1 + QVERIFY(!isFragmented(packedOdd.packets[4].flags)); + QCOMPARE(packedOdd.packets[4].data.size(), 1); +} + +void RTCMMavlinkTest::_testSequenceAdvances() +{ + uint8_t seq = 30; + auto packed = RTCMMavlink::pack(makePayload(10), seq); + QCOMPARE(packed.nextSequenceId, static_cast(31)); + + packed = RTCMMavlink::pack(makePayload(10), packed.nextSequenceId); + // Sequence field is 5 bits; packing stores (seq & 0x1f) in flags but nextSequenceId + // is free to wrap as uint8_t — only the low 5 bits appear in flags. + QCOMPARE(sequenceId(packed.packets[0].flags), static_cast(31 & 0x1F)); +} + +void RTCMMavlinkTest::_testFlagsBitLayout() +{ + const auto packed = RTCMMavlink::pack(makePayload(200), 0x15); // seq 21 + + QCOMPARE(packed.packets.size(), 2); + // flags: bit0=1, bits1-2=fragId, bits3-7=seq + QCOMPARE(packed.packets[0].flags, static_cast(0x01 | (0 << 1) | (0x15 << 3))); + QCOMPARE(packed.packets[1].flags, static_cast(0x01 | (1 << 1) | (0x15 << 3))); +} + +UT_REGISTER_TEST(RTCMMavlinkTest, TestLabel::Unit) diff --git a/test/GPS/RTCMMavlinkTest.h b/test/GPS/RTCMMavlinkTest.h new file mode 100644 index 000000000000..b31a87346706 --- /dev/null +++ b/test/GPS/RTCMMavlinkTest.h @@ -0,0 +1,20 @@ +#pragma once + +#include "UnitTest.h" + +class RTCMMavlinkTest : public UnitTest +{ + Q_OBJECT + +private slots: + void _testEmpty(); + void _testUnfragmentedSmall(); + void _testUnfragmentedExact180(); + void _testFragmented181(); + void _testExactMultiple360Terminator(); + void _testExactMultiple540Terminator(); + void _testExact720NoTerminator(); + void _testOversizedStreamsUnfragmented(); + void _testSequenceAdvances(); + void _testFlagsBitLayout(); +}; From 2f7c6b416837655391705f83426f77d4b1671c23 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 6 Aug 2026 20:42:43 -0600 Subject: [PATCH 2/3] refactor(GPS): remove unused RTCMParser::extractValidFrames RTCMUdpInput now emits one CRC-valid frame per signal instead of concatenating frames, which left extractValidFrames without callers. Concatenated multi-frame output is exactly the shape that broke GPS_RTCM_DATA sequencing, so drop it rather than leave it around. --- src/GPS/RTCM/RTCMParser.cc | 28 -------------------- src/GPS/RTCM/RTCMParser.h | 5 ---- test/GPS/RTCMParserTest.cc | 52 -------------------------------------- test/GPS/RTCMParserTest.h | 5 ---- 4 files changed, 90 deletions(-) diff --git a/src/GPS/RTCM/RTCMParser.cc b/src/GPS/RTCM/RTCMParser.cc index 44abd3cf4ceb..ca5078c5e293 100644 --- a/src/GPS/RTCM/RTCMParser.cc +++ b/src/GPS/RTCM/RTCMParser.cc @@ -106,31 +106,3 @@ QByteArray RTCMParser::currentFrame() const frame.append(reinterpret_cast(_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(ch))) { - continue; - } - if (validateCrc()) { - out.append(currentFrame()); - ++found; - } else { - ++dropped; - } - reset(); - } - - if (framesFound) { - *framesFound = found; - } - if (framesDropped) { - *framesDropped = dropped; - } - return out; -} diff --git a/src/GPS/RTCM/RTCMParser.h b/src/GPS/RTCM/RTCMParser.h index 5866aec1c9df..e49d3710d04b 100644 --- a/src/GPS/RTCM/RTCMParser.h +++ b/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 { diff --git a/test/GPS/RTCMParserTest.cc b/test/GPS/RTCMParserTest.cc index ade60557bdb5..a7ea0bd0a526 100644 --- a/test/GPS/RTCMParserTest.cc +++ b/test/GPS/RTCMParserTest.cc @@ -533,56 +533,4 @@ void RTCMParserTest::_testParserWhitelistEdgeCases() QVERIFY(!parser.isWhitelisted(1200)); } -void RTCMParserTest::_testExtractValidFramesMultiple() -{ - const QByteArray msg1 = GpsTestHelpers::buildRtcmFrame(1005, 4); - const QByteArray msg2 = GpsTestHelpers::buildRtcmFrame(1077, 8); - const QByteArray stream = msg1 + msg2; - - RTCMParser parser; - int found = 0; - int dropped = 0; - const QByteArray out = parser.extractValidFrames(stream, &found, &dropped); - - QCOMPARE(found, 2); - QCOMPARE(dropped, 0); - QCOMPARE(out, stream); -} - -void RTCMParserTest::_testExtractValidFramesDropsBadCrc() -{ - QByteArray msg1 = GpsTestHelpers::buildRtcmFrame(1005, 4); - QByteArray bad = GpsTestHelpers::buildRtcmFrame(1077, 8); - bad[bad.size() - 1] = static_cast(bad[bad.size() - 1] ^ 0xFF); - const QByteArray msg3 = GpsTestHelpers::buildRtcmFrame(1087, 2); - - RTCMParser parser; - int found = 0; - int dropped = 0; - const QByteArray out = parser.extractValidFrames(msg1 + bad + msg3, &found, &dropped); - - QCOMPARE(found, 2); - QCOMPARE(dropped, 1); - QCOMPARE(out, msg1 + msg3); -} - -void RTCMParserTest::_testExtractValidFramesCrossCallState() -{ - const QByteArray frame = GpsTestHelpers::buildRtcmFrame(1005, 6); - const int split = frame.size() / 2; - - RTCMParser parser; - int found = 0; - int dropped = 0; - - const QByteArray out1 = parser.extractValidFrames(frame.left(split), &found, &dropped); - QVERIFY(out1.isEmpty()); - QCOMPARE(found, 0); - - const QByteArray out2 = parser.extractValidFrames(frame.mid(split), &found, &dropped); - QCOMPARE(found, 1); - QCOMPARE(dropped, 0); - QCOMPARE(out2, frame); -} - UT_REGISTER_TEST(RTCMParserTest, TestLabel::Unit) diff --git a/test/GPS/RTCMParserTest.h b/test/GPS/RTCMParserTest.h index 80512a02af40..7e172286a2ae 100644 --- a/test/GPS/RTCMParserTest.h +++ b/test/GPS/RTCMParserTest.h @@ -35,9 +35,4 @@ private slots: void _testParserPreambleInPayload(); void _testParserTruncatedMidFrame(); void _testParserWhitelistEdgeCases(); - - // extractValidFrames - void _testExtractValidFramesMultiple(); - void _testExtractValidFramesDropsBadCrc(); - void _testExtractValidFramesCrossCallState(); }; From 20a96c16413b397a3d50cc16885879cade29d573 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 6 Aug 2026 20:42:51 -0600 Subject: [PATCH 3/3] test(GPS): cover RTCMUdpInput per-frame emission and pack fragment tail Add RTCMUdpInputTest: raw passthrough with validation off, one signal per CRC-valid frame, bad-CRC frames dropped mid-stream, and parser state carried across split datagrams. RTCMUdpInput now resolves port 0 to the actual bound port so tests can bind ephemerally without racing for a fixed port. Also cover the 541..719 pack() range (four fragments with a non-full tail, no terminator). --- src/GPS/RTCM/RTCMUdpInput.cc | 5 ++ src/GPS/RTCM/RTCMUdpInput.h | 1 + test/GPS/CMakeLists.txt | 3 + test/GPS/RTCMMavlinkTest.cc | 19 +++++++ test/GPS/RTCMMavlinkTest.h | 1 + test/GPS/RTCMUdpInputTest.cc | 104 +++++++++++++++++++++++++++++++++++ test/GPS/RTCMUdpInputTest.h | 15 +++++ 7 files changed, 148 insertions(+) create mode 100644 test/GPS/RTCMUdpInputTest.cc create mode 100644 test/GPS/RTCMUdpInputTest.h diff --git a/src/GPS/RTCM/RTCMUdpInput.cc b/src/GPS/RTCM/RTCMUdpInput.cc index dc4f14bd3796..ca37125c5a58 100644 --- a/src/GPS/RTCM/RTCMUdpInput.cc +++ b/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; diff --git a/src/GPS/RTCM/RTCMUdpInput.h b/src/GPS/RTCM/RTCMUdpInput.h index 78dae16e44e1..f0a477d02cfd 100644 --- a/src/GPS/RTCM/RTCMUdpInput.h +++ b/src/GPS/RTCM/RTCMUdpInput.h @@ -40,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. diff --git a/test/GPS/CMakeLists.txt b/test/GPS/CMakeLists.txt index 0e45f63416b9..319cdd06ce98 100644 --- a/test/GPS/CMakeLists.txt +++ b/test/GPS/CMakeLists.txt @@ -25,12 +25,15 @@ target_sources(${CMAKE_PROJECT_NAME} 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) diff --git a/test/GPS/RTCMMavlinkTest.cc b/test/GPS/RTCMMavlinkTest.cc index d1d9978830f0..96479fe7c039 100644 --- a/test/GPS/RTCMMavlinkTest.cc +++ b/test/GPS/RTCMMavlinkTest.cc @@ -107,6 +107,25 @@ void RTCMMavlinkTest::_testExactMultiple540Terminator() QCOMPARE(reassembled, data); } +void RTCMMavlinkTest::_testFourFragmentsPartialTail() +{ + // 541..719: four fragments with a non-full last fragment — the short tail + // itself marks completion, so no terminator. + const QByteArray data = makePayload(700); // 3 * 180 + 160 + const auto packed = RTCMMavlink::pack(data, 9); + + QCOMPARE(packed.packets.size(), 4); + QByteArray reassembled; + for (int i = 0; i < 4; ++i) { + QVERIFY(isFragmented(packed.packets[i].flags)); + QCOMPARE(fragmentId(packed.packets[i].flags), static_cast(i)); + reassembled += packed.packets[i].data; + } + QCOMPARE(packed.packets[3].data.size(), 160); + QCOMPARE(reassembled, data); + QCOMPARE(packed.nextSequenceId, static_cast(10)); +} + void RTCMMavlinkTest::_testExact720NoTerminator() { // All four full fragments complete by the "all fragments present" rule. diff --git a/test/GPS/RTCMMavlinkTest.h b/test/GPS/RTCMMavlinkTest.h index b31a87346706..4aa816eda6fa 100644 --- a/test/GPS/RTCMMavlinkTest.h +++ b/test/GPS/RTCMMavlinkTest.h @@ -13,6 +13,7 @@ private slots: void _testFragmented181(); void _testExactMultiple360Terminator(); void _testExactMultiple540Terminator(); + void _testFourFragmentsPartialTail(); void _testExact720NoTerminator(); void _testOversizedStreamsUnfragmented(); void _testSequenceAdvances(); diff --git a/test/GPS/RTCMUdpInputTest.cc b/test/GPS/RTCMUdpInputTest.cc new file mode 100644 index 000000000000..a9036d7ffda4 --- /dev/null +++ b/test/GPS/RTCMUdpInputTest.cc @@ -0,0 +1,104 @@ +#include "RTCMUdpInputTest.h" + +#include +#include +#include +#include + +#include "GpsTestHelpers.h" +#include "RTCMUdpInput.h" + +namespace { + +bool sendDatagram(quint16 port, const QByteArray& payload) +{ + QUdpSocket sender; + return sender.writeDatagram(payload, QHostAddress::LocalHost, port) == payload.size(); +} + +} // namespace + +void RTCMUdpInputTest::_testStartStop() +{ + RTCMUdpInput input(0); + QVERIFY(input.start()); + QVERIFY(input.isRunning()); + QVERIFY(input.port() != 0); // ephemeral port resolved on bind + + input.stop(); + QVERIFY(!input.isRunning()); +} + +void RTCMUdpInputTest::_testPassthroughWithoutValidation() +{ + RTCMUdpInput input(0); + QVERIFY(input.start()); + QSignalSpy spy(&input, &RTCMUdpInput::rtcmDataReceived); + + // Validation off (default): datagram forwarded as-is, valid RTCM or not. + const QByteArray payload = QByteArrayLiteral("not-rtcm-at-all"); + QVERIFY(sendDatagram(input.port(), payload)); + + QTRY_COMPARE_WITH_TIMEOUT(spy.count(), 1, 2000); + QCOMPARE(spy.at(0).at(0).toByteArray(), payload); +} + +void RTCMUdpInputTest::_testEmitsOneSignalPerFrame() +{ + RTCMUdpInput input(0); + input.setValidation(true); + QVERIFY(input.start()); + QSignalSpy spy(&input, &RTCMUdpInput::rtcmDataReceived); + + // One datagram carrying two frames plus leading garbage: each frame must be + // emitted separately so RTCMMavlink assigns it its own sequence. + const QByteArray frame1 = GpsTestHelpers::buildRtcmFrame(1005, 4); + const QByteArray frame2 = GpsTestHelpers::buildRtcmFrame(1077, 200); + const QByteArray garbage = QByteArrayLiteral("\x01\x02\x03"); + QVERIFY(sendDatagram(input.port(), garbage + frame1 + frame2)); + + QTRY_COMPARE_WITH_TIMEOUT(spy.count(), 2, 2000); + QCOMPARE(spy.at(0).at(0).toByteArray(), frame1); + QCOMPARE(spy.at(1).at(0).toByteArray(), frame2); +} + +void RTCMUdpInputTest::_testDropsBadCrcFrame() +{ + RTCMUdpInput input(0); + input.setValidation(true); + QVERIFY(input.start()); + QSignalSpy spy(&input, &RTCMUdpInput::rtcmDataReceived); + + const QByteArray frame1 = GpsTestHelpers::buildRtcmFrame(1005, 4); + QByteArray corrupted = GpsTestHelpers::buildRtcmFrame(1077, 8); + corrupted[corrupted.size() - 1] = static_cast(corrupted[corrupted.size() - 1] ^ 0xFF); + const QByteArray frame2 = GpsTestHelpers::buildRtcmFrame(1087, 2); + + expectLogMessage("GPS.RTCMUdpInput", QtWarningMsg, QRegularExpression(QStringLiteral("Dropped 1 RTCM frame"))); + QVERIFY(sendDatagram(input.port(), frame1 + corrupted + frame2)); + + QTRY_COMPARE_WITH_TIMEOUT(spy.count(), 2, 2000); + verifyExpectedLogMessage(); + QCOMPARE(spy.at(0).at(0).toByteArray(), frame1); + QCOMPARE(spy.at(1).at(0).toByteArray(), frame2); +} + +void RTCMUdpInputTest::_testFrameSplitAcrossDatagrams() +{ + RTCMUdpInput input(0); + input.setValidation(true); + QVERIFY(input.start()); + QSignalSpy spy(&input, &RTCMUdpInput::rtcmDataReceived); + + // Parser state must carry across datagrams so a frame split by the sender + // still comes out whole. + const QByteArray frame = GpsTestHelpers::buildRtcmFrame(1005, 6); + const int split = frame.size() / 2; + QVERIFY(sendDatagram(input.port(), frame.left(split))); + QVERIFY(sendDatagram(input.port(), frame.mid(split))); + + QTRY_COMPARE_WITH_TIMEOUT(spy.count(), 1, 2000); + QCOMPARE(spy.at(0).at(0).toByteArray(), frame); +} + +UT_REGISTER_TEST(RTCMUdpInputTest, TestLabel::Unit) diff --git a/test/GPS/RTCMUdpInputTest.h b/test/GPS/RTCMUdpInputTest.h new file mode 100644 index 000000000000..dde047a46846 --- /dev/null +++ b/test/GPS/RTCMUdpInputTest.h @@ -0,0 +1,15 @@ +#pragma once + +#include "UnitTest.h" + +class RTCMUdpInputTest : public UnitTest +{ + Q_OBJECT + +private slots: + void _testStartStop(); + void _testPassthroughWithoutValidation(); + void _testEmitsOneSignalPerFrame(); + void _testDropsBadCrcFrame(); + void _testFrameSplitAcrossDatagrams(); +};