Newer
Older
/****************************************************************************
*
* (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.
*
****************************************************************************/
/**
* @file
* @brief Definition of UDP connection (server) for unmanned vehicles
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
Lorenz Meier
committed
#include <QNetworkInterface>
dogmaphobic
committed
#define REMOVE_GONE_HOSTS 0
static const char* kZeroconfRegistration = "_qgroundcontrol._udp";
static bool is_ip(const QString& address)
{
int a,b,c,d;
if (sscanf(address.toStdString().c_str(), "%d.%d.%d.%d", &a, &b, &c, &d) != 4
&& strcmp("::1", address.toStdString().c_str())) {
}
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("");
}
dogmaphobic
committed
UDPLink::UDPLink(UDPConfiguration* config)
: _socket(NULL)
, _connectState(false)
, _running(false)
dogmaphobic
committed
Q_ASSERT(config != NULL);
_config = config;
_config->setLink(this);
Lorenz Meier
committed
// 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);
dogmaphobic
committed
// Disconnect link from configuration
_config->setLink(NULL);
_running = false;
dogmaphobic
committed
this->deleteLater();
}
/**
* @brief Runs the thread
*
**/
void UDPLink::run()
{
if(_hardwareConnect()) {
}
dogmaphobic
committed
void UDPLink::_restartConnection()
dogmaphobic
committed
if(this->isConnected())
{
_disconnect();
_connect();
}
dogmaphobic
committed
QString UDPLink::getName() const
dogmaphobic
committed
return _config->name();
}
void UDPLink::addHost(const QString& host)
{
dogmaphobic
committed
_config->addHost(host);
dogmaphobic
committed
void UDPLink::removeHost(const QString& host)
dogmaphobic
committed
_config->removeHost(host);
void UDPLink::_writeBytes(const QByteArray data)
if (!_socket)
return;
QStringList goneHosts;
// Send to all connected systems
QString host;
int port;
if(_config->firstHost(host, port)) {
do {
QHostAddress currentHost(host);
if(_socket->writeDatagram(data, currentHost, (quint16)port) < 0) {
// This host is gone. Add to list to be removed
// 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(data.size(), QDateTime::currentMSecsSinceEpoch());
}
} while (_config->nextHost(host, port));
//-- Remove hosts that are no longer there
foreach (const QString& ghost, goneHosts) {
_config->removeHost(ghost);
}
}
}
/**
* @brief Read a number of bytes from the interface.
**/
void UDPLink::readBytes()
dogmaphobic
committed
while (_socket->hasPendingDatagrams())
{
QByteArray datagram;
dogmaphobic
committed
datagram.resize(_socket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
dogmaphobic
committed
_socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
databuffer.append(datagram);
//-- Wait a bit before sending it over
if(databuffer.size() > 10 * 1024) {
emit bytesReceived(this, databuffer);
databuffer.clear();
}
_logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
dogmaphobic
committed
// 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
//-- Send whatever is left
if(databuffer.size()) {
emit bytesReceived(this, databuffer);
}
}
/**
* @brief Disconnect the connection.
*
* @return True if connection has been disconnected, false if connection couldn't be disconnected.
**/
_running = false;
wait();
dogmaphobic
committed
if (_socket) {
// Make sure delete happen on correct thread
_socket->deleteLater();
_socket = NULL;
dogmaphobic
committed
}
_connectState = false;
}
/**
* @brief Connect the connection.
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool UDPLink::_connect(void)
if(this->isRunning() || _running)
dogmaphobic
committed
{
_running = false;
wait();
dogmaphobic
committed
}
_running = true;
dogmaphobic
committed
bool UDPLink::_hardwareConnect()
if (_socket) {
delete _socket;
_socket = NULL;
}
QHostAddress host = QHostAddress::AnyIPv4;
dogmaphobic
committed
_socket = new QUdpSocket();
_socket->setProxy(QNetworkProxy::NoProxy);
_connectState = _socket->bind(host, _config->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress);
dogmaphobic
committed
if (_connectState) {
_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
_registerZeroconf(_config->localPort(), kZeroconfRegistration);
QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes);
emit connected();
} else {
emit communicationError("UDP Link Error", "Error binding UDP port");
}
dogmaphobic
committed
return _connectState;
}
/**
* @brief Check if connection is active.
*
* @return True if link is connected, false otherwise.
**/
bool UDPLink::isConnected() const
dogmaphobic
committed
return _connectState;
dogmaphobic
committed
qint64 UDPLink::getConnectionSpeed() const
dogmaphobic
committed
return 54000000; // 54 Mbit
}
qint64 UDPLink::getCurrentInDataRate() const
{
return 0;
dogmaphobic
committed
qint64 UDPLink::getCurrentOutDataRate() const
dogmaphobic
committed
return 0;
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
void UDPLink::_registerZeroconf(uint16_t port, const std::string ®Type)
{
#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
}
dogmaphobic
committed
//--------------------------------------------------------------------------
//-- UDPConfiguration
dogmaphobic
committed
UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name)
dogmaphobic
committed
_localPort = QGC_UDP_LOCAL_PORT;
dogmaphobic
committed
UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
dogmaphobic
committed
_localPort = source->localPort();
QString host;
int port;
dogmaphobic
committed
if(source->firstHost(host, port)) {
do {
addHost(host, port);
} while(source->nextHost(host, port));
}
dogmaphobic
committed
void UDPConfiguration::copyFrom(LinkConfiguration *source)
LinkConfiguration::copyFrom(source);
dogmaphobic
committed
UDPConfiguration* usource = dynamic_cast<UDPConfiguration*>(source);
Q_ASSERT(usource != NULL);
_localPort = usource->localPort();
dogmaphobic
committed
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)
dogmaphobic
committed
{
dogmaphobic
committed
if (host.contains(":"))
{
addHost(host.split(":").first(), host.split(":").last().toInt());
dogmaphobic
committed
}
dogmaphobic
committed
else
{
dogmaphobic
committed
}
}
void UDPConfiguration::addHost(const QString& host, int port)
{
QMutexLocker locker(&_confMutex);
if(_hosts.contains(host)) {
if(_hosts[host] != port) {
_hosts[host] = port;
QString ipAdd = get_ip_address(host);
if(ipAdd.isEmpty()) {
qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
Lorenz Meier
committed
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
// 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
committed
}
void UDPConfiguration::removeHost(const QString host)
dogmaphobic
committed
{
dogmaphobic
committed
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()) {
//qDebug() << "UDP:" << "Removed host:" << host;
dogmaphobic
committed
_hosts.erase(i);
} else {
qWarning() << "UDP:" << "Could not remove unknown host:" << host;
dogmaphobic
committed
}
dogmaphobic
committed
}
bool UDPConfiguration::firstHost(QString& host, int& port)
{
dogmaphobic
committed
_it = _hosts.begin();
if(_it == _hosts.end()) {
dogmaphobic
committed
return false;
}
dogmaphobic
committed
return nextHost(host, port);
}
bool UDPConfiguration::nextHost(QString& host, int& port)
{
dogmaphobic
committed
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
496
497
498
499
500
501
502
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
committed
_localPort = (quint16)settings.value("port", QGC_UDP_LOCAL_PORT).toUInt();
dogmaphobic
committed
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();
dogmaphobic
committed
}
void UDPConfiguration::updateSettings()
{
if(_link) {
UDPLink* ulink = dynamic_cast<UDPLink*>(_link);
if(ulink) {
ulink->_restartConnection();
}
}
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();
}