TCPLink.cc 7.43 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * 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
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
#include <iostream>
#include "TCPLink.h"
#include "LinkManager.h"
#include "QGC.h"
#include <QHostInfo>
19
#include <QSignalSpy>
Don Gagne's avatar
Don Gagne committed
20

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

26
TCPLink::TCPLink(SharedLinkConfigurationPtr& config)
27
    : LinkInterface(config)
28
    , _tcpConfig(qobject_cast<TCPConfiguration*>(config.get()))
29
    , _socket(nullptr)
30
    , _socketIsConnected(false)
Don Gagne's avatar
Don Gagne committed
31
{
32
    Q_ASSERT(_tcpConfig);
33
    moveToThread(this);
Don Gagne's avatar
Don Gagne committed
34 35 36 37
}

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

void TCPLink::run()
{
47
    _hardwareConnect();
48
    exec();
Don Gagne's avatar
Don Gagne committed
49 50
}

Don Gagne's avatar
Don Gagne committed
51
#ifdef TCPLINK_READWRITE_DEBUG
52
void TCPLink::_writeDebugBytes(const QByteArray data)
Don Gagne's avatar
Don Gagne committed
53 54 55
{
    QString bytes;
    QString ascii;
56
    for (int i=0, size = data.size(); i<size; i++)
Don Gagne's avatar
Don Gagne committed
57 58
    {
        unsigned char v = data[i];
59
        bytes.append(QString::asprintf("%02x ", v));
Don Gagne's avatar
Don Gagne committed
60 61 62 63 64 65 66 67 68
        if (data[i] > 31 && data[i] < 127)
        {
            ascii.append(data[i]);
        }
        else
        {
            ascii.append(219);
        }
    }
69
    qDebug() << "Sent" << size << "bytes to" << _tcpConfig->address().toString() << ":" << _tcpConfig->port() << "data:";
Don Gagne's avatar
Don Gagne committed
70 71
    qDebug() << bytes;
    qDebug() << "ASCII:" << ascii;
Don Gagne's avatar
Don Gagne committed
72 73 74
}
#endif

75
void TCPLink::_writeBytes(const QByteArray data)
Don Gagne's avatar
Don Gagne committed
76 77
{
#ifdef TCPLINK_READWRITE_DEBUG
78
    _writeDebugBytes(data);
Don Gagne's avatar
Don Gagne committed
79
#endif
80

81 82
    if (_socket) {
        _socket->write(data);
83
        emit bytesSent(this, data);
84
    }
Don Gagne's avatar
Don Gagne committed
85 86 87 88
}

void TCPLink::readBytes()
{
89 90 91 92 93 94 95 96
    if (_socket) {
        qint64 byteCount = _socket->bytesAvailable();
        if (byteCount)
        {
            QByteArray buffer;
            buffer.resize(byteCount);
            _socket->read(buffer.data(), buffer.size());
            emit bytesReceived(this, buffer);
Don Gagne's avatar
Don Gagne committed
97
#ifdef TCPLINK_READWRITE_DEBUG
98
            writeDebugBytes(buffer.data(), buffer.size());
Don Gagne's avatar
Don Gagne committed
99
#endif
100
        }
Don Gagne's avatar
Don Gagne committed
101 102 103
    }
}

104
void TCPLink::disconnect(void)
Don Gagne's avatar
Don Gagne committed
105
{
106 107
    quit();
    wait();
Don Gagne's avatar
Don Gagne committed
108
    if (_socket) {
109
        // This prevents stale signal from calling the link after it has been deleted
Don Gagne's avatar
Don Gagne committed
110
        QObject::disconnect(_socket, &QTcpSocket::readyRead, this, &TCPLink::readBytes);
111
        _socketIsConnected = false;
tzekian12's avatar
tzekian12 committed
112 113
        _socket->disconnectFromHost(); // Disconnect tcp
        _socket->waitForDisconnected();        
114
        _socket->deleteLater(); // Make sure delete happens on correct thread
115
        _socket = nullptr;
116
        emit disconnected();
117
    }
Don Gagne's avatar
Don Gagne committed
118 119
}

120
bool TCPLink::_connect(void)
Don Gagne's avatar
Don Gagne committed
121
{
122 123 124 125 126
    if (isRunning())
    {
        quit();
        wait();
    }
127 128
    start(HighPriority);
    return true;
Don Gagne's avatar
Don Gagne committed
129 130
}

131
bool TCPLink::_hardwareConnect()
Don Gagne's avatar
Don Gagne committed
132
{
133
    Q_ASSERT(_socket == nullptr);
134
    _socket = new QTcpSocket();
135

136
#if QT_VERSION < QT_VERSION_CHECK(5, 15, 0)
137
    QSignalSpy errorSpy(_socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error));
138 139 140 141
#else
    QSignalSpy errorSpy(_socket, &QAbstractSocket::errorOccurred);
#endif

142
    _socket->connectToHost(_tcpConfig->address(), _tcpConfig->port());
143 144
    QObject::connect(_socket, &QTcpSocket::readyRead, this, &TCPLink::readBytes);

145
#if QT_VERSION < QT_VERSION_CHECK(5, 15, 0)
146 147
    QObject::connect(_socket,static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error),
                     this, &TCPLink::_socketError);
148 149 150
#else
    QObject::connect(_socket, &QAbstractSocket::errorOccurred, this, &TCPLink::_socketError);
#endif
151

Don Gagne's avatar
Don Gagne committed
152
    // Give the socket a second to connect to the other side otherwise error out
153
    if (!_socket->waitForConnected(1000))
Don Gagne's avatar
Don Gagne committed
154
    {
155 156 157
        // 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) {
Don Gagne's avatar
Don Gagne committed
158
            emit communicationError(tr("Link Error"), tr("Error on link %1. Connection failed").arg(_config->name()));
159 160
        }
        delete _socket;
161
        _socket = nullptr;
Don Gagne's avatar
Don Gagne committed
162 163
        return false;
    }
164 165
    _socketIsConnected = true;
    emit connected();
Don Gagne's avatar
Don Gagne committed
166 167 168
    return true;
}

169
void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
Don Gagne's avatar
Don Gagne committed
170
{
171
    Q_UNUSED(socketError);
Don Gagne's avatar
Don Gagne committed
172
    emit communicationError(tr("Link Error"), tr("Error on link %1. Error on socket: %2.").arg(_config->name()).arg(_socket->errorString()));
Don Gagne's avatar
Don Gagne committed
173 174 175 176 177 178 179 180 181
}

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

185 186 187 188 189 190 191 192 193 194 195
void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

void TCPLink::waitForReadyRead(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForReadyRead(msecs);
}
196 197 198 199

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

200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228
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();
        for (int i = 0; i < hostAddresses.size(); i++)
        {
            // Exclude all IPv6 addresses
            if (!hostAddresses.at(i).toString().contains(":"))
            {
                return hostAddresses.at(i).toString();
            }
        }
    }
229
    return {};
230 231
}

232 233 234 235 236 237 238 239 240 241 242 243 244 245 246
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);
247
    auto* usource = qobject_cast<TCPConfiguration*>(source);
248
    Q_ASSERT(usource != nullptr);
249 250 251 252 253 254 255 256 257 258 259 260 261 262
    _port    = usource->port();
    _address = usource->address();
}

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

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

263 264
void TCPConfiguration::setHost(const QString host)
{
265 266 267 268
    QString ipAdd = get_ip_address(host);
    if(ipAdd.isEmpty()) {
        qWarning() << "TCP:" << "Could not resolve host:" << host;
    } else {
269
        _address = QHostAddress(ipAdd);
270
    }
271 272
}

273 274 275 276 277 278 279 280 281 282 283
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);
284
    _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt();
285
    QString address = settings.value("host", _address.toString()).toString();
286
    _address = QHostAddress(address);
287 288
    settings.endGroup();
}