/*===================================================================== QGroundControl Open Source Ground Control Station (c) 2009 - 2015 QGROUNDCONTROL PROJECT This file is part of the QGROUNDCONTROL project 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. 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. You should have received a copy of the GNU General Public License along with QGROUNDCONTROL. If not, see . ======================================================================*/ #include #include #include #include #include #include "TCPLink.h" #include "LinkManager.h" #include "QGC.h" #include #include /// @file /// @brief TCP link type for SITL support /// /// @author Don Gagne TCPLink::TCPLink(TCPConfiguration *config) : _config(config) , _socket(NULL) , _socketIsConnected(false) { Q_ASSERT(_config != NULL); // 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); //qDebug() << "TCP Created " << _config->name(); } TCPLink::~TCPLink() { _disconnect(); // Tell the thread to exit quit(); // Wait for it to exit wait(); } void TCPLink::run() { _hardwareConnect(); exec(); } #ifdef TCPLINK_READWRITE_DEBUG void TCPLink::_writeDebugBytes(const QByteArray data) { QString bytes; QString ascii; for (int i=0, size = data.size(); i 31 && data[i] < 127) { ascii.append(data[i]); } else { ascii.append(219); } } qDebug() << "Sent" << size << "bytes to" << _config->address().toString() << ":" << _config->port() << "data:"; qDebug() << bytes; qDebug() << "ASCII:" << ascii; } #endif void TCPLink::writeBytes(const QByteArray data) { #ifdef TCPLINK_READWRITE_DEBUG _writeDebugBytes(data); #endif if (!_socket) return; _socket->write(data); _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } /** * @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() { 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()); #ifdef TCPLINK_READWRITE_DEBUG writeDebugBytes(buffer.data(), buffer.size()); #endif } } /** * @brief Disconnect the connection. * * @return True if connection has been disconnected, false if connection couldn't be disconnected. **/ void TCPLink::_disconnect(void) { quit(); wait(); if (_socket) { _socketIsConnected = false; _socket->deleteLater(); // Make sure delete happens on correct thread _socket = NULL; emit disconnected(); } } /** * @brief Connect the connection. * * @return True if connection has been established, false if connection couldn't be established. **/ bool TCPLink::_connect(void) { if (isRunning()) { quit(); wait(); } start(HighPriority); return true; } bool TCPLink::_hardwareConnect() { Q_ASSERT(_socket == NULL); _socket = new QTcpSocket(); QSignalSpy errorSpy(_socket, static_cast(&QTcpSocket::error)); _socket->connectToHost(_config->address(), _config->port()); QObject::connect(_socket, &QTcpSocket::readyRead, this, &TCPLink::readBytes); QObject::connect(_socket,static_cast(&QTcpSocket::error), this, &TCPLink::_socketError); // Give the socket a second to connect to the other side otherwise error out if (!_socket->waitForConnected(1000)) { // 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) { emit communicationError(tr("Link Error"), QString("Error on link %1. Connection failed").arg(getName())); } delete _socket; _socket = NULL; return false; } _socketIsConnected = true; emit connected(); return true; } void TCPLink::_socketError(QAbstractSocket::SocketError socketError) { Q_UNUSED(socketError); emit communicationError(tr("Link Error"), QString("Error on link %1. Error on socket: %2.").arg(getName()).arg(_socket->errorString())); } /** * @brief Check if connection is active. * * @return True if link is connected, false otherwise. **/ bool TCPLink::isConnected() const { return _socketIsConnected; } QString TCPLink::getName() const { return _config->name(); } qint64 TCPLink::getConnectionSpeed() const { return 54000000; // 54 Mbit } qint64 TCPLink::getCurrentInDataRate() const { return 0; } qint64 TCPLink::getCurrentOutDataRate() const { return 0; } void TCPLink::waitForBytesWritten(int msecs) { Q_ASSERT(_socket); _socket->waitForBytesWritten(msecs); } void TCPLink::waitForReadyRead(int msecs) { Q_ASSERT(_socket); _socket->waitForReadyRead(msecs); } void TCPLink::_restartConnection() { if(this->isConnected()) { _disconnect(); _connect(); } } //-------------------------------------------------------------------------- //-- TCPConfiguration 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 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(""); } 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(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; } void TCPConfiguration::setHost(const QString host) { QString ipAdd = get_ip_address(host); if(ipAdd.isEmpty()) { qWarning() << "TCP:" << "Could not resolve host:" << host; } else { _address = ipAdd; } } 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); _port = (quint16)settings.value("port", QGC_TCP_PORT).toUInt(); QString address = settings.value("host", _address.toString()).toString(); _address = address; settings.endGroup(); } void TCPConfiguration::updateSettings() { if(_link) { TCPLink* ulink = dynamic_cast(_link); if(ulink) { ulink->_restartConnection(); } } }