TCPLink.cc 8.68 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1
/*=====================================================================
2

Don Gagne's avatar
Don Gagne committed
3
 QGroundControl Open Source Ground Control Station
4

5
 (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
6

Don Gagne's avatar
Don Gagne committed
7
 This file is part of the QGROUNDCONTROL project
8

Don Gagne's avatar
Don Gagne committed
9 10 11 12
 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.
13

Don Gagne's avatar
Don Gagne committed
14 15 16 17
 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.
18

Don Gagne's avatar
Don Gagne committed
19 20
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
21

Don Gagne's avatar
Don Gagne committed
22 23 24 25 26 27 28 29 30 31 32
 ======================================================================*/

#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);
dogmaphobic's avatar
dogmaphobic committed
49
    //qDebug() << "TCP Created " << _config->name();
Don Gagne's avatar
Don Gagne committed
50 51 52 53
}

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

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

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

91
void TCPLink::writeBytes(const QByteArray data)
Don Gagne's avatar
Don Gagne committed
92 93
{
#ifdef TCPLINK_READWRITE_DEBUG
94
    _writeDebugBytes(data);
Don Gagne's avatar
Don Gagne committed
95
#endif
96 97 98
    if (!_socket)
        return;

99 100
    _socket->write(data);
    _logOutputDataRate(data.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
        _logInputDataRate(byteCount, QDateTime::currentMSecsSinceEpoch());
Don Gagne's avatar
Don Gagne committed
119 120 121
#ifdef TCPLINK_READWRITE_DEBUG
        writeDebugBytes(buffer.data(), buffer.size());
#endif
Don Gagne's avatar
Don Gagne committed
122 123 124 125 126 127 128 129
    }
}

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

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

158
bool TCPLink::_hardwareConnect()
Don Gagne's avatar
Don Gagne committed
159
{
160
    Q_ASSERT(_socket == NULL);
161
    _socket = new QTcpSocket();
162 163

    QSignalSpy errorSpy(_socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error));
164
    _socket->connectToHost(_config->address(), _config->port());
165 166 167 168 169
    QObject::connect(_socket, &QTcpSocket::readyRead, this, &TCPLink::readBytes);

    QObject::connect(_socket,static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error),
                     this, &TCPLink::_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
}

QString TCPLink::getName() const
{
205
    return _config->name();
Don Gagne's avatar
Don Gagne committed
206 207
}

208
qint64 TCPLink::getConnectionSpeed() const
Don Gagne's avatar
Don Gagne committed
209 210
{
    return 54000000; // 54 Mbit
211 212 213 214 215 216 217 218 219 220 221
}

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

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

223 224 225 226 227 228 229 230 231 232 233
void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

void TCPLink::waitForReadyRead(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForReadyRead(msecs);
}
234 235 236 237 238 239 240 241 242 243 244 245 246

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

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

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
static bool is_ip(const QString& address)
{
    int a,b,c,d;
    if (sscanf(address.toStdString().c_str(), "%d.%d.%d.%d", &a, &b, &c, &d) != 4
            && strcmp("::1", address.toStdString().c_str())) {
        return false;
    } else {
        return true;
    }
}

static QString get_ip_address(const QString& address)
{
    if(is_ip(address))
        return address;
    // Need to look it up
    QHostInfo info = QHostInfo::fromName(address);
    if (info.error() == QHostInfo::NoError)
    {
        QList<QHostAddress> hostAddresses = info.addresses();
        QHostAddress address;
        for (int i = 0; i < hostAddresses.size(); i++)
        {
            // Exclude all IPv6 addresses
            if (!hostAddresses.at(i).toString().contains(":"))
            {
                return hostAddresses.at(i).toString();
            }
        }
    }
    return QString("");
}

280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310
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;
}

311 312
void TCPConfiguration::setHost(const QString host)
{
313 314 315 316 317 318
    QString ipAdd = get_ip_address(host);
    if(ipAdd.isEmpty()) {
        qWarning() << "TCP:" << "Could not resolve host:" << host;
    } else {
        _address = ipAdd;
    }
319 320
}

321 322 323 324 325 326 327 328 329 330 331
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);
332
    _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt();
333 334 335 336 337 338 339 340 341 342 343 344 345 346
    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();
        }
    }
}