rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'kenz/cc_hitl_merge' into next

Browse Source 
Conflicts:
	ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui
	ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui
	ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui
This commit is contained in:
Laura Sebesta 2012-10-08 15:30:27 +02:00
commit c2b49b6ef3
101 changed files with 4164 additions and 5609 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/CopterControl/Makefile
View File

@ -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

117
ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml
View File

@ -955,19 +955,74 @@
</GPS__PCT__20Mouse>
</GpsDisplayGadget>
<HITL>
<AeroSimRC>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
</configInfo>
<data>
<addNoise>false</addNoise>
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
<baroAltitudeEnabled>false</baroAltitudeEnabled>
<binPath>\home\lafargue\X-Plane 9\X-Plane-i686</binPath>
<dataPath>\usr\share\games\FlightGear</dataPath>
<gcsReceiverEnabled>true</gcsReceiverEnabled>
<gpsPosRate>100</gpsPosRate>
<gpsPositionEnabled>false</gpsPositionEnabled>
<groundTruthEnabled>true</groundTruthEnabled>
<groundTruthRate>100</groundTruthRate>
<hostAddress>0.0.0.0</hostAddress>
<inPort>40100</inPort>
<inputCommand>true</inputCommand>
<latitude></latitude>
<longitude></longitude>
<manualControlEnabled>false</manualControlEnabled>
<manualOutput>false</manualOutput>
<minOutputPeriod>40</minOutputPeriod>
<outPort>40200</outPort>
<remoteAddress>127.0.0.1</remoteAddress>
<simulatorId>ASimRC</simulatorId>
<startSim>false</startSim>
</data>
</AeroSimRC>
<Flightgear__PCT__20HITL>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
</configInfo>
<data>
<addNoise>false</addNoise>
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
<baroAltitudeEnabled>false</baroAltitudeEnabled>
<binPath>\usr\games\fgfs</binPath>
<dataPath>\usr\share\games\FlightGear</dataPath>
<gcsReceiverEnabled>false</gcsReceiverEnabled>
<gpsPosRate>100</gpsPosRate>
<gpsPositionEnabled>false</gpsPositionEnabled>
<groundTruthEnabled>true</groundTruthEnabled>
<groundTruthRate>100</groundTruthRate>
<hostAddress>127.0.0.1</hostAddress>
<inPort>9009</inPort>
<manual>false</manual>
<inputCommand>true</inputCommand>
<latitude></latitude>
<longitude></longitude>
<manualControlEnabled>true</manualControlEnabled>
<manualOutput>false</manualOutput>
<minOutputPeriod>40</minOutputPeriod>
<outPort>9010</outPort>
<remoteHostAddress>127.0.0.1</remoteHostAddress>
<remoteAddress>127.0.0.1</remoteAddress>
<simulatorId>FG</simulatorId>
<startSim>true</startSim>
</data>
@ -978,51 +1033,37 @@
<version>0.0.0</version>
</configInfo>
<data>
<addNoise>false</addNoise>
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
<baroAltitudeEnabled>false</baroAltitudeEnabled>
<binPath>\home\lafargue\X-Plane 9\X-Plane-i686</binPath>
<dataPath>\usr\share\games\FlightGear</dataPath>
<hostAddress>127.0.0.3</hostAddress>
<gcsReceiverEnabled>false</gcsReceiverEnabled>
<gpsPosRate>100</gpsPosRate>
<gpsPositionEnabled>false</gpsPositionEnabled>
<groundTruthEnabled>true</groundTruthEnabled>
<groundTruthRate>100</groundTruthRate>
<hostAddress>0.0.0.0</hostAddress>
<inPort>6756</inPort>
<manual>false</manual>
<inputCommand>true</inputCommand>
<latitude></latitude>
<longitude></longitude>
<manualControlEnabled>true</manualControlEnabled>
<manualOutput>false</manualOutput>
<minOutputPeriod>40</minOutputPeriod>
<outPort>49000</outPort>
<remoteHostAddress>127.0.0.1</remoteHostAddress>
<remoteAddress>127.0.0.1</remoteAddress>
<simulatorId>X-Plane</simulatorId>
<startSim>false</startSim>
</data>
</XPlane__PCT__20HITL>
</HITL>
<HITLv2>
<default>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
</configInfo>
<data>
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActual>true</attActual>
<attRaw>true</attRaw>
<attRawRate>20</attRawRate>
<gcsReciever>true</gcsReciever>
<gpsPosRate>200</gpsPosRate>
<gpsPosition>true</gpsPosition>
<homeLocRate>0</homeLocRate>
<homeLocation>true</homeLocation>
<hostAddress>127.0.0.1</hostAddress>
<inPort>40100</inPort>
<inputCommand>true</inputCommand>
<manualControl>false</manualControl>
<manualOutput>false</manualOutput>
<outPort>40200</outPort>
<outputRate>20</outputRate>
<remoteAddress>127.0.0.1</remoteAddress>
<simulatorId>ASimRC</simulatorId>
<sonarAltRate>50</sonarAltRate>
<sonarAltitude>false</sonarAltitude>
<sonarMaxAlt>@Variant(AAAAh0CgAAA=)</sonarMaxAlt>
</data>
</default>
</HITLv2>
<LineardialGadget>
<Accel__PCT__20Horizontal__PCT__20X>
<configInfo>

21
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -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;
}
}

1
ground/openpilotgcs/src/libs/utils/coordinateconversions.h
View File

@ -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]);
};
}

0
ground/openpilotgcs/src/plugins/hitlnew/Start Flightgear XP.bat → ground/openpilotgcs/src/plugins/hitl/Start Flightgear XP.bat
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro → ground/openpilotgcs/src/plugins/hitl/aerosimrc/aerosimrc.pro
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/aerosimrcdatastruct.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/enums.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/plugin.pro
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/qdebughandler.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_off.tga
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_off_hover.tga
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_on.tga
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_on_hover.tga
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/plugin.txt
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/settings.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptest.pro
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestmain.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.h
View File

0
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui → ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udptestwidget.ui
View File

273
ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp Normal file
View File

@ -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 <extensionsystem/pluginmanager.h>
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings &params)
: 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);
}

2
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h → ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.h
View File

@ -32,7 +32,7 @@
#include <QVector3D>
#include <QQuaternion>
#include <QMatrix4x4>
#include "simulatorv2.h"
#include "simulator.h"
class AeroSimRCSimulator: public Simulator
{

349
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp Normal file
View File

@ -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);
}

0
ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.h → ground/openpilotgcs/src/plugins/hitl/fgsimulator.h
View File

2
ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pluginspec → ground/openpilotgcs/src/plugins/hitl/hitl.pluginspec
View File

@ -1,4 +1,4 @@
<plugin name="HITLNEW" version="1.0.0" compatVersion="1.0.0">
<plugin name="HITL" version="1.0.0" compatVersion="1.0.0">
<vendor>The OpenPilot Project</vendor>
<copyright>(C) 2010 OpenPilot Project</copyright>
<license>The GNU Public License (GPL) Version 3</license>

0
ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro → ground/openpilotgcs/src/plugins/hitl/hitl.pro
View File

0
ground/openpilotgcs/src/plugins/hitlnew/hitlnew_dependencies.pri → ground/openpilotgcs/src/plugins/hitl/hitl_dependencies.pri
View File

164
ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp Normal file
View File

@ -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);
}

0
ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.h → ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.h
View File

0
ground/openpilotgcs/src/plugins/hitlnew/hitlfactory.cpp → ground/openpilotgcs/src/plugins/hitl/hitlfactory.cpp
View File

0
ground/openpilotgcs/src/plugins/hitlnew/hitlfactory.h → ground/openpilotgcs/src/plugins/hitl/hitlfactory.h
View File

18
ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.cpp → ground/openpilotgcs/src/plugins/hitl/hitlgadget.cpp
View File

@ -44,24 +44,8 @@ HITLGadget::~HITLGadget()
void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config)
{
HITLConfiguration *m = qobject_cast<HITLConfiguration*>(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());
}

0
ground/openpilotgcs/src/plugins/hitlnew/hitlgadget.h → ground/openpilotgcs/src/plugins/hitl/hitlgadget.h
View File

78
ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp Normal file
View File

@ -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;
}

66
ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h Normal file
View File

@ -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 <QObject>
//#include <simulator.h>
#include "xplanesimulator.h"
#include "hitlnoisegeneration.h"
#include "extensionsystem/pluginmanager.h"
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
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

175
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp Normal file
View File

@ -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 <hitlplugin.h>
#include <QFileDialog>
#include <QtAlgorithms>
#include <QStringList>
#include <simulator.h>
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;
}

0
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.h → ground/openpilotgcs/src/plugins/hitl/hitloptionspage.h
View File

1108
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui Normal file
View File

@ -0,0 +1,1108 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>HITLOptionsPage</class>
<widget class="QWidget" name="HITLOptionsPage">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>885</width>
<height>741</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>3</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="chooseFlightSimulatorLabel">
<property name="text">
<string>Choose flight simulator:</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="chooseFlightSimulator"/>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_3">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>100</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>350</width>
<height>16777215</height>
</size>
</property>
<property name="title">
<string>IP addresses</string>
</property>
<widget class="QWidget" name="layoutWidget">
<property name="geometry">
<rect>
<x>10</x>
<y>30</y>
<width>323</width>
<height>24</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_7">
<item>
<widget class="QLabel" name="label_6">
<property name="text">
<string>Local host: </string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="hostAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>125</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_5">
<property name="text">
<string>Port:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="inputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>55</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>IP port for receiving data from sim</string>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="layoutWidget1">
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<width>322</width>
<height>24</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QLabel" name="label_9">
<property name="text">
<string>Remote host:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="remoteAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>125</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Port:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="outputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>55</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>IP port for sending data to sim</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_4">
<property name="minimumSize">
<size>
<width>0</width>
<height>180</height>
</size>
</property>
<property name="title">
<string>Program parameters</string>
</property>
<widget class="Utils::PathChooser" name="dataPath" native="true">
<property name="geometry">
<rect>
<x>120</x>
<y>60</y>
<width>401</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<width>94</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>110</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Data directory:</string>
</property>
</widget>
<widget class="Utils::PathChooser" name="executablePath" native="true">
<property name="geometry">
<rect>
<x>120</x>
<y>34</y>
<width>401</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>10</x>
<y>34</y>
<width>104</width>
<height>16</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>110</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Path executable:</string>
</property>
</widget>
<widget class="QCheckBox" name="startSim">
<property name="geometry">
<rect>
<x>10</x>
<y>120</y>
<width>229</width>
<height>20</height>
</rect>
</property>
<property name="toolTip">
<string>Check this box to start the simulator on the local computer</string>
</property>
<property name="text">
<string>Start simulator on local machine</string>
</property>
</widget>
<widget class="QCheckBox" name="noiseCheckBox">
<property name="geometry">
<rect>
<x>10</x>
<y>150</y>
<width>89</width>
<height>20</height>
</rect>
</property>
<property name="toolTip">
<string>Add noise to sensor simulation</string>
</property>
<property name="text">
<string>Add noise</string>
</property>
</widget>
<widget class="Line" name="line">
<property name="geometry">
<rect>
<x>10</x>
<y>80</y>
<width>491</width>
<height>16</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
<widget class="QLabel" name="label_13">
<property name="geometry">
<rect>
<x>10</x>
<y>90</y>
<width>161</width>
<height>22</height>
</rect>
</property>
<property name="text">
<string>Initial latitude (decimal):</string>
</property>
</widget>
<widget class="QLabel" name="label_14">
<property name="geometry">
<rect>
<x>290</x>
<y>90</y>
<width>171</width>
<height>22</height>
</rect>
</property>
<property name="text">
<string>Initial longitude (decimal):</string>
</property>
</widget>
<widget class="QLineEdit" name="longitude">
<property name="geometry">
<rect>
<x>460</x>
<y>90</y>
<width>100</width>
<height>22</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
<widget class="QLineEdit" name="latitude">
<property name="geometry">
<rect>
<x>170</x>
<y>90</y>
<width>100</width>
<height>22</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<item row="0" column="0">
<widget class="QFrame" name="frame">
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<layout class="QVBoxLayout" name="verticalLayout_7">
<item>
<widget class="QGroupBox" name="groupBox">
<property name="maximumSize">
<size>
<width>16777215</width>
<height>150</height>
</size>
</property>
<property name="title">
<string>Attitude data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>3</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="attRawCheckbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="title">
<string>AttitudeRaw (gyro, accels)</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="label_8">
<property name="text">
<string>Refresh rate:</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="attRawRateSpinbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>10</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="attActualCheckbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="title">
<string>AttitudeActual</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QRadioButton" name="attActSim">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string/>
</property>
<property name="text">
<string>use values from simulator</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="attActHW">
<property name="text">
<string>send simulated inertial data to board</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="attActCalc">
<property name="toolTip">
<string/>
</property>
<property name="text">
<string>calculate from simulated sensor data</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_6">
<property name="minimumSize">
<size>
<width>0</width>
<height>150</height>
</size>
</property>
<property name="title">
<string>Spatial data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_9">
<item>
<widget class="QGroupBox" name="groundTruthCheckbox">
<property name="title">
<string>Ground truth position and velocity</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>true</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="label_7">
<property name="text">
<string>Refresh rate:</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="groundTruthRateSpinbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>5000</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="gpsPositionCheckbox">
<property name="title">
<string>GPS data</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_5">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="label_10">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string>Refresh rate:</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="gpsPosRateSpinbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>5000</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_3">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_8">
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="minimumSize">
<size>
<width>0</width>
<height>275</height>
</size>
</property>
<property name="title">
<string>Other data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>3</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="airspeedActualCheckbox">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="title">
<string>AirspeedActual</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_9">
<item>
<widget class="QLabel" name="label_15">
<property name="text">
<string>Refresh rate:</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="airspeedRateSpinbox">
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="maximum">
<number>5000</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="baroAltitudeCheckbox">
<property name="enabled">
<bool>false</bool>
</property>
<property name="title">
<string>BaroAltitude</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_6">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Range detection:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSpinBox" name="sonarMaxAlt">
<property name="suffix">
<string>m</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Refresh rate:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="baroAltRateSpinbox">
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>20</number>
</property>
<property name="maximum">
<number>2000</number>
</property>
<property name="singleStep">
<number>10</number>
</property>
<property name="value">
<number>250</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="inputCommandCheckbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="title">
<string>Map transmitter commands...</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QRadioButton" name="manualControlRadioButton">
<property name="enabled">
<bool>true</bool>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>from hardware to simulator (via ManualCtrl)</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="gcsReceiverRadioButton">
<property name="text">
<string>from simulator to hardware (via GCSReceiver)</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_6">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Maximum GCS to hardware output rate:</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="minOutputPeriodSpinbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="toolTip">
<string>Set the maximum rate at which GCS sends simulator data to the hardware</string>
</property>
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>10</number>
</property>
<property name="maximum">
<number>500</number>
</property>
<property name="singleStep">
<number>5</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>Utils::PathChooser</class>
<extends>QWidget</extends>
<header>utils/pathchooser.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<tabstops>
<tabstop>chooseFlightSimulator</tabstop>
<tabstop>hostAddress</tabstop>
<tabstop>inputPort</tabstop>
<tabstop>remoteAddress</tabstop>
<tabstop>outputPort</tabstop>
</tabstops>
<resources/>
<connections/>
</ui>

2
ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.cpp → ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp
View File

@ -29,6 +29,7 @@
#include <QtPlugin>
#include <QStringList>
#include <extensionsystem/pluginmanager.h>
#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"));

2
ground/openpilotgcs/src/plugins/hitlnew/hitlplugin.h → ground/openpilotgcs/src/plugins/hitl/hitlplugin.h
View File

@ -29,7 +29,7 @@
#define HITLPLUGIN_H
#include <extensionsystem/iplugin.h>
#include <qstringlist.h>
#include <QStringList>
#include <simulator.h>

0
ground/openpilotgcs/src/plugins/hitlnew/hitlresources.qrc → ground/openpilotgcs/src/plugins/hitl/hitlresources.qrc
View File

63
ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.cpp → ground/openpilotgcs/src/plugins/hitl/hitlwidget.cpp
View File

@ -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()));
}

5
ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.h → ground/openpilotgcs/src/plugins/hitl/hitlwidget.h
View File

@ -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 */

0
ground/openpilotgcs/src/plugins/hitlnew/hitlwidget.ui → ground/openpilotgcs/src/plugins/hitl/hitlwidget.ui
View File

324
ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp Normal file
View File

@ -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 <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
#include <math.h>
#include <qxtlogger.h>
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<fields.length(); t++) {
QStringList values = fields[t].split("\\");
// parse values
if (values.length()>=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;
}

0
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.h → ground/openpilotgcs/src/plugins/hitl/il2simulator.h
View File

0
ground/openpilotgcs/src/plugins/hitlnew/images/arrow-down.png → ground/openpilotgcs/src/plugins/hitl/images/arrow-down.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/arrow-down2.png → ground/openpilotgcs/src/plugins/hitl/images/arrow-down2.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/arrow-right.png → ground/openpilotgcs/src/plugins/hitl/images/arrow-right.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 304 B

After

Width:  |  Height:  |  Size: 304 B

0
ground/openpilotgcs/src/plugins/hitlnew/images/arrow-up.png → ground/openpilotgcs/src/plugins/hitl/images/arrow-up.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/arrow-up2.png → ground/openpilotgcs/src/plugins/hitl/images/arrow-up2.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/bullet_arrow_down.png → ground/openpilotgcs/src/plugins/hitl/images/bullet_arrow_down.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/bullet_arrow_up.png → ground/openpilotgcs/src/plugins/hitl/images/bullet_arrow_up.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/list_bullet_arrow.png → ground/openpilotgcs/src/plugins/hitl/images/list_bullet_arrow.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 280 B

After

Width:  |  Height:  |  Size: 280 B

0
ground/openpilotgcs/src/plugins/hitlnew/images/scrollbarvertical_down_arrow.png → ground/openpilotgcs/src/plugins/hitl/images/scrollbarvertical_down_arrow.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/images/scrollbarvertical_up_arrow.png → ground/openpilotgcs/src/plugins/hitl/images/scrollbarvertical_up_arrow.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
ground/openpilotgcs/src/plugins/hitlnew/isimulator.h → ground/openpilotgcs/src/plugins/hitl/isimulator.h
View File

0
ground/openpilotgcs/src/plugins/hitlnew/opfgprotocol.xml → ground/openpilotgcs/src/plugins/hitl/opfgprotocol.xml
View File

66
ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pro → ground/openpilotgcs/src/plugins/hitl/plugin.pro
View File

@ -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

715
ground/openpilotgcs/src/plugins/hitl/simulator.cpp Normal file
View File

@ -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<UAVObjectManager>();
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<TelemetryManager>();
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);
}
}
}

559
ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h → ground/openpilotgcs/src/plugins/hitl/simulator.h
View File

@ -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 <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <QProcess>
#include <QScopedPointer>
#include <qmath.h>
#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 <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <QProcess>
#include <qmath.h>
#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<QScopedPointer<UAVDataObject> > 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

371
ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp Normal file
View File

@ -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 <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
#include <math.h>
#include <qxtlogger.h>
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<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&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<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&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<QByteArray&>(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;
}
}

14
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.h → ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h
View File

@ -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
};

403
ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -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();
}

86
ground/openpilotgcs/src/plugins/hitlnew/hitlconfiguration.cpp
View File

@ -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);
}

116
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.cpp
View File

@ -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 <hitlplugin.h>
#include <QFileDialog>
#include <QtAlgorithms>
#include <QStringList>
#include <simulator.h>
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;
}

321
ground/openpilotgcs/src/plugins/hitlnew/hitloptionspage.ui
View File

@ -1,321 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>HITLOptionsPage</class>
<widget class="QWidget" name="HITLOptionsPage">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>626</width>
<height>305</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QScrollArea" name="scrollArea">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Plain</enum>
</property>
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>626</width>
<height>305</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="5" column="1">
<widget class="QLineEdit" name="inputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>For receiving data from sim</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_7">
<property name="text">
<string>Latitude in degrees:</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Input Port:</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLineEdit" name="latitude">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item row="0" column="0" colspan="2">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Choose flight simulator:</string>
</property>
</widget>
</item>
<item row="10" column="0" colspan="2">
<widget class="QCheckBox" name="startSim">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Check this box to start the simulator on the local computer</string>
</property>
<property name="text">
<string>Start simulator on local machine</string>
</property>
</widget>
</item>
<item row="11" column="1">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>182</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0" colspan="5">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="8" column="1" colspan="4">
<widget class="Utils::PathChooser" name="dataPath" native="true"/>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Path executable:</string>
</property>
</widget>
</item>
<item row="0" column="2" colspan="2">
<widget class="QComboBox" name="chooseFlightSimulator"/>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Data directory:</string>
</property>
</widget>
</item>
<item row="9" column="0" colspan="5">
<widget class="QCheckBox" name="manualControl">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Manual aircraft control (can be used when hardware is not available)</string>
</property>
<property name="text">
<string>Manual aircraft control (can be used when hardware is not available)</string>
</property>
</widget>
</item>
<item row="7" column="1" colspan="4">
<widget class="Utils::PathChooser" name="executablePath" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>1</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_6">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Local interface (IP):</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLineEdit" name="hostAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Remote interface (IP):</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QLineEdit" name="remoteHostAddress">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QLabel" name="label_4">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Output Port:</string>
</property>
</widget>
</item>
<item row="6" column="2">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Longitude in degrees:</string>
</property>
</widget>
</item>
<item row="5" column="3">
<widget class="QLineEdit" name="outputPort">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>For sending data to sim</string>
</property>
</widget>
</item>
<item row="6" column="3">
<widget class="QLineEdit" name="longitude">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
</widget>
</item>
</layout>
<zorder>label_3</zorder>
<zorder>verticalSpacer</zorder>
<zorder>label_7</zorder>
<zorder>label_5</zorder>
<zorder>startSim</zorder>
<zorder>label_2</zorder>
<zorder>executablePath</zorder>
<zorder>label</zorder>
<zorder>dataPath</zorder>
<zorder>manualControl</zorder>
<zorder>chooseFlightSimulator</zorder>
<zorder>latitude</zorder>
<zorder>line</zorder>
<zorder>inputPort</zorder>
<zorder>label_6</zorder>
<zorder>hostAddress</zorder>
<zorder>label_9</zorder>
<zorder>remoteHostAddress</zorder>
<zorder>label_4</zorder>
<zorder>label_8</zorder>
<zorder>outputPort</zorder>
<zorder>longitude</zorder>
</widget>
</widget>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>Utils::PathChooser</class>
<extends>QWidget</extends>
<header>utils/pathchooser.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>

371
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

@ -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 <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
#include <math.h>
#include <qxtlogger.h>
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<fields.length(); t++) {
QStringList values = fields[t].split("\\");
// parse values
if (values.length()>=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;
}

298
ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp
View File

@ -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<UAVObjectManager>();
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<TelemetryManager>();
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();
}
}

231
ground/openpilotgcs/src/plugins/hitlnew/simulator.h
View File

@ -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 <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <QProcess>
#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<QScopedPointer<UAVDataObject> > 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

436
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -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 <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
#include <math.h>
#include <qxtlogger.h>
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<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&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<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&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<QByteArray&>(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;
}
}

495
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp
View File

@ -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 <extensionsystem/pluginmanager.h>
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings &params)
: 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);
}

12
ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pluginspec
View File

@ -1,12 +0,0 @@
<plugin name="HITLv2" version="1.0.0" compatVersion="1.0.0">
<vendor>The OpenPilot Project</vendor>
<copyright>(C) 2011 OpenPilot Project</copyright>
<license>The GNU Public License (GPL) Version 3</license>
<description>Hardware In The Loop Simulation (v2)</description>
<url>http://www.openpilot.org</url>
<dependencyList>
<dependency name="Core" version="1.0.0"/>
<dependency name="UAVObjects" version="1.0.0"/>
<dependency name="UAVTalk" version="1.0.0"/>
</dependencyList>
</plugin>

4
ground/openpilotgcs/src/plugins/hitlv2/hitlv2_dependencies.pri
View File

@ -1,4 +0,0 @@
include(../../plugins/uavobjects/uavobjects.pri)
include(../../plugins/uavtalk/uavtalk.pri)
#include(../../plugins/coreplugin/coreplugin.pri)
#include(../../libs/utils/utils.pri)

178
ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp
View File

@ -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);
}

61
ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h
View File

@ -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 <coreplugin/iuavgadgetconfiguration.h>
#include <QtGui/QColor>
#include <QString>
#include <simulatorv2.h>
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

59
ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp
View File

@ -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 <coreplugin/iuavgadget.h>
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<HITLConfiguration*>(config));
}

52
ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h
View File

@ -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 <coreplugin/iuavgadgetfactory.h>
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

50
ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp
View File

@ -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<HITLConfiguration*>(config);
emit changeConfiguration();
m_widget->setSettingParameters(m->Settings());
}

60
ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h
View File

@ -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 <coreplugin/iuavgadget.h>
#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

146
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp
View File

@ -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 <QFileDialog>
#include <QtAlgorithms>
#include <QStringList>
#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;
}

61
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h
View File

@ -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 <coreplugin/dialogs/ioptionspage.h>
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

718
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui
View File

@ -1,718 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>HITLOptionsPage</class>
<widget class="QWidget" name="HITLOptionsPage">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>603</width>
<height>617</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_8">
<item>
<widget class="QScrollArea" name="scrollArea">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Plain</enum>
</property>
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents_2">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>579</width>
<height>593</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="margin">
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Choose flight simulator:</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="chooseFlightSimulator"/>
</item>
</layout>
</item>
<item>
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Local interface (IP):</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="hostAddress">
<property name="toolTip">
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Remote interface (IP):</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLineEdit" name="remoteAddress">
<property name="toolTip">
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Input Port:</string>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QLineEdit" name="inputPort">
<property name="toolTip">
<string>For receiving data from sim</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Output Port:</string>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QLineEdit" name="outputPort">
<property name="toolTip">
<string>For sending data to sim</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QFormLayout" name="formLayout">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Path executable:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="Utils::PathChooser" name="executablePath" native="true"/>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Data directory:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="Utils::PathChooser" name="dataPath" native="true"/>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<property name="topMargin">
<number>6</number>
</property>
<property name="bottomMargin">
<number>6</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Attitude data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<number>12</number>
</property>
<property name="leftMargin">
<number>3</number>
</property>
<property name="topMargin">
<number>18</number>
</property>
<property name="rightMargin">
<number>3</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="attRaw">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>AttitudeRaw (gyro, accels)</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_7">
<property name="spacing">
<number>6</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QLabel" name="label_8">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="attRawRate">
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>10</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="attActual">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>AttitudeActual</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<property name="spacing">
<number>6</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QRadioButton" name="attActHW">
<property name="text">
<string>send raw data to board</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="attActSim">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string/>
</property>
<property name="text">
<string>use values from simulator</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="attActCalc">
<property name="toolTip">
<string/>
</property>
<property name="text">
<string>calculate from AttitudeRaw</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Expanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
<string>Other data</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="spacing">
<number>12</number>
</property>
<property name="leftMargin">
<number>3</number>
</property>
<property name="topMargin">
<number>18</number>
</property>
<property name="rightMargin">
<number>3</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="homeLocation">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>HomeLocation</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="spacing">
<number>6</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QLabel" name="label_7">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="homeLocRate">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>0 - update once, or every N seconds</string>
</property>
<property name="suffix">
<string>sec</string>
</property>
<property name="maximum">
<number>10</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="gpsPosition">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>GPSPosition</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_5">
<property name="spacing">
<number>6</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QLabel" name="label_10">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="gpsPosRate">
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>100</number>
</property>
<property name="maximum">
<number>2000</number>
</property>
<property name="value">
<number>500</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="sonarAltitude">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>SonarAltitude</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_6">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
<number>3</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<layout class="QGridLayout" name="gridLayout_2">
<property name="topMargin">
<number>6</number>
</property>
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Range detection</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSpinBox" name="sonarMaxAlt">
<property name="suffix">
<string>m</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Refresh rate</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="sonarAltRate">
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>20</number>
</property>
<property name="maximum">
<number>2000</number>
</property>
<property name="singleStep">
<number>10</number>
</property>
<property name="value">
<number>50</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="inputCommand">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Map command from simulator</string>
</property>
<property name="flat">
<bool>true</bool>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5">
<property name="spacing">
<number>3</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QRadioButton" name="gcsReciever">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>to GCSReciver</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="manualControl">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>to ManualCtrll (not implemented)</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_6">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QCheckBox" name="manualOutput">
<property name="text">
<string>Maximum output rate</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="outputRate">
<property name="suffix">
<string>ms</string>
</property>
<property name="minimum">
<number>10</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="singleStep">
<number>5</number>
</property>
<property name="value">
<number>15</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Expanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>Utils::PathChooser</class>
<extends>QWidget</extends>
<header>utils/pathchooser.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<tabstops>
<tabstop>chooseFlightSimulator</tabstop>
<tabstop>hostAddress</tabstop>
<tabstop>inputPort</tabstop>
<tabstop>remoteAddress</tabstop>
<tabstop>outputPort</tabstop>
</tabstops>
<resources/>
<connections/>
</ui>

71
ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp
View File

@ -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 <QtPlugin>
#include <QStringList>
#include <extensionsystem/pluginmanager.h>
#include "aerosimrc.h"
QList<SimulatorCreator* > 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)

67
ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h
View File

@ -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 <extensionsystem/iplugin.h>
#include <QStringList>
#include <simulatorv2.h>
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<SimulatorCreator* > typeSimulators;
private:
HITLFactory *mf;
};
#endif /* HITLV2PLUGIN_H */

188
ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp
View File

@ -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 <QDebug>
#include <QFile>
#include <QDir>
#include <QDateTime>
#include <QThread>
#include "hitlv2plugin.h"
#include "simulatorv2.h"
#include "uavobjectmanager.h"
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
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 ");
}

72
ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h
View File

@ -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 <QtGui/QWidget>
#include <QProcess>
#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 */

314
ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.ui
View File

@ -1,314 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>HITLWidget</class>
<widget class="QWidget" name="HITLWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>477</width>
<height>300</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<property name="styleSheet">
<string notr="true">
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;
}</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QFrame" name="frame">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">
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);
}
</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="sizeConstraint">
<enum>QLayout::SetMaximumSize</enum>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="sizeConstraint">
<enum>QLayout::SetDefaultConstraint</enum>
</property>
<item>
<widget class="QPushButton" name="startButton">
<property name="toolTip">
<string>Request update</string>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Start</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="stopButton">
<property name="enabled">
<bool>false</bool>
</property>
<property name="toolTip">
<string>Send update</string>
</property>
<property name="text">
<string>Stop</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="apLabel">
<property name="minimumSize">
<size>
<width>0</width>
<height>22</height>
</size>
</property>
<property name="font">
<font>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="layoutDirection">
<enum>Qt::LeftToRight</enum>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">color: rgb(255, 255, 255);</string>
</property>
<property name="text">
<string>AP OFF</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="simLabel">
<property name="minimumSize">
<size>
<width>0</width>
<height>22</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">color: rgb(255, 255, 255);</string>
</property>
<property name="text">
<string>Sim OFF</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="buttonClearLog">
<property name="text">
<string>Clear Log</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QTextEdit" name="textBrowser">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true">QTextEdit {
background-color: white;
color: rgb(0, 0, 0);
}</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Plain</enum>
</property>
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOn</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>

32
ground/openpilotgcs/src/plugins/hitlv2/plugin.pro
View File

@ -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

341
ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp
View File

@ -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 <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);
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<UAVObjectManager>();
// 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<TelemetryManager>();
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();
}

19
ground/openpilotgcs/src/plugins/plugins.pro
View File

@ -151,19 +151,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

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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 \

Some files were not shown because too many files have changed in this diff Show More