FlightSim-Streaming/XPlaneConnectCSP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
* Updated POSI to use doubles for lat/lon/alt, as step size for floats was unacceptably large at high longitudes.
This commit is contained in:
Jan Zwiener
2017-06-28 21:04:59 +02:00
committed by Jason Watkins
parent 48656f2b4c
commit 0e493920fa
27 changed files with 201 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
C/monitorExample/main.c
View File

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

4
C/playbackExample/src/playback.c
View File

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

26
C/src/xplaneConnect.c
View File

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

2
C/src/xplaneConnect.h
View File

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

2
C/xpcExample/xpcExample/src/main.cpp
View File

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

4
Java/Examples/BasicOperation/src/Main.java
View File

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

2
Java/Examples/ContinuousOperation/src/Main.java
View File

@@ -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",

6
Java/Examples/Playback/src/gov/nasa/xpc/Main.java
View File

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

23
Java/src/XPlaneConnect.java
View File

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

2
MATLAB/+XPlaneConnect/sendPOSI.m
View File

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

1
Python/src/xpc.py
View File

@@ -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):

30
TestScripts/C Tests/PosiTests.h
View File

@@ -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,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);
}

2
TestScripts/C Tests/SimuTests.h
View File

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

24
TestScripts/C Tests/Test.c
View File

@@ -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;
}

2
TestScripts/C Tests/Test.h
View File

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

12
TestScripts/Java Tests/XPlaneConnectTest.java
View File

@@ -550,7 +550,7 @@ public class XPlaneConnectTest
"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);
}

16
xpcPlugin/DataManager.cpp
View File

@@ -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;
}

4
xpcPlugin/DataManager.h
View File

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

40
xpcPlugin/MessageHandlers.cpp
View File

@@ -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;
}
@@ -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 pos[3];
float gear = *((float*)(buffer + 42));
double posd[3];
float orient[3];
memcpy(pos, buffer + 6, 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)
{