Skip to content

Q
qgroundcontrol
Commit faee40e0 authored by Gus Grubba's avatar Gus Grubba
Browse files
Options

UDP link fixes 

Maintain a list of address/port pairs instead of a address/port map.
parent 6baee573
Hide whitespace changes
Inline Side-by-side
Showing with 176 additions and 214 deletions
src/comm/UDPLink.cc
View file @ faee40e0
......@@ -68,6 +68,37 @@ static QString get_ip_address(const QString& address)
return QString("");
}
static bool is_ip_local(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
foreach (const QHostAddress &address, QNetworkInterface::allAddresses()) {
if (address == add) {
// This is a local address of the same host
return true;
}
}
return false;
}
static bool containst_target(const QList<UDPCLient*> list, const QHostAddress& address, quint16 port)
{
foreach(UDPCLient* target, list) {
if(target->address == address && target->port == port) {
return true;
}
}
return false;
}
UDPLink::UDPLink(SharedLinkConfigurationPointer& config)
: LinkInterface(config)
#if defined(QGC_ZEROCONF_ENABLED)
......@@ -89,6 +120,11 @@ UDPLink::~UDPLink()
_disconnect();
// Tell the thread to exit
_running = false;
// Clear client list
while(_sessionTargets.size()) {
delete _sessionTargets.last();
_sessionTargets.removeLast();
}
quit();
// Wait for it to exit
wait();
......@@ -124,50 +160,34 @@ QString UDPLink::getName() const
return _udpConfig->name();
}
void UDPLink::addHost(const QString& host)
{
_udpConfig->addHost(host);
}
void UDPLink::removeHost(const QString& host)
{
_udpConfig->removeHost(host);
}
void UDPLink::_writeBytes(const QByteArray data)
{
if (!_socket)
return;
QStringList goneHosts;
// Send to all connected systems
QString host;
int port;
if(_udpConfig->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 (_udpConfig->nextHost(host, port));
//-- Remove hosts that are no longer there
foreach (const QString& ghost, goneHosts) {
_udpConfig->removeHost(ghost);
// Send to all manually targeted systems
foreach(UDPCLient* target, _udpConfig->targetHosts()) {
// Skip it if it's part of the session clients below
if(!containst_target(_sessionTargets, target->address, target->port)) {
_writeDataGram(data, target);
}
}
// Send to all connected systems
foreach(UDPCLient* target, _sessionTargets) {
_writeDataGram(data, target);
}
}
void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target)
{
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());
}
}
/**
......@@ -190,11 +210,19 @@ void UDPLink::readBytes()
databuffer.clear();
}
_logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
// TODO This doesn't validade the sender. Anything sending UDP packets to this port gets
// 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
_udpConfig->addHost(sender.toString(), (int)senderPort);
QHostAddress asender = sender;
if(is_ip_local(sender)) {
asender = QHostAddress(QString("127.0.0.1"));
}
if(!containst_target(_sessionTargets, asender, senderPort)) {
qDebug() << "Adding target" << asender << senderPort;
UDPCLient* target = new UDPCLient(asender, senderPort);
_sessionTargets.append(target);
}
}
//-- Send whatever is left
if(databuffer.size()) {
......@@ -336,42 +364,51 @@ UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name
_localPort = settings->udpListenPort()->rawValue().toInt();
QString targetHostIP = settings->udpTargetHostIP()->rawValue().toString();
if (!targetHostIP.isEmpty()) {
addHost(targetHostIP, settings->udpTargetHostPort()->rawValue().toInt());
addHost(targetHostIP, settings->udpTargetHostPort()->rawValue().toUInt());
}
}
UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
{
_localPort = source->localPort();
QString host;
int port;
_hostList.clear();
if(source->firstHost(host, port)) {
do {
addHost(host, port);
} while(source->nextHost(host, port));
}
_copyFrom(source);
}
UDPConfiguration::~UDPConfiguration()
{
_clearTargetHosts();
}
void UDPConfiguration::copyFrom(LinkConfiguration *source)
{
LinkConfiguration::copyFrom(source);
_copyFrom(source);
}
void UDPConfiguration::_copyFrom(LinkConfiguration *source)
{
UDPConfiguration* usource = dynamic_cast<UDPConfiguration*>(source);
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));
_clearTargetHosts();
foreach(UDPCLient* target, usource->targetHosts()) {
if(!containst_target(_targetHosts, target->address, target->port)) {
UDPCLient* newTarget = new UDPCLient(target);
_targetHosts.append(newTarget);
}
}
} else {
qWarning() << "Internal error";
}
}
void UDPConfiguration::_clearTargetHosts()
{
while(_targetHosts.size()) {
delete _targetHosts.last();
_targetHosts.removeLast();
}
}
/**
* @param host Hostname in standard formatt, e.g. localhost:14551 or 192.168.1.1:14551
*/
......@@ -380,106 +417,50 @@ void UDPConfiguration::addHost(const QString host)
// Handle x.x.x.x:p
if (host.contains(":"))
{
addHost(host.split(":").first(), host.split(":").last().toInt());
addHost(host.split(":").first(), host.split(":").last().toUInt());
}
// If no port, use default
else
{
addHost(host, (int)_localPort);
addHost(host, _localPort);
}
}
void UDPConfiguration::addHost(const QString& host, int port)
void UDPConfiguration::addHost(const QString& host, quint16 port)
{
bool changed = false;
QMutexLocker locker(&_confMutex);
if(_hosts.contains(host)) {
if(_hosts[host] != port) {
_hosts[host] = port;
changed = true;
}
QString ipAdd = get_ip_address(host);
if(ipAdd.isEmpty()) {
qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
} else {
QString ipAdd = get_ip_address(host);
if(ipAdd.isEmpty()) {
qWarning() << "UDP:" << "Could not resolve host:" << host << "port:" << port;
} else {
// 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;
}
changed = true;
QHostAddress address(ipAdd);
if(!containst_target(_targetHosts, address, port)) {
UDPCLient* newTarget = new UDPCLient(address, port);
_targetHosts.append(newTarget);
_updateHostList();
}
}
if(changed) {
_updateHostList();
}
}
void UDPConfiguration::removeHost(const QString host)
{
QMutexLocker locker(&_confMutex);
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;
_hosts.erase(i);
} else {
qWarning() << "UDP:" << "Could not remove unknown host:" << host;
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;
}
}
}
qWarning() << "UDP:" << "Could not remove unknown host:" << host;
_updateHostList();
}
bool UDPConfiguration::firstHost(QString& host, int& port)
{
_confMutex.lock();
_it = _hosts.begin();
if(_it == _hosts.end()) {
_confMutex.unlock();
return false;
}
_confMutex.unlock();
return nextHost(host, port);
}
bool UDPConfiguration::nextHost(QString& host, int& port)
{
QMutexLocker locker(&_confMutex);
if(_it != _hosts.end()) {
host = _it.key();
port = _it.value();
_it++;
return true;
}
return false;
}
void UDPConfiguration::setLocalPort(quint16 port)
{
_localPort = port;
......@@ -487,31 +468,23 @@ void UDPConfiguration::setLocalPort(quint16 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.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);
}
settings.endGroup();
_confMutex.unlock();
}
void UDPConfiguration::loadSettings(QSettings& settings, const QString& root)
{
AutoConnectSettings* acSettings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();
_confMutex.lock();
_hosts.clear();
_confMutex.unlock();
_clearTargetHosts();
settings.beginGroup(root);
_localPort = (quint16)settings.value("port", acSettings->udpListenPort()->rawValue().toInt()).toUInt();
int hostCount = settings.value("hostCount", 0).toInt();
......@@ -519,7 +492,7 @@ void UDPConfiguration::loadSettings(QSettings& settings, const QString& root)
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());
addHost(settings.value(hkey).toString(), settings.value(pkey).toUInt());
}
}
settings.endGroup();
......@@ -539,11 +512,10 @@ void UDPConfiguration::updateSettings()
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++;
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;
}
emit hostListChanged();
}
src/comm/UDPLink.h
View file @ faee40e0
......@@ -34,10 +34,23 @@
#include "QGCConfig.h"
#include "LinkManager.h"
class UDPCLient {
public:
UDPCLient(const QHostAddress& address_, quint16 port_)
: address(address_)
, port(port_)
{}
UDPCLient(const UDPCLient* other)
: address(other->address)
, port(other->port)
{}
QHostAddress address;
quint16 port;
};
class UDPConfiguration : public LinkConfiguration
{
Q_OBJECT
public:
Q_PROPERTY(quint16 localPort READ localPort WRITE setLocalPort NOTIFY localPortChanged)
......@@ -61,30 +74,7 @@ public:
*/
UDPConfiguration(UDPConfiguration* source);
/*!
* @brief Begin iteration through the list of target hosts
*
* @param[out] host Host name
* @param[out] port Port number
* @return Returns false if list is empty
*/
bool firstHost (QString& host, int& port);
/*!
* @brief Continues iteration through the list of target hosts
*
* @param[out] host Host name
* @param[out] port Port number
* @return Returns false if reached the end of the list (in which case, both host and port are unchanged)
*/
bool nextHost (QString& host, int& port);
/*!
* @brief Get the number of target hosts
*
* @return Number of hosts in list
*/
int hostCount () { return _hosts.count(); }
~UDPConfiguration();
/*!
* @brief The UDP port we bind to
......@@ -106,7 +96,7 @@ public:
* @param[in] host Host name, e.g. localhost or 192.168.1.1
* @param[in] port Port number
*/
void addHost (const QString& host, int port);
void addHost (const QString& host, quint16 port);
/*!
* @brief Remove a target host from the list
......@@ -127,6 +117,8 @@ public:
*/
QStringList hostList () { return _hostList; }
const QList<UDPCLient*> targetHosts() { return _targetHosts; }
/// From LinkConfiguration
LinkType type () { return LinkConfiguration::TypeUdp; }
void copyFrom (LinkConfiguration* source);
......@@ -142,13 +134,13 @@ signals:
private:
void _updateHostList ();
void _clearTargetHosts ();
void _copyFrom (LinkConfiguration *source);
private:
QMutex _confMutex;
QMap<QString, int>::iterator _it;
QMap<QString, int> _hosts; ///< ("host", port)
QStringList _hostList; ///< Exposed to QML
quint16 _localPort;
QList<UDPCLient*> _targetHosts;
QStringList _hostList; ///< Exposed to QML
quint16 _localPort;
};
class UDPLink : public LinkInterface
......@@ -159,32 +151,28 @@ class UDPLink : public LinkInterface
friend class LinkManager;
public:
void requestReset() { }
bool isConnected() const;
QString getName() const;
void requestReset () override { }
bool isConnected () const override;
QString getName () const override;
// Extensive statistics for scientific purposes
qint64 getConnectionSpeed() const;
qint64 getCurrentInDataRate() const;
qint64 getCurrentOutDataRate() const;
qint64 getConnectionSpeed () const override;
qint64 getCurrentInDataRate () const;
qint64 getCurrentOutDataRate () const;
void run();
// Thread
void run () override;
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
bool connect(void);
bool disconnect(void);
bool connect (void);
bool disconnect (void);
public slots:
/*! @brief Add a new host to broadcast messages to */
void addHost (const QString& host);
/*! @brief Remove a host from broadcasting messages to */
void removeHost (const QString& host);
void readBytes();
void readBytes ();
private slots:
void _writeBytes(const QByteArray data);
void _writeBytes (const QByteArray data) override;
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
......@@ -192,14 +180,14 @@ private:
~UDPLink();
// From LinkInterface
virtual bool _connect(void);
virtual void _disconnect(void);
bool _connect (void) override;
void _disconnect (void) override;
bool _hardwareConnect();
void _restartConnection();
void _registerZeroconf(uint16_t port, const std::string& regType);
void _deregisterZeroconf();
bool _hardwareConnect ();
void _restartConnection ();
void _registerZeroconf (uint16_t port, const std::string& regType);
void _deregisterZeroconf ();
void _writeDataGram (const QByteArray data, const UDPCLient* target);
#if defined(QGC_ZEROCONF_ENABLED)
DNSServiceRef _dnssServiceRef;
......@@ -209,6 +197,8 @@ private:
QUdpSocket* _socket;
UDPConfiguration* _udpConfig;
bool _connectState;
QList<UDPCLient*> _sessionTargets;
};
#endif // UDPLINK_H
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment