Merge pull request #420 from DonLakeFlyer/TCP_Support

Tcp support

qgcunittest.pro

@@ -200,6 +200,7 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/QGCMAVLinkLogPlayer.ui \
     src/ui/QGCWaypointListMulti.ui \
     src/ui/QGCUDPLinkConfiguration.ui \
+    src/ui/QGCTCPLinkConfiguration.ui \
     src/ui/QGCSettingsWidget.ui \
     src/ui/UASControlParameters.ui \
     src/ui/map/QGCMapTool.ui \
@@ -323,6 +324,7 @@ HEADERS += src/MG.h \
     src/uas/QGCMAVLinkUASFactory.h \
     src/ui/QGCWaypointListMulti.h \
     src/ui/QGCUDPLinkConfiguration.h \
+    src/ui/QGCTCPLinkConfiguration.h \
     src/ui/QGCSettingsWidget.h \
     src/ui/uas/UASControlParameters.h \
     src/uas/QGCUASParamManager.h \
@@ -490,6 +492,7 @@ SOURCES += src/QGCCore.cc \
     src/uas/QGCMAVLinkUASFactory.cc \
     src/ui/QGCWaypointListMulti.cc \
     src/ui/QGCUDPLinkConfiguration.cc \
+    src/ui/QGCTCPLinkConfiguration.cc \
     src/ui/QGCSettingsWidget.cc \
     src/ui/uas/UASControlParameters.cpp \
     src/uas/QGCUASParamManager.cc \

qgroundcontrol.pro

@@ -227,6 +227,7 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/QGCMAVLinkLogPlayer.ui \
     src/ui/QGCWaypointListMulti.ui \
     src/ui/QGCUDPLinkConfiguration.ui \
+    src/ui/QGCTCPLinkConfiguration.ui \
     src/ui/QGCSettingsWidget.ui \
     src/ui/UASControlParameters.ui \
     src/ui/map/QGCMapTool.ui \
@@ -352,6 +353,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 \
@@ -407,6 +409,7 @@ HEADERS += src/MG.h \
     src/uas/QGCMAVLinkUASFactory.h \
     src/ui/QGCWaypointListMulti.h \
     src/ui/QGCUDPLinkConfiguration.h \
+    src/ui/QGCTCPLinkConfiguration.h \
     src/ui/QGCSettingsWidget.h \
     src/ui/uas/UASControlParameters.h \
     src/uas/QGCUASParamManager.h \
@@ -578,6 +581,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 \
@@ -632,6 +636,7 @@ SOURCES += src/main.cc \
     src/uas/QGCMAVLinkUASFactory.cc \
     src/ui/QGCWaypointListMulti.cc \
     src/ui/QGCUDPLinkConfiguration.cc \
+    src/ui/QGCTCPLinkConfiguration.cc \
     src/ui/QGCSettingsWidget.cc \
     src/ui/uas/UASControlParameters.cpp \
     src/uas/QGCUASParamManager.cc \

src/comm/TCPLink.cc

+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ 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 <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of TCP connection (server) for unmanned vehicles
+ *   @author Lorenz Meier <mavteam@student.ethz.ch>
+ *
+ */
+
+#include <QTimer>
+#include <QList>
+#include <QDebug>
+#include <QMutexLocker>
+#include <iostream>
+#include "TCPLink.h"
+#include "LinkManager.h"
+#include "QGC.h"
+#include <QHostInfo>
+
+TCPLink::TCPLink(QHostAddress hostAddress, quint16 socketPort) :
+    host(hostAddress),
+    port(socketPort),
+    socket(NULL),
+    socketIsConnected(false)
+
+{
+    // Set unique ID and add link to the list of links
+    this->id = getNextLinkId();
+	this->name = tr("TCP Link (port:%1)").arg(this->port);
+	emit nameChanged(this->name);
+    
+    qDebug() << "TCP Created " << this->name;
+}
+
+TCPLink::~TCPLink()
+{
+    disconnect();
+	this->deleteLater();
+}
+
+void TCPLink::run()
+{
+	exec();
+}
+
+void TCPLink::setAddress(const QString &text)
+{
+    setAddress(QHostAddress(text));
+}
+
+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();
+	}
+}
+
+#ifdef TCPLINK_READWRITE_DEBUG
+void TCPLink::writeDebugBytes(const char *data, qint16 size)
+{
+    QString bytes;
+    QString ascii;
+    for (int i=0; i<size; i++)
+    {
+        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);
+        }
+    }
+    qDebug() << "Sent" << size << "bytes to" << host.toString() << ":" << port << "data:";
+    qDebug() << bytes;
+    qDebug() << "ASCII:" << ascii;
+}
+#endif
+
+void TCPLink::writeBytes(const char* data, qint64 size)
+{
+#ifdef TCPLINK_READWRITE_DEBUG
+    writeDebugBytes(data, size);
+#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);
+
+#ifdef TCPLINK_READWRITE_DEBUG
+        writeDebugBytes(buffer.data(), buffer.size());
+#endif
+    }
+}
+
+
+/**
+ * @brief Get the number of bytes to read.
+ *
+ * @return The number of bytes to read
+ **/
+qint64 TCPLink::bytesAvailable()
+{
+    return socket->bytesAvailable();
+}
+
+/**
+ * @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)
+	{
+        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(error(QAbstractSocket::SocketError)), this, SLOT(socketError(QAbstractSocket::SocketError)));
+    
+    // Give the socket a second to connect to the other side otherwise error out
+    if (!socket->waitForConnected(1000))
+    {
+        emit communicationError(getName(), "connection failed");
+        return false;
+    }
+    
+    socketIsConnected = true;
+    connectionStartTime = QGC::groundTimeUsecs()/1000;
+    emit connected(true);
+
+    return true;
+}
+
+void TCPLink::socketError(QAbstractSocket::SocketError socketError)
+{
+    emit communicationError(getName(), "Error on socket: " + socket->errorString());
+}
+
+/**
+ * @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;
+}

src/comm/TCPLink.h

+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ 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 <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
+/**
+ * @file
+ *   @brief TCP connection (server) for unmanned vehicles
+ *   @author Lorenz Meier <mavteam@student.ethz.ch>
+ *
+ */
+
+#ifndef TCPLINK_H
+#define TCPLINK_H
+
+#include <QString>
+#include <QList>
+#include <QMap>
+#include <QMutex>
+#include <QHostAddress>
+#include <QTcpSocket>
+#include <LinkInterface.h>
+#include <configuration.h>
+
+//#define TCPLINK_READWRITE_DEBUG   // Use to debug data reads/writes
+
+class TCPLink : public LinkInterface
+{
+    Q_OBJECT
+    
+public:
+    TCPLink(QHostAddress hostAddress = QHostAddress::LocalHost, quint16 socketPort = 5760);
+    ~TCPLink();
+    
+    void requestReset() { }
+    
+    bool isConnected() const;
+    qint64 bytesAvailable();
+    int getPort() const {
+        return port;
+    }
+    QHostAddress getHostAddress() const {
+        return host;
+    }
+    
+    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 socketError(QAbstractSocket::SocketError socketError);
+    void setAddress(const QString &text);
+
+    
+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);
+#ifdef TCPLINK_READWRITE_DEBUG
+    void writeDebugBytes(const char *data, qint16 size);
+#endif
+    
+signals:
+    //Signals are defined by LinkInterface
+    
+};
+
+#endif // TCPLINK_H

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"
@@ -52,6 +53,7 @@ This file is part of the QGROUNDCONTROL project
 #include "MAVLinkProtocol.h"
 #include "MAVLinkSettingsWidget.h"
 #include "QGCUDPLinkConfiguration.h"
+#include "QGCTCPLinkConfiguration.h"
 #include "LinkManager.h"
 #include "MainWindow.h"
 
@@ -82,6 +84,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<MAVLinkSimulationLink*>(link)) {
         //Only show simulation option if already setup elsewhere as a simulation
         ui.linkType->addItem(tr("Simulation"), QGC_LINK_SIMULATION);
@@ -148,6 +151,13 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
         ui.linkGroupBox->setTitle(tr("UDP Link"));
         ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_UDP));
     }
+    TCPLink* tcp = dynamic_cast<TCPLink*>(link);
+    if (tcp != 0) {
+        QWidget* conf = new QGCTCPLinkConfiguration(tcp, this);
+        ui.linkScrollArea->setWidget(conf);
+        ui.linkGroupBox->setTitle(tr("TCP Link"));
+        ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_TCP));
+    }
     MAVLinkSimulationLink* sim = dynamic_cast<MAVLinkSimulationLink*>(link);
     if (sim != 0) {
         ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_SIMULATION));
@@ -177,7 +187,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 +266,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:
 			{

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

src/ui/QGCTCPLinkConfiguration.cc

+#include <QInputDialog>
+
+#include "QGCTCPLinkConfiguration.h"
+#include "ui_QGCTCPLinkConfiguration.h"
+
+QGCTCPLinkConfiguration::QGCTCPLinkConfiguration(TCPLink* link, QWidget *parent) :
+    QWidget(parent),
+    link(link),
+    ui(new Ui::QGCTCPLinkConfiguration)
+{
+    ui->setupUi(this);
+    uint16_t port = link->getPort();
+    ui->portSpinBox->setValue(port);
+    QString addr = link->getHostAddress().toString();
+    ui->hostAddressLineEdit->setText(addr);
+    connect(ui->portSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setPort(int)));
+    connect(ui->hostAddressLineEdit, SIGNAL(textChanged (const QString &)), link, SLOT(setAddress(const QString &)));
+}
+
+QGCTCPLinkConfiguration::~QGCTCPLinkConfiguration()
+{
+    delete ui;
+}
+
+void QGCTCPLinkConfiguration::changeEvent(QEvent *e)
+{
+    QWidget::changeEvent(e);
+    switch (e->type()) {
+    case QEvent::LanguageChange:
+        ui->retranslateUi(this);
+        break;
+    default:
+        break;
+    }
+}
\ No newline at end of file

src/ui/QGCTCPLinkConfiguration.h

+#ifndef QGCTCPLINKCONFIGURATION_H
+#define QGCTCPLINKCONFIGURATION_H
+
+#include <QWidget>
+
+#include "TCPLink.h"
+
+namespace Ui
+{
+class QGCTCPLinkConfiguration;
+}
+
+class QGCTCPLinkConfiguration : public QWidget
+{
+    Q_OBJECT
+
+public:
+    explicit QGCTCPLinkConfiguration(TCPLink* link, QWidget *parent = 0);
+    ~QGCTCPLinkConfiguration();
+
+public slots:
+
+protected:
+    void changeEvent(QEvent *e);
+
+    TCPLink* link;    ///< TCP link instance this widget configures
+
+private:
+    Ui::QGCTCPLinkConfiguration *ui;
+};
+
+#endif // QGCTCPLINKCONFIGURATION_H

src/ui/QGCTCPLinkConfiguration.ui

+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>QGCTCPLinkConfiguration</class>
+ <widget class="QWidget" name="QGCTCPLinkConfiguration">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QFormLayout" name="formLayout">
+   <property name="fieldGrowthPolicy">
+    <enum>QFormLayout::FieldsStayAtSizeHint</enum>
+   </property>
+   <item row="0" column="0">
+    <widget class="QLabel" name="portLabel">
+     <property name="text">
+      <string>TCP Port</string>
+     </property>
+    </widget>
+   </item>
+   <item row="0" column="1">
+    <widget class="QSpinBox" name="portSpinBox">
+     <property name="minimum">
+      <number>3000</number>
+     </property>
+     <property name="maximum">
+      <number>100000</number>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="0">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>Host Address</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="1">
+    <widget class="QLineEdit" name="hostAddressLineEdit"/>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>