From d9fe006905bda92591b0ab5ad540fb81ef9ff23e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 28 Nov 2013 14:43:07 -0800 Subject: [PATCH] Initial TCP support --- qgroundcontrol.pro | 2 + src/comm/TCPLink.cc | 323 ++++++++++++++++++++++++++++++ src/comm/TCPLink.h | 121 +++++++++++ src/ui/CommConfigurationWindow.cc | 17 +- src/ui/CommConfigurationWindow.h | 1 + 5 files changed, 463 insertions(+), 1 deletion(-) create mode 100644 src/comm/TCPLink.cc create mode 100644 src/comm/TCPLink.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index e8940419a..b34aefbb2 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -352,6 +352,7 @@ HEADERS += src/MG.h \ src/ui/CameraView.h \ src/comm/MAVLinkSimulationLink.h \ src/comm/UDPLink.h \ + src/comm/TCPLink.h \ src/ui/ParameterInterface.h \ src/ui/WaypointList.h \ src/Waypoint.h \ @@ -578,6 +579,7 @@ SOURCES += src/main.cc \ src/ui/CameraView.cc \ src/comm/MAVLinkSimulationLink.cc \ src/comm/UDPLink.cc \ + src/comm/TCPLink.cc \ src/ui/ParameterInterface.cc \ src/ui/WaypointList.cc \ src/Waypoint.cc \ diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc new file mode 100644 index 000000000..c1c2d01ad --- /dev/null +++ b/src/comm/TCPLink.cc @@ -0,0 +1,323 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2011 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 . + + ======================================================================*/ + +/** + * @file + * @brief Definition of TCP connection (server) for unmanned vehicles + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include "TCPLink.h" +#include "LinkManager.h" +#include "QGC.h" +#include +//#include + +TCPLink::TCPLink(QHostAddress host, quint16 port) +: socket(NULL) +{ + // FIXFIX: host and port and hard-wired + this->host = "127.0.0.1"; + this->port = 5760; + + this->socketIsConnected = false; + + // Set unique ID and add link to the list of links + this->id = getNextLinkId(); + // FIXFIX: What about host name? + this->name = tr("TCP Link (port:%1)").arg(this->port); + emit nameChanged(this->name); + + qDebug() << "TCP Created " << name; +} + +TCPLink::~TCPLink() +{ + disconnect(); + this->deleteLater(); +} + +/** + * @brief Runs the thread + * + **/ +void TCPLink::run() +{ + exec(); +} + +void TCPLink::setAddress(QHostAddress host) +{ + bool reconnect(false); + if(this->isConnected()) + { + disconnect(); + reconnect = true; + } + this->host = host; + if(reconnect) + { + connect(); + } +} + +void TCPLink::setPort(int port) +{ + bool reconnect(false); + if(this->isConnected()) + { + disconnect(); + reconnect = true; + } + this->port = port; + this->name = tr("TCP Link (port:%1)").arg(this->port); + emit nameChanged(this->name); + if(reconnect) + { + connect(); + } +} + +void TCPLink::writeBytes(const char* data, qint64 size) +{ + //#define TCPLINK_DEBUG +#ifdef TCPLINK_DEBUG + QString bytes; + QString ascii; + for (int i=0; i 31 && data[i] < 127) + { + ascii.append(data[i]); + } + else + { + ascii.append(219); + } + } + qDebug() << "Sent" << size << "bytes to" << currentHost.toString() << ":" << currentPort << "data:"; + qDebug() << bytes; + qDebug() << "ASCII:" << ascii; +#endif + socket->write(data, size); +} + +/** + * @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); + + // // Echo data for debugging purposes + // std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; + // int i; + // for (i=0; ibytesAvailable(); +} + +/** + * @brief Disconnect the connection. + * + * @return True if connection has been disconnected, false if connection couldn't be disconnected. + **/ +bool TCPLink::disconnect() +{ + this->quit(); + this->wait(); + + if (socket) + { + if (socketIsConnected) { + socket->disconnect(); + socketIsConnected = false; + } + delete socket; + socket = NULL; + } + + return true; +} + +/** + * @brief Connect the connection. + * + * @return True if connection has been established, false if connection couldn't be established. + **/ +bool TCPLink::connect() +{ + if(this->isRunning()) + { + this->quit(); + this->wait(); + } + bool connected = this->hardwareConnect(); + start(HighPriority); + return connected; +} + +bool TCPLink::hardwareConnect(void) +{ + socket = new QTcpSocket(); + + socket->connectToHost(host, port); + + QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); + //QObject::connect(socket, SIGNAL(connected()), this, SLOT(socketConnected())); + socketIsConnected = true; + + connectionStartTime = QGC::groundTimeUsecs()/1000; + + return true; +} + +void TCPLink::socketConnected() +{ + socketIsConnected = true; +} + +/** + * @brief Check if connection is active. + * + * @return True if link is connected, false otherwise. + **/ +bool TCPLink::isConnected() const +{ + return socketIsConnected; +} + +int TCPLink::getId() const +{ + return id; +} + +QString TCPLink::getName() const +{ + return name; +} + +void TCPLink::setName(QString name) +{ + this->name = name; + emit nameChanged(this->name); +} + + +qint64 TCPLink::getNominalDataRate() const +{ + return 54000000; // 54 Mbit +} + +qint64 TCPLink::getTotalUpstream() +{ + statisticsMutex.lock(); + qint64 totalUpstream = bitsSentTotal / ((QGC::groundTimeUsecs()/1000 - connectionStartTime) / 1000); + statisticsMutex.unlock(); + return totalUpstream; +} + +qint64 TCPLink::getCurrentUpstream() +{ + return 0; // TODO +} + +qint64 TCPLink::getMaxUpstream() +{ + return 0; // TODO +} + +qint64 TCPLink::getBitsSent() const +{ + return bitsSentTotal; +} + +qint64 TCPLink::getBitsReceived() const +{ + return bitsReceivedTotal; +} + +qint64 TCPLink::getTotalDownstream() +{ + statisticsMutex.lock(); + qint64 totalDownstream = bitsReceivedTotal / ((QGC::groundTimeUsecs()/1000 - connectionStartTime) / 1000); + statisticsMutex.unlock(); + return totalDownstream; +} + +qint64 TCPLink::getCurrentDownstream() +{ + return 0; // TODO +} + +qint64 TCPLink::getMaxDownstream() +{ + return 0; // TODO +} + +bool TCPLink::isFullDuplex() const +{ + return true; +} + +int TCPLink::getLinkQuality() const +{ + /* This feature is not supported with this interface */ + return -1; +} diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h new file mode 100644 index 000000000..c2d639fb9 --- /dev/null +++ b/src/comm/TCPLink.h @@ -0,0 +1,121 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2011 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 . + + ======================================================================*/ + +/** + * @file + * @brief TCP connection (server) for unmanned vehicles + * @author Lorenz Meier + * + */ + +#ifndef TCPLINK_H +#define TCPLINK_H + +#include +#include +#include +#include +#include +#include +#include +#include + +class TCPLink : public LinkInterface +{ + Q_OBJECT + +public: + TCPLink(QHostAddress host = QHostAddress::Any, quint16 port = 14550); + ~TCPLink(); + + void requestReset() { } + + bool isConnected() const; + qint64 bytesAvailable(); + int getPort() const { + return port; + } + + QString getName() const; + int getBaudRate() const; + int getBaudRateType() const; + int getFlowType() const; + int getParityType() const; + int getDataBitsType() const; + int getStopBitsType() const; + + /* Extensive statistics for scientific purposes */ + qint64 getNominalDataRate() const; + qint64 getTotalUpstream(); + qint64 getCurrentUpstream(); + qint64 getMaxUpstream(); + qint64 getTotalDownstream(); + qint64 getCurrentDownstream(); + qint64 getMaxDownstream(); + qint64 getBitsSent() const; + qint64 getBitsReceived() const; + + void run(); + + int getLinkQuality() const; + bool isFullDuplex() const; + int getId() const; + +public slots: + void setAddress(QHostAddress host); + void setPort(int port); + void readBytes(); + void writeBytes(const char* data, qint64 length); + bool connect(); + bool disconnect(); + void socketConnected(); + +protected: + QString name; + QHostAddress host; + quint16 port; + int id; + QTcpSocket* socket; + bool socketIsConnected; + + quint64 bitsSentTotal; + quint64 bitsSentCurrent; + quint64 bitsSentMax; + quint64 bitsReceivedTotal; + quint64 bitsReceivedCurrent; + quint64 bitsReceivedMax; + quint64 connectionStartTime; + QMutex statisticsMutex; + QMutex dataMutex; + + void setName(QString name); + +private: + bool hardwareConnect(void); + +signals: + //Signals are defined by LinkInterface + +}; + +#endif // TCPLINK_H diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 462f1c42c..96894ed1d 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -40,6 +40,7 @@ This file is part of the QGROUNDCONTROL project #include "SerialConfigurationWindow.h" #include "SerialLink.h" #include "UDPLink.h" +#include "TCPLink.h" #include "MAVLinkSimulationLink.h" #ifdef XBEELINK #include "XbeeLink.h" @@ -82,6 +83,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn // add link types ui.linkType->addItem(tr("Serial"), QGC_LINK_SERIAL); ui.linkType->addItem(tr("UDP"), QGC_LINK_UDP); + ui.linkType->addItem(tr("TCP"), QGC_LINK_TCP); if(dynamic_cast(link)) { //Only show simulation option if already setup elsewhere as a simulation ui.linkType->addItem(tr("Simulation"), QGC_LINK_SIMULATION); @@ -148,6 +150,11 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn ui.linkGroupBox->setTitle(tr("UDP Link")); ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_UDP)); } + TCPLink* tcp = dynamic_cast(link); + if (tcp != 0) { + ui.linkGroupBox->setTitle(tr("TCP Link")); + ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_TCP)); + } MAVLinkSimulationLink* sim = dynamic_cast(link); if (sim != 0) { ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_SIMULATION)); @@ -177,7 +184,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn connect(xbee,SIGNAL(tryConnectEnd(bool)),ui.actionConnect,SLOT(setEnabled(bool))); } #endif // XBEELINK - if (serial == 0 && udp == 0 && sim == 0 + if (serial == 0 && udp == 0 && sim == 0 && tcp == 0 #ifdef OPAL_RT && opal == 0 #endif @@ -256,6 +263,14 @@ void CommConfigurationWindow::setLinkType(qgc_link_t linktype) break; } + case QGC_LINK_TCP: + { + TCPLink *tcp = new TCPLink(); + tmpLink = tcp; + MainWindow::instance()->addLink(tmpLink); + break; + } + #ifdef OPAL_RT case QGC_LINK_OPAL: { diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index 47e2b4c8f..406331604 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -42,6 +42,7 @@ This file is part of the QGROUNDCONTROL project enum qgc_link_t { QGC_LINK_SERIAL, QGC_LINK_UDP, + QGC_LINK_TCP, QGC_LINK_SIMULATION, QGC_LINK_FORWARDING, #ifdef XBEELINK -- 2.22.0