TCPLink.cc 8.18 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 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
TCPLink::TCPLink(SharedLinkConfigurationPointer& config)
    : LinkInterface(config)
    , _tcpConfig(qobject_cast<TCPConfiguration*>(config.data()))
30
    , _socket(nullptr)
31
    , _socketIsConnected(false)
Don Gagne's avatar
Don Gagne committed
32
{
33
    Q_ASSERT(_tcpConfig);
34
    moveToThread(this);
Don Gagne's avatar
Don Gagne committed
35 36 37 38
}

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

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

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

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

82 83
    if (_socket) {
        _socket->write(data);
84
        emit bytesSent(this, data);
85 86
        _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
    }
Don Gagne's avatar
Don Gagne committed
87 88 89 90 91 92 93 94 95 96
}

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

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

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

148
bool TCPLink::_hardwareConnect()
Don Gagne's avatar
Don Gagne committed
149
{
150
    Q_ASSERT(_socket == nullptr);
151
    _socket = new QTcpSocket();
152 153

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

177
void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
Don Gagne's avatar
Don Gagne committed
178
{
179
    Q_UNUSED(socketError);
180
    emit communicationError(tr("Link Error"), tr("Error on link %1. Error on socket: %2.").arg(getName()).arg(_socket->errorString()));
Don Gagne's avatar
Don Gagne committed
181 182 183 184 185 186 187 188 189
}

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

QString TCPLink::getName() const
{
195
    return _tcpConfig->name();
Don Gagne's avatar
Don Gagne committed
196 197
}

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

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

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

213 214 215 216 217 218 219 220 221 222 223
void TCPLink::waitForBytesWritten(int msecs)
{
    Q_ASSERT(_socket);
    _socket->waitForBytesWritten(msecs);
}

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

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

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

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
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();
            }
        }
    }
266
    return {};
267 268
}

269 270 271 272 273 274 275 276 277 278 279 280 281 282 283
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);
284
    auto* usource = qobject_cast<TCPConfiguration*>(source);
285
    Q_ASSERT(usource != nullptr);
286 287 288 289 290 291 292 293 294 295 296 297 298 299
    _port    = usource->port();
    _address = usource->address();
}

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

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

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

310 311 312 313 314 315 316 317 318 319 320
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);
321
    _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt();
322 323 324 325 326 327 328 329
    QString address = settings.value("host", _address.toString()).toString();
    _address = address;
    settings.endGroup();
}

void TCPConfiguration::updateSettings()
{
    if(_link) {
330
        auto* ulink = qobject_cast<TCPLink*>(_link);
331 332 333 334 335
        if(ulink) {
            ulink->_restartConnection();
        }
    }
}