From def596dd7624fb4861edd4bb861aa21feb5a72ae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Mar 2013 22:52:39 +0100 Subject: [PATCH 1/4] fix hardcoded start of fgfs and terrasync, use system environment variables for linux distro independence --- src/comm/QGCFlightGearLink.cc | 80 +++++++++++++++++++++++-------------------- 1 file changed, 43 insertions(+), 37 deletions(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index f245d34..a493c26 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -373,23 +373,23 @@ bool QGCFlightGearLink::connectSimulation() #ifdef Q_OS_MACX processFgfs = "/Applications/FlightGear.app/Contents/Resources/fgfs"; processTerraSync = "/Applications/FlightGear.app/Contents/Resources/terrasync"; - fgRoot = "/Applications/FlightGear.app/Contents/Resources/data"; + //fgRoot = "/Applications/FlightGear.app/Contents/Resources/data"; //fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery"; - fgScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery-TerraSync"; + terraSyncScenery = "/Applications/FlightGear.app/Contents/Resources/data/Scenery-TerraSync"; // /Applications/FlightGear.app/Contents/Resources/data/Scenery: #endif #ifdef Q_OS_WIN32 processFgfs = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs"; - fgRoot = "C:\\Program Files (x86)\\FlightGear\\data"; - fgScenery = "C:\\Program Files (x86)\\FlightGear\\data\\Scenery-Terrasync"; + //fgRoot = "C:\\Program Files (x86)\\FlightGear\\data"; + terraSyncScenery = "C:\\Program Files (x86)\\FlightGear\\data\\Scenery-Terrasync"; #endif #ifdef Q_OS_LINUX - processFgfs = "/usr/games/fgfs"; - fgRoot = "/usr/share/games/flightgear"; - fgScenery = "/usr/share/games/flightgear/Scenery/"; - processTerraSync = "/usr/bin/nice"; //according to http://wiki.flightgear.org/TerraSync, run with lower priority + processFgfs = "fgfs"; + //fgRoot = "/usr/share/games/flightgear"; + //fgScenery = "/usr/share/games/flightgear/Scenery/"; + processTerraSync = "nice"; //according to http://wiki.flightgear.org/TerraSync, run with lower priority terraSyncScenery = QDir::homePath() + "/.terrasync/Scenery"; //according to http://wiki.flightgear.org/TerraSync a separate directory is used #endif @@ -397,33 +397,33 @@ bool QGCFlightGearLink::connectSimulation() // Sanity checks bool sane = true; - QFileInfo executable(processFgfs); - if (!executable.isExecutable()) - { - MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear was not found at %1").arg(processFgfs)); - sane = false; - } - - QFileInfo root(fgRoot); - if (!root.isDir()) - { - MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear data directory was not found at %1").arg(fgRoot)); - sane = false; - } - - QFileInfo scenery(fgScenery); - if (!scenery.isDir()) - { - MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear scenery directory was not found at %1").arg(fgScenery)); - sane = false; - } - - QFileInfo terraSyncExecutableInfo(processTerraSync); - if (!terraSyncExecutableInfo.isExecutable()) - { - MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("TerraSync was not found at %1").arg(processTerraSync)); - sane = false; - } +// QFileInfo executable(processFgfs); +// if (!executable.isExecutable()) +// { +// MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear was not found at %1").arg(processFgfs)); +// sane = false; +// } + +// QFileInfo root(fgRoot); +// if (!root.isDir()) +// { +// MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear data directory was not found at %1").arg(fgRoot)); +// sane = false; +// } + +// QFileInfo scenery(fgScenery); +// if (!scenery.isDir()) +// { +// MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("FlightGear scenery directory was not found at %1").arg(fgScenery)); +// sane = false; +// } + +// QFileInfo terraSyncExecutableInfo(processTerraSync); +// if (!terraSyncExecutableInfo.isExecutable()) +// { +// MainWindow::instance()->showCriticalMessage(tr("FlightGear Failed to Start"), tr("TerraSync was not found at %1").arg(processTerraSync)); +// sane = false; +// } if (!sane) return false; @@ -433,7 +433,7 @@ bool QGCFlightGearLink::connectSimulation() /*Prepare FlightGear Arguments */ //flightGearArguments << QString("--fg-root=%1").arg(fgRoot); - flightGearArguments << QString("--fg-scenery=%1:%2").arg(fgScenery).arg(terraSyncScenery); //according to http://wiki.flightgear.org/TerraSync a separate directory is used + flightGearArguments << QString("--fg-scenery=%1:%2").arg(terraSyncScenery); //according to http://wiki.flightgear.org/TerraSync a separate directory is used flightGearArguments << QString("--fg-aircraft=%1").arg(fgAircraft); if (mav->getSystemType() == MAV_TYPE_QUADROTOR) { @@ -498,7 +498,7 @@ bool QGCFlightGearLink::connectSimulation() /*Prepare TerraSync Arguments */ QStringList terraSyncArguments; #ifdef Q_OS_LINUX - terraSyncArguments << "/usr/games/terrasync"; + terraSyncArguments << "terrasync"; #endif terraSyncArguments << "-p"; terraSyncArguments << "5505"; @@ -506,6 +506,12 @@ bool QGCFlightGearLink::connectSimulation() terraSyncArguments << "-d"; terraSyncArguments << terraSyncScenery; //according to http://wiki.flightgear.org/TerraSync a separate directory is used +#ifdef Q_OS_LINUX + /* Setting environment */ + QProcessEnvironment env = QProcessEnvironment::systemEnvironment(); + process->setProcessEnvironment(env); + terraSync->setProcessEnvironment(env); +#endif // connect (terraSync, SIGNAL(readyReadStandardOutput()), this, SLOT(printTerraSyncOutput())); // connect (terraSync, SIGNAL(readyReadStandardError()), this, SLOT(printTerraSyncError())); terraSync->start(processTerraSync, terraSyncArguments); From d49f98410dd2ef91ae8ec4297db1c02c8c694611 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Mar 2013 12:44:28 +0100 Subject: [PATCH 2/4] First stab at JSBSim interface, compiling, operational, but needs the correct startup folders and arguments --- qgroundcontrol.pro | 5 + src/comm/QGCJSBSimLink.cc | 422 ++++++++++++++++++++++++++++++++++++ src/comm/QGCJSBSimLink.h | 153 +++++++++++++ src/uas/UAS.cc | 27 +++ src/uas/UAS.h | 2 + src/ui/QGCHilConfiguration.cc | 16 +- src/ui/QGCHilConfiguration.ui | 7 +- src/ui/QGCHilJSBSimConfiguration.cc | 49 +++++ src/ui/QGCHilJSBSimConfiguration.h | 33 +++ src/ui/QGCHilJSBSimConfiguration.ui | 99 +++++++++ 10 files changed, 811 insertions(+), 2 deletions(-) create mode 100644 src/comm/QGCJSBSimLink.cc create mode 100644 src/comm/QGCJSBSimLink.h create mode 100644 src/ui/QGCHilJSBSimConfiguration.cc create mode 100644 src/ui/QGCHilJSBSimConfiguration.h create mode 100644 src/ui/QGCHilJSBSimConfiguration.ui diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 07e62fb..2721a93 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -226,6 +226,7 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCVehicleConfig.ui \ src/ui/QGCHilConfiguration.ui \ src/ui/QGCHilFlightGearConfiguration.ui \ + src/ui/QGCHilJSBSimConfiguration.ui \ src/ui/QGCHilXPlaneConfiguration.ui INCLUDEPATH += src \ src/ui \ @@ -255,6 +256,7 @@ HEADERS += src/MG.h \ src/comm/ProtocolInterface.h \ src/comm/MAVLinkProtocol.h \ src/comm/QGCFlightGearLink.h \ + src/comm/QGCJSBSimLink.h \ src/comm/QGCXPlaneLink.h \ src/ui/CommConfigurationWindow.h \ src/ui/SerialConfigurationWindow.h \ @@ -366,6 +368,7 @@ HEADERS += src/MG.h \ src/comm/QGCHilLink.h \ src/ui/QGCHilConfiguration.h \ src/ui/QGCHilFlightGearConfiguration.h \ + src/ui/QGCHilJSBSimConfiguration.h \ src/ui/QGCHilXPlaneConfiguration.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler @@ -420,6 +423,7 @@ SOURCES += src/main.cc \ src/comm/SerialLink.cc \ src/comm/MAVLinkProtocol.cc \ src/comm/QGCFlightGearLink.cc \ + src/comm/QGCJSBSimLink.cc \ src/comm/QGCXPlaneLink.cc \ src/ui/CommConfigurationWindow.cc \ src/ui/SerialConfigurationWindow.cc \ @@ -525,6 +529,7 @@ SOURCES += src/main.cc \ src/ui/QGCVehicleConfig.cc \ src/ui/QGCHilConfiguration.cc \ src/ui/QGCHilFlightGearConfiguration.cc \ + src/ui/QGCHilJSBSimConfiguration.cc \ src/ui/QGCHilXPlaneConfiguration.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc new file mode 100644 index 0000000..a1d9b60 --- /dev/null +++ b/src/comm/QGCJSBSimLink.cc @@ -0,0 +1,422 @@ +/*===================================================================== + +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 UDP connection (server) for unmanned vehicles + * @see Flightgear Manual http://mapserver.flightgear.org/getstart.pdf + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include "QGCJSBSimLink.h" +#include "QGC.h" +#include +#include "MainWindow.h" + +QGCJSBSimLink::QGCJSBSimLink(UASInterface* mav, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) : + socket(NULL), + process(NULL), + startupArguments(startupArguments) +{ + this->host = host; + this->port = port+mav->getUASID(); + this->connectState = false; + this->currentPort = 49000+mav->getUASID(); + this->mav = mav; + this->name = tr("JSBSim Link (port:%1)").arg(port); + setRemoteHost(remoteHost); +} + +QGCJSBSimLink::~QGCJSBSimLink() +{ //do not disconnect unless it is connected. + //disconnectSimulation will delete the memory that was allocated for proces, terraSync and socket + if(connectState){ + disconnectSimulation(); + } +} + +/** + * @brief Runs the thread + * + **/ +void QGCJSBSimLink::run() +{ + exec(); +} + +void QGCJSBSimLink::setPort(int port) +{ + this->port = port; + disconnectSimulation(); + connectSimulation(); +} + +void QGCJSBSimLink::processError(QProcess::ProcessError err) +{ + switch(err) + { + case QProcess::FailedToStart: + MainWindow::instance()->showCriticalMessage(tr("JSBSim Failed to Start"), tr("Please check if the path and command is correct")); + break; + case QProcess::Crashed: + MainWindow::instance()->showCriticalMessage(tr("JSBSim Crashed"), tr("This is a FlightGear-related problem. Please upgrade FlightGear")); + break; + case QProcess::Timedout: + MainWindow::instance()->showCriticalMessage(tr("JSBSim Start Timed Out"), tr("Please check if the path and command is correct")); + break; + case QProcess::WriteError: + MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with JSBSim"), tr("Please check if the path and command is correct")); + break; + case QProcess::ReadError: + MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with JSBSim"), tr("Please check if the path and command is correct")); + break; + case QProcess::UnknownError: + default: + MainWindow::instance()->showCriticalMessage(tr("JSBSim Error"), tr("Please check if the path and command is correct.")); + break; + } +} + +/** + * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 + */ +void QGCJSBSimLink::setRemoteHost(const QString& host) +{ + if (host.contains(":")) + { + //qDebug() << "HOST: " << host.split(":").first(); + QHostInfo info = QHostInfo::fromName(host.split(":").first()); + if (info.error() == QHostInfo::NoError) + { + // Add host + QList hostAddresses = info.addresses(); + QHostAddress address; + for (int i = 0; i < hostAddresses.size(); i++) + { + // Exclude loopback IPv4 and all IPv6 addresses + if (!hostAddresses.at(i).toString().contains(":")) + { + address = hostAddresses.at(i); + } + } + currentHost = address; + //qDebug() << "Address:" << address.toString(); + // Set port according to user input + currentPort = host.split(":").last().toInt(); + } + } + else + { + QHostInfo info = QHostInfo::fromName(host); + if (info.error() == QHostInfo::NoError) + { + // Add host + currentHost = info.addresses().first(); + } + } + +} + +void QGCJSBSimLink::updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8) +{ + Q_UNUSED(time); + Q_UNUSED(act1); + Q_UNUSED(act2); + Q_UNUSED(act3); + Q_UNUSED(act4); + Q_UNUSED(act5); + Q_UNUSED(act6); + Q_UNUSED(act7); + Q_UNUSED(act8); +} + +void QGCJSBSimLink::updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode) +{ + // magnetos,aileron,elevator,rudder,throttle\n + + //float magnetos = 3.0f; + Q_UNUSED(time); + Q_UNUSED(systemMode); + Q_UNUSED(navMode); + + if(!isnan(rollAilerons) && !isnan(pitchElevator) && !isnan(yawRudder) && !isnan(throttle)) + { + QString state("%1\t%2\t%3\t%4\t%5\n"); + state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle); + writeBytes(state.toAscii().constData(), state.length()); + } + else + { + qDebug() << "HIL: Got NaN values from the hardware: isnan output: roll: " << isnan(rollAilerons) << ", pitch: " << isnan(pitchElevator) << ", yaw: " << isnan(yawRudder) << ", throttle: " << isnan(throttle); + } + //qDebug() << "Updated controls" << state; +} + +void QGCJSBSimLink::writeBytes(const char* data, qint64 size) +{ + //#define QGCJSBSimLink_DEBUG +#ifdef QGCJSBSimLink_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 + if (connectState && socket) socket->writeDatagram(data, size, currentHost, currentPort); +} + +/** + * @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 QGCJSBSimLink::readBytes() +{ + const qint64 maxLength = 65536; + char data[maxLength]; + QHostAddress sender; + quint16 senderPort; + + unsigned int s = socket->pendingDatagramSize(); + if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size" << std::endl; + socket->readDatagram(data, maxLength, &sender, &senderPort); + + QByteArray b(data, s); + + // Print string +// QString state(b); + +// // Parse string +// float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; +// double lat, lon, alt; +// double vx, vy, vz, xacc, yacc, zacc; + +// // Send updated state +// emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, +// pitchspeed, yawspeed, lat, lon, alt, +// vx, vy, vz, xacc, yacc, zacc); + + + + + // Echo data for debugging purposes + std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; + int i; + for (i=0; ipendingDatagramSize(); +} + +/** + * @brief Disconnect the connection. + * + * @return True if connection has been disconnected, false if connection couldn't be disconnected. + **/ +bool QGCJSBSimLink::disconnectSimulation() +{ + disconnect(process, SIGNAL(error(QProcess::ProcessError)), + this, SLOT(processError(QProcess::ProcessError))); + disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); + disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); + + if (process) + { + process->close(); + delete process; + process = NULL; + } + if (socket) + { + socket->close(); + delete socket; + socket = NULL; + } + + connectState = false; + + emit simulationDisconnected(); + emit simulationConnected(false); + return !connectState; +} + +/** + * @brief Connect the connection. + * + * @return True if connection has been established, false if connection couldn't be established. + **/ +bool QGCJSBSimLink::connectSimulation() +{ + qDebug() << "STARTING FLIGHTGEAR LINK"; + + if (!mav) return false; + socket = new QUdpSocket(this); + connectState = socket->bind(host, port); + + QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); + + process = new QProcess(this); + + connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); + connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); + + + UAS* uas = dynamic_cast(mav); + if (uas) + { + uas->startHil(); + } + + //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); + // Catch process error + QObject::connect( process, SIGNAL(error(QProcess::ProcessError)), + this, SLOT(processError(QProcess::ProcessError))); + + // Start Flightgear + QStringList arguments; + QString processJSB; + QString rootJSB; + +#ifdef Q_OS_MACX + processJSB = "/usr/local/bin/JSBSim"; + rootJSB = "/Applications/FlightGear.app/Contents/Resources/data"; +#endif + +#ifdef Q_OS_WIN32 + processJSB = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs"; + rootJSB = "C:\\Program Files (x86)\\FlightGear\\data"; +#endif + +#ifdef Q_OS_LINUX + processJSB = "/usr/games/fgfs"; + rootJSB = "/usr/share/games/flightgear"; +#endif + + // Sanity checks + bool sane = true; + QFileInfo executable(processJSB); + if (!executable.isExecutable()) + { + MainWindow::instance()->showCriticalMessage(tr("JSBSim Failed to Start"), tr("JSBSim was not found at %1").arg(processJSB)); + sane = false; + } + + QFileInfo root(rootJSB); + if (!root.isDir()) + { + MainWindow::instance()->showCriticalMessage(tr("JSBSim Failed to Start"), tr("JSBSim data directory was not found at %1").arg(rootJSB)); + sane = false; + } + + if (!sane) return false; + + /*Prepare JSBSim Arguments */ + + if (mav->getSystemType() == MAV_TYPE_QUADROTOR) + { + arguments << QString("--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB).arg(rootJSB).arg(script); + } + else + { + arguments << QString("JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB).arg(rootJSB).arg(script); + } + + process->start(processJSB, arguments); + + emit simulationConnected(connectState); + if (connectState) { + emit simulationConnected(); + connectionStartTime = QGC::groundTimeUsecs()/1000; + } + qDebug() << "STARTING SIM"; + + start(LowPriority); + return connectState; +} + +/** + * @brief Set the startup arguments used to start flightgear + * + **/ +void QGCJSBSimLink::setStartupArguments(QString startupArguments) +{ + this->startupArguments = startupArguments; +} + +/** + * @brief Check if connection is active. + * + * @return True if link is connected, false otherwise. + **/ +bool QGCJSBSimLink::isConnected() +{ + return connectState; +} + +QString QGCJSBSimLink::getName() +{ + return name; +} + +QString QGCJSBSimLink::getRemoteHost() +{ + return QString("%1:%2").arg(currentHost.toString(), currentPort); +} + +void QGCJSBSimLink::setName(QString name) +{ + this->name = name; +} diff --git a/src/comm/QGCJSBSimLink.h b/src/comm/QGCJSBSimLink.h new file mode 100644 index 0000000..4492414 --- /dev/null +++ b/src/comm/QGCJSBSimLink.h @@ -0,0 +1,153 @@ +/*===================================================================== + +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 UDP connection (server) for unmanned vehicles + * @author Lorenz Meier + * + */ + +#ifndef QGCJSBSIMLINK_H +#define QGCJSBSIMLINK_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "UASInterface.h" +#include "QGCHilLink.h" + +class QGCJSBSimLink : public QGCHilLink +{ + Q_OBJECT + //Q_INTERFACES(QGCJSBSimLinkInterface:LinkInterface) + +public: + QGCJSBSimLink(UASInterface* mav, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); + ~QGCJSBSimLink(); + + bool isConnected(); + qint64 bytesAvailable(); + int getPort() const { + return port; + } + + /** + * @brief The human readable port name + */ + QString getName(); + + /** + * @brief Get remote host and port + * @return string in format : + */ + QString getRemoteHost(); + + QString getVersion() + { + return QString("FlightGear %1").arg(flightGearVersion); + } + + int getAirFrameIndex() + { + return -1; + } + + void run(); + +public slots: +// void setAddress(QString address); + void setPort(int port); + /** @brief Add a new host to broadcast messages to */ + void setRemoteHost(const QString& host); + /** @brief Send new control states to the simulation */ + void updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode); + void updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8); +// /** @brief Remove a host from broadcasting messages to */ +// void removeHost(const QString& host); + // void readPendingDatagrams(); + void processError(QProcess::ProcessError err); + /** @brief Set the simulator version as text string */ + void setVersion(const QString& version) + { + Q_UNUSED(version); + } + + void selectAirframe(const QString& airframe) + { + script = airframe; + } + + void readBytes(); + /** + * @brief Write a number of bytes to the interface. + * + * @param data Pointer to the data byte array + * @param size The size of the bytes array + **/ + void writeBytes(const char* data, qint64 length); + bool connectSimulation(); + bool disconnectSimulation(); + + void setStartupArguments(QString startupArguments); + +protected: + QString name; + QHostAddress host; + QHostAddress currentHost; + quint16 currentPort; + quint16 port; + int id; + QUdpSocket* socket; + bool connectState; + + quint64 bitsSentTotal; + quint64 bitsSentCurrent; + quint64 bitsSentMax; + quint64 bitsReceivedTotal; + quint64 bitsReceivedCurrent; + quint64 bitsReceivedMax; + quint64 connectionStartTime; + QMutex statisticsMutex; + QMutex dataMutex; + QTimer refreshTimer; + UASInterface* mav; + QProcess* process; + unsigned int flightGearVersion; + QString startupArguments; + QString script; + + void setName(QString name); + +signals: + + +}; + +#endif // QGCJSBSimLink_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b741117..3cd2f85 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2642,6 +2642,33 @@ void UAS::enableHilFlightGear(bool enable, QString options) } /** +* If enabled, connect the JSBSim link. +*/ +void UAS::enableHilJSBSim(bool enable, QString options) +{ + QGCJSBSimLink* link = dynamic_cast(simulation); + if (!link || !simulation) { + // Delete wrong sim + if (simulation) { + stopHil(); + delete simulation; + } + simulation = new QGCJSBSimLink(this, options); + } + // Connect Flight Gear Link + link = dynamic_cast(simulation); + link->setStartupArguments(options); + if (enable) + { + startHil(); + } + else + { + stopHil(); + } +} + +/** * If enabled, connect the X-plane gear link. */ void UAS::enableHilXPlane(bool enable) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e6d6c1b..4806865 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCMAVLink.h" #include "QGCHilLink.h" #include "QGCFlightGearLink.h" +#include "QGCJSBSimLink.h" #include "QGCXPlaneLink.h" /** @@ -532,6 +533,7 @@ public slots: /** @brief Enable / disable HIL */ void enableHilFlightGear(bool enable, QString options); + void enableHilJSBSim(bool enable, QString options); void enableHilXPlane(bool enable); diff --git a/src/ui/QGCHilConfiguration.cc b/src/ui/QGCHilConfiguration.cc index 854699b..941707d 100644 --- a/src/ui/QGCHilConfiguration.cc +++ b/src/ui/QGCHilConfiguration.cc @@ -2,6 +2,7 @@ #include "ui_QGCHilConfiguration.h" #include "QGCHilFlightGearConfiguration.h" +#include "QGCHilJSBSimConfiguration.h" #include "QGCHilXPlaneConfiguration.h" QGCHilConfiguration::QGCHilConfiguration(UAS *mav, QWidget *parent) : @@ -46,7 +47,7 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) } } - else if(2 == index || 3 == index) + else if (2 == index || 3 == index) { // Ensure the sim exists and is disabled mav->enableHilXPlane(false); @@ -62,4 +63,17 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) connect(xplane, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString))); } } + else if (4) + { + // Ensure the sim exists and is disabled + mav->enableHilJSBSim(false, ""); + QGCHilJSBSimConfiguration* hfgconf = new QGCHilJSBSimConfiguration(mav, this); + hfgconf->show(); + ui->simulatorConfigurationLayout->addWidget(hfgconf); + QGCJSBSimLink* jsb = dynamic_cast(mav->getHILSimulation()); + if (jsb) + { + connect(jsb, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString))); + } + } } diff --git a/src/ui/QGCHilConfiguration.ui b/src/ui/QGCHilConfiguration.ui index da8adaf..cffe03b 100644 --- a/src/ui/QGCHilConfiguration.ui +++ b/src/ui/QGCHilConfiguration.ui @@ -23,7 +23,7 @@ - Status + No simulator active.. @@ -59,6 +59,11 @@ X-Plane 9 + + + JSBSim + + diff --git a/src/ui/QGCHilJSBSimConfiguration.cc b/src/ui/QGCHilJSBSimConfiguration.cc new file mode 100644 index 0000000..d376027 --- /dev/null +++ b/src/ui/QGCHilJSBSimConfiguration.cc @@ -0,0 +1,49 @@ +#include "QGCHilJSBSimConfiguration.h" +#include "ui_QGCHilJSBSimConfiguration.h" + +#include "MainWindow.h" + +QGCHilJSBSimConfiguration::QGCHilJSBSimConfiguration(UAS* mav,QWidget *parent) : + QWidget(parent), + mav(mav), + ui(new Ui::QGCHilJSBSimConfiguration) +{ + ui->setupUi(this); + + QStringList items = QStringList(); + if (mav->getSystemType() == MAV_TYPE_FIXED_WING) + { + items << "EasyStar"; + items << "Rascal110-JSBSim"; + items << "c172p"; + items << "YardStik"; + items << "Malolo1"; + } + else if (mav->getSystemType() == MAV_TYPE_QUADROTOR) + { + items << "arducopter"; + } + else + { + items << ""; + } + ui->aircraftComboBox->addItems(items); +} + +QGCHilJSBSimConfiguration::~QGCHilJSBSimConfiguration() +{ + delete ui; +} + +void QGCHilJSBSimConfiguration::on_startButton_clicked() +{ + //XXX check validity of inputs + QString options = ui->optionsPlainTextEdit->toPlainText(); + options.append(" --script=" + ui->aircraftComboBox->currentText()); + mav->enableHilJSBSim(true, options); +} + +void QGCHilJSBSimConfiguration::on_stopButton_clicked() +{ + mav->stopHil(); +} diff --git a/src/ui/QGCHilJSBSimConfiguration.h b/src/ui/QGCHilJSBSimConfiguration.h new file mode 100644 index 0000000..d315600 --- /dev/null +++ b/src/ui/QGCHilJSBSimConfiguration.h @@ -0,0 +1,33 @@ +#ifndef QGCHILJSBSIMCONFIGURATION_H +#define QGCHILJSBSIMCONFIGURATION_H + +#include + +#include "QGCHilLink.h" +#include "QGCFlightGearLink.h" +#include "UAS.h" + +namespace Ui { +class QGCHilJSBSimConfiguration; +} + +class QGCHilJSBSimConfiguration : public QWidget +{ + Q_OBJECT + +public: + explicit QGCHilJSBSimConfiguration(UAS* mav, QWidget *parent = 0); + ~QGCHilJSBSimConfiguration(); + +protected: + UAS* mav; + +private slots: + void on_startButton_clicked(); + void on_stopButton_clicked(); + +private: + Ui::QGCHilJSBSimConfiguration *ui; +}; + +#endif // QGCHILJSBSIMCONFIGURATION_H diff --git a/src/ui/QGCHilJSBSimConfiguration.ui b/src/ui/QGCHilJSBSimConfiguration.ui new file mode 100644 index 0000000..438be89 --- /dev/null +++ b/src/ui/QGCHilJSBSimConfiguration.ui @@ -0,0 +1,99 @@ + + + QGCHilJSBSimConfiguration + + + + 0 + 0 + 237 + 204 + + + + + 0 + 0 + + + + Form + + + Qt::LeftToRight + + + false + + + + 0 + + + 6 + + + + + Airframe: + + + + + + + + 0 + 0 + + + + true + + + + + + + <html><head/><body><p>Additional Options:</p></body></html> + + + + + + + + 0 + 0 + + + + --in-air --roll=0 --pitch=0 --vc=90 --heading=300 --timeofday=noon --disable-hud-3d --disable-fullscreen --geometry=400x300 --disable-anti-alias-hud --wind=0@0 --turbulence=0.0 --prop:/sim/frame-rate-throttle-hz=30 --control=mouse --disable-intro-music --disable-sound --disable-random-objects --disable-ai-models --shading-flat --fog-disable --disable-specular-highlight --disable-random-objects --disable-panel --disable-clouds --fdm=jsb --units-meters --prop:/engines/engine/running=true + + + + + + + + 0 + 0 + + + + Start + + + + + + + Stop + + + + + + + + From 4656a8eda888cdff6f8dc6c4aa036107f00670dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Apr 2013 13:03:15 +0200 Subject: [PATCH 3/4] Potential Linux start fix --- src/ui/HUD.cc | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 305f0fc..9bb2023 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -1279,14 +1279,16 @@ void HUD::drawCircle(float refX, float refY, float radius, float startDeg, float void HUD::resizeGL(int w, int h) { - glViewport(0, 0, w, h); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - glOrtho(0, w, 0, h, -1, 1); - glMatrixMode(GL_MODELVIEW); - glPolygonMode(GL_FRONT, GL_FILL); - //FIXME - paintHUD(); + if (isVisible()) { + glViewport(0, 0, w, h); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glOrtho(0, w, 0, h, -1, 1); + glMatrixMode(GL_MODELVIEW); + glPolygonMode(GL_FRONT, GL_FILL); + //FIXME + paintHUD(); + } } void HUD::selectWaypoint(int uasId, int id) From 50c36bb59f84bf6c38667fb3d692938de5b743a7 Mon Sep 17 00:00:00 2001 From: Christopher Hrabia Date: Thu, 11 Apr 2013 12:42:15 +0800 Subject: [PATCH 4/4] Revert last change on new logfile structure and added nulling of buffer. The reason for this is, because the last change in the file structure is buggy, it was only written the message length of the buffer to the file and missed to add the length for the timestamp. For fixing this problem it would be possible to insert something like len += sizeof(quint64); after: int len = mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); But I think a consistent file structure has more adavatages for a logfile over a more compressed structure. E.g. it would be necessary to go through all packages to proceed a jump during a replay. Additional the LogPlayer was not updated during the last 7 month since the logfile structure was changed (even it was buggy)... If filesize is really a problem I would recommend to change the logfile handling in a way of writting a temporary consistend logfile and compress it afterwards (QCompress) like done by many programs --- src/comm/MAVLinkProtocol.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index cee05d9..e14d6bd 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -291,11 +291,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Log data if (m_loggingEnabled && m_logfile) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)]; + uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)] = {0}; quint64 time = QGC::groundTimeUsecs(); memcpy(buf, (void*)&time, sizeof(quint64)); // Write message to buffer - int len = mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); + mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); + //we need to write the maximum package length for having a + //consistent file structure and beeing able to parse it again + int len = MAVLINK_MAX_PACKET_LEN + sizeof(quint64); QByteArray b((const char*)buf, len); if(m_logfile->write(b) != len) {