Adding initial support for NTRIP / TCP source of RTCM data. #9493
+517
−4
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,17 @@ | ||
|
|
||
| add_library(NTRIP | ||
| NTRIP.cc | ||
| NTRIP.h | ||
| ) | ||
|
|
||
| target_link_libraries(NTRIP | ||
| PUBLIC | ||
| qgc | ||
| ) | ||
|
|
||
| target_include_directories(NTRIP | ||
| PUBLIC | ||
| ${CMAKE_CURRENT_SOURCE_DIR} | ||
| ) | ||
|
|
||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,172 @@ | ||
| /**************************************************************************** | ||
| * | ||
| * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org> | ||
| * | ||
| * 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 "QGCApplication.h" | ||
| #include "SettingsManager.h" | ||
| #include "NTRIPSettings.h" | ||
|
|
||
| #include <QDebug> | ||
|
|
||
| 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()) { | ||
| _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(), | ||
| this); | ||
| connect(_tcpLink, &NTRIPTCPLink::error, this, &NTRIP::_tcpError, Qt::QueuedConnection); | ||
| connect(_tcpLink, &NTRIPTCPLink::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); | ||
| } | ||
| } | ||
|
|
||
|
|
||
| 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, | ||
| QObject* parent) | ||
| : QThread (parent) | ||
| , _hostAddress (hostAddress) | ||
| , _port (port) | ||
| , _username (username) | ||
| , _password (password) | ||
| , _mountpoint (mountpoint) | ||
| { | ||
| moveToThread(this); | ||
| start(); | ||
|
There was a problem hiding this comment. You need to start at the end of initialization, otherwise there are race conditions |
||
| for(const auto& msg: whitelist.split(',')){ | ||
| int msg_int = msg.toInt(); | ||
| if(msg_int) | ||
|
There was a problem hiding this comment. In some parts of the code there is single line ifs with brackets and others without, be consistent in the some code base. |
||
| _whitelist.append(msg_int); | ||
| } | ||
| qCDebug(NTRIPLog) << "whitelist: " << _whitelist; | ||
| if (!_rtcm_parsing) { | ||
| _rtcm_parsing = new RTCMParsing(); | ||
| } | ||
| _rtcm_parsing->reset(); | ||
| _state = NTRIPState::uninitialised; | ||
| } | ||
|
|
||
| NTRIPTCPLink::~NTRIPTCPLink(void) | ||
| { | ||
| if (_socket) { | ||
| QObject::disconnect(_socket, &QTcpSocket::readyRead, this, &NTRIPTCPLink::_readBytes); | ||
| _socket->disconnectFromHost(); | ||
| _socket->deleteLater(); | ||
| _socket = nullptr; | ||
| } | ||
| quit(); | ||
| wait(); | ||
|
Comment on lines
+87
to
+88
There was a problem hiding this comment. Same here: quit & wait first, then cleanup. |
||
| } | ||
|
|
||
| void NTRIPTCPLink::run(void) | ||
| { | ||
| _hardwareConnect(); | ||
| exec(); | ||
| } | ||
|
|
||
| void NTRIPTCPLink::_hardwareConnect() | ||
| { | ||
| _socket = new QTcpSocket(); | ||
|
|
||
| QObject::connect(_socket, &QTcpSocket::readyRead, this, &NTRIPTCPLink::_readBytes); | ||
|
|
||
| _socket->connectToHost(_hostAddress, static_cast<quint16>(_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"; | ||
| QString auth = QString(_username + ":" + _password).toUtf8().toBase64(); | ||
|
There was a problem hiding this comment. QUrl can also be used for it |
||
| 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; | ||
| } | ||
| // If no mountpoint is set, assume we will just get data from the tcp stream | ||
| else{ | ||
| _state = NTRIPState::waiting_for_rtcm_header; | ||
| } | ||
| qCDebug(NTRIPLog) << "NTRIP Socket connected"; | ||
| } | ||
|
|
||
| void NTRIPTCPLink::_parse(const QByteArray &buffer) | ||
| { | ||
| 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<int>(_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) | ||
| { | ||
| if (_socket) { | ||
|
There was a problem hiding this comment. To avoid the extra indentation level |
||
| if(_state == NTRIPState::waiting_for_http_response){ | ||
| QString line = _socket->readLine(); | ||
| if (line.contains("200")){ | ||
| _state = NTRIPState::waiting_for_rtcm_header; | ||
| } | ||
| 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); | ||
| } | ||
| } | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change | ||||
|---|---|---|---|---|---|---|
| @@ -0,0 +1,92 @@ | ||||||
| /**************************************************************************** | ||||||
| * | ||||||
| * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org> | ||||||
| * | ||||||
| * QGroundControl is licensed according to the terms in the file | ||||||
| * COPYING.md in the root of the source code directory. | ||||||
| * | ||||||
| ****************************************************************************/ | ||||||
|
|
||||||
| #pragma once | ||||||
|
|
||||||
| #include "QGCToolbox.h" | ||||||
| #include "QmlObjectListModel.h" | ||||||
|
|
||||||
| #include <QThread> | ||||||
| #include <QTcpSocket> | ||||||
| #include <QTimer> | ||||||
|
There was a problem hiding this comment. QTimer appears to be not used |
||||||
| #include <QGeoCoordinate> | ||||||
|
There was a problem hiding this comment. The same things appears to happen with |
||||||
|
|
||||||
| #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, | ||||||
| QObject* parent); | ||||||
| ~NTRIPTCPLink(); | ||||||
|
|
||||||
| signals: | ||||||
| void error(const QString errorMsg); | ||||||
| void RTCMDataUpdate(QByteArray message); | ||||||
|
|
||||||
| protected: | ||||||
| void run(void) final; | ||||||
|
There was a problem hiding this comment. just a tip, |
||||||
|
|
||||||
| private slots: | ||||||
| void _readBytes(void); | ||||||
|
|
||||||
| 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; | ||||||
| QVector<int> _whitelist; | ||||||
|
There was a problem hiding this comment.
Suggested change
|
||||||
|
|
||||||
| RTCMParsing *_rtcm_parsing{nullptr}; | ||||||
| NTRIPState _state; | ||||||
| }; | ||||||
|
|
||||||
| 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; | ||||||
| }; | ||||||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why it's not using the QUrl object, it should provide all these fields for you.