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

Extended HiTL API to the rest of the simulators.

Browse Source
This commit is contained in:
Kenz Dale 2012-08-29 12:35:46 +02:00
parent cb377e0f1d
commit d9139c6529
5 changed files with 481 additions and 650 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

344
ground/openpilotgcs/src/plugins/hitl/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,13 +55,11 @@ 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)
{
Q_UNUSED(outPort);
if(inSocket->bind(QHostAddress(host), inPort))
emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n");
else
@ -73,31 +68,31 @@ 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!)
@ -139,19 +134,19 @@ bool FGSimulator::setupProcess()
"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()
@ -178,7 +173,7 @@ void FGSimulator::transmitUpdate()
}
else
{
// Read ActuatorDesired from autopilot
// Read ActuatorDesired from autopilot
actData = actDesired->getData();
ailerons = actData.Roll;
@ -195,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.remoteAddress), settings.outPort) == -1)
{
@ -237,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 baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = altitudeAGL;
baroAltData.Temperature = temperature;
baroAltData.Pressure = pressure;
baroAlt->setData(baroAltData);
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.calibratedAirspeed = 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);
}

371
ground/openpilotgcs/src/plugins/hitl/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)
{
}
@ -105,23 +105,23 @@ void IL2Simulator::setupUdpPorts(const QString& host, int inPort, int outPort)
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!!!!!!!!!!!!!
}
@ -129,17 +129,17 @@ 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;
}
@ -155,209 +155,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);
// 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 baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = current.Z;
baroAltData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
baroAltData.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().NED2LLA_HomeECEF(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.calibratedAirspeed = 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().NED2LLA_HomeECEF(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();
baroAlt->setData(baroAltData);
//baroAlt->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);
}
/**
@ -365,8 +322,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;
}

5
ground/openpilotgcs/src/plugins/hitl/simulator.cpp
View File

@ -368,6 +368,11 @@ void Simulator::telStatsUpdated(UAVObject* obj)
}
void Simulator::resetInitialHomePosition(){
once=false;
}
void Simulator::updateUAVOs(Output2OP out){
QTime currentTime = QTime::currentTime();

397
ground/openpilotgcs/src/plugins/hitl/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();
}
@ -82,7 +82,7 @@ void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort
inSocket->bind(QHostAddress(host), inPort);
//outSocket->bind(QHostAddress(host), outPort);
once = false;
resetInitialHomePosition();
}
@ -99,41 +99,41 @@ 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();
// 11th data settings (flight con: ail/elv/rud)
buf.clear();
code = 11;
//quint8 header[] = "DATA";
/*
stream << *((quint32*)header) <<
(quint8)0x30 <<
code <<
*((quint32*)&elevator) <<
*((quint32*)&ailerons) <<
*((quint32*)&rudder) <<
none <<
*((quint32*)&ailerons) <<
none <<
none <<
none;
*/
stream << *((quint32*)header) <<
(quint8)0x30 <<
code <<
*((quint32*)&elevator) <<
*((quint32*)&ailerons) <<
*((quint32*)&rudder) <<
none <<
*((quint32*)&ailerons) <<
none <<
none <<
none;
*/
buf.append("DATA0");
buf.append(reinterpret_cast<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
@ -173,18 +173,18 @@ void XplaneSimulator::transmitUpdate()
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);
*/
}
@ -193,83 +193,83 @@ 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_rad=0;
float pitchRate_rad=0;
float yawRate_rad=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_rad=0;
float pitchRate_rad=0;
float yawRate_rad=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: //In [rad/s]
pitchRate_rad = *((float*)(buf.data()+4*1));
@ -277,139 +277,70 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
yawRate_rad = *((float*)(buf.data()+4*3));
break;
case XplaneSimulator::Gload:
accX = *((float*)(buf.data()+4*6)) * GEE;
accY = *((float*)(buf.data()+4*7)) * GEE;
accZ = *((float*)(buf.data()+4*5)) * GEE;
break;
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.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
// Update BaroAltitude object
out.temperature = temperature;
out.pressure = pressure;
// Update attActual object
out.roll = roll; //roll;
out.pitch = pitch; // pitch
out.heading = heading; // yaw
// Update AltitudeActual object
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = altitude;
baroAltData.Temperature = temperature;
baroAltData.Pressure = pressure;
baroAlt->setData(baroAltData);
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_rad;
out.pitchRate = pitchRate_rad;
out.yawRate = yawRate_rad;
// 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_rad + sin(DEG2RAD * roll) * yawRate_rad;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate_rad - sin(DEG2RAD * roll) * pitchRate_rad;
//rawData.gyros[1] = pitchRate;
//rawData.gyros[2] = yawRate;
//rawData.accels[0] = accX;
//rawData.accels[1] = accY;
//rawData.accels[2] = -accZ;
//attRaw->setData(rawData);
Gyros::DataFields gyroData;
memset(&gyroData, 0, sizeof(Gyros::DataFields));
#define Pi 3.141529654
gyroData.x = rollRate_rad*180/Pi;
gyroData.y = pitchRate_rad*180/Pi;
gyroData.z = yawRate_rad*180/Pi;
gyros->setData(gyroData);
Accels::DataFields accelData;
memset(&accelData, 0, sizeof(Accels::DataFields));
accelData.x = accX;
accelData.y = accY;
accelData.z = -accZ;
accels->setData(accelData);
}
// issue manual update
//attActual->updated();
//baroAlt->updated();
//posActual->updated();
}

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

@ -45,12 +45,8 @@ private slots:
void transmitUpdate();
private:
bool once;
float initX;
float initY;
float initZ;
enum XplaneOutputData
{
enum XplaneOutputData //***WARNING***: Elements in this enum are in a precise order, do
{ // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml
FramRate,
Times,
SimStats,
@ -68,10 +64,10 @@ private:
Brakes,
AngularMoments,
AngularAccelerations,
AngularVelocities,
PitchRollHeading,
AngularVelocities,
PitchRollHeading,
AoA,
LatitudeLongitude,
LatitudeLongitudeAltitude,
LocVelDistTraveled
};