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) };