From 406c262bdc12e806301ccb2381530b003e7d28fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Sep 2012 16:08:14 +0200 Subject: [PATCH] Added PX4 calibration widget, switched HIL from FlightGear to XPlane, bringing up HIL --- .../px4/hexarotor/widgets/px4_calibration.qgw | 50 ++ .../px4/quadrotor/widgets/px4_calibration.qgw | 50 ++ qgroundcontrol.pro | 8 +- src/comm/QGCFlightGearLink.cc | 8 +- src/comm/QGCFlightGearLink.h | 22 +- src/comm/QGCHilLink.cc | 6 + src/comm/QGCHilLink.h | 66 +++ src/comm/QGCXPlaneLink.cc | 517 ++++++++++++++++++ src/comm/QGCXPlaneLink.h | 141 +++++ src/uas/UAS.cc | 19 +- src/uas/UAS.h | 4 +- src/ui/uas/UASView.cc | 10 +- src/ui/uas/UASView.h | 1 + 13 files changed, 869 insertions(+), 33 deletions(-) create mode 100644 files/px4/hexarotor/widgets/px4_calibration.qgw create mode 100644 files/px4/quadrotor/widgets/px4_calibration.qgw create mode 100644 src/comm/QGCHilLink.cc create mode 100644 src/comm/QGCHilLink.h create mode 100644 src/comm/QGCXPlaneLink.cc create mode 100644 src/comm/QGCXPlaneLink.h diff --git a/files/px4/hexarotor/widgets/px4_calibration.qgw b/files/px4/hexarotor/widgets/px4_calibration.qgw new file mode 100644 index 0000000000..dd81822ad5 --- /dev/null +++ b/files/px4/hexarotor/widgets/px4_calibration.qgw @@ -0,0 +1,50 @@ +[PX4%20Calibration%20Tool] +QGC_TOOL_WIDGET_ITEMS\1\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_DESCRIPTION=Reboot (only in standby) +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_BUTTONTEXT=REBOOT +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_COMMANDID=246 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM1=1 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM2=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM5=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\2\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_DESCRIPTION=Gyroscope calibration +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_BUTTONTEXT=MAG +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_COMMANDID=241 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM1=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM2=1 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM5=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\3\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_DESCRIPTION=Accelerometer calibration +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_BUTTONTEXT=ACCEL +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_COMMANDID=241 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM1=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM2=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM5=1 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\4\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_DESCRIPTION=Gyroscope calibration +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_BUTTONTEXT=GYRO +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_COMMANDID=241 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM1=1 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM2=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM5=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\size=4 diff --git a/files/px4/quadrotor/widgets/px4_calibration.qgw b/files/px4/quadrotor/widgets/px4_calibration.qgw new file mode 100644 index 0000000000..dd81822ad5 --- /dev/null +++ b/files/px4/quadrotor/widgets/px4_calibration.qgw @@ -0,0 +1,50 @@ +[PX4%20Calibration%20Tool] +QGC_TOOL_WIDGET_ITEMS\1\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_DESCRIPTION=Reboot (only in standby) +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_BUTTONTEXT=REBOOT +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_COMMANDID=246 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM1=1 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM2=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM5=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\1\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\2\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_DESCRIPTION=Gyroscope calibration +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_BUTTONTEXT=MAG +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_COMMANDID=241 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM1=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM2=1 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM5=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\2\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\3\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_DESCRIPTION=Accelerometer calibration +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_BUTTONTEXT=ACCEL +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_COMMANDID=241 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM1=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM2=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM5=1 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\3\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\4\TYPE=COMMANDBUTTON +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_DESCRIPTION=Gyroscope calibration +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_BUTTONTEXT=GYRO +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_COMMANDID=241 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAMS_VISIBLE=false +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM1=1 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM2=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM3=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM4=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM5=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM6=0 +QGC_TOOL_WIDGET_ITEMS\4\QGC_COMMAND_BUTTON_PARAM7=0 +QGC_TOOL_WIDGET_ITEMS\size=4 diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 82486d0dce..8222de0346 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -252,6 +252,7 @@ HEADERS += src/MG.h \ src/comm/ProtocolInterface.h \ src/comm/MAVLinkProtocol.h \ src/comm/QGCFlightGearLink.h \ + src/comm/QGCXPlaneLink.h \ src/ui/CommConfigurationWindow.h \ src/ui/SerialConfigurationWindow.h \ src/ui/MainWindow.h \ @@ -358,7 +359,8 @@ HEADERS += src/MG.h \ src/ui/mission/QGCMissionNavSweep.h \ src/ui/mission/QGCMissionDoStartSearch.h \ src/ui/mission/QGCMissionDoFinishSearch.h \ - src/ui/QGCVehicleConfig.h + src/ui/QGCVehicleConfig.h \ + src/comm/QGCHilLink.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -412,6 +414,7 @@ SOURCES += src/main.cc \ src/comm/SerialLink.cc \ src/comm/MAVLinkProtocol.cc \ src/comm/QGCFlightGearLink.cc \ + src/comm/QGCXPlaneLink.cc \ src/ui/CommConfigurationWindow.cc \ src/ui/SerialConfigurationWindow.cc \ src/ui/MainWindow.cc \ @@ -513,7 +516,8 @@ SOURCES += src/main.cc \ src/ui/mission/QGCMissionNavSweep.cc \ src/ui/mission/QGCMissionDoStartSearch.cc \ src/ui/mission/QGCMissionDoFinishSearch.cc \ - src/ui/QGCVehicleConfig.cc + src/ui/QGCVehicleConfig.cc \ + src/comm/QGCHilLink.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 13fcfd3244..f25655146d 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -304,8 +304,8 @@ bool QGCFlightGearLink::disconnectSimulation() connectState = false; - emit flightGearDisconnected(); - emit flightGearConnected(false); + emit simulationDisconnected(); + emit simulationConnected(false); return !connectState; } @@ -475,9 +475,9 @@ bool QGCFlightGearLink::connectSimulation() - emit flightGearConnected(connectState); + emit simulationConnected(connectState); if (connectState) { - emit flightGearConnected(); + emit simulationConnected(); connectionStartTime = QGC::groundTimeUsecs()/1000; } qDebug() << "STARTING SIM"; diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h index b2445deadd..d0a13a48e8 100644 --- a/src/comm/QGCFlightGearLink.h +++ b/src/comm/QGCFlightGearLink.h @@ -41,8 +41,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include "UASInterface.h" +#include "QGCHilLink.h" -class QGCFlightGearLink : public QThread +class QGCFlightGearLink : public QGCHilLink { Q_OBJECT //Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface) @@ -114,25 +115,6 @@ protected: void setName(QString name); signals: - /** - * @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); - - /** @brief State update from FlightGear */ - void hilStateChanged(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, - int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc); }; diff --git a/src/comm/QGCHilLink.cc b/src/comm/QGCHilLink.cc new file mode 100644 index 0000000000..9246fa41f0 --- /dev/null +++ b/src/comm/QGCHilLink.cc @@ -0,0 +1,6 @@ +#include "QGCHilLink.h" + +//QGCHilLink::QGCHilLink(QObject *parent) : +// QThread(parent) +//{ +//} diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h new file mode 100644 index 0000000000..c6aef61808 --- /dev/null +++ b/src/comm/QGCHilLink.h @@ -0,0 +1,66 @@ +#ifndef QGCHILLINK_H +#define QGCHILLINK_H + +#include +#include + +class QGCHilLink : public QThread +{ + Q_OBJECT +public: + + virtual bool isConnected() = 0; + virtual qint64 bytesAvailable() = 0; + virtual int getPort() const = 0; + + /** + * @brief The human readable port name + */ + virtual QString getName() = 0; + +public slots: + virtual void setPort(int port) = 0; + /** @brief Add a new host to broadcast messages to */ + virtual void setRemoteHost(const QString& host) = 0; + /** @brief Send new control states to the simulation */ + virtual void updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode) = 0; + virtual void processError(QProcess::ProcessError err) = 0; + + virtual void readBytes() = 0; + /** + * @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 + **/ + virtual void writeBytes(const char* data, qint64 length) = 0; + virtual bool connectSimulation() = 0; + virtual bool disconnectSimulation() = 0; + +protected: + virtual void setName(QString name) = 0; + +signals: + /** + * @brief This signal is emitted instantly when the link is connected + **/ + void simulationConnected(); + + /** + * @brief This signal is emitted instantly when the link is disconnected + **/ + void simulationDisconnected(); + + /** + * @brief This signal is emitted instantly when the link status changes + **/ + void simulationConnected(bool connected); + + /** @brief State update from FlightGear */ + void hilStateChanged(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, + float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, + int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc); + +}; + +#endif // QGCHILLINK_H diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc new file mode 100644 index 0000000000..30a3a55e57 --- /dev/null +++ b/src/comm/QGCXPlaneLink.cc @@ -0,0 +1,517 @@ +/*===================================================================== + +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 QGCXPlaneLink.cc + * Implementation of X-Plane interface + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include "QGCXPlaneLink.h" +#include "QGC.h" +#include +#include "MainWindow.h" + +QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress host, quint16 port) : + process(NULL), + terraSync(NULL) +{ + this->host = host; + this->port = port+mav->getUASID(); + this->connectState = false; + this->currentPort = 48000+mav->getUASID(); + this->mav = mav; + this->name = tr("X-Plane Link (port:%1)").arg(port); + setRemoteHost(remoteHost); +} + +QGCXPlaneLink::~QGCXPlaneLink() +{ //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 QGCXPlaneLink::run() +{ + exec(); +} + +void QGCXPlaneLink::setPort(int port) +{ + this->port = port; + disconnectSimulation(); + connectSimulation(); +} + +void QGCXPlaneLink::processError(QProcess::ProcessError err) +{ + switch(err) + { + case QProcess::FailedToStart: + MainWindow::instance()->showCriticalMessage(tr("X-Plane Failed to Start"), tr("Please check if the path and command is correct")); + break; + case QProcess::Crashed: + MainWindow::instance()->showCriticalMessage(tr("X-Plane Crashed"), tr("This is a X-Plane-related problem. Please upgrade X-Plane")); + break; + case QProcess::Timedout: + MainWindow::instance()->showCriticalMessage(tr("X-Plane 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 X-Plane"), tr("Please check if the path and command is correct")); + break; + case QProcess::ReadError: + MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with X-Plane"), tr("Please check if the path and command is correct")); + break; + case QProcess::UnknownError: + default: + MainWindow::instance()->showCriticalMessage(tr("X-Plane 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 QGCXPlaneLink::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 QGCXPlaneLink::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); + + 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()); + qDebug() << "Updated controls" << state; +} + +void QGCXPlaneLink::writeBytes(const char* data, qint64 size) +{ + //#define QGCXPlaneLink_DEBUG +#if 1 + 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 QGCXPlaneLink::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); + //qDebug() << "FG LINK GOT:" << state; + + QStringList values = state.split("\t"); + + // Check length + if (values.size() != 17) + { + qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << 17 << "BUT GOT" << values.size(); + qDebug() << state; + return; + } + + // Parse string + double time; + float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; + double lat, lon, alt; + double vx, vy, vz, xacc, yacc, zacc; + double airspeed; + + time = values.at(0).toDouble(); + lat = values.at(1).toDouble(); + lon = values.at(2).toDouble(); + alt = values.at(3).toDouble(); + roll = values.at(4).toDouble(); + pitch = values.at(5).toDouble(); + yaw = values.at(6).toDouble(); + rollspeed = values.at(7).toDouble(); + pitchspeed = values.at(8).toDouble(); + yawspeed = values.at(9).toDouble(); + + xacc = values.at(10).toDouble(); + yacc = values.at(11).toDouble(); + zacc = values.at(12).toDouble(); + + vx = values.at(13).toDouble(); + vy = values.at(14).toDouble(); + vz = values.at(15).toDouble(); + + airspeed = values.at(16).toDouble(); + + // 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 QGCXPlaneLink::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 (terraSync) + { + terraSync->close(); + delete terraSync; + terraSync = 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 QGCXPlaneLink::connectSimulation() +{ + 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); + //terraSync = 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))); + + // XXX ugly hack to set MAVLINK_MODE_HIL_FLAG_ENABLED + // without pulling MAVLINK dependencies in here + mav->setMode(32); + + // XXX This will later be enabled to start X-Plane from within QGroundControl with the right arguments + +// //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); +// // Catch process error +// QObject::connect( process, SIGNAL(error(QProcess::ProcessError)), +// this, SLOT(processError(QProcess::ProcessError))); +// QObject::connect( terraSync, SIGNAL(error(QProcess::ProcessError)), +// this, SLOT(processError(QProcess::ProcessError))); +// // Start X-Plane +// QStringList processCall; +// QString processFgfs; +// QString processTerraSync; +// QString fgRoot; +// QString fgScenery; +// QString aircraft; + +// if (mav->getSystemType() == MAV_TYPE_FIXED_WING) +// { +// aircraft = "Rascal110-JSBSim"; +// } +// else if (mav->getSystemType() == MAV_TYPE_QUADROTOR) +// { +// aircraft = "arducopter"; +// } +// else +// { +// aircraft = "Rascal110-JSBSim"; +// } + +//#ifdef Q_OS_MACX +// processFgfs = "/Applications/X-Plane.app/Contents/Resources/fgfs"; +// processTerraSync = "/Applications/X-Plane.app/Contents/Resources/terrasync"; +// fgRoot = "/Applications/X-Plane.app/Contents/Resources/data"; +// //fgScenery = "/Applications/X-Plane.app/Contents/Resources/data/Scenery"; +// fgScenery = "/Applications/X-Plane.app/Contents/Resources/data/Scenery-TerraSync"; +// // /Applications/X-Plane.app/Contents/Resources/data/Scenery: +//#endif + +//#ifdef Q_OS_WIN32 +// processFgfs = "C:\\Program Files (x86)\\X-Plane\\bin\\Win32\\fgfs"; +// fgRoot = "C:\\Program Files (x86)\\X-Plane\\data"; +// fgScenery = "C:\\Program Files (x86)\\X-Plane\\data\\Scenery-Terrasync"; +//#endif + +//#ifdef Q_OS_LINUX +// processFgfs = "fgfs"; +// fgRoot = "/usr/share/X-Plane/data"; +// fgScenery = "/usr/share/X-Plane/data/Scenery-Terrasync"; +//#endif + +// // Sanity checks +// bool sane = true; +// QFileInfo executable(processFgfs); +// if (!executable.isExecutable()) +// { +// MainWindow::instance()->showCriticalMessage(tr("X-Plane Failed to Start"), tr("X-Plane was not found at %1").arg(processFgfs)); +// sane = false; +// } + +// QFileInfo root(fgRoot); +// if (!root.isDir()) +// { +// MainWindow::instance()->showCriticalMessage(tr("X-Plane Failed to Start"), tr("X-Plane data directory was not found at %1").arg(fgRoot)); +// sane = false; +// } + +// QFileInfo scenery(fgScenery); +// if (!scenery.isDir()) +// { +// MainWindow::instance()->showCriticalMessage(tr("X-Plane Failed to Start"), tr("X-Plane scenery directory was not found at %1").arg(fgScenery)); +// sane = false; +// } + +// if (!sane) return false; + +// // --atlas=socket,out,1,localhost,5505,udp +// // terrasync -p 5505 -S -d /usr/local/share/TerraSync + +// processCall << QString("--fg-root=%1").arg(fgRoot); +// processCall << QString("--fg-scenery=%1").arg(fgScenery); +// if (mav->getSystemType() == MAV_TYPE_QUADROTOR) +// { +// // FIXME ADD QUAD-Specific protocol here +// processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port); +// processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort); +// } +// else +// { +// processCall << QString("--generic=socket,out,50,127.0.0.1,%1,udp,qgroundcontrol").arg(port); +// processCall << QString("--generic=socket,in,50,127.0.0.1,%1,udp,qgroundcontrol").arg(currentPort); +// } +// processCall << "--atlas=socket,out,1,localhost,5505,udp"; +// processCall << "--in-air"; +// processCall << "--roll=0"; +// processCall << "--pitch=0"; +// processCall << "--vc=90"; +// processCall << "--heading=300"; +// processCall << "--timeofday=noon"; +// processCall << "--disable-hud-3d"; +// processCall << "--disable-fullscreen"; +// processCall << "--geometry=400x300"; +// processCall << "--disable-anti-alias-hud"; +// processCall << "--wind=0@0"; +// processCall << "--turbulence=0.0"; +// processCall << "--prop:/sim/frame-rate-throttle-hz=30"; +// processCall << "--control=mouse"; +// processCall << "--disable-intro-music"; +// processCall << "--disable-sound"; +// processCall << "--disable-random-objects"; +// processCall << "--disable-ai-models"; +// processCall << "--shading-flat"; +// processCall << "--fog-disable"; +// processCall << "--disable-specular-highlight"; +// //processCall << "--disable-skyblend"; +// processCall << "--disable-random-objects"; +// processCall << "--disable-panel"; +// //processCall << "--disable-horizon-effect"; +// processCall << "--disable-clouds"; +// processCall << "--fdm=jsb"; +// processCall << "--units-meters"; +// if (mav->getSystemType() == MAV_TYPE_QUADROTOR) +// { +// // Start all engines of the quad +// processCall << "--prop:/engines/engine[0]/running=true"; +// processCall << "--prop:/engines/engine[1]/running=true"; +// processCall << "--prop:/engines/engine[2]/running=true"; +// processCall << "--prop:/engines/engine[3]/running=true"; +// } +// else +// { +// processCall << "--prop:/engines/engine/running=true"; +// } +// processCall << QString("--lat=%1").arg(UASManager::instance()->getHomeLatitude()); +// processCall << QString("--lon=%1").arg(UASManager::instance()->getHomeLongitude()); +// processCall << QString("--altitude=%1").arg(UASManager::instance()->getHomeAltitude()); +// // Add new argument with this: processCall << ""; +// processCall << QString("--aircraft=%2").arg(aircraft); + + +// QStringList terraSyncArguments; +// terraSyncArguments << "-p 5505"; +// terraSyncArguments << "-S"; +// terraSyncArguments << QString("-d=%1").arg(fgScenery); + +// terraSync->start(processTerraSync, terraSyncArguments); +// process->start(processFgfs, processCall); + + + +// emit X-PlaneConnected(connectState); +// if (connectState) { +// emit X-PlaneConnected(); +// connectionStartTime = QGC::groundTimeUsecs()/1000; +// } +// qDebug() << "STARTING SIM"; + +// qDebug() << "STARTING: " << processFgfs << processCall; + + qDebug() << "STARTING X-PLANE LINK"; + + start(LowPriority); + return connectState; +} + +/** + * @brief Check if connection is active. + * + * @return True if link is connected, false otherwise. + **/ +bool QGCXPlaneLink::isConnected() +{ + return connectState; +} + +QString QGCXPlaneLink::getName() +{ + return name; +} + +void QGCXPlaneLink::setName(QString name) +{ + this->name = name; + // emit nameChanged(this->name); +} diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h new file mode 100644 index 0000000000..f11c9e5776 --- /dev/null +++ b/src/comm/QGCXPlaneLink.h @@ -0,0 +1,141 @@ +/*===================================================================== + +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 QGCXPlaneLink.h + * @brief X-Plane simulation link + * @author Lorenz Meier + * + */ + +#ifndef QGCXPLANESIMULATIONLINK_H +#define QGCXPLANESIMULATIONLINK_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "UASInterface.h" +#include "QGCHilLink.h" + +class QGCXPlaneLink : public QGCHilLink +{ + Q_OBJECT + //Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface) + +public: + QGCXPlaneLink(UASInterface* mav, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); + ~QGCXPlaneLink(); + + bool isConnected(); + qint64 bytesAvailable(); + int getPort() const { + return port; + } + + /** + * @brief The human readable port name + */ + QString getName(); + + 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); +// /** @brief Remove a host from broadcasting messages to */ +// void removeHost(const QString& host); + // void readPendingDatagrams(); + void processError(QProcess::ProcessError err); + + 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(); + +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; + QProcess* terraSync; + + void setName(QString name); + +signals: + /** + * @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); + + /** @brief State update from FlightGear */ + void hilStateChanged(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, + float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, + int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc); + + +}; + +#endif // QGCXPLANESIMULATIONLINK_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1151536163..2498410064 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -97,7 +97,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), paramManager(NULL), attitudeStamped(false), lastAttitude(0), - simulation(new QGCFlightGearLink(this)), + simulation(new QGCXPlaneLink(this)), isLocalPositionKnown(false), isGlobalPositionKnown(false), systemIsArmed(false), @@ -2516,9 +2516,20 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) { - mavlink_message_t msg; - mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); - sendMessage(msg); + if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) + { + mavlink_message_t msg; + mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); + sendMessage(msg); + } + else + { + // Attempt to set HIL mode + mavlink_message_t msg; + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); + sendMessage(msg); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + } } /** diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 3f2e591c6b..c96d03a5b4 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -36,7 +36,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include "QGCMAVLink.h" +#include "QGCHilLink.h" #include "QGCFlightGearLink.h" +#include "QGCXPlaneLink.h" /** * @brief A generic MAVLINK-connected MAV/UAV @@ -330,7 +332,7 @@ protected: //COMMENTS FOR TEST UNIT QString shortModeText; ///< Short textual mode description bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV quint64 lastAttitude; ///< Timestamp of last attitude measurement - QGCFlightGearLink* simulation; ///< Hardware in the loop simulation link + QGCHilLink* simulation; ///< Hardware in the loop simulation link bool isLocalPositionKnown; ///< If the local position has been received for this MAV bool isGlobalPositionKnown; ///< If the global position has been received for this MAV bool systemIsArmed; ///< If the system is armed diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index aca094e142..e8eaa498d3 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -68,7 +68,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : removeAction(new QAction("Delete this system", this)), renameAction(new QAction("Rename..", this)), selectAction(new QAction("Control this system", this )), - hilAction(new QAction("Enable Hardware-in-the-Loop Simulation", this )), + hilAction(new QAction("Enable Flightgear Hardware-in-the-Loop Simulation", this )), + hilXAction(new QAction("Enable X-Plane Hardware-in-the-Loop Simulation", this )), selectAirframeAction(new QAction("Choose Airframe", this)), setBatterySpecsAction(new QAction("Set Battery Options", this)), lowPowerModeEnabled(true), @@ -80,6 +81,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : lowPowerModeEnabled = MainWindow::instance()->lowPowerModeEnabled(); hilAction->setCheckable(true); + // Flightgear is not ready for prime time + hilAction->setEnabled(false); + hilXAction->setCheckable(true); m_ui->setupUi(this); @@ -119,6 +123,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); connect(hilAction, SIGNAL(triggered(bool)), uas, SLOT(enableHil(bool))); + connect(hilXAction, SIGNAL(triggered(bool)), uas, SLOT(enableHil(bool))); connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); connect(setBatterySpecsAction, SIGNAL(triggered()), this, SLOT(setBatterySpecs())); connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); @@ -464,7 +469,8 @@ void UASView::contextMenuEvent (QContextMenuEvent* event) { menu.addAction(removeAction); } - menu.addAction(hilAction); + menu.addAction(hilXAction); + // XXX Re-enable later menu.addAction(hilXAction); menu.addAction(selectAirframeAction); menu.addAction(setBatterySpecsAction); menu.exec(event->globalPos()); diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 44b10fac95..889dc591e7 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -122,6 +122,7 @@ protected: QAction* renameAction; QAction* selectAction; QAction* hilAction; + QAction* hilXAction; QAction* selectAirframeAction; QAction* setBatterySpecsAction; static const int updateInterval = 800; -- GitLab