From d1db6938a9151241cc0aafdd2f92dade9bd3e102 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Wed, 5 Dec 2018 10:19:25 -0200
Subject: [PATCH] Create UdpIODevice

---
 qgroundcontrol.pro      |  2 ++
 src/comm/CMakeLists.txt |  1 +
 src/comm/UdpIODevice.cc | 45 +++++++++++++++++++++++++++++++++++++++++
 src/comm/UdpIODevice.h  | 35 ++++++++++++++++++++++++++++++++
 4 files changed, 83 insertions(+)
 create mode 100644 src/comm/UdpIODevice.cc
 create mode 100644 src/comm/UdpIODevice.h

diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 05b6447f6..a11388963 100644
--- a/qgroundcontrol.pro
+++ b/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 \
diff --git a/src/comm/CMakeLists.txt b/src/comm/CMakeLists.txt
index 315774cc9..4a5fa752f 100644
--- a/src/comm/CMakeLists.txt
+++ b/src/comm/CMakeLists.txt
@@ -24,6 +24,7 @@ add_library(comm
 	SerialLink.cc
 	TCPLink.cc
 	UDPLink.cc
+	UdpIODevice.cc
 
 	${EXTRA_SRC}
 
diff --git a/src/comm/UdpIODevice.cc b/src/comm/UdpIODevice.cc
new file mode 100644
index 000000000..7f85dd601
--- /dev/null
+++ b/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());
+    }
+}
diff --git a/src/comm/UdpIODevice.h b/src/comm/UdpIODevice.h
new file mode 100644
index 000000000..2ccd8a8af
--- /dev/null
+++ b/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;
+};
-- 
2.22.0