From 0e493920fa9c09d112892902613184f150936d94 Mon Sep 17 00:00:00 2001 From: Jan Zwiener Date: Wed, 28 Jun 2017 21:04:59 +0200 Subject: [PATCH] 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 | 4 +- C/playbackExample/src/chrome.c | 4 +- C/playbackExample/src/chrome.h | 4 +- C/playbackExample/src/main.c | 4 +- C/playbackExample/src/playback.c | 10 ++-- C/playbackExample/src/playback.h | 4 +- C/src/xplaneConnect.c | 48 +++++++++++-------- C/src/xplaneConnect.h | 10 ++-- C/xpcExample/xpcExample/src/main.cpp | 2 +- Java/Examples/BasicOperation/src/Main.java | 4 +- .../ContinuousOperation/src/Main.java | 2 +- .../Playback/src/gov/nasa/xpc/Main.java | 6 +-- Java/src/XPlaneConnect.java | 23 +++++---- MATLAB/+XPlaneConnect/sendPOSI.m | 14 +++--- MATLAB/MonitorExample/MonitorExample.m | 4 +- Python/src/xpc.py | 27 ++++++----- TestScripts/C Tests/CtrlTests.h | 4 +- TestScripts/C Tests/DrefTests.h | 2 +- TestScripts/C Tests/PosiTests.h | 32 ++++++------- TestScripts/C Tests/SimuTests.h | 4 +- TestScripts/C Tests/Test.c | 26 +++++++++- TestScripts/C Tests/Test.h | 2 + TestScripts/C Tests/main.c | 8 ++-- TestScripts/Java Tests/XPlaneConnectTest.java | 28 +++++------ xpcPlugin/DataManager.cpp | 16 +++---- xpcPlugin/DataManager.h | 4 +- xpcPlugin/MessageHandlers.cpp | 48 ++++++++++++------- 27 files changed, 201 insertions(+), 143 deletions(-) diff --git a/C/monitorExample/main.c b/C/monitorExample/main.c index e78f0cf..998c893 100644 --- a/C/monitorExample/main.c +++ b/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 { @@ -98,4 +98,4 @@ int main(void) printf("\n\nPress Any Key to exit..."); getchar(); return 0; -} \ No newline at end of file +} diff --git a/C/playbackExample/src/chrome.c b/C/playbackExample/src/chrome.c index f1850dd..563cef8 100644 --- a/C/playbackExample/src/chrome.c +++ b/C/playbackExample/src/chrome.c @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -68,4 +68,4 @@ int getInt(char* prompt) int result; scanf("%d", &result); return result; -} \ No newline at end of file +} diff --git a/C/playbackExample/src/chrome.h b/C/playbackExample/src/chrome.h index fd240e6..35b3f60 100644 --- a/C/playbackExample/src/chrome.h +++ b/C/playbackExample/src/chrome.h @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -37,4 +37,4 @@ void getString(char* prompt, char buffer[255]); int getInt(char* prompt); -#endif \ No newline at end of file +#endif diff --git a/C/playbackExample/src/main.c b/C/playbackExample/src/main.c index abdeb33..1787be6 100644 --- a/C/playbackExample/src/main.c +++ b/C/playbackExample/src/main.c @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -67,4 +67,4 @@ int main(void) } return 0; -} \ No newline at end of file +} diff --git a/C/playbackExample/src/playback.c b/C/playbackExample/src/playback.c index 3226c5a..1f4025e 100644 --- a/C/playbackExample/src/playback.c +++ b/C/playbackExample/src/playback.c @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -89,18 +89,18 @@ 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) { continue; - } + } sendPOSI(sock, posi, 7, 0); } closeUDP(sock); displayMsg("Playback Complete"); -} \ No newline at end of file +} diff --git a/C/playbackExample/src/playback.h b/C/playbackExample/src/playback.h index 98fa54b..889a7ee 100644 --- a/C/playbackExample/src/playback.h +++ b/C/playbackExample/src/playback.h @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -29,4 +29,4 @@ void record(char* path, int interval, int duration); void playback(char* path, int interval); -#endif \ No newline at end of file +#endif diff --git a/C/src/xplaneConnect.c b/C/src/xplaneConnect.c index e201a75..68529fb 100755 --- a/C/src/xplaneConnect.c +++ b/C/src/xplaneConnect.c @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -82,13 +82,13 @@ XPCSocket openUDP(const char *xpIP) XPCSocket aopenUDP(const char *xpIP, unsigned short xpPort, unsigned short port) { XPCSocket sock; - + // Setup Port struct sockaddr_in recvaddr; recvaddr.sin_family = AF_INET; recvaddr.sin_addr.s_addr = INADDR_ANY; recvaddr.sin_port = htons(port); - + // Set X-Plane Port and IP if (strcmp(xpIP, "localhost") == 0) { @@ -96,7 +96,7 @@ XPCSocket aopenUDP(const char *xpIP, unsigned short xpPort, unsigned short port) } strncpy(sock.xpIP, xpIP, 16); sock.xpPort = xpPort == 0 ? 49009 : xpPort; - + #ifdef _WIN32 WSADATA wsa; if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) @@ -105,7 +105,7 @@ XPCSocket aopenUDP(const char *xpIP, unsigned short xpPort, unsigned short port) exit(EXIT_FAILURE); } #endif - + if ((sock.sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { printError("OpenUDP", "Socket creation failed"); @@ -128,7 +128,7 @@ XPCSocket aopenUDP(const char *xpIP, unsigned short xpPort, unsigned short port) if (setsockopt(sock.sock, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout)) < 0) { printError("OpenUDP", "Failed to set timeout"); - } + } return sock; } @@ -160,7 +160,7 @@ int sendUDP(XPCSocket sock, char buffer[], int len) printError("sendUDP", "Message length must be positive."); return -1; } - + // Set up destination address struct sockaddr_in dst; dst.sin_family = AF_INET; @@ -311,7 +311,7 @@ int sendDATA(XPCSocket sock, float data[][9], int rows) // Setup command // 5 byte header + 134 rows * 9 values * 4 bytes per value => 4829 byte max length. - char buffer[4829] = "DATA"; + char buffer[4829] = "DATA"; int len = 5 + rows * 9 * sizeof(float); unsigned short step = 9 * sizeof(float); int i; // iterator @@ -462,7 +462,7 @@ int getDREFResponse(XPCSocket sock, float* values[], unsigned char count, int si { unsigned char buffer[65536]; int result = readUDP(sock, buffer, 65536); - + if (result < 0) { #ifdef _WIN32 @@ -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[40] = "POSI"; + unsigned char buffer[46] = "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,9 +745,9 @@ 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 if (sendUDP(sock, buffer, len) < 0) { @@ -807,4 +817,4 @@ int sendVIEW(XPCSocket sock, VIEW_TYPE view) } /*****************************************************************************/ /**** End View functions ****/ -/*****************************************************************************/ \ No newline at end of file +/*****************************************************************************/ diff --git a/C/src/xplaneConnect.h b/C/src/xplaneConnect.h index b701c88..1690e67 100644 --- a/C/src/xplaneConnect.h +++ b/C/src/xplaneConnect.h @@ -2,7 +2,7 @@ //National Aeronautics and Space Administration. All Rights Reserved. // //DISCLAIMERS -// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, +// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND, // EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT // THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY @@ -78,7 +78,7 @@ typedef enum XPC_VIEW_FULLSCREENWITHHUD, XPC_VIEW_FULLSCREENNOHUD, } VIEW_TYPE; - + // Low Level UDP Functions /// Opens a new connection to XPC on an OS chosen port. @@ -115,7 +115,7 @@ int setCONN(XPCSocket* sock, unsigned short port); /// \param pause 0 to unpause the sim; any other value to pause. /// \returns 0 if successful, otherwise a negative value. int pauseSim(XPCSocket sock, char pause); - + // X-Plane UDP DATA /// Reads X-Plane data from the specified socket. @@ -193,7 +193,7 @@ int getDREF(XPCSocket sock, const char* dref, float values[], int* size); /// to the actual number of elements copied in for that row. /// \returns 0 if successful, otherwise a negative value. int getDREFs(XPCSocket sock, const char* drefs[], float* values[], unsigned char count, int sizes[]); - + // Position /// Gets the position and orientation of the specified aircraft. @@ -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 diff --git a/C/xpcExample/xpcExample/src/main.cpp b/C/xpcExample/xpcExample/src/main.cpp index e9e645a..ba06f01 100644 --- a/C/xpcExample/xpcExample/src/main.cpp +++ b/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 diff --git a/Java/Examples/BasicOperation/src/Main.java b/Java/Examples/BasicOperation/src/Main.java index dc82c9f..92412d0 100644 --- a/Java/Examples/BasicOperation/src/Main.java +++ b/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); diff --git a/Java/Examples/ContinuousOperation/src/Main.java b/Java/Examples/ContinuousOperation/src/Main.java index 61a14eb..801a369 100644 --- a/Java/Examples/ContinuousOperation/src/Main.java +++ b/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", diff --git a/Java/Examples/Playback/src/gov/nasa/xpc/Main.java b/Java/Examples/Playback/src/gov/nasa/xpc/Main.java index 0e0b376..477e8f5 100644 --- a/Java/Examples/Playback/src/gov/nasa/xpc/Main.java +++ b/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); diff --git a/Java/src/XPlaneConnect.java b/Java/src/XPlaneConnect.java index 8b1f228..a212278 100644 --- a/Java/src/XPlaneConnect.java +++ b/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 *

* @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

An array containing position elements as follows:

*
    @@ -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 diff --git a/MATLAB/+XPlaneConnect/sendPOSI.m b/MATLAB/+XPlaneConnect/sendPOSI.m index bff56fb..a8c84a0 100644 --- a/MATLAB/+XPlaneConnect/sendPOSI.m +++ b/MATLAB/+XPlaneConnect/sendPOSI.m @@ -1,6 +1,6 @@ function sendPOSI( posi, ac, socket ) % sendPOSI Sets the position of the specified aircraft. -% +% % Inputs % posi: Position array where the elements are as follows: % 1. Latitiude (deg) @@ -12,14 +12,14 @@ function sendPOSI( posi, ac, socket ) % 7. Gear (0=up, 1=down) % acft (optional): The aircraft to set. 0 for the player aircraft. % socket (optional): The client to use when sending the command. -% +% % Use % 1. import XPlaneConnect.*; % 2. sendPOSI([37.5242422, -122.06899, 2500, 0, 0, 0, 1], 1); -% +% % Note: send the value -998 to not overwrite that parameter. That is, if % -998 is sent, the parameter will stay at the current X-Plane value. -% +% % Contributors % [CT] Christopher Teubert (SGT, Inc.) % christopher.a.teubert@nasa.gov @@ -33,14 +33,14 @@ global clients; if ~exist('socket', 'var') assert(isequal(length(clients) < 2, 1), '[sendPOSI] ERROR: Multiple clients open. You must specify which client to use.'); if isempty(clients) - socket = openUDP(); + socket = openUDP(); else socket = clients(1); end end %% Validate input -posi = single(posi); +posi = double(posi); if ~exist('ac', 'var') ac = 0; end @@ -49,4 +49,4 @@ ac = logical(ac); %% Send command socket.sendPOSI(posi, ac); -end \ No newline at end of file +end diff --git a/MATLAB/MonitorExample/MonitorExample.m b/MATLAB/MonitorExample/MonitorExample.m index ffbdc14..63e8cd4 100644 --- a/MATLAB/MonitorExample/MonitorExample.m +++ b/MATLAB/MonitorExample/MonitorExample.m @@ -11,9 +11,9 @@ Socket = openUDP(); while 1 posi = getPOSI(0, Socket); ctrl = getCTRL(0, Socket); - + fprintf('Loc: (%4f, %4f, %4f) Aileron:%2f Elevator:%2f Rudder:%2f\n', ... posi(1), posi(2), posi(3), ctrl(2), ctrl(1), ctrl(3)); pause(0.1); end -closeUDP(Socket); \ No newline at end of file +closeUDP(Socket); diff --git a/Python/src/xpc.py b/Python/src/xpc.py index e2b5c5e..a543c7b 100644 --- a/Python/src/xpc.py +++ b/Python/src/xpc.py @@ -8,7 +8,7 @@ class XPlaneConnect(object): # Basic Functions def __init__(self, xpHost = 'localhost', xpPort = 49009, port = 0, timeout = 100): '''Sets up a new connection to an X-Plane Connect plugin running in X-Plane. - + Args: xpHost: The hostname of the machine running X-Plane. xpPort: The port on which the XPC plugin is listening. Usually 49007. @@ -33,7 +33,7 @@ class XPlaneConnect(object): # Setup XPlane IP and port self.xpDst = (xpIP, xpPort) - # Create and bind socket + # Create and bind socket clientAddr = ("0.0.0.0", port) self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) self.socket.bind(clientAddr) @@ -71,18 +71,18 @@ class XPlaneConnect(object): # Configuration def setCONN(self, port): '''Sets the port on which the client sends and receives data. - + Args: port: The new port to use. ''' - #Validate parameters + #Validate parameters if port < 0 or port > 65535: raise ValueError("The specified port is not a valid port number.") #Send command buffer = struct.pack("<4sxH", "CONN", port) self.sendUDP(buffer) - + #Rebind socket clientAddr = ("0.0.0.0", port) timeout = self.socket.gettimeout(); @@ -96,7 +96,7 @@ class XPlaneConnect(object): def pauseSim(self, pause): '''Pauses or un-pauses the physics simulation engine in X-Plane. - + Args: pause: True to pause the simulation; False to resume. ''' @@ -110,7 +110,7 @@ class XPlaneConnect(object): # X-Plane UDP Data def readDATA(self): '''Reads X-Plane data. - + Returns: A 2 dimensional array containing 0 or more rows of data. Each array in the result will have 9 elements, the first of which is the row number which that array represents data for, and the rest of which are the data elements in @@ -127,7 +127,7 @@ class XPlaneConnect(object): def sendDATA(self, data): '''Sends X-Plane data over the underlying UDP socket. - + Args: data: An array of values representing data rows to be set. Each array in `data` should have 9 elements, the first of which is a row number in the range (0-134), @@ -143,7 +143,7 @@ class XPlaneConnect(object): buffer += struct.pack(" 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): @@ -264,8 +265,8 @@ class XPlaneConnect(object): # Send self.sendUDP(buffer) - - # DREF Manipulation + + # DREF Manipulation def sendDREF(self, dref, values): '''Sets the specified dataref to the specified value. @@ -294,7 +295,7 @@ class XPlaneConnect(object): raise ValueError("dref must be a non-empty string less than 256 characters.") if value == None: raise ValueError("value must be a scalar or sequence of floats.") - + # Pack message if hasattr(value, "__len__"): if len(value) > 255: @@ -310,7 +311,7 @@ class XPlaneConnect(object): def getDREF(self, dref): '''Gets the value of an X-Plane dataref. - + Args: dref: The name of the dataref to get. diff --git a/TestScripts/C Tests/CtrlTests.h b/TestScripts/C Tests/CtrlTests.h index 572ea07..52f2519 100644 --- a/TestScripts/C Tests/CtrlTests.h +++ b/TestScripts/C Tests/CtrlTests.h @@ -23,7 +23,7 @@ int doCTRLTest(XPCSocket *sock, char* drefs[7], float values[], int size, int ac { result = getDREFs(*sock, drefs, data, 7, sizes); } - + if (result < 0) { return -1; @@ -102,7 +102,7 @@ int basicCTRLTest(char** drefs, int ac) CTRL[4] = -998; CTRL[5] = -998; result = doCTRLTest(&sock, drefs, CTRL, 6, ac, expected); - + pauseSim(sock, 0); closeUDP(sock); if (result < 0) diff --git a/TestScripts/C Tests/DrefTests.h b/TestScripts/C Tests/DrefTests.h index 50338a7..9d1c754 100644 --- a/TestScripts/C Tests/DrefTests.h +++ b/TestScripts/C Tests/DrefTests.h @@ -244,4 +244,4 @@ int testDREF() return doDREFTest(drefs, values, expected, 6, sizes); } -#endif \ No newline at end of file +#endif diff --git a/TestScripts/C Tests/PosiTests.h b/TestScripts/C Tests/PosiTests.h index 104e517..317a623 100644 --- a/TestScripts/C Tests/PosiTests.h +++ b/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 }; - float expected[7] = { 37.524F, -122.06899F, 2500, 0, 0, 0, 1 }; + double POSI[7] = { 37.524, -122.06899, 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[1] = -998.0F; - POSI[2] = -998.0F; + POSI[0] = -998.0; + POSI[1] = -998.0; + 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[4] = -5.0F; - expected[5] = 10.0F; - expected[6] = 0.0F; + expected[3] = 5.0; + expected[4] = -5.0; + expected[5] = 10.0; + expected[6] = 0.0; result = doPOSITest(drefs, POSI, 7, ac, expected); if (result < 0) { @@ -145,14 +145,14 @@ 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); } -#endif \ No newline at end of file +#endif diff --git a/TestScripts/C Tests/SimuTests.h b/TestScripts/C Tests/SimuTests.h index 0d12012..60a3767 100644 --- a/TestScripts/C Tests/SimuTests.h +++ b/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) @@ -78,4 +78,4 @@ int testSIMU_Toggle() return 0; } -#endif \ No newline at end of file +#endif diff --git a/TestScripts/C Tests/Test.c b/TestScripts/C Tests/Test.c index 11ddc48..eec3124 100644 --- a/TestScripts/C Tests/Test.c +++ b/TestScripts/C Tests/Test.c @@ -56,4 +56,28 @@ int compareArrays(float* expected[], int esizes[], float* actual[], int asizes[] } } return 0; -} \ No newline at end of file +} + +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; +} diff --git a/TestScripts/C Tests/Test.h b/TestScripts/C Tests/Test.h index 5c40d40..2a40fb9 100644 --- a/TestScripts/C Tests/Test.h +++ b/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 \ No newline at end of file diff --git a/TestScripts/C Tests/main.c b/TestScripts/C Tests/main.c index ec2b752..802953c 100644 --- a/TestScripts/C Tests/main.c +++ b/TestScripts/C Tests/main.c @@ -13,7 +13,7 @@ int main(int argc, const char * argv[]) { printf("XPC Tests-c "); - + #ifdef _WIN32 printf("(Windows)\n"); #elif (__APPLE__) @@ -23,7 +23,7 @@ int main(int argc, const char * argv[]) { #else printf("(Unable to determine operating system) \n") #endif - + // Basic Networking runTest(testOpen, "open"); crossPlatformUSleep(SLEEP_AMOUNT); @@ -78,11 +78,11 @@ int main(int argc, const char * argv[]) { // setConn crossPlatformUSleep(SLEEP_AMOUNT); runTest(testCONN, "CONN"); - + printf( "----------------\nTest Summary\n\tFailed: %i\n\tPassed: %i\n", testFailed, testPassed ); printf("Press any key to exit."); getchar(); - + return 0; } diff --git a/TestScripts/Java Tests/XPlaneConnectTest.java b/TestScripts/Java Tests/XPlaneConnectTest.java index 2e94bb5..c58fd04 100644 --- a/TestScripts/Java Tests/XPlaneConnectTest.java +++ b/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/longitude", - "sim/flightmodel/position/y_agl", - "sim/flightmodel/position/phi", - "sim/flightmodel/position/theta", - "sim/flightmodel/position/psi", - "sim/cockpit/switches/gear_handle_status" + "sim/flightmodel/position/latitude", + "sim/flightmodel/position/longitude", + "sim/flightmodel/position/y_agl", + "sim/flightmodel/position/phi", + "sim/flightmodel/position/theta", + "sim/flightmodel/position/psi", + "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); } @@ -725,4 +725,4 @@ public class XPlaneConnectTest assertArrayEquals(values, actual, 1e-4F); } } -} \ No newline at end of file +} diff --git a/xpcPlugin/DataManager.cpp b/xpcPlugin/DataManager.cpp index ffac66a..9e098d0 100644 --- a/xpcPlugin/DataManager.cpp +++ b/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_Longitude, (double)pos[1], aircraft); - Set(DREF_Elevation, (double)pos[2], aircraft); + Set(DREF_Latitude, pos[0], aircraft); + Set(DREF_Longitude, pos[1], 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; } diff --git a/xpcPlugin/DataManager.h b/xpcPlugin/DataManager.h index c0e80c2..b0c416b 100644 --- a/xpcPlugin/DataManager.h +++ b/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 diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp index a78448e..6dd9b69 100644 --- a/xpcPlugin/MessageHandlers.cpp +++ b/xpcPlugin/MessageHandlers.cpp @@ -342,10 +342,11 @@ namespace XPC } case 20: // Position { - float pos[3]; - pos[0] = values[i][2]; - pos[1] = values[i][3]; - pos[2] = values[i][4]; + // TODO: loss of precision here + double pos[3]; + pos[0] = (double)values[i][2]; + pos[1] = (double)values[i][3]; + pos[2] = (double)values[i][4]; DataManager::SetPosition(pos); break; } @@ -439,7 +440,7 @@ namespace XPC unsigned char aircraft = buffer[5]; // TODO(jason-watkins): Get proper printf specifier for unsigned char Log::FormatLine(LOG_TRACE, "GCTL", "Getting control information for aircraft %u", aircraft); - + float throttle[8]; unsigned char response[31] = "CTRL"; *((float*)(response + 5)) = DataManager::GetFloat(DREF_Elevator, aircraft); @@ -524,13 +525,14 @@ 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); *((float*)(response + 18)) = DataManager::GetFloat(DREF_Pitch, aircraft); *((float*)(response + 22)) = DataManager::GetFloat(DREF_Roll, aircraft); *((float*)(response + 26)) = DataManager::GetFloat(DREF_HeadingTrue, aircraft); - + float gear[10]; DataManager::GetFloatArray(DREF_GearDeploy, gear, 10, aircraft); *((float*)(response + 30)) = gear[0]; @@ -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 pos[3]; + float gear = *((float*)(buffer + 42)); + 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) { @@ -589,7 +603,7 @@ namespace XPC Log::FormatLine(LOG_ERROR, "SIMU", "ERROR: Invalid argument: %i", v); return; } - + int value[20]; if (v == 2) {