UDPLink.cc 15.3 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
}

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();
        for (int i = 0; i < hostAddresses.size(); i++)
        {
            // Exclude all IPv6 addresses
            if (!hostAddresses.at(i).toString().contains(":"))
            {
                return hostAddresses.at(i).toString();
            }
        }
    }
67
    return {};
68 69
}

70
static bool contains_target(const QList<UDPCLient*> list, const QHostAddress& address, quint16 port)
Gus Grubba's avatar
Gus Grubba committed
71
{
72
    for(UDPCLient* target: list) {
Gus Grubba's avatar
Gus Grubba committed
73 74 75 76 77 78 79
        if(target->address == address && target->port == port) {
            return true;
        }
    }
    return false;
}

80 81
UDPLink::UDPLink(SharedLinkConfigurationPointer& config)
    : LinkInterface(config)
DonLakeFlyer's avatar
DonLakeFlyer committed
82
    #if defined(QGC_ZEROCONF_ENABLED)
83
    , _dnssServiceRef(nullptr)
DonLakeFlyer's avatar
DonLakeFlyer committed
84
    #endif
85
    , _running(false)
86
    , _socket(nullptr)
87 88
    , _udpConfig(qobject_cast<UDPConfiguration*>(config.data()))
    , _connectState(false)
pixhawk's avatar
pixhawk committed
89
{
DonLakeFlyer's avatar
DonLakeFlyer committed
90 91 92
    if (!_udpConfig) {
        qWarning() << "Internal error";
    }
93
    for (const QHostAddress &address: QNetworkInterface::allAddresses()) {
94 95
        _localAddress.append(QHostAddress(address));
    }
96
    moveToThread(this);
pixhawk's avatar
pixhawk committed
97 98 99 100
}

UDPLink::~UDPLink()
{
101
    _disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
102
    // Tell the thread to exit
103
    _running = false;
Gus Grubba's avatar
Gus Grubba committed
104
    // Clear client list
105 106
    qDeleteAll(_sessionTargets);
    _sessionTargets.clear();
107
    quit();
Lorenz Meier's avatar
Lorenz Meier committed
108 109
    // Wait for it to exit
    wait();
110
    this->deleteLater();
pixhawk's avatar
pixhawk committed
111 112 113 114 115 116 117 118
}

/**
 * @brief Runs the thread
 *
 **/
void UDPLink::run()
{
119
    if(_hardwareConnect()) {
120
        exec();
121
    }
122
    if (_socket) {
123
        _deregisterZeroconf();
124 125
        _socket->close();
    }
pixhawk's avatar
pixhawk committed
126 127
}

128
void UDPLink::_restartConnection()
pixhawk's avatar
pixhawk committed
129
{
130 131 132 133 134
    if(this->isConnected())
    {
        _disconnect();
        _connect();
    }
pixhawk's avatar
pixhawk committed
135 136
}

137
QString UDPLink::getName() const
pixhawk's avatar
pixhawk committed
138
{
139
    return _udpConfig->name();
140 141
}

142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157
bool UDPLink::_isIpLocal(const QHostAddress& add)
{
    // 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.
    // Run through all IPv4 interfaces and check if their canonical
    // IP address in string representation matches the source IP address
    //
    // On Windows, this is a very expensive call only Redmond would know
    // why. As such, we make it once and keep the list locally. If a new
    // interface shows up after we start, it won't be on this list.
158
    for (const QHostAddress &address: _localAddress) {
159 160 161 162 163 164 165 166
        if (address == add) {
            // This is a local address of the same host
            return true;
        }
    }
    return false;
}

167
void UDPLink::_writeBytes(const QByteArray data)
168
{
169
    if (!_socket) {
170
        return;
171
    }
172
    emit bytesSent(this, data);
Gus Grubba's avatar
Gus Grubba committed
173
    // Send to all manually targeted systems
174
    for(UDPCLient* target: _udpConfig->targetHosts()) {
Gus Grubba's avatar
Gus Grubba committed
175
        // Skip it if it's part of the session clients below
176
        if(!contains_target(_sessionTargets, target->address, target->port)) {
Gus Grubba's avatar
Gus Grubba committed
177
            _writeDataGram(data, target);
178 179
        }
    }
Gus Grubba's avatar
Gus Grubba committed
180
    // Send to all connected systems
181
    for(UDPCLient* target: _sessionTargets) {
Gus Grubba's avatar
Gus Grubba committed
182 183 184 185 186 187
        _writeDataGram(data, target);
    }
}

void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target)
{
188
    //qDebug() << "UDP Out" << target->address << target->port;
Gus Grubba's avatar
Gus Grubba committed
189 190 191 192 193 194 195 196 197
    if(_socket->writeDatagram(data, target->address, target->port) < 0) {
        qWarning() << "Error writing to" << target->address << target->port;
    } 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(data.size(), QDateTime::currentMSecsSinceEpoch());
    }
198 199
}

pixhawk's avatar
pixhawk committed
200 201 202
/**
 * @brief Read a number of bytes from the interface.
 **/
203
void UDPLink::readBytes()
pixhawk's avatar
pixhawk committed
204
{
205
    if (!_socket) {
DonLakeFlyer's avatar
DonLakeFlyer committed
206 207 208 209 210 211 212 213 214
        return;
    }
    QByteArray databuffer;
    while (_socket->hasPendingDatagrams())
    {
        QByteArray datagram;
        datagram.resize(_socket->pendingDatagramSize());
        QHostAddress sender;
        quint16 senderPort;
215
        //-- Note: This call is broken in Qt 5.9.3 on Windows. It always returns a blank sender and 0 for the port.
DonLakeFlyer's avatar
DonLakeFlyer committed
216 217 218 219
        _socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
        databuffer.append(datagram);
        //-- Wait a bit before sending it over
        if(databuffer.size() > 10 * 1024) {
dogmaphobic's avatar
dogmaphobic committed
220
            emit bytesReceived(this, databuffer);
DonLakeFlyer's avatar
DonLakeFlyer committed
221
            databuffer.clear();
dogmaphobic's avatar
dogmaphobic committed
222
        }
223
        _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
Gus Grubba's avatar
Gus Grubba committed
224
        // TODO: This doesn't validade the sender. Anything sending UDP packets to this port gets
225 226 227
        // 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
Gus Grubba's avatar
Gus Grubba committed
228
        QHostAddress asender = sender;
229
        if(_isIpLocal(sender)) {
Gus Grubba's avatar
Gus Grubba committed
230 231
            asender = QHostAddress(QString("127.0.0.1"));
        }
232
        if(!contains_target(_sessionTargets, asender, senderPort)) {
Gus Grubba's avatar
Gus Grubba committed
233 234 235 236
            qDebug() << "Adding target" << asender << senderPort;
            UDPCLient* target = new UDPCLient(asender, senderPort);
            _sessionTargets.append(target);
        }
pixhawk's avatar
pixhawk committed
237
    }
dogmaphobic's avatar
dogmaphobic committed
238 239 240 241
    //-- Send whatever is left
    if(databuffer.size()) {
        emit bytesReceived(this, databuffer);
    }
pixhawk's avatar
pixhawk committed
242 243 244 245 246 247 248
}

/**
 * @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
249
void UDPLink::_disconnect(void)
pixhawk's avatar
pixhawk committed
250
{
251
    _running = false;
252
    quit();
253
    wait();
254 255 256
    if (_socket) {
        // Make sure delete happen on correct thread
        _socket->deleteLater();
257
        _socket = nullptr;
258
        emit disconnected();
259 260
    }
    _connectState = false;
pixhawk's avatar
pixhawk committed
261 262 263 264 265 266 267
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
268
bool UDPLink::_connect(void)
pixhawk's avatar
pixhawk committed
269
{
270
    if(this->isRunning() || _running)
271
    {
272
        _running = false;
273
        quit();
274
        wait();
275
    }
276
    _running = true;
277
    start(NormalPriority);
278
    return true;
oberion's avatar
oberion committed
279 280
}

281
bool UDPLink::_hardwareConnect()
oberion's avatar
oberion committed
282
{
283 284
    if (_socket) {
        delete _socket;
285
        _socket = nullptr;
286
    }
287
    QHostAddress host = QHostAddress::AnyIPv4;
288
    _socket = new QUdpSocket(this);
289
    _socket->setProxy(QNetworkProxy::NoProxy);
290
    _connectState = _socket->bind(host, _udpConfig->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress);
291
    if (_connectState) {
292
        _socket->joinMulticastGroup(QHostAddress("224.0.0.1"));
dogmaphobic's avatar
dogmaphobic committed
293
        //-- Make sure we have a large enough IO buffers
Don Gagne's avatar
Don Gagne committed
294
#ifdef __mobile__
dogmaphobic's avatar
dogmaphobic committed
295 296 297 298 299 300
        _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
301
        _registerZeroconf(_udpConfig->localPort(), kZeroconfRegistration);
302
        QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes);
303
        emit connected();
304
    } else {
Don Gagne's avatar
Don Gagne committed
305
        emit communicationError(tr("UDP Link Error"), tr("Error binding UDP port: %1").arg(_socket->errorString()));
306
    }
307
    return _connectState;
pixhawk's avatar
pixhawk committed
308 309 310 311 312 313 314
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
315
bool UDPLink::isConnected() const
316
{
317
    return _connectState;
pixhawk's avatar
pixhawk committed
318 319
}

320
qint64 UDPLink::getConnectionSpeed() const
pixhawk's avatar
pixhawk committed
321
{
322 323 324 325 326 327
    return 54000000; // 54 Mbit
}

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

330
qint64 UDPLink::getCurrentOutDataRate() const
pixhawk's avatar
pixhawk committed
331
{
332
    return 0;
pixhawk's avatar
pixhawk committed
333 334
}

335 336 337 338
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
339 340 341 342 343 344 345 346
                                                    regType.c_str(),
                                                    NULL,
                                                    NULL,
                                                    htons(port),
                                                    0,
                                                    NULL,
                                                    NULL,
                                                    NULL);
347 348
    if (result != kDNSServiceErr_NoError)
    {
349
        emit communicationError(tr("UDP Link Error"), tr("Error registering Zeroconf"));
350 351 352 353 354 355 356 357 358 359 360 361
        _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
362 363 364 365
    {
        DNSServiceRefDeallocate(_dnssServiceRef);
        _dnssServiceRef = NULL;
    }
366 367 368
#endif
}

369 370
//--------------------------------------------------------------------------
//-- UDPConfiguration
371

372
UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name)
373
{
374 375 376 377
    AutoConnectSettings* settings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();
    _localPort = settings->udpListenPort()->rawValue().toInt();
    QString targetHostIP = settings->udpTargetHostIP()->rawValue().toString();
    if (!targetHostIP.isEmpty()) {
Gus Grubba's avatar
Gus Grubba committed
378
        addHost(targetHostIP, settings->udpTargetHostPort()->rawValue().toUInt());
379
    }
380 381
}

382
UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
383
{
Gus Grubba's avatar
Gus Grubba committed
384 385 386 387 388 389
    _copyFrom(source);
}

UDPConfiguration::~UDPConfiguration()
{
    _clearTargetHosts();
390 391
}

392
void UDPConfiguration::copyFrom(LinkConfiguration *source)
393
{
394
    LinkConfiguration::copyFrom(source);
Gus Grubba's avatar
Gus Grubba committed
395 396 397 398 399
    _copyFrom(source);
}

void UDPConfiguration::_copyFrom(LinkConfiguration *source)
{
400
    auto* usource = qobject_cast<UDPConfiguration*>(source);
DonLakeFlyer's avatar
DonLakeFlyer committed
401 402
    if (usource) {
        _localPort = usource->localPort();
Gus Grubba's avatar
Gus Grubba committed
403
        _clearTargetHosts();
404
        for(UDPCLient* target: usource->targetHosts()) {
405
            if(!contains_target(_targetHosts, target->address, target->port)) {
Gus Grubba's avatar
Gus Grubba committed
406 407
                UDPCLient* newTarget = new UDPCLient(target);
                _targetHosts.append(newTarget);
408
                _updateHostList();
Gus Grubba's avatar
Gus Grubba committed
409
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
410 411 412
        }
    } else {
        qWarning() << "Internal error";
413 414 415
    }
}

Gus Grubba's avatar
Gus Grubba committed
416 417
void UDPConfiguration::_clearTargetHosts()
{
418 419
    qDeleteAll(_targetHosts);
    _targetHosts.clear();
Gus Grubba's avatar
Gus Grubba committed
420 421
}

422 423 424
/**
 * @param host Hostname in standard formatt, e.g. localhost:14551 or 192.168.1.1:14551
 */
425
void UDPConfiguration::addHost(const QString host)
426
{
427
    // Handle x.x.x.x:p
428 429
    if (host.contains(":"))
    {
Gus Grubba's avatar
Gus Grubba committed
430
        addHost(host.split(":").first(), host.split(":").last().toUInt());
431
    }
432
    // If no port, use default
433 434
    else
    {
Gus Grubba's avatar
Gus Grubba committed
435
        addHost(host, _localPort);
436 437 438
    }
}

Gus Grubba's avatar
Gus Grubba committed
439
void UDPConfiguration::addHost(const QString& host, quint16 port)
440
{
Gus Grubba's avatar
Gus Grubba committed
441 442 443
    QString ipAdd = get_ip_address(host);
    if(ipAdd.isEmpty()) {
        qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
dogmaphobic's avatar
dogmaphobic committed
444
    } else {
Gus Grubba's avatar
Gus Grubba committed
445
        QHostAddress address(ipAdd);
446
        if(!contains_target(_targetHosts, address, port)) {
Gus Grubba's avatar
Gus Grubba committed
447 448 449
            UDPCLient* newTarget = new UDPCLient(address, port);
            _targetHosts.append(newTarget);
            _updateHostList();
dogmaphobic's avatar
dogmaphobic committed
450 451
        }
    }
452 453
}

454
void UDPConfiguration::removeHost(const QString host)
455
{
Gus Grubba's avatar
Gus Grubba committed
456 457 458 459 460 461 462 463 464 465 466 467 468
    if (host.contains(":"))
    {
        QHostAddress address = QHostAddress(get_ip_address(host.split(":").first()));
        quint16 port = host.split(":").last().toUInt();
        for(int i = 0; i < _targetHosts.size(); i++) {
            UDPCLient* target = _targetHosts.at(i);
            if(target->address == address && target->port == port) {
                _targetHosts.removeAt(i);
                delete target;
                _updateHostList();
                return;
            }
        }
469
    }
Gus Grubba's avatar
Gus Grubba committed
470
    qWarning() << "UDP:" << "Could not remove unknown host:" << host;
471
    _updateHostList();
472 473 474 475 476 477 478 479 480 481 482
}

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

void UDPConfiguration::saveSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
    settings.setValue("port", (int)_localPort);
Gus Grubba's avatar
Gus Grubba committed
483 484 485 486 487 488 489
    settings.setValue("hostCount", _targetHosts.size());
    for(int i = 0; i < _targetHosts.size(); i++) {
        UDPCLient* target = _targetHosts.at(i);
        QString hkey = QString("host%1").arg(i);
        settings.setValue(hkey, target->address.toString());
        QString pkey = QString("port%1").arg(i);
        settings.setValue(pkey, target->port);
490 491 492 493 494 495
    }
    settings.endGroup();
}

void UDPConfiguration::loadSettings(QSettings& settings, const QString& root)
{
496
    AutoConnectSettings* acSettings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();
Gus Grubba's avatar
Gus Grubba committed
497
    _clearTargetHosts();
dogmaphobic's avatar
dogmaphobic committed
498
    settings.beginGroup(root);
499
    _localPort = (quint16)settings.value("port", acSettings->udpListenPort()->rawValue().toInt()).toUInt();
500 501 502 503 504
    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)) {
Gus Grubba's avatar
Gus Grubba committed
505
            addHost(settings.value(hkey).toString(), settings.value(pkey).toUInt());
506 507 508
        }
    }
    settings.endGroup();
509
    _updateHostList();
510 511 512 513 514 515 516 517 518 519
}

void UDPConfiguration::updateSettings()
{
    if(_link) {
        UDPLink* ulink = dynamic_cast<UDPLink*>(_link);
        if(ulink) {
            ulink->_restartConnection();
        }
    }
520
}
521 522 523 524

void UDPConfiguration::_updateHostList()
{
    _hostList.clear();
Gus Grubba's avatar
Gus Grubba committed
525 526 527 528
    for(int i = 0; i < _targetHosts.size(); i++) {
        UDPCLient* target = _targetHosts.at(i);
        QString host = QString("%1").arg(target->address.toString()) + ":" + QString("%1").arg(target->port);
        _hostList << host;
529 530 531
    }
    emit hostListChanged();
}