Create UdpIODevice

qgroundcontrol.pro

@@ -620,6 +620,7 @@ HEADERS += \
     src/comm/QGCMAVLink.h \
     src/comm/TCPLink.h \
     src/comm/UDPLink.h \
+    src/comm/UdpIODevice.h \
     src/uas/UAS.h \
     src/uas/UASInterface.h \
     src/uas/UASMessageHandler.h \
@@ -820,6 +821,7 @@ SOURCES += \
     src/comm/QGCMAVLink.cc \
     src/comm/TCPLink.cc \
     src/comm/UDPLink.cc \
+    src/comm/UdpIODevice.cc \
     src/main.cc \
     src/uas/UAS.cc \
     src/uas/UASMessageHandler.cc \

src/comm/CMakeLists.txt

@@ -24,6 +24,7 @@ add_library(comm
 	SerialLink.cc
 	TCPLink.cc
 	UDPLink.cc
+	UdpIODevice.cc
 
 	${EXTRA_SRC}
 

src/comm/UdpIODevice.cc

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   (c) 2009-2018 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.
+ *
+ ****************************************************************************/
+
+#include "UdpIODevice.h"
+#include <algorithm>
+
+UdpIODevice::UdpIODevice(QObject *parent) : QUdpSocket(parent)
+{
+    // this might cause data to be available only after a second readyRead() signal
+    connect(this, &QUdpSocket::readyRead, this, &UdpIODevice::_readAvailableData);
+}
+
+bool UdpIODevice::canReadLine() const
+{
+    return _buffer.indexOf('\n') > -1;
+}
+
+qint64 UdpIODevice::readLineData(char *data, qint64 maxSize)
+{
+    int length = _buffer.indexOf('\n') + 1; // add 1 to include the '\n'
+    if (length == 0) {
+        return 0;
+    }
+    length = std::min(length, static_cast<int>(maxSize));
+    // copy lines to output
+    std::copy(_buffer.data(), _buffer.data() + length, data);
+    // trim buffer to remove consumed line
+    _buffer = _buffer.right(_buffer.size() - length);
+    // return number of bytes read
+    return length;
+}
+
+void UdpIODevice::_readAvailableData() {
+    while (hasPendingDatagrams()) {
+        int previousSize = _buffer.size();
+        _buffer.resize(static_cast<int>(_buffer.size() + pendingDatagramSize()));
+        readDatagram((_buffer.data() + previousSize), pendingDatagramSize());
+    }
+}

src/comm/UdpIODevice.h

@@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ *   (c) 2009-2018 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.
+ *
+ ****************************************************************************/
+#pragma once
+
+#include <QUdpSocket>
+
+/**
+ * @brief QUdpSocket implementation of canReadLine() readLineData() in server mode.
+ * The UdpIODevice class works almost exactly as a QUdpSocket, but
+ * also implements canReadLine() and readLineData() while in the bound state.
+ * Regular QUdpSocket only allows to use these QIODevice interfaces when using
+ * connectToHost(), which means it is working as a client instead of server.
+ *
+ **/
+
+class UdpIODevice: public QUdpSocket
+{
+    Q_OBJECT
+public:
+    UdpIODevice(QObject *parent = nullptr);
+    bool     canReadLine() const;
+    qint64   readLineData(char *data, qint64 maxSize);
+
+private slots:
+    void _readAvailableData();
+
+private:
+    QByteArray _buffer;
+};