TCPLink.cc 8.11 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14 15 16 17 18 19

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

22 23 24 25
/// @file
///     @brief TCP link type for SITL support
///
///     @author Don Gagne <don@thegagnes.com>
Don Gagne's avatar
Don Gagne committed
26

27 28 29 30
TCPLink::TCPLink(TCPConfiguration *config)
    : _config(config)
    , _socket(NULL)
    , _socketIsConnected(false)
Don Gagne's avatar
Don Gagne committed
31
{
32
    Q_ASSERT(_config != NULL);
33 34 35
    // 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
36
    //qDebug() << "TCP Created " << _config->name();
Don Gagne's avatar
Don Gagne committed
37 38 39 40
}

TCPLink::~TCPLink()
{
41
    _disconnect();
Lorenz Meier's avatar
Lorenz Meier committed
42 43 44 45
    // Tell the thread to exit
    quit();
    // Wait for it to exit
    wait();
Don Gagne's avatar
Don Gagne committed
46 47 48 49
}

void TCPLink::run()
{
50
    _hardwareConnect();
51
    exec();
Don Gagne's avatar
Don Gagne committed
52 53
}

Don Gagne's avatar
Don Gagne committed
54
#ifdef TCPLINK_READWRITE_DEBUG
55
void TCPLink::_writeDebugBytes(const QByteArray data)
Don Gagne's avatar
Don Gagne committed
56 57 58
{
    QString bytes;
    QString ascii;
59
    for (int i=0, size = data.size(); i<size; i++)
Don Gagne's avatar
Don Gagne committed
60 61 62 63 64 65 66 67 68 69 70 71
    {
        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);
        }
    }
72
    qDebug() << "Sent" << size << "bytes to" << _config->address().toString() << ":" << _config->port() << "data:";
Don Gagne's avatar
Don Gagne committed
73 74
    qDebug() << bytes;
    qDebug() << "ASCII:" << ascii;
Don Gagne's avatar
Don Gagne committed
75 76 77
}
#endif

78
void TCPLink::_writeBytes(const QByteArray data)
Don Gagne's avatar
Don Gagne committed
79 80
{
#ifdef TCPLINK_READWRITE_DEBUG
81
    _writeDebugBytes(data);
Don Gagne's avatar
Don Gagne committed
82
#endif
83 84 85
    if (!_socket)
        return;

86 87
    _socket->write(data);
    _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
Don Gagne's avatar
Don Gagne committed
88 89 90 91 92 93 94 95 96 97
}

/**
 * @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()
{
98
    qint64 byteCount = _socket->bytesAvailable();
Don Gagne's avatar
Don Gagne committed
99 100 101 102
    if (byteCount)
    {
        QByteArray buffer;
        buffer.resize(byteCount);
103
        _socket->read(buffer.data(), buffer.size());
Don Gagne's avatar
Don Gagne committed
104
        emit bytesReceived(this, buffer);
105
        _logInputDataRate(byteCount, QDateTime::currentMSecsSinceEpoch());
Don Gagne's avatar
Don Gagne committed
106 107 108
#ifdef TCPLINK_READWRITE_DEBUG
        writeDebugBytes(buffer.data(), buffer.size());
#endif
Don Gagne's avatar
Don Gagne committed
109 110 111 112 113 114 115 116
    }
}

/**
 * @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
117
void TCPLink::_disconnect(void)
Don Gagne's avatar
Don Gagne committed
118
{
119 120
    quit();
    wait();
Don Gagne's avatar
Don Gagne committed
121
    if (_socket) {
122
        _socketIsConnected = false;
123 124
        _socket->deleteLater(); // Make sure delete happens on correct thread
        _socket = NULL;
125
        emit disconnected();
126
    }
Don Gagne's avatar
Don Gagne committed
127 128 129 130 131 132 133
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
134
bool TCPLink::_connect(void)
Don Gagne's avatar
Don Gagne committed
135
{
136 137 138 139 140
    if (isRunning())
    {
        quit();
        wait();
    }
141 142
    start(HighPriority);
    return true;
Don Gagne's avatar
Don Gagne committed
143 144
}

145
bool TCPLink::_hardwareConnect()
Don Gagne's avatar
Don Gagne committed
146
{
147
    Q_ASSERT(_socket == NULL);
148
    _socket = new QTcpSocket();
149 150

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

174
void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
Don Gagne's avatar
Don Gagne committed
175
{
176
    Q_UNUSED(socketError);
177
    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
178 179 180 181 182 183 184 185 186
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
bool TCPLink::isConnected() const
{
187
    return _socketIsConnected;
Don Gagne's avatar
Don Gagne committed
188 189 190 191
}

QString TCPLink::getName() const
{
192
    return _config->name();
Don Gagne's avatar
Don Gagne committed
193 194
}

195
qint64 TCPLink::getConnectionSpeed() const
Don Gagne's avatar
Don Gagne committed
196 197
{
    return 54000000; // 54 Mbit
198 199 200 201 202 203 204 205 206 207 208
}

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

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

210 211 212 213 214 215 216 217 218 219 220
void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

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

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

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

234 235 236 237 238 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
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("");
}

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 294 295 296 297
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;
}

298 299
void TCPConfiguration::setHost(const QString host)
{
300 301 302 303 304 305
    QString ipAdd = get_ip_address(host);
    if(ipAdd.isEmpty()) {
        qWarning() << "TCP:" << "Could not resolve host:" << host;
    } else {
        _address = ipAdd;
    }
306 307
}

308 309 310 311 312 313 314 315 316 317 318
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);
319
    _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt();
320 321 322 323 324 325 326 327 328 329 330 331 332 333
    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();
        }
    }
}