TCPLink.cc 7.12 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 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.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 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
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
#include <iostream>
#include "TCPLink.h"
#include "LinkManager.h"
#include "QGC.h"
#include <QHostInfo>
33
#include <QSignalSpy>
Don Gagne's avatar
Don Gagne committed
34

35 36 37 38
/// @file
///     @brief TCP link type for SITL support
///
///     @author Don Gagne <don@thegagnes.com>
Don Gagne's avatar
Don Gagne committed
39

40 41 42 43 44
TCPLink::TCPLink(QHostAddress hostAddress, quint16 socketPort) :
    _hostAddress(hostAddress),
    _port(socketPort),
    _socket(NULL),
    _socketIsConnected(false)
Don Gagne's avatar
Don Gagne committed
45
{
46 47 48 49
    // 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);

50 51
    _linkId = getNextLinkId();
    _resetName();
Don Gagne's avatar
Don Gagne committed
52
    
53
    qDebug() << "TCP Created " << _name;
Don Gagne's avatar
Don Gagne committed
54 55 56 57 58
}

TCPLink::~TCPLink()
{
    disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
59 60 61 62 63 64

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

65
	deleteLater();
Don Gagne's avatar
Don Gagne committed
66 67 68 69
}

void TCPLink::run()
{
70 71
    _hardwareConnect();

Don Gagne's avatar
Don Gagne committed
72 73 74
	exec();
}

75
void TCPLink::setHostAddress(QHostAddress hostAddress)
Don Gagne's avatar
Don Gagne committed
76
{
77 78 79
    bool reconnect = false;
    
	if (this->isConnected()) {
Don Gagne's avatar
Don Gagne committed
80 81 82
		disconnect();
		reconnect = true;
	}
83 84 85 86 87
    
	_hostAddress = hostAddress;
    _resetName();
    
	if (reconnect) {
Don Gagne's avatar
Don Gagne committed
88 89 90 91
		connect();
	}
}

92 93 94 95 96
void TCPLink::setHostAddress(const QString& hostAddress)
{
    setHostAddress(QHostAddress(hostAddress));
}

Don Gagne's avatar
Don Gagne committed
97 98
void TCPLink::setPort(int port)
{
99 100 101
    bool reconnect = false;
    
	if (this->isConnected()) {
Don Gagne's avatar
Don Gagne committed
102 103 104
		disconnect();
		reconnect = true;
	}
105 106 107 108 109
    
	_port = port;
    _resetName();
    
	if (reconnect) {
Don Gagne's avatar
Don Gagne committed
110 111 112 113
		connect();
	}
}

Don Gagne's avatar
Don Gagne committed
114
#ifdef TCPLINK_READWRITE_DEBUG
115
void TCPLink::_writeDebugBytes(const char *data, qint16 size)
Don Gagne's avatar
Don Gagne committed
116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
{
    QString bytes;
    QString ascii;
    for (int i=0; i<size; i++)
    {
        unsigned char v = data[i];
        bytes.append(QString().sprintf("%02x ", v));
        if (data[i] > 31 && data[i] < 127)
        {
            ascii.append(data[i]);
        }
        else
        {
            ascii.append(219);
        }
    }
132
    qDebug() << "Sent" << size << "bytes to" << _hostAddress.toString() << ":" << _port << "data:";
Don Gagne's avatar
Don Gagne committed
133 134
    qDebug() << bytes;
    qDebug() << "ASCII:" << ascii;
Don Gagne's avatar
Don Gagne committed
135 136 137 138 139 140
}
#endif

void TCPLink::writeBytes(const char* data, qint64 size)
{
#ifdef TCPLINK_READWRITE_DEBUG
141
    _writeDebugBytes(data, size);
Don Gagne's avatar
Don Gagne committed
142
#endif
143
    _socket->write(data, size);
144 145 146 147

    // Log the amount and time written out for future data rate calculations.
    QMutexLocker dataRateLocker(&dataRateMutex);
    logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch());
Don Gagne's avatar
Don Gagne committed
148 149 150 151 152 153 154 155 156 157
}

/**
 * @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
 **/
void TCPLink::readBytes()
{
158
    qint64 byteCount = _socket->bytesAvailable();
Don Gagne's avatar
Don Gagne committed
159 160 161 162 163 164
    
    if (byteCount)
    {
        QByteArray buffer;
        buffer.resize(byteCount);
        
165
        _socket->read(buffer.data(), buffer.size());
Don Gagne's avatar
Don Gagne committed
166 167
        
        emit bytesReceived(this, buffer);
Don Gagne's avatar
Don Gagne committed
168

169 170 171 172
        // Log the amount and time received for future data rate calculations.
        QMutexLocker dataRateLocker(&dataRateMutex);
        logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, byteCount, QDateTime::currentMSecsSinceEpoch());

Don Gagne's avatar
Don Gagne committed
173 174 175
#ifdef TCPLINK_READWRITE_DEBUG
        writeDebugBytes(buffer.data(), buffer.size());
#endif
Don Gagne's avatar
Don Gagne committed
176 177 178 179 180 181 182 183 184 185
    }
}

/**
 * @brief Get the number of bytes to read.
 *
 * @return The number of bytes to read
 **/
qint64 TCPLink::bytesAvailable()
{
186
    return _socket->bytesAvailable();
Don Gagne's avatar
Don Gagne committed
187 188 189 190 191 192 193 194 195
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
bool TCPLink::disconnect()
{
196 197
	quit();
	wait();
Don Gagne's avatar
Don Gagne committed
198
    
199
    if (_socket)
Don Gagne's avatar
Don Gagne committed
200
	{
201
        _socketIsConnected = false;
Don Gagne's avatar
Don Gagne committed
202
		_socket->deleteLater(); // Make sure delete happens on correct thread
203 204 205 206
		_socket = NULL;

        emit disconnected();
        emit connected(false);
Don Gagne's avatar
Don Gagne committed
207 208 209 210 211 212 213 214 215 216 217 218
	}
    
    return true;
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
bool TCPLink::connect()
{
219
	if (isRunning())
Don Gagne's avatar
Don Gagne committed
220
	{
221 222
		quit();
		wait();
Don Gagne's avatar
Don Gagne committed
223
	}
224 225 226 227

    start(HighPriority);

    return true;
Don Gagne's avatar
Don Gagne committed
228 229
}

230
bool TCPLink::_hardwareConnect(void)
Don Gagne's avatar
Don Gagne committed
231
{
232 233
    Q_ASSERT(_socket == NULL);
	_socket = new QTcpSocket();
Don Gagne's avatar
Don Gagne committed
234
    
235
    QSignalSpy errorSpy(_socket, SIGNAL(error(QAbstractSocket::SocketError)));
Don Gagne's avatar
Don Gagne committed
236
    
237 238 239 240
    _socket->connectToHost(_hostAddress, _port);
    
    QObject::connect(_socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
    QObject::connect(_socket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(_socketError(QAbstractSocket::SocketError)));
Don Gagne's avatar
Don Gagne committed
241 242
    
    // Give the socket a second to connect to the other side otherwise error out
243
    if (!_socket->waitForConnected(1000))
Don Gagne's avatar
Don Gagne committed
244
    {
245 246 247 248 249 250 251
        // Whether a failed connection emits an error signal or not is platform specific.
        // So in cases where it is not emitted, we emit one ourselves.
        if (errorSpy.count() == 0) {
            emit communicationError(getName(), "Connection failed");
        }
        delete _socket;
        _socket = NULL;
Don Gagne's avatar
Don Gagne committed
252 253
        return false;
    }
Don Gagne's avatar
Don Gagne committed
254
    
255
    _socketIsConnected = true;
Don Gagne's avatar
Don Gagne committed
256
    emit connected(true);
257
    emit connected();
Don Gagne's avatar
Don Gagne committed
258

Don Gagne's avatar
Don Gagne committed
259 260 261
    return true;
}

262
void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
Don Gagne's avatar
Don Gagne committed
263
{
264
    Q_UNUSED(socketError);
265
    emit communicationError(getName(), "Error on socket: " + _socket->errorString());
Don Gagne's avatar
Don Gagne committed
266 267 268 269 270 271 272 273 274
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
bool TCPLink::isConnected() const
{
275
    return _socketIsConnected;
Don Gagne's avatar
Don Gagne committed
276 277 278 279
}

int TCPLink::getId() const
{
280
    return _linkId;
Don Gagne's avatar
Don Gagne committed
281 282 283 284
}

QString TCPLink::getName() const
{
285
    return _name;
Don Gagne's avatar
Don Gagne committed
286 287
}

288
qint64 TCPLink::getConnectionSpeed() const
Don Gagne's avatar
Don Gagne committed
289 290
{
    return 54000000; // 54 Mbit
291 292 293 294 295 296 297 298 299 300 301
}

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

qint64 TCPLink::getCurrentOutDataRate() const
{
    return 0;
}
302 303 304 305 306

void TCPLink::_resetName(void)
{
    _name = QString("TCP Link (host:%1 port:%2)").arg(_hostAddress.toString()).arg(_port);
    emit nameChanged(_name);
Don Gagne's avatar
Don Gagne committed
307
}
308 309 310 311 312 313 314 315 316 317 318 319

void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

void TCPLink::waitForReadyRead(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForReadyRead(msecs);
}