From 653488fc9e92383e1370c64134074d80300f4f43 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Thu, 14 May 2026 11:07:33 -0600 Subject: [PATCH 1/2] Validate RTCM from UDP before forwarding * Add related tests * Expose validation through settings but not in UI because most people will want RTCM validation on Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- src/GPS/NTRIP/NTRIPManager.cc | 4 ++- src/GPS/NTRIP/RTCMParser.h | 2 +- src/GPS/NTRIP/RTCMUdpInput.cc | 40 +++++++++++++++++++++++++-- src/GPS/NTRIP/RTCMUdpInput.h | 17 ++++++++++-- src/Settings/NTRIP.SettingsGroup.json | 8 ++++++ src/Settings/NTRIPSettings.cc | 1 + src/Settings/NTRIPSettings.h | 1 + test/GPS/RTCMParserTest.cc | 37 +++++++++++++++++++++++++ test/GPS/RTCMParserTest.h | 1 + 9 files changed, 103 insertions(+), 8 deletions(-) diff --git a/src/GPS/NTRIP/NTRIPManager.cc b/src/GPS/NTRIP/NTRIPManager.cc index 22d8dc8e37bd..f50dc0a5f021 100644 --- a/src/GPS/NTRIP/NTRIPManager.cc +++ b/src/GPS/NTRIP/NTRIPManager.cc @@ -48,6 +48,7 @@ NTRIPManager::NTRIPManager(QObject* parent) const quint16 port = static_cast( settings->rtcmUdpInputPort()->rawValue().toUInt()); + _rtcmUdpInput = new RTCMUdpInput(port, this); connect(_rtcmUdpInput, &RTCMUdpInput::rtcmDataReceived, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); @@ -55,7 +56,8 @@ NTRIPManager::NTRIPManager(QObject* parent) auto applyUdpInputSettings = [this, settings]() { const quint16 uin_port = static_cast( settings->rtcmUdpInputPort()->rawValue().toUInt()); - _rtcmUdpInput->setPort(uin_port); + _rtcmUdpInput->setPort(uin_port); + _rtcmUdpInput->setValidation(settings->rtcmUdpValidate()->rawValue().toBool()); if (settings->rtcmUdpInputEnabled()->rawValue().toBool()) { _rtcmUdpInput->start(); } else { diff --git a/src/GPS/NTRIP/RTCMParser.h b/src/GPS/NTRIP/RTCMParser.h index 23dd6df05f1e..2a7190fcc3e5 100644 --- a/src/GPS/NTRIP/RTCMParser.h +++ b/src/GPS/NTRIP/RTCMParser.h @@ -16,6 +16,7 @@ class RTCMParser uint16_t messageId() const; const uint8_t* crcBytes() const { return _crcBytes; } static constexpr int kCrcSize = 3; + static constexpr int kHeaderSize = 3; bool validateCrc() const; static uint32_t crc24q(const uint8_t* data, size_t len); @@ -29,7 +30,6 @@ class RTCMParser }; static constexpr uint16_t kMaxPayloadLength = 1023; - static constexpr int kHeaderSize = 3; State _state; uint8_t _buffer[kHeaderSize + kMaxPayloadLength]; diff --git a/src/GPS/NTRIP/RTCMUdpInput.cc b/src/GPS/NTRIP/RTCMUdpInput.cc index 662dea89a2c4..334e5bfe39b2 100644 --- a/src/GPS/NTRIP/RTCMUdpInput.cc +++ b/src/GPS/NTRIP/RTCMUdpInput.cc @@ -1,6 +1,7 @@ #include "RTCMUdpInput.h" #include "QGCLoggingCategory.h" + QGC_LOGGING_CATEGORY(RTCMUdpInputLog, "GPS.RTCMUdpInput") RTCMUdpInput::RTCMUdpInput(quint16 port, QObject *parent) @@ -18,6 +19,7 @@ RTCMUdpInput::~RTCMUdpInput() bool RTCMUdpInput::start() { stop(); + _rtcmParser.reset(); if (!_socket.bind(QHostAddress::AnyIPv4, _port)) { qCWarning(RTCMUdpInputLog) << "Failed to bind UDP socket on port" << _port @@ -62,7 +64,7 @@ void RTCMUdpInput::_readDatagrams() while (_socket.hasPendingDatagrams()) { const qint64 size = _socket.pendingDatagramSize(); if (size <= 0) { - (void) _socket.readDatagram(nullptr, 0); // discard malformed + (void) _socket.readDatagram(nullptr, 0); continue; } @@ -77,7 +79,39 @@ void RTCMUdpInput::_readDatagrams() data.resize(static_cast(read)); } - qCDebug(RTCMUdpInputLog) << "Received RTCM datagram:" << read << "bytes"; - emit rtcmDataReceived(data); + if (!_validateRtcm) { + qCDebug(RTCMUdpInputLog) << "Received RTCM datagram:" << read << "bytes"; + emit rtcmDataReceived(data); + return; + } + + // RTCM Validation keeps track of some stats to see what % of the stream is garbage. + + for (qsizetype i = 0; i < data.size(); ++i) { + if (!_rtcmParser.addByte(static_cast(data[i]))) { + continue; + } + + if (_rtcmParser.validateCrc()) { + const uint16_t frameSize = RTCMParser::kHeaderSize + _rtcmParser.messageLength() + RTCMParser::kCrcSize; + QByteArray frame(reinterpret_cast(_rtcmParser.message()), frameSize); + _validBytes += frameSize; + qCDebug(RTCMUdpInputLog) << "RTCM message" << _rtcmParser.messageId() << frameSize << "bytes"; + emit rtcmDataReceived(frame); + } else { + const uint16_t frameSize = RTCMParser::kHeaderSize + _rtcmParser.messageLength() + RTCMParser::kCrcSize; + qCWarning(RTCMUdpInputLog) << "Dropped RTCM message" << _rtcmParser.messageId() << "- CRC mismatch"; + _invalidBytes += frameSize; + } + + _rtcmParser.reset(); + } + + const quint64 totalBytes = _validBytes + _invalidBytes; + if (totalBytes > 0 && (totalBytes % 100000) < static_cast(data.size())) { + const double dropPct = 100.0 * _invalidBytes / totalBytes; + qCDebug(RTCMUdpInputLog) << QString("RTCM byte stats: %1 valid, %2 invalid, %3% dropped") + .arg(_validBytes).arg(_invalidBytes).arg(dropPct, 0, 'f', 1); + } } } diff --git a/src/GPS/NTRIP/RTCMUdpInput.h b/src/GPS/NTRIP/RTCMUdpInput.h index 23478f6cc7c2..ef2b3d73b5b4 100644 --- a/src/GPS/NTRIP/RTCMUdpInput.h +++ b/src/GPS/NTRIP/RTCMUdpInput.h @@ -4,9 +4,12 @@ #include #include #include +#include "RTCMParser.h" Q_DECLARE_LOGGING_CATEGORY(RTCMUdpInputLog) +class RTCMParser; + /** * @brief Listens on a UDP port for raw RTCM3 correction data and emits it * for forwarding to connected vehicles via RTCMMavlink::RTCMDataUpdate(). @@ -41,12 +44,16 @@ class RTCMUdpInput : public QObject /// Unbind the socket and stop accepting datagrams. void stop(); - bool isRunning() const { return _running; } + bool isRunning() const { return _running; } quint16 port() const { return _port; } /// Change the listen port. If already running, restarts automatically. void setPort(quint16 port); + /// Enable/disable validation of RTCM data. + /// With this enabled, only valid RTCM packets are converted to MAVLink. + void setValidation(const bool validate) { _validateRtcm = validate; } + signals: /// Emitted once per received datagram with the raw RTCM payload. /// Connect directly to RTCMMavlink::RTCMDataUpdate (same thread). @@ -60,6 +67,10 @@ private slots: private: QUdpSocket _socket; - quint16 _port; - bool _running = false; + quint16 _port; + bool _running = false; + bool _validateRtcm = false; + RTCMParser _rtcmParser; + quint64 _validBytes = 0; + quint64 _invalidBytes = 0; }; diff --git a/src/Settings/NTRIP.SettingsGroup.json b/src/Settings/NTRIP.SettingsGroup.json index 445d9e774224..d3c1353dc1ff 100644 --- a/src/Settings/NTRIP.SettingsGroup.json +++ b/src/Settings/NTRIP.SettingsGroup.json @@ -108,6 +108,14 @@ "max": 65535, "decimalPlaces": 0, "label": "UDP RTCM input port" + }, + { + "name": "rtcmUdpValidate", + "shortDesc": "UDP RTCM enable validation", + "longDesc": "Enable validation of incoming data as RTCM and drop garbage (improves security).", + "type": "bool", + "default": true, + "label": "UDP RTCM enable validation" } ] } diff --git a/src/Settings/NTRIPSettings.cc b/src/Settings/NTRIPSettings.cc index f3d37bb475c7..b1fc1f95f162 100644 --- a/src/Settings/NTRIPSettings.cc +++ b/src/Settings/NTRIPSettings.cc @@ -15,3 +15,4 @@ DECLARE_SETTINGSFACT(NTRIPSettings, ntripUdpTargetAddress) DECLARE_SETTINGSFACT(NTRIPSettings, ntripUdpTargetPort) DECLARE_SETTINGSFACT(NTRIPSettings, rtcmUdpInputEnabled) DECLARE_SETTINGSFACT(NTRIPSettings, rtcmUdpInputPort) +DECLARE_SETTINGSFACT(NTRIPSettings, rtcmUdpValidate) diff --git a/src/Settings/NTRIPSettings.h b/src/Settings/NTRIPSettings.h index b0741f33b680..838af1447108 100644 --- a/src/Settings/NTRIPSettings.h +++ b/src/Settings/NTRIPSettings.h @@ -24,4 +24,5 @@ class NTRIPSettings : public SettingsGroup DEFINE_SETTINGFACT(ntripUdpTargetPort) DEFINE_SETTINGFACT(rtcmUdpInputEnabled) DEFINE_SETTINGFACT(rtcmUdpInputPort) + DEFINE_SETTINGFACT(rtcmUdpValidate) }; diff --git a/test/GPS/RTCMParserTest.cc b/test/GPS/RTCMParserTest.cc index 9406d10fbb8c..74177e2f9fa3 100644 --- a/test/GPS/RTCMParserTest.cc +++ b/test/GPS/RTCMParserTest.cc @@ -322,4 +322,41 @@ void RTCMParserTest::_testParserCorruptedPreamble() QVERIFY(parser.validateCrc()); } +void RTCMParserTest::_testParserRecoveryAfterBadCrc() +{ + QByteArray msg1 = GpsTestHelpers::buildRtcmFrame(1005, 4); + QByteArray msg2 = GpsTestHelpers::buildRtcmFrame(1077, 8); + QByteArray msg3 = GpsTestHelpers::buildRtcmFrame(1087, 2); + + // Corrupt the CRC of the middle frame + msg2[msg2.size() - 1] = static_cast(msg2[msg2.size() - 1] ^ 0xFF); + + QByteArray stream = msg1 + msg2 + msg3; + + RTCMParser parser; + int validCount = 0; + int invalidCount = 0; + QVector validIds; + + for (int i = 0; i < stream.size(); i++) { + if (!parser.addByte(static_cast(stream[i]))) { + continue; + } + + if (parser.validateCrc()) { + validIds.append(parser.messageId()); + validCount++; + } else { + invalidCount++; + } + + parser.reset(); + } + + QCOMPARE(validCount, 2); + QCOMPARE(invalidCount, 1); + QCOMPARE(validIds[0], static_cast(1005)); + QCOMPARE(validIds[1], static_cast(1087)); +} + UT_REGISTER_TEST(RTCMParserTest, TestLabel::Unit) diff --git a/test/GPS/RTCMParserTest.h b/test/GPS/RTCMParserTest.h index 640c1f45a64c..7a740aa2fc0a 100644 --- a/test/GPS/RTCMParserTest.h +++ b/test/GPS/RTCMParserTest.h @@ -27,4 +27,5 @@ private slots: void _testParserMaxLength(); void _testParserTruncatedFrame(); void _testParserCorruptedPreamble(); + void _testParserRecoveryAfterBadCrc(); }; From 43ea3d752248cf8fdaf2f625c3d2f07625fa64ad Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Thu, 14 May 2026 16:30:04 -0600 Subject: [PATCH 2/2] Fix crc garbage and dropped unvalidated data Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- src/GPS/NTRIP/NTRIPManager.cc | 1 - src/GPS/NTRIP/RTCMUdpInput.cc | 26 +++++++++++++++++++------- src/GPS/NTRIP/RTCMUdpInput.h | 2 -- 3 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/GPS/NTRIP/NTRIPManager.cc b/src/GPS/NTRIP/NTRIPManager.cc index f50dc0a5f021..abf4fca11728 100644 --- a/src/GPS/NTRIP/NTRIPManager.cc +++ b/src/GPS/NTRIP/NTRIPManager.cc @@ -48,7 +48,6 @@ NTRIPManager::NTRIPManager(QObject* parent) const quint16 port = static_cast( settings->rtcmUdpInputPort()->rawValue().toUInt()); - _rtcmUdpInput = new RTCMUdpInput(port, this); connect(_rtcmUdpInput, &RTCMUdpInput::rtcmDataReceived, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); diff --git a/src/GPS/NTRIP/RTCMUdpInput.cc b/src/GPS/NTRIP/RTCMUdpInput.cc index 334e5bfe39b2..4d5a352691e0 100644 --- a/src/GPS/NTRIP/RTCMUdpInput.cc +++ b/src/GPS/NTRIP/RTCMUdpInput.cc @@ -1,7 +1,6 @@ #include "RTCMUdpInput.h" #include "QGCLoggingCategory.h" - QGC_LOGGING_CATEGORY(RTCMUdpInputLog, "GPS.RTCMUdpInput") RTCMUdpInput::RTCMUdpInput(quint16 port, QObject *parent) @@ -64,7 +63,7 @@ void RTCMUdpInput::_readDatagrams() while (_socket.hasPendingDatagrams()) { const qint64 size = _socket.pendingDatagramSize(); if (size <= 0) { - (void) _socket.readDatagram(nullptr, 0); + (void) _socket.readDatagram(nullptr, 0); // discard malformed continue; } @@ -82,10 +81,12 @@ void RTCMUdpInput::_readDatagrams() if (!_validateRtcm) { qCDebug(RTCMUdpInputLog) << "Received RTCM datagram:" << read << "bytes"; emit rtcmDataReceived(data); - return; + continue; } - // RTCM Validation keeps track of some stats to see what % of the stream is garbage. + QByteArray validData; + int framesFound = 0; + int framesDropped = 0; for (qsizetype i = 0; i < data.size(); ++i) { if (!_rtcmParser.addByte(static_cast(data[i]))) { @@ -94,21 +95,32 @@ void RTCMUdpInput::_readDatagrams() if (_rtcmParser.validateCrc()) { const uint16_t frameSize = RTCMParser::kHeaderSize + _rtcmParser.messageLength() + RTCMParser::kCrcSize; - QByteArray frame(reinterpret_cast(_rtcmParser.message()), frameSize); + validData.append(reinterpret_cast(_rtcmParser.message()), RTCMParser::kHeaderSize + _rtcmParser.messageLength()); + validData.append(reinterpret_cast(_rtcmParser.crcBytes()), RTCMParser::kCrcSize); _validBytes += frameSize; + framesFound++; qCDebug(RTCMUdpInputLog) << "RTCM message" << _rtcmParser.messageId() << frameSize << "bytes"; - emit rtcmDataReceived(frame); } else { const uint16_t frameSize = RTCMParser::kHeaderSize + _rtcmParser.messageLength() + RTCMParser::kCrcSize; qCWarning(RTCMUdpInputLog) << "Dropped RTCM message" << _rtcmParser.messageId() << "- CRC mismatch"; _invalidBytes += frameSize; + framesDropped++; } _rtcmParser.reset(); } + qCDebug(RTCMUdpInputLog) << "Datagram" << read << "bytes -" + << "framesFound:" << framesFound + << "framesDropped:" << framesDropped + << "validData:" << validData.size() << "bytes"; + + if (!validData.isEmpty()) { + emit rtcmDataReceived(validData); + } + const quint64 totalBytes = _validBytes + _invalidBytes; - if (totalBytes > 0 && (totalBytes % 100000) < static_cast(data.size())) { + if (totalBytes > 0) { const double dropPct = 100.0 * _invalidBytes / totalBytes; qCDebug(RTCMUdpInputLog) << QString("RTCM byte stats: %1 valid, %2 invalid, %3% dropped") .arg(_validBytes).arg(_invalidBytes).arg(dropPct, 0, 'f', 1); diff --git a/src/GPS/NTRIP/RTCMUdpInput.h b/src/GPS/NTRIP/RTCMUdpInput.h index ef2b3d73b5b4..74e66e98c9c5 100644 --- a/src/GPS/NTRIP/RTCMUdpInput.h +++ b/src/GPS/NTRIP/RTCMUdpInput.h @@ -8,8 +8,6 @@ Q_DECLARE_LOGGING_CATEGORY(RTCMUdpInputLog) -class RTCMParser; - /** * @brief Listens on a UDP port for raw RTCM3 correction data and emits it * for forwarding to connected vehicles via RTCMMavlink::RTCMDataUpdate().