UDPLink.cc 16 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2
/*=====================================================================

lm's avatar
lm committed
3
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
4

5
(c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6

lm's avatar
lm committed
7
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed
8

lm's avatar
lm committed
9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10 11 12 13
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

lm's avatar
lm committed
14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17 18 19
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
21 22 23 24 25 26 27 28 29 30

======================================================================*/

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

31 32 33 34 35 36 37
#include <QtGlobal>
#if QT_VERSION > 0x050401
#define UDP_BROKEN_SIGNAL 1
#else
#define UDP_BROKEN_SIGNAL 0
#endif

pixhawk's avatar
pixhawk committed
38 39 40 41
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
42
#include <QNetworkProxy>
43
#include <QNetworkInterface>
pixhawk's avatar
pixhawk committed
44
#include <iostream>
45

pixhawk's avatar
pixhawk committed
46
#include "UDPLink.h"
47
#include "QGC.h"
48
#include <QHostInfo>
pixhawk's avatar
pixhawk committed
49

50 51
#define REMOVE_GONE_HOSTS 0

52 53
static const char* kZeroconfRegistration = "_qgroundcontrol._udp";

54 55 56
static bool is_ip(const QString& address)
{
    int a,b,c,d;
57 58
    if (sscanf(address.toStdString().c_str(), "%d.%d.%d.%d", &a, &b, &c, &d) != 4
            && strcmp("::1", address.toStdString().c_str())) {
59
        return false;
60 61 62
    } else {
        return true;
    }
63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86
}

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("");
}

87 88 89
UDPLink::UDPLink(UDPConfiguration* config)
    : _socket(NULL)
    , _connectState(false)
Gus Grubba's avatar
Gus Grubba committed
90
    #if defined(QGC_ZEROCONF_ENABLED)
91
    , _dnssServiceRef(NULL)
Gus Grubba's avatar
Gus Grubba committed
92
    #endif
93
    , _running(false)
pixhawk's avatar
pixhawk committed
94
{
95 96 97 98
    Q_ASSERT(config != NULL);
    _config = config;
    _config->setLink(this);

99 100 101
    // We're doing it wrong - because the Qt folks got the API wrong:
    // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
    moveToThread(this);
102

103
    //qDebug() << "UDP Created " << _config->name();
pixhawk's avatar
pixhawk committed
104 105 106 107
}

UDPLink::~UDPLink()
{
108 109
    // Disconnect link from configuration
    _config->setLink(NULL);
110
    _disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
111
    // Tell the thread to exit
112
    _running = false;
113
    quit();
Lorenz Meier's avatar
Lorenz Meier committed
114 115
    // Wait for it to exit
    wait();
116 117 118
    while(_outQueue.count() > 0) {
        delete _outQueue.dequeue();
    }
119
    this->deleteLater();
pixhawk's avatar
pixhawk committed
120 121 122 123 124 125 126 127
}

/**
 * @brief Runs the thread
 *
 **/
void UDPLink::run()
{
128
    if(_hardwareConnect()) {
129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146
        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();
147 148
        }
    }
149
    if (_socket) {
150
        _deregisterZeroconf();
151 152
        _socket->close();
    }
pixhawk's avatar
pixhawk committed
153 154
}

155
void UDPLink::_restartConnection()
pixhawk's avatar
pixhawk committed
156
{
157 158 159 160 161
    if(this->isConnected())
    {
        _disconnect();
        _connect();
    }
pixhawk's avatar
pixhawk committed
162 163
}

164
QString UDPLink::getName() const
pixhawk's avatar
pixhawk committed
165
{
166
    return _config->name();
167 168 169 170
}

void UDPLink::addHost(const QString& host)
{
171
    _config->addHost(host);
172 173
}

174
void UDPLink::removeHost(const QString& host)
175
{
176
    _config->removeHost(host);
pixhawk's avatar
pixhawk committed
177 178 179 180
}

void UDPLink::writeBytes(const char* data, qint64 size)
{
181 182 183
    if (!_socket) {
        return;
    }
184 185 186 187 188 189 190
    if(UDP_BROKEN_SIGNAL) {
        QByteArray* qdata = new QByteArray(data, size);
        QMutexLocker lock(&_mutex);
        _outQueue.enqueue(qdata);
    } else {
        _sendBytes(data, size);
    }
191
}
192

193

194
bool UDPLink::_dequeBytes()
195 196 197 198 199
{
    QMutexLocker lock(&_mutex);
    if(_outQueue.count() > 0) {
        QByteArray* qdata = _outQueue.dequeue();
        lock.unlock();
200
        _sendBytes(qdata->data(), qdata->size());
201 202
        delete qdata;
        lock.relock();
203
    }
204
    return (_outQueue.count() > 0);
pixhawk's avatar
pixhawk committed
205 206
}

207 208 209 210 211 212 213 214 215 216 217
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 {
            QHostAddress currentHost(host);
            if(_socket->writeDatagram(data, size, currentHost, (quint16)port) < 0) {
                // This host is gone. Add to list to be removed
218 219 220 221 222 223 224 225 226 227 228 229 230
                // 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.
                _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
231 232 233 234 235 236 237 238 239
            }
        } while (_config->nextHost(host, port));
        //-- Remove hosts that are no longer there
        foreach (QString ghost, goneHosts) {
            _config->removeHost(ghost);
        }
    }
}

pixhawk's avatar
pixhawk committed
240 241 242
/**
 * @brief Read a number of bytes from the interface.
 **/
243
void UDPLink::readBytes()
pixhawk's avatar
pixhawk committed
244
{
245
    while (_socket->hasPendingDatagrams())
246 247
    {
        QByteArray datagram;
248
        datagram.resize(_socket->pendingDatagramSize());
pixhawk's avatar
pixhawk committed
249

250 251
        QHostAddress sender;
        quint16 senderPort;
252
        _socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
pixhawk's avatar
pixhawk committed
253

254 255
        // FIXME TODO Check if this method is better than retrieving the data by individual processes
        emit bytesReceived(this, datagram);
pixhawk's avatar
pixhawk committed
256

257
        _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
258

259 260 261 262 263 264 265 266 267
//        // Echo data for debugging purposes
//        std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
//        int i;
//        for (i=0; i<s; i++)
//        {
//            unsigned int v=data[i];
//            fprintf(stderr,"%02x ", v);
//        }
//        std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
268

269 270 271 272
        // 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
dogmaphobic's avatar
dogmaphobic committed
273
        _config->addHost(sender.toString(), (int)senderPort);
274
        if(UDP_BROKEN_SIGNAL && !_running)
275
            break;
pixhawk's avatar
pixhawk committed
276 277 278 279 280 281 282 283
    }
}

/**
 * @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
284
void UDPLink::_disconnect(void)
pixhawk's avatar
pixhawk committed
285
{
286
    _running = false;
287
    quit();
288
    wait();
289 290 291 292
    if (_socket) {
        // Make sure delete happen on correct thread
        _socket->deleteLater();
        _socket = NULL;
293
        emit disconnected();
294 295
    }
    _connectState = false;
pixhawk's avatar
pixhawk committed
296 297 298 299 300 301 302
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
303
bool UDPLink::_connect(void)
pixhawk's avatar
pixhawk committed
304
{
305
    if(this->isRunning() || _running)
306
    {
307
        _running = false;
308
        quit();
309
        wait();
310
    }
311
    _running = true;
312
    start(NormalPriority);
313
    return true;
oberion's avatar
oberion committed
314 315
}

316
bool UDPLink::_hardwareConnect()
oberion's avatar
oberion committed
317
{
318 319 320 321
    if (_socket) {
        delete _socket;
        _socket = NULL;
    }
322
    QHostAddress host = QHostAddress::AnyIPv4;
323
    _socket = new QUdpSocket();
324
    _socket->setProxy(QNetworkProxy::NoProxy);
325
    _connectState = _socket->bind(host, _config->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress);
326
    if (_connectState) {
327
        _registerZeroconf(_config->localPort(), kZeroconfRegistration);
328 329 330 331
        //-- Connect signal if this version of Qt is not broken
        if(!UDP_BROKEN_SIGNAL) {
            QObject::connect(_socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
        }
332
        emit connected();
333 334
    } else {
        emit communicationError("UDP Link Error", "Error binding UDP port");
335
    }
336
    return _connectState;
pixhawk's avatar
pixhawk committed
337 338 339 340 341 342 343
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
344
bool UDPLink::isConnected() const
345
{
346
    return _connectState;
pixhawk's avatar
pixhawk committed
347 348
}

349
qint64 UDPLink::getConnectionSpeed() const
pixhawk's avatar
pixhawk committed
350
{
351 352 353 354 355 356
    return 54000000; // 54 Mbit
}

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

359
qint64 UDPLink::getCurrentOutDataRate() const
pixhawk's avatar
pixhawk committed
360
{
361
    return 0;
pixhawk's avatar
pixhawk committed
362 363
}

364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397
void UDPLink::_registerZeroconf(uint16_t port, const std::string &regType)
{
#if defined(QGC_ZEROCONF_ENABLED)
    DNSServiceErrorType result = DNSServiceRegister(&_dnssServiceRef, 0, 0, 0,
        regType.c_str(),
        NULL,
        NULL,
        htons(port),
        0,
        NULL,
        NULL,
        NULL);
    if (result != kDNSServiceErr_NoError)
    {
        emit communicationError("UDP Link Error", "Error registering Zeroconf");
        _dnssServiceRef = NULL;
    }
#else
    Q_UNUSED(port);
    Q_UNUSED(regType);
#endif
}

void UDPLink::_deregisterZeroconf()
{
#if defined(QGC_ZEROCONF_ENABLED)
    if (_dnssServiceRef)
     {
         DNSServiceRefDeallocate(_dnssServiceRef);
         _dnssServiceRef = NULL;
     }
#endif
}

398 399
//--------------------------------------------------------------------------
//-- UDPConfiguration
400

401
UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name)
402
{
403
    _localPort = QGC_UDP_LOCAL_PORT;
404 405
}

406
UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
407
{
408 409 410 411 412 413 414 415 416
    _localPort = source->localPort();
    _hosts.clear();
    QString host;
    int port;
    if(source->firstHost(host, port)) {
        do {
            addHost(host, port);
        } while(source->nextHost(host, port));
    }
417 418
}

419
void UDPConfiguration::copyFrom(LinkConfiguration *source)
420
{
421
    LinkConfiguration::copyFrom(source);
422 423 424
    UDPConfiguration* usource = dynamic_cast<UDPConfiguration*>(source);
    Q_ASSERT(usource != NULL);
    _localPort = usource->localPort();
425
    _hosts.clear();
426 427 428 429 430 431 432 433 434 435 436 437 438 439
    QString host;
    int port;
    if(usource->firstHost(host, port)) {
        do {
            addHost(host, port);
        } while(usource->nextHost(host, port));
    }
}

/**
 * @param host Hostname in standard formatt, e.g. localhost:14551 or 192.168.1.1:14551
 */
void UDPConfiguration::addHost(const QString& host)
{
440
    // Handle x.x.x.x:p
441 442
    if (host.contains(":"))
    {
443
        addHost(host.split(":").first(), host.split(":").last().toInt());
444
    }
445
    // If no port, use default
446 447
    else
    {
dogmaphobic's avatar
dogmaphobic committed
448
        addHost(host, (int)_localPort);
449 450 451 452 453
    }
}

void UDPConfiguration::addHost(const QString& host, int port)
{
dogmaphobic's avatar
dogmaphobic committed
454 455 456 457 458 459
    QMutexLocker locker(&_confMutex);
    if(_hosts.contains(host)) {
        if(_hosts[host] != port) {
            _hosts[host] = port;
        }
    } else {
460 461
        QString ipAdd = get_ip_address(host);
        if(ipAdd.isEmpty()) {
462
            qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
463
        } else {
464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495

            // 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
496 497
        }
    }
498 499 500 501
}

void UDPConfiguration::removeHost(const QString& host)
{
dogmaphobic's avatar
dogmaphobic committed
502
    QMutexLocker locker(&_confMutex);
503 504 505 506 507 508 509
    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()) {
510
        //qDebug() << "UDP:" << "Removed host:" << host;
511
        _hosts.erase(i);
512 513
    } else {
        qWarning() << "UDP:" << "Could not remove unknown host:" << host;
514 515 516 517 518
    }
}

bool UDPConfiguration::firstHost(QString& host, int& port)
{
dogmaphobic's avatar
dogmaphobic committed
519
    _confMutex.lock();
520 521
    _it = _hosts.begin();
    if(_it == _hosts.end()) {
dogmaphobic's avatar
dogmaphobic committed
522
        _confMutex.unlock();
523 524
        return false;
    }
dogmaphobic's avatar
dogmaphobic committed
525
    _confMutex.unlock();
526 527 528 529 530
    return nextHost(host, port);
}

bool UDPConfiguration::nextHost(QString& host, int& port)
{
dogmaphobic's avatar
dogmaphobic committed
531
    QMutexLocker locker(&_confMutex);
532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569
    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)
{
    _confMutex.lock();
    _hosts.clear();
dogmaphobic's avatar
dogmaphobic committed
570 571
    _confMutex.unlock();
    settings.beginGroup(root);
572
    _localPort = (quint16)settings.value("port", QGC_UDP_LOCAL_PORT).toUInt();
573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591
    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();
}

void UDPConfiguration::updateSettings()
{
    if(_link) {
        UDPLink* ulink = dynamic_cast<UDPLink*>(_link);
        if(ulink) {
            ulink->_restartConnection();
        }
    }
592
}