Merge pull request #3457 from bkueng/mavlink_rtcm_message

GPS RTK: use GPS_RTCM_DATA mavlink message

src/GPS/Drivers

@@ -1 +1 @@
-Subproject commit 60739aaace1723c700f23b22212696dc75169559
+Subproject commit 5c1ae956552c3887c5fddd303a8f242efa715333

src/GPS/GPSProvider.cc

@@ -18,9 +18,25 @@
 #include "Drivers/src/gps_helper.h"
 #include "definitions.h"
 
+//#define SIMULATE_RTCM_OUTPUT //if defined, generate simulated RTCM messages
+                               //additionally make sure to call connectGPS(""), eg. from QGCToolbox.cc
+
 
 void GPSProvider::run()
 {
+#ifdef SIMULATE_RTCM_OUTPUT
+        const int fakeMsgLengths[3] = { 30, 170, 240 };
+        uint8_t* fakeData = new uint8_t[fakeMsgLengths[2]];
+        while (!_requestStop) {
+            for (int i = 0; i < 3; ++i) {
+                gotRTCMData((uint8_t*) fakeData, fakeMsgLengths[i]);
+                msleep(4);
+            }
+            msleep(100);
+        }
+        delete[] fakeData;
+#endif /* SIMULATE_RTCM_OUTPUT */
+
     if (_serial) delete _serial;
 
     _serial = new QSerialPort();
@@ -125,10 +141,11 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
 {
     switch (type) {
         case GPSCallbackType::readDeviceData: {
-            int timeout = *((int *) data1);
-            if (!_serial->waitForReadyRead(timeout))
-                return 0; //timeout
-            msleep(10); //give some more time to buffer data
+            if (_serial->bytesAvailable() == 0) {
+                int timeout = *((int *) data1);
+                if (!_serial->waitForReadyRead(timeout))
+                    return 0; //timeout
+            }
             return (int)_serial->read((char*) data1, data2);
         }
         case GPSCallbackType::writeDeviceData:

src/GPS/RTCM/RTCMMavlink.cc

@@ -11,7 +11,6 @@
 #include "RTCMMavlink.h"
 
 #include "MultiVehicleManager.h"
-#include "MAVLinkProtocol.h"
 #include "Vehicle.h"
 
 #include <cstdio>
@@ -24,8 +23,6 @@ RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox)
 
 void RTCMMavlink::RTCMDataUpdate(QByteArray message)
 {
-    Q_ASSERT(message.size() <= 110); //mavlink message uses a fixed-size buffer
-
     /* statistics */
     _bandwidthByteCounter += message.size();
     qint64 elapsed = _bandwidthTimer.elapsed();
@@ -34,19 +31,38 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message)
         _bandwidthTimer.restart();
         _bandwidthByteCounter = 0;
     }
-    QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
-    MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
-    mavlink_gps_inject_data_t mavlinkRtcmData;
-    memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_inject_data_t));
 
-    mavlinkRtcmData.len = message.size();
-    memcpy(&mavlinkRtcmData.data, message.data(), message.size());
+    const int maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
+    mavlink_gps_rtcm_data_t mavlinkRtcmData;
+    memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t));
 
+    if (message.size() < maxMessageLength) {
+        mavlinkRtcmData.len = message.size();
+        memcpy(&mavlinkRtcmData.data, message.data(), message.size());
+        sendMessageToVehicle(mavlinkRtcmData);
+    } else {
+        //we need to fragment
+        int start = 0;
+        while (start < message.size()) {
+            int length = std::min(message.size() - start, maxMessageLength);
+            mavlinkRtcmData.flags = 1; //fragmented
+            mavlinkRtcmData.len = length;
+            memcpy(&mavlinkRtcmData.data, message.data() + start, length);
+            sendMessageToVehicle(mavlinkRtcmData);
+            start += length;
+        }
+    }
+}
+
+void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
+{
+    QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
+    MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
     for (int i = 0; i < vehicles.count(); i++) {
         Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
         mavlink_message_t message;
-        mavlink_msg_gps_inject_data_encode(mavlinkProtocol->getSystemId(),
-                mavlinkProtocol->getComponentId(), &message, &mavlinkRtcmData);
+        mavlink_msg_gps_rtcm_data_encode(mavlinkProtocol->getSystemId(),
+                mavlinkProtocol->getComponentId(), &message, &msg);
         vehicle->sendMessage(message);
     }
 }

src/GPS/RTCM/RTCMMavlink.h

@@ -14,6 +14,7 @@
 #include <QElapsedTimer>
 
 #include "QGCToolbox.h"
+#include "MAVLinkProtocol.h"
 
 /**
  ** class RTCMMavlink
@@ -30,6 +31,8 @@ public slots:
     void RTCMDataUpdate(QByteArray message);
 
 private:
+    void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg);
+
     QGCToolbox& _toolbox;
     QElapsedTimer _bandwidthTimer;
     int _bandwidthByteCounter = 0;

src/GPS/definitions.h

@@ -41,6 +41,8 @@
 
 #include <QtGlobal>
 
+#define GPS_READ_BUFFER_SIZE 1024
+
 #define GPS_INFO(...) qInfo(__VA_ARGS__)
 #define GPS_WARN(...) qWarning(__VA_ARGS__)
 #define GPS_ERR(...) qCritical(__VA_ARGS__)
@@ -59,7 +61,9 @@ public:
     static void usleep(unsigned long usecs) { QThread::usleep(usecs); }
 };
 
-#define usleep Sleeper::usleep
+static inline void usleep(unsigned long usecs) {
+    Sleeper::usleep(usecs);
+}
 
 
 typedef uint64_t gps_abstime;

src/Vehicle/GPSFact.json

@@ -25,8 +25,8 @@
             "name":             "lock",
             "shortDescription": "GPS Lock",
             "type":             "uint32",
-            "enumStrings":      "None,None,2D Lock,3D Lock,3D DGPS Lock,3D RTK GPS Lock",
-            "enumValues":       "0,1,2,3,4,5",
+            "enumStrings":      "None,None,2D Lock,3D Lock,3D DGPS Lock,3D RTK GPS Lock (float),3D RTK GPS Lock (fixed)",
+            "enumValues":       "0,1,2,3,4,5,6",
             "decimalPlaces":    0
         },
         {