From 42b9971c6b70a57667f6a97c3cea5df4e128ff36 Mon Sep 17 00:00:00 2001 From: WTPENGUIN Date: Thu, 22 Dec 2022 16:27:11 +0900 Subject: [PATCH 1/7] Add NTRIP with VRS support --- qgroundcontrol.pro | 13 +- qgroundcontrol.qrc | 1 + src/CMakeLists.txt | 2 + src/GPS/RTCM/RTCMMavlink.cc | 2 +- src/NTRIP/CMakeLists.txt | 14 + src/NTRIP/NTRIP.cc | 286 +++++++++++++++++++++ src/NTRIP/NTRIP.h | 101 ++++++++ src/QGCLoggingCategory.cc | 1 + src/QGCLoggingCategory.h | 1 + src/QGCToolbox.cc | 3 + src/QGCToolbox.h | 3 + src/QmlControls/QGroundControlQmlGlobal.cc | 1 + src/QmlControls/QGroundControlQmlGlobal.h | 3 + src/Settings/CMakeLists.txt | 2 + src/Settings/NTRIP.SettingsGroup.json | 67 +++++ src/Settings/NTRIPSettings.cc | 27 ++ src/Settings/NTRIPSettings.h | 29 +++ src/Settings/SettingsManager.cc | 4 +- src/Settings/SettingsManager.h | 4 + src/ui/preferences/GeneralSettings.qml | 99 +++++++ 20 files changed, 657 insertions(+), 6 deletions(-) create mode 100644 src/NTRIP/CMakeLists.txt create mode 100644 src/NTRIP/NTRIP.cc create mode 100644 src/NTRIP/NTRIP.h create mode 100644 src/Settings/NTRIP.SettingsGroup.json create mode 100644 src/Settings/NTRIPSettings.cc create mode 100644 src/Settings/NTRIPSettings.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bfbfa444c77..0a9c8318caf 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -400,6 +400,7 @@ INCLUDEPATH += \ src/Joystick \ src/PlanView \ src/MissionManager \ + src/NTRIP \ src/PositionManager \ src/QmlControls \ src/QtLocationPlugin \ @@ -589,6 +590,8 @@ HEADERS += \ src/Compression/QGCZlib.h \ src/FirmwarePlugin/PX4/px4_custom_mode.h \ src/FollowMe/FollowMe.h \ + src/GPS/Drivers/src/rtcm.h \ + src/GPS/RTCM/RTCMMavlink.h \ src/Joystick/Joystick.h \ src/Joystick/JoystickManager.h \ src/Joystick/JoystickMavCommand.h \ @@ -638,6 +641,7 @@ HEADERS += \ src/MissionManager/TransectStyleComplexItem.h \ src/MissionManager/VisualMissionItem.h \ src/MissionManager/VTOLLandingComplexItem.h \ + src/NTRIP/NTRIP.h \ src/PositionManager/PositionManager.h \ src/PositionManager/SimulatedPosition.h \ src/Geo/QGCGeo.h \ @@ -688,6 +692,7 @@ HEADERS += \ src/Settings/FirmwareUpgradeSettings.h \ src/Settings/FlightMapSettings.h \ src/Settings/FlyViewSettings.h \ + src/Settings/NTRIPSettings.h \ src/Settings/OfflineMapsSettings.h \ src/Settings/PlanViewSettings.h \ src/Settings/RTKSettings.h \ @@ -811,14 +816,12 @@ HEADERS += \ !MobileBuild { HEADERS += \ src/GPS/Drivers/src/gps_helper.h \ - src/GPS/Drivers/src/rtcm.h \ src/GPS/Drivers/src/ashtech.h \ src/GPS/Drivers/src/ubx.h \ src/GPS/Drivers/src/sbf.h \ src/GPS/GPSManager.h \ src/GPS/GPSPositionMessage.h \ src/GPS/GPSProvider.h \ - src/GPS/RTCM/RTCMMavlink.h \ src/GPS/definitions.h \ src/GPS/satellite_info.h \ src/GPS/sensor_gps.h \ @@ -853,6 +856,8 @@ SOURCES += \ src/Compression/QGCLZMA.cc \ src/Compression/QGCZlib.cc \ src/FollowMe/FollowMe.cc \ + src/GPS/Drivers/src/rtcm.cpp \ + src/GPS/RTCM/RTCMMavlink.cc \ src/Joystick/Joystick.cc \ src/Joystick/JoystickManager.cc \ src/Joystick/JoystickMavCommand.cc \ @@ -901,6 +906,7 @@ SOURCES += \ src/MissionManager/TransectStyleComplexItem.cc \ src/MissionManager/VisualMissionItem.cc \ src/MissionManager/VTOLLandingComplexItem.cc \ + src/NTRIP/NTRIP.cc \ src/PositionManager/PositionManager.cpp \ src/PositionManager/SimulatedPosition.cc \ src/Geo/QGCGeo.cc \ @@ -948,6 +954,7 @@ SOURCES += \ src/Settings/RemoteIDSettings.cc \ src/Settings/FirmwareUpgradeSettings.cc \ src/Settings/FlightMapSettings.cc \ + src/Settings/NTRIPSettings.cc \ src/Settings/FlyViewSettings.cc \ src/Settings/OfflineMapsSettings.cc \ src/Settings/PlanViewSettings.cc \ @@ -1060,13 +1067,11 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { !MobileBuild { SOURCES += \ src/GPS/Drivers/src/gps_helper.cpp \ - src/GPS/Drivers/src/rtcm.cpp \ src/GPS/Drivers/src/ashtech.cpp \ src/GPS/Drivers/src/ubx.cpp \ src/GPS/Drivers/src/sbf.cpp \ src/GPS/GPSManager.cc \ src/GPS/GPSProvider.cc \ - src/GPS/RTCM/RTCMMavlink.cc \ src/Joystick/JoystickSDL.cc \ src/RunGuard.cc \ } diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 0907af417ee..975306f154b 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -319,6 +319,7 @@ src/MissionManager/MavCmdInfoSub.json src/MissionManager/MavCmdInfoVTOL.json src/MissionManager/MissionSettings.FactMetaData.json + src/Settings/NTRIP.SettingsGroup.json src/Settings/OfflineMaps.SettingsGroup.json src/Settings/PlanView.SettingsGroup.json src/MissionManager/QGCMapCircle.Facts.json diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 25eb9e466cb..83d60ce443a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -140,6 +140,7 @@ if (${QGC_GST_MICROHARD_ENABLED}) add_subdirectory(Microhard) endif () add_subdirectory(MissionManager) +add_subdirectory(NTRIP) add_subdirectory(PlanView) add_subdirectory(PositionManager) add_subdirectory(QmlControls) @@ -181,6 +182,7 @@ target_link_libraries(qgc gps Joystick MissionManager + NTRIP PositionManager QmlControls QtLocationPlugin diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index 35113793c61..b06d2dbf212 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -23,7 +23,7 @@ RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) void RTCMMavlink::RTCMDataUpdate(QByteArray message) { - /* statistics */ + qCDebug(NTRIPLog) << "RTCMDataUpdate: " << message.size() << " bytes"; /* statistics */ _bandwidthByteCounter += message.size(); qint64 elapsed = _bandwidthTimer.elapsed(); if (elapsed > 1000) { diff --git a/src/NTRIP/CMakeLists.txt b/src/NTRIP/CMakeLists.txt new file mode 100644 index 00000000000..88bab3dd00a --- /dev/null +++ b/src/NTRIP/CMakeLists.txt @@ -0,0 +1,14 @@ +add_library(NTRIP + NTRIP.cc + NTRIP.h +) + +target_link_libraries(NTRIP + PUBLIC + qgc +) + +target_include_directories(NTRIP + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/src/NTRIP/NTRIP.cc b/src/NTRIP/NTRIP.cc new file mode 100644 index 00000000000..6207b61e07d --- /dev/null +++ b/src/NTRIP/NTRIP.cc @@ -0,0 +1,286 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "NTRIP.h" +#include "QGCLoggingCategory.h" +#include "QGCToolbox.h" +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "PositionManager.h" +#include "NTRIPSettings.h" + +#include + +NTRIP::NTRIP(QGCApplication* app, QGCToolbox* toolbox) + : QGCTool(app, toolbox) +{ +} + +void NTRIP::setToolbox(QGCToolbox* toolbox) +{ + QGCTool::setToolbox(toolbox); + + NTRIPSettings* settings = qgcApp()->toolbox()->settingsManager()->ntripSettings(); + if (settings->ntripServerConnectEnabled()->rawValue().toBool()) { + qCDebug(NTRIPLog) << settings->ntripEnableVRS()->rawValue().toBool(); + _rtcmMavlink = new RTCMMavlink(*toolbox); + + _tcpLink = new NTRIPTCPLink(settings->ntripServerHostAddress()->rawValue().toString(), + settings->ntripServerPort()->rawValue().toInt(), + settings->ntripUsername()->rawValue().toString(), + settings->ntripPassword()->rawValue().toString(), + settings->ntripMountpoint()->rawValue().toString(), + settings->ntripWhitelist()->rawValue().toString(), + settings->ntripEnableVRS()->rawValue().toBool()); + connect(_tcpLink, &NTRIPTCPLink::error, this, &NTRIP::_tcpError, Qt::QueuedConnection); + connect(_tcpLink, &NTRIPTCPLink::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); + } else { + qCDebug(NTRIPLog) << "NTRIP Server is not enabled"; + } +} + + +void NTRIP::_tcpError(const QString errorMsg) +{ + qgcApp()->showAppMessage(tr("NTRIP Server Error: %1").arg(errorMsg)); +} + + +NTRIPTCPLink::NTRIPTCPLink(const QString& hostAddress, + int port, + const QString &username, + const QString &password, + const QString &mountpoint, + const QString &whitelist, + const bool &enableVRS) + : QThread () + , _hostAddress (hostAddress) + , _port (port) + , _username (username) + , _password (password) + , _mountpoint (mountpoint) + , _isVRSEnable (enableVRS) + , _toolbox (qgcApp()->toolbox()) +{ + for(const auto& msg: whitelist.split(',')) { + int msg_int = msg.toInt(); + if(msg_int) { + _whitelist.insert(msg_int); + } + } + qCDebug(NTRIPLog) << "whitelist: " << _whitelist; + if (!_rtcm_parsing) { + _rtcm_parsing = new RTCMParsing(); + } + _rtcm_parsing->reset(); + _state = NTRIPState::uninitialised; + + // Start TCP Socket + moveToThread(this); + start(); +} + +NTRIPTCPLink::~NTRIPTCPLink(void) +{ + qCDebug(NTRIPLog) << "NTRIP Thread stopped"; + if (_socket) { + if(_isVRSEnable) { + _vrsSendTimer->stop(); + QObject::disconnect(_vrsSendTimer, &QTimer::timeout, this, &NTRIPTCPLink::_sendNMEA); + delete _vrsSendTimer; + _vrsSendTimer = nullptr; + } + QObject::disconnect(_socket, &QTcpSocket::readyRead, this, &NTRIPTCPLink::_readBytes); + _socket->disconnectFromHost(); + _socket->deleteLater(); + _socket = nullptr; + + // Delete Rtcm Parsing instance + delete(_rtcm_parsing); + _rtcm_parsing = nullptr; + } + quit(); + wait(); +} + +void NTRIPTCPLink::run(void) +{ + qCDebug(NTRIPLog) << "NTRIP Thread started"; + _hardwareConnect(); + + // Init VRS Timer + if(_isVRSEnable) { + _vrsSendTimer = new QTimer(); + _vrsSendTimer->setInterval(_vrsSendRateMSecs); + _vrsSendTimer->setSingleShot(false); + QObject::connect(_vrsSendTimer, &QTimer::timeout, this, &NTRIPTCPLink::_sendNMEA); + _vrsSendTimer->start(); + } + + exec(); +} + +void NTRIPTCPLink::_hardwareConnect() +{ + qCDebug(NTRIPLog) << "Connecting to NTRIP Server: " << _hostAddress << ":" << _port; + _socket = new QTcpSocket(); + QObject::connect(_socket, &QTcpSocket::readyRead, this, &NTRIPTCPLink::_readBytes); + _socket->connectToHost(_hostAddress, static_cast(_port)); + + // Give the socket a second to connect to the other side otherwise error out + if (!_socket->waitForConnected(1000)) { + qCDebug(NTRIPLog) << "NTRIP Socket failed to connect"; + emit error(_socket->errorString()); + delete _socket; + _socket = nullptr; + return; + } + + // If mountpoint is specified, send an http get request for data + if ( !_mountpoint.isEmpty()) { + qCDebug(NTRIPLog) << "Sending HTTP request, using mount point: " << _mountpoint; + QString auth = QString(_username + ":" + _password).toUtf8().toBase64(); + QString query = "GET /%1 HTTP/1.0\r\nUser-Agent: NTRIP\r\nAuthorization: Basic %2\r\n\r\n"; + _socket->write(query.arg(_mountpoint).arg(auth).toUtf8()); + _state = NTRIPState::waiting_for_http_response; + } else { + // If no mountpoint is set, assume we will just get data from the tcp stream + _state = NTRIPState::waiting_for_rtcm_header; + } + + qCDebug(NTRIPLog) << "NTRIP Socket connected"; +} + +void NTRIPTCPLink::_parse(const QByteArray &buffer) +{ + qCDebug(NTRIPLog) << "Parsing " << buffer.size() << " bytes"; for(const uint8_t byte : buffer) { + if(_state == NTRIPState::waiting_for_rtcm_header) { + if(byte != RTCM3_PREAMBLE) + continue; + _state = NTRIPState::accumulating_rtcm_packet; + } + if(_rtcm_parsing->addByte(byte)) { + _state = NTRIPState::waiting_for_rtcm_header; + QByteArray message((char*)_rtcm_parsing->message(), static_cast(_rtcm_parsing->messageLength())); + //TODO: Restore the following when upstreamed in Driver repo + //uint16_t id = _rtcm_parsing->messageId(); + uint16_t id = ((uint8_t)message[3] << 4) | ((uint8_t)message[4] >> 4); + if(_whitelist.empty() || _whitelist.contains(id)) { + emit RTCMDataUpdate(message); + qCDebug(NTRIPLog) << "Sending " << id << "of size " << message.length(); + } else { + qCDebug(NTRIPLog) << "Ignoring " << id; + } + _rtcm_parsing->reset(); + } + } +} + +void NTRIPTCPLink::_readBytes(void) +{ + qCDebug(NTRIPLog) << "Reading bytes"; + if (!_socket) { + return; + } + if(_state == NTRIPState::waiting_for_http_response) { + QString line = _socket->readLine(); + if (line.contains("200")){ + qCDebug(NTRIPLog) << "Server responded with " << line; + _state = NTRIPState::waiting_for_rtcm_header; + } else if (line.contains("401")) { + qCWarning(NTRIPLog) << "Server responded with " << line; + emit error("NTRIP Server responded with 401 Unauthorized"); + _state = NTRIPState::uninitialised; + } else { + qCWarning(NTRIPLog) << "Server responded with " << line; + // TODO: Handle failure. Reconnect? + // Just move into parsing mode and hope for now. + _state = NTRIPState::waiting_for_rtcm_header; + } + } + QByteArray bytes = _socket->readAll(); + _parse(bytes); +} + +void NTRIPTCPLink::_sendNMEA() { + qCDebug(NTRIPLog) << "Sending NMEA"; + QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition(); + + if(!gcsPosition.isValid()) { + qCDebug(NTRIPLog) << "GCS Position is not valid"; + return; + } + + double lat = gcsPosition.latitude(); + double lng = gcsPosition.longitude(); + double alt = gcsPosition.altitude(); + + qCDebug(NTRIPLog) << "lat : " << lat << " lon : " << lng << " alt : " << alt; + + QString time = QDateTime::currentDateTimeUtc().toString("hhmmss.zzz"); + + if(lat != 0 || lng != 0) { + double latdms = (int) lat + (lat - (int) lat) * .6f; + double lngdms = (int) lng + (lng - (int) lng) * .6f; + if(isnan(alt)) alt = 0.0; + + QString line = QString("$GP%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14,%15") + .arg("GGA", time, + QString::number(qFabs(latdms * 100), 'f', 2), lat < 0 ? "S" : "N", + QString::number(qFabs(lngdms * 100), 'f', 2), lng < 0 ? "W" : "E", + "1", "10", "1", + QString::number(alt, 'f', 2), + "M", "0", "M", "0.0", "0"); + + // Calculrate checksum and send message + QString checkSum = _getCheckSum(line); + QString* nmeaMessage = new QString(line + "*" + checkSum + "\r\n"); + + // Write nmea message + if(_socket) { + _socket->write(nmeaMessage->toUtf8()); + } + + qCDebug(NTRIPLog) << "NMEA Message : " << nmeaMessage->toUtf8(); + } +} + +QString NTRIPTCPLink::_getCheckSum(QString line) { + qCDebug(NTRIPLog) << "Calculating checksum"; + QByteArray temp_Byte = line.toUtf8(); + const char* buf = temp_Byte.constData(); + + char character; + int checksum = 0; + + for(int i = 0; i < line.length(); i++) { + character = buf[i]; + switch(character) { + case '$': + // Ignore the dollar sign + break; + case '*': + // Stop processing before the asterisk + i = line.length(); + continue; + default: + // First value for the checksum + if(checksum == 0) { + // Set the checksum to the value + checksum = character; + } + else { + // XOR the checksum with this character's value + checksum = checksum ^ character; + } + } + } + + return QString("%1").arg(checksum, 0, 16); +} diff --git a/src/NTRIP/NTRIP.h b/src/NTRIP/NTRIP.h new file mode 100644 index 00000000000..13a94646df4 --- /dev/null +++ b/src/NTRIP/NTRIP.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "Drivers/src/rtcm.h" +#include "RTCM/RTCMMavlink.h" + +class NTRIPSettings; + +class NTRIPTCPLink : public QThread +{ + Q_OBJECT + +public: + NTRIPTCPLink(const QString& hostAddress, + int port, + const QString& username, + const QString& password, + const QString& mountpoint, + const QString& whitelist, + const bool& enableVRS); + ~NTRIPTCPLink(); + +signals: + void error(const QString errorMsg); + void RTCMDataUpdate(QByteArray message); + +protected: + void run() final; + +private slots: + void _readBytes(); + +private: + enum class NTRIPState { + uninitialised, + waiting_for_http_response, + waiting_for_rtcm_header, + accumulating_rtcm_packet, + }; + + void _hardwareConnect(void); + void _parse(const QByteArray &buffer); + + QTcpSocket* _socket = nullptr; + + QString _hostAddress; + int _port; + QString _username; + QString _password; + QString _mountpoint; + QSet _whitelist; + bool _isVRSEnable; + + // QUrl + QUrl _ntripURL; + + // Send NMEA + void _sendNMEA(); + QString _getCheckSum(QString line); + + // VRS Timer + QTimer* _vrsSendTimer; + static const int _vrsSendRateMSecs = 3000; + + RTCMParsing *_rtcm_parsing{nullptr}; + NTRIPState _state; + + QGCToolbox* _toolbox = nullptr; +}; + +class NTRIP : public QGCTool { + Q_OBJECT + +public: + NTRIP(QGCApplication* app, QGCToolbox* toolbox); + + // QGCTool overrides + void setToolbox(QGCToolbox* toolbox) final; + +public slots: + void _tcpError (const QString errorMsg); + +private slots: + +private: + NTRIPTCPLink* _tcpLink = nullptr; + RTCMMavlink* _rtcmMavlink = nullptr; +}; diff --git a/src/QGCLoggingCategory.cc b/src/QGCLoggingCategory.cc index 791de78414e..35dbe50e307 100644 --- a/src/QGCLoggingCategory.cc +++ b/src/QGCLoggingCategory.cc @@ -27,6 +27,7 @@ QGC_LOGGING_CATEGORY(GeotaggingLog, "GeotaggingLog") QGC_LOGGING_CATEGORY(RTKGPSLog, "RTKGPSLog") QGC_LOGGING_CATEGORY(GuidedActionsControllerLog, "GuidedActionsControllerLog") QGC_LOGGING_CATEGORY(ADSBVehicleManagerLog, "ADSBVehicleManagerLog") +QGC_LOGGING_CATEGORY(NTRIPLog, "NTRIPLog") QGC_LOGGING_CATEGORY(LocalizationLog, "LocalizationLog") QGC_LOGGING_CATEGORY(VideoAllLog, kVideoAllLogCategory) QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") diff --git a/src/QGCLoggingCategory.h b/src/QGCLoggingCategory.h index e4d1de3bc1d..dc13642b044 100644 --- a/src/QGCLoggingCategory.h +++ b/src/QGCLoggingCategory.h @@ -22,6 +22,7 @@ Q_DECLARE_LOGGING_CATEGORY(GeotaggingLog) Q_DECLARE_LOGGING_CATEGORY(RTKGPSLog) Q_DECLARE_LOGGING_CATEGORY(GuidedActionsControllerLog) Q_DECLARE_LOGGING_CATEGORY(ADSBVehicleManagerLog) +Q_DECLARE_LOGGING_CATEGORY(NTRIPLog) Q_DECLARE_LOGGING_CATEGORY(LocalizationLog) Q_DECLARE_LOGGING_CATEGORY(VideoAllLog) // turns on all individual QGC video logs Q_DECLARE_LOGGING_CATEGORY(JoystickLog) diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc index 7fede01381d..0ae940992c5 100644 --- a/src/QGCToolbox.cc +++ b/src/QGCToolbox.cc @@ -31,6 +31,7 @@ #include "SettingsManager.h" #include "QGCApplication.h" #include "ADSBVehicleManager.h" +#include "NTRIP.h" #if defined(QGC_ENABLE_PAIRING) #include "PairingManager.h" #endif @@ -70,6 +71,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) _videoManager = new VideoManager (app, this); _mavlinkLogManager = new MAVLinkLogManager (app, this); _adsbVehicleManager = new ADSBVehicleManager (app, this); + _ntrip = new NTRIP (app, this); #if defined(QGC_ENABLE_PAIRING) _pairingManager = new PairingManager (app, this); #endif @@ -106,6 +108,7 @@ void QGCToolbox::setChildToolboxes(void) _videoManager->setToolbox(this); _mavlinkLogManager->setToolbox(this); _adsbVehicleManager->setToolbox(this); + _ntrip->setToolbox(this); #if defined(QGC_GST_TAISYNC_ENABLED) _taisyncManager->setToolbox(this); #endif diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index 44b87fe1ec7..3391ddc6738 100644 --- a/src/QGCToolbox.h +++ b/src/QGCToolbox.h @@ -33,6 +33,7 @@ class MAVLinkLogManager; class QGCCorePlugin; class SettingsManager; class ADSBVehicleManager; +class NTRIP; #if defined(QGC_ENABLE_PAIRING) class PairingManager; #endif @@ -67,6 +68,7 @@ class QGCToolbox : public QObject { QGCCorePlugin* corePlugin () { return _corePlugin; } SettingsManager* settingsManager () { return _settingsManager; } ADSBVehicleManager* adsbVehicleManager () { return _adsbVehicleManager; } + NTRIP* ntrip () { return _ntrip; } #if defined(QGC_ENABLE_PAIRING) PairingManager* pairingManager () { return _pairingManager; } #endif @@ -106,6 +108,7 @@ class QGCToolbox : public QObject { QGCCorePlugin* _corePlugin = nullptr; SettingsManager* _settingsManager = nullptr; ADSBVehicleManager* _adsbVehicleManager = nullptr; + NTRIP* _ntrip = nullptr; #if defined(QGC_ENABLE_PAIRING) PairingManager* _pairingManager = nullptr; #endif diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 2a32c717825..5b4033b853d 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -78,6 +78,7 @@ void QGroundControlQmlGlobal::setToolbox(QGCToolbox* toolbox) _settingsManager = toolbox->settingsManager(); _gpsRtkFactGroup = qgcApp()->gpsRtkFactGroup(); _adsbVehicleManager = toolbox->adsbVehicleManager(); + _ntrip = toolbox->ntrip(); _globalPalette = new QGCPalette(this); #if defined(QGC_ENABLE_PAIRING) _pairingManager = toolbox->pairingManager(); diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 1a54275ad93..4be308d5965 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -66,6 +66,7 @@ class QGroundControlQmlGlobal : public QGCTool Q_PROPERTY(MAVLinkLogManager* mavlinkLogManager READ mavlinkLogManager CONSTANT) Q_PROPERTY(SettingsManager* settingsManager READ settingsManager CONSTANT) Q_PROPERTY(ADSBVehicleManager* adsbVehicleManager READ adsbVehicleManager CONSTANT) + Q_PROPERTY(NTRIP* ntrip READ ntrip CONSTANT) Q_PROPERTY(QGCCorePlugin* corePlugin READ corePlugin CONSTANT) Q_PROPERTY(MissionCommandTree* missionCommandTree READ missionCommandTree CONSTANT) Q_PROPERTY(FactGroup* gpsRtk READ gpsRtkFactGroup CONSTANT) @@ -163,6 +164,7 @@ class QGroundControlQmlGlobal : public QGCTool SettingsManager* settingsManager () { return _settingsManager; } FactGroup* gpsRtkFactGroup () { return _gpsRtkFactGroup; } ADSBVehicleManager* adsbVehicleManager () { return _adsbVehicleManager; } + NTRIP* ntrip () { return _ntrip; } QmlUnitsConversion* unitsConversion () { return &_unitsConversion; } #if defined(QGC_ENABLE_PAIRING) bool supportsPairing () { return true; } @@ -257,6 +259,7 @@ class QGroundControlQmlGlobal : public QGCTool TaisyncManager* _taisyncManager = nullptr; MicrohardManager* _microhardManager = nullptr; ADSBVehicleManager* _adsbVehicleManager = nullptr; + NTRIP* _ntrip = nullptr; QGCPalette* _globalPalette = nullptr; QmlUnitsConversion _unitsConversion; #if defined(QGC_ENABLE_PAIRING) diff --git a/src/Settings/CMakeLists.txt b/src/Settings/CMakeLists.txt index 5e8f5e6942f..9daf094151d 100644 --- a/src/Settings/CMakeLists.txt +++ b/src/Settings/CMakeLists.txt @@ -18,6 +18,8 @@ add_library(Settings FlightMapSettings.h FlyViewSettings.cc FlyViewSettings.h + NTRIPSettings.cc + NTRIPSettings.h OfflineMapsSettings.cc OfflineMapsSettings.h PlanViewSettings.cc diff --git a/src/Settings/NTRIP.SettingsGroup.json b/src/Settings/NTRIP.SettingsGroup.json new file mode 100644 index 00000000000..723e664e09c --- /dev/null +++ b/src/Settings/NTRIP.SettingsGroup.json @@ -0,0 +1,67 @@ +{ + "version": 1, + "fileType": "FactMetaData", + "QGC.MetaData.Facts": +[ +{ + "name": "ntripServerConnectEnabled", + "shortDesc": "Connect to NTRIP server", + "longDesc": "Connect to NTRIP server using specified address/port", + "type": "bool", + "default": false, + "qgcRebootRequired": true +}, +{ + "name": "ntripServerHostAddress", + "shortDesc": "Host address", + "type": "string", + "default": "127.0.0.1", + "qgcRebootRequired": true +}, +{ + "name": "ntripServerPort", + "shortDesc": "Server port", + "type": "string", + "default": 2101, + "qgcRebootRequired": true +}, +{ + "name": "ntripUsername", + "shortDesc": "Username", + "type": "string", + "default": "", + "qgcRebootRequired": true +}, +{ + "name": "ntripPassword", + "shortDesc": "Password", + "type": "string", + "default": "", + "qgcRebootRequired": true +}, +{ + "name": "ntripMountpoint", + "shortDesc": "Mount Point", + "longDesc": "NTRIP Mount Point. Leave blank for RTCM over TCP", + "type": "string", + "default": "", + "qgcRebootRequired": true +}, +{ + "name": "ntripWhitelist", + "shortDesc": "RTCM Message Whitelist", + "longDesc": "RTCM Messages to send over mavlink. Leave blank for all messages", + "type": "string", + "default": "", + "qgcRebootRequired": true +}, +{ + "name": "ntripEnableVRS", + "shortDesc": "Enable NTRIP VRS", + "longDesc": "Check to Enable VRS RTK", + "type": "bool", + "default": false, + "qgcRebootRequired": true +} +] +} diff --git a/src/Settings/NTRIPSettings.cc b/src/Settings/NTRIPSettings.cc new file mode 100644 index 00000000000..c4ca6b697ff --- /dev/null +++ b/src/Settings/NTRIPSettings.cc @@ -0,0 +1,27 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "NTRIPSettings.h" + +#include +#include + +DECLARE_SETTINGGROUP(NTRIP, "NTRIP") +{ + qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "NTRIPSettings", "Reference only"); +} + +DECLARE_SETTINGSFACT(NTRIPSettings, ntripServerConnectEnabled) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripServerHostAddress) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripServerPort) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripUsername) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripPassword) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripMountpoint) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripWhitelist) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripEnableVRS) diff --git a/src/Settings/NTRIPSettings.h b/src/Settings/NTRIPSettings.h new file mode 100644 index 00000000000..0bcd7f7d59f --- /dev/null +++ b/src/Settings/NTRIPSettings.h @@ -0,0 +1,29 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "SettingsGroup.h" + +class NTRIPSettings : public SettingsGroup +{ + Q_OBJECT +public: + NTRIPSettings(QObject* parent = nullptr); + DEFINE_SETTING_NAME_GROUP() + + DEFINE_SETTINGFACT(ntripServerConnectEnabled) + DEFINE_SETTINGFACT(ntripServerHostAddress) + DEFINE_SETTINGFACT(ntripServerPort) + DEFINE_SETTINGFACT(ntripUsername) + DEFINE_SETTINGFACT(ntripPassword) + DEFINE_SETTINGFACT(ntripMountpoint) + DEFINE_SETTINGFACT(ntripWhitelist) + DEFINE_SETTINGFACT(ntripEnableVRS) +}; diff --git a/src/Settings/SettingsManager.cc b/src/Settings/SettingsManager.cc index 36648cd0e08..31a11095c61 100644 --- a/src/Settings/SettingsManager.cc +++ b/src/Settings/SettingsManager.cc @@ -27,6 +27,7 @@ SettingsManager::SettingsManager(QGCApplication* app, QGCToolbox* toolbox) , _firmwareUpgradeSettings (nullptr) , _adsbVehicleManagerSettings (nullptr) , _gimbalControllerSettings (nullptr) + , _ntripSettings (nullptr) #if !defined(NO_ARDUPILOT_DIALECT) , _apmMavlinkStreamRateSettings (nullptr) #endif @@ -54,8 +55,9 @@ void SettingsManager::setToolbox(QGCToolbox *toolbox) _firmwareUpgradeSettings = new FirmwareUpgradeSettings (this); _adsbVehicleManagerSettings = new ADSBVehicleManagerSettings (this); _gimbalControllerSettings = new GimbalControllerSettings (this); + _ntripSettings = new NTRIPSettings (this); #if !defined(NO_ARDUPILOT_DIALECT) _apmMavlinkStreamRateSettings = new APMMavlinkStreamRateSettings(this); #endif - _remoteIDSettings = new RemoteIDSettings (this); + _remoteIDSettings = new RemoteIDSettings (this); } diff --git a/src/Settings/SettingsManager.h b/src/Settings/SettingsManager.h index 8950c80c043..6bbcc97780d 100644 --- a/src/Settings/SettingsManager.h +++ b/src/Settings/SettingsManager.h @@ -28,6 +28,7 @@ #include "FirmwareUpgradeSettings.h" #include "ADSBVehicleManagerSettings.h" #include "GimbalControllerSettings.h" +#include "NTRIPSettings.h" #include #include "RemoteIDSettings.h" @@ -52,6 +53,7 @@ class SettingsManager : public QGCTool Q_PROPERTY(QObject* firmwareUpgradeSettings READ firmwareUpgradeSettings CONSTANT) Q_PROPERTY(QObject* adsbVehicleManagerSettings READ adsbVehicleManagerSettings CONSTANT) Q_PROPERTY(QObject* gimbalControllerSettings READ gimbalControllerSettings CONSTANT) + Q_PROPERTY(QObject* ntripSettings READ ntripSettings CONSTANT) #if !defined(NO_ARDUPILOT_DIALECT) Q_PROPERTY(QObject* apmMavlinkStreamRateSettings READ apmMavlinkStreamRateSettings CONSTANT) #endif @@ -72,6 +74,7 @@ class SettingsManager : public QGCTool FirmwareUpgradeSettings* firmwareUpgradeSettings (void) { return _firmwareUpgradeSettings; } ADSBVehicleManagerSettings* adsbVehicleManagerSettings (void) { return _adsbVehicleManagerSettings; } GimbalControllerSettings* gimbalControllerSettings (void) { return _gimbalControllerSettings; } + NTRIPSettings* ntripSettings (void) { return _ntripSettings; } #if !defined(NO_ARDUPILOT_DIALECT) APMMavlinkStreamRateSettings* apmMavlinkStreamRateSettings(void) { return _apmMavlinkStreamRateSettings; } #endif @@ -90,6 +93,7 @@ class SettingsManager : public QGCTool FirmwareUpgradeSettings* _firmwareUpgradeSettings; ADSBVehicleManagerSettings* _adsbVehicleManagerSettings; GimbalControllerSettings* _gimbalControllerSettings; + NTRIPSettings* _ntripSettings; #if !defined(NO_ARDUPILOT_DIALECT) APMMavlinkStreamRateSettings* _apmMavlinkStreamRateSettings; #endif diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index 1b9b9d68f20..3e44420715b 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -1059,6 +1059,105 @@ Rectangle { } } + Item { width: 1; height: _margins; visible: ntripSectionLabel.visible } + QGCLabel { + id: ntripSectionLabel + text: qsTr("NTRIP / RTCM") + visible: QGroundControl.settingsManager.ntripSettings.visible + } + Rectangle { + Layout.preferredHeight: ntripGrid.height + _margins + Layout.preferredWidth: ntripGrid.width + (_margins * 2) + color: qgcPal.windowShade + visible: ntripSectionLabel.visible + Layout.fillWidth: true + + GridLayout { + id: ntripGrid + anchors.topMargin: _margins + anchors.top: parent.top + Layout.fillWidth: true + anchors.horizontalCenter: parent.horizontalCenter + columns: 2 + + property var ntripSettings: QGroundControl.settingsManager.ntripSettings + + FactCheckBox { + text: ntripGrid.ntripSettings.ntripServerConnectEnabled.shortDescription + fact: ntripGrid.ntripSettings.ntripServerConnectEnabled + visible: ntripGrid.ntripSettings.ntripServerConnectEnabled.visible + Layout.columnSpan: 2 + } + + FactCheckBox { + text: ntripGrid.ntripSettings.ntripEnableVRS.shortDescription + fact: ntripGrid.ntripSettings.ntripEnableVRS + visible: ntripGrid.ntripSettings.ntripEnableVRS.visible + Layout.columnSpan: 2 + } + + QGCLabel { + text: ntripGrid.ntripSettings.ntripServerHostAddress.shortDescription + visible: ntripGrid.ntripSettings.ntripServerHostAddress.visible + } + FactTextField { + fact: ntripGrid.ntripSettings.ntripServerHostAddress + visible: ntripGrid.ntripSettings.ntripServerHostAddress.visible + Layout.fillWidth: true + } + + QGCLabel { + text: ntripGrid.ntripSettings.ntripServerPort.shortDescription + visible: ntripGrid.ntripSettings.ntripServerPort.visible + } + FactTextField { + fact: ntripGrid.ntripSettings.ntripServerPort + visible: ntripGrid.ntripSettings.ntripServerPort.visible + Layout.preferredWidth: _valueFieldWidth + } + + QGCLabel { + text: ntripGrid.ntripSettings.ntripUsername.shortDescription + visible: ntripGrid.ntripSettings.ntripUsername.visible + } + FactTextField { + fact: ntripGrid.ntripSettings.ntripUsername + visible: ntripGrid.ntripSettings.ntripUsername.visible + Layout.fillWidth: true + } + + QGCLabel { + text: ntripGrid.ntripSettings.ntripPassword.shortDescription + visible: ntripGrid.ntripSettings.ntripPassword.visible + } + FactTextField { + fact: ntripGrid.ntripSettings.ntripPassword + visible: ntripGrid.ntripSettings.ntripPassword.visible + Layout.fillWidth: true + } + + QGCLabel { + text: ntripGrid.ntripSettings.ntripMountpoint.shortDescription + visible: ntripGrid.ntripSettings.ntripMountpoint.visible + } + FactTextField { + fact: ntripGrid.ntripSettings.ntripMountpoint + visible: ntripGrid.ntripSettings.ntripMountpoint.visible + Layout.fillWidth: true + } + + QGCLabel { + text: ntripGrid.ntripSettings.ntripWhitelist.shortDescription + visible: ntripGrid.ntripSettings.ntripWhitelist.visible + } + FactTextField { + fact: ntripGrid.ntripSettings.ntripWhitelist + visible: ntripGrid.ntripSettings.ntripWhitelist.visible + Layout.fillWidth: true + } + } + } + Item { width: 1; height: _margins; visible: adsbSectionLabel.visible } QGCLabel { id: adsbSectionLabel From df3a3a9fe76605daa500b2835c707e68c5d21482 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Tue, 5 Nov 2024 13:14:38 +0100 Subject: [PATCH 2/7] Improve RTCM connection --- src/GPS/RTCM/RTCMMavlink.cc | 26 +++--- src/GPS/RTCM/RTCMMavlink.h | 5 +- src/NTRIP/NTRIP.cc | 119 ++++++++++++++++++-------- src/NTRIP/NTRIP.h | 6 +- src/Settings/NTRIP.SettingsGroup.json | 18 +++- src/Settings/NTRIPSettings.cc | 1 + src/Settings/NTRIPSettings.h | 1 + 7 files changed, 128 insertions(+), 48 deletions(-) diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index b06d2dbf212..e8043383c4e 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2020 QGROUNDCONTROL PROJECT + * (c) 2009-2024 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -15,6 +15,8 @@ #include +QGC_LOGGING_CATEGORY(RTCMMavlinkLog, "RTCMMavlinkLog") + RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) : _toolbox(toolbox) { @@ -23,13 +25,13 @@ RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) void RTCMMavlink::RTCMDataUpdate(QByteArray message) { - qCDebug(NTRIPLog) << "RTCMDataUpdate: " << message.size() << " bytes"; /* statistics */ + qCDebug(RTCMMavlinkLog) << "RTCMDataUpdate: " << message.size() << " bytes"; /* statistics */ _bandwidthByteCounter += message.size(); qint64 elapsed = _bandwidthTimer.elapsed(); if (elapsed > 1000) { - printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f); - _bandwidthTimer.restart(); - _bandwidthByteCounter = 0; + qCDebug(RTCMMavlinkLog) << "RTCM bandwidth: " << ((float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f) << " KB/s"; + _bandwidthTimer.restart(); + _bandwidthByteCounter = 0; } const int maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN; @@ -72,12 +74,14 @@ void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) mavlink_message_t message; SharedLinkInterfacePtr sharedLink = weakLink.lock(); - mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), - sharedLink->mavlinkChannel(), - &message, - &msg); - vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message); + if (sharedLink->isConnected()) { + mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(), + mavlinkProtocol->getComponentId(), + sharedLink->mavlinkChannel(), + &message, + &msg); + vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message); + } } } } diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h index 14b649d7d60..2de59fad6cd 100644 --- a/src/GPS/RTCM/RTCMMavlink.h +++ b/src/GPS/RTCM/RTCMMavlink.h @@ -13,8 +13,11 @@ #include #include -#include "QGCToolbox.h" #include "MAVLinkProtocol.h" +#include "QGCLoggingCategory.h" +#include "QGCToolbox.h" + +Q_DECLARE_LOGGING_CATEGORY(RTCMMavlinkLog) /** ** class RTCMMavlink diff --git a/src/NTRIP/NTRIP.cc b/src/NTRIP/NTRIP.cc index 6207b61e07d..7bda3fafda1 100644 --- a/src/NTRIP/NTRIP.cc +++ b/src/NTRIP/NTRIP.cc @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2020 QGROUNDCONTROL PROJECT + * (c) 2009-2024 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -15,8 +15,6 @@ #include "PositionManager.h" #include "NTRIPSettings.h" -#include - NTRIP::NTRIP(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) { @@ -145,8 +143,14 @@ void NTRIPTCPLink::_hardwareConnect() // If mountpoint is specified, send an http get request for data if ( !_mountpoint.isEmpty()) { qCDebug(NTRIPLog) << "Sending HTTP request, using mount point: " << _mountpoint; - QString auth = QString(_username + ":" + _password).toUtf8().toBase64(); - QString query = "GET /%1 HTTP/1.0\r\nUser-Agent: NTRIP\r\nAuthorization: Basic %2\r\n\r\n"; + // TODO(zdanek) Add support for NTRIP v2 + QString digest = QString(_username + ":" + _password).toUtf8().toBase64(); + QString auth = QString("Authorization: Basic %1\r\n").arg(digest); + QString query = "GET /%1 HTTP/1.0\r\n" + "User-Agent: NTRIP QGroundControl\r\n" + "%2" + "Connection: close\r\n\r\n"; + _socket->write(query.arg(_mountpoint).arg(auth).toUtf8()); _state = NTRIPState::waiting_for_http_response; } else { @@ -157,29 +161,47 @@ void NTRIPTCPLink::_hardwareConnect() qCDebug(NTRIPLog) << "NTRIP Socket connected"; } -void NTRIPTCPLink::_parse(const QByteArray &buffer) -{ - qCDebug(NTRIPLog) << "Parsing " << buffer.size() << " bytes"; for(const uint8_t byte : buffer) { - if(_state == NTRIPState::waiting_for_rtcm_header) { - if(byte != RTCM3_PREAMBLE) - continue; - _state = NTRIPState::accumulating_rtcm_packet; - } - if(_rtcm_parsing->addByte(byte)) { - _state = NTRIPState::waiting_for_rtcm_header; - QByteArray message((char*)_rtcm_parsing->message(), static_cast(_rtcm_parsing->messageLength())); - //TODO: Restore the following when upstreamed in Driver repo - //uint16_t id = _rtcm_parsing->messageId(); - uint16_t id = ((uint8_t)message[3] << 4) | ((uint8_t)message[4] >> 4); - if(_whitelist.empty() || _whitelist.contains(id)) { - emit RTCMDataUpdate(message); - qCDebug(NTRIPLog) << "Sending " << id << "of size " << message.length(); - } else { - qCDebug(NTRIPLog) << "Ignoring " << id; - } - _rtcm_parsing->reset(); - } +void NTRIPTCPLink::_parse(const QByteArray &buffer) { + qCDebug(NTRIPLog) << "Parsing " << buffer.size() << " bytes"; + for (const uint8_t byte : buffer) { + if (_state == NTRIPState::waiting_for_rtcm_header) { + if (byte != RTCM3_PREAMBLE && byte != RTCM2_PREAMBLE) { + qCDebug(NTRIPLog) << "NOT RTCM 2/3 preamble, ignoring byte " << byte; + continue; + } + _state = NTRIPState::accumulating_rtcm_packet; } + + if (_rtcm_parsing->addByte(byte)) { + _state = NTRIPState::waiting_for_rtcm_header; + QByteArray message((char *)_rtcm_parsing->message(), + static_cast(_rtcm_parsing->messageLength())); + uint16_t id = _rtcm_parsing->messageId(); + uint8_t version = _rtcm_parsing->rtcmVersion(); + qCDebug(NTRIPLog) << "RTCM version " << version; + qCDebug(NTRIPLog) << "RTCM message ID " << id; + qCDebug(NTRIPLog) << "RTCM message size " << message.size(); + + if (version == 2) { + qCWarning(NTRIPLog) << "RTCM 2 not supported"; + emit error("Server sent RTCM 2 message. Not supported!"); + continue; + } else if (version != 3) { + qCWarning(NTRIPLog) << "Unknown RTCM version " << version; + emit error("Server sent unknown RTCM version"); + continue; + } + + // uint16_t id = ((uint8_t)message[3] << 4) | ((uint8_t)message[4] >> 4); + if (_whitelist.empty() || _whitelist.contains(id)) { + qCDebug(NTRIPLog) << "Sending message ID [" << id << "] of size " << message.length(); + emit RTCMDataUpdate(message); + } else { + qCDebug(NTRIPLog) << "Ignoring " << id; + } + _rtcm_parsing->reset(); + } + } } void NTRIPTCPLink::_readBytes(void) @@ -189,10 +211,21 @@ void NTRIPTCPLink::_readBytes(void) return; } if(_state == NTRIPState::waiting_for_http_response) { - QString line = _socket->readLine(); + + //Content-Type: gnss/sourcetable + // Content-Type: gnss/sourcetable + + QString line = _socket->readLine(); + qCDebug(NTRIPLog) << "Server responded with " << line; if (line.contains("200")){ qCDebug(NTRIPLog) << "Server responded with " << line; - _state = NTRIPState::waiting_for_rtcm_header; + if (line.contains("SOURCETABLE")) { + qCDebug(NTRIPLog) << "Server responded with SOURCETABLE, not supported"; + emit error("NTRIP Server responded with SOURCETABLE. Bad mountpoint?"); + _state = NTRIPState::uninitialised; + } else { + _state = NTRIPState::waiting_for_rtcm_header; + } } else if (line.contains("401")) { qCWarning(NTRIPLog) << "Server responded with " << line; emit error("NTRIP Server responded with 401 Unauthorized"); @@ -204,20 +237,38 @@ void NTRIPTCPLink::_readBytes(void) _state = NTRIPState::waiting_for_rtcm_header; } } + + +// throw new Exception("Got SOURCETABLE - Bad ntrip mount point\n\n" + line); + + QByteArray bytes = _socket->readAll(); _parse(bytes); } void NTRIPTCPLink::_sendNMEA() { + // TODO(zdanek) check if this is proper NMEA message qCDebug(NTRIPLog) << "Sending NMEA"; - QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition(); - - if(!gcsPosition.isValid()) { - qCDebug(NTRIPLog) << "GCS Position is not valid"; + QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition(); + + if (!gcsPosition.isValid()) { + qCDebug(NTRIPLog) << "GCS Position is not valid"; + if (_takePosFromVehicle) { + Vehicle *vehicle = _toolbox->multiVehicleManager()->activeVehicle(); + if (vehicle) { + qCDebug(NTRIPLog) << "Active vehicle found. Using vehicle position"; + gcsPosition = vehicle->coordinate(); + } else { + qCDebug(NTRIPLog) << "No active vehicle"; return; + } + } else { + qCDebug(NTRIPLog) << "No valid GCS position"; + return; } + } - double lat = gcsPosition.latitude(); + double lat = gcsPosition.latitude(); double lng = gcsPosition.longitude(); double alt = gcsPosition.altitude(); diff --git a/src/NTRIP/NTRIP.h b/src/NTRIP/NTRIP.h index 13a94646df4..2dc607e7ecd 100644 --- a/src/NTRIP/NTRIP.h +++ b/src/NTRIP/NTRIP.h @@ -63,6 +63,7 @@ private slots: QString _mountpoint; QSet _whitelist; bool _isVRSEnable; + bool _ntripForceV1 = false; // QUrl QUrl _ntripURL; @@ -73,12 +74,15 @@ private slots: // VRS Timer QTimer* _vrsSendTimer; - static const int _vrsSendRateMSecs = 3000; + // this is perfectly fine to send VRS data every 30 seconds + static const int _vrsSendRateMSecs = 30000; RTCMParsing *_rtcm_parsing{nullptr}; NTRIPState _state; QGCToolbox* _toolbox = nullptr; + // TODO(zdanek): take from settings + bool _takePosFromVehicle = true; }; class NTRIP : public QGCTool { diff --git a/src/Settings/NTRIP.SettingsGroup.json b/src/Settings/NTRIP.SettingsGroup.json index 723e664e09c..61918be6871 100644 --- a/src/Settings/NTRIP.SettingsGroup.json +++ b/src/Settings/NTRIP.SettingsGroup.json @@ -49,7 +49,7 @@ }, { "name": "ntripWhitelist", - "shortDesc": "RTCM Message Whitelist", + "shortDesc": "RTCM Message ID Whitelist, comma separated", "longDesc": "RTCM Messages to send over mavlink. Leave blank for all messages", "type": "string", "default": "", @@ -62,6 +62,22 @@ "type": "bool", "default": false, "qgcRebootRequired": true +}, +{ + "name": "ntripForceNtripV1", + "shortDesc": "Force NTRIP V1 instead of V2", + "longDesc": "By default the system will connect using NTRIP V2. Check this box to force NTRIP V1", + "type": "bool", + "default": false, + "qgcRebootRequired": true +}, +{ + "name": "ntripTakeNmeaPosFromVehicle", + "shortDesc": "For VRS, take NMEA position from vehicle's home position", + "longDesc": "When VRS is enabled, take NMEA position from vehicle's home position when Base Station position is not available", + "type": "bool", + "default": true, + "qgcRebootRequired": true } ] } diff --git a/src/Settings/NTRIPSettings.cc b/src/Settings/NTRIPSettings.cc index c4ca6b697ff..e5732d63eb7 100644 --- a/src/Settings/NTRIPSettings.cc +++ b/src/Settings/NTRIPSettings.cc @@ -25,3 +25,4 @@ DECLARE_SETTINGSFACT(NTRIPSettings, ntripPassword) DECLARE_SETTINGSFACT(NTRIPSettings, ntripMountpoint) DECLARE_SETTINGSFACT(NTRIPSettings, ntripWhitelist) DECLARE_SETTINGSFACT(NTRIPSettings, ntripEnableVRS) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripForceNtripV1) diff --git a/src/Settings/NTRIPSettings.h b/src/Settings/NTRIPSettings.h index 0bcd7f7d59f..c45721b00b2 100644 --- a/src/Settings/NTRIPSettings.h +++ b/src/Settings/NTRIPSettings.h @@ -26,4 +26,5 @@ class NTRIPSettings : public SettingsGroup DEFINE_SETTINGFACT(ntripMountpoint) DEFINE_SETTINGFACT(ntripWhitelist) DEFINE_SETTINGFACT(ntripEnableVRS) + DEFINE_SETTINGFACT(ntripForceNtripV1) }; From a3035908da0652c05c132a07ad95ad662ee57167 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Tue, 12 Nov 2024 14:19:33 +0100 Subject: [PATCH 3/7] Block reading data if bad mount point --- src/NTRIP/NTRIP.cc | 7 +++++++ src/NTRIP/NTRIP.h | 2 ++ src/QGCLoggingCategory.cc | 1 - 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/NTRIP/NTRIP.cc b/src/NTRIP/NTRIP.cc index 7bda3fafda1..429a3d26773 100644 --- a/src/NTRIP/NTRIP.cc +++ b/src/NTRIP/NTRIP.cc @@ -15,6 +15,8 @@ #include "PositionManager.h" #include "NTRIPSettings.h" +QGC_LOGGING_CATEGORY(NTRIPLog, "NTRIP") + NTRIP::NTRIP(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) { @@ -241,6 +243,11 @@ void NTRIPTCPLink::_readBytes(void) // throw new Exception("Got SOURCETABLE - Bad ntrip mount point\n\n" + line); + if (_state == NTRIPState::uninitialised) { + qCDebug(NTRIPLog) << "NTRIP State is uninitialised. Discarding bytes"; + _socket->readAll(); + return; + } QByteArray bytes = _socket->readAll(); _parse(bytes); diff --git a/src/NTRIP/NTRIP.h b/src/NTRIP/NTRIP.h index 2dc607e7ecd..062a3eeae3b 100644 --- a/src/NTRIP/NTRIP.h +++ b/src/NTRIP/NTRIP.h @@ -17,6 +17,8 @@ #include "Drivers/src/rtcm.h" #include "RTCM/RTCMMavlink.h" +Q_DECLARE_LOGGING_CATEGORY(NTRIPLog) + class NTRIPSettings; class NTRIPTCPLink : public QThread diff --git a/src/QGCLoggingCategory.cc b/src/QGCLoggingCategory.cc index 35dbe50e307..791de78414e 100644 --- a/src/QGCLoggingCategory.cc +++ b/src/QGCLoggingCategory.cc @@ -27,7 +27,6 @@ QGC_LOGGING_CATEGORY(GeotaggingLog, "GeotaggingLog") QGC_LOGGING_CATEGORY(RTKGPSLog, "RTKGPSLog") QGC_LOGGING_CATEGORY(GuidedActionsControllerLog, "GuidedActionsControllerLog") QGC_LOGGING_CATEGORY(ADSBVehicleManagerLog, "ADSBVehicleManagerLog") -QGC_LOGGING_CATEGORY(NTRIPLog, "NTRIPLog") QGC_LOGGING_CATEGORY(LocalizationLog, "LocalizationLog") QGC_LOGGING_CATEGORY(VideoAllLog, kVideoAllLogCategory) QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") From 9c90439902c6edc132af3a83835d5dc42d57bd01 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Mon, 25 Nov 2024 15:11:30 +0100 Subject: [PATCH 4/7] Fixes in rtcm mesg parsing --- qgroundcontrol.pro | 1 + src/GPS/CMakeLists.txt | 1 + src/GPS/NmeaMessage.cc | 92 +++++++++ src/GPS/NmeaMessage.h | 20 ++ src/NTRIP/NTRIP.cc | 431 +++++++++++++++++------------------------ src/NTRIP/NTRIP.h | 7 +- 6 files changed, 297 insertions(+), 255 deletions(-) create mode 100644 src/GPS/NmeaMessage.cc create mode 100644 src/GPS/NmeaMessage.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0a9c8318caf..010fa4a681f 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -1072,6 +1072,7 @@ SOURCES += \ src/GPS/Drivers/src/sbf.cpp \ src/GPS/GPSManager.cc \ src/GPS/GPSProvider.cc \ + src/GPS/NmeaMessage.cc \ src/Joystick/JoystickSDL.cc \ src/RunGuard.cc \ } diff --git a/src/GPS/CMakeLists.txt b/src/GPS/CMakeLists.txt index b7284578058..5685488ce92 100644 --- a/src/GPS/CMakeLists.txt +++ b/src/GPS/CMakeLists.txt @@ -9,6 +9,7 @@ add_library(gps Drivers/src/ubx.cpp GPSManager.cc GPSProvider.cc + NmeaMessage.cc RTCM/RTCMMavlink.cc ) diff --git a/src/GPS/NmeaMessage.cc b/src/GPS/NmeaMessage.cc new file mode 100644 index 00000000000..3829bcc5503 --- /dev/null +++ b/src/GPS/NmeaMessage.cc @@ -0,0 +1,92 @@ +// +// Created by zdanek on 13.11.24. +// + +#include "NmeaMessage.h" + +#include +#include +#include + +NmeaMessage::NmeaMessage(QGeoCoordinate coordinate) { + _coordinate = coordinate; +} + +QString NmeaMessage::getGGA() { + + double lat = _coordinate.latitude(); + double lng = _coordinate.longitude(); + double alt = _coordinate.altitude(); + + // qCDebug(NTRIPLog) << "lat : " << lat << " lon : " << lng << " alt : " << alt; + + QString time = QDateTime::currentDateTimeUtc().toString("hhmmss.zzz"); + + if (lat != 0 || lng != 0) { + if (std::isnan(alt)) + alt = 0.0; + + int latDegrees = (int)qFabs(lat); + double latMinutes = (qFabs(lat) - latDegrees) * 60.0; + + int lngDegrees = (int)qFabs(lng); + double lngMinutes = (qFabs(lng) - lngDegrees) * 60.0; + + // Format latitude degrees and minutes with leading zeros + QString latDegreesStr = QString("%1").arg(latDegrees, 2, 10, QChar('0')); + QString latMinutesStr = + QString("%1").arg(latMinutes, 7, 'f', 4, QChar('0')); + QString latStr = latDegreesStr + latMinutesStr; + + // Format longitude degrees and minutes with leading zeros + QString lngDegreesStr = QString("%1").arg(lngDegrees, 3, 10, QChar('0')); + QString lngMinutesStr = + QString("%1").arg(lngMinutes, 7, 'f', 4, QChar('0')); + QString lngStr = lngDegreesStr + lngMinutesStr; + + QString line = + QString("$GPGGA,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14") + .arg(time) // %1 - UTC Time + .arg(latStr) // %2 - Latitude in NMEA format + .arg(lat < 0 ? "S" : "N") // %3 - N/S Indicator + .arg(lngStr) // %4 - Longitude in NMEA format + .arg(lng < 0 ? "W" : "E") // %5 - E/W Indicator + .arg("1") // %6 - Fix Quality + .arg("10") // %7 - Number of Satellites + .arg("1.0") // %8 - Horizontal Dilution of Precision (HDOP) + .arg(QString::number(alt, 'f', 2)) // %9 - Altitude + .arg("M") // %10 - Altitude Units + .arg("0") // %11 - Geoidal Separation + .arg("M") // %12 - Geoidal Separation Units + .arg("0.0") // %13 - Age of Differential GPS Data + .arg("0"); // %14 - Differential Reference Station ID + + // Calculate checksum and send message + QString checkSum = _getCheckSum(line); + QString nmeaMessage = line + "*" + checkSum + "\r\n"; + + return nmeaMessage; + } + return QString(); +} + +QString NmeaMessage::_getCheckSum(QString line) { +// qCDebug(NTRIPLog) << "Calculating checksum"; + QByteArray temp_Byte = line.toUtf8(); + const char* buf = temp_Byte.constData(); + + char character; + int checksum = 0; + + // Start from index 1 to skip the '$' character + for(int i = 1; i < line.length(); i++) { + character = buf[i]; + if(character == '*') { + break; + } + checksum ^= character; + } + + // Ensure the checksum is a two-digit uppercase hexadecimal string + return QString("%1").arg(checksum, 2, 16, QChar('0')).toUpper(); +} \ No newline at end of file diff --git a/src/GPS/NmeaMessage.h b/src/GPS/NmeaMessage.h new file mode 100644 index 00000000000..c183a75ed8c --- /dev/null +++ b/src/GPS/NmeaMessage.h @@ -0,0 +1,20 @@ +// +// Created by zdanek on 13.11.24. +// + +#pragma once + +#include +#include + +class NmeaMessage { +public: + NmeaMessage(QGeoCoordinate coordinate); + QString getGGA(); + +private: + QString _getCheckSum(QString line); + QGeoCoordinate _coordinate; +}; + + diff --git a/src/NTRIP/NTRIP.cc b/src/NTRIP/NTRIP.cc index 429a3d26773..8f5ce84a37f 100644 --- a/src/NTRIP/NTRIP.cc +++ b/src/NTRIP/NTRIP.cc @@ -8,163 +8,166 @@ ****************************************************************************/ #include "NTRIP.h" +#include "NTRIPSettings.h" +#include "NmeaMessage.h" +#include "PositionManager.h" +#include "QGCApplication.h" #include "QGCLoggingCategory.h" #include "QGCToolbox.h" -#include "QGCApplication.h" #include "SettingsManager.h" -#include "PositionManager.h" -#include "NTRIPSettings.h" QGC_LOGGING_CATEGORY(NTRIPLog, "NTRIP") -NTRIP::NTRIP(QGCApplication* app, QGCToolbox* toolbox) - : QGCTool(app, toolbox) -{ -} - -void NTRIP::setToolbox(QGCToolbox* toolbox) -{ - QGCTool::setToolbox(toolbox); - - NTRIPSettings* settings = qgcApp()->toolbox()->settingsManager()->ntripSettings(); - if (settings->ntripServerConnectEnabled()->rawValue().toBool()) { - qCDebug(NTRIPLog) << settings->ntripEnableVRS()->rawValue().toBool(); - _rtcmMavlink = new RTCMMavlink(*toolbox); - - _tcpLink = new NTRIPTCPLink(settings->ntripServerHostAddress()->rawValue().toString(), - settings->ntripServerPort()->rawValue().toInt(), - settings->ntripUsername()->rawValue().toString(), - settings->ntripPassword()->rawValue().toString(), - settings->ntripMountpoint()->rawValue().toString(), - settings->ntripWhitelist()->rawValue().toString(), - settings->ntripEnableVRS()->rawValue().toBool()); - connect(_tcpLink, &NTRIPTCPLink::error, this, &NTRIP::_tcpError, Qt::QueuedConnection); - connect(_tcpLink, &NTRIPTCPLink::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); - } else { - qCDebug(NTRIPLog) << "NTRIP Server is not enabled"; - } +NTRIP::NTRIP(QGCApplication *app, QGCToolbox *toolbox) + : QGCTool(app, toolbox) {} + +void NTRIP::setToolbox(QGCToolbox *toolbox) { + QGCTool::setToolbox(toolbox); + + NTRIPSettings *settings = + qgcApp()->toolbox()->settingsManager()->ntripSettings(); + if (settings->ntripServerConnectEnabled()->rawValue().toBool()) { + qCDebug(NTRIPLog) << settings->ntripEnableVRS()->rawValue().toBool(); + _rtcmMavlink = new RTCMMavlink(*toolbox); + + _tcpLink = new NTRIPTCPLink( + settings->ntripServerHostAddress()->rawValue().toString(), + settings->ntripServerPort()->rawValue().toInt(), + settings->ntripUsername()->rawValue().toString(), + settings->ntripPassword()->rawValue().toString(), + settings->ntripMountpoint()->rawValue().toString(), + settings->ntripWhitelist()->rawValue().toString(), + settings->ntripEnableVRS()->rawValue().toBool()); + connect(_tcpLink, &NTRIPTCPLink::error, this, &NTRIP::_tcpError, + Qt::QueuedConnection); + connect(_tcpLink, &NTRIPTCPLink::RTCMDataUpdate, _rtcmMavlink, + &RTCMMavlink::RTCMDataUpdate); + } else { + qCDebug(NTRIPLog) << "NTRIP Server is not enabled"; + } } - -void NTRIP::_tcpError(const QString errorMsg) -{ - qgcApp()->showAppMessage(tr("NTRIP Server Error: %1").arg(errorMsg)); +void NTRIP::_tcpError(const QString errorMsg) { + qgcApp()->showAppMessage(tr("NTRIP Server Error: %1").arg(errorMsg)); } - -NTRIPTCPLink::NTRIPTCPLink(const QString& hostAddress, - int port, - const QString &username, - const QString &password, - const QString &mountpoint, - const QString &whitelist, - const bool &enableVRS) - : QThread () - , _hostAddress (hostAddress) - , _port (port) - , _username (username) - , _password (password) - , _mountpoint (mountpoint) - , _isVRSEnable (enableVRS) - , _toolbox (qgcApp()->toolbox()) -{ - for(const auto& msg: whitelist.split(',')) { - int msg_int = msg.toInt(); - if(msg_int) { - _whitelist.insert(msg_int); - } +NTRIPTCPLink::NTRIPTCPLink(const QString &hostAddress, int port, + const QString &username, const QString &password, + const QString &mountpoint, const QString &whitelist, + const bool &enableVRS) + : QThread(), _hostAddress(hostAddress), _port(port), _username(username), + _password(password), _mountpoint(mountpoint), _isVRSEnable(enableVRS), + _toolbox(qgcApp()->toolbox()) { + for (const auto &msg : whitelist.split(',')) { + int msg_int = msg.toInt(); + if (msg_int) { + _whitelist.insert(msg_int); } - qCDebug(NTRIPLog) << "whitelist: " << _whitelist; - if (!_rtcm_parsing) { - _rtcm_parsing = new RTCMParsing(); - } - _rtcm_parsing->reset(); - _state = NTRIPState::uninitialised; + } + qCDebug(NTRIPLog) << "whitelist: " << _whitelist; + if (!_rtcm_parsing) { + _rtcm_parsing = new RTCMParsing(); + } + _rtcm_parsing->reset(); + _state = NTRIPState::uninitialised; - // Start TCP Socket - moveToThread(this); - start(); + // Start TCP Socket + moveToThread(this); + start(); } -NTRIPTCPLink::~NTRIPTCPLink(void) -{ +NTRIPTCPLink::~NTRIPTCPLink(void) { qCDebug(NTRIPLog) << "NTRIP Thread stopped"; - if (_socket) { - if(_isVRSEnable) { - _vrsSendTimer->stop(); - QObject::disconnect(_vrsSendTimer, &QTimer::timeout, this, &NTRIPTCPLink::_sendNMEA); - delete _vrsSendTimer; - _vrsSendTimer = nullptr; - } - QObject::disconnect(_socket, &QTcpSocket::readyRead, this, &NTRIPTCPLink::_readBytes); - _socket->disconnectFromHost(); - _socket->deleteLater(); - _socket = nullptr; - - // Delete Rtcm Parsing instance - delete(_rtcm_parsing); - _rtcm_parsing = nullptr; + if (_socket) { + if (_isVRSEnable) { + _vrsSendTimer->stop(); + QObject::disconnect(_vrsSendTimer, &QTimer::timeout, this, + &NTRIPTCPLink::_sendNmeaGga); + delete _vrsSendTimer; + _vrsSendTimer = nullptr; } - quit(); - wait(); + QObject::disconnect(_socket, &QTcpSocket::readyRead, this, + &NTRIPTCPLink::_readBytes); + _socket->disconnectFromHost(); + _socket->deleteLater(); + _socket = nullptr; + + // Delete Rtcm Parsing instance + delete (_rtcm_parsing); + _rtcm_parsing = nullptr; + } + quit(); + wait(); } -void NTRIPTCPLink::run(void) -{ +void NTRIPTCPLink::run(void) { qCDebug(NTRIPLog) << "NTRIP Thread started"; - _hardwareConnect(); - - // Init VRS Timer - if(_isVRSEnable) { - _vrsSendTimer = new QTimer(); - _vrsSendTimer->setInterval(_vrsSendRateMSecs); - _vrsSendTimer->setSingleShot(false); - QObject::connect(_vrsSendTimer, &QTimer::timeout, this, &NTRIPTCPLink::_sendNMEA); - _vrsSendTimer->start(); - } + _hardwareConnect(); + + // Init VRS Timer + if (_isVRSEnable) { + _vrsSendTimer = new QTimer(); + _vrsSendTimer->setInterval(_vrsSendRateMSecs); + _vrsSendTimer->setSingleShot(false); + QObject::connect(_vrsSendTimer, &QTimer::timeout, this, + &NTRIPTCPLink::_sendNmeaGga); + _vrsSendTimer->start(); + } - exec(); + exec(); } -void NTRIPTCPLink::_hardwareConnect() -{ - qCDebug(NTRIPLog) << "Connecting to NTRIP Server: " << _hostAddress << ":" << _port; - _socket = new QTcpSocket(); - QObject::connect(_socket, &QTcpSocket::readyRead, this, &NTRIPTCPLink::_readBytes); - _socket->connectToHost(_hostAddress, static_cast(_port)); - - // Give the socket a second to connect to the other side otherwise error out - if (!_socket->waitForConnected(1000)) { - qCDebug(NTRIPLog) << "NTRIP Socket failed to connect"; - emit error(_socket->errorString()); - delete _socket; - _socket = nullptr; - return; - } - - // If mountpoint is specified, send an http get request for data - if ( !_mountpoint.isEmpty()) { - qCDebug(NTRIPLog) << "Sending HTTP request, using mount point: " << _mountpoint; - // TODO(zdanek) Add support for NTRIP v2 - QString digest = QString(_username + ":" + _password).toUtf8().toBase64(); - QString auth = QString("Authorization: Basic %1\r\n").arg(digest); - QString query = "GET /%1 HTTP/1.0\r\n" - "User-Agent: NTRIP QGroundControl\r\n" - "%2" - "Connection: close\r\n\r\n"; +void NTRIPTCPLink::_hardwareConnect() { + qCDebug(NTRIPLog) << "Connecting to NTRIP Server: " << _hostAddress << ":" + << _port; + _socket = new QTcpSocket(); + QObject::connect(_socket, &QTcpSocket::readyRead, this, + &NTRIPTCPLink::_readBytes); + _socket->connectToHost(_hostAddress, static_cast(_port)); + + // Give the socket a second to connect to the other side otherwise error out + if (!_socket->waitForConnected(1000)) { + qCDebug(NTRIPLog) << "NTRIP Socket failed to connect"; + emit error(_socket->errorString()); + delete _socket; + _socket = nullptr; + return; + } - _socket->write(query.arg(_mountpoint).arg(auth).toUtf8()); - _state = NTRIPState::waiting_for_http_response; + // If mountpoint is specified, send an http get request for data + if (!_mountpoint.isEmpty()) { + qCDebug(NTRIPLog) << "Sending HTTP request, using mount point: " + << _mountpoint; + + QString digest = QString(_username + ":" + _password).toUtf8().toBase64(); + QString auth = QString("Authorization: Basic %1\r\n").arg(digest); + QString query; + if (_ntripForceV1) { + query = "GET /%1 HTTP/1.0\r\n" + "User-Agent: NTRIP QGroundControl\r\n" + "%2" + "Connection: close\r\n\r\n"; } else { - // If no mountpoint is set, assume we will just get data from the tcp stream - _state = NTRIPState::waiting_for_rtcm_header; + query = "GET /%1 HTTP/1.1\r\n" + "User-Agent: NTRIP QGroundControl\r\n" + "Ntrip-Version: Ntrip/2.0\r\n" + "%2" + "Connection: close\r\n\r\n"; } - qCDebug(NTRIPLog) << "NTRIP Socket connected"; + _socket->write(query.arg(_mountpoint).arg(auth).toUtf8()); + _state = NTRIPState::waiting_for_http_response; + } else { + // If no mountpoint is set, assume we will just get data from the tcp stream + _state = NTRIPState::waiting_for_rtcm_header; + } + + qCDebug(NTRIPLog) << "NTRIP Socket connected"; } void NTRIPTCPLink::_parse(const QByteArray &buffer) { qCDebug(NTRIPLog) << "Parsing " << buffer.size() << " bytes"; + qCDebug(NTRIPLog) << "Buffer: " << QString(buffer); for (const uint8_t byte : buffer) { if (_state == NTRIPState::waiting_for_rtcm_header) { if (byte != RTCM3_PREAMBLE && byte != RTCM2_PREAMBLE) { @@ -194,9 +197,9 @@ void NTRIPTCPLink::_parse(const QByteArray &buffer) { continue; } - // uint16_t id = ((uint8_t)message[3] << 4) | ((uint8_t)message[4] >> 4); if (_whitelist.empty() || _whitelist.contains(id)) { - qCDebug(NTRIPLog) << "Sending message ID [" << id << "] of size " << message.length(); + qCDebug(NTRIPLog) << "Sending message ID [" << id << "] of size " + << message.length(); emit RTCMDataUpdate(message); } else { qCDebug(NTRIPLog) << "Ignoring " << id; @@ -206,139 +209,67 @@ void NTRIPTCPLink::_parse(const QByteArray &buffer) { } } -void NTRIPTCPLink::_readBytes(void) -{ +void NTRIPTCPLink::_readBytes(void) { qCDebug(NTRIPLog) << "Reading bytes"; - if (!_socket) { - return; - } - if(_state == NTRIPState::waiting_for_http_response) { - - //Content-Type: gnss/sourcetable - // Content-Type: gnss/sourcetable - - QString line = _socket->readLine(); + if (!_socket) { + return; + } + if (_state == NTRIPState::waiting_for_http_response) { + QString line = _socket->readLine(); + qCDebug(NTRIPLog) << "Server responded with " << line; + if (line.contains("200")) { qCDebug(NTRIPLog) << "Server responded with " << line; - if (line.contains("200")){ - qCDebug(NTRIPLog) << "Server responded with " << line; - if (line.contains("SOURCETABLE")) { - qCDebug(NTRIPLog) << "Server responded with SOURCETABLE, not supported"; - emit error("NTRIP Server responded with SOURCETABLE. Bad mountpoint?"); - _state = NTRIPState::uninitialised; - } else { - _state = NTRIPState::waiting_for_rtcm_header; - } - } else if (line.contains("401")) { - qCWarning(NTRIPLog) << "Server responded with " << line; - emit error("NTRIP Server responded with 401 Unauthorized"); - _state = NTRIPState::uninitialised; - } else { - qCWarning(NTRIPLog) << "Server responded with " << line; - // TODO: Handle failure. Reconnect? - // Just move into parsing mode and hope for now. - _state = NTRIPState::waiting_for_rtcm_header; - } - } - - -// throw new Exception("Got SOURCETABLE - Bad ntrip mount point\n\n" + line); - - if (_state == NTRIPState::uninitialised) { - qCDebug(NTRIPLog) << "NTRIP State is uninitialised. Discarding bytes"; - _socket->readAll(); - return; - } - - QByteArray bytes = _socket->readAll(); - _parse(bytes); -} - -void NTRIPTCPLink::_sendNMEA() { - // TODO(zdanek) check if this is proper NMEA message - qCDebug(NTRIPLog) << "Sending NMEA"; - QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition(); - - if (!gcsPosition.isValid()) { - qCDebug(NTRIPLog) << "GCS Position is not valid"; - if (_takePosFromVehicle) { - Vehicle *vehicle = _toolbox->multiVehicleManager()->activeVehicle(); - if (vehicle) { - qCDebug(NTRIPLog) << "Active vehicle found. Using vehicle position"; - gcsPosition = vehicle->coordinate(); + if (line.contains("SOURCETABLE")) { + qCDebug(NTRIPLog) << "Server responded with SOURCETABLE, not supported"; + emit error("NTRIP Server responded with SOURCETABLE. Bad mountpoint?"); + _state = NTRIPState::uninitialised; } else { - qCDebug(NTRIPLog) << "No active vehicle"; - return; + _state = NTRIPState::waiting_for_rtcm_header; } + } else if (line.contains("401")) { + qCWarning(NTRIPLog) << "Server responded with " << line; + emit error("NTRIP Server responded with 401 Unauthorized"); + _state = NTRIPState::uninitialised; } else { - qCDebug(NTRIPLog) << "No valid GCS position"; - return; + qCWarning(NTRIPLog) << "Server responded with " << line; + // TODO: Handle failure. Reconnect? + // Just move into parsing mode and hope for now. + _state = NTRIPState::waiting_for_rtcm_header; } } - double lat = gcsPosition.latitude(); - double lng = gcsPosition.longitude(); - double alt = gcsPosition.altitude(); - - qCDebug(NTRIPLog) << "lat : " << lat << " lon : " << lng << " alt : " << alt; - - QString time = QDateTime::currentDateTimeUtc().toString("hhmmss.zzz"); - - if(lat != 0 || lng != 0) { - double latdms = (int) lat + (lat - (int) lat) * .6f; - double lngdms = (int) lng + (lng - (int) lng) * .6f; - if(isnan(alt)) alt = 0.0; - - QString line = QString("$GP%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14,%15") - .arg("GGA", time, - QString::number(qFabs(latdms * 100), 'f', 2), lat < 0 ? "S" : "N", - QString::number(qFabs(lngdms * 100), 'f', 2), lng < 0 ? "W" : "E", - "1", "10", "1", - QString::number(alt, 'f', 2), - "M", "0", "M", "0.0", "0"); - - // Calculrate checksum and send message - QString checkSum = _getCheckSum(line); - QString* nmeaMessage = new QString(line + "*" + checkSum + "\r\n"); - - // Write nmea message - if(_socket) { - _socket->write(nmeaMessage->toUtf8()); - } + if (_state == NTRIPState::uninitialised) { + qCDebug(NTRIPLog) << "NTRIP State is uninitialised. Discarding bytes"; + _socket->readAll(); + return; + } - qCDebug(NTRIPLog) << "NMEA Message : " << nmeaMessage->toUtf8(); - } + QByteArray bytes = _socket->readAll(); + _parse(bytes); } -QString NTRIPTCPLink::_getCheckSum(QString line) { - qCDebug(NTRIPLog) << "Calculating checksum"; - QByteArray temp_Byte = line.toUtf8(); - const char* buf = temp_Byte.constData(); +void NTRIPTCPLink::_sendNmeaGga() { + qCDebug(NTRIPLog) << "Sending NMEA GGA"; - char character; - int checksum = 0; + if (!_toolbox->multiVehicleManager()->activeVehicleAvailable()) { + qCDebug(NTRIPLog) << "No active vehicle"; + return; + } + qCDebug(NTRIPLog) << "Active vehicle found. Using vehicle position"; - for(int i = 0; i < line.length(); i++) { - character = buf[i]; - switch(character) { - case '$': - // Ignore the dollar sign - break; - case '*': - // Stop processing before the asterisk - i = line.length(); - continue; - default: - // First value for the checksum - if(checksum == 0) { - // Set the checksum to the value - checksum = character; - } - else { - // XOR the checksum with this character's value - checksum = checksum ^ character; - } - } - } + if (_toolbox->multiVehicleManager()->vehicles()->count() > 1) { + // TODO(bzd): Consider how to handle multiple vehicles - best option would be + // send separate RTCM messages for each vehicle or at least + // calculate the average position of all vehicles and use one RTCM for all + qCDebug(NTRIPLog) << "More than one vehicle found. Using active vehicle for correction"; + } - return QString("%1").arg(checksum, 0, 16); + Vehicle *vehicle = _toolbox->multiVehicleManager()->activeVehicle(); + NmeaMessage nmeaMessage(vehicle->coordinate()); + // Write NMEA message + if (_socket) { + const QString &gga = nmeaMessage.getGGA(); + _socket->write(gga.toUtf8()); + qCDebug(NTRIPLog) << "NMEA GGA Message : " << gga; + } } diff --git a/src/NTRIP/NTRIP.h b/src/NTRIP/NTRIP.h index 062a3eeae3b..df1d92ce36e 100644 --- a/src/NTRIP/NTRIP.h +++ b/src/NTRIP/NTRIP.h @@ -65,26 +65,23 @@ private slots: QString _mountpoint; QSet _whitelist; bool _isVRSEnable; + int _vrsSendRateMSecs = 3000; bool _ntripForceV1 = false; // QUrl QUrl _ntripURL; // Send NMEA - void _sendNMEA(); - QString _getCheckSum(QString line); + void _sendNmeaGga(); // VRS Timer QTimer* _vrsSendTimer; // this is perfectly fine to send VRS data every 30 seconds - static const int _vrsSendRateMSecs = 30000; RTCMParsing *_rtcm_parsing{nullptr}; NTRIPState _state; QGCToolbox* _toolbox = nullptr; - // TODO(zdanek): take from settings - bool _takePosFromVehicle = true; }; class NTRIP : public QGCTool { From 3c71d9e29420b92eb85fa4f66648fd3a41d35eb5 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Wed, 27 Nov 2024 12:50:43 +0100 Subject: [PATCH 5/7] Switch nmea message to be linked in all configs --- qgroundcontrol.pro | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 010fa4a681f..611f0b6a5b2 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -1072,11 +1072,12 @@ SOURCES += \ src/GPS/Drivers/src/sbf.cpp \ src/GPS/GPSManager.cc \ src/GPS/GPSProvider.cc \ - src/GPS/NmeaMessage.cc \ src/Joystick/JoystickSDL.cc \ src/RunGuard.cc \ } +SOURCES += src/GPS/NmeaMessage.cc + # # Firmware Plugin Support # From 5e4b81a8c8dfb0271ff519141d6700ec41727100 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Wed, 27 Nov 2024 12:55:03 +0100 Subject: [PATCH 6/7] Change GPS Drivers repo url temporarily to Zdanek --- src/GPS/Drivers | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/GPS/Drivers b/src/GPS/Drivers index a41210ede8c..47fdaa665a5 160000 --- a/src/GPS/Drivers +++ b/src/GPS/Drivers @@ -1 +1 @@ -Subproject commit a41210ede8c2d22dd8e9fdcf388fca927c1fc5e1 +Subproject commit 47fdaa665a59d319791b6c3bf408611205ecc0ec From 5425fbd5f0e317ee4c95293c3d69e5afbfdd40a5 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Thu, 28 Nov 2024 09:53:40 +0100 Subject: [PATCH 7/7] Temp fix to include serial for mobile --- qgroundcontrol.pro | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 611f0b6a5b2..b62a02b663c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -267,9 +267,10 @@ QT += \ AndroidBuild || iOSBuild { # Android and iOS don't unclude these } else { +} QT += \ serialport \ -} + contains(DEFINES, QGC_ENABLE_BLUETOOTH) { QT += \