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

UDPLink::~UDPLink()
{
    disconnect();
oberion's avatar
oberion committed
63
	this->deleteLater();
pixhawk's avatar
pixhawk committed
64 65 66 67 68 69 70 71
}

/**
 * @brief Runs the thread
 *
 **/
void UDPLink::run()
{
72 73 74
    hardwareConnect();

    exec();
pixhawk's avatar
pixhawk committed
75 76
}

oberion's avatar
oberion committed
77
void UDPLink::setAddress(QHostAddress host)
pixhawk's avatar
pixhawk committed
78
{
oberion's avatar
oberion committed
79 80 81 82 83 84 85 86 87 88 89
    bool reconnect(false);
	if(this->isConnected())
	{
		disconnect();
		reconnect = true;
	}
	this->host = host;
	if(reconnect)
	{
		connect();
	}
pixhawk's avatar
pixhawk committed
90 91
}

92
void UDPLink::setPort(int port)
pixhawk's avatar
pixhawk committed
93
{
oberion's avatar
oberion committed
94 95 96 97 98 99
	bool reconnect(false);
	if(this->isConnected())
	{
		disconnect();
		reconnect = true;
	}
pixhawk's avatar
pixhawk committed
100
    this->port = port;
oberion's avatar
oberion committed
101 102 103 104 105 106
	this->name = tr("UDP Link (port:%1)").arg(this->port);
	emit nameChanged(this->name);
	if(reconnect)
	{
		connect();
	}
107 108 109 110 111 112 113
}

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

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();
159 160
    for (int i = 0; i < hostAddresses.size(); i++)
    {
161
        // Exclude loopback IPv4 and all IPv6 addresses
162 163
        if (!hostAddresses.at(i).toString().contains(":"))
        {
164 165 166
            address = hostAddresses.at(i);
        }
    }
167 168 169 170
    for (int i = 0; i < hosts.count(); ++i)
    {
        if (hosts.at(i) == address)
        {
171 172 173
            hosts.removeAt(i);
            ports.removeAt(i);
        }
174
    }
pixhawk's avatar
pixhawk committed
175 176 177 178 179
}

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

        // Log the amount and time written out for future data rate calculations.
        QMutexLocker dataRateLocker(&dataRateMutex);
        logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch());
210
    }
pixhawk's avatar
pixhawk committed
211 212 213 214 215 216 217 218
}

/**
 * @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
 **/
219
void UDPLink::readBytes()
pixhawk's avatar
pixhawk committed
220
{
221 222 223 224
    while (socket->hasPendingDatagrams())
    {
        QByteArray datagram;
        datagram.resize(socket->pendingDatagramSize());
pixhawk's avatar
pixhawk committed
225

226 227 228
        QHostAddress sender;
        quint16 senderPort;
        socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
pixhawk's avatar
pixhawk committed
229

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

233 234 235 236 237
        // Log this data reception for this timestep
        QMutexLocker dataRateLocker(&dataRateMutex);
        logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, datagram.length(), QDateTime::currentMSecsSinceEpoch());


238 239 240 241 242 243 244 245 246
//        // 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
247 248


249 250 251 252 253 254 255 256 257 258 259 260
        // 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
261 262 263 264 265 266 267 268 269
    }
}


/**
 * @brief Get the number of bytes to read.
 *
 * @return The number of bytes to read
 **/
270 271
qint64 UDPLink::bytesAvailable()
{
pixhawk's avatar
pixhawk committed
272 273 274 275 276 277 278 279 280 281
    return socket->pendingDatagramSize();
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
bool UDPLink::disconnect()
{
oberion's avatar
oberion committed
282 283 284
	this->quit();
	this->wait();

285
        if(socket)
oberion's avatar
oberion committed
286 287 288 289
	{
		delete socket;
		socket = NULL;
	}
pixhawk's avatar
pixhawk committed
290 291 292 293 294 295 296 297 298 299 300 301 302 303 304

    connectState = false;

    emit disconnected();
    emit connected(false);
    return !connectState;
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
bool UDPLink::connect()
{
oberion's avatar
oberion committed
305 306 307 308 309
	if(this->isRunning())
	{
		this->quit();
		this->wait();
	}
310
    bool connected = true;
oberion's avatar
oberion committed
311
    start(HighPriority);
312
    return connected;
oberion's avatar
oberion committed
313 314 315 316 317
}

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

pixhawk's avatar
pixhawk committed
319 320 321 322 323 324 325 326 327
    //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
//    {
328
    connectState = socket->bind(host, port);
pixhawk's avatar
pixhawk committed
329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355
//    }

    //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
356
    //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
357
    QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
pixhawk's avatar
pixhawk committed
358 359

    emit connected(connectState);
360
    if (connectState) {
361 362
        emit connected();
    }
oberion's avatar
oberion committed
363
	return connectState;
pixhawk's avatar
pixhawk committed
364 365
}

oberion's avatar
oberion committed
366

pixhawk's avatar
pixhawk committed
367 368 369 370 371
/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
372
bool UDPLink::isConnected() const
373
{
pixhawk's avatar
pixhawk committed
374 375 376
    return connectState;
}

377
int UDPLink::getId() const
pixhawk's avatar
pixhawk committed
378 379 380 381
{
    return id;
}

382
QString UDPLink::getName() const
pixhawk's avatar
pixhawk committed
383 384 385 386 387 388 389 390 391 392
{
    return name;
}

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

393

394
qint64 UDPLink::getConnectionSpeed() const
395
{
pixhawk's avatar
pixhawk committed
396
    return 54000000; // 54 Mbit
397 398 399 400 401 402 403 404 405 406 407
}

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

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