rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

Refactored HiTL for a common API from simulators. Also added noise generator. In addition, PFD now displays calibrated airspeed.

Browse Source 
Also added throttle commands for all four channesl (it is now possible to fly a 747)
This commit is contained in:
Laura Sebesta 2012-06-15 15:22:07 +03:00
parent 0115c810f3
commit 23651cddd1
15 changed files with 1408 additions and 1177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -7,7 +7,7 @@
* @{
* @addtogroup HITLPlugin HITL Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -34,9 +34,6 @@
#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)
@ -50,7 +47,7 @@
//}
FGSimulator::FGSimulator(const SimulatorSettings& params) :
Simulator(params)
Simulator(params)
{
udpCounterFGrecv = 0;
udpCounterGCSsend = 0;
@ -58,7 +55,7 @@ FGSimulator::FGSimulator(const SimulatorSettings& params) :
FGSimulator::~FGSimulator()
{
disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
}
void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
@ -71,85 +68,85 @@ void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
bool FGSimulator::setupProcess()
{
QMutexLocker locker(&lock);
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();
// 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;
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
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");
QString cmdShell("c:/windows/system32/cmd.exe");
#else
QString cmdShell("bash");
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;
}
// 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");
}
// 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.");
}
// 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;
udpCounterGCSsend = 0;
return true;
return true;
}
void FGSimulator::processReadyRead()
{
QByteArray bytes = simProcess->readAllStandardOutput();
QString str(bytes);
if ( !str.contains("Error reading data") ) // ignore error
{
emit processOutput(str);
}
QByteArray bytes = simProcess->readAllStandardOutput();
QString str(bytes);
if ( !str.contains("Error reading data") ) // ignore error
{
emit processOutput(str);
}
}
void FGSimulator::transmitUpdate()
@ -176,7 +173,7 @@ void FGSimulator::transmitUpdate()
}
else
{
// Read ActuatorDesired from autopilot
// Read ActuatorDesired from autopilot
actData = actDesired->getData();
ailerons = actData.Roll;
@ -193,19 +190,19 @@ void FGSimulator::transmitUpdate()
udpCounterGCSsend = 0;
if((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv==0) ) //FG udp queue is not delayed
{
{
udpCounterGCSsend++;
// Send update to FlightGear
QString cmd;
// 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
.arg(ailerons) //ailerons
.arg(elevator) //elevator
.arg(rudder) //rudder
.arg(throttle) //throttle
.arg(udpCounterGCSsend); //UDP packet counter delay
QByteArray data = cmd.toAscii();
QByteArray data = cmd.toAscii();
if(outSocket->writeDatagram(data, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1)
{
@ -235,169 +232,116 @@ void FGSimulator::transmitUpdate()
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;
// 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;
// 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);
///////
// Output formatting
///////
Output2OP out;
memset(&out, 0, sizeof(Output2OP));
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = altitudeAGL;
altActualData.Temperature = temperature;
altActualData.Pressure = pressure;
altActual->setData(altActualData);
float NED[3];
// convert from cm back to meters
// 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);
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 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
// Update GPS Position objects
out.latitude = latitude * 1e7;
out.longitude = longitude * 1e7;
out.altitude = altitude;
out.groundspeed = groundspeed;
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);
out.airspeed = airspeed;
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();
// 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);
}

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

@ -7,66 +7,68 @@
* @{
* @addtogroup HITLPlugin HITL Plugin
* @{
* @brief The Hardware In The Loop 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
* 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
*
* 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.,
*
* 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)
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 = "";
settings.simulatorId = "";
settings.binPath = "";
settings.dataPath = "";
settings.manual = false;
settings.startSim = false;
settings.addNoise = 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();
}
//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.addNoise = qSettings->value("noiseCheckBox").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());
HITLConfiguration *m = new HITLConfiguration(this->classId());
m->settings = settings;
m->settings = settings;
return m;
}
/**
/**
* Saves a configuration.
*
*/
@ -75,6 +77,7 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const {
qSettings->setValue("binPath", settings.binPath);
qSettings->setValue("dataPath", settings.dataPath);
qSettings->setValue("manual", settings.manual);
qSettings->setValue("addNoise", settings.addNoise);
qSettings->setValue("startSim", settings.startSim);
qSettings->setValue("hostAddress", settings.hostAddress);
qSettings->setValue("remoteHostAddress", settings.remoteHostAddress);

60
ground/openpilotgcs/src/plugins/hitlnew/hitlnew.pro
View File

@ -1,29 +1,31 @@
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 = 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 \
hitlnoisegeneration.h
SOURCES += hitlplugin.cpp \
hitlwidget.cpp \
hitloptionspage.cpp \
hitlfactory.cpp \
hitlconfiguration.cpp \
hitlgadget.cpp \
simulator.cpp \
il2simulator.cpp \
fgsimulator.cpp \
xplanesimulator.cpp \
hitlnoisegeneration.cpp
OTHER_FILES += hitlnew.pluginspec
FORMS += hitloptionspage.ui \
hitlwidget.ui
RESOURCES += hitlresources.qrc

77
ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp Normal file
View File

@ -0,0 +1,77 @@
/**
******************************************************************************
*
* @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.baroAirspeed.Airspeed=0;
}

66
ground/openpilotgcs/src/plugins/hitlnew/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;
BaroAirspeed::DataFields baroAirspeed;
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

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

@ -7,21 +7,21 @@
* @{
* @addtogroup HITLPlugin HITL Plugin
* @{
* @brief The Hardware In The Loop 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
* 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
*
* 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.,
*
* 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
*/
@ -39,7 +39,7 @@
HITLOptionsPage::HITLOptionsPage(HITLConfiguration *conf, QObject *parent) :
IOptionsPage(parent),
config(conf)
config(conf)
{
}
@ -49,65 +49,67 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
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());
}
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);
//QString classId = widget->listSimulators->itemData(0).toString();
//SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(classId);
//QWidget* embedPage = creator->createOptionsPage();
//m_optionsPage->verticalLayout->addWidget(embedPage);
//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"));
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));
}
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->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->noiseCheckBox->setChecked(config->Settings().addNoise);
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);
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();
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.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.addNoise = m_optionsPage->noiseCheckBox->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();
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);
config->setSimulatorSettings(settings);
}
void HITLOptionsPage::finish()

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

@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>590</width>
<width>599</width>
<height>367</height>
</rect>
</property>
@ -40,7 +40,7 @@
</property>
</widget>
</item>
<item row="11" column="2">
<item row="12" column="2">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
@ -259,6 +259,13 @@
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QCheckBox" name="noiseCheckBox">
<property name="text">
<string>Add noise</string>
</property>
</widget>
</item>
</layout>
</widget>
<customwidgets>

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

@ -84,7 +84,7 @@ 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)
Simulator(params)
{
}
@ -96,31 +96,31 @@ 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));
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;
// 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!!!!!!!!!!!!!
// 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!!!!!!!!!!!!!
}
@ -128,25 +128,25 @@ void IL2Simulator::transmitUpdate()
* 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) )
);
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;
return DENSITY(alt)*(TEMP_GROUND+(alt*TEMP_LAPSE_RATE))*AIR_CONST;
}
/**
* calculate TAS from IAS and altitude
* calculate CAS from IAS and altitude
*/
float IL2Simulator::TAS(float IAS, float alt) {
return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt)));
float IL2Simulator::CAS(float IAS, float alt) {
return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt)));
}
/**
@ -154,209 +154,166 @@ float IL2Simulator::TAS(float IAS, float alt) {
*/
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("/");
// 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("\\");
// 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;
}
}
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;
}
// 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);
// calculate CAS from alt and IAS
current.tas = CAS(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);
// 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);
// 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;
// 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);
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
// 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 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];
///////
// Output formatting
///////
Output2OP out;
memset(&out, 0, sizeof(Output2OP));
// 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.;
// 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 velocityActual objects
VelocityActual::DataFields velData;
memset(&velData, 0, sizeof(VelocityActual::DataFields));
velData.North = current.dY;
velData.East = current.dX;
velData.Down = current.dZ*-1.;
//Calculate ECEF
double RNE[9];
double ECEF[3];
double LLA[3];
LLA[0]=settings.latitude.toFloat();
LLA[1]=settings.longitude.toFloat();
LLA[2]=0;
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
// 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();
// Update GPS Position objects
double NED[3];
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().GetLLA(ECEF,NED,LLA);
out.latitude = settings.latitude.toFloat() * 1e7;
out.longitude = settings.longitude.toFloat() * 1e7;
out.groundspeed = current.groundspeed;
// 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]);
out.airspeed = current.ias;
// 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;
out.dstN=current.Y;
out.dstE=current.X;
out.dstD=-current.Z;
// 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().GetLLA(ECEF,NED,LLA);
gpsData.Latitude = LLA[0] * 10e6;
gpsData.Longitude = LLA[1] * 10e6;
gpsData.Satellites = 7;
gpsData.Status = GPSPosition::STATUS_FIX3D;
// 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
// 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();
// 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);
}
/**
@ -364,8 +321,8 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
*/
float IL2Simulator::angleDifference(float a, float b)
{
float d=a-b;
if (d>180) d-=360;
if (d<-180)d+=360;
return d;
float d=a-b;
if (d>180) d-=360;
if (d<-180)d+=360;
return d;
}

2
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.h
View File

@ -62,7 +62,7 @@ private:
float DENSITY(float pressure);
float PRESSURE(float alt);
float TAS(float ias,float alt);
float CAS(float ias,float alt);
void processUpdate(const QByteArray& data);
float angleDifference(float a,float b);

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

@ -31,6 +31,8 @@
#include "extensionsystem/pluginmanager.h"
#include "coreplugin/icore.h"
#include "coreplugin/threadmanager.h"
#include "hitlnoisegeneration.h"
#include <QDebug>
volatile bool Simulator::isStarted = false;
@ -43,113 +45,116 @@ const float Simulator::DEG2RAD = (M_PI/180.0);
Simulator::Simulator(const SimulatorSettings& params) :
simProcess(NULL),
time(NULL),
inSocket(NULL),
outSocket(NULL),
settings(params),
simProcess(NULL),
time(NULL),
inSocket(NULL),
outSocket(NULL),
settings(params),
updatePeriod(50),
simTimeout(8000),
autopilotConnectionStatus(false),
simConnectionStatus(false),
txTimer(NULL),
simTimer(NULL),
name("")
autopilotConnectionStatus(false),
simConnectionStatus(false),
txTimer(NULL),
simTimer(NULL),
name(""),
once(false)
{
// move to thread
moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
// move to thread
moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
connect(this, SIGNAL(myStart()), this, SLOT(onStart()),Qt::QueuedConnection);
emit myStart();
emit myStart();
}
Simulator::~Simulator()
{
if(inSocket)
{
delete inSocket;
inSocket = NULL;
}
if(inSocket)
{
delete inSocket;
inSocket = NULL;
}
if(outSocket)
{
delete outSocket;
outSocket = NULL;
}
if(outSocket)
{
delete outSocket;
outSocket = NULL;
}
if(txTimer)
{
delete txTimer;
txTimer = 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)));
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;
}
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);
// [1]
Simulator::setStarted(false);
// [2]
Simulator::Instances().removeOne(simulatorId);
disconnect(this);
delete this;
disconnect(this);
delete this;
}
void Simulator::onStart()
{
QMutexLocker locker(&lock);
QMutexLocker locker(&lock);
QThread* mainThread = QThread::currentThread();
qDebug() << "Simulator Thread: "<< mainThread;
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);
// 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);
baroAlt = BaroAltitude::GetInstance(objManager);
baroAirspeed = BaroAirspeed::GetInstance(objManager);
attActual = AttitudeActual::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*)));
// 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();
// 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);
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" + \
@ -168,131 +173,263 @@ void Simulator::onStart()
// 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);
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 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;
// 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();
}
// Update connection timer and status
simTimer->setInterval(simTimeout);
simTimer->stop();
simTimer->start();
if ( !simConnectionStatus )
{
simConnectionStatus = true;
emit simulatorConnected();
}
// Process data
// 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);
}
// 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);
setupInputObject(actDesired, 100); //100ms update period
setupOutputObject(baroAlt, 100); //250ms update period
setupOutputObject(baroAirspeed, 100); //250ms update period
setupOutputObject(attActual, 10); //etc...
setupOutputObject(gpsPos, 100);
setupOutputObject(gpsVel, 100);
setupOutputObject(posActual, 250);
setupOutputObject(velActual, 250);
// setupOutputObject(posHome, 1000); //WE DON'T WANT TO UPDATE HOME
setupOutputObject(accels, 10);
setupOutputObject(gyros, 10);
}
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);
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);
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();
autopilotConnectionStatus = true;
setupObjects();
emit autopilotConnected();
}
void Simulator::onAutopilotDisconnect()
{
autopilotConnectionStatus = false;
emit autopilotDisconnected();
autopilotConnectionStatus = false;
emit autopilotDisconnected();
}
void Simulator::onSimulatorConnectionTimeout()
{
if ( simConnectionStatus )
{
simConnectionStatus = false;
emit simulatorDisconnected();
}
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();
}
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(Output2OP out){
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;
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
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];
attActual->setData(attActualData);
// 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.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);
// Update VelocityActual.{North,East,Down}
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);
// Update BaroAltitude object
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);
// Update BaroAirspeed object
BaroAirspeed::DataFields baroAirspeedData;
memset(&baroAirspeedData, 0, sizeof(BaroAirspeed::DataFields));
baroAirspeedData.Airspeed = out.airspeed + noise.baroAirspeed.Airspeed;
baroAirspeed->setData(baroAirspeedData);
//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);
}

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

@ -37,12 +37,13 @@
#include "uavobjectmanager.h"
#include "actuatordesired.h"
#include "manualcontrolcommand.h"
// #include "altitudeactual.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "baroaltitude.h"
#include "baroairspeed.h"
#include "attitudeactual.h"
#include "gpsposition.h"
#include "gpsvelocity.h"
#include "homelocation.h"
#include "accels.h"
#include "gyros.h"
@ -54,157 +55,195 @@
/**
* just imagine this was a class without methods and all public properties
*/
typedef struct _FLIGHT_PARAM {
typedef struct _FLIGHT_PARAM {
// time
float T;
float dT;
unsigned int i;
// time
float T;
float dT;
unsigned int i;
// speed (relative)
float ias;
float tas;
float groundspeed;
// speed (relative)
float ias;
float cas;
float tas;
float groundspeed;
// position (absolute)
float X;
float Y;
float Z;
// position (absolute)
float X;
float Y;
float Z;
// speed (absolute)
float dX;
float dY;
float dZ;
// speed (absolute)
float dX;
float dY;
float dZ;
// acceleration (absolute)
float ddX;
float ddY;
float ddZ;
// acceleration (absolute)
float ddX;
float ddY;
float ddZ;
//angle
float azimuth;
float pitch;
float roll;
//rotation speed
float dAzimuth;
float dPitch;
float dRoll;
//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;
QString simulatorId;
QString binPath;
QString dataPath;
QString hostAddress;
QString remoteHostAddress;
int outPort;
int inPort;
bool manual;
bool startSim;
bool addNoise;
QString latitude;
QString longitude;
} SimulatorSettings;
struct Output2OP{
float latitude;
float longitude;
float altitude;
float heading;
float groundspeed; //[m/s]
float airspeed; //[m/s]
float pitch;
float roll;
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;
float pitchRate;
float yawRate;
};
class Simulator : public QObject
{
Q_OBJECT
Q_OBJECT
public:
Simulator(const SimulatorSettings& params);
virtual ~Simulator();
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; }
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; }
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); }
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)}
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(Output2OP out);
signals:
void autopilotConnected();
void autopilotDisconnected();
void simulatorConnected();
void simulatorDisconnected();
void processOutput(QString str);
void deleteSimProcess();
void myStart();
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;}
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);
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;
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 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;
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;
ActuatorDesired* actDesired;
ManualControlCommand* manCtrlCommand;
FlightStatus* flightStatus;
BaroAltitude* baroAlt;
BaroAirspeed* baroAirspeed;
AttitudeActual* attActual;
VelocityActual* velActual;
GPSPosition* gpsPos;
GPSVelocity* gpsVel;
PositionActual* posActual;
HomeLocation* posHome;
Accels* accels;
Gyros* gyros;
GCSTelemetryStats* telStats;
SimulatorSettings settings;
SimulatorSettings settings;
FLIGHT_PARAM current;
FLIGHT_PARAM old;
QMutex lock;
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;
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();
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();
};
@ -212,20 +251,20 @@ private:
class SimulatorCreator
{
public:
SimulatorCreator(QString id, QString descr) :
classId(id),
description(descr)
{}
virtual ~SimulatorCreator() {}
SimulatorCreator(QString id, QString descr) :
classId(id),
description(descr)
{}
virtual ~SimulatorCreator() {}
QString ClassId() const {return classId;}
QString Description() const {return description;}
QString ClassId() const {return classId;}
QString Description() const {return description;}
virtual Simulator* createSimulator(const SimulatorSettings& params) = 0;
virtual Simulator* createSimulator(const SimulatorSettings& params) = 0;
private:
QString classId;
QString description;
QString classId;
QString description;
};
#endif // ISIMULATOR_H

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

@ -25,7 +25,7 @@
* 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
@ -41,9 +41,9 @@
* 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.
// 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.
// many outputs do not use all 8, though.
* };
*
* For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set)
@ -66,9 +66,9 @@
void TraceBuf(const char* buf,int len);
XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) :
Simulator(params)
Simulator(params)
{
once = false;
resetInitialHomePosition();
}
@ -78,9 +78,9 @@ XplaneSimulator::~XplaneSimulator()
void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
{
inSocket->bind(QHostAddress(host), inPort);
//outSocket->bind(QHostAddress(host), outPort);
once = false;
inSocket->bind(QHostAddress(host), inPort);
//outSocket->bind(QHostAddress(host), outPort);
resetInitialHomePosition();
}
@ -97,29 +97,29 @@ bool XplaneSimulator::setupProcess()
*/
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
//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);
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
// !!! 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";
/*
// 11th data settings (flight con: ail/elv/rud)
buf.clear();
code = 11;
//quint8 header[] = "DATA";
/*
stream << *((quint32*)header) <<
(quint8)0x30 <<
code <<
@ -132,57 +132,57 @@ void XplaneSimulator::transmitUpdate()
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);
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);
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));
// 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.remoteHostAddress), settings.outPort) == -1)
{
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
}
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1)
{
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
}
//outSocket->write(buf);
//outSocket->write(buf);
/** !!! this settings was given from ardupilot X-Plane.pl, I comment them
but if it needed comment should be removed !!!
/** !!! 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);
*/
// 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);
*/
}
@ -191,244 +191,176 @@ void XplaneSimulator::transmitUpdate()
*/
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=0;
float pitchRate=0;
float yawRate=0;
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=0;
float pitchRate=0;
float yawRate=0;
QString str;
QByteArray& buf = const_cast<QByteArray&>(dataBuf);
QString data(buf);
// 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;
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 = *((float*)(buf.data()+4*7));
speed = *((float*)(buf.data()+4*8));
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::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;
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::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::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:
pitchRate = *((float*)(buf.data()+4*1));
rollRate = *((float*)(buf.data()+4*2));
yawRate = *((float*)(buf.data()+4*3));
break;
case XplaneSimulator::AngularVelocities:
pitchRate = *((float*)(buf.data()+4*1));
rollRate = *((float*)(buf.data()+4*2));
yawRate = *((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;
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);
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();
///////
// Output formatting
///////
Output2OP out;
memset(&out, 0, sizeof(Output2OP));
// Initialize the initial distance
initX = dstX;
initY = dstY;
initZ = dstZ;
once=1;
}
// 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.airspeed = 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
// Update AltitudeActual object
BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = altitude;
altActualData.Temperature = temperature;
altActualData.Pressure = pressure;
altActual->setData(altActualData);
out.dstN=dstY;
out.dstE=dstX;
out.dstD=-dstZ;
// 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 VelocityActual.{North,East,Down}
out.velNorth = velY;
out.velEast = velX;
out.velDown = -velZ;
// 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 gyroscope sensor data
out.rollRate = rollRate;
out.pitchRate = pitchRate;
out.yawRate = yawRate;
// 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 accelerometer sensor data
out.accX = accX;
out.accY = accY;
out.accZ = -accZ;
// 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);
updateUAVOs(out);
}
// issue manual update
//attActual->updated();
//altActual->updated();
//posActual->updated();
// 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 + sin(DEG2RAD * roll) * yawRate;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate;
//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));
gyroData.x = rollRate;
gyroData.y = pitchRate;
gyroData.z = yawRate;
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'));
}
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;
if(reminder)
qDebug() << str;
}

14
ground/openpilotgcs/src/plugins/hitlnew/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
};

99
ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp
View File

@ -128,9 +128,16 @@ void PFDGadgetWidget::connectNeedles() {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
airspeedObj = dynamic_cast<UAVDataObject*>(objManager->getObject("VelocityActual"));
airspeedObj = dynamic_cast<UAVDataObject*>(objManager->getObject("BaroAirspeed"));
if (airspeedObj != NULL ) {
connect(airspeedObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateAirspeed(UAVObject*)));
} else {
qDebug() << "Error: Object is unknown (BaroAirspeed).";
}
groundspeedObj = dynamic_cast<UAVDataObject*>(objManager->getObject("VelocityActual"));
if (groundspeedObj != NULL ) {
connect(groundspeedObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateGroundspeed(UAVObject*)));
} else {
qDebug() << "Error: Object is unknown (VelocityActual).";
}
@ -297,9 +304,9 @@ void PFDGadgetWidget::updateHeading(UAVObject *object) {
}
/*!
\brief Called by updates to @PositionActual to compute airspeed from velocity
\brief Called by updates to @PositionActual to compute groundspeed from velocity
*/
void PFDGadgetWidget::updateAirspeed(UAVObject *object) {
void PFDGadgetWidget::updateGroundspeed(UAVObject *object) {
UAVObjectField* northField = object->getField("North");
UAVObjectField* eastField = object->getField("East");
if (northField && eastField) {
@ -314,14 +321,31 @@ void PFDGadgetWidget::updateAirspeed(UAVObject *object) {
}
}
/*!
\brief Called by updates to @BaroAirspeed
*/
void PFDGadgetWidget::updateAirspeed(UAVObject *object) {
UAVObjectField* airspeedField = object->getField("Airspeed");
if (airspeedField) {
airspeedTarget = airspeedField->getDouble();
if (!dialTimer.isActive())
dialTimer.start(); // Rearm the dial Timer which might be stopped.
} else {
qDebug() << "UpdateHeading: Wrong field, maybe an issue with object disconnection ?";
}
}
/*!
\brief Called by the @ref PositionActual updates to show altitude
*/
void PFDGadgetWidget::updateAltitude(UAVObject *object) {
UAVObjectField* downField = object->getField("Down");
if (downField) {
// The altitude scale represents 30 meters
altitudeTarget = -floor(downField->getDouble()*10)/10*altitudeScaleHeight/30;
altitudeTarget = -downField->getDouble();
if (!dialTimer.isActive())
dialTimer.start(); // Rearm the dial Timer which might be stopped.
@ -936,22 +960,26 @@ void PFDGadgetWidget::moveNeedles()
}
//////
// Speed
// Airspeed
//////
if (groundspeedValue != groundspeedTarget) {
if ((abs(groundspeedValue-groundspeedTarget) > speedScaleHeight/100) && beSmooth ) {
groundspeedValue += (groundspeedTarget-groundspeedValue)/2;
if (airspeedValue != airspeedTarget) {
if ((abs(airspeedValue-airspeedTarget) > speedScaleHeight/100) && beSmooth ) {
airspeedValue += (airspeedTarget-airspeedValue)/2;
} else {
groundspeedValue = groundspeedTarget;
airspeedValue = airspeedTarget;
dialCount--;
}
float airspeed_kph=airspeedValue*3.6;
float airspeed_kph_scale = airspeed_kph*speedScaleHeight/30;
qreal x = m_speedscale->transform().dx();
//opd = QPointF(x,fmod(groundspeedValue,speedScaleHeight/6));
//opd = QPointF(x,fmod(airspeed_kph,speedScaleHeight/6));
// fmod does rounding errors!! the formula below works better:
QPointF opd = QPointF(x,groundspeedValue-floor(groundspeedValue/speedScaleHeight*6)*speedScaleHeight/6);
QPointF opd = QPointF(x, airspeed_kph_scale-floor(airspeed_kph_scale/speedScaleHeight*6)*speedScaleHeight/6);
m_speedscale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false);
double speedText = groundspeedValue/speedScaleHeight*30;
double speedText = airspeed_kph;
QString s = QString().sprintf("%.0f",speedText);
m_speedtext->setPlainText(s);
@ -959,7 +987,7 @@ void PFDGadgetWidget::moveNeedles()
// (Qt documentation states that the list has the same order
// as the item add order in the QGraphicsItemGroup)
QList <QGraphicsItem *> textList = m_speedscale->childItems();
qreal val = 5*floor(groundspeedValue/speedScaleHeight*6)+20;
qreal val = 5*floor(airspeed_kph_scale/speedScaleHeight*6)+20;
foreach( QGraphicsItem * item, textList) {
if (QGraphicsTextItem *text = qgraphicsitem_cast<QGraphicsTextItem *>(item)) {
QString s = (val<0) ? QString() : QString().sprintf("%.0f",val);
@ -974,6 +1002,38 @@ void PFDGadgetWidget::moveNeedles()
dialCount--;
}
//////
// Groundspeed
//////
if (groundspeedValue != groundspeedTarget) {
groundspeedValue = groundspeedTarget;
// qreal x = m_speedscale->transform().dx();
// //opd = QPointF(x,fmod(groundspeedValue,speedScaleHeight/6));
// // fmod does rounding errors!! the formula below works better:
// QPointF opd = QPointF(x,groundspeedValue-floor(groundspeedValue/speedScaleHeight*6)*speedScaleHeight/6);
// m_speedscale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false);
// double speedText = groundspeedValue/speedScaleHeight*30;
// QString s = QString().sprintf("%.0f",speedText);
// m_speedtext->setPlainText(s);
// // Now update the text elements inside the scale:
// // (Qt documentation states that the list has the same order
// // as the item add order in the QGraphicsItemGroup)
// QList <QGraphicsItem *> textList = m_speedscale->childItems();
// qreal val = 5*floor(groundspeedValue/speedScaleHeight*6)+20;
// foreach( QGraphicsItem * item, textList) {
// if (QGraphicsTextItem *text = qgraphicsitem_cast<QGraphicsTextItem *>(item)) {
// QString s = (val<0) ? QString() : QString().sprintf("%.0f",val);
// if (text->toPlainText() == s)
// break; // This should happen at element one if is has not changed, indicating
// // that it's not necessary to do the rest of the list
// text->setPlainText(s);
// val -= 5;
// }
// }
}
//////
// Altitude
//////
@ -984,12 +1044,17 @@ void PFDGadgetWidget::moveNeedles()
altitudeValue = altitudeTarget;
dialCount--;
}
// The altitude scale represents 30 meters
float altitudeValue_scale = floor(altitudeValue*10)/10*altitudeScaleHeight/30;
qreal x = m_altitudescale->transform().dx();
//opd = QPointF(x,fmod(altitudeValue,altitudeScaleHeight/6));
// fmod does rounding errors!! the formula below works better:
QPointF opd = QPointF(x,altitudeValue-floor(altitudeValue/altitudeScaleHeight*6)*altitudeScaleHeight/6);
QPointF opd = QPointF(x,altitudeValue_scale-floor(altitudeValue_scale/altitudeScaleHeight*6)*altitudeScaleHeight/6);
m_altitudescale->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false);
double altitudeText = altitudeValue/altitudeScaleHeight*30;
double altitudeText = altitudeValue;
QString s = QString().sprintf("%.0f",altitudeText);
m_altitudetext->setPlainText(s);
@ -997,7 +1062,7 @@ void PFDGadgetWidget::moveNeedles()
// (Qt documentation states that the list has the same order
// as the item add order in the QGraphicsItemGroup)
QList <QGraphicsItem *> textList = m_altitudescale->childItems();
qreal val = 5*floor(altitudeValue/altitudeScaleHeight*6)+20;
qreal val = 5*floor(altitudeValue_scale/altitudeScaleHeight*6)+20;
foreach( QGraphicsItem * item, textList) {
if (QGraphicsTextItem *text = qgraphicsitem_cast<QGraphicsTextItem *>(item)) {
QString s = (val<0) ? QString() : QString().sprintf("%.0f",val);

4
ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.h
View File

@ -59,6 +59,7 @@ public slots:
void updateAttitude(UAVObject *object1);
void updateHeading(UAVObject *object1);
void updateGPS(UAVObject *object1);
void updateGroundspeed(UAVObject *object1);
void updateAirspeed(UAVObject *object1);
void updateAltitude(UAVObject *object1);
void updateBattery(UAVObject *object1);
@ -119,6 +120,8 @@ private:
double headingValue;
double groundspeedTarget;
double groundspeedValue;
double airspeedTarget;
double airspeedValue;
double altitudeTarget;
double altitudeValue;
@ -130,6 +133,7 @@ private:
UAVDataObject* airspeedObj;
UAVDataObject* altitudeObj;
UAVDataObject* attitudeObj;
UAVDataObject* groundspeedObj;
UAVDataObject* headingObj;
UAVDataObject* gpsObj;
UAVDataObject* gcsTelemetryObj;