From 2dfa002e54ce9f6c9042cc26b39e76d9f495d1fb Mon Sep 17 00:00:00 2001 From: lm Date: Thu, 4 Aug 2011 22:21:54 +0200 Subject: [PATCH] Added ports for FlightGear in/out --- qgroundcontrol.pro | 2 ++ src/comm/QGCFlightGearLink.cc | 65 ++++++++++++++++------------------- src/comm/QGCFlightGearLink.h | 39 +++++++++++++++------ src/ui/MainWindow.cc | 5 +++ src/ui/MainWindow.h | 2 ++ 5 files changed, 67 insertions(+), 46 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 7b645d1b7..637c5f07c 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 3a4817935..12f8a422e 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -34,19 +34,20 @@ This file is part of the QGROUNDCONTROL project #include #include #include "QGCFlightGearLink.h" -#include "LinkManager.h" #include "QGC.h" #include -//#include 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); } diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h index 2e2bdb948..49aaa0268 100644 --- a/src/comm/QGCFlightGearLink.h +++ b/src/comm/QGCFlightGearLink.h @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include #include @@ -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,14 +81,14 @@ 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 - quint16 currentPort; + QHostAddress currentHost; + quint16 currentPort; quint16 port; int id; QUdpSocket* socket; @@ -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); + }; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 1f39e4a27..6f9f0f0fa 100644 --- a/src/ui/MainWindow.cc +++ b/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); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 6b21e2f43..cd74ae1c1 100644 --- a/src/ui/MainWindow.h +++ b/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; -- 2.22.0