TCPLink.cc 8.65 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 char *data, qint16 size)
Don Gagne's avatar
Don Gagne committed
69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84
{
    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);
        }
    }
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 91 92 93
}
#endif

void TCPLink::writeBytes(const char* data, qint64 size)
{
#ifdef TCPLINK_READWRITE_DEBUG
94
    _writeDebugBytes(data, size);
Don Gagne's avatar
Don Gagne committed
95
#endif
96
    _socket->write(data, size);
97
    _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
Don Gagne's avatar
Don Gagne committed
98 99 100 101 102 103 104 105 106 107
}

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

/**
 * @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
127
void TCPLink::_disconnect(void)
Don Gagne's avatar
Don Gagne committed
128
{
129 130
    quit();
    wait();
Don Gagne's avatar
Don Gagne committed
131
    if (_socket) {
132
        _socketIsConnected = false;
133 134
        _socket->deleteLater(); // Make sure delete happens on correct thread
        _socket = NULL;
135
        emit disconnected();
136
    }
Don Gagne's avatar
Don Gagne committed
137 138 139 140 141 142 143
}

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

155
bool TCPLink::_hardwareConnect()
Don Gagne's avatar
Don Gagne committed
156
{
157
    Q_ASSERT(_socket == NULL);
158
    _socket = new QTcpSocket();
159 160

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

184
void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
Don Gagne's avatar
Don Gagne committed
185
{
186
    Q_UNUSED(socketError);
187
    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
188 189 190 191 192 193 194 195 196
}

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

QString TCPLink::getName() const
{
202
    return _config->name();
Don Gagne's avatar
Don Gagne committed
203 204
}

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

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

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

220 221 222 223 224 225 226 227 228 229 230
void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

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

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

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

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
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("");
}

277 278 279 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
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;
}

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

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