From 0a99aad48a0d9530640c26fb1855a9e689a14f97 Mon Sep 17 00:00:00 2001 From: "Bartek Zdanowski (Zdanek)" Date: Mon, 25 Nov 2024 15:11:30 +0100 Subject: [PATCH] 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 {