Improve RTCM connection

mavlink · Nov 8, 2024 · df3a3a9 · df3a3a9
1 parent 42b9971
commit df3a3a9
Show file tree

Hide file tree

Showing 7 changed files with 128 additions and 48 deletions.
diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ * (c) 2009-2024 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.
@@ -15,6 +15,8 @@
 
 #include <cstdio>
 
+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
@@ -13,8 +13,11 @@
 #include <QObject>
 #include <QElapsedTimer>
 
-#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
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ * (c) 2009-2024 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.
@@ -15,8 +15,6 @@
 #include "PositionManager.h"
 #include "NTRIPSettings.h"
 
-#include <QDebug>
-
 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<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::_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<int>(_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
@@ -63,6 +63,7 @@ private slots:
     QString         _mountpoint;
     QSet<int>       _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
@@ -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
@@ -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
@@ -26,4 +26,5 @@ class NTRIPSettings : public SettingsGroup
     DEFINE_SETTINGFACT(ntripMountpoint)
     DEFINE_SETTINGFACT(ntripWhitelist)
     DEFINE_SETTINGFACT(ntripEnableVRS)
+    DEFINE_SETTINGFACT(ntripForceNtripV1)
 };