From d9139c65299d41fdb677589292344ee354edf9f2 Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Wed, 29 Aug 2012 12:35:46 +0200 Subject: [PATCH] Extended HiTL API to the rest of the simulators. --- .../src/plugins/hitl/fgsimulator.cpp | 344 +++++++-------- .../src/plugins/hitl/il2simulator.cpp | 371 ++++++++-------- .../src/plugins/hitl/simulator.cpp | 5 + .../src/plugins/hitl/xplanesimulator.cpp | 397 ++++++++---------- .../src/plugins/hitl/xplanesimulator.h | 14 +- 5 files changed, 481 insertions(+), 650 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index e2420bc72..8ffa7d192 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -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); } diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index e03f9385c..b585e83f1 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -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=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; } diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 003799efb..547e36492 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -368,6 +368,11 @@ void Simulator::telStatsUpdated(UAVObject* obj) } +void Simulator::resetInitialHomePosition(){ + once=false; +} + + void Simulator::updateUAVOs(Output2OP out){ QTime currentTime = QTime::currentTime(); diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index bbf2c7509..a203557ee 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -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(&code), sizeof(code)); buf.append(reinterpret_cast(&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(dataBuf); - QString data(buf); + // QString str; + QByteArray& buf = const_cast(dataBuf); + QString data(buf); - if(data.left(4) == "DATA") // check type of packet - { - buf.remove(0,5); - if(dataBuf.size() % 36) - { - qxtLog->info("incorrect length of UDP packet: ",buf); - return; // incorrect length of struct - } - // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36 - int channelCounter = dataBuf.size() / 36; - do - { - switch(buf[0]) // switch by id - { - case XplaneSimulator::LatitudeLongitude: - latitude = *((float*)(buf.data()+4*1)); - longitude = *((float*)(buf.data()+4*2)); - altitude = *((float*)(buf.data()+4*3))* FT2M; - break; + 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(); } diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h index 2202f1b79..ca3437475 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h @@ -45,12 +45,8 @@ private slots: void transmitUpdate(); private: - bool once; - float initX; - float initY; - float initZ; - enum XplaneOutputData - { + enum XplaneOutputData //***WARNING***: Elements in this enum are in a precise order, do + { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml FramRate, Times, SimStats, @@ -68,10 +64,10 @@ private: Brakes, AngularMoments, AngularAccelerations, - AngularVelocities, - PitchRollHeading, + AngularVelocities, + PitchRollHeading, AoA, - LatitudeLongitude, + LatitudeLongitudeAltitude, LocVelDistTraveled };