TCPLink.cc 7.89 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1 2 3 4
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
5
 (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
Don Gagne's avatar
Don Gagne committed
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
 
 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
TCPLink::TCPLink(TCPConfiguration *config)
    : _config(config)
    , _socket(NULL)
    , _socketIsConnected(false)
Don Gagne's avatar
Don Gagne committed
44
{
45
    Q_ASSERT(_config != NULL);
46 47 48
    // 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);
49
    _linkId = getNextLinkId();
50
    qDebug() << "TCP Created " << _config->name();
Don Gagne's avatar
Don Gagne committed
51 52 53 54
}

TCPLink::~TCPLink()
{
55
    _disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
56 57 58 59
    // Tell the thread to exit
    quit();
    // Wait for it to exit
    wait();
Don Gagne's avatar
Don Gagne committed
60 61 62 63
}

void TCPLink::run()
{
64
    _hardwareConnect();
Don Gagne's avatar
Don Gagne committed
65 66 67
	exec();
}

Don Gagne's avatar
Don Gagne committed
68
#ifdef TCPLINK_READWRITE_DEBUG
69
void TCPLink::_writeDebugBytes(const char *data, qint16 size)
Don Gagne's avatar
Don Gagne committed
70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85
{
    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);
        }
    }
86
    qDebug() << "Sent" << size << "bytes to" << _config->address().toString() << ":" << _config->port() << "data:";
Don Gagne's avatar
Don Gagne committed
87 88
    qDebug() << bytes;
    qDebug() << "ASCII:" << ascii;
Don Gagne's avatar
Don Gagne committed
89 90 91 92 93 94
}
#endif

void TCPLink::writeBytes(const char* data, qint64 size)
{
#ifdef TCPLINK_READWRITE_DEBUG
95
    _writeDebugBytes(data, size);
Don Gagne's avatar
Don Gagne committed
96
#endif
97
    _socket->write(data, size);
98 99 100
    // 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
101 102 103 104 105 106 107 108 109 110
}

/**
 * @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()
{
111
    qint64 byteCount = _socket->bytesAvailable();
Don Gagne's avatar
Don Gagne committed
112 113 114 115
    if (byteCount)
    {
        QByteArray buffer;
        buffer.resize(byteCount);
116
        _socket->read(buffer.data(), buffer.size());
Don Gagne's avatar
Don Gagne committed
117
        emit bytesReceived(this, buffer);
118 119 120
        // 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
121 122 123
#ifdef TCPLINK_READWRITE_DEBUG
        writeDebugBytes(buffer.data(), buffer.size());
#endif
Don Gagne's avatar
Don Gagne committed
124 125 126 127 128 129 130 131
    }
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
132
bool TCPLink::_disconnect(void)
Don Gagne's avatar
Don Gagne committed
133
{
134 135 136
	quit();
	wait();
    if (_socket)
Don Gagne's avatar
Don Gagne committed
137
	{
138
        _socketIsConnected = false;
Don Gagne's avatar
Don Gagne committed
139
		_socket->deleteLater(); // Make sure delete happens on correct thread
140 141
		_socket = NULL;
        emit disconnected();
Don Gagne's avatar
Don Gagne committed
142 143 144 145 146 147 148 149 150
	}
    return true;
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
151
bool TCPLink::_connect(void)
Don Gagne's avatar
Don Gagne committed
152
{
153
	if (isRunning())
Don Gagne's avatar
Don Gagne committed
154
	{
155 156
		quit();
		wait();
Don Gagne's avatar
Don Gagne committed
157
	}
158 159
    start(HighPriority);
    return true;
Don Gagne's avatar
Don Gagne committed
160 161
}

162
bool TCPLink::_hardwareConnect()
Don Gagne's avatar
Don Gagne committed
163
{
164 165 166
    Q_ASSERT(_socket == NULL);
	_socket = new QTcpSocket();
    QSignalSpy errorSpy(_socket, SIGNAL(error(QAbstractSocket::SocketError)));
167
    _socket->connectToHost(_config->address(), _config->port());
168 169
    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
170
    // Give the socket a second to connect to the other side otherwise error out
171
    if (!_socket->waitForConnected(1000))
Don Gagne's avatar
Don Gagne committed
172
    {
173 174 175
        // 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) {
176
            emit communicationError(tr("Link Error"), QString("Error on link %1. Connection failed").arg(getName()));
177 178 179
        }
        delete _socket;
        _socket = NULL;
Don Gagne's avatar
Don Gagne committed
180 181
        return false;
    }
182 183
    _socketIsConnected = true;
    emit connected();
Don Gagne's avatar
Don Gagne committed
184 185 186
    return true;
}

187
void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
Don Gagne's avatar
Don Gagne committed
188
{
189
    Q_UNUSED(socketError);
190
    emit communicationError(tr("Link Error"), QString("Error on link %1. Error on socket: %2.").arg(getName()).arg(_socket->errorString()));
Don Gagne's avatar
Don Gagne committed
191 192 193 194 195 196 197 198 199
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
bool TCPLink::isConnected() const
{
200
    return _socketIsConnected;
Don Gagne's avatar
Don Gagne committed
201 202 203 204
}

int TCPLink::getId() const
{
205
    return _linkId;
Don Gagne's avatar
Don Gagne committed
206 207 208 209
}

QString TCPLink::getName() const
{
210
    return _config->name();
Don Gagne's avatar
Don Gagne committed
211 212
}

213
qint64 TCPLink::getConnectionSpeed() const
Don Gagne's avatar
Don Gagne committed
214 215
{
    return 54000000; // 54 Mbit
216 217 218 219 220 221 222 223 224 225 226
}

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

qint64 TCPLink::getCurrentOutDataRate() const
{
    return 0;
}
227

228 229 230 231 232 233 234 235 236 237 238
void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

void TCPLink::waitForReadyRead(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForReadyRead(msecs);
}
239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293

void TCPLink::_restartConnection()
{
    if(this->isConnected())
    {
        _disconnect();
        _connect();
    }
}

//--------------------------------------------------------------------------
//-- TCPConfiguration

TCPConfiguration::TCPConfiguration(const QString& name) : LinkConfiguration(name)
{
    _port    = QGC_TCP_PORT;
    _address = QHostAddress::Any;
}

TCPConfiguration::TCPConfiguration(TCPConfiguration* source) : LinkConfiguration(source)
{
    _port    = source->port();
    _address = source->address();
}

void TCPConfiguration::copyFrom(LinkConfiguration *source)
{
    LinkConfiguration::copyFrom(source);
    TCPConfiguration* usource = dynamic_cast<TCPConfiguration*>(source);
    Q_ASSERT(usource != NULL);
    _port    = usource->port();
    _address = usource->address();
}

void TCPConfiguration::setPort(quint16 port)
{
    _port = port;
}

void TCPConfiguration::setAddress(const QHostAddress& address)
{
    _address = address;
}

void TCPConfiguration::saveSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
    settings.setValue("port", (int)_port);
    settings.setValue("host", address().toString());
    settings.endGroup();
}

void TCPConfiguration::loadSettings(QSettings& settings, const QString& root)
{
    settings.beginGroup(root);
294
    _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt();
295 296 297 298 299 300 301 302 303 304 305 306 307 308
    QString address = settings.value("host", _address.toString()).toString();
    _address = address;
    settings.endGroup();
}

void TCPConfiguration::updateSettings()
{
    if(_link) {
        TCPLink* ulink = dynamic_cast<TCPLink*>(_link);
        if(ulink) {
            ulink->_restartConnection();
        }
    }
}