diff --git a/src/GPS/NTRIP/NTRIPManager.cc b/src/GPS/NTRIP/NTRIPManager.cc index 22d8dc8e37bd..abf4fca11728 100644 --- a/src/GPS/NTRIP/NTRIPManager.cc +++ b/src/GPS/NTRIP/NTRIPManager.cc @@ -55,7 +55,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..4d5a352691e0 100644 --- a/src/GPS/NTRIP/RTCMUdpInput.cc +++ b/src/GPS/NTRIP/RTCMUdpInput.cc @@ -18,6 +18,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 +63,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); // discard malformed continue; } @@ -77,7 +78,52 @@ 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); + continue; + } + + QByteArray validData; + int framesFound = 0; + int framesDropped = 0; + + 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; + 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"; + } 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) { + 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..74e66e98c9c5 100644 --- a/src/GPS/NTRIP/RTCMUdpInput.h +++ b/src/GPS/NTRIP/RTCMUdpInput.h @@ -4,6 +4,7 @@ #include #include #include +#include "RTCMParser.h" Q_DECLARE_LOGGING_CATEGORY(RTCMUdpInputLog) @@ -41,12 +42,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 +65,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(); };