Merge pull request #1683 from dogmaphobic/udpOwnEventLoop

Fixing Qt event loop issue where UDP readReady signal would not work.

src/comm/UDPLink.cc

@@ -28,6 +28,13 @@ This file is part of the QGROUNDCONTROL project
  *
  */
 
+#include <QtGlobal>
+#if QT_VERSION > 0x050401
+#define UDP_BROKEN_SIGNAL 1
+#else
+#define UDP_BROKEN_SIGNAL 0
+#endif
+
 #include <QTimer>
 #include <QList>
 #include <QDebug>
@@ -80,6 +87,7 @@ UDPLink::UDPLink(UDPConfiguration* config)
     #if defined(QGC_ZEROCONF_ENABLED)
     , _dnssServiceRef(NULL)
     #endif
+    , _running(false)
 {
     Q_ASSERT(config != NULL);
     _config = config;
@@ -89,7 +97,7 @@ UDPLink::UDPLink(UDPConfiguration* config)
     // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
     moveToThread(this);
 
-    qDebug() << "UDP Created " << _config->name();
+    //qDebug() << "UDP Created " << _config->name();
 }
 
 UDPLink::~UDPLink()
@@ -98,9 +106,13 @@ UDPLink::~UDPLink()
     _config->setLink(NULL);
     _disconnect();
     // Tell the thread to exit
+    _running = false;
     quit();
     // Wait for it to exit
     wait();
+    while(_outQueue.count() > 0) {
+        delete _outQueue.dequeue();
+    }
     this->deleteLater();
 }
 
@@ -110,8 +122,27 @@ UDPLink::~UDPLink()
  **/
 void UDPLink::run()
 {
-    _hardwareConnect();
-    exec();
+    if(_hardwareConnect()) {
+        if(UDP_BROKEN_SIGNAL) {
+            bool loop = false;
+            while(true) {
+                //-- Anything to read?
+                loop = _socket->hasPendingDatagrams();
+                if(loop) {
+                    readBytes();
+                }
+                //-- Loop right away if busy
+                if((_dequeBytes() || loop) && _running)
+                    continue;
+                if(!_running)
+                    break;
+                //-- Settle down (it gets here if there is nothing to read or write)
+                this->msleep(50);
+            }
+        } else {
+            exec();
+        }
+    }
     if (_socket) {
         _deregisterZeroconf();
         _socket->close();
@@ -142,45 +173,55 @@ void UDPLink::removeHost(const QString& host)
     _config->removeHost(host);
 }
 
-#define UDPLINK_DEBUG 0
-
 void UDPLink::writeBytes(const char* data, qint64 size)
 {
     if (!_socket) {
         return;
     }
+    if(UDP_BROKEN_SIGNAL) {
+        QByteArray* qdata = new QByteArray(data, size);
+        QMutexLocker lock(&_mutex);
+        _outQueue.enqueue(qdata);
+    } else {
+        _sendBytes(data, size);
+    }
+}
+
 
-    // Broadcast to all connected systems
+bool UDPLink::_dequeBytes()
+{
+    QMutexLocker lock(&_mutex);
+    if(_outQueue.count() > 0) {
+        QByteArray* qdata = _outQueue.dequeue();
+        lock.unlock();
+        _sendBytes(qdata->data(), qdata->size());
+        delete qdata;
+        lock.relock();
+    }
+    return (_outQueue.count() > 0);
+}
+
+void UDPLink::_sendBytes(const char* data, qint64 size)
+{
+    QStringList goneHosts;
+    // Send to all connected systems
     QString host;
     int port;
     if(_config->firstHost(host, port)) {
         do {
-            if(UDPLINK_DEBUG) {
-                QString bytes;
-                QString ascii;
-                for (int i=0; i<size; i++)
-                {
-                    unsigned char v = data[i];
-                    bytes.append(QString().sprintf("%02x ", v));
-                    if (data[i] > 31 && data[i] < 127)
-                    {
-                        ascii.append(data[i]);
-                    }
-                    else
-                    {
-                        ascii.append(219);
-                    }
-                }
-                qDebug() << "Sent" << size << "bytes to" << host << ":" << port << "data:";
-                qDebug() << bytes;
-                qDebug() << "ASCII:" << ascii;
-            }
             QHostAddress currentHost(host);
-            _socket->writeDatagram(data, size, currentHost, (quint16)port);
+            if(_socket->writeDatagram(data, size, currentHost, (quint16)port) < 0) {
+                // This host is gone. Add to list to be removed
+                goneHosts.append(host);
+            }
             // Log the amount and time written out for future data rate calculations.
             QMutexLocker dataRateLocker(&dataRateMutex);
             logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch());
         } while (_config->nextHost(host, port));
+        //-- Remove hosts that are no longer there
+        foreach (QString ghost, goneHosts) {
+            _config->removeHost(ghost);
+        }
     }
 }
 
@@ -220,6 +261,8 @@ void UDPLink::readBytes()
         // would trigger this.
         // Add host to broadcast list if not yet present, or update its port
         _config->addHost(sender.toString(), (int)senderPort);
+        if(UDP_BROKEN_SIGNAL && !_running)
+            break;
     }
 }
 
@@ -230,17 +273,17 @@ void UDPLink::readBytes()
  **/
 bool UDPLink::_disconnect(void)
 {
-    this->quit();
-    this->wait();
+    _running = false;
+    quit();
+    wait();
     if (_socket) {
         // Make sure delete happen on correct thread
         _socket->deleteLater();
         _socket = NULL;
         emit disconnected();
     }
-    // TODO When would this ever return false?
     _connectState = false;
-    return !_connectState;
+    return true;
 }
 
 /**
@@ -250,16 +293,15 @@ bool UDPLink::_disconnect(void)
  **/
 bool UDPLink::_connect(void)
 {
-    if(this->isRunning())
+    if(this->isRunning() || _running)
     {
-        this->quit();
-        this->wait();
+        _running = false;
+        quit();
+        wait();
     }
-    // TODO When would this ever return false?
-    bool connected = true;
-    // I see no reason to run this in "HighPriority"
+    _running = true;
     start(NormalPriority);
-    return connected;
+    return true;
 }
 
 bool UDPLink::_hardwareConnect()
@@ -268,13 +310,16 @@ bool UDPLink::_hardwareConnect()
         delete _socket;
         _socket = NULL;
     }
-    QHostAddress host = QHostAddress::Any;
+    QHostAddress host = QHostAddress::AnyIPv4;
     _socket = new QUdpSocket();
     _socket->setProxy(QNetworkProxy::NoProxy);
     _connectState = _socket->bind(host, _config->localPort(), QAbstractSocket::ReuseAddressHint);
     if (_connectState) {
         _registerZeroconf(_config->localPort(), kZeroconfRegistration);
-        QObject::connect(_socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
+        //-- Connect signal if this version of Qt is not broken
+        if(!UDP_BROKEN_SIGNAL) {
+            QObject::connect(_socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
+        }
         emit connected();
     } else {
         emit communicationError("UDP Link Error", "Error binding UDP port");
@@ -423,7 +468,10 @@ void UDPConfiguration::removeHost(const QString& host)
     tHost = tHost.trimmed();
     QMap<QString, int>::iterator i = _hosts.find(tHost);
     if(i != _hosts.end()) {
+        //qDebug() << "UDP:" << "Removed host:" << host;
         _hosts.erase(i);
+    } else {
+        qWarning() << "UDP:" << "Could not remove unknown host:" << host;
     }
 }
 

src/comm/UDPLink.h

@@ -36,6 +36,9 @@ This file is part of the QGROUNDCONTROL project
 #include <QMap>
 #include <QMutex>
 #include <QUdpSocket>
+#include <QMutexLocker>
+#include <QQueue>
+#include <QByteArray>
 
 #if defined(QGC_ZEROCONF_ENABLED)
 #include <dns_sd.h>
@@ -212,8 +215,12 @@ private:
     DNSServiceRef  _dnssServiceRef;
 #endif
 
-signals:
-    //Signals are defined by LinkInterface
+    bool                _running;
+    QMutex              _mutex;
+    QQueue<QByteArray*> _outQueue;
+
+    bool _dequeBytes    ();
+    void _sendBytes     (const char* data, qint64 size);
 
 };