Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improved MessageHandler HandleGetT and created getTERR C function #204

Merged
merged 6 commits into from
May 2, 2020
Merged

Improved MessageHandler HandleGetT and created getTERR C function #204

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
a3175a8
Added getTERR function
NPrincen Apr 19, 2020
f630dbd
Improved accuracy of HandleGetT terrain probe
NPrincen Apr 19, 2020
54f5e43
Revert "Improved accuracy of HandleGetT terrain probe"
NPrincen Apr 19, 2020
70b4af9
Improved accuracy of HandleGetT terrain probe
NPrincen Apr 19, 2020
c49168d
Improved accuracy of HandleGetT terrain probe and requested changes
NPrincen May 1, 2020
6dcbec8
Added SendTerr function
NPrincen May 1, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
87 changes: 82 additions & 5 deletions C/src/xplaneConnect.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,7 @@ int sendUDP(XPCSocket sock, char buffer[], int len)
/// \param sock The socket to read from.
/// \param buffer A pointer to the location to store the data.
/// \param len The number of bytes to read.
/// \returns If an error occurs, a negative number. Otehrwise, the number of bytes read.
/// \returns If an error occurs, a negative number. Otherwise, the number of bytes read.
int readUDP(XPCSocket sock, char buffer[], int len)
{
#ifdef _WIN32
Expand Down Expand Up @@ -378,9 +378,9 @@ int readDATA(XPCSocket sock, float data[][9], int rows)
/*****************************************************************************/
/**** DREF functions ****/
/*****************************************************************************/
int sendDREF(XPCSocket sock, const char* dref, float value[], int size)
int sendDREF(XPCSocket sock, const char* dref, float values[], int size)
{
return sendDREFs(sock, &dref, &value, &size, 1);
return sendDREFs(sock, &dref, &values, &size, 1);
}

int sendDREFs(XPCSocket sock, const char* drefs[], float* values[], int sizes[], int count)
Expand Down Expand Up @@ -516,15 +516,15 @@ int getDREFs(XPCSocket sock, const char* drefs[], float* values[], unsigned char
int result = sendDREFRequest(sock, drefs, count);
if (result < 0)
{
// A error ocurred while sending.
// An error ocurred while sending.
NPrincen marked this conversation as resolved.
Show resolved Hide resolved
// sendDREFRequest will print an error message, so just return.
return -1;
}

// Read Response
if (getDREFResponse(sock, values, count, sizes) < 0)
{
// A error ocurred while reading the response.
// An error ocurred while reading the response.
// getDREFResponse will print an error message, so just return.
return -2;
}
Expand Down Expand Up @@ -640,6 +640,83 @@ int sendPOSI(XPCSocket sock, double values[], int size, char ac)
/**** End POSI functions ****/
/*****************************************************************************/

/*****************************************************************************/
/**** TERR functions ****/
/*****************************************************************************/
int sendTERRRequest(XPCSocket sock, double posi[3], char ac)
{
// Setup send command
char buffer[30] = "GETT";
buffer[5] = ac;
memcpy(&buffer[6], posi, 3 * sizeof(double));

// Send command
if (sendUDP(sock, buffer, 30) < 0)
{
printError("getTERR", "Failed to send command.");
return -1;
}
return 0;
}

int getTERRResponse(XPCSocket sock, double values[11], char ac)
{
// Get response
char readBuffer[62];
int readResult = readUDP(sock, readBuffer, 62);
if (readResult < 0)
{
printError("getTERR", "Failed to read response.");
return -2;
}
if (readResult != 62)
{
printError("getTERR", "Unexpected response length.");
return -3;
}

// Copy response into outputs
float f[8];
ac = readBuffer[5];
memcpy(values, readBuffer + 6, 3 * sizeof(double));
memcpy(f, readBuffer + 30, 8 * sizeof(float));
values[ 3] = (double)f[0];
values[ 4] = (double)f[1];
values[ 5] = (double)f[2];
values[ 6] = (double)f[3];
values[ 7] = (double)f[4];
values[ 8] = (double)f[5];
values[ 9] = (double)f[6];
values[10] = (double)f[7];

return 0;
}

int getTERR(XPCSocket sock, double posi[3], double values[11], char ac)
{
// Send Command
int result = sendTERRRequest(sock, posi, ac);
if (result < 0)
{
// An error ocurred while sending.
// sendTERRRequest will print an error message, so just return.
return result;
}

// Read Response
result = getTERRResponse(sock, values, ac);
if (result < 0)
{
// An error ocurred while reading the response.
// getTERRResponse will print an error message, so just return.
return result;
}
return 0;
}
/*****************************************************************************/
/**** End TERR functions ****/
/*****************************************************************************/

/*****************************************************************************/
/**** CTRL functions ****/
/*****************************************************************************/
Expand Down
46 changes: 32 additions & 14 deletions C/src/xplaneConnect.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ int setCONN(XPCSocket* sock, unsigned short port);
///
/// \param sock The socket to use to send the command.
/// \param pause 0 to unpause the sim; 1 to pause, 100:119 to pause a/c 0:19, 200:219 to unpause a/c 0:19.
/// \returns 0 if successful, otherwise a negative value.
/// \returns 0 if successful, otherwise a negative value.
int pauseSim(XPCSocket sock, char pause);

// X-Plane UDP DATA
Expand All @@ -122,7 +122,7 @@ int pauseSim(XPCSocket sock, char pause);
///
/// \details This command is compatible with the X-Plane data API.
/// \param sock The socket to use to send the command.
/// \param data A 2D array of data rows to read into.
/// \param data A 2D array of data rows to read into.
/// \param rows The number of rows in dataRef.
/// \returns 0 if successful, otherwise a negative value.
int readDATA(XPCSocket sock, float data[][9], int rows);
Expand All @@ -131,7 +131,7 @@ int readDATA(XPCSocket sock, float data[][9], int rows);
///
/// \details This command is compatible with the X-Plane data API.
/// \param sock The socket to use to send the command.
/// \param data A 2D array of data rows to send.
/// \param data A 2D array of data rows to send.
/// \param rows The number of rows in dataRef.
/// \returns 0 if successful, otherwise a negative value.
int sendDATA(XPCSocket sock, float data[][9], int rows);
Expand All @@ -144,12 +144,12 @@ int sendDATA(XPCSocket sock, float data[][9], int rows);
/// http://www.xsquawkbox.net/xpsdk/docs/DataRefs.html. The size of values should match
/// the size given on that page. XPC currently sends all values as floats regardless of
/// the type described on the wiki. This doesn't cause any data loss for most datarefs.
/// \param sock The socket to use to send the command.
/// \param dref The name of the dataref to set.
/// \param value An array of values representing the data to set.
/// \param size The number of elements in values.
/// \returns 0 if successful, otherwise a negative value.
int sendDREF(XPCSocket sock, const char* dref, float value[], int size);
/// \param sock The socket to use to send the command.
/// \param dref The name of the dataref to set.
/// \param values An array of values representing the data to set.
/// \param size The number of elements in values.
/// \returns 0 if successful, otherwise a negative value.
int sendDREF(XPCSocket sock, const char* dref, float values[], int size);

/// Sets the specified datarefs to the specified values.
///
Expand All @@ -159,7 +159,7 @@ int sendDREF(XPCSocket sock, const char* dref, float value[], int size);
/// the type described on the wiki. This doesn't cause any data loss for most datarefs.
/// \param sock The socket to use to send the command.
/// \param drefs The names of the datarefs to set.
/// \param values A multidimensional array containing the values for each dataref to set.
/// \param values A 2D array array containing the values for each dataref to set.
/// \param sizes The number of elements in each array in values
/// \param count The number of datarefs being set.
/// \returns 0 if successful, otherwise a negative value.
Expand All @@ -173,13 +173,13 @@ int sendDREFs(XPCSocket sock, const char* drefs[], float* values[], int sizes[],
/// the type described on the wiki. This doesn't cause any data loss for most datarefs.
/// \param sock The socket to use to send the command.
/// \param dref The name of the dataref to get.
/// \param values The array in which the value of the dataref will be stored.
/// \param values The array in which the values of the dataref will be stored.
/// \param size The number of elements in values. The actual number of elements copied in will
/// be set when the function returns.
/// \returns 0 if successful, otherwise a negative value.
int getDREF(XPCSocket sock, const char* dref, float values[], int* size);

/// Gets the value of the specified dataref.
/// Gets the values of the specified datarefs.
///
/// \details dref names and their associated data types can be found on the XPSDK wiki at
/// http://www.xsquawkbox.net/xpsdk/docs/DataRefs.html. The size of values should match
Expand All @@ -189,7 +189,7 @@ int getDREF(XPCSocket sock, const char* dref, float values[], int* size);
/// \param drefs The names of the datarefs to get.
/// \param values A 2D array in which the values of the datarefs will be stored.
/// \param count The number of datarefs being requested.
/// \param size The number of elements in each row of values. The size of each row will be set
/// \param sizes The number of elements in each row of values. The size of each row will be set
/// 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[]);
Expand All @@ -216,6 +216,24 @@ int getPOSI(XPCSocket sock, double values[7], char ac);
/// \returns 0 if successful, otherwise a negative value.
int sendPOSI(XPCSocket sock, double values[], int size, char ac);

// Terrain

/// Gets the terrain information of the specified aircraft.
///
/// \param sock The socket to use to send the command.
/// \param posi A double array representing position data about the aircraft. The format of values is
/// [Lat, Lon, Alt].
/// -998 used for [Lat, Lon, Alt] to request terrain info at the current aircraft position.
/// \param values A double array with the information for the terrain output. The format of values is
/// [Lat, Lon, Alt, Nx, Ny, Nz, Vx, Vy, Vz, wet, result]. The first three are for output of
/// the Lat and Lon of the aircraft with the terrain height directly below. The next three
/// represent the terrain normal. The next three represent the velocity of the terrain.
/// The wet variable is 0.0 if the terrain is dry and 1.0 if wet.
/// The last output is the terrain probe result parameter.
/// \param ac The aircraft number to get the terrain data of. 0 for the main/user's aircraft.
/// \returns 0 if successful, otherwise a negative value.
int getTERR(XPCSocket sock, double posi[3], double values[11], char ac);

// Controls

/// Gets the control surface information for the specified aircraft.
Expand All @@ -224,7 +242,7 @@ int sendPOSI(XPCSocket sock, double values[], int size, char ac);
/// \param values An array to store the position information returned by the
/// plugin. The format of values is [Elevator, Aileron, Rudder,
/// Throttle, Gear, Flaps, Speed Brakes]
/// \param ac The aircraft to set the control surfaces of. 0 is the main/user's aircraft.
/// \param ac The aircraft to get the control surfaces of. 0 is the main/user's aircraft.
/// \returns 0 if successful, otherwise a negative value.
int getCTRL(XPCSocket sock, float values[7], char ac);

Expand Down
90 changes: 58 additions & 32 deletions xpcPlugin/MessageHandlers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -642,81 +642,107 @@ namespace XPC
unsigned char aircraft = buffer[5];
Log::FormatLine(LOG_TRACE, "GETT", "Getting terrain information for aircraft %u", aircraft);

double loc[3];
double X;
double Y;
double Z;
memcpy(loc, buffer + 6, 24);
double pos[3];
memcpy(pos, buffer + 6, 24);

if(loc[0] == -998 || loc[1] == -998 || loc[2] == -998)
if(pos[0] == -998 || pos[1] == -998 || pos[2] == -998)
{
// get terrain properties at aircraft location
// probe needs to be below terrain to work...
X = DataManager::GetDouble(DREF_LocalX, aircraft);
Z = DataManager::GetDouble(DREF_LocalZ, aircraft);
Y = -100.0;
}
else
{
// terrain probe at specified location
XPLMWorldToLocal(loc[0], loc[1], loc[2], &X, &Y, &Z);
pos[0] = DataManager::GetDouble(DREF_Latitude, aircraft);
pos[1] = DataManager::GetDouble(DREF_Longitude, aircraft);
pos[2] = 0.0;
}

MessageHandlers::SendTerr(pos, aircraft);
}

void MessageHandlers::SendTerr(double pos[3], char aircraft)
NPrincen marked this conversation as resolved.
Show resolved Hide resolved
{
double lat, lon, alt, X, Y, Z;

// Init terrain probe (if required) and probe data struct
XPLMProbeInfo_t probe_data;
static XPLMProbeInfo_t probe_data;
probe_data.structSize = sizeof(XPLMProbeInfo_t);

if(Terrain_probe == nullptr)
{
Log::FormatLine(LOG_TRACE, "GETT", "Create terrain probe for aircraft %u", aircraft);
Log::FormatLine(LOG_TRACE, "TERR", "Create terrain probe for aircraft %u", aircraft);
Terrain_probe = XPLMCreateProbe(0);
}

// terrain probe at specified location
// Follow the process in the following post to get accurate results
// https://forums.x-plane.org/index.php?/forums/topic/38688-how-do-i-use-xplmprobeterrainxyz/&page=2

// transform probe location to local coordinates
// Step 1. Convert lat/lon/0 to XYZ
XPLMWorldToLocal(pos[0], pos[1], pos[2], &X, &Y, &Z);

// query probe
// Step 2. Probe XYZ to get a new Y
int rc = XPLMProbeTerrainXYZ(Terrain_probe, X, Y, Z, &probe_data);
if(rc > 0)
{
Log::FormatLine(LOG_ERROR, "TERR", "Probe failed. Return Value %u", rc);
XPLMDestroyProbe(Terrain_probe);
Terrain_probe = nullptr;
return;
NPrincen marked this conversation as resolved.
Show resolved Hide resolved
}

// transform probe location to world coordinates
double lat;
double lon;
double alt;

// Step 3. Convert that new XYZ back to LLE
XPLMLocalToWorld(probe_data.locationX, probe_data.locationY, probe_data.locationZ, &lat, &lon, &alt);
Log::FormatLine(LOG_TRACE, "TERR", "Conv LLA=%f, %f, %f", lat, lon, alt);

// transform probe location to local coordinates
// Step 4. NOW convert your original lat/lon with the elevation from step 3 to XYZ
XPLMWorldToLocal(pos[0], pos[1], alt, &X, &Y, &Z);

// query probe
// Step 5. Re-probe with the NEW XYZ
rc = XPLMProbeTerrainXYZ(Terrain_probe, X, Y, Z, &probe_data);
if(rc == 0)
{
// transform probe location to world coordinates
// Step 6. You now have a new Y, and your XYZ will be closer to correct for high elevations far from the origin.
XPLMLocalToWorld(probe_data.locationX, probe_data.locationY, probe_data.locationZ, &lat, &lon, &alt);

Log::FormatLine(LOG_TRACE, "GETT", "Probe LLA %lf %lf %lf", lat, lon, alt);
Log::FormatLine(LOG_TRACE, "TERR", "Probe LLA %lf %lf %lf", lat, lon, alt);
}
else
{
lat = -998;
lon = -998;
alt = -998;

Log::FormatLine(LOG_TRACE, "GETT", "Probe failed. Return Value %u", rc);
Log::FormatLine(LOG_TRACE, "TERR", "Probe failed. Return Value %u", rc);
}

// keep probe for next query
// XPLMDestroyProbe(probe);
// XPLMDestroyProbe(Terrain_probe);

// Assemble response message
unsigned char response[50] = "TERR";
unsigned char response[62] = "TERR";
response[5] = aircraft;
// terrain height over msl at lat/lon point
memcpy(response + 6, &lat, 8);
memcpy(response + 14, &lon, 8);
memcpy(response + 22, &alt, 8);
// terrain incidence
// terrain normal vector
memcpy(response + 30, &probe_data.normalX, 4);
memcpy(response + 34, &probe_data.normalY, 4);
memcpy(response + 38, &probe_data.normalZ, 4);
// terrain velocity
memcpy(response + 42, &probe_data.velocityX, 4);
memcpy(response + 46, &probe_data.velocityY, 4);
memcpy(response + 50, &probe_data.velocityZ, 4);
// terrain type
memcpy(response + 42, &probe_data.is_wet, 4);
memcpy(response + 54, &probe_data.is_wet, 4);
// probe status
memcpy(response + 46, &rc, 4);

sock->SendTo(response, 50, &connection.addr);
}
memcpy(response + 58, &rc, 4);

sock->SendTo(response, 62, &connection.addr);
}

void MessageHandlers::HandleSimu(const Message& msg)
{
Expand Down
2 changes: 2 additions & 0 deletions xpcPlugin/MessageHandlers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ namespace XPC
static void SetSocket(UDPSocket* socket);

static void SendBeacon(const std::string& pluginVersion, unsigned short pluginReceivePort, int xplaneVersion);

static void SendTerr(double pos[3], char aircraft);

private:
// One handler per message type. Message types are descripbed on the
Expand Down