UDPLink.cc 9.96 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 - 2011 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 31 32 33 34 35 36 37

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

/**
 * @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>
#include <iostream>
#include "UDPLink.h"
#include "LinkManager.h"
38
#include "QGC.h"
39
#include <QHostInfo>
40
//#include <netinet/in.h>
pixhawk's avatar
pixhawk committed
41

42 43
UDPLink::UDPLink(QHostAddress host, quint16 port) :
    socket(NULL)
pixhawk's avatar
pixhawk committed
44
{
45 46 47
    // 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);
48

pixhawk's avatar
pixhawk committed
49 50 51 52 53
    this->host = host;
    this->port = port;
    this->connectState = false;
    // Set unique ID and add link to the list of links
    this->id = getNextLinkId();
oberion's avatar
oberion committed
54 55 56
	this->name = tr("UDP Link (port:%1)").arg(this->port);
	emit nameChanged(this->name);
    // LinkManager::instance()->add(this);
57
    qDebug() << "UDP Created " << name;
pixhawk's avatar
pixhawk committed
58 59 60 61
}

UDPLink::~UDPLink()
{
62
    _disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
63 64 65 66 67 68

    // Tell the thread to exit
    quit();
    // Wait for it to exit
    wait();

oberion's avatar
oberion committed
69
	this->deleteLater();
pixhawk's avatar
pixhawk committed
70 71 72 73 74 75 76 77
}

/**
 * @brief Runs the thread
 *
 **/
void UDPLink::run()
{
78 79 80
    hardwareConnect();

    exec();
pixhawk's avatar
pixhawk committed
81 82
}

oberion's avatar
oberion committed
83
void UDPLink::setAddress(QHostAddress host)
pixhawk's avatar
pixhawk committed
84
{
oberion's avatar
oberion committed
85 86 87
    bool reconnect(false);
	if(this->isConnected())
	{
88
		_disconnect();
oberion's avatar
oberion committed
89 90 91 92 93
		reconnect = true;
	}
	this->host = host;
	if(reconnect)
	{
94
		_connect();
oberion's avatar
oberion committed
95
	}
pixhawk's avatar
pixhawk committed
96 97
}

98
void UDPLink::setPort(int port)
pixhawk's avatar
pixhawk committed
99
{
oberion's avatar
oberion committed
100 101 102
	bool reconnect(false);
	if(this->isConnected())
	{
103
		_disconnect();
oberion's avatar
oberion committed
104 105
		reconnect = true;
	}
pixhawk's avatar
pixhawk committed
106
    this->port = port;
oberion's avatar
oberion committed
107 108 109 110
	this->name = tr("UDP Link (port:%1)").arg(this->port);
	emit nameChanged(this->name);
	if(reconnect)
	{
111
		_connect();
oberion's avatar
oberion committed
112
	}
113 114 115 116 117 118 119
}

/**
 * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
 */
void UDPLink::addHost(const QString& host)
{
120
    qDebug() << "UDP:" << "ADDING HOST:" << host;
121 122
    if (host.contains(":"))
    {
123
        //qDebug() << "HOST: " << host.split(":").first();
124
        QHostInfo info = QHostInfo::fromName(host.split(":").first());
lm's avatar
lm committed
125
        if (info.error() == QHostInfo::NoError)
126
        {
lm's avatar
lm committed
127 128 129 130
            // Add host
            QList<QHostAddress> hostAddresses = info.addresses();
            QHostAddress address;
            for (int i = 0; i < hostAddresses.size(); i++)
131
            {
lm's avatar
lm committed
132 133 134 135 136
                // Exclude loopback IPv4 and all IPv6 addresses
                if (!hostAddresses.at(i).toString().contains(":"))
                {
                    address = hostAddresses.at(i);
                }
137
            }
lm's avatar
lm committed
138
            hosts.append(address);
139
            //qDebug() << "Address:" << address.toString();
lm's avatar
lm committed
140 141
            // Set port according to user input
            ports.append(host.split(":").last().toInt());
142
        }
143 144 145
    }
    else
    {
146
        QHostInfo info = QHostInfo::fromName(host);
lm's avatar
lm committed
147 148 149 150 151 152 153
        if (info.error() == QHostInfo::NoError)
        {
            // Add host
            hosts.append(info.addresses().first());
            // Set port according to default (this port)
            ports.append(port);
        }
154 155 156 157 158 159 160 161 162 163 164
    }
}

void UDPLink::removeHost(const QString& hostname)
{
    QString host = hostname;
    if (host.contains(":")) host = host.split(":").first();
    host = host.trimmed();
    QHostInfo info = QHostInfo::fromName(host);
    QHostAddress address;
    QList<QHostAddress> hostAddresses = info.addresses();
165 166
    for (int i = 0; i < hostAddresses.size(); i++)
    {
167
        // Exclude loopback IPv4 and all IPv6 addresses
168 169
        if (!hostAddresses.at(i).toString().contains(":"))
        {
170 171 172
            address = hostAddresses.at(i);
        }
    }
173 174 175 176
    for (int i = 0; i < hosts.count(); ++i)
    {
        if (hosts.at(i) == address)
        {
177 178 179
            hosts.removeAt(i);
            ports.removeAt(i);
        }
180
    }
pixhawk's avatar
pixhawk committed
181 182 183 184 185
}

void UDPLink::writeBytes(const char* data, qint64 size)
{
    // Broadcast to all connected systems
lm's avatar
lm committed
186 187
    for (int h = 0; h < hosts.size(); h++)
    {
188 189
        QHostAddress currentHost = hosts.at(h);
        quint16 currentPort = ports.at(h);
lm's avatar
lm committed
190 191
//#define UDPLINK_DEBUG
#ifdef UDPLINK_DEBUG
lm's avatar
lm committed
192 193
        QString bytes;
        QString ascii;
194 195
        for (int i=0; i<size; i++)
        {
lm's avatar
lm committed
196 197
            unsigned char v = data[i];
            bytes.append(QString().sprintf("%02x ", v));
lm's avatar
lm committed
198 199 200 201 202 203 204 205
            if (data[i] > 31 && data[i] < 127)
            {
                ascii.append(data[i]);
            }
            else
            {
                ascii.append(219);
            }
206
        }
lm's avatar
lm committed
207 208 209
        qDebug() << "Sent" << size << "bytes to" << currentHost.toString() << ":" << currentPort << "data:";
        qDebug() << bytes;
        qDebug() << "ASCII:" << ascii;
lm's avatar
lm committed
210
#endif
211
        socket->writeDatagram(data, size, currentHost, currentPort);
212 213 214 215

        // Log the amount and time written out for future data rate calculations.
        QMutexLocker dataRateLocker(&dataRateMutex);
        logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch());
216
    }
pixhawk's avatar
pixhawk committed
217 218 219 220 221 222 223 224
}

/**
 * @brief Read a number of bytes from the interface.
 *
 * @param data Pointer to the data byte array to write the bytes to
 * @param maxLength The maximum number of bytes to write
 **/
225
void UDPLink::readBytes()
pixhawk's avatar
pixhawk committed
226
{
227 228 229 230
    while (socket->hasPendingDatagrams())
    {
        QByteArray datagram;
        datagram.resize(socket->pendingDatagramSize());
pixhawk's avatar
pixhawk committed
231

232 233 234
        QHostAddress sender;
        quint16 senderPort;
        socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
pixhawk's avatar
pixhawk committed
235

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

239 240 241 242 243
        // Log this data reception for this timestep
        QMutexLocker dataRateLocker(&dataRateMutex);
        logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, datagram.length(), QDateTime::currentMSecsSinceEpoch());


244 245 246 247 248 249 250 251 252
//        // 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
253 254


255 256 257 258 259 260 261 262 263 264 265 266
        // Add host to broadcast list if not yet present
        if (!hosts.contains(sender))
        {
            hosts.append(sender);
            ports.append(senderPort);
            //        ports->insert(sender, senderPort);
        }
        else
        {
            int index = hosts.indexOf(sender);
            ports.replace(index, senderPort);
        }
pixhawk's avatar
pixhawk committed
267 268 269 270 271 272 273 274
    }
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
275
bool UDPLink::_disconnect(void)
pixhawk's avatar
pixhawk committed
276
{
oberion's avatar
oberion committed
277 278 279
	this->quit();
	this->wait();

280
    if (socket) {
Don Gagne's avatar
Don Gagne committed
281
		// Make sure delete happen on correct thread
282
		socket->deleteLater();
oberion's avatar
oberion committed
283
		socket = NULL;
284
        emit disconnected();
oberion's avatar
oberion committed
285
	}
pixhawk's avatar
pixhawk committed
286 287 288 289 290 291 292 293 294 295 296

    connectState = false;

    return !connectState;
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
297
bool UDPLink::_connect(void)
pixhawk's avatar
pixhawk committed
298
{
oberion's avatar
oberion committed
299 300 301 302 303
	if(this->isRunning())
	{
		this->quit();
		this->wait();
	}
304
    bool connected = true;
oberion's avatar
oberion committed
305
    start(HighPriority);
306
    return connected;
oberion's avatar
oberion committed
307 308 309 310 311
}

bool UDPLink::hardwareConnect(void)
{
	socket = new QUdpSocket();
pixhawk's avatar
pixhawk committed
312

pixhawk's avatar
pixhawk committed
313 314 315 316 317 318 319 320 321
    //Check if we are using a multicast-address
//    bool multicast = false;
//    if (host.isInSubnet(QHostAddress("224.0.0.0"),4))
//    {
//        multicast = true;
//        connectState = socket->bind(port, QUdpSocket::ShareAddress);
//    }
//    else
//    {
322
    connectState = socket->bind(host, port);
pixhawk's avatar
pixhawk committed
323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349
//    }

    //Provides Multicast functionality to UdpSocket
    /* not working yet
    if (multicast)
    {
        int sendingFd = socket->socketDescriptor();

        if (sendingFd != -1)
        {
            // set up destination address
            struct sockaddr_in sendAddr;
            memset(&sendAddr,0,sizeof(sendAddr));
            sendAddr.sin_family=AF_INET;
            sendAddr.sin_addr.s_addr=inet_addr(HELLO_GROUP);
            sendAddr.sin_port=htons(port);

            // set TTL
            unsigned int ttl = 1; // restricted to the same subnet
            if (setsockopt(sendingFd, IPPROTO_IP, IP_MULTICAST_TTL, (unsigned int*)&ttl, sizeof(ttl) ) < 0)
            {
                std::cout << "TTL failed\n";
            }
        }
    }
    */

pixhawk's avatar
pixhawk committed
350
    //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
351
    QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
pixhawk's avatar
pixhawk committed
352

353
    if (connectState) {
354 355
        emit connected();
    }
oberion's avatar
oberion committed
356
	return connectState;
pixhawk's avatar
pixhawk committed
357 358
}

oberion's avatar
oberion committed
359

pixhawk's avatar
pixhawk committed
360 361 362 363 364
/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
365
bool UDPLink::isConnected() const
366
{
pixhawk's avatar
pixhawk committed
367 368 369
    return connectState;
}

370
int UDPLink::getId() const
pixhawk's avatar
pixhawk committed
371 372 373 374
{
    return id;
}

375
QString UDPLink::getName() const
pixhawk's avatar
pixhawk committed
376 377 378 379 380 381 382 383 384 385
{
    return name;
}

void UDPLink::setName(QString name)
{
    this->name = name;
    emit nameChanged(this->name);
}

386

387
qint64 UDPLink::getConnectionSpeed() const
388
{
pixhawk's avatar
pixhawk committed
389
    return 54000000; // 54 Mbit
390 391 392 393 394 395 396 397 398 399 400
}

qint64 UDPLink::getCurrentInDataRate() const
{
    return 0;
}

qint64 UDPLink::getCurrentOutDataRate() const
{
    return 0;
}