Merge pull request #4955 from DonLakeFlyer/RTK

RTK fixes

src/GPS/GPSManager.cc

@@ -9,7 +9,7 @@
 
 
 #include "GPSManager.h"
-#include <QDebug>
+#include "QGCLoggingCategory.h"
 
 GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
     : QGCTool(app, toolbox)
@@ -35,26 +35,21 @@ void GPSManager::connectGPS(const QString& device)
     //create RTCM device
     _rtcmMavlink = new RTCMMavlink(*_toolbox);
 
-    connect(_gpsProvider, SIGNAL(RTCMDataUpdate(QByteArray)), _rtcmMavlink,
-            SLOT(RTCMDataUpdate(QByteArray)));
+    connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate);
 
     //test: connect to position update
-    connect(_gpsProvider, SIGNAL(positionUpdate(GPSPositionMessage)), this,
-            SLOT(GPSPositionUpdate(GPSPositionMessage)));
-    connect(_gpsProvider, SIGNAL(satelliteInfoUpdate(GPSSatelliteMessage)), this,
-            SLOT(GPSSatelliteUpdate(GPSSatelliteMessage)));
+    connect(_gpsProvider, &GPSProvider::positionUpdate, this, &GPSManager::GPSPositionUpdate);
+    connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate, this, &GPSManager::GPSSatelliteUpdate);
 
 }
 
 void GPSManager::GPSPositionUpdate(GPSPositionMessage msg)
 {
-    qDebug("GPS: got position update: alt=%i, long=%i, lat=%i",
-            msg.position_data.alt, msg.position_data.lon,
-            msg.position_data.lat);
+    qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat);
 }
 void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg)
 {
-    qDebug("GPS: got satellite info update, %i satellites", (int)msg.satellite_data.count);
+    qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count);
 }
 
 void GPSManager::cleanup()

src/GPS/GPSProvider.cc

@@ -9,6 +9,7 @@
 
 
 #include "GPSProvider.h"
+#include "QGCLoggingCategory.h"
 
 #define GPS_RECEIVE_TIMEOUT 1200
 
@@ -96,7 +97,7 @@ void GPSProvider::run()
             }
         }
     }
-    qDebug() << "Exiting GPS thread";
+    qCDebug(RTKGPSLog) << "Exiting GPS thread";
 }
 
 GPSProvider::GPSProvider(const QString& device, bool enableSatInfo, const std::atomic_bool& requestStop)
@@ -165,8 +166,7 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
         case GPSCallbackType::surveyInStatus:
         {
             SurveyInStatus* status = (SurveyInStatus*)data1;
-            qInfo("Survey-in status: %is cur accuracy: %imm valid: %i active: %i",
-                status->duration, status->mean_accuracy, (int)(status->flags & 1), (int)((status->flags>>1) & 1));
+            qCDebug(RTKGPSLog) << QString("Survey-in status: %1s cur accuracy: %2mm valid: %3 active: %4").arg(status->duration).arg(status->mean_accuracy).arg((int)(status->flags & 1)).arg((int)((status->flags>>1) & 1));
         }
             break;
 

src/GPS/RTCM/RTCMMavlink.cc

@@ -41,16 +41,25 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message)
         memcpy(&mavlinkRtcmData.data, message.data(), message.size());
         sendMessageToVehicle(mavlinkRtcmData);
     } else {
-        //we need to fragment
+        // We need to fragment
+
+        static uint8_t sequenceId = 0;  // Sequence id is used to indicate that the individual fragements belong to the same set
+        uint8_t fragmentId = 0;         // Fragment id indicates the fragement within a set
+
         int start = 0;
         while (start < message.size()) {
             int length = std::min(message.size() - start, maxMessageLength);
-            mavlinkRtcmData.flags = 1; //fragmented
+            mavlinkRtcmData.flags = 1;                      // LSB set indicates messsage is fragmented
+            mavlinkRtcmData.flags |= fragmentId++ << 1;     // Next 2 bits are fragment id
+            mavlinkRtcmData.flags |= sequenceId++ << 3;     // Next 5 bits are sequence id
             mavlinkRtcmData.len = length;
             memcpy(&mavlinkRtcmData.data, message.data() + start, length);
             sendMessageToVehicle(mavlinkRtcmData);
             start += length;
         }
+        if (sequenceId == 0x1F) {
+            sequenceId = 0;
+        }
     }
 }
 

src/QGCLoggingCategory.cc

@@ -22,6 +22,7 @@ QGC_LOGGING_CATEGORY(MissionCommandsLog,        "MissionCommandsLog")
 QGC_LOGGING_CATEGORY(MissionItemLog,            "MissionItemLog")
 QGC_LOGGING_CATEGORY(ParameterManagerLog,       "ParameterManagerLog")
 QGC_LOGGING_CATEGORY(GeotaggingLog,             "GeotaggingLog")
+QGC_LOGGING_CATEGORY(RTKGPSLog,                 "RTKGPSLog")
 
 QGCLoggingCategoryRegister* _instance = NULL;
 const char* QGCLoggingCategoryRegister::_filterRulesSettingsGroup = "LoggingFilters";

src/QGCLoggingCategory.h

@@ -24,6 +24,7 @@ Q_DECLARE_LOGGING_CATEGORY(MissionCommandsLog)
 Q_DECLARE_LOGGING_CATEGORY(MissionItemLog)
 Q_DECLARE_LOGGING_CATEGORY(ParameterManagerLog)
 Q_DECLARE_LOGGING_CATEGORY(GeotaggingLog)
+Q_DECLARE_LOGGING_CATEGORY(RTKGPSLog)
 
 /// @def QGC_LOGGING_CATEGORY
 /// This is a QGC specific replacement for Q_LOGGING_CATEGORY. It will register the category name into a

src/comm/USBBoardInfo.json

@@ -21,7 +21,7 @@
         { "vendorID": 1027, "productID": 24597,     "boardClass": "SiK Radio",  "name": "SiK Radio",            "comment": "3DR Radio" },
         { "vendorID": 4292, "productID": 600000,    "boardClass": "SiK Radio",  "name": "SiK Radio",            "comment": "SILabs Radio" },
 
-        { "vendorID": 546, "productID": 424,        "boardClass": "RTK GPS",    "name": "RTK GPS",              "comment": "Ublox RTK GPS" },
+        { "vendorID": 5446, "productID": 424,        "boardClass": "RTK GPS",    "name": "RTK GPS",              "comment": "Ublox RTK GPS" },
 
         { "vendorID": 8352, "productID": 16732,     "boardClass": "OpenPilot",  "name": "OpenPilot OPLink" },
         { "vendorID": 8352, "productID": 16733,     "boardClass": "OpenPilot",  "name": "OpenPilot CC3D" },