Initial TCP support

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 \

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>
+//#include <netinet/in.h>
+
+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<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" << 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; i<s; i++)
+        //        {
+        //            unsigned int v=data[i];
+        //            fprintf(stderr,"%02x ", v);
+        //        }
+        //        std::cerr << std::endl;
+        
+    }
+}
+
+
+/**
+ * @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)
+	{
+        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;
+}

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>
+
+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

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<MAVLinkSimulationLink*>(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<TCPLink*>(link);
+    if (tcp != 0) {
+        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 +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:
 			{

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