UDPLink.cc 15.6 KB
Newer Older
1
2
3
4
5
6
7
8
/****************************************************************************
 *
 *   (c) 2009-2016 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.
 *
 ****************************************************************************/
pixhawk's avatar
pixhawk committed
9
10
11
12
13
14
15
16
17


/**
 * @file
 *   @brief Definition of UDP connection (server) for unmanned vehicles
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

18
#include <QtGlobal>
pixhawk's avatar
pixhawk committed
19
20
21
22
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
23
#include <QNetworkProxy>
24
#include <QNetworkInterface>
pixhawk's avatar
pixhawk committed
25
#include <iostream>
26
#include <QHostInfo>
27

pixhawk's avatar
pixhawk committed
28
#include "UDPLink.h"
29
#include "QGC.h"
30
31
32
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "AutoConnectSettings.h"
pixhawk's avatar
pixhawk committed
33

34
35
#define REMOVE_GONE_HOSTS 0

36
37
static const char* kZeroconfRegistration = "_qgroundcontrol._udp";

38
39
40
static bool is_ip(const QString& address)
{
    int a,b,c,d;
41
42
    if (sscanf(address.toStdString().c_str(), "%d.%d.%d.%d", &a, &b, &c, &d) != 4
            && strcmp("::1", address.toStdString().c_str())) {
43
        return false;
44
45
46
    } else {
        return true;
    }
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
}

static QString get_ip_address(const QString& address)
{
    if(is_ip(address))
        return address;
    // Need to look it up
    QHostInfo info = QHostInfo::fromName(address);
    if (info.error() == QHostInfo::NoError)
    {
        QList<QHostAddress> hostAddresses = info.addresses();
        QHostAddress address;
        for (int i = 0; i < hostAddresses.size(); i++)
        {
            // Exclude all IPv6 addresses
            if (!hostAddresses.at(i).toString().contains(":"))
            {
                return hostAddresses.at(i).toString();
            }
        }
    }
    return QString("");
}

71
72
UDPLink::UDPLink(SharedLinkConfigurationPointer& config)
    : LinkInterface(config)
DonLakeFlyer's avatar
DonLakeFlyer committed
73
    #if defined(QGC_ZEROCONF_ENABLED)
74
    , _dnssServiceRef(NULL)
DonLakeFlyer's avatar
DonLakeFlyer committed
75
    #endif
76
    , _running(false)
77
78
79
    , _socket(NULL)
    , _udpConfig(qobject_cast<UDPConfiguration*>(config.data()))
    , _connectState(false)
pixhawk's avatar
pixhawk committed
80
{
DonLakeFlyer's avatar
DonLakeFlyer committed
81
82
83
    if (!_udpConfig) {
        qWarning() << "Internal error";
    }
84
    moveToThread(this);
pixhawk's avatar
pixhawk committed
85
86
87
88
}

UDPLink::~UDPLink()
{
89
    _disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
90
    // Tell the thread to exit
91
    _running = false;
92
    quit();
Lorenz Meier's avatar
Lorenz Meier committed
93
94
    // Wait for it to exit
    wait();
95
    this->deleteLater();
pixhawk's avatar
pixhawk committed
96
97
98
99
100
101
102
103
}

/**
 * @brief Runs the thread
 *
 **/
void UDPLink::run()
{
104
    if(_hardwareConnect()) {
105
        exec();
106
    }
107
    if (_socket) {
108
        _deregisterZeroconf();
109
110
        _socket->close();
    }
pixhawk's avatar
pixhawk committed
111
112
}

113
void UDPLink::_restartConnection()
pixhawk's avatar
pixhawk committed
114
{
115
116
117
118
119
    if(this->isConnected())
    {
        _disconnect();
        _connect();
    }
pixhawk's avatar
pixhawk committed
120
121
}

122
QString UDPLink::getName() const
pixhawk's avatar
pixhawk committed
123
{
124
    return _udpConfig->name();
125
126
127
128
}

void UDPLink::addHost(const QString& host)
{
129
    _udpConfig->addHost(host);
130
131
}

132
void UDPLink::removeHost(const QString& host)
133
{
134
    _udpConfig->removeHost(host);
pixhawk's avatar
pixhawk committed
135
136
}

137
void UDPLink::_writeBytes(const QByteArray data)
138
{
139
140
141
    if (!_socket)
        return;

142
143
144
145
    QStringList goneHosts;
    // Send to all connected systems
    QString host;
    int port;
146
    if(_udpConfig->firstHost(host, port)) {
147
148
        do {
            QHostAddress currentHost(host);
149
            if(_socket->writeDatagram(data, currentHost, (quint16)port) < 0) {
150
                // This host is gone. Add to list to be removed
151
152
153
154
155
156
157
158
159
160
161
162
                // We should keep track of hosts that were manually added (static) and
                // hosts that were added because we heard from them (dynamic). Only
                // dynamic hosts should be removed and even then, after a few tries, not
                // the first failure. In the mean time, we don't remove anything.
                if(REMOVE_GONE_HOSTS) {
                    goneHosts.append(host);
                }
            } else {
                // Only log rate if data actually got sent. Not sure about this as
                // "host not there" takes time too regardless of size of data. In fact,
                // 1 byte or "UDP frame size" bytes are the same as that's the data
                // unit sent by UDP.
163
                _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
164
            }
165
        } while (_udpConfig->nextHost(host, port));
166
        //-- Remove hosts that are no longer there
167
        foreach (const QString& ghost, goneHosts) {
168
            _udpConfig->removeHost(ghost);
169
170
171
172
        }
    }
}

pixhawk's avatar
pixhawk committed
173
174
175
/**
 * @brief Read a number of bytes from the interface.
 **/
176
void UDPLink::readBytes()
pixhawk's avatar
pixhawk committed
177
{
dogmaphobic's avatar
dogmaphobic committed
178
    QByteArray databuffer;
179
    while (_socket->hasPendingDatagrams())
180
181
    {
        QByteArray datagram;
182
        datagram.resize(_socket->pendingDatagramSize());
183
184
        QHostAddress sender;
        quint16 senderPort;
185
        _socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
dogmaphobic's avatar
dogmaphobic committed
186
187
188
189
190
191
        databuffer.append(datagram);
        //-- Wait a bit before sending it over
        if(databuffer.size() > 10 * 1024) {
            emit bytesReceived(this, databuffer);
            databuffer.clear();
        }
192
        _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
193
194
195
196
        // TODO This doesn't validade the sender. Anything sending UDP packets to this port gets
        // added to the list and will start receiving datagrams from here. Even a port scanner
        // would trigger this.
        // Add host to broadcast list if not yet present, or update its port
197
        _udpConfig->addHost(sender.toString(), (int)senderPort);
pixhawk's avatar
pixhawk committed
198
    }
dogmaphobic's avatar
dogmaphobic committed
199
200
201
202
    //-- Send whatever is left
    if(databuffer.size()) {
        emit bytesReceived(this, databuffer);
    }
pixhawk's avatar
pixhawk committed
203
204
205
206
207
208
209
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
Don Gagne's avatar
Don Gagne committed
210
void UDPLink::_disconnect(void)
pixhawk's avatar
pixhawk committed
211
{
212
    _running = false;
213
    quit();
214
    wait();
215
216
217
218
    if (_socket) {
        // Make sure delete happen on correct thread
        _socket->deleteLater();
        _socket = NULL;
219
        emit disconnected();
220
221
    }
    _connectState = false;
pixhawk's avatar
pixhawk committed
222
223
224
225
226
227
228
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
229
bool UDPLink::_connect(void)
pixhawk's avatar
pixhawk committed
230
{
231
    if(this->isRunning() || _running)
232
    {
233
        _running = false;
234
        quit();
235
        wait();
236
    }
237
    _running = true;
238
    start(NormalPriority);
239
    return true;
oberion's avatar
oberion committed
240
241
}

242
bool UDPLink::_hardwareConnect()
oberion's avatar
oberion committed
243
{
244
245
246
247
    if (_socket) {
        delete _socket;
        _socket = NULL;
    }
248
    QHostAddress host = QHostAddress::AnyIPv4;
249
    _socket = new QUdpSocket();
250
    _socket->setProxy(QNetworkProxy::NoProxy);
251
    _connectState = _socket->bind(host, _udpConfig->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress);
252
    if (_connectState) {
253
        _socket->joinMulticastGroup(QHostAddress("224.0.0.1"));
dogmaphobic's avatar
dogmaphobic committed
254
        //-- Make sure we have a large enough IO buffers
Don Gagne's avatar
Don Gagne committed
255
#ifdef __mobile__
dogmaphobic's avatar
dogmaphobic committed
256
257
258
259
260
261
        _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption,     64 * 1024);
        _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 128 * 1024);
#else
        _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption,    256 * 1024);
        _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 512 * 1024);
#endif
262
        _registerZeroconf(_udpConfig->localPort(), kZeroconfRegistration);
263
        QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes);
264
        emit connected();
dogmaphobic's avatar
dogmaphobic committed
265
    } else {
Don Gagne's avatar
Don Gagne committed
266
        emit communicationError(tr("UDP Link Error"), tr("Error binding UDP port: %1").arg(_socket->errorString()));
267
    }
268
    return _connectState;
pixhawk's avatar
pixhawk committed
269
270
271
272
273
274
275
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
276
bool UDPLink::isConnected() const
277
{
278
    return _connectState;
pixhawk's avatar
pixhawk committed
279
280
}

281
qint64 UDPLink::getConnectionSpeed() const
pixhawk's avatar
pixhawk committed
282
{
283
284
285
286
287
288
    return 54000000; // 54 Mbit
}

qint64 UDPLink::getCurrentInDataRate() const
{
    return 0;
pixhawk's avatar
pixhawk committed
289
290
}

291
qint64 UDPLink::getCurrentOutDataRate() const
pixhawk's avatar
pixhawk committed
292
{
293
    return 0;
pixhawk's avatar
pixhawk committed
294
295
}

296
297
298
299
void UDPLink::_registerZeroconf(uint16_t port, const std::string &regType)
{
#if defined(QGC_ZEROCONF_ENABLED)
    DNSServiceErrorType result = DNSServiceRegister(&_dnssServiceRef, 0, 0, 0,
DonLakeFlyer's avatar
DonLakeFlyer committed
300
301
302
303
304
305
306
307
                                                    regType.c_str(),
                                                    NULL,
                                                    NULL,
                                                    htons(port),
                                                    0,
                                                    NULL,
                                                    NULL,
                                                    NULL);
308
309
    if (result != kDNSServiceErr_NoError)
    {
DonLakeFlyer's avatar
DonLakeFlyer committed
310
        emit communicationError(tr("UDP Link Error"), tr("Error registering Zeroconf"));
311
312
313
314
315
316
317
318
319
320
321
322
        _dnssServiceRef = NULL;
    }
#else
    Q_UNUSED(port);
    Q_UNUSED(regType);
#endif
}

void UDPLink::_deregisterZeroconf()
{
#if defined(QGC_ZEROCONF_ENABLED)
    if (_dnssServiceRef)
DonLakeFlyer's avatar
DonLakeFlyer committed
323
324
325
326
    {
        DNSServiceRefDeallocate(_dnssServiceRef);
        _dnssServiceRef = NULL;
    }
327
328
329
#endif
}

330
331
//--------------------------------------------------------------------------
//-- UDPConfiguration
332

333
UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name)
334
{
335
336
337
338
339
340
    AutoConnectSettings* settings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();
    _localPort = settings->udpListenPort()->rawValue().toInt();
    QString targetHostIP = settings->udpTargetHostIP()->rawValue().toString();
    if (!targetHostIP.isEmpty()) {
        addHost(targetHostIP, settings->udpTargetHostPort()->rawValue().toInt());
    }
341
342
}

343
UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
344
{
345
346
347
    _localPort = source->localPort();
    QString host;
    int port;
348
    _hostList.clear();
349
350
351
352
353
    if(source->firstHost(host, port)) {
        do {
            addHost(host, port);
        } while(source->nextHost(host, port));
    }
354
355
}

356
void UDPConfiguration::copyFrom(LinkConfiguration *source)
357
{
358
    LinkConfiguration::copyFrom(source);
359
    UDPConfiguration* usource = dynamic_cast<UDPConfiguration*>(source);
DonLakeFlyer's avatar
DonLakeFlyer committed
360
361
362
363
364
365
366
367
368
369
370
371
    if (usource) {
        _localPort = usource->localPort();
        _hosts.clear();
        QString host;
        int port;
        if(usource->firstHost(host, port)) {
            do {
                addHost(host, port);
            } while(usource->nextHost(host, port));
        }
    } else {
        qWarning() << "Internal error";
372
373
374
375
376
377
    }
}

/**
 * @param host Hostname in standard formatt, e.g. localhost:14551 or 192.168.1.1:14551
 */
378
void UDPConfiguration::addHost(const QString host)
379
{
380
    // Handle x.x.x.x:p
381
382
    if (host.contains(":"))
    {
383
        addHost(host.split(":").first(), host.split(":").last().toInt());
384
    }
385
    // If no port, use default
386
387
    else
    {
dogmaphobic's avatar
dogmaphobic committed
388
        addHost(host, (int)_localPort);
389
390
391
392
393
    }
}

void UDPConfiguration::addHost(const QString& host, int port)
{
dogmaphobic's avatar
dogmaphobic committed
394
    bool changed = false;
dogmaphobic's avatar
dogmaphobic committed
395
396
397
398
    QMutexLocker locker(&_confMutex);
    if(_hosts.contains(host)) {
        if(_hosts[host] != port) {
            _hosts[host] = port;
dogmaphobic's avatar
dogmaphobic committed
399
            changed = true;
dogmaphobic's avatar
dogmaphobic committed
400
401
        }
    } else {
402
403
        QString ipAdd = get_ip_address(host);
        if(ipAdd.isEmpty()) {
404
            qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
405
        } else {
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
            // In simulation and testing setups the vehicle and the GCS can be
            // running on the same host. This leads to packets arriving through
            // the local network or the loopback adapter, which makes it look
            // like the vehicle is connected through two different links,
            // complicating routing.
            //
            // We detect this case and force all traffic to a simulated instance
            // onto the local loopback interface.
            bool not_local = true;
            // Run through all IPv4 interfaces and check if their canonical
            // IP address in string representation matches the source IP address
            foreach (const QHostAddress &address, QNetworkInterface::allAddresses()) {
                if (address.protocol() == QAbstractSocket::IPv4Protocol) {
                    if (ipAdd.endsWith(address.toString())) {
                        // This is a local address of the same host
                        not_local = false;
                    }
                }
            }
            if (not_local) {
                // This is a normal remote host, add it using its IPv4 address
                _hosts[ipAdd] = port;
                //qDebug() << "UDP:" << "Adding Host:" << ipAdd << ":" << port;
            } else {
                // It is localhost, so talk to it through the IPv4 loopback interface
                _hosts["127.0.0.1"] = port;
            }
dogmaphobic's avatar
dogmaphobic committed
433
            changed = true;
dogmaphobic's avatar
dogmaphobic committed
434
435
        }
    }
dogmaphobic's avatar
dogmaphobic committed
436
437
438
    if(changed) {
        _updateHostList();
    }
439
440
}

441
void UDPConfiguration::removeHost(const QString host)
442
{
dogmaphobic's avatar
dogmaphobic committed
443
    QMutexLocker locker(&_confMutex);
444
445
446
447
448
449
450
    QString tHost = host;
    if (tHost.contains(":")) {
        tHost = tHost.split(":").first();
    }
    tHost = tHost.trimmed();
    QMap<QString, int>::iterator i = _hosts.find(tHost);
    if(i != _hosts.end()) {
dogmaphobic's avatar
dogmaphobic committed
451
        //qDebug() << "UDP:" << "Removed host:" << host;
452
        _hosts.erase(i);
453
454
    } else {
        qWarning() << "UDP:" << "Could not remove unknown host:" << host;
455
    }
456
    _updateHostList();
457
458
459
460
}

bool UDPConfiguration::firstHost(QString& host, int& port)
{
dogmaphobic's avatar
dogmaphobic committed
461
    _confMutex.lock();
462
463
    _it = _hosts.begin();
    if(_it == _hosts.end()) {
dogmaphobic's avatar
dogmaphobic committed
464
        _confMutex.unlock();
465
466
        return false;
    }
dogmaphobic's avatar
dogmaphobic committed
467
    _confMutex.unlock();
468
469
470
471
472
    return nextHost(host, port);
}

bool UDPConfiguration::nextHost(QString& host, int& port)
{
dogmaphobic's avatar
dogmaphobic committed
473
    QMutexLocker locker(&_confMutex);
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
    if(_it != _hosts.end()) {
        host = _it.key();
        port = _it.value();
        _it++;
        return true;
    }
    return false;
}

void UDPConfiguration::setLocalPort(quint16 port)
{
    _localPort = port;
}

void UDPConfiguration::saveSettings(QSettings& settings, const QString& root)
{
    _confMutex.lock();
    settings.beginGroup(root);
    settings.setValue("port", (int)_localPort);
    settings.setValue("hostCount", _hosts.count());
    int index = 0;
    QMap<QString, int>::const_iterator it = _hosts.begin();
    while(it != _hosts.end()) {
        QString hkey = QString("host%1").arg(index);
        settings.setValue(hkey, it.key());
        QString pkey = QString("port%1").arg(index);
        settings.setValue(pkey, it.value());
        it++;
        index++;
    }
    settings.endGroup();
    _confMutex.unlock();
}

void UDPConfiguration::loadSettings(QSettings& settings, const QString& root)
{
510
511
    AutoConnectSettings* acSettings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();

512
513
    _confMutex.lock();
    _hosts.clear();
dogmaphobic's avatar
dogmaphobic committed
514
515
    _confMutex.unlock();
    settings.beginGroup(root);
516
    _localPort = (quint16)settings.value("port", acSettings->udpListenPort()->rawValue().toInt()).toUInt();
517
518
519
520
521
522
523
524
525
    int hostCount = settings.value("hostCount", 0).toInt();
    for(int i = 0; i < hostCount; i++) {
        QString hkey = QString("host%1").arg(i);
        QString pkey = QString("port%1").arg(i);
        if(settings.contains(hkey) && settings.contains(pkey)) {
            addHost(settings.value(hkey).toString(), settings.value(pkey).toInt());
        }
    }
    settings.endGroup();
526
    _updateHostList();
527
528
529
530
531
532
533
534
535
536
}

void UDPConfiguration::updateSettings()
{
    if(_link) {
        UDPLink* ulink = dynamic_cast<UDPLink*>(_link);
        if(ulink) {
            ulink->_restartConnection();
        }
    }
537
}
538
539
540
541
542
543
544
545
546
547
548
549

void UDPConfiguration::_updateHostList()
{
    _hostList.clear();
    QMap<QString, int>::const_iterator it = _hosts.begin();
    while(it != _hosts.end()) {
        QString host = QString("%1").arg(it.key()) + ":" + QString("%1").arg(it.value());
        _hostList += host;
        it++;
    }
    emit hostListChanged();
}