sendPOSI command change (double for lat/lon/h) (#111)

* Updated POSI to use doubles for lat/lon/alt, as step size for floats was unacceptably large at high longitudes.

C/monitorExample/main.c

@@ -70,7 +70,7 @@ int main(void)
     tv.tv_usec = 100 * 1000;
 	while (1)
 	{
-		float posi[7];
+		float posi[7]; // FIXME: change this to the 64-bit lat/lon/h
 		int result = getPOSI(client, posi, aircraftNum);
 		if (result < 0) // Error in getPOSI
 		{

C/playbackExample/src/playback.c

@@ -89,10 +89,10 @@ void playback(char* path, int interval)
 	displayMsg("Starting Playback...");
 
 	XPCSocket sock = openUDP("127.0.0.1");
-	float posi[7];
+	double posi[7];
 	while (!feof(fd) && !ferror(fd))
 	{
-		int result = fscanf(fd, "%f, %f, %f, %f, %f, %f, %f\n",
+		int result = fscanf(fd, "%lf, %lf, %lf, %lf, %lf, %lf, %lf\n",
 			&posi[0], &posi[1], &posi[2], &posi[3], &posi[4], &posi[5], &posi[6]);
 		playbackSleep(interval);
 		if (result != 7)

C/src/xplaneConnect.c

@@ -563,13 +563,14 @@ int getPOSI(XPCSocket sock, float values[7], char ac)
 		printError("getPOSI", "Unexpected response length.");
 		return -3;
 	}
+    // TODO: change this to the 64-bit lat/lon/h
 
 	// Copy response into values
 	memcpy(values, readBuffer + 6, 7 * sizeof(float));
 	return 0;
 }
 
-int sendPOSI(XPCSocket sock, float values[], int size, char ac)
+int sendPOSI(XPCSocket sock, double values[], int size, char ac)
 {
 	// Validate input
 	if (ac < 0 || ac > 20)
@@ -584,23 +585,32 @@ int sendPOSI(XPCSocket sock, float values[], int size, char ac)
 	}
 
 	// Setup command
-	// 5 byte header + up to 7 values * 5 bytes each
+	unsigned char buffer[46] = "POSI";
-	unsigned char buffer[40] = "POSI";
+	buffer[4] = 0xff; //Placeholder for message length
 	buffer[5] = ac;
     int i; // iterator
-	for (i = 0; i < 7; i++)
+
+	for (i = 0; i < 7; i++) // double for lat/lon/h
 	{
-		float val = -998;
+		double val = -998;
 
 		if (i < size)
 		{
 			val = values[i];
 		}
-		*((float*)(buffer + 6 + i * 4)) = val;
+		if (i < 3) /* lat/lon/h */
+		{
+			memcpy(&buffer[6 + i*8], &val, sizeof(double));
+		}
+		else /* attitude and gear */
+		{
+            float f = (float)val;
+            memcpy(&buffer[18 + i*4], &f, sizeof(float));
+		}
 	}
 
 	// Send Command
-	if (sendUDP(sock, buffer, 40) < 0)
+	if (sendUDP(sock, buffer, 46) < 0)
 	{
 		printError("sendPOSI", "Failed to send command");
 		return -3;
@@ -735,7 +745,7 @@ int sendTEXT(XPCSocket sock, char* msg, int x, int y)
 	size_t len = 14 + msgLen;
 	memcpy(buffer + 5, &x, sizeof(int));
 	memcpy(buffer + 9, &y, sizeof(int));
-	buffer[13] = msgLen;
+	buffer[13] = (unsigned char)msgLen;
 	strncpy(buffer + 14, msg, msgLen);
 
 	// Send Command

C/src/xplaneConnect.h

@@ -213,7 +213,7 @@ int getPOSI(XPCSocket sock, float values[7], char ac);
 /// \param size   The number of elements in values.
 /// \param ac     The aircraft number to set the position of. 0 for the player aircraft.
 /// \returns      0 if successful, otherwise a negative value.
-int sendPOSI(XPCSocket sock, float values[], int size, char ac);
+int sendPOSI(XPCSocket sock, double values[], int size, char ac);
 
 // Controls
 

C/xpcExample/xpcExample/src/main.cpp

@@ -33,7 +33,7 @@ int main()
 
 	// Set Location/Orientation (sendPOSI)
 	// Set Up Position Array
-	float POSI[9] = { 0.0 };
+	double POSI[9] = { 0.0 };
 	POSI[0] = 37.524;     // Lat
 	POSI[1] = -122.06899; // Lon
 	POSI[2] = 2500;       // Alt

Java/Examples/BasicOperation/src/Main.java

@@ -25,11 +25,11 @@ public class Main
             xpc.getDREF("sim/test/test_float");
 
             System.out.println("Setting player aircraft position");
-            float[] posi = new float[] {37.524F, -122.06899F, 2500, 0, 0, 0, 1};
+            double[] posi = new double[] {37.524, -122.06899, 2500, 0, 0, 0, 1};
             xpc.sendPOSI(posi);
 
             System.out.println("Setting another aircraft position");
-            posi[0] = 37.52465F;
+            posi[0] = 37.52465;
             posi[4] = 20;
             xpc.sendPOSI(posi, 1);
 

Java/Examples/ContinuousOperation/src/Main.java

@@ -20,7 +20,7 @@ public class Main
             int aircraft = 0;
             while(true)
             {
-                float[] posi = xpc.getPOSI(aircraft);
+                float[] posi = xpc.getPOSI(aircraft); // FIXME: change this to 64-bit double
                 float[] ctrl = xpc.getCTRL(aircraft);
 
                 System.out.format("Loc: (%4f, %4f, %4f) Aileron:%2f Elevator:%2f Rudder:%2f\n",

Java/Examples/Playback/src/gov/nasa/xpc/Main.java

@@ -99,7 +99,7 @@ public class Main
             {
                 for (int i = 0; i < count; ++i)
                 {
-                    float[] posi = xpc.getPOSI(0);
+                    float[] posi = xpc.getPOSI(0); // FIXME: change this to 64-bit double
                     writer.write(String.format("%1$f, %2$f, %3$f, %4$f, %5$f, %6$f, %7$f\n",
                             posi[0], posi[1], posi[2], posi[3], posi[4], posi[5], posi[6]));
                     try
@@ -125,11 +125,11 @@ public class Main
             {
                 while(reader.hasNextLine())
                 {
-                    float[] posi = new float[7];
+                    double[] posi = new double[7];
                     for (int i = 0; i < 7; ++i)
                     {
                         String s = reader.next();
-                        posi[i] = Float.parseFloat(s);
+                        posi[i] = Double.parseDouble(s);
                     }
                     reader.nextLine();
                     xpc.sendPOSI(posi);

Java/src/XPlaneConnect.java

@@ -549,7 +549,7 @@ public class XPlaneConnect implements AutoCloseable
      * @return An array containing control surface data in the same format as {@code sendPOSI}.
      * @throws IOException If the command cannot be sent or a response cannot be read.
      */
-    public float[] getPOSI(int ac) throws IOException
+    public double[] getPOSI(int ac) throws IOException
     {
         // Send request
         ByteArrayOutputStream os = new ByteArrayOutputStream();
@@ -570,7 +570,7 @@ public class XPlaneConnect implements AutoCloseable
         }
 
         // Parse response
-        float[] result = new float[7];
+        double[] result = new double[7];
         ByteBuffer bb = ByteBuffer.wrap(data);
         bb.order(ByteOrder.LITTLE_ENDIAN);
         for(int i = 0; i < 7; ++i)
@@ -600,13 +600,13 @@ public class XPlaneConnect implements AutoCloseable
      *                 </p>
      * @throws IOException If the command can not be sent.
      */
-    public void sendPOSI(float[] values) throws IOException
+    public void sendPOSI(double[] values) throws IOException
     {
         sendPOSI(values, 0);
     }
 
     /**
-     * Sets the position of the specified ac.
+     * Sets the position of the specified ac with double precision coordinates.
      *
      * @param values   <p>An array containing position elements as follows:</p>
      *                 <ol>
@@ -626,7 +626,7 @@ public class XPlaneConnect implements AutoCloseable
      * @param ac The ac to set. 0 for the player ac.
      * @throws IOException If the command can not be sent.
      */
-    public void sendPOSI(float[] values, int ac) throws IOException
+    public void sendPOSI(double[] values, int ac) throws IOException
     {
         //Preconditions
         if(values == null)
@@ -644,15 +644,22 @@ public class XPlaneConnect implements AutoCloseable
 
         //Pad command values and convert to bytes
         int i;
-        ByteBuffer bb = ByteBuffer.allocate(28);
+        ByteBuffer bb = ByteBuffer.allocate(40);
         bb.order(ByteOrder.LITTLE_ENDIAN);
         for(i = 0; i < values.length; ++i)
         {
-            bb.putFloat(i * 4, values[i]);
+            if(i<3) /* lat/lon/height as double */
+            {
+                bb.putDouble(values[i]);
+            }
+            else
+            {
+                bb.putFloat((float)values[i]);
+            }
         }
         for(; i < 7; ++i)
         {
-            bb.putFloat(i * 4, -998);
+            bb.putFloat(-998);
         }
 
         //Build and send message

MATLAB/+XPlaneConnect/sendPOSI.m

@@ -40,7 +40,7 @@ if ~exist('socket', 'var')
 end
 
 %% Validate input
-posi = single(posi);
+posi = double(posi);
 if ~exist('ac', 'var')
     ac = 0;
 end

Python/src/xpc.py

@@ -189,6 +189,7 @@ class XPlaneConnect(object):
         if ac < 0 or ac > 20:
             raise ValueError("Aircraft number must be between 0 and 20.")
 
+        # FIXME update this to the 64-bit double lat/lon/h
         # Pack message
         buffer = struct.pack("<4sxB", "POSI", ac)
         for i in range(7):

TestScripts/C Tests/PosiTests.h

@@ -6,7 +6,7 @@
 #include "Test.h"
 #include "xplaneConnect.h"
 
-int doPOSITest(char* drefs[7], float values[], int size, int ac, float expected[7])
+int doPOSITest(char* drefs[7], double values[], int size, int ac, double expected[7])
 {
 	float* data[7];
 	int sizes[7];
@@ -32,15 +32,15 @@ int doPOSITest(char* drefs[7], float values[], int size, int ac, float expected[
 	}
 
 	// Test values
-	float actual[7];
+	double actual[7];
 	for (int i = 0; i < 7; ++i)
 	{
 		actual[i] = data[i][0];
 	}
-	return compareArray(expected, actual, 7);
+	return compareDoubleArray(expected, actual, 7);
 }
 
-int doGETPTest(float values[7], int ac, float expected[7])
+int doGETPTest(double values[7], int ac, double expected[7])
 {
 	// Execute Test
 	float actual[7];
@@ -70,8 +70,8 @@ int doGETPTest(float values[7], int ac, float expected[7])
 int basicPOSITest(char** drefs, int ac)
 {
 	// Set psoition and initial orientation
-	float POSI[7] = { 37.524F, -122.06899F, 2500, 0, 0, 0, 1 };
+	double POSI[7] = { 37.524, -122.06899, 2500, 0, 0, 0, 1 };
-	float expected[7] = { 37.524F, -122.06899F, 2500, 0, 0, 0, 1 };
+	double expected[7] = { 37.524, -122.06899, 2500, 0, 0, 0, 1 };
 	int result = doPOSITest(drefs, POSI, 7, ac, expected);
 	if (result < 0)
 	{
@@ -79,9 +79,9 @@ int basicPOSITest(char** drefs, int ac)
 	}
 
 	// Set orientation
-	POSI[0] = -998.0F;
+	POSI[0] = -998.0;
-	POSI[1] = -998.0F;
+	POSI[1] = -998.0;
-	POSI[2] = -998.0F;
+	POSI[2] = -998.0;
 	POSI[3] = 5.0F;
 	POSI[4] = -5.0F;
 	POSI[5] = 10.0F;
@@ -101,10 +101,10 @@ int basicPOSITest(char** drefs, int ac)
 	expected[0] = loc[0][0];
 	expected[1] = loc[1][0];
 	expected[2] = loc[2][0];
-	expected[3] = 5.0F;
+	expected[3] = 5.0;
-	expected[4] = -5.0F;
+	expected[4] = -5.0;
-	expected[5] = 10.0F;
+	expected[5] = 10.0;
-	expected[6] = 0.0F;
+	expected[6] = 0.0;
 	result = doPOSITest(drefs, POSI, 7, ac, expected);
 	if (result < 0)
 	{
@@ -145,13 +145,13 @@ int testPOSI_NonPlayer()
 
 int testGetPOSI_Player()
 {
-	float POSI[7] = { 37.524F, -122.06899F, 2500, 0, 0, 0, 1 };
+	double POSI[7] = { 37.524, -122.06899, 2500, 0, 0, 0, 1 };
 	return doGETPTest(POSI, 0, POSI);
 }
 
 int testGetPOSI_NonPlayer()
 {
-	float POSI[7] = { 37.624F, -122.06899F, 1500, 0, 0, 0, 1 };
+	double POSI[7] = { 37.624, -122.06899, 1500, 0, 0, 0, 1 };
 	return doGETPTest(POSI, 3, POSI);
 }
 

TestScripts/C Tests/SimuTests.h

@@ -16,7 +16,7 @@ int doSIMUTest(int value, float expected)
 	int result = pauseSim(sock, value);
 	if (result >= 0)
 	{
-		result = getDREF(sock, dref, &actual, &size);
+		result = getDREF(sock, dref, actual, &size);
 	}
 	closeUDP(sock);
 	if (result < 0)

TestScripts/C Tests/Test.c

@@ -57,3 +57,27 @@ int compareArrays(float* expected[], int esizes[], float* actual[], int asizes[]
 	}
 	return 0;
 }
+
+int compareDoubleArray(double expected[], double actual[], int size)
+{
+	return compareDoubleArrays(&expected, &size, &actual, &size, 1);
+}
+
+int compareDoubleArrays(double* expected[], int esizes[], double* actual[], int asizes[], int count)
+{
+	for (int i = 0; i < count; ++i)
+	{
+		if (esizes[i] != asizes[i])
+		{
+			return -100 - i;
+		}
+		for (int j = 0; j < esizes[i]; ++j)
+		{
+			if (!feq(actual[i][j], expected[i][j]) && !isnan(expected[i][j]))
+			{
+				return -1000 - i * 100 - j;
+			}
+		}
+	}
+	return 0;
+}

TestScripts/C Tests/Test.h

@@ -33,5 +33,7 @@ void runTest(int(*test)(), char* name);
 int compareFloat(float expected, float actual);
 int compareArray(float expected[], float actual[], int size);
 int compareArrays(float* expected[], int esizes[], float* actual[], int asizes[], int count);
+int compareDoubleArray(double expected[], double actual[], int size);
+int compareDoubleArrays(double* expected[], int esizes[], double* actual[], int asizes[], int count);
 
 #endif

TestScripts/Java Tests/XPlaneConnectTest.java

@@ -542,15 +542,15 @@ public class XPlaneConnectTest
     public void testSendPOSI() throws IOException
     {
         String[] drefs = {
-            "sim/flightmodel/position/latitude",
+                "sim/flightmodel/position/latitude",
-            "sim/flightmodel/position/longitude",
+                "sim/flightmodel/position/longitude",
-            "sim/flightmodel/position/y_agl",
+                "sim/flightmodel/position/y_agl",
-            "sim/flightmodel/position/phi",
+                "sim/flightmodel/position/phi",
-            "sim/flightmodel/position/theta",
+                "sim/flightmodel/position/theta",
-            "sim/flightmodel/position/psi",
+                "sim/flightmodel/position/psi",
-            "sim/cockpit/switches/gear_handle_status"
+                "sim/cockpit/switches/gear_handle_status"
         };
-        float[] posi = new float[] {37.524F, -122.06899F, 2500, 0, 0, 0, 1};
+        double[] posi = new double[] {37.524, -122.06899, 2500, 0, 0, 0, 1};
         try(XPlaneConnect xpc = new XPlaneConnect())
         {
             xpc.pauseSim(true);
@@ -585,7 +585,7 @@ public class XPlaneConnectTest
     @Test(expected = IllegalArgumentException.class)
     public void testSendPOSI_LongCtrl() throws IOException
     {
-        float[] posi = new float[] {37.524F, -122.06899F, 2500, 0, 0, 0, 1, -998};
+        double[] posi = new double[] {37.524, -122.06899, 2500, 0, 0, 0, 1, -998};
         try(XPlaneConnect xpc = new XPlaneConnect())
         {
             xpc.sendPOSI(posi);
@@ -595,7 +595,7 @@ public class XPlaneConnectTest
     @Test(expected = IllegalArgumentException.class)
     public void testSendPOSI_NegativeAircraftNum() throws IOException
     {
-        float[] posi = new float[] {37.524F, -122.06899F, 2500, 0, 0, 0, 1, -998};
+        double[] posi = new double[] {37.524, -122.06899, 2500, 0, 0, 0, 1, -998};
         try(XPlaneConnect xpc = new XPlaneConnect())
         {
             xpc.sendPOSI(posi, -1);
@@ -605,7 +605,7 @@ public class XPlaneConnectTest
     @Test(expected = IllegalArgumentException.class)
     public void testSendPOSI_LargeAircraftNum() throws IOException
     {
-        float[] posi = new float[] {37.524F, -122.06899F, 2500, 0, 0, 0, 1, -998};
+        double[] posi = new double[] {37.524, -122.06899, 2500, 0, 0, 0, 1, -998};
         try(XPlaneConnect xpc = new XPlaneConnect())
         {
             xpc.sendPOSI(posi, 300);
@@ -702,12 +702,12 @@ public class XPlaneConnectTest
     @Test
     public void testGetPOSI() throws IOException
     {
-        float[] values = { 37.524F, -122.06899F, 2500.0F, 45.0F, -45.0F, 15.0F, 1.0F };
+        double[] values = { 37.524, -122.06899, 2500.0, 45.0, -45.0, 15.0, 1.0 };
         try(XPlaneConnect xpc = new XPlaneConnect())
         {
             xpc.pauseSim(true);
             xpc.sendPOSI(values);
-            float[] actual = xpc.getPOSI(0);
+            double[] actual = xpc.getPOSI(0);
 
             assertArrayEquals(values, actual, 1e-4F);
         }

xpcPlugin/DataManager.cpp

@@ -658,7 +658,7 @@ namespace XPC
 		}
 	}
 
-	void DataManager::SetPosition(float pos[3], char aircraft)
+	void DataManager::SetPosition(double pos[3], char aircraft)
 	{
 		Log::FormatLine(LOG_INFO, "DMAN", "Setting position (%f, %f, %f) for aircraft %i",
 			pos[0], pos[1], pos[2], aircraft);
@@ -670,15 +670,15 @@ namespace XPC
 
 		if (IsDefault(pos[0]))
 		{
-			pos[0] = (float)GetDouble(DREF_Latitude, aircraft);
+			pos[0] = GetDouble(DREF_Latitude, aircraft);
 		}
 		if (IsDefault(pos[1]))
 		{
-			pos[1] = (float)GetDouble(DREF_Longitude, aircraft);
+			pos[1] = GetDouble(DREF_Longitude, aircraft);
 		}
 		if (IsDefault(pos[2]))
 		{
-			pos[2] = (float)GetDouble(DREF_Elevation, aircraft);
+			pos[2] = GetDouble(DREF_Elevation, aircraft);
 		}
 
 		// See: http://www.xsquawkbox.net/xpsdk/mediawiki/MovingThePlane
@@ -700,9 +700,9 @@ namespace XPC
 		Set(DREF_LocalY, local[1], aircraft);
 		Set(DREF_LocalZ, local[2], aircraft);
 		// If the sim is unpaused, this will override the above settings.
-		Set(DREF_Latitude, (double)pos[0], aircraft);
+		Set(DREF_Latitude,  pos[0], aircraft);
-		Set(DREF_Longitude, (double)pos[1], aircraft);
+		Set(DREF_Longitude, pos[1], aircraft);
-		Set(DREF_Elevation, (double)pos[2], aircraft);
+		Set(DREF_Elevation, pos[2], aircraft);
 
 		// Now reset orientation to update q
 		SetOrientation(orient, aircraft);
@@ -785,7 +785,7 @@ namespace XPC
 		return -998.0F;
 	}
 
-	bool DataManager::IsDefault(float value)
+	bool DataManager::IsDefault(double value)
 	{
 		return value < -997.9 && value > -999.1;
 	}

xpcPlugin/DataManager.h

@@ -396,7 +396,7 @@ namespace XPC
 		/// \param pos      An array containing latitude, longitude and altitude in
 		///                 fractional degrees and meters above sea level.
 		/// \param aircraft The aircraft to set the position of.
-		static void SetPosition(float pos[3], char aircraft = 0);
+		static void SetPosition(double pos[3], char aircraft = 0);
 
 		/// Sets the orientation of the specified aircraft.
 		///
@@ -417,7 +417,7 @@ namespace XPC
 		///
 		/// \param value The value to check.
 		/// \returns     true if value is a default value; otherwise false.
-		static bool IsDefault(float value);
+		static bool IsDefault(double value);
 	};
 }
 #endif

xpcPlugin/MessageHandlers.cpp

@@ -342,10 +342,11 @@ namespace XPC
 			}
 			case 20: // Position
 			{
-				float pos[3];
+                // TODO: loss of precision here
-				pos[0] = values[i][2];
+				double pos[3];
-				pos[1] = values[i][3];
+				pos[0] = (double)values[i][2];
-				pos[2] = values[i][4];
+				pos[1] = (double)values[i][3];
+				pos[2] = (double)values[i][4];
 				DataManager::SetPosition(pos);
 				break;
 			}
@@ -524,6 +525,7 @@ namespace XPC
 
 		unsigned char response[34] = "POSI";
 		response[5] = (char)DataManager::GetInt(DREF_GearHandle, aircraft);
+        // TODO change lat/lon/h to double?
 		*((float*)(response + 6)) = (float)DataManager::GetDouble(DREF_Latitude, aircraft);
 		*((float*)(response + 10)) = (float)DataManager::GetDouble(DREF_Longitude, aircraft);
 		*((float*)(response + 14)) = (float)DataManager::GetDouble(DREF_Elevation, aircraft);
@@ -545,18 +547,29 @@ namespace XPC
 
 		const unsigned char* buffer = msg.GetBuffer();
 		const std::size_t size = msg.GetSize();
-		if (size < 34)
-		{
-			Log::FormatLine(LOG_ERROR, "POSI", "ERROR: Unexpected size: %i (Expected at least 34)", size);
-			return;
-		}
 
 		char aircraftNumber = buffer[5];
-		float gear = *((float*)(buffer + 30));
+		float gear = *((float*)(buffer + 42));
-		float pos[3];
+		double posd[3];
 		float orient[3];
-		memcpy(pos, buffer + 6, 12);
+
-		memcpy(orient, buffer + 18, 12);
+		if (size == 34) /* lat/lon/h as 32-bit float */
+        {
+            posd[0] = *((float*)&buffer[6]);
+            posd[1] = *((float*)&buffer[10]);
+            posd[2] = *((float*)&buffer[14]);
+            memcpy(orient, buffer + 18, 12);
+        }
+        else if (size == 46) /* lat/lon/h as 64-bit double */
+		{
+            memcpy(posd, buffer + 6, 3*8);
+            memcpy(orient, buffer + 30, 12);
+		}
+        else
+        {
+			Log::FormatLine(LOG_ERROR, "POSI", "ERROR: Unexpected size: %i (Expected 34 or 46)", size);
+			return;
+        }
 
 		if (aircraftNumber > 0)
 		{
@@ -570,7 +583,8 @@ namespace XPC
 			}
 		}
 
-		DataManager::SetPosition(pos, aircraftNumber);
+		/* convert float to double */
+		DataManager::SetPosition(posd, aircraftNumber);
 		DataManager::SetOrientation(orient, aircraftNumber);
 		if (gear >= 0)
 		{