diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index adb3de6ae..6815c92ff 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -223,6 +223,7 @@ SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c SRC += $(OPUAVSYNTHDIR)/baroaltitude.c SRC += $(OPUAVSYNTHDIR)/txpidsettings.c +SRC += $(OPUAVSYNTHDIR)/airspeedactual.c endif diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index db715a74b..9ecb4097b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -955,19 +955,74 @@ + + + false + 0.0.0 + + + false + false + false + true + true + true + 20 + 50 + false + \home\lafargue\X-Plane 9\X-Plane-i686 + \usr\share\games\FlightGear + true + 100 + false + true + 100 + 0.0.0.0 + 40100 + true + + + false + false + 40 + 40200 + 127.0.0.1 + ASimRC + false + + false 0.0.0 + false + false + false + true + true + true + 20 + 50 + false \usr\games\fgfs \usr\share\games\FlightGear + false + 100 + false + true + 100 127.0.0.1 9009 - false + true + + + true + false + 40 9010 - 127.0.0.1 + 127.0.0.1 FG true @@ -978,51 +1033,37 @@ 0.0.0 + false + false + false + true + true + true + 20 + 50 + false \home\lafargue\X-Plane 9\X-Plane-i686 \usr\share\games\FlightGear - 127.0.0.3 + false + 100 + false + true + 100 + 0.0.0.0 6756 - false + true + + + true + false + 40 49000 - 127.0.0.1 + 127.0.0.1 X-Plane false - - - - false - 0.0.0 - - - false - false - true - true - true - 20 - true - 200 - true - 0 - true - 127.0.0.1 - 40100 - true - false - false - 40200 - 20 - 127.0.0.1 - ASimRC - 50 - false - @Variant(AAAAh0CgAAA=) - - - diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp index a7f7117b1..bf5b987e8 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp @@ -257,5 +257,26 @@ void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3]) Rbe[2][2] = q0s - q1s - q2s + q3s; } +//** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame ** +void CoordinateConversions::R2Quaternion(float const Rbe[3][3], float q[4]) +{ + qreal w, x, y, z; + + // w always >= 0 + w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0; + x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0; + y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0; + z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0; + + x = copysign(x, (Rbe[1][2] - Rbe[2][1])); + y = copysign(y, (Rbe[2][0] - Rbe[0][2])); + z = copysign(z, (Rbe[0][1] - Rbe[1][0])); + + q[0]=w; + q[1]=x; + q[2]=y; + q[3]=z; +} + } diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h index ca077a0c4..5f515c147 100644 --- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h +++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h @@ -50,6 +50,7 @@ public: void Quaternion2RPY(const float q[4], float rpy[3]); void RPY2Quaternion(const float rpy[3], float q[4]); void Quaternion2R(const float q[4], float Rbe[3][3]); + void R2Quaternion(float const Rbe[3][3], float q[4]); }; } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/Start Flightgear XP.bat b/ground/openpilotgcs/src/plugins/hitl/Start Flightgear XP.bat similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/Start Flightgear XP.bat rename to ground/openpilotgcs/src/plugins/hitl/Start Flightgear XP.bat diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/aerosimrc.pro similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/aerosimrc.pro diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.pro similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.pro diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_off.tga similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_off.tga diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_off_hover.tga similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_off_hover.tga diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_on.tga similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_on.tga diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_on_hover.tga similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_on_hover.tga diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/plugin.txt similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/plugin.txt diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptest.pro similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptest.pro diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui b/ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.ui similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui rename to ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.ui diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp new file mode 100644 index 000000000..e1cbb6b78 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp @@ -0,0 +1,273 @@ +/** + ****************************************************************************** + * + * @file aerosimrc.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITLv2 Plugin + * @{ + * @brief The Hardware In The Loop plugin version 2 + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "aerosimrcsimulator.h" +#include +#include +#include + +AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms) + : Simulator(params) +{ + udpCounterASrecv = 0; +} + +AeroSimRCSimulator::~AeroSimRCSimulator() +{ +} + +bool AeroSimRCSimulator::setupProcess() +{ + QMutexLocker locker(&lock); + return true; +} + +void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort) +{ + Q_UNUSED(outPort) + if (inSocket->bind(QHostAddress(host), inPort)) + emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n"); + else + emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n"); +} + +void AeroSimRCSimulator::transmitUpdate() +{ + // read actuator output + ActuatorCommand::DataFields actCmdData; + actCmdData = actCommand->getData(); + float channels[10]; + for (int i = 0; i < 10; ++i) { + qint16 ch = actCmdData.Channel[i]; + float out = -1.0; + if (ch >= 1000 && ch <= 2000) { + ch -= 1000; + out = ((float)ch / 500.0) - 1.0; + } + channels[i] = out; + } + + // read flight status + FlightStatus::DataFields flightData; + flightData = flightStatus->getData(); + quint8 armed; + quint8 mode; + armed = flightData.Armed; + mode = flightData.FlightMode; + + QByteArray data; + // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32) + data.resize(50); + QDataStream stream(&data, QIODevice::WriteOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); + stream << quint32(0x52434D44); // magic header, "RCMD" + for (int i = 0; i < 10; ++i) + stream << channels[i]; // channels + stream << armed << mode; // flight status + stream << udpCounterASrecv; // packet counter + + if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { + qDebug() << "write failed: " << outSocket->errorString(); + } + +#ifdef DBG_TIMERS + static int cntTX = 0; + if (cntTX >= 100) { + qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart(); + cntTX = 0; + } else { + ++cntTX; + } +#endif +} + +void AeroSimRCSimulator::processUpdate(const QByteArray &data) +{ + // check size + if (data.size() > 188) { + qDebug() << "!!! big datagram: " << data.size(); + return; + } + + QByteArray buf = data; + QDataStream stream(&buf, QIODevice::ReadOnly); + stream.setFloatingPointPrecision(QDataStream::SinglePrecision); + + // check magic header + quint32 magic; + stream >> magic; + if (magic != 0x4153494D) { // "AERO" + qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D); + return; + } + +#define AEROSIM_RCCHANNEL_NUMELEM 8 + float delT, + homeX, homeY, homeZ, + WpHX, WpHY, WpLat, WpLon, + posX, posY, posZ, // world + velX, velY, velZ, // world + angX, angY, angZ, // model + accX, accY, accZ, // model + lat, lon, agl, // world + yaw, pitch, roll, // model + volt, curr, + rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix + ch[AEROSIM_RCCHANNEL_NUMELEM]; + + stream >> delT; + stream >> homeX >> homeY >> homeZ; + stream >> WpHX >> WpHY >> WpLat >> WpLon; + stream >> posX >> posY >> posZ; + stream >> velX >> velY >> velZ; + stream >> angX >> angY >> angZ; + stream >> accX >> accY >> accZ; + stream >> lat >> lon >> agl; + stream >> yaw >> pitch >> roll; + stream >> volt >> curr; + stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz; + stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7]; + stream >> udpCounterASrecv; + + Output2Hardware out; + memset(&out, 0, sizeof(Output2Hardware)); + + + out.delT=delT; + + /*************************************************************************************/ + for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){ + out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1 + } + + /**********************************************************************************************/ + QMatrix4x4 mat; + mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix + ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) + -uy, -ux, uz, 0.0, + 0.0, 0.0, 0.0, 1.0); + mat.optimize(); + + QQuaternion quat; // model quat + asMatrix2Quat(mat, quat); + + /*************************************************************************************/ + // rotate gravity + QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) + QVector3D gee = QVector3D(0.0, 0.0, -GEE); + QQuaternion qWorld = quat.conjugate(); + gee = qWorld.rotatedVector(gee); + acc += gee; + + out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) + out.pitchRate = angX * RAD2DEG; + out.yawRate = angZ * -RAD2DEG; + + out.accX = acc.x(); + out.accY = acc.y(); + out.accZ = acc.z(); + + /*************************************************************************************/ + QVector3D rpy; // model roll, pitch, yaw + asMatrix2RPY(mat, rpy); + + out.roll = rpy.x(); + out.pitch = rpy.y(); + out.heading = rpy.z(); + + + /**********************************************************************************************/ + out.altitude = posZ; + out.heading = yaw * RAD2DEG; + out.latitude = lat * 10e6; + out.longitude = lon * 10e6; + out.groundspeed = qSqrt(velX * velX + velY * velY); + + /**********************************************************************************************/ + out.dstN = posY * 100; + out.dstE = posX * 100; + out.dstD = posZ * -100; + + out.velDown = velY * 100; + out.velEast = velX * 100; + out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`??? + + updateUAVOs(out); + + +#ifdef DBG_TIMERS + static int cntRX = 0; + if (cntRX >= 100) { + qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart(); + cntRX = 0; + } else { + ++cntRX; + } +#endif +} + +// transfomations + +void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q) +{ + qreal w, x, y, z; + + // w always >= 0 + w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0; + x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0; + y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0; + z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0; + + x = copysign(x, (m(1, 2) - m(2, 1))); + y = copysign(y, (m(2, 0) - m(0, 2))); + z = copysign(z, (m(0, 1) - m(1, 0))); + + q.setScalar(w); + q.setX(x); + q.setY(y); + q.setZ(z); +} + +void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) +{ + qreal roll, pitch, yaw; + + if (qFabs(m(0, 2)) > 0.998) { + // ~86.3°, gimbal lock + roll = 0.0; + pitch = copysign(M_PI_2, -m(0, 2)); + yaw = qAtan2(-m(1, 0), m(1, 1)); + } else { + roll = qAtan2(m(1, 2), m(2, 2)); + pitch = qAsin(-m(0, 2)); + yaw = qAtan2(m(0, 1), m(0, 0)); + } + + rpy.setX(roll * RAD2DEG); + rpy.setY(pitch * RAD2DEG); + rpy.setZ(yaw * RAD2DEG); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h similarity index 98% rename from ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h rename to ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h index 42b7d4aa5..b265b9463 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h @@ -32,7 +32,7 @@ #include #include #include -#include "simulatorv2.h" +#include "simulator.h" class AeroSimRCSimulator: public Simulator { diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp new file mode 100644 index 000000000..99cbeb82e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -0,0 +1,349 @@ +/** + ****************************************************************************** + * + * @file flightgearbridge.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop plugin + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "fgsimulator.h" +#include "extensionsystem/pluginmanager.h" +#include "coreplugin/icore.h" +#include "coreplugin/threadmanager.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +//FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : +// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), +// fgProcess(NULL) +//{ +// // Note: Only tested on windows 7 +//#if defined(Q_WS_WIN) +// cmdShell = QString("c:/windows/system32/cmd.exe"); +//#else +// cmdShell = QString("bash"); +//#endif +//} + +FGSimulator::FGSimulator(const SimulatorSettings& params) : + Simulator(params) +{ + udpCounterFGrecv = 0; + udpCounterGCSsend = 0; +} + +FGSimulator::~FGSimulator() +{ + disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); +} + +void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +{ + Q_UNUSED(outPort); + + if(inSocket->bind(QHostAddress(host), inPort)) + emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n"); + else + emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n"); +} + +bool FGSimulator::setupProcess() +{ + QMutexLocker locker(&lock); + + // Copy FlightGear generic protocol configuration file to the FG protocol directory + // NOTE: Not working on Windows 7, if FG is installed in the "Program Files", + // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml + // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); + // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); + // QString xml = xmlFile.readAll(); + // xmlFile.close(); + // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); + // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); + // xmlFileOut.write(xml.toAscii()); + // xmlFileOut.close(); + + Qt::HANDLE mainThread = QThread::currentThreadId(); + qDebug() << "setupProcess Thread: "<< mainThread; + + simProcess = new QProcess(); + simProcess->setReadChannelMode(QProcess::MergedChannels); + connect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); + // Note: Only tested on windows 7 +#if defined(Q_WS_WIN) + QString cmdShell("c:/windows/system32/cmd.exe"); +#else + QString cmdShell("bash"); +#endif + + // Start shell (Note: Could not start FG directly on Windows, only through terminal!) + simProcess->start(cmdShell); + if (simProcess->waitForStarted() == false) + { + emit processOutput("Error:" + simProcess->errorString()); + return false; + } + + // Setup arguments + // Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS. + // If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup. + QString args("--fg-root=\"" + settings.dataPath + "\" " + + "--timeofday=noon " + + "--httpd=5400 " + + "--enable-hud " + + "--in-air " + + "--altitude=3000 " + + "--vc=100 " + + "--log-level=alert " + + "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); + if(settings.manualControlEnabled) // <--[BCH] What does this do? Why does it depend on ManualControl? + { + args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); + } + + // Start FlightGear - only if checkbox is selected in HITL options page + if (settings.startSim) + { + QString cmd("\"" + settings.binPath + "\" " + args + "\n"); + simProcess->write(cmd.toAscii()); + } + else + { + emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + + "You can optionally run Flightgear from a networked computer.\n" + + "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n" + "Remote computer must have the correct OpenPilot protocol installed."); + } + + udpCounterGCSsend = 0; + + return true; +} + +void FGSimulator::processReadyRead() +{ + QByteArray bytes = simProcess->readAllStandardOutput(); + QString str(bytes); + if ( !str.contains("Error reading data") ) // ignore error + { + emit processOutput(str); + } +} + +void FGSimulator::transmitUpdate() +{ + ActuatorDesired::DataFields actData; + FlightStatus::DataFields flightStatusData = flightStatus->getData(); + ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); + float ailerons = -1; + float elevator = -1; + float rudder = -1; + float throttle = -1; + + if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) + { + // Read joystick input + if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) + { + // Note: Pitch sign is reversed in FG ? + ailerons = manCtrlData.Roll; + elevator = -manCtrlData.Pitch; + rudder = manCtrlData.Yaw; + throttle = manCtrlData.Throttle; + } + } + else + { + // Read ActuatorDesired from autopilot + actData = actDesired->getData(); + + ailerons = actData.Roll; + elevator = -actData.Pitch; + rudder = actData.Yaw; + throttle = actData.Throttle; + } + + int allowableDifference = 10; + + //qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv; + + if(udpCounterFGrecv == udpCounterGCSsend) + udpCounterGCSsend = 0; + + if((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv==0) ) //FG udp queue is not delayed + { + udpCounterGCSsend++; + + // Send update to FlightGear + QString cmd; + cmd = QString("%1,%2,%3,%4,%5\n") + .arg(ailerons) //ailerons + .arg(elevator) //elevator + .arg(rudder) //rudder + .arg(throttle) //throttle + .arg(udpCounterGCSsend); //UDP packet counter delay + + QByteArray data = cmd.toAscii(); + + if(outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) + { + emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n"); + } + } + else + { + // don't send new packet. Flightgear cannot process UDP fast enough. + // V1.9.1 reads udp packets at set frequency and will get delayed if packets are sent too fast + // V2.0 does not currently work with --generic-protocol + } + + if(settings.manualControlEnabled) + { + actData.Roll = ailerons; + actData.Pitch = -elevator; + actData.Yaw = rudder; + actData.Throttle = throttle; + //actData.NumLongUpdates = (float)udpCounterFGrecv; + //actData.UpdateTime = (float)udpCounterGCSsend; + actDesired->setData(actData); + } +} + + +void FGSimulator::processUpdate(const QByteArray& inp) +{ + //TODO: this does not use the FLIGHT_PARAM structure, it should! + // Split + QString data(inp); + QStringList fields = data.split(","); + // Get xRate (deg/s) + // float xRate = fields[0].toFloat() * 180.0/M_PI; + // Get yRate (deg/s) + // float yRate = fields[1].toFloat() * 180.0/M_PI; + // Get zRate (deg/s) + // float zRate = fields[2].toFloat() * 180.0/M_PI; + // Get xAccel (m/s^2) + float xAccel = fields[3].toFloat() * FT2M; + // Get yAccel (m/s^2) + float yAccel = fields[4].toFloat() * FT2M; + // Get xAccel (m/s^2) + float zAccel = fields[5].toFloat() * FT2M; + // Get pitch (deg) + float pitch = fields[6].toFloat(); + // Get pitchRate (deg/s) + float pitchRate = fields[7].toFloat(); + // Get roll (deg) + float roll = fields[8].toFloat(); + // Get rollRate (deg/s) + float rollRate = fields[9].toFloat(); + // Get yaw (deg) + float yaw = fields[10].toFloat(); + // Get yawRate (deg/s) + float yawRate = fields[11].toFloat(); + // Get latitude (deg) + float latitude = fields[12].toFloat(); + // Get longitude (deg) + float longitude = fields[13].toFloat(); + // Get heading (deg) + float heading = fields[14].toFloat(); + // Get altitude (m) + float altitude = fields[15].toFloat() * FT2M; + // Get altitudeAGL (m) + float altitudeAGL = fields[16].toFloat() * FT2M; + // Get groundspeed (m/s) + float groundspeed = fields[17].toFloat() * KT2MPS; + // Get airspeed (m/s) + float airspeed = fields[18].toFloat() * KT2MPS; + // Get temperature (degC) + float temperature = fields[19].toFloat(); + // Get pressure (kpa) + float pressure = fields[20].toFloat() * INHG2KPA; + // Get VelocityActual Down (cm/s) + float velocityActualDown = - fields[21].toFloat() * FPS2CMPS; + // Get VelocityActual East (cm/s) + float velocityActualEast = fields[22].toFloat() * FPS2CMPS; + // Get VelocityActual Down (cm/s) + float velocityActualNorth = fields[23].toFloat() * FPS2CMPS; + + // Get UDP packets received by FG + int n = fields[24].toInt(); + udpCounterFGrecv = n; + + /////// + // Output formatting + /////// + Output2Hardware out; + memset(&out, 0, sizeof(Output2Hardware)); + + float NED[3]; + // convert from cm back to meters + + double LLA[3] = {latitude, longitude, altitude}; + double ECEF[3]; + double RNE[9]; + Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); + Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); + Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) RNE, NED); + + + // Update GPS Position objects + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude; + out.groundspeed = groundspeed; + + out.calibratedAirspeed = airspeed; + + + // Update BaroAltitude object + out.temperature = temperature; + out.pressure = pressure; + + // Update attActual object + out.roll = roll; //roll; + out.pitch = pitch; // pitch + out.heading = yaw; // yaw + + out.dstN= NED[0]; + out.dstE= NED[1]; + out.dstD= NED[2]; + + // Update VelocityActual.{North,East,Down} + out.velNorth = velocityActualNorth; + out.velEast = velocityActualEast; + out.velDown = velocityActualDown; + + //Update gyroscope sensor data + out.rollRate = rollRate; + out.pitchRate = pitchRate; + out.yawRate = yawRate; + + //Update accelerometer sensor data + out.accX = xAccel; + out.accY = yAccel; + out.accZ = -zAccel; + + updateUAVOs(out); +} + diff --git a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.h b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.h rename to ground/openpilotgcs/src/plugins/hitl/fgsimulator.h diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pluginspec b/ground/openpilotgcs/src/plugins/hitl/hitl.pluginspec similarity index 86% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pluginspec rename to ground/openpilotgcs/src/plugins/hitl/hitl.pluginspec index a52539fdc..573d8094e 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pluginspec +++ b/ground/openpilotgcs/src/plugins/hitl/hitl.pluginspec @@ -1,4 +1,4 @@ - + The OpenPilot Project (C) 2010 OpenPilot Project The GNU Public License (GPL) Version 3 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro b/ground/openpilotgcs/src/plugins/hitl/hitl.pro similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro rename to ground/openpilotgcs/src/plugins/hitl/hitl.pro diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlnew_dependencies.pri b/ground/openpilotgcs/src/plugins/hitl/hitl_dependencies.pri similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlnew_dependencies.pri rename to ground/openpilotgcs/src/plugins/hitl/hitl_dependencies.pri diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp new file mode 100644 index 000000000..3ea4d3bf2 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp @@ -0,0 +1,164 @@ +/** + ****************************************************************************** + * + * @file hitlconfiguration.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop plugin + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "hitlconfiguration.h" + +HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) : + IUAVGadgetConfiguration(classId, parent) +{ + + //Default settings values + settings.simulatorId = ""; + settings.binPath = ""; + settings.dataPath = ""; + settings.manualControlEnabled = true; + settings.startSim = false; + settings.addNoise = false; + settings.hostAddress = "127.0.0.1"; + settings.remoteAddress = "127.0.0.1"; + settings.outPort = 0; + settings.inPort = 0; + settings.latitude = ""; + settings.longitude = ""; + + settings.attRawEnabled = false; + settings.attRawRate = 20; + + settings.attActualEnabled = true; + settings.attActHW = false; + settings.attActSim = true; + settings.attActCalc = false; + + settings.gpsPositionEnabled = false; + settings.gpsPosRate = 100; + + settings.groundTruthEnabled = false; + settings.groundTruthRate = 100; + + settings.inputCommand = false; + settings.gcsReceiverEnabled = false; + settings.manualControlEnabled= false; + settings.minOutputPeriod = 100; + + settings.airspeedActualEnabled= false; + settings.airspeedActualRate = 100; + + + // if a saved configuration exists load it, and overwrite defaults + if (qSettings != 0) { + + settings.simulatorId = qSettings->value("simulatorId").toString(); + settings.binPath = qSettings->value("binPath").toString(); + settings.dataPath = qSettings->value("dataPath").toString(); + + settings.hostAddress = qSettings->value("hostAddress").toString(); + settings.remoteAddress = qSettings->value("remoteAddress").toString(); + settings.outPort = qSettings->value("outPort").toInt(); + settings.inPort = qSettings->value("inPort").toInt(); + + settings.latitude = qSettings->value("latitude").toString(); + settings.longitude = qSettings->value("longitude").toString(); + settings.startSim = qSettings->value("startSim").toBool(); + settings.addNoise = qSettings->value("noiseCheckBox").toBool(); + + settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); + settings.manualControlEnabled= qSettings->value("manualControlEnabled").toBool(); + + settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); + settings.attRawRate = qSettings->value("attRawRate").toInt(); + + settings.attActualEnabled = qSettings->value("attActualEnabled").toBool(); + settings.attActHW = qSettings->value("attActHW").toBool(); + settings.attActSim = qSettings->value("attActSim").toBool(); + settings.attActCalc = qSettings->value("attActCalc").toBool(); + + settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); + settings.baroAltRate = qSettings->value("baroAltRate").toInt(); + + settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); + + settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); + settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); + + settings.inputCommand = qSettings->value("inputCommand").toBool(); + settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); + + settings.airspeedActualEnabled=qSettings->value("airspeedActualEnabled").toBool(); + settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt(); + } +} + +IUAVGadgetConfiguration *HITLConfiguration::clone() +{ + HITLConfiguration *m = new HITLConfiguration(this->classId()); + + m->settings = settings; + return m; +} + + /** + * Saves a configuration. + * + */ +void HITLConfiguration::saveConfig(QSettings* qSettings) const { + qSettings->setValue("simulatorId", settings.simulatorId); + qSettings->setValue("binPath", settings.binPath); + qSettings->setValue("dataPath", settings.dataPath); + + qSettings->setValue("hostAddress", settings.hostAddress); + qSettings->setValue("remoteAddress", settings.remoteAddress); + qSettings->setValue("outPort", settings.outPort); + qSettings->setValue("inPort", settings.inPort); + + qSettings->setValue("latitude", settings.latitude); + qSettings->setValue("longitude", settings.longitude); + qSettings->setValue("addNoise", settings.addNoise); + qSettings->setValue("startSim", settings.startSim); + + qSettings->setValue("gcsReceiverEnabled", settings.gcsReceiverEnabled); + qSettings->setValue("manualControlEnabled", settings.manualControlEnabled); + + qSettings->setValue("attRawEnabled", settings.attRawEnabled); + qSettings->setValue("attRawRate", settings.attRawRate); + qSettings->setValue("attActualEnabled", settings.attActualEnabled); + qSettings->setValue("attActHW", settings.attActHW); + qSettings->setValue("attActSim", settings.attActSim); + qSettings->setValue("attActCalc", settings.attActCalc); + qSettings->setValue("baroAltitudeEnabled", settings.baroAltitudeEnabled); + qSettings->setValue("baroAltRate", settings.baroAltRate); + qSettings->setValue("gpsPositionEnabled", settings.gpsPositionEnabled); + qSettings->setValue("gpsPosRate", settings.gpsPosRate); + qSettings->setValue("groundTruthEnabled", settings.groundTruthEnabled); + qSettings->setValue("groundTruthRate", settings.groundTruthRate); + qSettings->setValue("inputCommand", settings.inputCommand); + qSettings->setValue("minOutputPeriod", settings.minOutputPeriod); + + qSettings->setValue("airspeedActualEnabled", settings.airspeedActualEnabled); + qSettings->setValue("airspeedActualRate", settings.airspeedActualRate); +} + diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.h b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.h rename to ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlfactory.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlfactory.cpp rename to ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlfactory.h b/ground/openpilotgcs/src/plugins/hitl/hitlfactory.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlfactory.h rename to ground/openpilotgcs/src/plugins/hitl/hitlfactory.h diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp similarity index 75% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.cpp rename to ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp index 4965ee3aa..a05b5ce71 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp @@ -44,24 +44,8 @@ HITLGadget::~HITLGadget() void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config) { HITLConfiguration *m = qobject_cast(config); - // FG -// simulator->setFGPathBin( m->fgPathBin() ); -// simulator->setFGPathData( m->fgPathData() ); -// simulator->setFGManualControl( m->fgManualControl() ); -// // IL2 + // IL2 <-- Is this still necessary? [KDS] emit changeConfiguration(); m_widget->setSettingParameters(m->Settings()); - -// m_widget->setSimulatorId(m->SimulatorId()); -// m_widget->setPathBin(m->PathBin()); -// m_widget->setPathData(m->PathData()); -// m_widget->setHostName(m->HostName()); -// m_widget->setLatitude(m->Latitude()); -// m_widget->setLongitude(m->Longitude()); -// m_widget->setOutPort(m->OutPort()); -// m_widget->setInPort(m->InPort()); -// m_widget->setManualControl(m->ManualControl()); - - } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.h b/ground/openpilotgcs/src/plugins/hitl/hitlgadget.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.h rename to ground/openpilotgcs/src/plugins/hitl/hitlgadget.h diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp new file mode 100644 index 000000000..4efb72852 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp @@ -0,0 +1,78 @@ +/** + ****************************************************************************** + * + * @file hitlnoisegeneration.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup hitlplugin + * @{ + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "hitlnoisegeneration.h" + + +HitlNoiseGeneration::HitlNoiseGeneration() +{ + memset(&noise, 0, sizeof(Noise)); +} + + +HitlNoiseGeneration::~HitlNoiseGeneration() +{ +} + +Noise HitlNoiseGeneration::getNoise(){ + return noise; +} + +Noise HitlNoiseGeneration::generateNoise(){ + + noise.accelData.x=0; + noise.accelData.y=0; + noise.accelData.z=0; + + noise.gpsPosData.Latitude=0; + noise.gpsPosData.Longitude=0; + noise.gpsPosData.Groundspeed=0; + noise.gpsPosData.Heading=0; + noise.gpsPosData.Altitude=0; + + noise.gpsVelData.North=0; + noise.gpsVelData.East=0; + noise.gpsVelData.Down=0; + + noise.baroAltData.Altitude=0; + + noise.attActualData.Roll=0; + noise.attActualData.Pitch=0; + noise.attActualData.Yaw=0; + + noise.gyroData.x=0; + noise.gyroData.y=0; + noise.gyroData.z=0; + + noise.accelData.x=0; + noise.accelData.y=0; + noise.accelData.z=0; + + noise.airspeedActual.CalibratedAirspeed=0; + + return noise; +} diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h new file mode 100644 index 000000000..0a740707e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -0,0 +1,66 @@ +/** + ****************************************************************************** + * + * @file hitlnoisegeneration.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup hitlplugin + * @{ + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef HITLNOISEGENERATION_H +#define HITLNOISEGENERATION_H + +//#include +//#include +#include "xplanesimulator.h" +#include "hitlnoisegeneration.h" +#include "extensionsystem/pluginmanager.h" +#include +#include + +struct Noise{ + Accels::DataFields accelData; + AttitudeActual::DataFields attActualData; + BaroAltitude::DataFields baroAltData; + AirspeedActual::DataFields airspeedActual; + GPSPosition::DataFields gpsPosData; + GPSVelocity::DataFields gpsVelData; + Gyros::DataFields gyroData; + HomeLocation::DataFields homeData; + PositionActual::DataFields positionActualData; + VelocityActual::DataFields velocityActualData; +}; + +class HitlNoiseGeneration +{ +// Q_OBJECT +public: + HitlNoiseGeneration(); + ~HitlNoiseGeneration(); + + Noise getNoise(); + Noise generateNoise(); +private slots: + +private: + Noise noise; +}; +#endif // HITLNOISEGENERATION_H diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp new file mode 100644 index 000000000..9188e66c8 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp @@ -0,0 +1,175 @@ +/** + ****************************************************************************** + * + * @file hitloptionspage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop plugin + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "hitloptionspage.h" +#include "hitlconfiguration.h" +#include "ui_hitloptionspage.h" +#include + +#include +#include +#include +#include + + + +HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : + IOptionsPage(parent), + config(conf) +{ +} + +QWidget *HITLOptionsPage::createPage(QWidget *parent) +{ + Q_UNUSED(parent); + + // Create page + m_optionsPage = new Ui::HITLOptionsPage(); + QWidget* optionsPageWidget = new QWidget; + m_optionsPage->setupUi(optionsPageWidget); + int index = 0; + foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) + { + m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(),creator->ClassId()); + } + + //QString classId = widget->listSimulators->itemData(0).toString(); + //SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId); + + //QWidget* embedPage = creator->createOptionsPage(); + //m_optionsPage->verticalLayout->addWidget(embedPage); + + m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); + m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); + m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); + m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); + + // Restore the contents from the settings: + foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) + { + QString id = config->Settings().simulatorId; + if(creator->ClassId() == id) + m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); + } + + m_optionsPage->executablePath->setPath(config->Settings().binPath); + m_optionsPage->dataPath->setPath(config->Settings().dataPath); + + m_optionsPage->manualControlRadioButton->setChecked(config->Settings().manualControlEnabled); + m_optionsPage->gcsReceiverRadioButton->setChecked(config->Settings().gcsReceiverEnabled); + + m_optionsPage->startSim->setChecked(config->Settings().startSim); + m_optionsPage->noiseCheckBox->setChecked(config->Settings().addNoise); + + m_optionsPage->hostAddress->setText(config->Settings().hostAddress); + m_optionsPage->remoteAddress->setText(config->Settings().remoteAddress); + m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); + m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); + m_optionsPage->latitude->setText(config->Settings().latitude); + m_optionsPage->longitude->setText(config->Settings().longitude); + + m_optionsPage->groundTruthCheckbox->setChecked(config->Settings().groundTruthEnabled); + m_optionsPage->gpsPositionCheckbox->setChecked(config->Settings().gpsPositionEnabled); + m_optionsPage->attActualCheckbox->setChecked(config->Settings().attActualEnabled); + m_optionsPage->attRawCheckbox->setChecked(config->Settings().attRawEnabled); + + + + m_optionsPage->attRawRateSpinbox->setValue(config->Settings().attRawRate); + m_optionsPage->gpsPosRateSpinbox->setValue(config->Settings().gpsPosRate); + m_optionsPage->groundTruthRateSpinbox->setValue(config->Settings().groundTruthRate); +// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate); + + m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroAltitudeEnabled); + m_optionsPage->baroAltRateSpinbox->setValue(config->Settings().baroAltRate); + + m_optionsPage->minOutputPeriodSpinbox->setValue(config->Settings().minOutputPeriod); + + m_optionsPage->attActHW->setChecked(config->Settings().attActHW); + m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc); + m_optionsPage->attActSim->setChecked(config->Settings().attActSim); + + m_optionsPage->airspeedActualCheckbox->setChecked(config->Settings().airspeedActualEnabled); + m_optionsPage->airspeedRateSpinbox->setValue(config->Settings().airspeedActualRate); + + return optionsPageWidget; +} + +void HITLOptionsPage::apply() +{ + SimulatorSettings settings; + int i = m_optionsPage->chooseFlightSimulator->currentIndex(); + + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + settings.startSim = m_optionsPage->startSim->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); + + settings.inPort = m_optionsPage->inputPort->text().toInt(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.longitude = m_optionsPage->longitude->text(); + settings.latitude = m_optionsPage->latitude->text(); + + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + + settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); + settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); + + settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked(); + + settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); + + settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); + settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); + + settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); + settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); + + settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); + + settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); + settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); + + settings.attActHW = m_optionsPage->attActHW->isChecked(); + settings.attActSim = m_optionsPage->attActSim->isChecked(); + settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + + settings.airspeedActualEnabled=m_optionsPage->airspeedActualCheckbox->isChecked(); + settings.airspeedActualRate=m_optionsPage->airspeedRateSpinbox->value(); + + //Write settings to file + config->setSimulatorSettings(settings); +} + +void HITLOptionsPage::finish() +{ + delete m_optionsPage; +} diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.h b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.h rename to ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui new file mode 100644 index 000000000..30eb578c5 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui @@ -0,0 +1,1108 @@ + + + HITLOptionsPage + + + + 0 + 0 + 885 + 741 + + + + + 0 + 0 + + + + + 0 + 0 + + + + Form + + + + 3 + + + 0 + + + 0 + + + 0 + + + 3 + + + + + + + Choose flight simulator: + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 100 + + + + + 350 + 16777215 + + + + IP addresses + + + + + 10 + 30 + 323 + 24 + + + + + + + Local host: + + + + + + + + 0 + 0 + + + + + 125 + 16777215 + + + + For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Port: + + + + + + + + 0 + 0 + + + + + 55 + 16777215 + + + + IP port for receiving data from sim + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + 10 + 60 + 322 + 24 + + + + + + + Remote host: + + + + + + + + 0 + 0 + + + + + 125 + 16777215 + + + + Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Port: + + + + + + + + 0 + 0 + + + + + 55 + 16777215 + + + + IP port for sending data to sim + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + + 0 + 180 + + + + Program parameters + + + + + 120 + 60 + 401 + 16 + + + + + 0 + 0 + + + + + + + 10 + 60 + 94 + 16 + + + + + 0 + 0 + + + + + 110 + 16777215 + + + + Data directory: + + + + + + 120 + 34 + 401 + 16 + + + + + 0 + 0 + + + + + + + 10 + 34 + 104 + 16 + + + + + 0 + 0 + + + + + 110 + 16777215 + + + + Path executable: + + + + + + 10 + 120 + 229 + 20 + + + + Check this box to start the simulator on the local computer + + + Start simulator on local machine + + + + + + 10 + 150 + 89 + 20 + + + + Add noise to sensor simulation + + + Add noise + + + + + + 10 + 80 + 491 + 16 + + + + Qt::Horizontal + + + + + + 10 + 90 + 161 + 22 + + + + Initial latitude (decimal): + + + + + + 290 + 90 + 171 + 22 + + + + Initial longitude (decimal): + + + + + + 460 + 90 + 100 + 22 + + + + + 0 + 0 + + + + + 100 + 16777215 + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + 170 + 90 + 100 + 22 + + + + + 0 + 0 + + + + + 100 + 16777215 + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + QFormLayout::AllNonFixedFieldsGrow + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + + + + + + + 16777215 + 150 + + + + Attitude data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + true + + + AttitudeRaw (gyro, accels) + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate: + + + + + + + true + + + 0 - update once, or every N seconds + + + ms + + + 10 + + + 100 + + + 20 + + + + + + + + + + true + + + AttitudeActual + + + true + + + true + + + true + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + + 75 + true + + + + + + + use values from simulator + + + true + + + + + + + send simulated inertial data to board + + + + + + + + + + calculate from simulated sensor data + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + 0 + 150 + + + + Spatial data + + + + + + Ground truth position and velocity + + + true + + + true + + + true + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + Refresh rate: + + + + + + + true + + + 0 - update once, or every N seconds + + + ms + + + 1 + + + 5000 + + + 100 + + + 100 + + + + + + + + + + GPS data + + + true + + + true + + + false + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + true + + + Refresh rate: + + + + + + + true + + + 0 - update once, or every N seconds + + + ms + + + 1 + + + 5000 + + + 100 + + + 100 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + + 0 + 275 + + + + Other data + + + + 3 + + + 3 + + + 3 + + + 3 + + + 0 + + + + + + 0 + 0 + + + + AirspeedActual + + + true + + + true + + + + + + Refresh rate: + + + + + + + 0 - update once, or every N seconds + + + ms + + + 5000 + + + 100 + + + + + + + + + + false + + + BaroAltitude + + + true + + + true + + + false + + + + 3 + + + 6 + + + 3 + + + 0 + + + 0 + + + + + + + Range detection: + + + + + + + m + + + 1 + + + 10 + + + 5 + + + + + + + Refresh rate: + + + + + + + 0 - update once, or every N seconds + + + ms + + + 20 + + + 2000 + + + 10 + + + 250 + + + + + + + + + + + + true + + + Map transmitter commands... + + + true + + + true + + + true + + + + 3 + + + 3 + + + 0 + + + 0 + + + + + true + + + + 75 + true + + + + from hardware to simulator (via ManualCtrl) + + + true + + + + + + + from simulator to hardware (via GCSReceiver) + + + false + + + + + + + + + + 6 + + + + + Maximum GCS to hardware output rate: + + + + + + + true + + + Set the maximum rate at which GCS sends simulator data to the hardware + + + ms + + + 10 + + + 500 + + + 5 + + + 100 + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + + Utils::PathChooser + QWidget +
utils/pathchooser.h
+ 1 + + + + chooseFlightSimulator + hostAddress + inputPort + remoteAddress + outputPort + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp similarity index 91% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.cpp rename to ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp index dbfb27ba5..9ecca808e 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp @@ -29,6 +29,7 @@ #include #include #include +#include "aerosimrcsimulator.h" #include "fgsimulator.h" #include "il2simulator.h" #include "xplanesimulator.h" @@ -53,6 +54,7 @@ bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) addAutoReleasedObject(mf); + addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); addSimulator(new FGSimulatorCreator("FG","FlightGear")); addSimulator(new IL2SimulatorCreator("IL2","IL2")); addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane")); diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.h b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h similarity index 95% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.h rename to ground/openpilotgcs/src/plugins/hitl/hitlplugin.h index 6caecedca..658111e79 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.h @@ -29,7 +29,7 @@ #define HITLPLUGIN_H #include -#include +#include #include diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlresources.qrc b/ground/openpilotgcs/src/plugins/hitl/hitlresources.qrc similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlresources.qrc rename to ground/openpilotgcs/src/plugins/hitl/hitlresources.qrc diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp similarity index 74% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.cpp rename to ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp index f965b5ad3..beef24f70 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp @@ -2,7 +2,7 @@ ****************************************************************************** * * @file hitlwidget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup HITLPlugin HITL Plugin @@ -51,12 +51,13 @@ HITLWidget::HITLWidget(QWidget *parent) widget->startButton->setEnabled(true); widget->stopButton->setEnabled(false); - greenColor = "rgb(35, 221, 35)"; + greenColor = "rgb(35, 221, 35)"; //Change the green color in order to make it a bit more vibrant + strStyleEnable = QString("QFrame{background-color: %1; color: white}").arg(greenColor); + strStyleDisable = "QFrame{background-color: red; color: white}"; - strAutopilotDisconnected = " Autopilot OFF "; - strSimulatorDisconnected = " Simulator OFF "; - strAutopilotConnected = " Autopilot ON "; - strSimulatorConnected = " Simulator ON "; + strAutopilotDisconnected = " Autopilot OFF "; + strSimulatorDisconnected = " Simulator OFF "; + strAutopilotConnected = " Autopilot ON "; widget->apLabel->setText(strAutopilotDisconnected); widget->simLabel->setText(strSimulatorDisconnected); @@ -64,7 +65,6 @@ HITLWidget::HITLWidget(QWidget *parent) connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); - } HITLWidget::~HITLWidget() @@ -77,15 +77,7 @@ void HITLWidget::startButtonClicked() QThread* mainThread = QThread::currentThread(); qDebug() << "Main Thread: "<< mainThread; - // [1] -// if(Simulator::IsStarted()) -// { -// widget->textBrowser->append("HITL alreary started!"); -// return; -// } -// Simulator::setStarted(true); - - // [2] allow only one instance per simulator + //Allow only one instance per simulator if(Simulator::Instances().indexOf(settings.simulatorId) != -1) { widget->textBrowser->append(settings.simulatorId + " alreary started!"); @@ -104,25 +96,27 @@ void HITLWidget::startButtonClicked() QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection); simulator = NULL; } - //fgBridge = new FlightGearBridge(); - //simulator = new Simulator(); - if(settings.hostAddress == "" || settings.inPort == 0) + + if(settings.hostAddress == "" || settings.inPort == 0) { widget->textBrowser->append("Before start, set UDP parameters in options page!"); return; } SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); - simulator = creator->createSimulator(settings);//hostName, outPort, inPort, manualControl, pathBin, pathData); - // move to thread + simulator = creator->createSimulator(settings); + + // move to thread <--[BCH] simulator->setName(creator->Description()); simulator->setSimulatorId(creator->ClassId()); + + connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString))); + // Setup process widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description())); qxtLog->info("HITL: Starting " + creator->Description()); // Start bridge - //bool ret = simulator->setupProcess(); bool ret = QMetaObject::invokeMethod(simulator, "setupProcess",Qt::QueuedConnection); if(ret) { @@ -134,7 +128,6 @@ void HITLWidget::startButtonClicked() widget->stopButton->setEnabled(true); qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections"); - connect(simulator, SIGNAL(processOutput(QString)), widget->textBrowser, SLOT(append(QString))); connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()),Qt::QueuedConnection); connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()),Qt::QueuedConnection); connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()),Qt::QueuedConnection); @@ -169,9 +162,9 @@ void HITLWidget::stopButtonClicked() widget->startButton->setEnabled(true); widget->stopButton->setEnabled(false); - widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: transparent; color: white}")); - widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: transparent; color: white}")); - widget->apLabel->setText(strAutopilotDisconnected); + widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); + widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); + widget->apLabel->setText(strAutopilotDisconnected); widget->simLabel->setText(strSimulatorDisconnected); if(simulator) { @@ -185,31 +178,35 @@ void HITLWidget::buttonClearLogClicked() widget->textBrowser->clear(); } +void HITLWidget::onProcessOutput(QString text) +{ + widget->textBrowser->append(text); +} void HITLWidget::onAutopilotConnect() { - widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: %1; color: white}").arg(greenColor)); - widget->apLabel->setText(strAutopilotConnected); + widget->apLabel->setStyleSheet(strStyleEnable); + widget->apLabel->setText(strAutopilotConnected); qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation"); } void HITLWidget::onAutopilotDisconnect() { - widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: red; color: white}")); + widget->apLabel->setStyleSheet(strStyleDisable); widget->apLabel->setText(strAutopilotDisconnected); qxtLog->info(strAutopilotDisconnected); } void HITLWidget::onSimulatorConnect() { - widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: %1; color: white}").arg(greenColor)); - widget->simLabel->setText(" " + simulator->Name() +" connected "); + widget->simLabel->setStyleSheet(strStyleEnable); + widget->simLabel->setText(" " + simulator->Name() +" connected "); qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name())); } void HITLWidget::onSimulatorDisconnect() { - widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: red; color: white}")); - widget->simLabel->setText(" " + simulator->Name() +" disconnected "); + widget->simLabel->setStyleSheet(strStyleDisable); + widget->simLabel->setText(" " + simulator->Name() +" disconnected "); qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name())); } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.h b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h similarity index 92% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.h rename to ground/openpilotgcs/src/plugins/hitl/hitlwidget.h index b5865a842..f63352f9e 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.h @@ -50,6 +50,7 @@ private slots: void startButtonClicked(); void stopButtonClicked(); void buttonClearLogClicked(); + void onProcessOutput(QString text); void onAutopilotConnect(); void onAutopilotDisconnect(); void onSimulatorConnect(); @@ -64,8 +65,8 @@ private: QString strAutopilotDisconnected; QString strSimulatorDisconnected; QString strAutopilotConnected; - QString strSimulatorConnected; - + QString strStyleEnable; + QString strStyleDisable; }; #endif /* HITLWIDGET_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.ui b/ground/openpilotgcs/src/plugins/hitl/hitlwidget.ui similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.ui rename to ground/openpilotgcs/src/plugins/hitl/hitlwidget.ui diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp new file mode 100644 index 000000000..6bec9fbf3 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -0,0 +1,324 @@ +/** + ****************************************************************************** + * + * @file il2simulator.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup hitlplugin + * @{ + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* + * Description of DeviceLink Protocol: + * A Request is initiated with R/ followed by id's of to be requested settings + * even id's indicate read only values, odd are write only + * (usually id =get value id+1= set - for same setting) + * id's are separated by / + * requests can contain values to set, or to select a subsystem + * values are separated by \ + * example: R/30/48/64\0/64\1/ + * request read only settings 30,48 and 64 with parameters 0 and 1 + * the answer consists of an A followed by id value pairs in the same format + * example: A/30\0/48\0/64\0\22/64\1\102/ + * + * A full protocol description as well as a list of ID's and their meanings + * can be found shipped with IL2 in the file DeviceLink.txt + * + * id's used in this file: + * 30: IAS in km/h (float) + * 32: vario in m/s (float) + * 38: angular speed °/s (float) (which direction? azimuth?) + * 40: barometric alt in m (float) + * 42: flight course in ° (0-360) (float) + * 46: roll angle in ° (-180 - 180) (floatniguration) + * 48: pitch angle in ° (-90 - 90) (float) + * 80/81: engine power (-1.0 (0%) - 1.0 (100%)) (float) + * 84/85: aileron servo (-1.0 - 1.0) (float) + * 86/87: elevator servo (-1.0 - 1.0) (float) + * 88/89: rudder servo (-1.0 - 1.0) (float) + * + * IL2 currently offers no useful way of providing GPS data + * therefore fake GPS data will be calculated using IMS + * + * unfortunately angular acceleration provided is very limited, too + */ + + +#include "il2simulator.h" +#include "extensionsystem/pluginmanager.h" +#include +#include +#include +#include + +const float IL2Simulator::FT2M = 0.3048; +const float IL2Simulator::KT2MPS = 0.514444444; +const float IL2Simulator::MPS2KMH = 3.6; +const float IL2Simulator::KMH2MPS = (1.0/3.6); +const float IL2Simulator::INHG2KPA = 3.386; +const float IL2Simulator::RAD2DEG = (180.0/M_PI); +const float IL2Simulator::DEG2RAD = (M_PI/180.0); +const float IL2Simulator::M2DEG = 60.*1852.; // 60 miles per degree times 1852 meters per mile +const float IL2Simulator::DEG2M = (1.0/(60.*1852.)); +const float IL2Simulator::AIR_CONST = 287.058; // J/(kg*K) +const float IL2Simulator::GROUNDDENSITY = 1.225; // kg/m³ ;) +const float IL2Simulator::TEMP_GROUND = (15.0 + 273.0); // 15°C in Kelvin +const float IL2Simulator::TEMP_LAPSE_RATE = -0.0065; //degrees per meter +const float IL2Simulator::AIR_CONST_FACTOR = -0.0341631947363104; //several nature constants calculated into one + +IL2Simulator::IL2Simulator(const SimulatorSettings& params) : + Simulator(params) +{ + +} + + +IL2Simulator::~IL2Simulator() +{ +} + +void IL2Simulator::setupUdpPorts(const QString& host, int inPort, int outPort) +{ + Q_UNUSED(outPort); + + inSocket->connectToHost(host,inPort); // IL2 + if(!inSocket->waitForConnected()) + qxtLog->error(Name() + " cann't connect to UDP Port: " + QString::number(inPort)); +} + +void IL2Simulator::transmitUpdate() +{ + // Read ActuatorDesired from autopilot + ActuatorDesired::DataFields actData = actDesired->getData(); + float ailerons = actData.Roll; + float elevator = actData.Pitch; + float rudder = actData.Yaw; + float throttle = actData.Throttle*2-1.0; + + // Send update to Il2 + QString cmd; + cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") + .arg(throttle) + .arg(ailerons) + .arg(elevator) + .arg(rudder); + QByteArray data = cmd.toAscii(); + //outSocket->write(data); + inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! +} + + +/** + * calculate air density from altitude + */ +float IL2Simulator::DENSITY(float alt) { + return (GROUNDDENSITY * pow( + ((TEMP_GROUND+(TEMP_LAPSE_RATE*alt))/TEMP_GROUND), + ((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) ) + ); +} + +/** + * calculate air pressure from altitude + */ +float IL2Simulator::PRESSURE(float alt) { + return DENSITY(alt)*(TEMP_GROUND+(alt*TEMP_LAPSE_RATE))*AIR_CONST; + +} + +/** + * calculate TAS from IAS and altitude + */ +float IL2Simulator::TAS(float IAS, float alt) { + return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt))); +} + +/** + * process data string from flight simulator + */ +void IL2Simulator::processUpdate(const QByteArray& inp) +{ + // save old flight data to calculate delta's later + old=current; + QString data(inp); + // Split + QStringList fields = data.split("/"); + + // split up response string + int t; + for (t=0; t=2) { + int id = values[0].toInt(); + float value = values[1].toFloat(); + switch (id) { + case 30: + current.ias=value * KMH2MPS; + break; + case 32: + current.dZ=value; + break; + case 40: + current.Z=value; + break; + case 42: + current.azimuth=value; + break; + case 46: + current.roll=-value; + break; + case 48: + current.pitch=value; + break; + } + } + } + + // measure time + current.dT = ((float)time->restart()) / 1000.0; + if (current.dT<0.001) current.dT=0.001; + current.T = old.T+current.dT; + current.i = old.i+1; + if (current.i==1) { + old.dRoll=0; + old.dPitch=0; + old.dAzimuth=0; + old.ddX=0; + old.ddX=0; + old.ddX=0; + } + + // calculate TAS from alt and IAS + current.tas = TAS(current.ias,current.Z); + + // assume the plane actually flies straight and no wind + // groundspeed is horizontal vector of TAS + current.groundspeed = current.tas*cos(current.pitch*DEG2RAD); + // x and y vector components + current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD); + current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD); + + // simple IMS - integration over time the easy way... + current.X = old.X + (current.dX*current.dT); + current.Y = old.Y + (current.dY*current.dT); + + // accelerations (filtered) + if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0; + if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0; + if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0; +#define SPEED_FILTER 10 + current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1); + current.ddY = ((current.dY-old.dY)/current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1); + current.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1); + +#define TURN_FILTER 10 + // turn speeds (filtered) + if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0; + if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0; + if (isnan(old.dRoll) || isinf(old.dRoll)) old.dRoll=0; + current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1); + current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1); + current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1); + + /////// + // Output formatting + /////// + Output2Hardware out; + memset(&out, 0, sizeof(Output2Hardware)); + + // Compute rotation matrix, for later calculations + float Rbe[3][3]; + float rpy[3]; + float quat[4]; + rpy[0]=current.roll; + rpy[1]=current.pitch; + rpy[2]=current.azimuth; + Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); + Utils::CoordinateConversions().Quaternion2R(quat,Rbe); + + // Update GPS Position objects + double HomeLLA[3]; + double LLA[3]; + double NED[3]; + HomeLLA[0]=settings.latitude.toFloat(); + HomeLLA[1]=settings.longitude.toFloat(); + HomeLLA[2]=0; + NED[0] = current.Y; + NED[1] = current.X; + NED[2] = -current.Z; + Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA,NED,LLA); + out.latitude = LLA[0] * 1e7; + out.longitude = LLA[1] * 1e7; + out.groundspeed = current.groundspeed; + + out.calibratedAirspeed = current.ias; + + out.dstN=current.Y; + out.dstE=current.X; + out.dstD=-current.Z; + + // Update BaroAltitude object + out.altitude = current.Z; + out.temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0; + out.pressure = PRESSURE(current.Z)/1000.0; // kpa + + + // Update attActual object + out.roll = current.roll; //roll; + out.pitch = current.pitch; // pitch + out.heading = current.azimuth; // yaw + + + // Update VelocityActual.{North,East,Down} + out.velNorth = current.dY; + out.velEast = current.dX; + out.velDown = -current.dZ; + + // rotate turn rates and accelerations into body frame + // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!) + out.rollRate = current.dRoll; + out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth; + out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; + + //Update accelerometer sensor data + out.accX = current.ddX*Rbe[0][0] + +current.ddY*Rbe[0][1] + +(current.ddZ+GEE)*Rbe[0][2]; + out.accY = current.ddX*Rbe[1][0] + +current.ddY*Rbe[1][1] + +(current.ddZ+GEE)*Rbe[1][2]; + out.accZ = - (current.ddX*Rbe[2][0] + +current.ddY*Rbe[2][1] + +(current.ddZ+GEE)*Rbe[2][2]); + + updateUAVOs(out); +} + +/** + * process data string from flight simulator + */ +float IL2Simulator::angleDifference(float a, float b) +{ + float d=a-b; + if (d>180) d-=360; + if (d<-180)d+=360; + return d; +} diff --git a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.h b/ground/openpilotgcs/src/plugins/hitl/il2simulator.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/il2simulator.h rename to ground/openpilotgcs/src/plugins/hitl/il2simulator.h diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/arrow-down.png b/ground/openpilotgcs/src/plugins/hitl/images/arrow-down.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/arrow-down.png rename to ground/openpilotgcs/src/plugins/hitl/images/arrow-down.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/arrow-down2.png b/ground/openpilotgcs/src/plugins/hitl/images/arrow-down2.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/arrow-down2.png rename to ground/openpilotgcs/src/plugins/hitl/images/arrow-down2.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/arrow-right.png b/ground/openpilotgcs/src/plugins/hitl/images/arrow-right.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/arrow-right.png rename to ground/openpilotgcs/src/plugins/hitl/images/arrow-right.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/arrow-up.png b/ground/openpilotgcs/src/plugins/hitl/images/arrow-up.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/arrow-up.png rename to ground/openpilotgcs/src/plugins/hitl/images/arrow-up.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/arrow-up2.png b/ground/openpilotgcs/src/plugins/hitl/images/arrow-up2.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/arrow-up2.png rename to ground/openpilotgcs/src/plugins/hitl/images/arrow-up2.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/bullet_arrow_down.png b/ground/openpilotgcs/src/plugins/hitl/images/bullet_arrow_down.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/bullet_arrow_down.png rename to ground/openpilotgcs/src/plugins/hitl/images/bullet_arrow_down.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/bullet_arrow_up.png b/ground/openpilotgcs/src/plugins/hitl/images/bullet_arrow_up.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/bullet_arrow_up.png rename to ground/openpilotgcs/src/plugins/hitl/images/bullet_arrow_up.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/list_bullet_arrow.png b/ground/openpilotgcs/src/plugins/hitl/images/list_bullet_arrow.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/list_bullet_arrow.png rename to ground/openpilotgcs/src/plugins/hitl/images/list_bullet_arrow.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/scrollbarvertical_down_arrow.png b/ground/openpilotgcs/src/plugins/hitl/images/scrollbarvertical_down_arrow.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/scrollbarvertical_down_arrow.png rename to ground/openpilotgcs/src/plugins/hitl/images/scrollbarvertical_down_arrow.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/images/scrollbarvertical_up_arrow.png b/ground/openpilotgcs/src/plugins/hitl/images/scrollbarvertical_up_arrow.png similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/images/scrollbarvertical_up_arrow.png rename to ground/openpilotgcs/src/plugins/hitl/images/scrollbarvertical_up_arrow.png diff --git a/ground/openpilotgcs/src/plugins/hitlnew/isimulator.h b/ground/openpilotgcs/src/plugins/hitl/isimulator.h similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/isimulator.h rename to ground/openpilotgcs/src/plugins/hitl/isimulator.h diff --git a/ground/openpilotgcs/src/plugins/hitlnew/opfgprotocol.xml b/ground/openpilotgcs/src/plugins/hitl/opfgprotocol.xml similarity index 100% rename from ground/openpilotgcs/src/plugins/hitlnew/opfgprotocol.xml rename to ground/openpilotgcs/src/plugins/hitl/opfgprotocol.xml diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pro b/ground/openpilotgcs/src/plugins/hitl/plugin.pro similarity index 75% rename from ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pro rename to ground/openpilotgcs/src/plugins/hitl/plugin.pro index 70e46e939..066c60ec0 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pro +++ b/ground/openpilotgcs/src/plugins/hitl/plugin.pro @@ -1,29 +1,37 @@ -TEMPLATE = lib -TARGET = HITLNEW -QT += network -include(../../openpilotgcsplugin.pri) -include(hitlnew_dependencies.pri) -HEADERS += hitlplugin.h \ - hitlwidget.h \ - hitloptionspage.h \ - hitlfactory.h \ - hitlconfiguration.h \ - hitlgadget.h \ - simulator.h \ - fgsimulator.h \ - il2simulator.h \ - xplanesimulator.h -SOURCES += hitlplugin.cpp \ - hitlwidget.cpp \ - hitloptionspage.cpp \ - hitlfactory.cpp \ - hitlconfiguration.cpp \ - hitlgadget.cpp \ - simulator.cpp \ - il2simulator.cpp \ - fgsimulator.cpp \ - xplanesimulator.cpp -OTHER_FILES += hitlnew.pluginspec -FORMS += hitloptionspage.ui \ - hitlwidget.ui -RESOURCES += hitlresources.qrc +TEMPLATE = lib +TARGET = HITL +QT += network + +include(../../openpilotgcsplugin.pri) +include(hitl_dependencies.pri) + +HEADERS += hitlplugin.h \ + hitlwidget.h \ + hitloptionspage.h \ + hitlfactory.h \ + hitlconfiguration.h \ + hitlgadget.h \ + hitlnoisegeneration.h \ + simulator.h \ + aerosimrcsimulator.h \ + fgsimulator.h \ + il2simulator.h \ + xplanesimulator.h +SOURCES += hitlplugin.cpp \ + hitlwidget.cpp \ + hitloptionspage.cpp \ + hitlfactory.cpp \ + hitlconfiguration.cpp \ + hitlgadget.cpp \ + hitlnoisegeneration.cpp \ + simulator.cpp \ + aerosimrcsimulator.cpp \ + fgsimulator.cpp \ + il2simulator.cpp \ + xplanesimulator.cpp +OTHER_FILES += hitl.pluginspec +FORMS += hitloptionspage.ui \ + hitlwidget.ui +RESOURCES += hitlresources.qrc + + diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp new file mode 100644 index 000000000..d414049a1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -0,0 +1,715 @@ +/** + ****************************************************************************** + * + * @file simulator.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop plugin + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#include "simulator.h" +#include "qxtlogger.h" +#include "extensionsystem/pluginmanager.h" +#include "coreplugin/icore.h" +#include "coreplugin/threadmanager.h" +#include "hitlnoisegeneration.h" + +volatile bool Simulator::isStarted = false; + +const float Simulator::GEE = 9.81; +const float Simulator::FT2M = 0.3048; +const float Simulator::KT2MPS = 0.514444444; +const float Simulator::INHG2KPA = 3.386; +const float Simulator::FPS2CMPS = 30.48; +const float Simulator::DEG2RAD = (M_PI/180.0); +const float Simulator::RAD2DEG = (180.0/M_PI); + + +Simulator::Simulator(const SimulatorSettings& params) : + simProcess(NULL), + time(NULL), + inSocket(NULL), + outSocket(NULL), + settings(params), + updatePeriod(50), + simTimeout(8000), + autopilotConnectionStatus(false), + simConnectionStatus(false), + txTimer(NULL), + simTimer(NULL), + name("") +{ + // move to thread + moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); + connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection); + emit myStart(); + + QTime currentTime=QTime::currentTime(); + gpsPosTime = currentTime; + groundTruthTime = currentTime; + gcsRcvrTime = currentTime; + attRawTime = currentTime; + baroAltTime = currentTime; + airspeedActualTime=currentTime; + +} + +Simulator::~Simulator() +{ + if(inSocket) + { + delete inSocket; + inSocket = NULL; + } + + if(outSocket) + { + delete outSocket; + outSocket = NULL; + } + + if(txTimer) + { + delete txTimer; + txTimer = NULL; + } + + if(simTimer) + { + delete simTimer; + simTimer = NULL; + } + // NOTE: Does not currently work, may need to send control+c to through the terminal + if (simProcess != NULL) + { + //connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus))); + + simProcess->disconnect(); + if(simProcess->state() == QProcess::Running) + simProcess->kill(); + //if(simProcess->waitForFinished()) + //emit deleteSimProcess(); + delete simProcess; + simProcess = NULL; + } +} + +void Simulator::onDeleteSimulator(void) +{ + // [1] + Simulator::setStarted(false); + // [2] + Simulator::Instances().removeOne(simulatorId); + + disconnect(this); + delete this; +} + +void Simulator::onStart() +{ + QMutexLocker locker(&lock); + + QThread* mainThread = QThread::currentThread(); + + qDebug() << "Simulator Thread: "<< mainThread; + + // Get required UAVObjects + ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager* objManager = pm->getObject(); + actDesired = ActuatorDesired::GetInstance(objManager); + actCommand = ActuatorCommand::GetInstance(objManager); + manCtrlCommand = ManualControlCommand::GetInstance(objManager); + gcsReceiver= GCSReceiver::GetInstance(objManager); + flightStatus = FlightStatus::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + velActual = VelocityActual::GetInstance(objManager); + posActual = PositionActual::GetInstance(objManager); + baroAlt = BaroAltitude::GetInstance(objManager); + airspeedActual = AirspeedActual::GetInstance(objManager); + attActual = AttitudeActual::GetInstance(objManager); + attSettings = AttitudeSettings::GetInstance(objManager); + accels = Accels::GetInstance(objManager); + gyros = Gyros::GetInstance(objManager); + gpsPos = GPSPosition::GetInstance(objManager); + gpsVel = GPSVelocity::GetInstance(objManager); + telStats = GCSTelemetryStats::GetInstance(objManager); + + // Listen to autopilot connection events + TelemetryManager* telMngr = pm->getObject(); + connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + //connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); + + // If already connect setup autopilot + GCSTelemetryStats::DataFields stats = telStats->getData(); + if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) + onAutopilotConnect(); + + inSocket = new QUdpSocket(); + outSocket = new QUdpSocket(); + setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); + + emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ + "inputPort: " + QString::number(settings.inPort) + "\n" + \ + "outputPort: " + QString::number(settings.outPort) + "\n"); + + qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ + "Remote interface: " + settings.remoteAddress + "\n" + \ + "inputPort: " + QString::number(settings.inPort) + "\n" + \ + "outputPort: " + QString::number(settings.outPort) + "\n"); + +// if(!inSocket->waitForConnected(5000)) +// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort)); +// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG +// if(!outSocket->waitForConnected(5000)) +// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort)); + + + connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()),Qt::DirectConnection); + + // Setup transmit timer + txTimer = new QTimer(); + connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()),Qt::DirectConnection); + txTimer->setInterval(updatePeriod); + txTimer->start(); + // Setup simulator connection timer + simTimer = new QTimer(); + connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()),Qt::DirectConnection); + simTimer->setInterval(simTimeout); + simTimer->start(); + + // setup time + time = new QTime(); + time->start(); + current.T=0; + current.i=0; + +} + +void Simulator::receiveUpdate() +{ + // Update connection timer and status + simTimer->setInterval(simTimeout); + simTimer->stop(); + simTimer->start(); + if ( !simConnectionStatus ) + { + simConnectionStatus = true; + emit simulatorConnected(); + } + + // Process data + while(inSocket->hasPendingDatagrams()) { + // Receive datagram + QByteArray datagram; + datagram.resize(inSocket->pendingDatagramSize()); + QHostAddress sender; + quint16 senderPort; + inSocket->readDatagram(datagram.data(), datagram.size(), + &sender, &senderPort); + //QString datastr(datagram); + // Process incomming data + processUpdate(datagram); + } +} + +void Simulator::setupObjects() +{ + + if (settings.gcsReceiverEnabled) { + setupInputObject(actCommand, settings.minOutputPeriod); //Input to the simulator + setupOutputObject(gcsReceiver, settings.minOutputPeriod); + } else if (settings.manualControlEnabled) { + setupInputObject(actDesired, settings.minOutputPeriod); //Input to the simulator + } + + setupOutputObject(posHome, 10000); //Hardcoded? Bleh. + + if (settings.gpsPositionEnabled) + setupOutputObject(gpsPos, settings.gpsPosRate); + + if (settings.groundTruthEnabled){ + setupOutputObject(posActual, settings.groundTruthRate); + setupOutputObject(velActual, settings.groundTruthRate); + } + + if (settings.attRawEnabled) { + setupOutputObject(accels, settings.attRawRate); + setupOutputObject(gyros, settings.attRawRate); + } + + if (settings.attActualEnabled && settings.attActHW) { + setupOutputObject(accels, settings.attRawRate); + setupOutputObject(gyros, settings.attRawRate); + } + + if (settings.attActualEnabled && !settings.attActHW) + setupOutputObject(attActual, 20); //Hardcoded? Bleh. + else + setupWatchedObject(attActual, 100); //Hardcoded? Bleh. + + if(settings.airspeedActualEnabled) + setupOutputObject(airspeedActual, settings.airspeedActualRate); + + if(settings.baroAltitudeEnabled) + setupOutputObject(baroAlt, settings.baroAltRate); + +} + + +void Simulator::setupInputObject(UAVObject* obj, quint32 updatePeriod) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); + UAVObject::SetGcsTelemetryAcked(mdata, false); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); + mdata.gcsTelemetryUpdatePeriod = 0; + + UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); + UAVObject::SetFlightTelemetryAcked(mdata, false); + + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = updatePeriod; + + obj->setMetadata(mdata); +} + + +void Simulator::setupWatchedObject(UAVObject *obj, quint32 updatePeriod) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); + UAVObject::SetGcsTelemetryAcked(mdata, false); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); + mdata.gcsTelemetryUpdatePeriod = 0; + + UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); + UAVObject::SetFlightTelemetryAcked(mdata, false); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = updatePeriod; + + obj->setMetadata(mdata); +} + + +void Simulator::setupOutputObject(UAVObject* obj, quint32 updatePeriod) +{ + UAVObject::Metadata mdata; + mdata = obj->getDefaultMetadata(); + + UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); + UAVObject::SetGcsTelemetryAcked(mdata, false); + UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.gcsTelemetryUpdatePeriod = updatePeriod; + + UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); + UAVObject::SetFlightTelemetryUpdateMode(mdata,UAVObject::UPDATEMODE_MANUAL); + + obj->setMetadata(mdata); +} + +void Simulator::onAutopilotConnect() +{ + autopilotConnectionStatus = true; + setupObjects(); + emit autopilotConnected(); +} + +void Simulator::onAutopilotDisconnect() +{ + autopilotConnectionStatus = false; + emit autopilotDisconnected(); +} + +void Simulator::onSimulatorConnectionTimeout() +{ + if ( simConnectionStatus ) + { + simConnectionStatus = false; + emit simulatorDisconnected(); + } +} + + +void Simulator::telStatsUpdated(UAVObject* obj) +{ + Q_UNUSED(obj); + + GCSTelemetryStats::DataFields stats = telStats->getData(); + if ( !autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) + { + onAutopilotConnect(); + } + else if ( autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED ) + { + onAutopilotDisconnect(); + } +} + + +void Simulator::resetInitialHomePosition(){ + once=false; +} + + +void Simulator::updateUAVOs(Output2Hardware out){ + + QTime currentTime = QTime::currentTime(); + + Noise noise; + HitlNoiseGeneration noiseSource; + + if(settings.addNoise){ + noise = noiseSource.generateNoise(); + } + else{ + memset(&noise, 0, sizeof(Noise)); + } + + HomeLocation::DataFields homeData = posHome->getData(); + if(!once) + { + // Upon startup, we reset the HomeLocation object to + // the plane's location: + memset(&homeData, 0, sizeof(HomeLocation::DataFields)); + // Update homelocation + homeData.Latitude = out.latitude; //Already in *10^7 integer format + homeData.Longitude = out.longitude; //Already in *10^7 integer format + homeData.Altitude = out.altitude; + double LLA[3]; + LLA[0]=out.latitude; + LLA[1]=out.longitude; + LLA[2]=out.altitude; + double ECEF[3]; + double RNE[9]; + Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); + Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); + homeData.Be[0]=0; + homeData.Be[1]=0; + homeData.Be[2]=0; + posHome->setData(homeData); + posHome->updated(); + + // Compute initial distance + initN = out.dstN; + initE = out.dstE; + initD = out.dstD; + + once=true; + } + + + // Update attActual object + AttitudeActual::DataFields attActualData; + attActualData = attActual->getData(); + + if (settings.attActHW) { + // do nothing + /*****************************************/ + } else if (settings.attActSim) { + // take all data from simulator + attActualData.Roll = out.roll + noise.attActualData.Roll; //roll; + attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch + attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw + float rpy[3]; + float quat[4]; + rpy[0] = attActualData.Roll; + rpy[1] = attActualData.Pitch; + rpy[2] = attActualData.Yaw; + Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); + attActualData.q1 = quat[0]; + attActualData.q2 = quat[1]; + attActualData.q3 = quat[2]; + attActualData.q4 = quat[3]; + + //Set UAVO + attActual->setData(attActualData); + /*****************************************/ + } else if (settings.attActCalc) { + // calculate RPY with code from Attitude module + static float q[4] = {1, 0, 0, 0}; + static float gyro_correct_int2 = 0; + + float dT = out.delT; + + AttitudeSettings::DataFields attSettData = attSettings->getData(); + float accelKp = attSettData.AccelKp * 0.1666666666666667; + float accelKi = attSettData.AccelKp * 0.1666666666666667; + float yawBiasRate = attSettData.YawBiasRate; + + // calibrate sensors on arming + if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) { + accelKp = 2.0; + accelKi = 0.9; + } + + float gyro[3] = {out.rollRate, out.pitchRate, out.yawRate}; + float attRawAcc[3] = {out.accX, out.accY, out.accZ}; + + // code from Attitude module begin /////////////////////////////// + float *accels = attRawAcc; + float grot[3]; + float accel_err[3]; + + // Rotate gravity to body frame and cross with accels + grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); + grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); + grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); + + // CrossProduct + { + accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; + accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; + accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; + } + + // Account for accel magnitude + float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]); + accel_err[0] /= accel_mag; + accel_err[1] /= accel_mag; + accel_err[2] /= accel_mag; + + // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s + gyro_correct_int2 += -gyro[2] * yawBiasRate; + + // Correct rates based on error, integral component dealt with in updateSensors + gyro[0] += accel_err[0] * accelKp / dT; + gyro[1] += accel_err[1] * accelKp / dT; + gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2; + + // Work out time derivative from INSAlgo writeup + // Also accounts for the fact that gyros are in deg/s + float qdot[4]; + qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; + qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; + + // Take a time step + q[0] += qdot[0]; + q[1] += qdot[1]; + q[2] += qdot[2]; + q[3] += qdot[3]; + + if(q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + + // Renomalize + float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + q[0] /= qmag; + q[1] /= qmag; + q[2] /= qmag; + q[3] /= qmag; + + // If quaternion has become inappropriately short or is nan reinit. + // THIS SHOULD NEVER ACTUALLY HAPPEN + if((fabs(qmag) < 1e-3) || (qmag != qmag)) { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } + + float rpy2[3]; + // Quaternion2RPY + { + float q0s, q1s, q2s, q3s; + q0s = q[0] * q[0]; + q1s = q[1] * q[1]; + q2s = q[2] * q[2]; + q3s = q[3] * q[3]; + + float R13, R11, R12, R23, R33; + R13 = 2 * (q[1] * q[3] - q[0] * q[2]); + R11 = q0s + q1s - q2s - q3s; + R12 = 2 * (q[1] * q[2] + q[0] * q[3]); + R23 = 2 * (q[2] * q[3] + q[0] * q[1]); + R33 = q0s - q1s - q2s + q3s; + + rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 + rpy2[2] = RAD2DEG * atan2f(R12, R11); + rpy2[0] = RAD2DEG * atan2f(R23, R33); + } + + attActualData.Roll = rpy2[0]; + attActualData.Pitch = rpy2[1]; + attActualData.Yaw = rpy2[2]; + attActualData.q1 = q[0]; + attActualData.q2 = q[1]; + attActualData.q3 = q[2]; + attActualData.q4 = q[3]; + + //Set UAVO + attActual->setData(attActualData); + /*****************************************/ + } + + if (settings.gcsReceiverEnabled) { + if (gcsRcvrTime.msecsTo(currentTime) >= settings.minOutputPeriod) { + GCSReceiver::DataFields gcsRcvrData; + memset(&gcsRcvrData, 0, sizeof(GCSReceiver::DataFields)); + + for (quint16 i = 0; i < GCSReceiver::CHANNEL_NUMELEM; i++){ + gcsRcvrData.Channel[i] = 1500 + (out.rc_channel[i]*500); //Elements in rc_channel are between -1 and 1 + } + + gcsReceiver->setData(gcsRcvrData); + + gcsRcvrTime=gcsRcvrTime.addMSecs(settings.minOutputPeriod); + + } + } + + + if (settings.gpsPositionEnabled) { + if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { + qDebug()<< " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime); + // Update GPS Position objects + GPSPosition::DataFields gpsPosData; + memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields)); + gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude; + gpsPosData.Heading = out.heading + noise.gpsPosData.Heading; + gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed; + gpsPosData.Latitude = out.latitude + noise.gpsPosData.Latitude; //Already in *10^7 integer format + gpsPosData.Longitude = out.longitude + noise.gpsPosData.Longitude; //Already in *10^7 integer format + gpsPosData.GeoidSeparation = 0.0; + gpsPosData.PDOP = 3.0; + gpsPosData.VDOP = gpsPosData.PDOP*1.5; + gpsPosData.Satellites = 10; + gpsPosData.Status = GPSPosition::STATUS_FIX3D; + + gpsPos->setData(gpsPosData); + + // Update GPS Velocity.{North,East,Down} + GPSVelocity::DataFields gpsVelData; + memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields)); + gpsVelData.North = out.velNorth + noise.gpsVelData.North; + gpsVelData.East = out.velEast + noise.gpsVelData.East; + gpsVelData.Down = out.velDown + noise.gpsVelData.Down; + + gpsVel->setData(gpsVelData); + + gpsPosTime=gpsPosTime.addMSecs(settings.gpsPosRate); + } + } + + // Update VelocityActual.{North,East,Down} + if (settings.groundTruthEnabled) { + if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) { + VelocityActual::DataFields velocityActualData; + memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); + velocityActualData.North = out.velNorth + noise.velocityActualData.North; + velocityActualData.East = out.velEast + noise.velocityActualData.East; + velocityActualData.Down = out.velDown + noise.velocityActualData.Down; + velActual->setData(velocityActualData); + + // Update PositionActual.{Nort,East,Down} + PositionActual::DataFields positionActualData; + memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); + positionActualData.North = (out.dstN-initN) + noise.positionActualData.North; + positionActualData.East = (out.dstE-initE) + noise.positionActualData.East; + positionActualData.Down = (out.dstD/*-initD*/) + noise.positionActualData.Down; + posActual->setData(positionActualData); + + groundTruthTime=groundTruthTime.addMSecs(settings.groundTruthRate); + } + } + +// if (settings.sonarAltitude) { +// static QTime sonarAltTime = currentTime; +// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { +// SonarAltitude::DataFields sonarAltData; +// sonarAltData = sonarAlt->getData(); + +// float sAlt = settings.sonarMaxAlt; +// // 0.35 rad ~= 20 degree +// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { +// float x = agl * qTan(roll); +// float y = agl * qTan(pitch); +// float h = qSqrt(x*x + y*y + agl*agl); +// sAlt = qMin(h, sAlt); +// } + +// sonarAltData.Altitude = sAlt; +// sonarAlt->setData(sonarAltData); +// sonarAltTime = currentTime; +// } +// } + + // Update BaroAltitude object + if (settings.baroAltitudeEnabled){ + if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) { + BaroAltitude::DataFields baroAltData; + memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); + baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude; + baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature; + baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure; + baroAlt->setData(baroAltData); + + baroAltTime=baroAltTime.addMSecs(settings.baroAltRate); + } + } + + // Update AirspeedActual object + if (settings.airspeedActualEnabled){ + if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) { + AirspeedActual::DataFields airspeedActualData; + memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); + airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; + airspeedActual->setData(airspeedActualData); + + airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate); + } + } + + // Update raw attitude sensors + if (settings.attRawEnabled) { + if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) { + //Update gyroscope sensor data + Gyros::DataFields gyroData; + memset(&gyroData, 0, sizeof(Gyros::DataFields)); + gyroData.x = out.rollRate + noise.gyroData.x; + gyroData.y = out.pitchRate + noise.gyroData.y; + gyroData.z = out.yawRate + noise.gyroData.z; + gyros->setData(gyroData); + + //Update accelerometer sensor data + Accels::DataFields accelData; + memset(&accelData, 0, sizeof(Accels::DataFields)); + accelData.x = out.accX + noise.accelData.x; + accelData.y = out.accY + noise.accelData.y; + accelData.z = out.accZ + noise.accelData.z; + accels->setData(accelData); + + attRawTime=attRawTime.addMSecs(settings.attRawRate); + } + } +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h similarity index 50% rename from ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h rename to ground/openpilotgcs/src/plugins/hitl/simulator.h index 445358a22..41de0937d 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -1,222 +1,337 @@ -/** - ****************************************************************************** - * - * @file simulatorv2.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef ISIMULATORV2_H -#define ISIMULATORV2_H - -#include -#include -#include -#include -#include -#include -#include "uavtalk/telemetrymanager.h" -#include "uavobjectmanager.h" -#include "homelocation.h" -#include "accels.h" -#include "gyros.h" -#include "attitudeactual.h" -#include "gpsposition.h" -#include "flightstatus.h" -#include "gcsreceiver.h" -#include "actuatorcommand.h" -#include "gcstelemetrystats.h" -#include "attitudesettings.h" -#include "sonaraltitude.h" - -//#define DBG_TIMERS -#undef DBG_TIMERS - -/** - * just imagine this was a class without methods and all public properties - */ - -typedef struct _CONNECTION -{ - QString simulatorId; - QString hostAddress; - int inPort; - QString remoteAddress; - int outPort; - QString binPath; - QString dataPath; - - bool homeLocation; - quint16 homeLocRate; - - bool attRaw; - quint8 attRawRate; - - bool attActual; - bool attActHW; - bool attActSim; - bool attActCalc; - - bool sonarAltitude; - float sonarMaxAlt; - quint16 sonarAltRate; - - bool gpsPosition; - quint16 gpsPosRate; - - bool inputCommand; - bool gcsReciever; - bool manualControl; - bool manualOutput; - quint8 outputRate; - -} SimulatorSettings; - -class Simulator : public QObject -{ - Q_OBJECT - -public: - Simulator(const SimulatorSettings& params); - virtual ~Simulator(); - - bool isAutopilotConnected() const { return autopilotConnectionStatus; } - bool isSimulatorConnected() const { return simConnectionStatus; } - - QString Name() const { return name; } - void setName(QString str) { name = str; } - - QString SimulatorId() const { return simulatorId; } - void setSimulatorId(QString str) { simulatorId = str; } - - static bool IsStarted() { return isStarted; } - static void setStarted(bool val) { isStarted = val; } - - static QStringList& Instances() { return Simulator::instances; } - static void setInstance(const QString& str) { Simulator::instances.append(str); } - - virtual void stopProcess() {} - virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} - -signals: - void processOutput(QString str); - void autopilotConnected(); - void autopilotDisconnected(); - void simulatorConnected(); - void simulatorDisconnected(); - void myStart(); - -public slots: - Q_INVOKABLE virtual bool setupProcess() { return true; } - -private slots: - void onStart(); - void receiveUpdate(); - void onAutopilotConnect(); - void onAutopilotDisconnect(); - void onSimulatorConnectionTimeout(); - void telStatsUpdated(UAVObject* obj); - Q_INVOKABLE void onDeleteSimulator(void); - - virtual void transmitUpdate() = 0; - virtual void processUpdate(const QByteArray& data) = 0; - -protected: - static const float GEE; - static const float FT2M; - static const float KT2MPS; - static const float INHG2KPA; - static const float FPS2CMPS; - static const float DEG2RAD; - static const float RAD2DEG; - -#ifdef DBG_TIMERS - QTime timeRX; - QTime timeTX; -#endif - - QUdpSocket* inSocket; - QUdpSocket* outSocket; - -// ActuatorDesired* actDesired; -// ManualControlCommand* manCtrlCommand; -// VelocityActual* velActual; -// PositionActual* posActual; -// BaroAltitude* altActual; -// CameraDesired *camDesired; -// AccessoryDesired *acsDesired; - Accels *accels; - Gyros *gyros; - AttitudeActual *attActual; - HomeLocation *posHome; - FlightStatus *flightStatus; - GPSPosition *gpsPosition; - GCSReceiver *gcsReceiver; - ActuatorCommand *actCommand; - AttitudeSettings *attSettings; - SonarAltitude *sonarAlt; - - GCSTelemetryStats* telStats; - SimulatorSettings settings; - - QMutex lock; - -private: - int updatePeriod; - int simTimeout; - volatile bool autopilotConnectionStatus; - volatile bool simConnectionStatus; - QTimer* txTimer; - QTimer* simTimer; - QString name; - QString simulatorId; - volatile static bool isStarted; - static QStringList instances; - - void setupObjects(); - void resetAllObjects(); - void setupInputObject(UAVObject* obj, quint32 updateRate); - void setupOutputObject(UAVObject* obj); - void setupWatchedObject(UAVObject *obj); - void setupDefaultObject(UAVObject *obj); -}; - -class SimulatorCreator -{ -public: - SimulatorCreator(QString id, QString descr) : - classId(id), - description(descr) - {} - virtual ~SimulatorCreator() {} - - QString ClassId() const { return classId; } - QString Description() const { return description; } - - virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; - -private: - QString classId; - QString description; -}; - -#endif // ISIMULATORV2_H +/** + ****************************************************************************** + * + * @file simulator.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup HITLPlugin HITL Plugin + * @{ + * @brief The Hardware In The Loop plugin + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef ISIMULATOR_H +#define ISIMULATOR_H + +#include +#include +#include +#include +#include + +#include "qscopedpointer.h" +#include "uavtalk/telemetrymanager.h" +#include "uavobjectmanager.h" + +#include "accels.h" +#include "actuatorcommand.h" +#include "actuatordesired.h" +#include "attitudeactual.h" +#include "attitudesettings.h" +#include "airspeedactual.h" +#include "baroaltitude.h" +#include "flightstatus.h" +#include "gcsreceiver.h" +#include "gcstelemetrystats.h" +#include "gpsposition.h" +#include "gpsvelocity.h" +#include "gyros.h" +#include "homelocation.h" +#include "manualcontrolcommand.h" +#include "positionactual.h" +#include "sonaraltitude.h" +#include "velocityactual.h" + +#include "utils/coordinateconversions.h" + +/** + * just imagine this was a class without methods and all public properties + */ +typedef struct _FLIGHT_PARAM { + + // time + float T; + float dT; + unsigned int i; + + // speed (relative) + float ias; + float cas; + float tas; + float groundspeed; + + // position (absolute) + float X; + float Y; + float Z; + + // speed (absolute) + float dX; + float dY; + float dZ; + + // acceleration (absolute) + float ddX; + float ddY; + float ddZ; + + //angle + float azimuth; + float pitch; + float roll; + + //rotation speed + float dAzimuth; + float dPitch; + float dRoll; + +} FLIGHT_PARAM; + +typedef struct _CONNECTION +{ + QString simulatorId; + QString binPath; + QString dataPath; + QString hostAddress; + QString remoteAddress; + int outPort; + int inPort; + bool startSim; + bool addNoise; + QString latitude; + QString longitude; + +// bool homeLocation; + + bool attRawEnabled; + quint8 attRawRate; + + bool attActualEnabled; + bool attActHW; + bool attActSim; + bool attActCalc; + + bool baroAltitudeEnabled; + quint16 baroAltRate; + + bool groundTruthEnabled; + quint16 groundTruthRate; + + bool gpsPositionEnabled; + quint16 gpsPosRate; + + bool inputCommand; + bool gcsReceiverEnabled; + bool manualControlEnabled; + quint16 minOutputPeriod; + + bool airspeedActualEnabled; + quint16 airspeedActualRate; + +} SimulatorSettings; + + +struct Output2Hardware{ + float latitude; + float longitude; + float altitude; + float heading; + float groundspeed; //[m/s] + float calibratedAirspeed; //[m/s] + float roll; + float pitch; + float pressure; + float temperature; + float velNorth; //[m/s] + float velEast; //[m/s] + float velDown; //[m/s] + float dstN; //[m] + float dstE; //[m] + float dstD; //[m] + float accX; //[m/s^2] + float accY; //[m/s^2] + float accZ; //[m/s^2] + float rollRate; //[deg/s] + float pitchRate; //[deg/s] + float yawRate; //[deg/s] + float delT; + + float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; //Elements in rc_channel are between -1 and 1 + + + float rollDesired; + float pitchDesired; + float yawDesired; + float throttleDesired; +}; + +//struct Output2Simulator{ +// float roll; +// float pitch; +// float yaw; +// float throttle; + +// float ailerons; +// float rudder; +// float elevator; +// float motor; +//}; + +class Simulator : public QObject +{ + Q_OBJECT + +public: + Simulator(const SimulatorSettings& params); + virtual ~Simulator(); + + bool isAutopilotConnected() const { return autopilotConnectionStatus; } + bool isSimulatorConnected() const { return simConnectionStatus; } + QString Name() const { return name; } + void setName(QString str) { name = str; } + + QString SimulatorId() const { return simulatorId; } + void setSimulatorId(QString str) { simulatorId = str; } + + + + static bool IsStarted() { return isStarted; } + static void setStarted(bool val) { isStarted = val; } + static QStringList& Instances() { return Simulator::instances; } + static void setInstance(const QString& str) { Simulator::instances.append(str); } + + virtual void stopProcess() {} + virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} + + void resetInitialHomePosition(); + void updateUAVOs(Output2Hardware out); + +signals: + void autopilotConnected(); + void autopilotDisconnected(); + void simulatorConnected(); + void simulatorDisconnected(); + void processOutput(QString str); + void deleteSimProcess(); + void myStart(); +public slots: + Q_INVOKABLE virtual bool setupProcess() { return true;} +private slots: + void onStart(); + //void transmitUpdate(); + void receiveUpdate(); + void onAutopilotConnect(); + void onAutopilotDisconnect(); + void onSimulatorConnectionTimeout(); + void telStatsUpdated(UAVObject* obj); + Q_INVOKABLE void onDeleteSimulator(void); + + virtual void transmitUpdate() = 0; + virtual void processUpdate(const QByteArray& data) = 0; + +protected: + static const float GEE; + static const float FT2M; + static const float KT2MPS; + static const float INHG2KPA; + static const float FPS2CMPS; + static const float DEG2RAD; + static const float RAD2DEG; + + QProcess* simProcess; + QTime* time; + QUdpSocket* inSocket;//(new QUdpSocket()); + QUdpSocket* outSocket; + + ActuatorCommand* actCommand; + ActuatorDesired* actDesired; + ManualControlCommand* manCtrlCommand; + FlightStatus* flightStatus; + BaroAltitude* baroAlt; + AirspeedActual* airspeedActual; + AttitudeActual* attActual; + AttitudeSettings* attSettings; + VelocityActual* velActual; + GPSPosition* gpsPos; + GPSVelocity* gpsVel; + PositionActual* posActual; + HomeLocation* posHome; + Accels* accels; + Gyros* gyros; + GCSTelemetryStats* telStats; + GCSReceiver* gcsReceiver; + + SimulatorSettings settings; + + FLIGHT_PARAM current; + FLIGHT_PARAM old; + QMutex lock; + +private: + bool once; + float initN; + float initE; + float initD; + + int updatePeriod; + int simTimeout; + volatile bool autopilotConnectionStatus; + volatile bool simConnectionStatus; + QTimer* txTimer; + QTimer* simTimer; + + QTime attRawTime; + QTime gpsPosTime; + QTime groundTruthTime; + QTime baroAltTime; + QTime gcsRcvrTime; + QTime airspeedActualTime; + + QString name; + QString simulatorId; + volatile static bool isStarted; + static QStringList instances; + //QList > requiredUAVObjects; + void setupOutputObject(UAVObject* obj, quint32 updatePeriod); + void setupInputObject(UAVObject* obj, quint32 updatePeriod); + void setupWatchedObject(UAVObject *obj, quint32 updatePeriod); + void setupObjects(); +}; + + + +class SimulatorCreator +{ +public: + SimulatorCreator(QString id, QString descr) : + classId(id), + description(descr) + {} + virtual ~SimulatorCreator() {} + + QString ClassId() const {return classId;} + QString Description() const {return description;} + + virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; + +private: + QString classId; + QString description; +}; + +#endif // ISIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp new file mode 100644 index 000000000..448e6f45a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -0,0 +1,371 @@ +/** + ****************************************************************************** + * + * @file xplanesimulator.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup hitlplugin + * @{ + * + *****************************************************************************/ +/* + * This program 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. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Description of X-Plane Protocol: + * + * To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu + * (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set. + * Data Set shown all X-Plane params, + * each row has four checkbox: 1st check - out to UDP; 4 check - show on screen + * All the UDP messages for X-Plane have the same format, which is: + * 5-character MESSAGE PROLOUGE (to indicate the type of message) + * and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive) + * + * DATA INPUT/OUTPUT STRUCTURE is the following stuct: + * + * struct data_struct + * { + * int index; // data index, the index into the list of variables + // you can output from the Data Output screen in X-Plane. + * float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection.. + // many outputs do not use all 8, though. + * }; + * + * For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set) + * bytes value description + * [0-3] DATA message type + * [4] none no matter + * [5-8] 11 code of setting param(row in Data Set) + * [9-41] data message data (8 float values) + * total size: 41 byte + * + */ + +#include "xplanesimulator.h" +#include "extensionsystem/pluginmanager.h" +#include +#include +#include +#include + +void TraceBuf(const char* buf,int len); + +XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : + Simulator(params) +{ + resetInitialHomePosition(); +} + + +XplaneSimulator::~XplaneSimulator() +{ +} + +void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +{ + Q_UNUSED(outPort); + + inSocket->bind(QHostAddress(host), inPort); + //outSocket->bind(QHostAddress(host), outPort); + resetInitialHomePosition(); + +} + +bool XplaneSimulator::setupProcess() +{ + emit processOutput(QString("Please start X-Plane manually, and make sure it is setup to output its ") + + "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort)); + return true; + +} + +/** + * update data in X-Plane simulator + */ +void XplaneSimulator::transmitUpdate() +{ + if (settings.manualControlEnabled) { + //Read ActuatorDesired from autopilot + ActuatorDesired::DataFields actData = actDesired->getData(); + float ailerons = actData.Roll; + float elevator = actData.Pitch; + float rudder = actData.Yaw; + float throttle = actData.Throttle > 0? actData.Throttle : 0; + float none = -999; + //quint32 none = *((quint32*)&tmp); // get float as 4 bytes + + quint32 code; + QByteArray buf; + QDataStream stream(&buf,QIODevice::ReadWrite); + + // !!! LAN byte order - Big Endian +#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN + stream.setByteOrder(QDataStream::LittleEndian); +#endif + + // 11th data settings (flight con: ail/elv/rud) + buf.clear(); + code = 11; + //quint8 header[] = "DATA"; + /* + stream << *((quint32*)header) << + (quint8)0x30 << + code << + *((quint32*)&elevator) << + *((quint32*)&ailerons) << + *((quint32*)&rudder) << + none << + *((quint32*)&ailerons) << + none << + none << + none; + */ + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); +// TraceBuf(buf.data(),41); + + if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) + { + emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); + } + //outSocket->write(buf); + + // 25th data settings (throttle command) + buf.clear(); + code = 25; + //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none + // << none << none << none << none << none; + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + + if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) + { + emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); + } + + //outSocket->write(buf); + + + + /** !!! this settings was given from ardupilot X-Plane.pl, I comment them + but if it needed comment should be removed !!! + + // 8th data settings (joystick 1 ail/elv/rud) + stream << "DATA0" << quint32(11) << elevator << ailerons << rudder + << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); + outSocket->write(buf); + */ + } + +} + +/** + * process data string from X-Plane simulator + */ +void XplaneSimulator::processUpdate(const QByteArray& dataBuf) +{ + float altitude = 0; + float latitude = 0; + float longitude = 0; + float airspeed_keas = 0; + float groundspeed_ktgs = 0; + float pitch = 0; + float roll = 0; + float heading = 0; + float pressure = 0; + float temperature = 0; + float velX = 0; + float velY = 0; + float velZ = 0; + float dstX = 0; + float dstY = 0; + float dstZ = 0; + float accX = 0; + float accY = 0; + float accZ = 0; + float rollRate_rad=0; + float pitchRate_rad=0; + float yawRate_rad=0; + + // QString str; + QByteArray& buf = const_cast(dataBuf); + QString data(buf); + + if(data.left(4) == "DATA") // check type of packet + { + buf.remove(0,5); + if(dataBuf.size() % 36) + { + qxtLog->info("incorrect length of UDP packet: ",buf); + return; // incorrect length of struct + } + // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 + int channelCounter = dataBuf.size() / 36; + do + { + switch(buf[0]) // switch by id + { + case XplaneSimulator::LatitudeLongitudeAltitude: + latitude = *((float*)(buf.data()+4*1)); + longitude = *((float*)(buf.data()+4*2)); + altitude = *((float*)(buf.data()+4*3))* FT2M; + break; + + case XplaneSimulator::Speed: + airspeed_keas = *((float*)(buf.data()+4*2)); + groundspeed_ktgs = *((float*)(buf.data()+4*4)); + break; + + case XplaneSimulator::PitchRollHeading: + pitch = *((float*)(buf.data()+4*1)); + roll = *((float*)(buf.data()+4*2)); + heading = *((float*)(buf.data()+4*3)); + break; + + /* + case XplaneSimulator::SystemPressures: + pressure = *((float*)(buf.data()+4*1)); + break; + */ + + case XplaneSimulator::AtmosphereWeather: + pressure = *((float*)(buf.data()+4*1)) * INHG2KPA; + temperature = *((float*)(buf.data()+4*2)); + break; + + case XplaneSimulator::LocVelDistTraveled: + dstX = *((float*)(buf.data()+4*1)); + dstY = - *((float*)(buf.data()+4*3)); + dstZ = *((float*)(buf.data()+4*2)); + velX = *((float*)(buf.data()+4*4)); + velY = - *((float*)(buf.data()+4*6)); + velZ = *((float*)(buf.data()+4*5)); + break; + + case XplaneSimulator::AngularVelocities: //In [rad/s] + pitchRate_rad = *((float*)(buf.data()+4*1)); + rollRate_rad = *((float*)(buf.data()+4*2)); + yawRate_rad = *((float*)(buf.data()+4*3)); + break; + + case XplaneSimulator::Gload: + accX = *((float*)(buf.data()+4*6)) * GEE; + accY = *((float*)(buf.data()+4*7)) * GEE; + accZ = *((float*)(buf.data()+4*5)) * GEE; + break; + + default: + break; + } + channelCounter--; + buf.remove(0,36); + } while (channelCounter); + + + /////// + // Output formatting + /////// + Output2Hardware out; + memset(&out, 0, sizeof(Output2Hardware)); + + // Update GPS Position objects + out.latitude = latitude * 1e7; + out.longitude = longitude * 1e7; + out.altitude = altitude; + out.groundspeed = groundspeed_ktgs*1.15*1.6089/3.6; //Convert from [kts] to [m/s] + + out.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s] + + // Update BaroAltitude object + out.temperature = temperature; + out.pressure = pressure; + + // Update attActual object + out.roll = roll; //roll; + out.pitch = pitch; // pitch + out.heading = heading; // yaw + + + out.dstN=dstY; + out.dstE=dstX; + out.dstD=-dstZ; + + // Update VelocityActual.{North,East,Down} + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; + + //Update gyroscope sensor data + out.rollRate = rollRate_rad; + out.pitchRate = pitchRate_rad; + out.yawRate = yawRate_rad; + + //Update accelerometer sensor data + out.accX = accX; + out.accY = accY; + out.accZ = -accZ; + + updateUAVOs(out); + } + // issue manual update + //attActual->updated(); + //altActual->updated(); + //posActual->updated(); + +} + + +void TraceBuf(const char* buf,int len) +{ + QString str; + bool reminder=true; + for(int i=0; i < len; i++) + { + if(!(i%16)) + { + if(i>0) + { + qDebug() << str; + str.clear(); + reminder=false; + } + reminder=true; + } + str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0')); + } + + if(reminder){ + qDebug() << str; + } +} diff --git a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h similarity index 85% rename from ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.h rename to ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h index 2202f1b79..ca3437475 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h @@ -45,12 +45,8 @@ private slots: void transmitUpdate(); private: - bool once; - float initX; - float initY; - float initZ; - enum XplaneOutputData - { + enum XplaneOutputData //***WARNING***: Elements in this enum are in a precise order, do + { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml FramRate, Times, SimStats, @@ -68,10 +64,10 @@ private: Brakes, AngularMoments, AngularAccelerations, - AngularVelocities, - PitchRollHeading, + AngularVelocities, + PitchRollHeading, AoA, - LatitudeLongitude, + LatitudeLongitudeAltitude, LocVelDistTraveled }; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp deleted file mode 100644 index fec61c88d..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp +++ /dev/null @@ -1,403 +0,0 @@ -/** - ****************************************************************************** - * - * @file flightgearbridge.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITL Plugin - * @{ - * @brief The Hardware In The Loop plugin - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "fgsimulator.h" -#include "extensionsystem/pluginmanager.h" -#include "coreplugin/icore.h" -#include "coreplugin/threadmanager.h" - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - - - - -//FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : -// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), -// fgProcess(NULL) -//{ -// // Note: Only tested on windows 7 -//#if defined(Q_WS_WIN) -// cmdShell = QString("c:/windows/system32/cmd.exe"); -//#else -// cmdShell = QString("bash"); -//#endif -//} - -FGSimulator::FGSimulator(const SimulatorSettings& params) : - Simulator(params) -{ - udpCounterFGrecv = 0; - udpCounterGCSsend = 0; -} - -FGSimulator::~FGSimulator() -{ - disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); -} - -void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) -{ - if(inSocket->bind(QHostAddress(host), inPort)) - emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n"); - else - emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n"); -} - -bool FGSimulator::setupProcess() -{ - QMutexLocker locker(&lock); - - // Copy FlightGear generic protocol configuration file to the FG protocol directory - // NOTE: Not working on Windows 7, if FG is installed in the "Program Files", - // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml -// QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); -// xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); -// QString xml = xmlFile.readAll(); -// xmlFile.close(); -// QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); -// xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); -// xmlFileOut.write(xml.toAscii()); -// xmlFileOut.close(); - - Qt::HANDLE mainThread = QThread::currentThreadId(); - qDebug() << "setupProcess Thread: "<< mainThread; - - simProcess = new QProcess(); - simProcess->setReadChannelMode(QProcess::MergedChannels); - connect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); - // Note: Only tested on windows 7 -#if defined(Q_WS_WIN) - QString cmdShell("c:/windows/system32/cmd.exe"); -#else - QString cmdShell("bash"); -#endif - - // Start shell (Note: Could not start FG directly on Windows, only through terminal!) - simProcess->start(cmdShell); - if (simProcess->waitForStarted() == false) - { - emit processOutput("Error:" + simProcess->errorString()); - return false; - } - - // Setup arguments - // Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS. - // If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup. - QString args("--fg-root=\"" + settings.dataPath + "\" " + - "--timeofday=noon " + - "--httpd=5400 " + - "--enable-hud " + - "--in-air " + - "--altitude=3000 " + - "--vc=100 " + - "--log-level=alert " + - "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol"); - if(!settings.manual) - { - args.append(" --generic=socket,in,400," + settings.remoteHostAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol"); - } - - // Start FlightGear - only if checkbox is selected in HITL options page - if (settings.startSim) - { - QString cmd("\"" + settings.binPath + "\" " + args + "\n"); - simProcess->write(cmd.toAscii()); - } - else - { - emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" + - "You can optionally run Flightgear from a networked computer.\n" + - "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n" - "Remote computer must have the correct OpenPilot protocol installed."); - } - - udpCounterGCSsend = 0; - - return true; -} - -void FGSimulator::processReadyRead() -{ - QByteArray bytes = simProcess->readAllStandardOutput(); - QString str(bytes); - if ( !str.contains("Error reading data") ) // ignore error - { - emit processOutput(str); - } -} - -void FGSimulator::transmitUpdate() -{ - ActuatorDesired::DataFields actData; - FlightStatus::DataFields flightStatusData = flightStatus->getData(); - ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData(); - float ailerons = -1; - float elevator = -1; - float rudder = -1; - float throttle = -1; - - if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) - { - // Read joystick input - if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) - { - // Note: Pitch sign is reversed in FG ? - ailerons = manCtrlData.Roll; - elevator = -manCtrlData.Pitch; - rudder = manCtrlData.Yaw; - throttle = manCtrlData.Throttle; - } - } - else - { - // Read ActuatorDesired from autopilot - actData = actDesired->getData(); - - ailerons = actData.Roll; - elevator = -actData.Pitch; - rudder = actData.Yaw; - throttle = actData.Throttle; - } - - int allowableDifference = 10; - - //qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv; - - if(udpCounterFGrecv == udpCounterGCSsend) - udpCounterGCSsend = 0; - - if((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv==0) ) //FG udp queue is not delayed - { - udpCounterGCSsend++; - - // Send update to FlightGear - QString cmd; - cmd = QString("%1,%2,%3,%4,%5\n") - .arg(ailerons) //ailerons - .arg(elevator) //elevator - .arg(rudder) //rudder - .arg(throttle) //throttle - .arg(udpCounterGCSsend); //UDP packet counter delay - - QByteArray data = cmd.toAscii(); - - if(outSocket->writeDatagram(data, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) - { - emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n"); - } - } - else - { - // don't send new packet. Flightgear cannot process UDP fast enough. - // V1.9.1 reads udp packets at set frequency and will get delayed if packets are sent too fast - // V2.0 does not currently work with --generic-protocol - } - - if(!settings.manual) - { - actData.Roll = ailerons; - actData.Pitch = -elevator; - actData.Yaw = rudder; - actData.Throttle = throttle; - //actData.NumLongUpdates = (float)udpCounterFGrecv; - //actData.UpdateTime = (float)udpCounterGCSsend; - actDesired->setData(actData); - } -} - - -void FGSimulator::processUpdate(const QByteArray& inp) -{ - //TODO: this does not use the FLIGHT_PARAM structure, it should! - static char once=0; - // Split - QString data(inp); - QStringList fields = data.split(","); - // Get xRate (deg/s) -// float xRate = fields[0].toFloat() * 180.0/M_PI; - // Get yRate (deg/s) -// float yRate = fields[1].toFloat() * 180.0/M_PI; - // Get zRate (deg/s) -// float zRate = fields[2].toFloat() * 180.0/M_PI; - // Get xAccel (m/s^2) -// float xAccel = fields[3].toFloat() * FT2M; - // Get yAccel (m/s^2) -// float yAccel = fields[4].toFloat() * FT2M; - // Get xAccel (m/s^2) -// float zAccel = fields[5].toFloat() * FT2M; - // Get pitch (deg) - float pitch = fields[6].toFloat(); - // Get pitchRate (deg/s) - float pitchRate = fields[7].toFloat(); - // Get roll (deg) - float roll = fields[8].toFloat(); - // Get rollRate (deg/s) - float rollRate = fields[9].toFloat(); - // Get yaw (deg) - float yaw = fields[10].toFloat(); - // Get yawRate (deg/s) - float yawRate = fields[11].toFloat(); - // Get latitude (deg) - float latitude = fields[12].toFloat(); - // Get longitude (deg) - float longitude = fields[13].toFloat(); - // Get heading (deg) - float heading = fields[14].toFloat(); - // Get altitude (m) - float altitude = fields[15].toFloat() * FT2M; - // Get altitudeAGL (m) - float altitudeAGL = fields[16].toFloat() * FT2M; - // Get groundspeed (m/s) - float groundspeed = fields[17].toFloat() * KT2MPS; - // Get airspeed (m/s) -// float airspeed = fields[18].toFloat() * KT2MPS; - // Get temperature (degC) - float temperature = fields[19].toFloat(); - // Get pressure (kpa) - float pressure = fields[20].toFloat() * INHG2KPA; - // Get VelocityActual Down (cm/s) - float velocityActualDown = - fields[21].toFloat() * FPS2CMPS; - // Get VelocityActual East (cm/s) - float velocityActualEast = fields[22].toFloat() * FPS2CMPS; - // Get VelocityActual Down (cm/s) - float velocityActualNorth = fields[23].toFloat() * FPS2CMPS; - - // Get UDP packets received by FG - int n = fields[24].toInt(); - udpCounterFGrecv = n; - - //run once - HomeLocation::DataFields homeData = posHome->getData(); - if(!once) - { - memset(&homeData, 0, sizeof(HomeLocation::DataFields)); - // Update homelocation - homeData.Latitude = latitude * 10e6; - homeData.Longitude = longitude * 10e6; - homeData.Altitude = 0; - double LLA[3]; - LLA[0]=latitude; - LLA[1]=longitude; - LLA[2]=0; - double ECEF[3]; - double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - homeData.Be[0]=0; - homeData.Be[1]=0; - homeData.Be[2]=0; - posHome->setData(homeData); - once=1; - } - - // Update VelocityActual.{Nort,East,Down} - VelocityActual::DataFields velocityActualData; - memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); - velocityActualData.North = velocityActualNorth; - velocityActualData.East = velocityActualEast; - velocityActualData.Down = velocityActualDown; - velActual->setData(velocityActualData); - - // Update PositionActual.{Nort,East,Down} - PositionActual::DataFields positionActualData; - memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); - positionActualData.North = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.East = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.Down = altitude ; //Multiply by 1 because positionActual expects input in meters. - posActual->setData(positionActualData); - - // Update AltitudeActual object - BaroAltitude::DataFields altActualData; - memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); - altActualData.Altitude = altitudeAGL; - altActualData.Temperature = temperature; - altActualData.Pressure = pressure; - altActual->setData(altActualData); - - // Update attActual object - AttitudeActual::DataFields attActualData; - memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); - attActualData.Roll = roll; - attActualData.Pitch = pitch; - attActualData.Yaw = yaw; - attActualData.q1 = 0; - attActualData.q2 = 0; - attActualData.q3 = 0; - attActualData.q4 = 0; - attActual->setData(attActualData); - - // Update gps objects - GPSPosition::DataFields gpsData; - memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); - gpsData.Altitude = altitude; - gpsData.Heading = heading; - gpsData.Groundspeed = groundspeed; - gpsData.Latitude = latitude*1e7; - gpsData.Longitude = longitude*1e7; - gpsData.Satellites = 10; - gpsData.Status = GPSPosition::STATUS_FIX3D; - gpsPos->setData(gpsData); - - float NED[3]; - // convert from cm back to meters - - double hLLA[3] = {(double) homeData.Latitude / 1e7, (double) homeData.Longitude / 1e7, (double) (homeData.Altitude)}; - double ECEF[3]; - double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(hLLA,(double (*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(hLLA,ECEF); - Utils::CoordinateConversions().LLA2Base(hLLA, ECEF, (float (*)[3]) RNE, NED); - - positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance - positionActualData.Down = NED[2]; //Multiply by 1 because positionActual expects input in meters. - posActual->setData(positionActualData); - - // Update AttitudeRaw object (filtered gyros only for now) - //AttitudeRaw::DataFields rawData; - //AttitudeRaw::DataFields rawData; - Gyros::DataFields gyroData; - Accels::DataFields accelData; - memset(&gyroData, 0, sizeof(Gyros::DataFields)); - memset(&accelData, 0, sizeof(Accels::DataFields)); - gyroData = gyros->getData(); - accelData = accels->getData(); - //rawData.gyros[0] = rollRate; - //rawData.gyros[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate; - //rawData.gyros[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate; - //rawData.gyros[1] = pitchRate; - //rawData.gyros[2] = yawRate; - gyroData.x = rollRate; - gyroData.y = pitchRate; - gyroData.z = yawRate; - // TODO: Accels are still missing!!!! - gyros->setData(gyroData); - // attRaw->updated(); -} - diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp deleted file mode 100644 index e6fa37b09..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlconfiguration.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITL Plugin - * @{ - * @brief The Hardware In The Loop plugin - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlconfiguration.h" - -HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObject *parent) : - IUAVGadgetConfiguration(classId, parent) -{ - settings.simulatorId = ""; - settings.binPath = ""; - settings.dataPath = ""; - settings.manual = false; - settings.startSim = false; - settings.hostAddress = "127.0.0.1"; - settings.remoteHostAddress = "127.0.0.1"; - settings.outPort = 0; - settings.inPort = 0; - settings.latitude = ""; - settings.longitude = ""; - - //if a saved configuration exists load it - if(qSettings != 0) { - settings.simulatorId = qSettings->value("simulatorId").toString(); - settings.binPath = qSettings->value("binPath").toString(); - settings.dataPath = qSettings->value("dataPath").toString(); - settings.manual = qSettings->value("manual").toBool(); - settings.startSim = qSettings->value("startSim").toBool(); - settings.hostAddress = qSettings->value("hostAddress").toString(); - settings.remoteHostAddress = qSettings->value("remoteHostAddress").toString(); - settings.outPort = qSettings->value("outPort").toInt(); - settings.inPort = qSettings->value("inPort").toInt(); - settings.latitude = qSettings->value("latitude").toString(); - settings.longitude = qSettings->value("longitude").toString(); - } -} - -IUAVGadgetConfiguration *HITLConfiguration::clone() -{ - HITLConfiguration *m = new HITLConfiguration(this->classId()); - - m->settings = settings; - return m; -} - - /** - * Saves a configuration. - * - */ -void HITLConfiguration::saveConfig(QSettings* qSettings) const { - qSettings->setValue("simulatorId", settings.simulatorId); - qSettings->setValue("binPath", settings.binPath); - qSettings->setValue("dataPath", settings.dataPath); - qSettings->setValue("manual", settings.manual); - qSettings->setValue("startSim", settings.startSim); - qSettings->setValue("hostAddress", settings.hostAddress); - qSettings->setValue("remoteHostAddress", settings.remoteHostAddress); - qSettings->setValue("outPort", settings.outPort); - qSettings->setValue("inPort", settings.inPort); - qSettings->setValue("latitude", settings.latitude); - qSettings->setValue("longitude", settings.longitude); -} - diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp deleted file mode 100644 index dd9cadb63..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitloptionspage.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITL Plugin - * @{ - * @brief The Hardware In The Loop plugin - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitloptionspage.h" -#include "hitlconfiguration.h" -#include "ui_hitloptionspage.h" -#include - -#include -#include -#include -#include - - - -HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : - IOptionsPage(parent), - config(conf) -{ -} - -QWidget *HITLOptionsPage::createPage(QWidget *parent) -{ - // Create page - m_optionsPage = new Ui::HITLOptionsPage(); - QWidget* optionsPageWidget = new QWidget; - m_optionsPage->setupUi(optionsPageWidget); - int index = 0; - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(),creator->ClassId()); - } - - //QString classId = widget->listSimulators->itemData(0).toString(); - //SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId); - - //QWidget* embedPage = creator->createOptionsPage(); - //m_optionsPage->verticalLayout->addWidget(embedPage); - - m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); - m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); - m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); - m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); - - // Restore the contents from the settings: - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) - { - QString id = config->Settings().simulatorId; - if(creator->ClassId() == id) - m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); - } - - m_optionsPage->executablePath->setPath(config->Settings().binPath); - m_optionsPage->dataPath->setPath(config->Settings().dataPath); - m_optionsPage->manualControl->setChecked(config->Settings().manual); - m_optionsPage->startSim->setChecked(config->Settings().startSim); - - m_optionsPage->hostAddress->setText(config->Settings().hostAddress); - m_optionsPage->remoteHostAddress->setText(config->Settings().remoteHostAddress); - m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); - m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); - m_optionsPage->latitude->setText(config->Settings().latitude); - m_optionsPage->longitude->setText(config->Settings().longitude); - - return optionsPageWidget; -} - -void HITLOptionsPage::apply() -{ - SimulatorSettings settings; - int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - settings.manual = m_optionsPage->manualControl->isChecked(); - settings.startSim = m_optionsPage->startSim->isChecked(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.remoteHostAddress = m_optionsPage->remoteHostAddress->text(); - - settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.longitude = m_optionsPage->longitude->text(); - settings.latitude = m_optionsPage->latitude->text(); - - config->setSimulatorSettings(settings); -} - -void HITLOptionsPage::finish() -{ - delete m_optionsPage; -} diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui deleted file mode 100644 index e7eb23232..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui +++ /dev/null @@ -1,321 +0,0 @@ - - - HITLOptionsPage - - - - 0 - 0 - 626 - 305 - - - - - 0 - 0 - - - - Form - - - - 0 - - - - - QFrame::NoFrame - - - QFrame::Plain - - - true - - - - - 0 - 0 - 626 - 305 - - - - - - - - 0 - 0 - - - - For receiving data from sim - - - - - - - Latitude in degrees: - - - - - - - Input Port: - - - - - - - - 0 - 0 - - - - - - - - Choose flight simulator: - - - - - - - true - - - - 0 - 0 - - - - Check this box to start the simulator on the local computer - - - Start simulator on local machine - - - - - - - Qt::Vertical - - - - 20 - 182 - - - - - - - - Qt::Horizontal - - - - - - - - - - - 0 - 0 - - - - Path executable: - - - - - - - - - - - 0 - 0 - - - - Data directory: - - - - - - - - 0 - 0 - - - - Manual aircraft control (can be used when hardware is not available) - - - Manual aircraft control (can be used when hardware is not available) - - - - - - - - 1 - 0 - - - - - - - - - 0 - 0 - - - - Local interface (IP): - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. - - - - - - - Remote interface (IP): - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. - - - - - - - - 0 - 0 - - - - Output Port: - - - - - - - Longitude in degrees: - - - - - - - - 0 - 0 - - - - For sending data to sim - - - - - - - - 0 - 0 - - - - - - label_3 - verticalSpacer - label_7 - label_5 - startSim - label_2 - executablePath - label - dataPath - manualControl - chooseFlightSimulator - latitude - line - inputPort - label_6 - hostAddress - label_9 - remoteHostAddress - label_4 - label_8 - outputPort - longitude - - - - - - - - Utils::PathChooser - QWidget -
utils/pathchooser.h
- 1 - - - - - diff --git a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp deleted file mode 100644 index f61feb9cf..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp +++ /dev/null @@ -1,371 +0,0 @@ -/** - ****************************************************************************** - * - * @file il2simulator.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief - * @see The GNU Public License (GPL) Version 3 - * @defgroup hitlplugin - * @{ - * - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -/* - * Description of DeviceLink Protocol: - * A Request is initiated with R/ followed by id's of to be requested settings - * even id's indicate read only values, odd are write only - * (usually id =get value id+1= set - for same setting) - * id's are separated by / - * requests can contain values to set, or to select a subsystem - * values are separated by \ - * example: R/30/48/64\0/64\1/ - * request read only settings 30,48 and 64 with parameters 0 and 1 - * the answer consists of an A followed by id value pairs in the same format - * example: A/30\0/48\0/64\0\22/64\1\102/ - * - * A full protocol description as well as a list of ID's and their meanings - * can be found shipped with IL2 in the file DeviceLink.txt - * - * id's used in this file: - * 30: IAS in km/h (float) - * 32: vario in m/s (float) - * 38: angular speed °/s (float) (which direction? azimuth?) - * 40: barometric alt in m (float) - * 42: flight course in ° (0-360) (float) - * 46: roll angle in ° (-180 - 180) (floatniguration) - * 48: pitch angle in ° (-90 - 90) (float) - * 80/81: engine power (-1.0 (0%) - 1.0 (100%)) (float) - * 84/85: aileron servo (-1.0 - 1.0) (float) - * 86/87: elevator servo (-1.0 - 1.0) (float) - * 88/89: rudder servo (-1.0 - 1.0) (float) - * - * IL2 currently offers no useful way of providing GPS data - * therefore fake GPS data will be calculated using IMS - * - * unfortunately angular acceleration provided is very limited, too - */ - - -#include "il2simulator.h" -#include "extensionsystem/pluginmanager.h" -#include -#include -#include -#include - -const float IL2Simulator::FT2M = 0.3048; -const float IL2Simulator::KT2MPS = 0.514444444; -const float IL2Simulator::MPS2KMH = 3.6; -const float IL2Simulator::KMH2MPS = (1.0/3.6); -const float IL2Simulator::INHG2KPA = 3.386; -const float IL2Simulator::RAD2DEG = (180.0/M_PI); -const float IL2Simulator::DEG2RAD = (M_PI/180.0); -const float IL2Simulator::M2DEG = 60.*1852.; // 60 miles per degree times 1852 meters per mile -const float IL2Simulator::DEG2M = (1.0/(60.*1852.)); -const float IL2Simulator::AIR_CONST = 287.058; // J/(kg*K) -const float IL2Simulator::GROUNDDENSITY = 1.225; // kg/m³ ;) -const float IL2Simulator::TEMP_GROUND = (15.0 + 273.0); // 15°C in Kelvin -const float IL2Simulator::TEMP_LAPSE_RATE = -0.0065; //degrees per meter -const float IL2Simulator::AIR_CONST_FACTOR = -0.0341631947363104; //several nature constants calculated into one - -IL2Simulator::IL2Simulator(const SimulatorSettings& params) : - Simulator(params) -{ - -} - - -IL2Simulator::~IL2Simulator() -{ -} - -void IL2Simulator::setupUdpPorts(const QString& host, int inPort, int outPort) -{ - inSocket->connectToHost(host,inPort); // IL2 - if(!inSocket->waitForConnected()) - qxtLog->error(Name() + " cann't connect to UDP Port: " + QString::number(inPort)); - -} - -void IL2Simulator::transmitUpdate() -{ - // Read ActuatorDesired from autopilot - ActuatorDesired::DataFields actData = actDesired->getData(); - float ailerons = actData.Roll; - float elevator = actData.Pitch; - float rudder = actData.Yaw; - float throttle = actData.Throttle*2-1.0; - - // Send update to Il2 - QString cmd; - cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") - .arg(throttle) - .arg(ailerons) - .arg(elevator) - .arg(rudder); - QByteArray data = cmd.toAscii(); - //outSocket->write(data); - inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! -} - - -/** - * calculate air density from altitude - */ -float IL2Simulator::DENSITY(float alt) { - return (GROUNDDENSITY * pow( - ((TEMP_GROUND+(TEMP_LAPSE_RATE*alt))/TEMP_GROUND), - ((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) ) - ); -} - -/** - * calculate air pressure from altitude - */ -float IL2Simulator::PRESSURE(float alt) { - return DENSITY(alt)*(TEMP_GROUND+(alt*TEMP_LAPSE_RATE))*AIR_CONST; - -} - -/** - * calculate TAS from IAS and altitude - */ -float IL2Simulator::TAS(float IAS, float alt) { - return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt))); -} - -/** - * process data string from flight simulator - */ -void IL2Simulator::processUpdate(const QByteArray& inp) -{ - // save old flight data to calculate delta's later - old=current; - QString data(inp); - // Split - QStringList fields = data.split("/"); - - // split up response string - int t; - for (t=0; t=2) { - int id = values[0].toInt(); - float value = values[1].toFloat(); - switch (id) { - case 30: - current.ias=value * KMH2MPS; - break; - case 32: - current.dZ=value; - break; - case 40: - current.Z=value; - break; - case 42: - current.azimuth=value; - break; - case 46: - current.roll=-value; - break; - case 48: - current.pitch=value; - break; - } - } - } - - // measure time - current.dT = ((float)time->restart()) / 1000.0; - if (current.dT<0.001) current.dT=0.001; - current.T = old.T+current.dT; - current.i = old.i+1; - if (current.i==1) { - old.dRoll=0; - old.dPitch=0; - old.dAzimuth=0; - old.ddX=0; - old.ddX=0; - old.ddX=0; - } - - // calculate TAS from alt and IAS - current.tas = TAS(current.ias,current.Z); - - // assume the plane actually flies straight and no wind - // groundspeed is horizontal vector of TAS - current.groundspeed = current.tas*cos(current.pitch*DEG2RAD); - // x and y vector components - current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD); - current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD); - - // simple IMS - integration over time the easy way... - current.X = old.X + (current.dX*current.dT); - current.Y = old.Y + (current.dY*current.dT); - - // accelerations (filtered) - if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0; - if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0; - if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0; -#define SPEED_FILTER 10 - current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1); - current.ddY = ((current.dY-old.dY)/current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1); - current.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1); - -#define TURN_FILTER 10 - // turn speeds (filtered) - if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0; - if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0; - if (isnan(old.dRoll) || isinf(old.dRoll)) old.dRoll=0; - current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1); - current.dPitch = (angleDifference(current.pitch,old.pitch)/current.dT + TURN_FILTER * (old.dPitch)) / (TURN_FILTER+1); - current.dRoll = (angleDifference(current.roll,old.roll)/current.dT + TURN_FILTER * (old.dRoll)) / (TURN_FILTER+1); - // Update AltitudeActual object - BaroAltitude::DataFields altActualData; - memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); - altActualData.Altitude = current.Z; - altActualData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0; - altActualData.Pressure = PRESSURE(current.Z)/1000.0; // kpa - - // Update attActual object - AttitudeActual::DataFields attActualData; - memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); - attActualData.Roll = current.roll; - attActualData.Pitch = current.pitch; - attActualData.Yaw = current.azimuth; - float rpy[3]; - float quat[4]; - rpy[0]=current.roll; - rpy[1]=current.pitch; - rpy[2]=current.azimuth; - Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); - attActualData.q1 = quat[0]; - attActualData.q2 = quat[1]; - attActualData.q3 = quat[2]; - attActualData.q4 = quat[3]; - - // Update positionActual objects - PositionActual::DataFields posData; - memset(&posData, 0, sizeof(PositionActual::DataFields)); - posData.North = current.Y; - posData.East = current.X; - posData.Down = current.Z*-1.; - - // Update velocityActual objects - VelocityActual::DataFields velData; - memset(&velData, 0, sizeof(VelocityActual::DataFields)); - velData.North = current.dY; - velData.East = current.dX; - velData.Down = current.dZ*-1.; - - // Update AttitudeRaw object (filtered gyros and accels only for now) - //AttitudeRaw::DataFields rawData; - //memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); - //rawData = attRaw->getData(); - Gyros::DataFields gyroData; - Accels::DataFields accelData; - memset(&gyroData, 0, sizeof(Gyros::DataFields)); - memset(&accelData, 0, sizeof(Accels::DataFields)); - gyroData = gyros->getData(); - accelData = accels->getData(); - - // rotate turn rates and accelerations into body frame - // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!) - gyroData.x = current.dRoll; - gyroData.y = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth; - gyroData.z = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch; - // accels are in NED and can be converted using standard ned->local rotation matrix - float Rbe[3][3]; - Utils::CoordinateConversions().Quaternion2R(quat,Rbe); - accelData.x = current.ddX*Rbe[0][0] - +current.ddY*Rbe[0][1] - +(current.ddZ+GEE)*Rbe[0][2]; - accelData.y = current.ddX*Rbe[1][0] - +current.ddY*Rbe[1][1] - +(current.ddZ+GEE)*Rbe[1][2]; - accelData.z = - (current.ddX*Rbe[2][0] - +current.ddY*Rbe[2][1] - +(current.ddZ+GEE)*Rbe[2][2]); - - // Update homelocation - HomeLocation::DataFields homeData; - memset(&homeData, 0, sizeof(HomeLocation::DataFields)); - homeData = posHome->getData(); - homeData.Latitude = settings.latitude.toFloat() * 10e6; - homeData.Longitude = settings.longitude.toFloat() * 10e6; - homeData.Altitude = 0; - double LLA[3]; - LLA[0]=settings.latitude.toFloat(); - LLA[1]=settings.longitude.toFloat(); - LLA[2]=0; - double ECEF[3]; - double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - homeData.Be[0]=0; - homeData.Be[1]=0; - homeData.Be[2]=0; - homeData.Set=1; - - // Update gps objects - GPSPosition::DataFields gpsData; - memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); - gpsData.Altitude = current.Z; - gpsData.Heading = current.azimuth; - gpsData.Groundspeed = current.groundspeed; - double NED[3]; - NED[0] = current.Y; - NED[1] = current.X; - NED[2] = -current.Z; - Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA); - gpsData.Latitude = LLA[0] * 10e6; - gpsData.Longitude = LLA[1] * 10e6; - gpsData.Satellites = 7; - gpsData.Status = GPSPosition::STATUS_FIX3D; - - // issue manual update - // update every time (50ms) - attActual->setData(attActualData); - //attActual->updated(); - //attRaw->setData(rawData); - gyros->setData(gyroData); - accels->setData(accelData); - //attRaw->updated(); - velActual->setData(velData); - //velActual->updated(); - posActual->setData(posData); - //posActual->updated(); - altActual->setData(altActualData); - //altActual->updated(); - gpsPos->setData(gpsData); - //gpsPos->updated(); - posHome->setData(homeData); - //posHome->updated(); -} - -/** - * process data string from flight simulator - */ -float IL2Simulator::angleDifference(float a, float b) -{ - float d=a-b; - if (d>180) d-=360; - if (d<-180)d+=360; - return d; -} diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp deleted file mode 100644 index 2cb7f9ae8..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp +++ /dev/null @@ -1,298 +0,0 @@ -/** - ****************************************************************************** - * - * @file simulator.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITL Plugin - * @{ - * @brief The Hardware In The Loop plugin - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - -#include "simulator.h" -#include "qxtlogger.h" -#include "extensionsystem/pluginmanager.h" -#include "coreplugin/icore.h" -#include "coreplugin/threadmanager.h" - -volatile bool Simulator::isStarted = false; - -const float Simulator::GEE = 9.81; -const float Simulator::FT2M = 0.3048; -const float Simulator::KT2MPS = 0.514444444; -const float Simulator::INHG2KPA = 3.386; -const float Simulator::FPS2CMPS = 30.48; -const float Simulator::DEG2RAD = (M_PI/180.0); - - -Simulator::Simulator(const SimulatorSettings& params) : - simProcess(NULL), - time(NULL), - inSocket(NULL), - outSocket(NULL), - settings(params), - updatePeriod(50), - simTimeout(8000), - autopilotConnectionStatus(false), - simConnectionStatus(false), - txTimer(NULL), - simTimer(NULL), - name("") -{ - // move to thread - moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); - connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection); - emit myStart(); -} - -Simulator::~Simulator() -{ - if(inSocket) - { - delete inSocket; - inSocket = NULL; - } - - if(outSocket) - { - delete outSocket; - outSocket = NULL; - } - - if(txTimer) - { - delete txTimer; - txTimer = NULL; - } - - if(simTimer) - { - delete simTimer; - simTimer = NULL; - } - // NOTE: Does not currently work, may need to send control+c to through the terminal - if (simProcess != NULL) - { - //connect(simProcess,SIGNAL(finished(int, QProcess::ExitStatus)),this,SLOT(onFinished(int, QProcess::ExitStatus))); - - simProcess->disconnect(); - if(simProcess->state() == QProcess::Running) - simProcess->kill(); - //if(simProcess->waitForFinished()) - //emit deleteSimProcess(); - delete simProcess; - simProcess = NULL; - } -} - -void Simulator::onDeleteSimulator(void) -{ - // [1] - Simulator::setStarted(false); - // [2] - Simulator::Instances().removeOne(simulatorId); - - disconnect(this); - delete this; -} - -void Simulator::onStart() -{ - QMutexLocker locker(&lock); - - QThread* mainThread = QThread::currentThread(); - qDebug() << "Simulator Thread: "<< mainThread; - - // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); - actDesired = ActuatorDesired::GetInstance(objManager); - manCtrlCommand = ManualControlCommand::GetInstance(objManager); - flightStatus = FlightStatus::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - velActual = VelocityActual::GetInstance(objManager); - posActual = PositionActual::GetInstance(objManager); - altActual = BaroAltitude::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - telStats = GCSTelemetryStats::GetInstance(objManager); - - // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); - connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - //connect(telStats, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(telStatsUpdated(UAVObject*))); - - // If already connect setup autopilot - GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - onAutopilotConnect(); - - inSocket = new QUdpSocket(); - outSocket = new QUdpSocket(); - setupUdpPorts(settings.hostAddress,settings.inPort,settings.outPort); - - emit processOutput("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteHostAddress + "\n" + \ - "inputPort: " + QString::number(settings.inPort) + "\n" + \ - "outputPort: " + QString::number(settings.outPort) + "\n"); - - qxtLog->info("\nLocal interface: " + settings.hostAddress + "\n" + \ - "Remote interface: " + settings.remoteHostAddress + "\n" + \ - "inputPort: " + QString::number(settings.inPort) + "\n" + \ - "outputPort: " + QString::number(settings.outPort) + "\n"); - -// if(!inSocket->waitForConnected(5000)) -// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.inPort)); -// outSocket->connectToHost(settings.hostAddress,settings.outPort); // FG -// if(!outSocket->waitForConnected(5000)) -// emit processOutput(QString("Can't connect to %1 on %2 port!").arg(settings.hostAddress).arg(settings.outPort)); - - - connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate()),Qt::DirectConnection); - - // Setup transmit timer - txTimer = new QTimer(); - connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate()),Qt::DirectConnection); - txTimer->setInterval(updatePeriod); - txTimer->start(); - // Setup simulator connection timer - simTimer = new QTimer(); - connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout()),Qt::DirectConnection); - simTimer->setInterval(simTimeout); - simTimer->start(); - - // setup time - time = new QTime(); - time->start(); - current.T=0; - current.i=0; - -} - -void Simulator::receiveUpdate() -{ - // Update connection timer and status - simTimer->setInterval(simTimeout); - simTimer->stop(); - simTimer->start(); - if ( !simConnectionStatus ) - { - simConnectionStatus = true; - emit simulatorConnected(); - } - - // Process data - while(inSocket->hasPendingDatagrams()) { - // Receive datagram - QByteArray datagram; - datagram.resize(inSocket->pendingDatagramSize()); - QHostAddress sender; - quint16 senderPort; - inSocket->readDatagram(datagram.data(), datagram.size(), - &sender, &senderPort); - //QString datastr(datagram); - // Process incomming data - processUpdate(datagram); - } -} - -void Simulator::setupObjects() -{ - setupInputObject(actDesired, 100); - setupOutputObject(altActual, 250); - setupOutputObject(attActual, 10); - //setupOutputObject(attActual, 100); - setupOutputObject(gpsPos, 250); - setupOutputObject(posActual, 250); - setupOutputObject(velActual, 250); - setupOutputObject(posHome, 1000); - setupOutputObject(accels, 10); - setupOutputObject(gyros, 10); - //setupOutputObject(attRaw, 100); - - - -} - -void Simulator::setupInputObject(UAVObject* obj, int updatePeriod) -{ - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); - UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); - UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); - UAVObject::SetFlightTelemetryAcked(mdata, false); - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = updatePeriod; - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); - obj->setMetadata(mdata); -} - -void Simulator::setupOutputObject(UAVObject* obj, int updatePeriod) -{ - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); - UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); - UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); - UAVObject::SetFlightTelemetryUpdateMode(mdata,UAVObject::UPDATEMODE_MANUAL); - UAVObject::SetGcsTelemetryAcked(mdata, false); - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.gcsTelemetryUpdatePeriod = updatePeriod; - obj->setMetadata(mdata); -} - -void Simulator::onAutopilotConnect() -{ - autopilotConnectionStatus = true; - setupObjects(); - emit autopilotConnected(); -} - -void Simulator::onAutopilotDisconnect() -{ - autopilotConnectionStatus = false; - emit autopilotDisconnected(); -} - -void Simulator::onSimulatorConnectionTimeout() -{ - if ( simConnectionStatus ) - { - simConnectionStatus = false; - emit simulatorDisconnected(); - } -} - - -void Simulator::telStatsUpdated(UAVObject* obj) -{ - GCSTelemetryStats::DataFields stats = telStats->getData(); - if ( !autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED ) - { - onAutopilotConnect(); - } - else if ( autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED ) - { - onAutopilotDisconnect(); - } -} - diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h deleted file mode 100644 index b345f0a5e..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h +++ /dev/null @@ -1,231 +0,0 @@ -/** - ****************************************************************************** - * - * @file simulator.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITL Plugin - * @{ - * @brief The Hardware In The Loop plugin - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef ISIMULATOR_H -#define ISIMULATOR_H - -#include -#include -#include -#include -#include "qscopedpointer.h" -#include "uavtalk/telemetrymanager.h" -#include "uavobjectmanager.h" -#include "actuatordesired.h" -#include "manualcontrolcommand.h" -// #include "altitudeactual.h" -#include "positionactual.h" -#include "velocityactual.h" -#include "baroaltitude.h" -#include "attitudeactual.h" -#include "gpsposition.h" -#include "homelocation.h" -#include "accels.h" -#include "gyros.h" -#include "gcstelemetrystats.h" -#include "flightstatus.h" - -#include "utils/coordinateconversions.h" - -/** - * just imagine this was a class without methods and all public properties - */ - typedef struct _FLIGHT_PARAM { - - // time - float T; - float dT; - unsigned int i; - - // speed (relative) - float ias; - float tas; - float groundspeed; - - // position (absolute) - float X; - float Y; - float Z; - - // speed (absolute) - float dX; - float dY; - float dZ; - - // acceleration (absolute) - float ddX; - float ddY; - float ddZ; - - //angle - float azimuth; - float pitch; - float roll; - - //rotation speed - float dAzimuth; - float dPitch; - float dRoll; - -} FLIGHT_PARAM; - -typedef struct _CONNECTION -{ - QString simulatorId; - QString binPath; - QString dataPath; - QString hostAddress; - QString remoteHostAddress; - int outPort; - int inPort; - bool manual; - bool startSim; - QString latitude; - QString longitude; -} SimulatorSettings; - -class Simulator : public QObject -{ - Q_OBJECT - -public: - Simulator(const SimulatorSettings& params); - virtual ~Simulator(); - - bool isAutopilotConnected() const { return autopilotConnectionStatus; } - bool isSimulatorConnected() const { return simConnectionStatus; } - QString Name() const { return name; } - void setName(QString str) { name = str; } - - QString SimulatorId() const { return simulatorId; } - void setSimulatorId(QString str) { simulatorId = str; } - - - - static bool IsStarted() { return isStarted; } - static void setStarted(bool val) { isStarted = val; } - static QStringList& Instances() { return Simulator::instances; } - static void setInstance(const QString& str) { Simulator::instances.append(str); } - - virtual void stopProcess() {} - virtual void setupUdpPorts(const QString& host, int inPort, int outPort) { Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)} - -signals: - void autopilotConnected(); - void autopilotDisconnected(); - void simulatorConnected(); - void simulatorDisconnected(); - void processOutput(QString str); - void deleteSimProcess(); - void myStart(); -public slots: - Q_INVOKABLE virtual bool setupProcess() { return true;} -private slots: - void onStart(); - //void transmitUpdate(); - void receiveUpdate(); - void onAutopilotConnect(); - void onAutopilotDisconnect(); - void onSimulatorConnectionTimeout(); - void telStatsUpdated(UAVObject* obj); - Q_INVOKABLE void onDeleteSimulator(void); - - virtual void transmitUpdate() = 0; - virtual void processUpdate(const QByteArray& data) = 0; - -protected: - static const float GEE; - static const float FT2M; - static const float KT2MPS; - static const float INHG2KPA; - static const float FPS2CMPS; - static const float DEG2RAD; - - QProcess* simProcess; - QTime* time; - QUdpSocket* inSocket;//(new QUdpSocket()); - QUdpSocket* outSocket; - - ActuatorDesired* actDesired; - ManualControlCommand* manCtrlCommand; - FlightStatus* flightStatus; - BaroAltitude* altActual; - AttitudeActual* attActual; - VelocityActual* velActual; - PositionActual* posActual; - HomeLocation* posHome; - Accels* accels; - Gyros* gyros; - GPSPosition* gpsPos; - GCSTelemetryStats* telStats; - - SimulatorSettings settings; - - FLIGHT_PARAM current; - FLIGHT_PARAM old; - QMutex lock; - -private: - - int updatePeriod; - int simTimeout; - volatile bool autopilotConnectionStatus; - volatile bool simConnectionStatus; - QTimer* txTimer; - QTimer* simTimer; - QString name; - QString simulatorId; - volatile static bool isStarted; - static QStringList instances; - //QList > requiredUAVObjects; - void setupOutputObject(UAVObject* obj, int updatePeriod); - void setupInputObject(UAVObject* obj, int updatePeriod); - void setupObjects(); -}; - - - -class SimulatorCreator -{ -public: - SimulatorCreator(QString id, QString descr) : - classId(id), - description(descr) - {} - virtual ~SimulatorCreator() {} - - QString ClassId() const {return classId;} - QString Description() const {return description;} - - virtual Simulator* createSimulator(const SimulatorSettings& params) = 0; - -private: - QString classId; - QString description; -}; - -#endif // ISIMULATOR_H diff --git a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp deleted file mode 100644 index 1546b862e..000000000 --- a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp +++ /dev/null @@ -1,436 +0,0 @@ -/** - ****************************************************************************** - * - * @file xplanesimulator.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief - * @see The GNU Public License (GPL) Version 3 - * @defgroup hitlplugin - * @{ - * - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - /** - * Description of X-Plane Protocol: - * - * To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu - * (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set. - * Data Set shown all X-Plane params, - * each row has four checkbox: 1st check - out to UDP; 4 check - show on screen - * All the UDP messages for X-Plane have the same format, which is: - * 5-character MESSAGE PROLOUGE (to indicate the type of message) - * and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive) - * - * DATA INPUT/OUTPUT STRUCTURE is the following stuct: - * - * struct data_struct - * { - * int index; // data index, the index into the list of variables - // you can output from the Data Output screen in X-Plane. - * float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection.. - // many outputs do not use all 8, though. - * }; - * - * For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set) - * bytes value description - * [0-3] DATA message type - * [4] none no matter - * [5-8] 11 code of setting param(row in Data Set) - * [9-41] data message data (8 float values) - * total size: 41 byte - * - */ - -#include "xplanesimulator.h" -#include "extensionsystem/pluginmanager.h" -#include -#include -#include -#include - -void TraceBuf(const char* buf,int len); - -XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : - Simulator(params) -{ - once = false; -} - - -XplaneSimulator::~XplaneSimulator() -{ -} - -void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) -{ - inSocket->bind(QHostAddress(host), inPort); - //outSocket->bind(QHostAddress(host), outPort); - once = false; - -} - -bool XplaneSimulator::setupProcess() -{ - emit processOutput(QString("Please start X-Plane manually, and make sure it is setup to output its ") + - "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort)); - return true; - -} - -/** - * update data in X-Plane simulator - */ -void XplaneSimulator::transmitUpdate() -{ - //Read ActuatorDesired from autopilot - ActuatorDesired::DataFields actData = actDesired->getData(); - float ailerons = actData.Roll; - float elevator = actData.Pitch; - float rudder = actData.Yaw; - float throttle = actData.Throttle*2-1.0; - float none = -999; - //quint32 none = *((quint32*)&tmp); // get float as 4 bytes - - quint32 code; - QByteArray buf; - QDataStream stream(&buf,QIODevice::ReadWrite); - - // !!! LAN byte order - Big Endian - #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN - stream.setByteOrder(QDataStream::LittleEndian); - #endif - - // 11th data settings (flight con: ail/elv/rud) - buf.clear(); - code = 11; - //quint8 header[] = "DATA"; - /* - stream << *((quint32*)header) << - (quint8)0x30 << - code << - *((quint32*)&elevator) << - *((quint32*)&ailerons) << - *((quint32*)&rudder) << - none << - *((quint32*)&ailerons) << - none << - none << - none; - */ - buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&elevator), sizeof(elevator)); - buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&rudder), sizeof(rudder)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - TraceBuf(buf.data(),41); - - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) - { - emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); - } - //outSocket->write(buf); - - // 25th data settings (throttle command) - buf.clear(); - code = 25; - //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none - // << none << none << none << none << none; - buf.append("DATA0"); - buf.append(reinterpret_cast(&code), sizeof(code)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&throttle), sizeof(throttle)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - buf.append(reinterpret_cast(&none), sizeof(none)); - - if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) - { - emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n"); - } - - //outSocket->write(buf); - - - - /** !!! this settings was given from ardupilot X-Plane.pl, I comment them - but if it needed comment should be removed !!! - - // 8th data settings (joystick 1 ail/elv/rud) - stream << "DATA0" << quint32(11) << elevator << ailerons << rudder - << float(-999) << float(-999) << float(-999) << float(-999) << float(-999); - outSocket->write(buf); - */ - -} - -/** - * process data string from X-Plane simulator - */ -void XplaneSimulator::processUpdate(const QByteArray& dataBuf) -{ - float altitude = 0; - float latitude = 0; - float longitude = 0; - float airspeed = 0; - float speed = 0; - float pitch = 0; - float roll = 0; - float heading = 0; - float pressure = 0; - float temperature = 0; - float velX = 0; - float velY = 0; - float velZ = 0; - float dstX = 0; - float dstY = 0; - float dstZ = 0; - float accX = 0; - float accY = 0; - float accZ = 0; - float rollRate_rad=0; - float pitchRate_rad=0; - float yawRate_rad=0; - - QString str; - QByteArray& buf = const_cast(dataBuf); - QString data(buf); - - if(data.left(4) == "DATA") // check type of packet - { - buf.remove(0,5); - if(dataBuf.size() % 36) - { - qxtLog->info("incorrect length of UDP packet: ",buf); - return; // incorrect length of struct - } - // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 - int channelCounter = dataBuf.size() / 36; - do - { - switch(buf[0]) // switch by id - { - case XplaneSimulator::LatitudeLongitude: - latitude = *((float*)(buf.data()+4*1)); - longitude = *((float*)(buf.data()+4*2)); - altitude = *((float*)(buf.data()+4*3))* FT2M; - break; - - case XplaneSimulator::Speed: - airspeed = *((float*)(buf.data()+4*7)); - speed = *((float*)(buf.data()+4*8)); - break; - - case XplaneSimulator::PitchRollHeading: - pitch = *((float*)(buf.data()+4*1)); - roll = *((float*)(buf.data()+4*2)); - heading = *((float*)(buf.data()+4*3)); - break; - - /* - case XplaneSimulator::SystemPressures: - pressure = *((float*)(buf.data()+4*1)); - break; - */ - - case XplaneSimulator::AtmosphereWeather: - pressure = *((float*)(buf.data()+4*1)) * INHG2KPA; - temperature = *((float*)(buf.data()+4*2)); - break; - - case XplaneSimulator::LocVelDistTraveled: - dstX = *((float*)(buf.data()+4*1)); - dstY = - *((float*)(buf.data()+4*3)); - dstZ = *((float*)(buf.data()+4*2)); - velX = *((float*)(buf.data()+4*4)); - velY = - *((float*)(buf.data()+4*6)); - velZ = *((float*)(buf.data()+4*5)); - break; - - case XplaneSimulator::AngularVelocities: //In [rad/s] - pitchRate_rad = *((float*)(buf.data()+4*1)); - rollRate_rad = *((float*)(buf.data()+4*2)); - yawRate_rad = *((float*)(buf.data()+4*3)); - break; - - case XplaneSimulator::Gload: - accX = *((float*)(buf.data()+4*6)) * GEE; - accY = *((float*)(buf.data()+4*7)) * GEE; - accZ = *((float*)(buf.data()+4*5)) * GEE; - break; - - default: - break; - } - channelCounter--; - buf.remove(0,36); - } while (channelCounter); - - - HomeLocation::DataFields homeData = posHome->getData(); - if(!once) - { - // Upon startup, we reset the HomeLocation object to - // the plane's location: - memset(&homeData, 0, sizeof(HomeLocation::DataFields)); - // Update homelocation - homeData.Latitude = latitude * 1e7; - homeData.Longitude = longitude * 1e7; - homeData.Altitude = altitude; - double LLA[3]; - LLA[0]=latitude; - LLA[1]=longitude; - LLA[2]=altitude; - double ECEF[3]; - double RNE[9]; - Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE); - Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF); - homeData.Be[0]=0; - homeData.Be[1]=0; - homeData.Be[2]=0; - posHome->setData(homeData); - posHome->updated(); - - // Initialize the initial distance - initX = dstX; - initY = dstY; - initZ = dstZ; - once=1; - } - - - // Update AltitudeActual object - BaroAltitude::DataFields altActualData; - memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); - altActualData.Altitude = altitude; - altActualData.Temperature = temperature; - altActualData.Pressure = pressure; - altActual->setData(altActualData); - - // Update attActual object - AttitudeActual::DataFields attActualData; - memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); - attActualData.Roll = roll; //roll; - attActualData.Pitch = pitch; // pitch - attActualData.Yaw = heading; // Yaw - float rpy[3]; - float quat[4]; - rpy[0] = roll; - rpy[1] = pitch; - rpy[2] = heading; - Utils::CoordinateConversions().RPY2Quaternion(rpy,quat); - attActualData.q1 = quat[0]; - attActualData.q2 = quat[1]; - attActualData.q3 = quat[2]; - attActualData.q4 = quat[3]; - attActual->setData(attActualData); - - // Update gps objects - GPSPosition::DataFields gpsData; - memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); - gpsData.Altitude = altitude; - gpsData.Heading = heading; - gpsData.Groundspeed = speed; - gpsData.Latitude = latitude*1e7; - gpsData.Longitude = longitude*1e7; - gpsData.Satellites = 10; - gpsData.Status = GPSPosition::STATUS_FIX3D; - gpsPos->setData(gpsData); - - // Update VelocityActual.{Nort,East,Down} - VelocityActual::DataFields velocityActualData; - memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); - velocityActualData.North = velY; - velocityActualData.East = velX; - velocityActualData.Down = -velZ; - velActual->setData(velocityActualData); - - // Update PositionActual.{Nort,East,Down} - PositionActual::DataFields positionActualData; - memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); - positionActualData.North = (dstY-initY); - positionActualData.East = (dstX-initX); - positionActualData.Down = -(dstZ-initZ); - posActual->setData(positionActualData); - - // Update AttitudeRaw object (filtered gyros only for now) - //AttitudeRaw::DataFields rawData; - //memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); - //rawData = attRaw->getData(); - //rawData.gyros[0] = rollRate; - //rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate_rad + sin(DEG2RAD * roll) * yawRate_rad; - //rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate_rad - sin(DEG2RAD * roll) * pitchRate_rad; - //rawData.gyros[1] = pitchRate; - //rawData.gyros[2] = yawRate; - //rawData.accels[0] = accX; - //rawData.accels[1] = accY; - //rawData.accels[2] = -accZ; - //attRaw->setData(rawData); - Gyros::DataFields gyroData; - memset(&gyroData, 0, sizeof(Gyros::DataFields)); -#define Pi 3.141529654 - gyroData.x = rollRate_rad*180/Pi; - gyroData.y = pitchRate_rad*180/Pi; - gyroData.z = yawRate_rad*180/Pi; - gyros->setData(gyroData); - - Accels::DataFields accelData; - memset(&accelData, 0, sizeof(Accels::DataFields)); - accelData.x = accX; - accelData.y = accY; - accelData.z = -accZ; - accels->setData(accelData); - - } - // issue manual update - //attActual->updated(); - //altActual->updated(); - //posActual->updated(); -} - - -void TraceBuf(const char* buf,int len) -{ - QString str; - bool reminder=true; - for(int i=0; i < len; i++) - { - if(!(i%16)) - { - if(i>0) - { -// qDebug() << str; - str.clear(); - reminder=false; - } - reminder=true; - } - str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0')); - } - - if(reminder){ -// qDebug() << str; - } -} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp deleted file mode 100644 index f8306e30d..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp +++ /dev/null @@ -1,495 +0,0 @@ -/** - ****************************************************************************** - * - * @file aerosimrc.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "aerosimrc.h" -#include -#include -#include - -AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms) - : Simulator(params) -{ - udpCounterASrecv = 0; -} - -AeroSimRCSimulator::~AeroSimRCSimulator() -{ -} - -bool AeroSimRCSimulator::setupProcess() -{ - QMutexLocker locker(&lock); - return true; -} - -void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort) -{ - Q_UNUSED(outPort) - if (inSocket->bind(QHostAddress(host), inPort)) - emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n"); - else - emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n"); -} - -void AeroSimRCSimulator::transmitUpdate() -{ - // read actuator output - ActuatorCommand::DataFields actCmdData; - actCmdData = actCommand->getData(); - float channels[10]; - for (int i = 0; i < 10; ++i) { - qint16 ch = actCmdData.Channel[i]; - float out = -1.0; - if (ch >= 1000 && ch <= 2000) { - ch -= 1000; - out = ((float)ch / 500.0) - 1.0; - } - channels[i] = out; - } - - // read flight status - FlightStatus::DataFields flightData; - flightData = flightStatus->getData(); - quint8 armed; - quint8 mode; - armed = flightData.Armed; - mode = flightData.FlightMode; - - QByteArray data; - // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32) - data.resize(50); - QDataStream stream(&data, QIODevice::WriteOnly); - stream.setFloatingPointPrecision(QDataStream::SinglePrecision); - stream << quint32(0x52434D44); // magic header, "RCMD" - for (int i = 0; i < 10; ++i) - stream << channels[i]; // channels - stream << armed << mode; // flight status - stream << udpCounterASrecv; // packet counter - - if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) { - qDebug() << "write failed: " << outSocket->errorString(); - } - -#ifdef DBG_TIMERS - static int cntTX = 0; - if (cntTX >= 100) { - qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart(); - cntTX = 0; - } else { - ++cntTX; - } -#endif -} - -void AeroSimRCSimulator::processUpdate(const QByteArray &data) -{ - // check size - if (data.size() > 188) { - qDebug() << "!!! big datagram: " << data.size(); - return; - } - - QByteArray buf = data; - QDataStream stream(&buf, QIODevice::ReadOnly); - stream.setFloatingPointPrecision(QDataStream::SinglePrecision); - - // check magic header - quint32 magic; - stream >> magic; - if (magic != 0x4153494D) { // "AERO" - qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D); - return; - } - - float timeStep, - homeX, homeY, homeZ, - WpHX, WpHY, WpLat, WpLon, - posX, posY, posZ, // world - velX, velY, velZ, // world - angX, angY, angZ, // model - accX, accY, accZ, // model - lat, lon, agl, // world - yaw, pitch, roll, // model - volt, curr, - rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix - ch[8]; - - stream >> timeStep; - stream >> homeX >> homeY >> homeZ; - stream >> WpHX >> WpHY >> WpLat >> WpLon; - stream >> posX >> posY >> posZ; - stream >> velX >> velY >> velZ; - stream >> angX >> angY >> angZ; - stream >> accX >> accY >> accZ; - stream >> lat >> lon >> agl; - stream >> yaw >> pitch >> roll; - stream >> volt >> curr; - stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz; - stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7]; - stream >> udpCounterASrecv; - - /**********************************************************************************************/ - QTime currentTime = QTime::currentTime(); - /**********************************************************************************************/ - static bool firstRun = true; - if (settings.homeLocation) { - if (firstRun) { - HomeLocation::DataFields homeData; - homeData = posHome->getData(); - - homeData.Latitude = WpLat * 10e6; - homeData.Longitude = WpLon * 10e6; - homeData.Altitude = homeZ; - homeData.Set = HomeLocation::SET_TRUE; - - posHome->setData(homeData); - - firstRun = false; - } - if (settings.homeLocRate > 0) { - static QTime homeLocTime = currentTime; - if (homeLocTime.secsTo(currentTime) >= settings.homeLocRate) { - firstRun = true; - homeLocTime = currentTime; - } - } - } - /**********************************************************************************************/ - if (settings.attRaw || settings.attActual) { - QMatrix4x4 mat; - mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix - ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z) - -uy, -ux, uz, 0.0, - 0.0, 0.0, 0.0, 1.0); - mat.optimize(); - - QQuaternion quat; // model quat - asMatrix2Quat(mat, quat); - - // rotate gravity - QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z) - QVector3D gee = QVector3D(0.0, 0.0, -GEE); - QQuaternion qWorld = quat.conjugate(); - gee = qWorld.rotatedVector(gee); - acc += gee; - - /*************************************************************************************/ - if (settings.attRaw) { - Accels::DataFields accelsData; - accelsData = accels->getData(); - Gyros::DataFields gyrosData; - gyrosData = gyros->getData(); - - gyrosData.x = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z) - gyrosData.y = angX * RAD2DEG; - gyrosData.z = angZ * -RAD2DEG; - accelsData.x = acc.x(); - accelsData.y = acc.y(); - accelsData.z = acc.z(); - - accels->setData(accelsData); - gyros->setData(gyrosData); - } - /*************************************************************************************/ - if (settings.attActHW) { - // do nothing - /*****************************************/ - } else if (settings.attActSim) { - // take all data from simulator - AttitudeActual::DataFields attActData; - attActData = attActual->getData(); - - QVector3D rpy; // model roll, pitch, yaw - asMatrix2RPY(mat, rpy); - - attActData.Roll = rpy.x(); - attActData.Pitch = rpy.y(); - attActData.Yaw = rpy.z(); - attActData.q1 = quat.scalar(); - attActData.q2 = quat.x(); - attActData.q3 = quat.y(); - attActData.q4 = quat.z(); - - attActual->setData(attActData); - /*****************************************/ - } else if (settings.attActCalc) { - // calculate RPY with code from Attitude module - AttitudeActual::DataFields attActData; - attActData = attActual->getData(); - - static float q[4] = {1, 0, 0, 0}; - static float gyro_correct_int2 = 0; - - float dT = timeStep; - - AttitudeSettings::DataFields attSettData = attSettings->getData(); - float accelKp = attSettData.AccelKp * 0.1666666666666667; - float accelKi = attSettData.AccelKp * 0.1666666666666667; - float yawBiasRate = attSettData.YawBiasRate; - - // calibrate sensors on arming - if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) { - accelKp = 2.0; - accelKi = 0.9; - } - - float gyro[3] = {angY * RAD2DEG, angX * RAD2DEG, angZ * -RAD2DEG}; - float attRawAcc[3] = {acc.x(), acc.y(), acc.z()}; - - // code from Attitude module begin /////////////////////////////// - float *accels = attRawAcc; - float grot[3]; - float accel_err[3]; - - // Rotate gravity to body frame and cross with accels - grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); - - // CrossProduct - { - accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2]; - accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2]; - accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1]; - } - - // Account for accel magnitude - float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]); - accel_err[0] /= accel_mag; - accel_err[1] /= accel_mag; - accel_err[2] /= accel_mag; - - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - gyro_correct_int2 += -gyro[2] * yawBiasRate; - - // Correct rates based on error, integral component dealt with in updateSensors - gyro[0] += accel_err[0] * accelKp / dT; - gyro[1] += accel_err[1] * accelKp / dT; - gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2; - - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; - qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; - qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; - qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; - - // Take a time step - q[0] += qdot[0]; - q[1] += qdot[1]; - q[2] += qdot[2]; - q[3] += qdot[3]; - - if(q[0] < 0) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } - - // Renomalize - float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); - q[0] /= qmag; - q[1] /= qmag; - q[2] /= qmag; - q[3] /= qmag; - - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if((fabs(qmag) < 1e-3) || (qmag != qmag)) { - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; - } - - float rpy2[3]; - // Quaternion2RPY - { - float q0s, q1s, q2s, q3s; - q0s = q[0] * q[0]; - q1s = q[1] * q[1]; - q2s = q[2] * q[2]; - q3s = q[3] * q[3]; - - float R13, R11, R12, R23, R33; - R13 = 2 * (q[1] * q[3] - q[0] * q[2]); - R11 = q0s + q1s - q2s - q3s; - R12 = 2 * (q[1] * q[2] + q[0] * q[3]); - R23 = 2 * (q[2] * q[3] + q[0] * q[1]); - R33 = q0s - q1s - q2s + q3s; - - rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 - rpy2[2] = RAD2DEG * atan2f(R12, R11); - rpy2[0] = RAD2DEG * atan2f(R23, R33); - } - - attActData.Roll = rpy2[0]; - attActData.Pitch = rpy2[1]; - attActData.Yaw = rpy2[2]; - attActData.q1 = q[0]; - attActData.q2 = q[1]; - attActData.q3 = q[2]; - attActData.q4 = q[3]; - attActual->setData(attActData); - /*****************************************/ - } - } - /**********************************************************************************************/ - if (settings.gcsReciever) { - static QTime gcsRcvrTime = currentTime; - if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) { - GCSReceiver::DataFields gcsRcvrData; - gcsRcvrData = gcsReceiver->getData(); - - for (int i = 0; i < 8; ++i) - gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500); - - gcsReceiver->setData(gcsRcvrData); - if (settings.manualOutput) - gcsRcvrTime = currentTime; - } - } else if (settings.manualControl) { - // not implemented yet - } - /**********************************************************************************************/ - if (settings.gpsPosition) { - static QTime gpsPosTime = currentTime; - if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) { - GPSPosition::DataFields gpsPosData; - gpsPosData = gpsPosition->getData(); - - gpsPosData.Altitude = posZ; - gpsPosData.Heading = yaw * RAD2DEG; - gpsPosData.Latitude = lat * 10e6; - gpsPosData.Longitude = lon * 10e6; - gpsPosData.Groundspeed = qSqrt(velX * velX + velY * velY); - gpsPosData.GeoidSeparation = 0.0; - gpsPosData.Satellites = 8; - gpsPosData.PDOP = 3.0; - gpsPosData.Status = GPSPosition::STATUS_FIX3D; - - gpsPosition->setData(gpsPosData); - gpsPosTime = currentTime; - } - } - /**********************************************************************************************/ - if (settings.sonarAltitude) { - static QTime sonarAltTime = currentTime; - if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) { - SonarAltitude::DataFields sonarAltData; - sonarAltData = sonarAlt->getData(); - - float sAlt = settings.sonarMaxAlt; - // 0.35 rad ~= 20 degree - if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) { - float x = agl * qTan(roll); - float y = agl * qTan(pitch); - float h = qSqrt(x*x + y*y + agl*agl); - sAlt = qMin(h, sAlt); - } - - sonarAltData.Altitude = sAlt; - sonarAlt->setData(sonarAltData); - sonarAltTime = currentTime; - } - } - /**********************************************************************************************/ -/* - BaroAltitude::DataFields altActData; - altActData = altActual->getData(); - altActData.Altitude = posZ; - altActual->setData(altActData); - - PositionActual::DataFields posActData; - posActData = posActual->getData(); - posActData.North = posY * 100; - posActData.East = posX * 100; - posActData.Down = posZ * -100; - posActual->setData(posActData); - - VelocityActual::DataFields velActData; - velActData = velActual->getData(); - velActData.North = velY * 100; - velActData.East = velX * 100; - velActData.Down = velZ * 100; - velActual->setData(velActData); -*/ - -#ifdef DBG_TIMERS - static int cntRX = 0; - if (cntRX >= 100) { - qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart(); - cntRX = 0; - } else { - ++cntRX; - } -#endif -} - -// transfomations - -void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q) -{ - qreal w, x, y, z; - - // w always >= 0 - w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0; - x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0; - y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0; - z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0; - - x = copysign(x, (m(1, 2) - m(2, 1))); - y = copysign(y, (m(2, 0) - m(0, 2))); - z = copysign(z, (m(0, 1) - m(1, 0))); - - q.setScalar(w); - q.setX(x); - q.setY(y); - q.setZ(z); -} - -void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy) -{ - qreal roll, pitch, yaw; - - if (qFabs(m(0, 2)) > 0.998) { - // ~86.3°, gimbal lock - roll = 0.0; - pitch = copysign(M_PI_2, -m(0, 2)); - yaw = qAtan2(-m(1, 0), m(1, 1)); - } else { - roll = qAtan2(m(1, 2), m(2, 2)); - pitch = qAsin(-m(0, 2)); - yaw = qAtan2(m(0, 1), m(0, 0)); - } - - rpy.setX(roll * RAD2DEG); - rpy.setY(pitch * RAD2DEG); - rpy.setZ(yaw * RAD2DEG); -} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec deleted file mode 100644 index 26fede385..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec +++ /dev/null @@ -1,12 +0,0 @@ - - The OpenPilot Project - (C) 2011 OpenPilot Project - The GNU Public License (GPL) Version 3 - Hardware In The Loop Simulation (v2) - http://www.openpilot.org - - - - - - diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri deleted file mode 100644 index 50268face..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri +++ /dev/null @@ -1,4 +0,0 @@ -include(../../plugins/uavobjects/uavobjects.pri) -include(../../plugins/uavtalk/uavtalk.pri) -#include(../../plugins/coreplugin/coreplugin.pri) -#include(../../libs/utils/utils.pri) diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp deleted file mode 100644 index 5977e5220..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2configuration.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlv2configuration.h" - -HITLConfiguration::HITLConfiguration(QString classId, - QSettings* qSettings, - QObject *parent) - : IUAVGadgetConfiguration(classId, parent) -{ -// qDebug() << "HITLV2Configuration"; - // default values - QString simulatorId = "ASimRC"; - QString hostAddress = "127.0.0.1"; - int inPort = 40100; - QString remoteAddress = "127.0.0.1"; - int outPort = 40200; - QString binPath = ""; - QString dataPath = ""; - - bool homeLocation = true; - quint16 homeLocRate = 0; - - bool attRaw = true; - quint8 attRawRate = 20; - - bool attActual = true; - bool attActHW = false; - bool attActSim = true; - bool attActCalc = false; - - bool sonarAltitude = false; - float sonarMaxAlt = 5.0; - quint16 sonarAltRate = 50; - - bool gpsPosition = true; - quint16 gpsPosRate = 200; - - bool inputCommand = true; - bool gcsReciever = true; - bool manualControl = false; - - bool manualOutput = false; - quint8 outputRate = 20; - - // if a saved configuration exists load it - if (qSettings != 0) { - settings.simulatorId = qSettings->value("simulatorId", simulatorId).toString(); - settings.hostAddress = qSettings->value("hostAddress", hostAddress).toString(); - settings.inPort = qSettings->value("inPort", inPort).toInt(); - settings.remoteAddress = qSettings->value("remoteAddress", remoteAddress).toString(); - settings.outPort = qSettings->value("outPort", outPort).toInt(); - settings.binPath = qSettings->value("binPath", binPath).toString(); - settings.dataPath = qSettings->value("dataPath", dataPath).toString(); - - settings.homeLocation = qSettings->value("homeLocation", homeLocation).toBool(); - settings.homeLocRate = qSettings->value("homeLocRate", homeLocRate).toInt(); - - settings.attRaw = qSettings->value("attRaw", attRaw).toBool(); - settings.attRawRate = qSettings->value("attRawRate", attRawRate).toInt(); - - settings.attActual = qSettings->value("attActual", attActual).toBool(); - settings.attActHW = qSettings->value("attActHW", attActHW).toBool(); - settings.attActSim = qSettings->value("attActSim", attActSim).toBool(); - settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool(); - - settings.sonarAltitude = qSettings->value("sonarAltitude", sonarAltitude).toBool(); - settings.sonarMaxAlt = qSettings->value("sonarMaxAlt", sonarMaxAlt).toFloat(); - settings.sonarAltRate = qSettings->value("sonarAltRate", sonarAltRate).toInt(); - - settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool(); - settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt(); - - settings.inputCommand = qSettings->value("inputCommand", inputCommand).toBool(); - settings.gcsReciever = qSettings->value("gcsReciever", gcsReciever).toBool(); - settings.manualControl = qSettings->value("manualControl", manualControl).toBool(); - settings.manualOutput = qSettings->value("manualOutput", manualOutput).toBool(); - settings.outputRate = qSettings->value("outputRate", outputRate).toInt(); - } else { - settings.simulatorId = simulatorId; - settings.hostAddress = hostAddress; - settings.inPort = inPort; - settings.remoteAddress = remoteAddress; - settings.outPort = outPort; - settings.binPath = binPath; - settings.dataPath = dataPath; - - settings.homeLocation = homeLocation; - settings.homeLocRate = homeLocRate; - - settings.attRaw = attRaw; - settings.attRawRate = attRawRate; - - settings.attActual = attActual; - settings.attActHW = attActHW; - settings.attActSim = attActSim; - settings.attActCalc = attActCalc; - - settings.sonarAltitude = sonarAltitude; - settings.sonarMaxAlt = sonarMaxAlt; - settings.sonarAltRate = sonarAltRate; - - settings.gpsPosition = gpsPosition; - settings.gpsPosRate = gpsPosRate; - - settings.inputCommand = inputCommand; - settings.gcsReciever = gcsReciever; - settings.manualControl = manualControl; - settings.manualOutput = manualOutput; - settings.outputRate = outputRate; - } -} - -IUAVGadgetConfiguration *HITLConfiguration::clone() -{ - HITLConfiguration *m = new HITLConfiguration(this->classId()); - m->settings = settings; - return m; -} - -void HITLConfiguration::saveConfig(QSettings* qSettings) const -{ - qSettings->setValue("simulatorId", settings.simulatorId); - qSettings->setValue("hostAddress", settings.hostAddress); - qSettings->setValue("inPort", settings.inPort); - qSettings->setValue("remoteAddress", settings.remoteAddress); - qSettings->setValue("outPort", settings.outPort); - qSettings->setValue("binPath", settings.binPath); - qSettings->setValue("dataPath", settings.dataPath); - - qSettings->setValue("homeLocation", settings.homeLocation); - qSettings->setValue("homeLocRate", settings.homeLocRate); - - qSettings->setValue("attRaw", settings.attRaw); - qSettings->setValue("attRawRate", settings.attRawRate); - - qSettings->setValue("attActual", settings.attActual); - qSettings->setValue("attActHW", settings.attActHW); - qSettings->setValue("attActSim", settings.attActSim); - qSettings->setValue("attActCalc", settings.attActCalc); - - qSettings->setValue("sonarAltitude", settings.sonarAltitude); - qSettings->setValue("sonarMaxAlt", settings.sonarMaxAlt); - qSettings->setValue("sonarAltRate", settings.sonarAltRate); - - qSettings->setValue("gpsPosition", settings.gpsPosition); - qSettings->setValue("gpsPosRate", settings.gpsPosRate); - - qSettings->setValue("inputCommand", settings.inputCommand); - qSettings->setValue("gcsReciever", settings.gcsReciever); - qSettings->setValue("manualControl", settings.manualControl); - qSettings->setValue("manualOutput", settings.manualOutput); - qSettings->setValue("outputRate", settings.outputRate); -} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h deleted file mode 100644 index 3e6a96df6..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h +++ /dev/null @@ -1,61 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2configuration.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef HITLV2CONFIGURATION_H -#define HITLV2CONFIGURATION_H - -#include -#include -#include -#include - -using namespace Core; - -class HITLConfiguration : public IUAVGadgetConfiguration -{ - - Q_OBJECT - - Q_PROPERTY(SimulatorSettings settings READ Settings WRITE setSimulatorSettings) - -public: - explicit HITLConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); - - void saveConfig(QSettings* settings) const; - IUAVGadgetConfiguration *clone(); - - SimulatorSettings Settings() const { return settings; } - -public slots: - void setSimulatorSettings (const SimulatorSettings& params ) { settings = params; } - - -private: - SimulatorSettings settings; -}; - -#endif // HITLV2CONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp deleted file mode 100644 index c9e52ebb4..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2factory.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlv2factory.h" -#include "hitlv2widget.h" -#include "hitlv2gadget.h" -#include "hitlv2configuration.h" -#include "hitlv2optionspage.h" -#include - -HITLFactory::HITLFactory(QObject *parent) - : IUAVGadgetFactory(QString("HITLv2"), tr("HITL Simulation (v2)"), parent) -{ -} - -HITLFactory::~HITLFactory() -{ -} - -Core::IUAVGadget* HITLFactory::createGadget(QWidget *parent) -{ - HITLWidget* gadgetWidget = new HITLWidget(parent); - return new HITLGadget(QString("HITLv2"), gadgetWidget, parent); -} - -IUAVGadgetConfiguration *HITLFactory::createConfiguration(QSettings* qSettings) -{ - return new HITLConfiguration(QString("HITLv2"), qSettings); -} - -IOptionsPage *HITLFactory::createOptionsPage(IUAVGadgetConfiguration *config) -{ - return new HITLOptionsPage(qobject_cast(config)); -} - diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h deleted file mode 100644 index cfa91750d..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h +++ /dev/null @@ -1,52 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2factory.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef HITLV2FACTORY_H -#define HITLV2FACTORY_H - -#include - -namespace Core { -class IUAVGadget; -class IUAVGadgetFactory; -} - -using namespace Core; - -class HITLFactory : public Core::IUAVGadgetFactory -{ - Q_OBJECT -public: - HITLFactory(QObject *parent = 0); - ~HITLFactory(); - - IUAVGadget *createGadget(QWidget *parent); - IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); - IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); -}; - -#endif // HITLV2FACTORY_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp deleted file mode 100644 index b114cbde5..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2gadget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlv2gadget.h" -#include "hitlv2widget.h" -#include "hitlv2configuration.h" -#include "simulatorv2.h" - -HITLGadget::HITLGadget(QString classId, HITLWidget *widget, QWidget *parent) : - IUAVGadget(classId, parent), - m_widget(widget) -{ - connect(this, SIGNAL(changeConfiguration(void)), m_widget, SLOT(stopButtonClicked(void))); -} - -HITLGadget::~HITLGadget() -{ - delete m_widget; -} - -void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config) -{ - HITLConfiguration *m = qobject_cast(config); - emit changeConfiguration(); - m_widget->setSettingParameters(m->Settings()); -} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h deleted file mode 100644 index 45302abb8..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h +++ /dev/null @@ -1,60 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2gadget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef HITLV2_H -#define HITLV2_H - -#include -#include "hitlv2widget.h" - -class IUAVGadget; -class QWidget; -class QString; -class Simulator; - -using namespace Core; - -class HITLGadget : public Core::IUAVGadget -{ - Q_OBJECT -public: - HITLGadget(QString classId, HITLWidget *widget, QWidget *parent = 0); - ~HITLGadget(); - - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); - -signals: - void changeConfiguration(); - -private: - HITLWidget* m_widget; - Simulator* simulator; -}; - - -#endif // HITLV2_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp deleted file mode 100644 index 280ccf030..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2optionspage.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlv2optionspage.h" -#include "hitlv2configuration.h" -#include "ui_hitlv2optionspage.h" -#include "hitlv2plugin.h" - -#include -#include -#include -#include "simulatorv2.h" - -HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) : - IOptionsPage(parent), - config(conf) -{ -} - -QWidget *HITLOptionsPage::createPage(QWidget *parent) -{ -// qDebug() << "HITLOptionsPage::createPage"; - // Create page - m_optionsPage = new Ui::HITLOptionsPage(); - QWidget* optionsPageWidget = new QWidget; - m_optionsPage->setupUi(optionsPageWidget); - int index = 0; - foreach (SimulatorCreator* creator, HITLPlugin::typeSimulators) - m_optionsPage->chooseFlightSimulator->insertItem(index++, creator->Description(), creator->ClassId()); - - m_optionsPage->executablePath->setExpectedKind(Utils::PathChooser::File); - m_optionsPage->executablePath->setPromptDialogTitle(tr("Choose flight simulator executable")); - m_optionsPage->dataPath->setExpectedKind(Utils::PathChooser::Directory); - m_optionsPage->dataPath->setPromptDialogTitle(tr("Choose flight simulator data directory")); - - // Restore the contents from the settings: - foreach (SimulatorCreator* creator, HITLPlugin::typeSimulators) { - QString id = config->Settings().simulatorId; - if (creator->ClassId() == id) - m_optionsPage->chooseFlightSimulator->setCurrentIndex(HITLPlugin::typeSimulators.indexOf(creator)); - } - - m_optionsPage->hostAddress->setText(config->Settings().hostAddress); - m_optionsPage->inputPort->setText(QString::number(config->Settings().inPort)); - m_optionsPage->remoteAddress->setText(config->Settings().remoteAddress); - m_optionsPage->outputPort->setText(QString::number(config->Settings().outPort)); - m_optionsPage->executablePath->setPath(config->Settings().binPath); - m_optionsPage->dataPath->setPath(config->Settings().dataPath); - - m_optionsPage->homeLocation->setChecked(config->Settings().homeLocation); - m_optionsPage->homeLocRate->setValue(config->Settings().homeLocRate); - - m_optionsPage->attRaw->setChecked(config->Settings().attRaw); - m_optionsPage->attRawRate->setValue(config->Settings().attRawRate); - - m_optionsPage->attActual->setChecked(config->Settings().attActual); - m_optionsPage->attActHW->setChecked(config->Settings().attActHW); - m_optionsPage->attActSim->setChecked(config->Settings().attActSim); - m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc); - - m_optionsPage->sonarAltitude->setChecked(config->Settings().sonarAltitude); - m_optionsPage->sonarMaxAlt->setValue(config->Settings().sonarMaxAlt); - m_optionsPage->sonarAltRate->setValue(config->Settings().sonarAltRate); - - m_optionsPage->gpsPosition->setChecked(config->Settings().gpsPosition); - m_optionsPage->gpsPosRate->setValue(config->Settings().gpsPosRate); - - m_optionsPage->inputCommand->setChecked(config->Settings().inputCommand); - m_optionsPage->gcsReciever->setChecked(config->Settings().gcsReciever); - m_optionsPage->manualControl->setChecked(config->Settings().manualControl); - - m_optionsPage->manualOutput->setChecked(config->Settings().manualOutput); - m_optionsPage->outputRate->setValue(config->Settings().outputRate); - - return optionsPageWidget; -} - -void HITLOptionsPage::apply() -{ - SimulatorSettings settings; - int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.remoteAddress = m_optionsPage->remoteAddress->text(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - - settings.homeLocation = m_optionsPage->homeLocation->isChecked(); - settings.homeLocRate = m_optionsPage->homeLocRate->value(); - - settings.attRaw = m_optionsPage->attRaw->isChecked(); - settings.attRawRate = m_optionsPage->attRawRate->value(); - - settings.attActual = m_optionsPage->attActual->isChecked(); - settings.attActHW = m_optionsPage->attActHW->isChecked(); - settings.attActSim = m_optionsPage->attActSim->isChecked(); - settings.attActCalc = m_optionsPage->attActCalc->isChecked(); - - settings.sonarAltitude = m_optionsPage->sonarAltitude->isChecked(); - settings.sonarMaxAlt = m_optionsPage->sonarMaxAlt->value(); - settings.sonarAltRate = m_optionsPage->sonarAltRate->value(); - - settings.gpsPosition = m_optionsPage->gpsPosition->isChecked(); - settings.gpsPosRate = m_optionsPage->gpsPosRate->value(); - - settings.inputCommand = m_optionsPage->inputCommand->isChecked(); - settings.gcsReciever = m_optionsPage->gcsReciever->isChecked(); - settings.manualControl = m_optionsPage->manualControl->isChecked(); - - settings.manualOutput = m_optionsPage->manualOutput->isChecked(); - settings.outputRate = m_optionsPage->outputRate->value(); - - config->setSimulatorSettings(settings); -} - -void HITLOptionsPage::finish() -{ - delete m_optionsPage; -} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h deleted file mode 100644 index dbfe028bc..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h +++ /dev/null @@ -1,61 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2optionspage.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef HITLV2OPTIONSPAGE_H -#define HITLV2OPTIONSPAGE_H - -#include - -namespace Core { -class IUAVGadgetConfiguration; -} - -class HITLConfiguration; - -using namespace Core; - -namespace Ui { -class HITLOptionsPage; -} - -class HITLOptionsPage : public IOptionsPage -{ - Q_OBJECT -public: - explicit HITLOptionsPage(HITLConfiguration *conf, QObject *parent = 0); - - QWidget *createPage(QWidget *parent); - void apply(); - void finish(); - bool isDecorated() const { return true; } - -private: - HITLConfiguration* config; - Ui::HITLOptionsPage* m_optionsPage; -}; - -#endif // HITLV2OPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui deleted file mode 100644 index f321daea1..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui +++ /dev/null @@ -1,718 +0,0 @@ - - - HITLOptionsPage - - - - 0 - 0 - 603 - 617 - - - - Form - - - - - - QFrame::NoFrame - - - QFrame::Plain - - - true - - - - - 0 - 0 - 579 - 593 - - - - - 0 - - - - - - - Choose flight simulator: - - - - - - - - - - - - - - Local interface (IP): - - - - - - - For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer. - - - - - - - Remote interface (IP): - - - - - - - Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running. - - - - - - - Input Port: - - - - - - - For receiving data from sim - - - - - - - Output Port: - - - - - - - For sending data to sim - - - - - - - - - QFormLayout::AllNonFixedFieldsGrow - - - - - - 0 - 0 - - - - Path executable: - - - - - - - - - - - 0 - 0 - - - - Data directory: - - - - - - - - - - - - 6 - - - 6 - - - - - Attitude data - - - - 12 - - - 3 - - - 18 - - - 3 - - - 0 - - - - - - 0 - 0 - - - - AttitudeRaw (gyro, accels) - - - true - - - true - - - false - - - - 6 - - - 12 - - - 0 - - - 12 - - - - - Refresh rate - - - - - - - ms - - - 10 - - - 100 - - - 20 - - - - - - - - - - - 0 - 0 - - - - AttitudeActual - - - true - - - true - - - false - - - - 6 - - - 12 - - - 0 - - - 12 - - - - - send raw data to board - - - - - - - - 75 - true - - - - - - - use values from simulator - - - true - - - - - - - - - - calculate from AttitudeRaw - - - - - - - - - - Qt::Vertical - - - QSizePolicy::Expanding - - - - 20 - 40 - - - - - - - - - - - Other data - - - - 12 - - - 3 - - - 18 - - - 3 - - - 0 - - - - - - 0 - 0 - - - - HomeLocation - - - true - - - true - - - false - - - - 6 - - - 12 - - - 0 - - - 12 - - - - - - 0 - 0 - - - - Refresh rate - - - - - - - - 0 - 0 - - - - 0 - update once, or every N seconds - - - sec - - - 10 - - - - - - - - - - - 0 - 0 - - - - GPSPosition - - - true - - - true - - - false - - - - 6 - - - 12 - - - 0 - - - 12 - - - - - Refresh rate - - - - - - - ms - - - 100 - - - 2000 - - - 500 - - - - - - - - - - - 0 - 0 - - - - SonarAltitude - - - true - - - true - - - false - - - - 3 - - - 6 - - - 3 - - - 0 - - - 0 - - - - - 6 - - - 6 - - - - - Range detection - - - - - - - m - - - 1 - - - 10 - - - 5 - - - - - - - Refresh rate - - - - - - - ms - - - 20 - - - 2000 - - - 10 - - - 50 - - - - - - - - - - - - - 0 - 0 - - - - Map command from simulator - - - true - - - true - - - false - - - - 3 - - - 12 - - - 0 - - - 12 - - - - - - 75 - true - - - - to GCSReciver - - - true - - - - - - - false - - - to ManualCtrll (not implemented) - - - - - - - - - - 6 - - - - - Maximum output rate - - - - - - - ms - - - 10 - - - 100 - - - 5 - - - 15 - - - - - - - - - Qt::Vertical - - - QSizePolicy::Expanding - - - - 20 - 40 - - - - - - - - - - - - - - - - - - Utils::PathChooser - QWidget -
utils/pathchooser.h
- 1 - - - - chooseFlightSimulator - hostAddress - inputPort - remoteAddress - outputPort - - - - diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp deleted file mode 100644 index c63ba5c86..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2plugin.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlv2plugin.h" -#include "hitlv2factory.h" -#include -#include -#include - -#include "aerosimrc.h" - -QList HITLPlugin::typeSimulators; - -HITLPlugin::HITLPlugin() -{ - // Do nothing -} - -HITLPlugin::~HITLPlugin() -{ - // Do nothing -} - -bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) -{ - Q_UNUSED(args); - Q_UNUSED(errMsg); - mf = new HITLFactory(this); - - addAutoReleasedObject(mf); - - addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC")); - - return true; -} - -void HITLPlugin::extensionsInitialized() -{ - // Do nothing -} - -void HITLPlugin::shutdown() -{ - // Do nothing -} -Q_EXPORT_PLUGIN(HITLPlugin) - diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h deleted file mode 100644 index 3e83915b3..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h +++ /dev/null @@ -1,67 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2plugin.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef HITLV2PLUGIN_H -#define HITLV2PLUGIN_H - -#include -#include - -#include - -class HITLFactory; - -class HITLPlugin : public ExtensionSystem::IPlugin -{ -public: - HITLPlugin(); - ~HITLPlugin(); - - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void shutdown(); - - static void addSimulator(SimulatorCreator* creator) - { - HITLPlugin::typeSimulators.append(creator); - } - - static SimulatorCreator* getSimulatorCreator(const QString classId) - { - foreach(SimulatorCreator* creator, HITLPlugin::typeSimulators) { - if(classId == creator->ClassId()) - return creator; - } - return 0; - } - - static QList typeSimulators; - -private: - HITLFactory *mf; -}; -#endif /* HITLV2PLUGIN_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp deleted file mode 100644 index 8933d7748..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2widget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "hitlv2widget.h" -#include "ui_hitlv2widget.h" -#include -#include -#include -#include -#include - -#include "hitlv2plugin.h" -#include "simulatorv2.h" -#include "uavobjectmanager.h" -#include -#include - -QStringList Simulator::instances; - -HITLWidget::HITLWidget(QWidget *parent) - : QWidget(parent), - simulator(0) -{ - widget = new Ui_HITLWidget(); - widget->setupUi(this); - widget->startButton->setEnabled(true); - widget->stopButton->setEnabled(false); - - strAutopilotDisconnected = " AP OFF "; - strSimulatorDisconnected = " Sim OFF "; - strAutopilotConnected = " AP ON "; -// strSimulatorConnected = " Sim ON "; - strStyleEnable = "QFrame{background-color: green; color: white}"; - strStyleDisable = "QFrame{background-color: red; color: grey}"; - - widget->apLabel->setText(strAutopilotDisconnected); - widget->simLabel->setText(strSimulatorDisconnected); - - connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked())); - connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked())); - connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked())); -} - -HITLWidget::~HITLWidget() -{ - delete widget; -} - -void HITLWidget::startButtonClicked() -{ - // allow only one instance per simulator - if (Simulator::Instances().indexOf(settings.simulatorId) != -1) { - widget->textBrowser->append(settings.simulatorId + " alreary started!"); - return; - } - - if (!HITLPlugin::typeSimulators.size()) { - widget->textBrowser->append("There is no registered simulators, add through HITLPlugin::addSimulator"); - return; - } - - // Stop running process if one is active - if (simulator) { - QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); - simulator = NULL; - } - - if (settings.hostAddress == "" || settings.inPort == 0) { - widget->textBrowser->append("Before start, set UDP parameters in options page!"); - return; - } - - SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId); - simulator = creator->createSimulator(settings); - simulator->setName(creator->Description()); - simulator->setSimulatorId(creator->ClassId()); - - connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString))); - - // Setup process - onProcessOutput(QString("[%1] Starting %2... ") - .arg(QTime::currentTime().toString("hh:mm:ss")) - .arg(creator->Description())); - - // Start bridge - bool ret = QMetaObject::invokeMethod(simulator, "setupProcess", Qt::QueuedConnection); - if (ret) { - Simulator::setInstance(settings.simulatorId); - - connect(this, SIGNAL(deleteSimulator()), simulator, SLOT(onDeleteSimulator()), Qt::QueuedConnection); - - widget->startButton->setEnabled(false); - widget->stopButton->setEnabled(true); - - connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()), Qt::QueuedConnection); - connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()), Qt::QueuedConnection); - connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()), Qt::QueuedConnection); - connect(simulator, SIGNAL(simulatorDisconnected()), this, SLOT(onSimulatorDisconnect()), Qt::QueuedConnection); - - // Initialize connection status - if (simulator->isAutopilotConnected()) - onAutopilotConnect(); - else - onAutopilotDisconnect(); - - if (simulator->isSimulatorConnected()) - onSimulatorConnect(); - else - onSimulatorDisconnect(); - } -} - -void HITLWidget::stopButtonClicked() -{ - if (simulator) - widget->textBrowser->append(QString("[%1] Terminate %2 ") - .arg(QTime::currentTime().toString("hh:mm:ss")) - .arg(simulator->Name())); - - widget->startButton->setEnabled(true); - widget->stopButton->setEnabled(false); - widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); - widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}")); - widget->apLabel->setText(strAutopilotDisconnected); - widget->simLabel->setText(strSimulatorDisconnected); - if (simulator) { - QMetaObject::invokeMethod(simulator, "onDeleteSimulator", Qt::QueuedConnection); - simulator = NULL; - } -} - -void HITLWidget::buttonClearLogClicked() -{ - widget->textBrowser->clear(); -} - -void HITLWidget::onProcessOutput(QString text) -{ - widget->textBrowser->append(text); -} - -void HITLWidget::onAutopilotConnect() -{ - widget->apLabel->setStyleSheet(strStyleEnable); - widget->apLabel->setText(strAutopilotConnected); -} - -void HITLWidget::onAutopilotDisconnect() -{ - widget->apLabel->setStyleSheet(strStyleDisable); - widget->apLabel->setText(strAutopilotDisconnected); -} - -void HITLWidget::onSimulatorConnect() -{ - widget->simLabel->setStyleSheet(strStyleEnable); - widget->simLabel->setText(" " + simulator->Name() + " ON "); -} - -void HITLWidget::onSimulatorDisconnect() -{ - widget->simLabel->setStyleSheet(strStyleDisable); - widget->simLabel->setText(" " + simulator->Name() + " OFF "); -} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h deleted file mode 100644 index 6cf1c66c7..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h +++ /dev/null @@ -1,72 +0,0 @@ -/** - ****************************************************************************** - * - * @file hitlv2widget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef HITLV2WIDGET_H -#define HITLV2WIDGET_H - -#include -#include -#include "simulatorv2.h" - -class Ui_HITLWidget; - -class HITLWidget : public QWidget -{ - Q_OBJECT - -public: - HITLWidget(QWidget *parent = 0); - ~HITLWidget(); - - void setSettingParameters(const SimulatorSettings& params) { settings = params; } - -signals: - void deleteSimulator(); - -private slots: - void startButtonClicked(); - void stopButtonClicked(); - void buttonClearLogClicked(); - void onProcessOutput(QString text); - void onAutopilotConnect(); - void onAutopilotDisconnect(); - void onSimulatorConnect(); - void onSimulatorDisconnect(); - -private: - Ui_HITLWidget* widget; - Simulator* simulator; - SimulatorSettings settings; - - QString strAutopilotDisconnected; - QString strSimulatorDisconnected; - QString strAutopilotConnected; -// QString strSimulatorConnected; - QString strStyleEnable; - QString strStyleDisable; -}; -#endif /* HITLV2WIDGET_H */ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui deleted file mode 100644 index 48d2c2db2..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui +++ /dev/null @@ -1,314 +0,0 @@ - - - HITLWidget - - - - 0 - 0 - 477 - 300 - - - - - 0 - 0 - - - - Form - - - - -QScrollBar:vertical { - border: 1px solid grey; - background: grey; - margin: 22px 0 22px 0; - } - -QScrollBar:vertical:disabled { - border: 1px solid grey; - - background-color: grey; - margin: 22px 0 22px 0; - } - - QScrollBar::handle:vertical { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(120, 120, 255, 255), stop:1 rgba(80, 80, 160, 255)); - min-height: 20px; - } - -QScrollBar::handle:vertical:disabled{ -background-color: grey; - min-height: 20px; - } - - - QScrollBar::handle:vertical:pressed { - - background-color: rgb(85, 85, 255); -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(170, 170, 255, 255), stop:1 rgba(80, 80, 160, 255)); - - min-height: 20px; - } - - QScrollBar::add-line:vertical { - border: 1px solid black; -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); - height: 20px; - subcontrol-position: bottom; - subcontrol-origin: margin; - } - - QScrollBar::add-line:vertical:disabled { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(100, 100, 100, 255)); - border: 1px solid grey; - - } - - QScrollBar::sub-line:vertical:disabled { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(100, 100, 100, 255)); - border: 1px solid grey; - - } - - QScrollBar::add-line:vertical:pressed { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); - } - - QScrollBar::sub-line:vertical:pressed { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); - } - - QScrollBar::sub-line:vertical { - border: 1px solid black; -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); - height: 20px; - subcontrol-position: top; - subcontrol-origin: margin; - } - QScrollBar::down-arrow:vertical { - - image: url(:/hitlnew/images/arrow-down2.png); - } - - QScrollBar::up-arrow:vertical { - image: url(:/hitlnew/images/arrow-up2.png); - } - - QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical { - background: none; - } - - -QPushButton { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(110, 110, 110, 255), stop:1 rgba(71, 71, 71, 255)); - - color: rgb(255, 255, 255); -border: 1px solid black; -width: 66px; -height: 20px; -} - -QPushButton:disabled { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(120, 120, 120, 255)); - color: rgb(194, 194, 194); -border: 1px solid gray; -width: 66px; -height: 20px; -} - -QPushButton:hover { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(255, 255, 255, 200), stop:1 rgba(180, 180, 180, 200)); -color: rgb(255, 255, 255); -border: 0px; -} -QPushButton:pressed { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); -color: rgb(255, 255, 255); -border: 0px; -} - -QPushButton:checked { -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(48, 48, 48, 255), stop:1 rgba(120, 120, 120, 255)); -color: rgb(255, 255, 255); -border: 0px; -} - - - - 0 - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - -QFrame{ -background-color: qlineargradient(spread:pad, x1:0.5, y1:0, x2:0.5, y2:1, stop:0 rgba(110, 110, 110, 255), stop:1 rgba(71, 71, 71, 255)); -color: rgba(0, 0, 0, 128); -} - - - - QFrame::StyledPanel - - - QFrame::Raised - - - - QLayout::SetMaximumSize - - - - - QLayout::SetDefaultConstraint - - - - - Request update - - - - - - Start - - - - - - - false - - - Send update - - - Stop - - - - - - - - 0 - 22 - - - - - 50 - false - - - - Qt::LeftToRight - - - false - - - color: rgb(255, 255, 255); - - - AP OFF - - - Qt::AlignCenter - - - - - - - - 0 - 22 - - - - color: rgb(255, 255, 255); - - - Sim OFF - - - Qt::AlignCenter - - - - - - - Clear Log - - - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - false - - - QTextEdit { - background-color: white; - color: rgb(0, 0, 0); -} - - - QFrame::NoFrame - - - QFrame::Plain - - - Qt::ScrollBarAlwaysOn - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/hitlv2/plugin.pro b/ground/openpilotgcs/src/plugins/hitlv2/plugin.pro deleted file mode 100644 index 2d375e426..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/plugin.pro +++ /dev/null @@ -1,32 +0,0 @@ -TEMPLATE = lib -TARGET = HITLv2 -QT += network - -include(../../openpilotgcsplugin.pri) -include(hitlv2_dependencies.pri) - -HEADERS += \ - aerosimrc.h \ - hitlv2configuration.h \ - hitlv2factory.h \ - hitlv2gadget.h \ - hitlv2optionspage.h \ - hitlv2plugin.h \ - hitlv2widget.h \ - simulatorv2.h - -SOURCES += \ - aerosimrc.cpp \ - hitlv2configuration.cpp \ - hitlv2factory.cpp \ - hitlv2gadget.cpp \ - hitlv2optionspage.cpp \ - hitlv2plugin.cpp \ - hitlv2widget.cpp \ - simulatorv2.cpp - -FORMS += \ - hitlv2optionspage.ui \ - hitlv2widget.ui - -OTHER_FILES += hitlv2.pluginspec diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp deleted file mode 100644 index bc5e0b0b1..000000000 --- a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp +++ /dev/null @@ -1,341 +0,0 @@ -/** - ****************************************************************************** - * - * @file simulatorv2.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup HITLPlugin HITLv2 Plugin - * @{ - * @brief The Hardware In The Loop plugin version 2 - *****************************************************************************/ -/* - * This program 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. - * - * This program 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 this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "simulatorv2.h" -#include -#include -#include - -volatile bool Simulator::isStarted = false; - -const float Simulator::GEE = 9.81; -const float Simulator::FT2M = 0.3048; -const float Simulator::KT2MPS = 0.514444444; -const float Simulator::INHG2KPA = 3.386; -const float Simulator::FPS2CMPS = 30.48; -const float Simulator::DEG2RAD = (M_PI/180.0); -const float Simulator::RAD2DEG = (180.0/M_PI); - - -Simulator::Simulator(const SimulatorSettings& params) : - inSocket(NULL), - outSocket(NULL), - settings(params), - updatePeriod(50), - simTimeout(2000), - autopilotConnectionStatus(false), - simConnectionStatus(false), - txTimer(NULL), - simTimer(NULL), - name("") -{ - // move to thread - moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread()); - connect(this, SIGNAL(myStart()), this, SLOT(onStart()), Qt::QueuedConnection); - emit myStart(); -} - -Simulator::~Simulator() -{ -// qDebug() << "Simulator::~Simulator"; - if (inSocket) { - delete inSocket; - inSocket = NULL; - } - if (outSocket) { - delete outSocket; - outSocket = NULL; - } - if (txTimer) { - delete txTimer; - txTimer = NULL; - } - if (simTimer) { - delete simTimer; - simTimer = NULL; - } -} - -void Simulator::onDeleteSimulator(void) -{ -// qDebug() << "Simulator::onDeleteSimulator"; - resetAllObjects(); - - Simulator::setStarted(false); - Simulator::Instances().removeOne(simulatorId); - - disconnect(this); - delete this; -} - -void Simulator::onStart() -{ -// qDebug() << "Simulator::onStart"; - QMutexLocker locker(&lock); - - // Get required UAVObjects - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); - -// actDesired = ActuatorDesired::GetInstance(objManager); -// manCtrlCommand = ManualControlCommand::GetInstance(objManager); -// velActual = VelocityActual::GetInstance(objManager); -// posActual = PositionActual::GetInstance(objManager); -// altActual = BaroAltitude::GetInstance(objManager); -// camDesired = CameraDesired::GetInstance(objManager); -// acsDesired = AccessoryDesired::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - gpsPosition = GPSPosition::GetInstance(objManager); - flightStatus = FlightStatus::GetInstance(objManager); - gcsReceiver = GCSReceiver::GetInstance(objManager); - actCommand = ActuatorCommand::GetInstance(objManager); - attSettings = AttitudeSettings::GetInstance(objManager); - sonarAlt = SonarAltitude::GetInstance(objManager); - - telStats = GCSTelemetryStats::GetInstance(objManager); - - // Listen to autopilot connection events - TelemetryManager* telMngr = pm->getObject(); - connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect())); - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); - - // If already connect setup autopilot - GCSTelemetryStats::DataFields stats = telStats->getData(); - if (stats.Status == GCSTelemetryStats::STATUS_CONNECTED) - onAutopilotConnect(); - - emit processOutput("Local interface: " + settings.hostAddress + ":" + \ - QString::number(settings.inPort) + "\n" + \ - "Remote interface: " + settings.remoteAddress + ":" + \ - QString::number(settings.outPort) + "\n"); - - inSocket = new QUdpSocket(); - outSocket = new QUdpSocket(); - setupUdpPorts(settings.hostAddress, settings.inPort, settings.outPort); - - connect(inSocket, SIGNAL(readyRead()), this, SLOT(receiveUpdate())/*, Qt::DirectConnection*/); - - // Setup transmit timer - if (settings.manualOutput) { - txTimer = new QTimer(); - connect(txTimer, SIGNAL(timeout()), this, SLOT(transmitUpdate())/*, Qt::DirectConnection*/); - txTimer->setInterval(settings.outputRate); - txTimer->start(); - } - - // Setup simulator connection timer - simTimer = new QTimer(); - connect(simTimer, SIGNAL(timeout()), this, SLOT(onSimulatorConnectionTimeout())/*, Qt::DirectConnection*/); - simTimer->setInterval(simTimeout); - simTimer->start(); - -#ifdef DBG_TIMERS - timeRX = QTime(); - timeRX.start(); - timeTX = QTime(); - timeTX.start(); -#endif - - setupObjects(); -} - -void Simulator::receiveUpdate() -{ - // Update connection timer and status - simTimer->start(); - if (!simConnectionStatus) { - simConnectionStatus = true; - emit simulatorConnected(); - } - - // Process data - while (inSocket->hasPendingDatagrams()) { - // Receive datagram - QByteArray datagram; - datagram.resize(inSocket->pendingDatagramSize()); - QHostAddress sender; - quint16 senderPort; - inSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); - // Process incomming data - processUpdate(datagram); - } - if (!settings.manualOutput) - transmitUpdate(); -} - -void Simulator::setupObjects() -{ - if (settings.gcsReciever) { - setupInputObject(actCommand, settings.outputRate); - setupOutputObject(gcsReceiver); - } else if (settings.manualControl) { -// setupInputObject(actDesired); -// setupInputObject(camDesired); -// setupInputObject(acsDesired); -// setupOutputObject(manCtrlCommand); - qDebug() << "ManualControlCommand not implemented yet"; - } - - if (settings.homeLocation) - setupOutputObject(posHome); - - if (settings.sonarAltitude) - setupOutputObject(sonarAlt); - - if (settings.gpsPosition) - setupOutputObject(gpsPosition); - - if (settings.attRaw || settings.attActual) { - setupOutputObject(accels); - setupOutputObject(gyros); - } - - if (settings.attActual && !settings.attActHW) - setupOutputObject(attActual); - else - setupWatchedObject(attActual); -} - -void Simulator::resetAllObjects() -{ - setupDefaultObject(posHome); - setupDefaultObject(accels); - setupDefaultObject(gyros); - setupDefaultObject(attActual); - setupDefaultObject(gpsPosition); - setupDefaultObject(gcsReceiver); - setupDefaultObject(actCommand); - setupDefaultObject(sonarAlt); -// setupDefaultObject(manCtrlCommand); -// setupDefaultObject(actDesired); -// setupDefaultObject(camDesired); -// setupDefaultObject(acsDesired); -// setupDefaultObject(altActual); -// setupDefaultObject(posActual); -// setupDefaultObject(velActual); -} - -void Simulator::setupInputObject(UAVObject* obj, quint32 updateRate) -{ - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); - - UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); - UAVObject::SetGcsTelemetryAcked(mdata, false); - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); - mdata.gcsTelemetryUpdatePeriod = 0; - - UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); - UAVObject::SetFlightTelemetryAcked(mdata, false); - - if (settings.manualOutput) { - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = updateRate; - } else { - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); - mdata.flightTelemetryUpdatePeriod = 0; - } - - obj->setMetadata(mdata); -} - -void Simulator::setupWatchedObject(UAVObject *obj) -{ - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); - - UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READONLY); - UAVObject::SetGcsTelemetryAcked(mdata, false); - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); - mdata.gcsTelemetryUpdatePeriod = 0; - - UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READWRITE); - UAVObject::SetFlightTelemetryAcked(mdata, false); - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 100; - - obj->setMetadata(mdata); -} - -void Simulator::setupOutputObject(UAVObject* obj) -{ - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); - - UAVObject::SetGcsAccess(mdata, UAVObject::ACCESS_READWRITE); - UAVObject::SetGcsTelemetryAcked(mdata, false); - UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); - mdata.gcsTelemetryUpdatePeriod = 0; - - UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY); - UAVObject::SetFlightTelemetryAcked(mdata, false); - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL); - mdata.flightTelemetryUpdatePeriod = 0; - - obj->setMetadata(mdata); -} - -void Simulator::setupDefaultObject(UAVObject *obj) -{ - UAVObject::Metadata mdata; - mdata = obj->getDefaultMetadata(); - - obj->setMetadata(mdata); -} - -void Simulator::onAutopilotConnect() -{ - autopilotConnectionStatus = true; - emit autopilotConnected(); -} - -void Simulator::onAutopilotDisconnect() -{ - autopilotConnectionStatus = false; - emit autopilotDisconnected(); -} - -void Simulator::onSimulatorConnectionTimeout() -{ - if (simConnectionStatus) { - simConnectionStatus = false; - emit simulatorDisconnected(); - } -} - -void Simulator::telStatsUpdated(UAVObject* obj) -{ - GCSTelemetryStats::DataFields stats = telStats->getData(); - if (!autopilotConnectionStatus && stats.Status == GCSTelemetryStats::STATUS_CONNECTED) - onAutopilotConnect(); - else if (autopilotConnectionStatus && stats.Status != GCSTelemetryStats::STATUS_CONNECTED) - onAutopilotDisconnect(); -} - diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index 1fbb6f3d7..b96005e92 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -155,19 +155,12 @@ plugin_ipconnection.subdir = ipconnection plugin_ipconnection.depends = plugin_coreplugin SUBDIRS += plugin_ipconnection -# HITLNEW Simulation gadget -plugin_hitlnew.subdir = hitlnew -plugin_hitlnew.depends = plugin_coreplugin -plugin_hitlnew.depends += plugin_uavobjects -plugin_hitlnew.depends += plugin_uavtalk -SUBDIRS += plugin_hitlnew - -# HITLNEW Simulation gadget v2 -plugin_hitl_v2.subdir = hitlv2 -plugin_hitl_v2.depends = plugin_coreplugin -plugin_hitl_v2.depends += plugin_uavobjects -plugin_hitl_v2.depends += plugin_uavtalk -SUBDIRS += plugin_hitl_v2 +#HITL Simulation gadget +plugin_hitl.subdir = hitl +plugin_hitl.depends = plugin_coreplugin +plugin_hitl.depends += plugin_uavobjects +plugin_hitl.depends += plugin_uavtalk +SUBDIRS += plugin_hitl # Export and Import GCS Configuration plugin_importexport.subdir = importexport diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 65fc12a24..5d958d301 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -25,6 +25,7 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/baroaltitude.h \ + $$UAVOBJECT_SYNTHETICS/airspeedactual.h \ $$UAVOBJECT_SYNTHETICS/attitudeactual.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ @@ -92,6 +93,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ + $$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \ $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ diff --git a/shared/uavobjectdefinition/airspeedactual.xml b/shared/uavobjectdefinition/airspeedactual.xml new file mode 100644 index 000000000..e612b3e31 --- /dev/null +++ b/shared/uavobjectdefinition/airspeedactual.xml @@ -0,0 +1,13 @@ + + + UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip + + + + + + + + + +