Added ports for FlightGear in/out

qgroundcontrol.pro

@@ -236,6 +236,7 @@ HEADERS += src/MG.h \
     src/comm/ProtocolInterface.h \
     src/comm/MAVLinkProtocol.h \
     src/comm/AS4Protocol.h \
+    src/comm/QGCFlightGearLink.h \
     src/ui/CommConfigurationWindow.h \
     src/ui/SerialConfigurationWindow.h \
     src/ui/MainWindow.h \
@@ -364,6 +365,7 @@ SOURCES += src/main.cc \
     src/comm/SerialSimulationLink.cc \
     src/comm/MAVLinkProtocol.cc \
     src/comm/AS4Protocol.cc \
+    src/comm/QGCFlightGearLink.cc \
     src/ui/CommConfigurationWindow.cc \
     src/ui/SerialConfigurationWindow.cc \
     src/ui/MainWindow.cc \

src/comm/QGCFlightGearLink.cc

@@ -34,19 +34,20 @@ This file is part of the QGROUNDCONTROL project
 #include <QMutexLocker>
 #include <iostream>
 #include "QGCFlightGearLink.h"
-#include "LinkManager.h"
 #include "QGC.h"
 #include <QHostInfo>
-//#include <netinet/in.h>
 
 QGCFlightGearLink::QGCFlightGearLink(QHostAddress host, quint16 port)
 {
     this->host = host;
     this->port = port;
     this->connectState = false;
+
     // Set unique ID and add link to the list of links
-    this->name = tr("FlightGear Link (port:%1)").arg(5401);
-    LinkManager::instance()->add(this);
+    this->name = tr("FlightGear Link (port:%1)").arg(port);
+    setRemoteHost(QString("127.0.0.1:%1").arg(port));
+    connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
+    refreshTimer.start(20); // 50 Hz UAV -> Simulation update rate
 }
 
 QGCFlightGearLink::~QGCFlightGearLink()
@@ -70,8 +71,8 @@ void QGCFlightGearLink::run()
 void QGCFlightGearLink::setPort(int port)
 {
     this->port = port;
-    disconnect();
-    connect();
+    disconnectSimulation();
+    connectSimulation();
 }
 
 /**
@@ -100,7 +101,7 @@ void QGCFlightGearLink::setRemoteHost(const QString& host)
             currentHost = address;
             //qDebug() << "Address:" << address.toString();
             // Set port according to user input
-            currentPort = host.split(":");
+            currentPort = host.split(":").last().toInt();
         }
     }
     else
@@ -114,12 +115,19 @@ void QGCFlightGearLink::setRemoteHost(const QString& host)
     }
 }
 
+void QGCFlightGearLink::updateGlobalPosition(quint64 time, double lat, double lon, double alt)
+{
+
+}
+
+void QGCFlightGearLink::sendUAVUpdate()
+{
+    QString state("");
+    writeBytes(state.toAscii().constData(), state.length());
+}
+
 void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
 {
-    // Broadcast to all connected systems
-    for (int h = 0; h < hosts.size(); h++)
-    {
-        quint16 currentPort = ports.at(h);
 //#define QGCFlightGearLink_DEBUG
 #ifdef QGCFlightGearLink_DEBUG
         QString bytes;
@@ -142,7 +150,6 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
         qDebug() << "ASCII:" << ascii;
 #endif
         socket->writeDatagram(data, size, currentHost, currentPort);
-    }
 }
 
 /**
@@ -164,7 +171,10 @@ void QGCFlightGearLink::readBytes()
 
     // FIXME TODO Check if this method is better than retrieving the data by individual processes
     QByteArray b(data, s);
-    emit bytesReceived(this, b);
+    //emit bytesReceived(this, b);
+	
+	// Print string
+	qDebug() << "FG LINK GOT:" << QString(b);
 
 //    // Echo data for debugging purposes
 //    std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
@@ -175,21 +185,6 @@ void QGCFlightGearLink::readBytes()
 //        fprintf(stderr,"%02x ", v);
 //    }
 //    std::cerr << std::endl;
-
-
-    // Add host to broadcast list if not yet present
-    if (!hosts.contains(sender))
-    {
-        hosts.append(sender);
-        ports.append(senderPort);
-        //        ports->insert(sender, senderPort);
-    }
-    else
-    {
-        int index = hosts.indexOf(sender);
-        ports.replace(index, senderPort);
-    }
-
 }
 
 
@@ -208,15 +203,15 @@ qint64 QGCFlightGearLink::bytesAvailable()
  *
  * @return True if connection has been disconnected, false if connection couldn't be disconnected.
  **/
-bool QGCFlightGearLink::disconnect()
+bool QGCFlightGearLink::disconnectSimulation()
 {
     delete socket;
     socket = NULL;
 
     connectState = false;
 
-    emit disconnected();
-    emit connected(false);
+    emit flightGearDisconnected();
+    emit flightGearConnected(false);
     return !connectState;
 }
 
@@ -225,7 +220,7 @@ bool QGCFlightGearLink::disconnect()
  *
  * @return True if connection has been established, false if connection couldn't be established.
  **/
-bool QGCFlightGearLink::connect()
+bool QGCFlightGearLink::connectSimulation()
 {
     socket = new QUdpSocket(this);
 
@@ -269,9 +264,9 @@ bool QGCFlightGearLink::connect()
     //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
     QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
-    emit connected(connectState);
+    emit flightGearConnected(connectState);
     if (connectState) {
-        emit connected();
+        emit flightGearConnected();
         connectionStartTime = QGC::groundTimeUsecs()/1000;
     }
 
@@ -297,5 +292,5 @@ QString QGCFlightGearLink::getName()
 void QGCFlightGearLink::setName(QString name)
 {
     this->name = name;
-    emit nameChanged(this->name);
+//    emit nameChanged(this->name);
 }

src/comm/QGCFlightGearLink.h

@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QMap>
 #include <QMutex>
 #include <QUdpSocket>
+#include <QTimer>
 #include <LinkInterface.h>
 #include <configuration.h>
 
@@ -45,8 +46,7 @@ class QGCFlightGearLink : public QThread
     //Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface)
 
 public:
-    QGCFlightGearLink(QHostAddress host = QHostAddress::Any, quint16 port = 5401);
-    //QGCFlightGearLink(QHostAddress host = "239.255.76.67", quint16 port = 7667);
+    QGCFlightGearLink(QHostAddress host = QHostAddress::Any, quint16 port = 49005);
     ~QGCFlightGearLink();
 
     bool isConnected();
@@ -63,12 +63,14 @@ public:
     void run();
 
 public slots:
-    void setAddress(QString address);
+//    void setAddress(QString address);
     void setPort(int port);
     /** @brief Add a new host to broadcast messages to */
-    void addHost(const QString& host);
-    /** @brief Remove a host from broadcasting messages to */
-    void removeHost(const QString& host);
+    void setRemoteHost(const QString& host);
+    void updateGlobalPosition(quint64 time, double lat, double lon, double alt);
+    void sendUAVUpdate();
+//    /** @brief Remove a host from broadcasting messages to */
+//    void removeHost(const QString& host);
     //    void readPendingDatagrams();
 
     void readBytes();
@@ -79,13 +81,13 @@ public slots:
      * @param size The size of the bytes array
      **/
     void writeBytes(const char* data, qint64 length);
-    bool connect();
-    bool disconnect();
+    bool connectSimulation();
+    bool disconnectSimulation();
 
 protected:
     QString name;
     QHostAddress host;
-	QHostAddress currentHost
+    QHostAddress currentHost;
     quint16 currentPort;
     quint16 port;
     int id;
@@ -101,11 +103,26 @@ protected:
     quint64 connectionStartTime;
     QMutex statisticsMutex;
     QMutex dataMutex;
+    QTimer refreshTimer;
 
     void setName(QString name);
 
 signals:
-    // Signals are defined by LinkInterface
+    /**
+     * @brief This signal is emitted instantly when the link is connected
+     **/
+    void flightGearConnected();
+
+    /**
+     * @brief This signal is emitted instantly when the link is disconnected
+     **/
+    void flightGearDisconnected();
+
+    /**
+     * @brief This signal is emitted instantly when the link status changes
+     **/
+    void flightGearConnected(bool connected);
+
 
 };
 

src/ui/MainWindow.cc

@@ -157,6 +157,11 @@ MainWindow::MainWindow(QWidget *parent):
     joystickWidget = 0;
     joystick = new JoystickInput();
 
+    // Connect flighgear test link
+    // FIXME MOVE INTO UAV OBJECT
+    fgLink = new QGCFlightGearLink();
+    fgLink->connectSimulation();
+
     // Load Toolbar
     toolBar = new QGCToolBar(this);
     this->addToolBar(toolBar);

src/ui/MainWindow.h

@@ -75,6 +75,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include "SlugsPadCameraControl.h"
 #include "UASControlParameters.h"
+#include "QGCFlightGearLink.h"
 
 class QGCMapTool;
 
@@ -445,6 +446,7 @@ protected:
     QGC_MAINWINDOW_STYLE currentStyle;
     Qt::WindowStates windowStateVal;
     bool lowPowerMode; ///< If enabled, QGC reduces the update rates of all widgets
+    QGCFlightGearLink* fgLink;
 
 private:
     Ui::MainWindow ui;