diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bfbfa444c77..611f0b6a5b2 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,17 +1067,17 @@ 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 \ } +SOURCES += src/GPS/NmeaMessage.cc + # # Firmware Plugin Support # 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/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/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 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/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index 35113793c61..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) { - /* 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/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..8f5ce84a37f --- /dev/null +++ b/src/NTRIP/NTRIP.cc @@ -0,0 +1,275 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#include "NTRIP.h" +#include "NTRIPSettings.h" +#include "NmeaMessage.h" +#include "PositionManager.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "QGCToolbox.h" +#include "SettingsManager.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"; + } +} + +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::_sendNmeaGga); + 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::_sendNmeaGga); + _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 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 { + 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"; + } + + _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) { + 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; + } + + 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) { + qCDebug(NTRIPLog) << "Reading bytes"; + 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("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; + } + } + + if (_state == NTRIPState::uninitialised) { + qCDebug(NTRIPLog) << "NTRIP State is uninitialised. Discarding bytes"; + _socket->readAll(); + return; + } + + QByteArray bytes = _socket->readAll(); + _parse(bytes); +} + +void NTRIPTCPLink::_sendNmeaGga() { + qCDebug(NTRIPLog) << "Sending NMEA GGA"; + + if (!_toolbox->multiVehicleManager()->activeVehicleAvailable()) { + qCDebug(NTRIPLog) << "No active vehicle"; + return; + } + qCDebug(NTRIPLog) << "Active vehicle found. Using vehicle position"; + + 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"; + } + + 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 new file mode 100644 index 00000000000..df1d92ce36e --- /dev/null +++ b/src/NTRIP/NTRIP.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * (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" + +Q_DECLARE_LOGGING_CATEGORY(NTRIPLog) + +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; + int _vrsSendRateMSecs = 3000; + bool _ntripForceV1 = false; + + // QUrl + QUrl _ntripURL; + + // Send NMEA + void _sendNmeaGga(); + + // VRS Timer + QTimer* _vrsSendTimer; + // this is perfectly fine to send VRS data every 30 seconds + + 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.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..61918be6871 --- /dev/null +++ b/src/Settings/NTRIP.SettingsGroup.json @@ -0,0 +1,83 @@ +{ + "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 ID Whitelist, comma separated", + "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 +}, +{ + "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 new file mode 100644 index 00000000000..e5732d63eb7 --- /dev/null +++ b/src/Settings/NTRIPSettings.cc @@ -0,0 +1,28 @@ +/**************************************************************************** + * + * (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) +DECLARE_SETTINGSFACT(NTRIPSettings, ntripForceNtripV1) diff --git a/src/Settings/NTRIPSettings.h b/src/Settings/NTRIPSettings.h new file mode 100644 index 00000000000..c45721b00b2 --- /dev/null +++ b/src/Settings/NTRIPSettings.h @@ -0,0 +1,30 @@ +/**************************************************************************** + * + * (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) + DEFINE_SETTINGFACT(ntripForceNtripV1) +}; 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