rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-10 18: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

336
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp
View File

@ -34,9 +34,6 @@
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846
#endif #endif
//FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : //FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) :
// Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath), // Simulator(hostAddr, outPort, inPort, manual, binPath, dataPath),
// fgProcess(NULL) // fgProcess(NULL)
@ -50,7 +47,7 @@
//} //}
FGSimulator::FGSimulator(const SimulatorSettings& params) : FGSimulator::FGSimulator(const SimulatorSettings& params) :
Simulator(params) Simulator(params)
{ {
udpCounterFGrecv = 0; udpCounterFGrecv = 0;
udpCounterGCSsend = 0; udpCounterGCSsend = 0;
@ -58,13 +55,11 @@ FGSimulator::FGSimulator(const SimulatorSettings& params) :
FGSimulator::~FGSimulator() 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) void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
{ {
Q_UNUSED(outPort);
if(inSocket->bind(QHostAddress(host), inPort)) if(inSocket->bind(QHostAddress(host), inPort))
emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n"); emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n");
else else
@ -73,31 +68,31 @@ void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
bool FGSimulator::setupProcess() bool FGSimulator::setupProcess()
{ {
QMutexLocker locker(&lock); QMutexLocker locker(&lock);
// Copy FlightGear generic protocol configuration file to the FG protocol directory // 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", // 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 // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml
// QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml"); // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml");
// xmlFile.open(QIODevice::ReadOnly | QIODevice::Text); // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text);
// QString xml = xmlFile.readAll(); // QString xml = xmlFile.readAll();
// xmlFile.close(); // xmlFile.close();
// QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml"); // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml");
// xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text); // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text);
// xmlFileOut.write(xml.toAscii()); // xmlFileOut.write(xml.toAscii());
// xmlFileOut.close(); // xmlFileOut.close();
Qt::HANDLE mainThread = QThread::currentThreadId(); Qt::HANDLE mainThread = QThread::currentThreadId();
qDebug() << "setupProcess Thread: "<< mainThread; qDebug() << "setupProcess Thread: "<< mainThread;
simProcess = new QProcess(); simProcess = new QProcess();
simProcess->setReadChannelMode(QProcess::MergedChannels); simProcess->setReadChannelMode(QProcess::MergedChannels);
connect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead())); connect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
// Note: Only tested on windows 7 // Note: Only tested on windows 7
#if defined(Q_WS_WIN) #if defined(Q_WS_WIN)
QString cmdShell("c:/windows/system32/cmd.exe"); QString cmdShell("c:/windows/system32/cmd.exe");
#else #else
QString cmdShell("bash"); QString cmdShell("bash");
#endif #endif
// Start shell (Note: Could not start FG directly on Windows, only through terminal!) // 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."); "Remote computer must have the correct OpenPilot protocol installed.");
} }
udpCounterGCSsend = 0; udpCounterGCSsend = 0;
return true; return true;
} }
void FGSimulator::processReadyRead() void FGSimulator::processReadyRead()
{ {
QByteArray bytes = simProcess->readAllStandardOutput(); QByteArray bytes = simProcess->readAllStandardOutput();
QString str(bytes); QString str(bytes);
if ( !str.contains("Error reading data") ) // ignore error if ( !str.contains("Error reading data") ) // ignore error
{ {
emit processOutput(str); emit processOutput(str);
} }
} }
void FGSimulator::transmitUpdate() void FGSimulator::transmitUpdate()
@ -178,7 +173,7 @@ void FGSimulator::transmitUpdate()
} }
else else
{ {
// Read ActuatorDesired from autopilot // Read ActuatorDesired from autopilot
actData = actDesired->getData(); actData = actDesired->getData();
ailerons = actData.Roll; ailerons = actData.Roll;
@ -198,16 +193,16 @@ void FGSimulator::transmitUpdate()
{ {
udpCounterGCSsend++; udpCounterGCSsend++;
// Send update to FlightGear // Send update to FlightGear
QString cmd; QString cmd;
cmd = QString("%1,%2,%3,%4,%5\n") cmd = QString("%1,%2,%3,%4,%5\n")
.arg(ailerons) //ailerons .arg(ailerons) //ailerons
.arg(elevator) //elevator .arg(elevator) //elevator
.arg(rudder) //rudder .arg(rudder) //rudder
.arg(throttle) //throttle .arg(throttle) //throttle
.arg(udpCounterGCSsend); //UDP packet counter delay .arg(udpCounterGCSsend); //UDP packet counter delay
QByteArray data = cmd.toAscii(); QByteArray data = cmd.toAscii();
if(outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) if(outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1)
{ {
@ -237,169 +232,116 @@ void FGSimulator::transmitUpdate()
void FGSimulator::processUpdate(const QByteArray& inp) void FGSimulator::processUpdate(const QByteArray& inp)
{ {
//TODO: this does not use the FLIGHT_PARAM structure, it should! //TODO: this does not use the FLIGHT_PARAM structure, it should!
static char once=0; // Split
// Split QString data(inp);
QString data(inp); QStringList fields = data.split(",");
QStringList fields = data.split(","); // Get xRate (deg/s)
// Get xRate (deg/s) // float xRate = fields[0].toFloat() * 180.0/M_PI;
// float xRate = fields[0].toFloat() * 180.0/M_PI; // Get yRate (deg/s)
// Get yRate (deg/s) // float yRate = fields[1].toFloat() * 180.0/M_PI;
// float yRate = fields[1].toFloat() * 180.0/M_PI; // Get zRate (deg/s)
// Get zRate (deg/s) // float zRate = fields[2].toFloat() * 180.0/M_PI;
// float zRate = fields[2].toFloat() * 180.0/M_PI; // Get xAccel (m/s^2)
// Get xAccel (m/s^2) float xAccel = fields[3].toFloat() * FT2M;
// float xAccel = fields[3].toFloat() * FT2M; // Get yAccel (m/s^2)
// Get yAccel (m/s^2) float yAccel = fields[4].toFloat() * FT2M;
// float yAccel = fields[4].toFloat() * FT2M; // Get xAccel (m/s^2)
// Get xAccel (m/s^2) float zAccel = fields[5].toFloat() * FT2M;
// float zAccel = fields[5].toFloat() * FT2M; // Get pitch (deg)
// Get pitch (deg) float pitch = fields[6].toFloat();
float pitch = fields[6].toFloat(); // Get pitchRate (deg/s)
// Get pitchRate (deg/s) float pitchRate = fields[7].toFloat();
float pitchRate = fields[7].toFloat(); // Get roll (deg)
// Get roll (deg) float roll = fields[8].toFloat();
float roll = fields[8].toFloat(); // Get rollRate (deg/s)
// Get rollRate (deg/s) float rollRate = fields[9].toFloat();
float rollRate = fields[9].toFloat(); // Get yaw (deg)
// Get yaw (deg) float yaw = fields[10].toFloat();
float yaw = fields[10].toFloat(); // Get yawRate (deg/s)
// Get yawRate (deg/s) float yawRate = fields[11].toFloat();
float yawRate = fields[11].toFloat(); // Get latitude (deg)
// Get latitude (deg) float latitude = fields[12].toFloat();
float latitude = fields[12].toFloat(); // Get longitude (deg)
// Get longitude (deg) float longitude = fields[13].toFloat();
float longitude = fields[13].toFloat(); // Get heading (deg)
// Get heading (deg) float heading = fields[14].toFloat();
float heading = fields[14].toFloat(); // Get altitude (m)
// Get altitude (m) float altitude = fields[15].toFloat() * FT2M;
float altitude = fields[15].toFloat() * FT2M; // Get altitudeAGL (m)
// Get altitudeAGL (m) float altitudeAGL = fields[16].toFloat() * FT2M;
float altitudeAGL = fields[16].toFloat() * FT2M; // Get groundspeed (m/s)
// Get groundspeed (m/s) float groundspeed = fields[17].toFloat() * KT2MPS;
float groundspeed = fields[17].toFloat() * KT2MPS; // Get airspeed (m/s)
// Get airspeed (m/s) float airspeed = fields[18].toFloat() * KT2MPS;
// float airspeed = fields[18].toFloat() * KT2MPS; // Get temperature (degC)
// Get temperature (degC) float temperature = fields[19].toFloat();
float temperature = fields[19].toFloat(); // Get pressure (kpa)
// Get pressure (kpa) float pressure = fields[20].toFloat() * INHG2KPA;
float pressure = fields[20].toFloat() * INHG2KPA; // Get VelocityActual Down (cm/s)
// Get VelocityActual Down (cm/s) float velocityActualDown = - fields[21].toFloat() * FPS2CMPS;
float velocityActualDown = - fields[21].toFloat() * FPS2CMPS; // Get VelocityActual East (cm/s)
// Get VelocityActual East (cm/s) float velocityActualEast = fields[22].toFloat() * FPS2CMPS;
float velocityActualEast = fields[22].toFloat() * FPS2CMPS; // Get VelocityActual Down (cm/s)
// Get VelocityActual Down (cm/s) float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;
float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;
// Get UDP packets received by FG // Get UDP packets received by FG
int n = fields[24].toInt(); int n = fields[24].toInt();
udpCounterFGrecv = n; udpCounterFGrecv = n;
//run once ///////
HomeLocation::DataFields homeData = posHome->getData(); // Output formatting
if(!once) ///////
{ Output2OP out;
memset(&homeData, 0, sizeof(HomeLocation::DataFields)); memset(&out, 0, sizeof(Output2OP));
// 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} float NED[3];
VelocityActual::DataFields velocityActualData; // convert from cm back to meters
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
velocityActualData.North = velocityActualNorth;
velocityActualData.East = velocityActualEast;
velocityActualData.Down = velocityActualDown;
velActual->setData(velocityActualData);
// Update PositionActual.{Nort,East,Down} double LLA[3] = {latitude, longitude, altitude};
PositionActual::DataFields positionActualData; double ECEF[3];
memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); double RNE[9];
positionActualData.North = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
positionActualData.East = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
positionActualData.Down = altitude ; //Multiply by 1 because positionActual expects input in meters. Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) RNE, NED);
posActual->setData(positionActualData);
// Update AltitudeActual object
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
baroAltData.Altitude = altitudeAGL;
baroAltData.Temperature = temperature;
baroAltData.Pressure = pressure;
baroAlt->setData(baroAltData);
// Update attActual object // Update GPS Position objects
AttitudeActual::DataFields attActualData; out.latitude = latitude * 1e7;
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); out.longitude = longitude * 1e7;
attActualData.Roll = roll; out.altitude = altitude;
attActualData.Pitch = pitch; out.groundspeed = groundspeed;
attActualData.Yaw = yaw;
attActualData.q1 = 0;
attActualData.q2 = 0;
attActualData.q3 = 0;
attActualData.q4 = 0;
attActual->setData(attActualData);
// Update gps objects out.calibratedAirspeed = airspeed;
GPSPosition::DataFields gpsData;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
gpsData.Altitude = altitude;
gpsData.Heading = heading;
gpsData.Groundspeed = groundspeed;
gpsData.Latitude = latitude*1e7;
gpsData.Longitude = longitude*1e7;
gpsData.Satellites = 10;
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
float NED[3];
// convert from cm back to meters
double hLLA[3] = {(double) homeData.Latitude / 1e7, (double) homeData.Longitude / 1e7, (double) (homeData.Altitude)}; // Update BaroAltitude object
double ECEF[3]; out.temperature = temperature;
double RNE[9]; out.pressure = pressure;
Utils::CoordinateConversions().RneFromLLA(hLLA,(double (*)[3])RNE);
Utils::CoordinateConversions().LLA2ECEF(hLLA,ECEF);
Utils::CoordinateConversions().LLA2Base(hLLA, ECEF, (float (*)[3]) RNE, NED);
positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance // Update attActual object
positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance out.roll = roll; //roll;
positionActualData.Down = NED[2]; //Multiply by 1 because positionActual expects input in meters. out.pitch = pitch; // pitch
posActual->setData(positionActualData); out.heading = yaw; // yaw
// Update AttitudeRaw object (filtered gyros only for now) out.dstN= NED[0];
//AttitudeRaw::DataFields rawData; out.dstE= NED[1];
//AttitudeRaw::DataFields rawData; out.dstD= NED[2];
Gyros::DataFields gyroData;
Accels::DataFields accelData; // Update VelocityActual.{North,East,Down}
memset(&gyroData, 0, sizeof(Gyros::DataFields)); out.velNorth = velocityActualNorth;
memset(&accelData, 0, sizeof(Accels::DataFields)); out.velEast = velocityActualEast;
gyroData = gyros->getData(); out.velDown = velocityActualDown;
accelData = accels->getData();
//rawData.gyros[0] = rollRate; //Update gyroscope sensor data
//rawData.gyros[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate; out.rollRate = rollRate;
//rawData.gyros[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate; out.pitchRate = pitchRate;
//rawData.gyros[1] = pitchRate; out.yawRate = yawRate;
//rawData.gyros[2] = yawRate;
gyroData.x = rollRate; //Update accelerometer sensor data
gyroData.y = pitchRate; out.accX = xAccel;
gyroData.z = yawRate; out.accY = yAccel;
// TODO: Accels are still missing!!!! out.accZ = -zAccel;
gyros->setData(gyroData);
// attRaw->updated(); 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 const float IL2Simulator::AIR_CONST_FACTOR = -0.0341631947363104; //several nature constants calculated into one
IL2Simulator::IL2Simulator(const SimulatorSettings& params) : 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() void IL2Simulator::transmitUpdate()
{ {
// Read ActuatorDesired from autopilot // Read ActuatorDesired from autopilot
ActuatorDesired::DataFields actData = actDesired->getData(); ActuatorDesired::DataFields actData = actDesired->getData();
float ailerons = actData.Roll; float ailerons = actData.Roll;
float elevator = actData.Pitch; float elevator = actData.Pitch;
float rudder = actData.Yaw; float rudder = actData.Yaw;
float throttle = actData.Throttle*2-1.0; float throttle = actData.Throttle*2-1.0;
// Send update to Il2 // Send update to Il2
QString cmd; QString cmd;
cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/") cmd=QString("R/30/32/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/")
.arg(throttle) .arg(throttle)
.arg(ailerons) .arg(ailerons)
.arg(elevator) .arg(elevator)
.arg(rudder); .arg(rudder);
QByteArray data = cmd.toAscii(); QByteArray data = cmd.toAscii();
//outSocket->write(data); //outSocket->write(data);
inSocket->write(data); // for IL2 must send to the same port as input!!!!!!!!!!!!! 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 * calculate air density from altitude
*/ */
float IL2Simulator::DENSITY(float alt) { float IL2Simulator::DENSITY(float alt) {
return (GROUNDDENSITY * pow( return (GROUNDDENSITY * pow(
((TEMP_GROUND+(TEMP_LAPSE_RATE*alt))/TEMP_GROUND), ((TEMP_GROUND+(TEMP_LAPSE_RATE*alt))/TEMP_GROUND),
((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) ) ((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) )
); );
} }
/** /**
* calculate air pressure from altitude * calculate air pressure from altitude
*/ */
float IL2Simulator::PRESSURE(float alt) { 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) void IL2Simulator::processUpdate(const QByteArray& inp)
{ {
// save old flight data to calculate delta's later // save old flight data to calculate delta's later
old=current; old=current;
QString data(inp); QString data(inp);
// Split // Split
QStringList fields = data.split("/"); QStringList fields = data.split("/");
// split up response string // split up response string
int t; int t;
for (t=0; t<fields.length(); t++) { for (t=0; t<fields.length(); t++) {
QStringList values = fields[t].split("\\"); QStringList values = fields[t].split("\\");
// parse values // parse values
if (values.length()>=2) { if (values.length()>=2) {
int id = values[0].toInt(); int id = values[0].toInt();
float value = values[1].toFloat(); float value = values[1].toFloat();
switch (id) { switch (id) {
case 30: case 30:
current.ias=value * KMH2MPS; current.ias=value * KMH2MPS;
break; break;
case 32: case 32:
current.dZ=value; current.dZ=value;
break; break;
case 40: case 40:
current.Z=value; current.Z=value;
break; break;
case 42: case 42:
current.azimuth=value; current.azimuth=value;
break; break;
case 46: case 46:
current.roll=-value; current.roll=-value;
break; break;
case 48: case 48:
current.pitch=value; current.pitch=value;
break; break;
} }
}
} }
}
// measure time // measure time
current.dT = ((float)time->restart()) / 1000.0; current.dT = ((float)time->restart()) / 1000.0;
if (current.dT<0.001) current.dT=0.001; if (current.dT<0.001) current.dT=0.001;
current.T = old.T+current.dT; current.T = old.T+current.dT;
current.i = old.i+1; current.i = old.i+1;
if (current.i==1) { if (current.i==1) {
old.dRoll=0; old.dRoll=0;
old.dPitch=0; old.dPitch=0;
old.dAzimuth=0; old.dAzimuth=0;
old.ddX=0; old.ddX=0;
old.ddX=0; old.ddX=0;
old.ddX=0; old.ddX=0;
} }
// calculate TAS from alt and IAS // calculate TAS from alt and IAS
current.tas = TAS(current.ias,current.Z); current.tas = TAS(current.ias,current.Z);
// assume the plane actually flies straight and no wind // assume the plane actually flies straight and no wind
// groundspeed is horizontal vector of TAS // groundspeed is horizontal vector of TAS
current.groundspeed = current.tas*cos(current.pitch*DEG2RAD); current.groundspeed = current.tas*cos(current.pitch*DEG2RAD);
// x and y vector components // x and y vector components
current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD); current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD);
current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD); current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD);
// simple IMS - integration over time the easy way... // simple IMS - integration over time the easy way...
current.X = old.X + (current.dX*current.dT); current.X = old.X + (current.dX*current.dT);
current.Y = old.Y + (current.dY*current.dT); current.Y = old.Y + (current.dY*current.dT);
// accelerations (filtered) // accelerations (filtered)
if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0; if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0;
if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0; if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0;
if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0; if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0;
#define SPEED_FILTER 10 #define SPEED_FILTER 10
current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (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.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.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1);
#define TURN_FILTER 10 #define TURN_FILTER 10
// turn speeds (filtered) // turn speeds (filtered)
if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0; if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0;
if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0; if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0;
if (isnan(old.dRoll) || isinf(old.dRoll)) old.dRoll=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.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.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); 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
// Update attActual object ///////
AttitudeActual::DataFields attActualData; // Output formatting
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); ///////
attActualData.Roll = current.roll; Output2OP out;
attActualData.Pitch = current.pitch; memset(&out, 0, sizeof(Output2OP));
attActualData.Yaw = current.azimuth;
float rpy[3];
float quat[4];
rpy[0]=current.roll;
rpy[1]=current.pitch;
rpy[2]=current.azimuth;
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
attActualData.q1 = quat[0];
attActualData.q2 = quat[1];
attActualData.q3 = quat[2];
attActualData.q4 = quat[3];
// Update positionActual objects // Compute rotation matrix, for later calculations
PositionActual::DataFields posData; float Rbe[3][3];
memset(&posData, 0, sizeof(PositionActual::DataFields)); float rpy[3];
posData.North = current.Y; float quat[4];
posData.East = current.X; rpy[0]=current.roll;
posData.Down = current.Z*-1.; rpy[1]=current.pitch;
rpy[2]=current.azimuth;
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
Utils::CoordinateConversions().Quaternion2R(quat,Rbe);
// Update velocityActual objects //Calculate ECEF
VelocityActual::DataFields velData; double RNE[9];
memset(&velData, 0, sizeof(VelocityActual::DataFields)); double ECEF[3];
velData.North = current.dY; double LLA[3];
velData.East = current.dX; LLA[0]=settings.latitude.toFloat();
velData.Down = current.dZ*-1.; 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) // Update GPS Position objects
//AttitudeRaw::DataFields rawData; double NED[3];
//memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); NED[0] = current.Y;
//rawData = attRaw->getData(); NED[1] = current.X;
Gyros::DataFields gyroData; NED[2] = -current.Z;
Accels::DataFields accelData; Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA);
memset(&gyroData, 0, sizeof(Gyros::DataFields)); out.latitude = settings.latitude.toFloat() * 1e7;
memset(&accelData, 0, sizeof(Accels::DataFields)); out.longitude = settings.longitude.toFloat() * 1e7;
gyroData = gyros->getData(); out.groundspeed = current.groundspeed;
accelData = accels->getData();
// rotate turn rates and accelerations into body frame out.calibratedAirspeed = current.ias;
// (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
gyroData.x = current.dRoll;
gyroData.y = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
gyroData.z = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
// accels are in NED and can be converted using standard ned->local rotation matrix
float Rbe[3][3];
Utils::CoordinateConversions().Quaternion2R(quat,Rbe);
accelData.x = current.ddX*Rbe[0][0]
+current.ddY*Rbe[0][1]
+(current.ddZ+GEE)*Rbe[0][2];
accelData.y = current.ddX*Rbe[1][0]
+current.ddY*Rbe[1][1]
+(current.ddZ+GEE)*Rbe[1][2];
accelData.z = - (current.ddX*Rbe[2][0]
+current.ddY*Rbe[2][1]
+(current.ddZ+GEE)*Rbe[2][2]);
// Update homelocation out.dstN=current.Y;
HomeLocation::DataFields homeData; out.dstE=current.X;
memset(&homeData, 0, sizeof(HomeLocation::DataFields)); out.dstD=-current.Z;
homeData = posHome->getData();
homeData.Latitude = settings.latitude.toFloat() * 10e6;
homeData.Longitude = settings.longitude.toFloat() * 10e6;
homeData.Altitude = 0;
double LLA[3];
LLA[0]=settings.latitude.toFloat();
LLA[1]=settings.longitude.toFloat();
LLA[2]=0;
double ECEF[3];
double RNE[9];
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
homeData.Be[0]=0;
homeData.Be[1]=0;
homeData.Be[2]=0;
homeData.Set=1;
// Update gps objects // Update BaroAltitude object
GPSPosition::DataFields gpsData; out.altitude = current.Z;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); out.temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
gpsData.Altitude = current.Z; out.pressure = PRESSURE(current.Z)/1000.0; // kpa
gpsData.Heading = current.azimuth;
gpsData.Groundspeed = current.groundspeed;
double NED[3];
NED[0] = current.Y;
NED[1] = current.X;
NED[2] = -current.Z;
Utils::CoordinateConversions().NED2LLA_HomeECEF(ECEF,NED,LLA);
gpsData.Latitude = LLA[0] * 10e6;
gpsData.Longitude = LLA[1] * 10e6;
gpsData.Satellites = 7;
gpsData.Status = GPSPosition::STATUS_FIX3D;
// issue manual update
// update every time (50ms) // Update attActual object
attActual->setData(attActualData); out.roll = current.roll; //roll;
//attActual->updated(); out.pitch = current.pitch; // pitch
//attRaw->setData(rawData); out.heading = current.azimuth; // yaw
gyros->setData(gyroData);
accels->setData(accelData);
//attRaw->updated(); // Update VelocityActual.{North,East,Down}
velActual->setData(velData); out.velNorth = current.dY;
//velActual->updated(); out.velEast = current.dX;
posActual->setData(posData); out.velDown = -current.dZ;
//posActual->updated();
baroAlt->setData(baroAltData); // rotate turn rates and accelerations into body frame
//baroAlt->updated(); // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
gpsPos->setData(gpsData); out.rollRate = current.dRoll;
//gpsPos->updated(); out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
posHome->setData(homeData); out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
//posHome->updated();
//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 IL2Simulator::angleDifference(float a, float b)
{ {
float d=a-b; float d=a-b;
if (d>180) d-=360; if (d>180) d-=360;
if (d<-180)d+=360; if (d<-180)d+=360;
return d; 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){ void Simulator::updateUAVOs(Output2OP out){
QTime currentTime = QTime::currentTime(); 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 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
/** /**
* Description of X-Plane Protocol: * Description of X-Plane Protocol:
* *
* To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu * 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 * struct data_struct
* { * {
* int index; // data index, the index into the list of variables * 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.. * 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) * 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); void TraceBuf(const char* buf,int len);
XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : 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); inSocket->bind(QHostAddress(host), inPort);
//outSocket->bind(QHostAddress(host), outPort); //outSocket->bind(QHostAddress(host), outPort);
once = false; resetInitialHomePosition();
} }
@ -99,41 +99,41 @@ bool XplaneSimulator::setupProcess()
*/ */
void XplaneSimulator::transmitUpdate() void XplaneSimulator::transmitUpdate()
{ {
//Read ActuatorDesired from autopilot //Read ActuatorDesired from autopilot
ActuatorDesired::DataFields actData = actDesired->getData(); ActuatorDesired::DataFields actData = actDesired->getData();
float ailerons = actData.Roll; float ailerons = actData.Roll;
float elevator = actData.Pitch; float elevator = actData.Pitch;
float rudder = actData.Yaw; float rudder = actData.Yaw;
float throttle = actData.Throttle*2-1.0; float throttle = actData.Throttle > 0? actData.Throttle : 0;
float none = -999; float none = -999;
//quint32 none = *((quint32*)&tmp); // get float as 4 bytes //quint32 none = *((quint32*)&tmp); // get float as 4 bytes
quint32 code; quint32 code;
QByteArray buf; QByteArray buf;
QDataStream stream(&buf,QIODevice::ReadWrite); QDataStream stream(&buf,QIODevice::ReadWrite);
// !!! LAN byte order - Big Endian // !!! LAN byte order - Big Endian
#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
stream.setByteOrder(QDataStream::LittleEndian); stream.setByteOrder(QDataStream::LittleEndian);
#endif #endif
// 11th data settings (flight con: ail/elv/rud) // 11th data settings (flight con: ail/elv/rud)
buf.clear(); buf.clear();
code = 11; code = 11;
//quint8 header[] = "DATA"; //quint8 header[] = "DATA";
/* /*
stream << *((quint32*)header) << stream << *((quint32*)header) <<
(quint8)0x30 << (quint8)0x30 <<
code << code <<
*((quint32*)&elevator) << *((quint32*)&elevator) <<
*((quint32*)&ailerons) << *((quint32*)&ailerons) <<
*((quint32*)&rudder) << *((quint32*)&rudder) <<
none << none <<
*((quint32*)&ailerons) << *((quint32*)&ailerons) <<
none << none <<
none << none <<
none; none;
*/ */
buf.append("DATA0"); buf.append("DATA0");
buf.append(reinterpret_cast<const char*>(&code), sizeof(code)); buf.append(reinterpret_cast<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator)); 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"); 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 /** !!! this settings was given from ardupilot X-Plane.pl, I comment them
but if it needed comment should be removed !!! but if it needed comment should be removed !!!
// 8th data settings (joystick 1 ail/elv/rud) // 8th data settings (joystick 1 ail/elv/rud)
stream << "DATA0" << quint32(11) << elevator << ailerons << rudder stream << "DATA0" << quint32(11) << elevator << ailerons << rudder
<< float(-999) << float(-999) << float(-999) << float(-999) << float(-999); << float(-999) << float(-999) << float(-999) << float(-999) << float(-999);
outSocket->write(buf); outSocket->write(buf);
*/ */
} }
@ -193,83 +193,83 @@ void XplaneSimulator::transmitUpdate()
*/ */
void XplaneSimulator::processUpdate(const QByteArray& dataBuf) void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
{ {
float altitude = 0; float altitude = 0;
float latitude = 0; float latitude = 0;
float longitude = 0; float longitude = 0;
float airspeed = 0; float airspeed_keas = 0;
float speed = 0; float groundspeed_ktgs = 0;
float pitch = 0; float pitch = 0;
float roll = 0; float roll = 0;
float heading = 0; float heading = 0;
float pressure = 0; float pressure = 0;
float temperature = 0; float temperature = 0;
float velX = 0; float velX = 0;
float velY = 0; float velY = 0;
float velZ = 0; float velZ = 0;
float dstX = 0; float dstX = 0;
float dstY = 0; float dstY = 0;
float dstZ = 0; float dstZ = 0;
float accX = 0; float accX = 0;
float accY = 0; float accY = 0;
float accZ = 0; float accZ = 0;
float rollRate_rad=0; float rollRate_rad=0;
float pitchRate_rad=0; float pitchRate_rad=0;
float yawRate_rad=0; float yawRate_rad=0;
QString str; // QString str;
QByteArray& buf = const_cast<QByteArray&>(dataBuf); QByteArray& buf = const_cast<QByteArray&>(dataBuf);
QString data(buf); QString data(buf);
if(data.left(4) == "DATA") // check type of packet if(data.left(4) == "DATA") // check type of packet
{ {
buf.remove(0,5); buf.remove(0,5);
if(dataBuf.size() % 36) if(dataBuf.size() % 36)
{ {
qxtLog->info("incorrect length of UDP packet: ",buf); qxtLog->info("incorrect length of UDP packet: ",buf);
return; // incorrect length of struct return; // incorrect length of struct
} }
// check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36
int channelCounter = dataBuf.size() / 36; int channelCounter = dataBuf.size() / 36;
do do
{ {
switch(buf[0]) // switch by id switch(buf[0]) // switch by id
{ {
case XplaneSimulator::LatitudeLongitude: case XplaneSimulator::LatitudeLongitudeAltitude:
latitude = *((float*)(buf.data()+4*1)); latitude = *((float*)(buf.data()+4*1));
longitude = *((float*)(buf.data()+4*2)); longitude = *((float*)(buf.data()+4*2));
altitude = *((float*)(buf.data()+4*3))* FT2M; altitude = *((float*)(buf.data()+4*3))* FT2M;
break; break;
case XplaneSimulator::Speed: case XplaneSimulator::Speed:
airspeed = *((float*)(buf.data()+4*7)); airspeed_keas = *((float*)(buf.data()+4*2));
speed = *((float*)(buf.data()+4*8)); groundspeed_ktgs = *((float*)(buf.data()+4*4));
break; break;
case XplaneSimulator::PitchRollHeading: case XplaneSimulator::PitchRollHeading:
pitch = *((float*)(buf.data()+4*1)); pitch = *((float*)(buf.data()+4*1));
roll = *((float*)(buf.data()+4*2)); roll = *((float*)(buf.data()+4*2));
heading = *((float*)(buf.data()+4*3)); heading = *((float*)(buf.data()+4*3));
break; break;
/* /*
case XplaneSimulator::SystemPressures: case XplaneSimulator::SystemPressures:
pressure = *((float*)(buf.data()+4*1)); pressure = *((float*)(buf.data()+4*1));
break; break;
*/ */
case XplaneSimulator::AtmosphereWeather: case XplaneSimulator::AtmosphereWeather:
pressure = *((float*)(buf.data()+4*1)) * INHG2KPA; pressure = *((float*)(buf.data()+4*1)) * INHG2KPA;
temperature = *((float*)(buf.data()+4*2)); temperature = *((float*)(buf.data()+4*2));
break; break;
case XplaneSimulator::LocVelDistTraveled: case XplaneSimulator::LocVelDistTraveled:
dstX = *((float*)(buf.data()+4*1)); dstX = *((float*)(buf.data()+4*1));
dstY = - *((float*)(buf.data()+4*3)); dstY = - *((float*)(buf.data()+4*3));
dstZ = *((float*)(buf.data()+4*2)); dstZ = *((float*)(buf.data()+4*2));
velX = *((float*)(buf.data()+4*4)); velX = *((float*)(buf.data()+4*4));
velY = - *((float*)(buf.data()+4*6)); velY = - *((float*)(buf.data()+4*6));
velZ = *((float*)(buf.data()+4*5)); velZ = *((float*)(buf.data()+4*5));
break; break;
case XplaneSimulator::AngularVelocities: //In [rad/s] case XplaneSimulator::AngularVelocities: //In [rad/s]
pitchRate_rad = *((float*)(buf.data()+4*1)); pitchRate_rad = *((float*)(buf.data()+4*1));
@ -277,139 +277,70 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
yawRate_rad = *((float*)(buf.data()+4*3)); yawRate_rad = *((float*)(buf.data()+4*3));
break; break;
case XplaneSimulator::Gload: case XplaneSimulator::Gload:
accX = *((float*)(buf.data()+4*6)) * GEE; accX = *((float*)(buf.data()+4*6)) * GEE;
accY = *((float*)(buf.data()+4*7)) * GEE; accY = *((float*)(buf.data()+4*7)) * GEE;
accZ = *((float*)(buf.data()+4*5)) * GEE; accZ = *((float*)(buf.data()+4*5)) * GEE;
break; break;
default: default:
break; break;
} }
channelCounter--; channelCounter--;
buf.remove(0,36); buf.remove(0,36);
} while (channelCounter); } while (channelCounter);
HomeLocation::DataFields homeData = posHome->getData(); ///////
if(!once) // Output formatting
{ ///////
// Upon startup, we reset the HomeLocation object to Output2OP out;
// the plane's location: memset(&out, 0, sizeof(Output2OP));
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
// Update homelocation
homeData.Latitude = latitude * 1e7;
homeData.Longitude = longitude * 1e7;
homeData.Altitude = altitude;
double LLA[3];
LLA[0]=latitude;
LLA[1]=longitude;
LLA[2]=altitude;
double ECEF[3];
double RNE[9];
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
homeData.Be[0]=0;
homeData.Be[1]=0;
homeData.Be[2]=0;
posHome->setData(homeData);
posHome->updated();
// Initialize the initial distance // Update GPS Position objects
initX = dstX; out.latitude = latitude * 1e7;
initY = dstY; out.longitude = longitude * 1e7;
initZ = dstZ; out.altitude = altitude;
once=1; 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 out.dstN=dstY;
BaroAltitude::DataFields baroAltData; out.dstE=dstX;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields)); out.dstD=-dstZ;
baroAltData.Altitude = altitude;
baroAltData.Temperature = temperature;
baroAltData.Pressure = pressure;
baroAlt->setData(baroAltData);
// Update attActual object // Update VelocityActual.{North,East,Down}
AttitudeActual::DataFields attActualData; out.velNorth = velY;
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); out.velEast = velX;
attActualData.Roll = roll; //roll; out.velDown = -velZ;
attActualData.Pitch = pitch; // pitch
attActualData.Yaw = heading; // Yaw
float rpy[3];
float quat[4];
rpy[0] = roll;
rpy[1] = pitch;
rpy[2] = heading;
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
attActualData.q1 = quat[0];
attActualData.q2 = quat[1];
attActualData.q3 = quat[2];
attActualData.q4 = quat[3];
attActual->setData(attActualData);
// Update gps objects //Update gyroscope sensor data
GPSPosition::DataFields gpsData; out.rollRate = rollRate_rad;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); out.pitchRate = pitchRate_rad;
gpsData.Altitude = altitude; out.yawRate = yawRate_rad;
gpsData.Heading = heading;
gpsData.Groundspeed = speed;
gpsData.Latitude = latitude*1e7;
gpsData.Longitude = longitude*1e7;
gpsData.Satellites = 10;
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
// Update VelocityActual.{Nort,East,Down} //Update accelerometer sensor data
VelocityActual::DataFields velocityActualData; out.accX = accX;
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields)); out.accY = accY;
velocityActualData.North = velY; out.accZ = -accZ;
velocityActualData.East = velX;
velocityActualData.Down = -velZ;
velActual->setData(velocityActualData);
// Update PositionActual.{Nort,East,Down} updateUAVOs(out);
PositionActual::DataFields positionActualData; }
memset(&positionActualData, 0, sizeof(PositionActual::DataFields)); // issue manual update
positionActualData.North = (dstY-initY); //attActual->updated();
positionActualData.East = (dstX-initX); //altActual->updated();
positionActualData.Down = -(dstZ-initZ); //posActual->updated();
posActual->setData(positionActualData);
// Update AttitudeRaw object (filtered gyros only for now)
//AttitudeRaw::DataFields rawData;
//memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
//rawData = attRaw->getData();
//rawData.gyros[0] = rollRate;
//rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate_rad + sin(DEG2RAD * roll) * yawRate_rad;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate_rad - sin(DEG2RAD * roll) * pitchRate_rad;
//rawData.gyros[1] = pitchRate;
//rawData.gyros[2] = yawRate;
//rawData.accels[0] = accX;
//rawData.accels[1] = accY;
//rawData.accels[2] = -accZ;
//attRaw->setData(rawData);
Gyros::DataFields gyroData;
memset(&gyroData, 0, sizeof(Gyros::DataFields));
#define Pi 3.141529654
gyroData.x = rollRate_rad*180/Pi;
gyroData.y = pitchRate_rad*180/Pi;
gyroData.z = yawRate_rad*180/Pi;
gyros->setData(gyroData);
Accels::DataFields accelData;
memset(&accelData, 0, sizeof(Accels::DataFields));
accelData.x = accX;
accelData.y = accY;
accelData.z = -accZ;
accels->setData(accelData);
}
// issue manual update
//attActual->updated();
//baroAlt->updated();
//posActual->updated();
} }

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

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