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