Extended HiTL API to the rest of the simulators.

ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp

@@ -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)
@@ -63,8 +60,6 @@ FGSimulator::~FGSimulator()
 
 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
@@ -78,14 +73,14 @@ bool FGSimulator::setupProcess()
     // 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");
+    //	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();
     qDebug() << "setupProcess Thread: "<< mainThread;
@@ -237,22 +232,21 @@ 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;
+    //        float xRate = fields[0].toFloat() * 180.0/M_PI;
     // 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)
-//        float zRate = fields[2].toFloat() * 180.0/M_PI;
+    //        float zRate = fields[2].toFloat() * 180.0/M_PI;
     // Get xAccel (m/s^2)
-//        float xAccel = fields[3].toFloat() * FT2M;
+    float xAccel = fields[3].toFloat() * FT2M;
     // Get yAccel (m/s^2)
-//        float yAccel = fields[4].toFloat() * FT2M;
+    float yAccel = fields[4].toFloat() * FT2M;
     // Get xAccel (m/s^2)
-//        float zAccel = fields[5].toFloat() * FT2M;
+    float zAccel = fields[5].toFloat() * FT2M;
     // Get pitch (deg)
     float pitch = fields[6].toFloat();
     // Get pitchRate (deg/s)
@@ -278,7 +272,7 @@ void FGSimulator::processUpdate(const QByteArray& inp)
     // Get groundspeed (m/s)
     float groundspeed = fields[17].toFloat() * KT2MPS;
     // Get airspeed (m/s)
-//	float airspeed = fields[18].toFloat() * KT2MPS;
+    float airspeed = fields[18].toFloat() * KT2MPS;
     // Get temperature (degC)
     float temperature = fields[19].toFloat();
     // Get pressure (kpa)
@@ -294,112 +288,60 @@ void FGSimulator::processUpdate(const QByteArray& inp)
     int n = fields[24].toInt();
     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}
-	VelocityActual::DataFields velocityActualData;
-	memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
-	velocityActualData.North = velocityActualNorth;
-	velocityActualData.East = velocityActualEast;
-	velocityActualData.Down = velocityActualDown;
-	velActual->setData(velocityActualData);
-	
-	// Update PositionActual.{Nort,East,Down}
-	PositionActual::DataFields positionActualData;
-	memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
-        positionActualData.North = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
-	positionActualData.East = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
-	positionActualData.Down = altitude ; //Multiply by 1 because positionActual expects input in meters.
-        posActual->setData(positionActualData);
-
-    // Update AltitudeActual object
-    BaroAltitude::DataFields baroAltData;
-    memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
-    baroAltData.Altitude = altitudeAGL;
-    baroAltData.Temperature = temperature;
-    baroAltData.Pressure = pressure;
-    baroAlt->setData(baroAltData);
-
-	// Update attActual object
-	AttitudeActual::DataFields attActualData;
-        memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
-	attActualData.Roll = roll;
-	attActualData.Pitch = pitch;
-	attActualData.Yaw = yaw;
-	attActualData.q1 = 0;
-	attActualData.q2 = 0;
-	attActualData.q3 = 0;
-	attActualData.q4 = 0;
-	attActual->setData(attActualData);
-
-	// Update gps objects
-        GPSPosition::DataFields gpsData;
-        memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
-        gpsData.Altitude = altitude;
-        gpsData.Heading = heading;
-	gpsData.Groundspeed = groundspeed;
-        gpsData.Latitude = latitude*1e7;
-        gpsData.Longitude = longitude*1e7;
-	gpsData.Satellites = 10;
-        gpsData.Status = GPSPosition::STATUS_FIX3D;
-        gpsPos->setData(gpsData);
 
     float NED[3];
     // convert from cm back to meters
 
-        double hLLA[3] = {(double) homeData.Latitude / 1e7, (double) homeData.Longitude / 1e7, (double) (homeData.Altitude)};
+    double LLA[3] = {latitude, longitude, altitude};
     double ECEF[3];
     double RNE[9];
-            Utils::CoordinateConversions().RneFromLLA(hLLA,(double (*)[3])RNE);
+    Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
-            Utils::CoordinateConversions().LLA2ECEF(hLLA,ECEF);
+    Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
-                Utils::CoordinateConversions().LLA2Base(hLLA, ECEF, (float (*)[3]) RNE, NED);
+    Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) RNE, NED);
 
-        positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
-        positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
-        positionActualData.Down = NED[2]; //Multiply by 1 because positionActual expects input in meters.
-        posActual->setData(positionActualData);
 
-        // Update AttitudeRaw object (filtered gyros only for now)
+    // Update GPS Position objects
-        //AttitudeRaw::DataFields rawData;
+    out.latitude = latitude * 1e7;
-        //AttitudeRaw::DataFields rawData;
+    out.longitude = longitude * 1e7;
-        Gyros::DataFields gyroData;
+    out.altitude = altitude;
-        Accels::DataFields accelData;
+    out.groundspeed = groundspeed;
-        memset(&gyroData, 0, sizeof(Gyros::DataFields));
+
-        memset(&accelData, 0, sizeof(Accels::DataFields));
+    out.calibratedAirspeed = airspeed;
-        gyroData = gyros->getData();
+
-        accelData = accels->getData();
+
-        //rawData.gyros[0] = rollRate;
+    // Update BaroAltitude object
-        //rawData.gyros[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate;
+    out.temperature = temperature;
-        //rawData.gyros[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate;
+    out.pressure = pressure;
-        //rawData.gyros[1] = pitchRate;
+
-        //rawData.gyros[2] = yawRate;
+    // Update attActual object
-	gyroData.x = rollRate;
+    out.roll = roll;       //roll;
-	gyroData.y = pitchRate;
+    out.pitch = pitch;     // pitch
-	gyroData.z = yawRate;
+    out.heading = yaw; // yaw
-	// TODO: Accels are still missing!!!!
+
-        gyros->setData(gyroData);
+    out.dstN= NED[0];
-        // attRaw->updated();
+    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);
 }
 

ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp

@@ -237,127 +237,84 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
     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
 
-        // 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;
+
+    // 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);
-        attActualData.q1 = quat[0];
-        attActualData.q2 = quat[1];
-        attActualData.q3 = quat[2];
-        attActualData.q4 = quat[3];
-
-        // Update positionActual objects
-        PositionActual::DataFields posData;
-        memset(&posData, 0, sizeof(PositionActual::DataFields));
-        posData.North =  current.Y;
-        posData.East = current.X;
-        posData.Down = current.Z*-1.;
-
-        // Update velocityActual objects
-        VelocityActual::DataFields velData;
-        memset(&velData, 0, sizeof(VelocityActual::DataFields));
-        velData.North = current.dY;
-        velData.East = current.dX;
-        velData.Down = current.dZ*-1.;
-
-        // Update AttitudeRaw object (filtered gyros and accels only for now)
-        //AttitudeRaw::DataFields rawData;
-        //memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
-        //rawData = attRaw->getData();
-	Gyros::DataFields gyroData;
-	Accels::DataFields accelData;
-        memset(&gyroData, 0, sizeof(Gyros::DataFields));
-        memset(&accelData, 0, sizeof(Accels::DataFields));
-	gyroData = gyros->getData();
-	accelData = accels->getData();
-
-	// rotate turn rates and accelerations into body frame
-	// (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
-	gyroData.x = current.dRoll;
-	gyroData.y = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
-	gyroData.z = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
-	// accels are in NED and can be converted using standard ned->local rotation matrix
-	float Rbe[3][3];
     Utils::CoordinateConversions().Quaternion2R(quat,Rbe);
-		accelData.x = current.ddX*Rbe[0][0]
-				+current.ddY*Rbe[0][1]
-				+(current.ddZ+GEE)*Rbe[0][2];
-		accelData.y = current.ddX*Rbe[1][0]
-				+current.ddY*Rbe[1][1]
-				+(current.ddZ+GEE)*Rbe[1][2];
-		accelData.z = - (current.ddX*Rbe[2][0]
-				+current.ddY*Rbe[2][1]
-				+(current.ddZ+GEE)*Rbe[2][2]);
 
-        // Update homelocation
+    //Calculate ECEF
-        HomeLocation::DataFields homeData;
+    double RNE[9];
-        memset(&homeData, 0, sizeof(HomeLocation::DataFields));
+    double ECEF[3];
-        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 GPS Position 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;
+    out.latitude = settings.latitude.toFloat() * 1e7;
-        gpsData.Longitude = LLA[1] * 10e6;
+    out.longitude = settings.longitude.toFloat() * 1e7;
-        gpsData.Satellites = 7;
+    out.groundspeed = current.groundspeed;
-        gpsData.Status = GPSPosition::STATUS_FIX3D;
 
-        // issue manual update
+    out.calibratedAirspeed = current.ias;
-        // update every time (50ms)
+
-        attActual->setData(attActualData);
+    out.dstN=current.Y;
-        //attActual->updated();
+    out.dstE=current.X;
-        //attRaw->setData(rawData);
+    out.dstD=-current.Z;
-	gyros->setData(gyroData);
+
-	accels->setData(accelData);
+    // Update BaroAltitude object
-        //attRaw->updated();
+    out.altitude = current.Z;
-	velActual->setData(velData);
+    out.temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
-	//velActual->updated();
+    out.pressure = PRESSURE(current.Z)/1000.0; // kpa
-	posActual->setData(posData);
+
-	//posActual->updated();
+
-    baroAlt->setData(baroAltData);
+    // Update attActual object
-    //baroAlt->updated();
+    out.roll = current.roll;   //roll;
-	gpsPos->setData(gpsData);
+    out.pitch = current.pitch;  // pitch
-	//gpsPos->updated();
+    out.heading = current.azimuth; // yaw
-	posHome->setData(homeData);
+
-	//posHome->updated();
+
+    // 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);
 }
 
 /**

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

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
@@ -68,7 +68,7 @@ void TraceBuf(const char* buf,int len);
 XplaneSimulator::XplaneSimulator(const SimulatorSettings& 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();
 
 }
 
@@ -104,7 +104,7 @@ void XplaneSimulator::transmitUpdate()
     float ailerons = actData.Roll;
     float elevator = actData.Pitch;
     float rudder = actData.Yaw;
-	float throttle = actData.Throttle*2-1.0;
+    float throttle = actData.Throttle > 0? actData.Throttle : 0;
     float none = -999;
     //quint32 none = *((quint32*)&tmp); // get float as 4 bytes
 
@@ -113,9 +113,9 @@ void XplaneSimulator::transmitUpdate()
     QDataStream stream(&buf,QIODevice::ReadWrite);
 
     // !!! LAN byte order - Big Endian
-        #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
+#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
     stream.setByteOrder(QDataStream::LittleEndian);
-        #endif
+#endif
 
     // 11th data settings (flight con: ail/elv/rud)
     buf.clear();
@@ -196,8 +196,8 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
     float altitude = 0;
     float latitude = 0;
     float longitude = 0;
-	float airspeed = 0;
+    float airspeed_keas = 0;
-	float speed = 0;
+    float groundspeed_ktgs = 0;
     float pitch = 0;
     float roll = 0;
     float heading = 0;
@@ -216,7 +216,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
     float pitchRate_rad=0;
     float yawRate_rad=0;
 
-	QString str;
+    //	QString str;
     QByteArray& buf = const_cast<QByteArray&>(dataBuf);
     QString data(buf);
 
@@ -234,15 +234,15 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
         {
             switch(buf[0]) // switch by id
             {
-			case XplaneSimulator::LatitudeLongitude:
+            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));
+                airspeed_keas = *((float*)(buf.data()+4*2));
-                                speed = *((float*)(buf.data()+4*8));
+                groundspeed_ktgs = *((float*)(buf.data()+4*4));
                 break;
 
             case XplaneSimulator::PitchRollHeading:
@@ -291,125 +291,56 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
         } 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 AltitudeActual object
+        // Update BaroAltitude object
-        BaroAltitude::DataFields baroAltData;
+        out.temperature = temperature;
-        memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
+        out.pressure = pressure;
-        baroAltData.Altitude = altitude;
-        baroAltData.Temperature = temperature;
-        baroAltData.Pressure = pressure;
-        baroAlt->setData(baroAltData);
 
         // Update attActual object
-		AttitudeActual::DataFields attActualData;
+        out.roll = roll;       //roll;
-		memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
+        out.pitch = pitch;     // pitch
-		attActualData.Roll = roll;   //roll;
+        out.heading = heading; // yaw
-		attActualData.Pitch = pitch;  // pitch
-                attActualData.Yaw = heading; // Yaw
-		float rpy[3];
-		float quat[4];
-		rpy[0] = roll;
-		rpy[1] = pitch;
-		rpy[2] = heading;
-		Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
-		attActualData.q1 = quat[0];
-		attActualData.q2 = quat[1];
-		attActualData.q3 = quat[2];
-		attActualData.q4 = quat[3];
-		attActual->setData(attActualData);
 
-		// Update gps objects
-                GPSPosition::DataFields gpsData;
-                memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
-                gpsData.Altitude = altitude;
-                gpsData.Heading = heading;
-                gpsData.Groundspeed = speed;
-                gpsData.Latitude = latitude*1e7;
-                gpsData.Longitude = longitude*1e7;
-                gpsData.Satellites = 10;
-                gpsData.Status = GPSPosition::STATUS_FIX3D;
-                gpsPos->setData(gpsData);
 
-                // Update VelocityActual.{Nort,East,Down}
+        out.dstN=dstY;
-                VelocityActual::DataFields velocityActualData;
+        out.dstE=dstX;
-                memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
+        out.dstD=-dstZ;
-                velocityActualData.North = velY;
-                velocityActualData.East = velX;
-                velocityActualData.Down = -velZ;
-                velActual->setData(velocityActualData);
 
-                // Update PositionActual.{Nort,East,Down}
+        // Update VelocityActual.{North,East,Down}
-                PositionActual::DataFields positionActualData;
+        out.velNorth = velY;
-                memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
+        out.velEast = velX;
-                positionActualData.North = (dstY-initY);
+        out.velDown = -velZ;
-                positionActualData.East = (dstX-initX);
-                positionActualData.Down = -(dstZ-initZ);
-                posActual->setData(positionActualData);
 
-                // Update AttitudeRaw object (filtered gyros only for now)
+        //Update gyroscope sensor data
-                //AttitudeRaw::DataFields rawData;
+        out.rollRate = rollRate_rad;
-                //memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
+        out.pitchRate = pitchRate_rad;
-                //rawData = attRaw->getData();
+        out.yawRate = yawRate_rad;
-                //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;
+        //Update accelerometer sensor data
-                memset(&accelData, 0, sizeof(Accels::DataFields));
+        out.accX = accX;
-		accelData.x = accX;
+        out.accY = accY;
-		accelData.y = accY;
+        out.accZ = -accZ;
-		accelData.z = -accZ;
-		accels->setData(accelData);
 
+        updateUAVOs(out);
     }
     // issue manual update
     //attActual->updated();
-    //baroAlt->updated();
+    //altActual->updated();
     //posActual->updated();
+
 }
 
 

ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h

@@ -45,12 +45,8 @@ private slots:
 	void transmitUpdate();
 
 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,
 		Times,
 		SimStats,
@@ -71,7 +67,7 @@ private:
         AngularVelocities,
         PitchRollHeading,
 		AoA,
-		LatitudeLongitude,
+        LatitudeLongitudeAltitude,
 		LocVelDistTraveled
 	};