From a3175a81809686ec1729e52245fdb88316c289ad Mon Sep 17 00:00:00 2001 From: Norman Princen <63923352+NPrincen@users.noreply.github.com> Date: Sun, 19 Apr 2020 13:02:25 -0700 Subject: [PATCH 1/6] Added getTERR function Also corrected minor typos in comments. --- C/src/xplaneConnect.c | 87 ++++++++++++++++++++++++++++++++++++++++--- C/src/xplaneConnect.h | 46 ++++++++++++++++------- 2 files changed, 114 insertions(+), 19 deletions(-) diff --git a/C/src/xplaneConnect.c b/C/src/xplaneConnect.c index 9425c94..fdcba8e 100755 --- a/C/src/xplaneConnect.c +++ b/C/src/xplaneConnect.c @@ -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 @@ -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) @@ -516,7 +516,7 @@ 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. // sendDREFRequest will print an error message, so just return. return -1; } @@ -524,7 +524,7 @@ int getDREFs(XPCSocket sock, const char* drefs[], float* values[], unsigned char // 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; } @@ -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 ****/ /*****************************************************************************/ diff --git a/C/src/xplaneConnect.h b/C/src/xplaneConnect.h index 4a62b17..67eafba 100644 --- a/C/src/xplaneConnect.h +++ b/C/src/xplaneConnect.h @@ -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 @@ -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); @@ -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); @@ -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. /// @@ -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. @@ -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 @@ -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[]); @@ -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. @@ -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); From f630dbdf07b020484f0c3ace6631d5d68deda736 Mon Sep 17 00:00:00 2001 From: Norman Princen <63923352+NPrincen@users.noreply.github.com> Date: Sun, 19 Apr 2020 13:33:00 -0700 Subject: [PATCH 2/6] Improved accuracy of HandleGetT terrain probe 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 Also added the ground velocity vector to the output. This is used for carrier landing where "ground" is moving. --- xpcPlugin/MessageHandlers.cpp | 88 ++++++++++++++++++++++------------- 1 file changed, 56 insertions(+), 32 deletions(-) diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp index 5163edc..453811b 100644 --- a/xpcPlugin/MessageHandlers.cpp +++ b/xpcPlugin/MessageHandlers.cpp @@ -642,49 +642,70 @@ 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) { // 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) + { + 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); + return; + } // 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 origina 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 { @@ -692,31 +713,34 @@ namespace XPC 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) { From 54f5e43ddc7117dc044d39982da2754f6d7d55f8 Mon Sep 17 00:00:00 2001 From: Norman Princen <63923352+NPrincen@users.noreply.github.com> Date: Sun, 19 Apr 2020 13:43:55 -0700 Subject: [PATCH 3/6] Revert "Improved accuracy of HandleGetT terrain probe" This reverts commit f630dbdf07b020484f0c3ace6631d5d68deda736. --- xpcPlugin/MessageHandlers.cpp | 88 +++++++++++++---------------------- 1 file changed, 32 insertions(+), 56 deletions(-) diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp index 453811b..5163edc 100644 --- a/xpcPlugin/MessageHandlers.cpp +++ b/xpcPlugin/MessageHandlers.cpp @@ -642,70 +642,49 @@ namespace XPC unsigned char aircraft = buffer[5]; Log::FormatLine(LOG_TRACE, "GETT", "Getting terrain information for aircraft %u", aircraft); - double pos[3]; - memcpy(pos, buffer + 6, 24); + double loc[3]; + double X; + double Y; + double Z; + memcpy(loc, buffer + 6, 24); + if(loc[0] == -998 || loc[1] == -998 || loc[2] == -998) { // get terrain properties at aircraft location - pos[0] = DataManager::GetDouble(DREF_Latitude, aircraft); - pos[1] = DataManager::GetDouble(DREF_Longitude, aircraft); - pos[2] = 0.0; + // 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); } - - MessageHandlers::SendTerr(pos, aircraft); - } - - void MessageHandlers::SendTerr(double pos[3], char aircraft) - { - double lat, lon, alt, X, Y, Z; // Init terrain probe (if required) and probe data struct - static XPLMProbeInfo_t probe_data; + XPLMProbeInfo_t probe_data; probe_data.structSize = sizeof(XPLMProbeInfo_t); if(Terrain_probe == nullptr) { - Log::FormatLine(LOG_TRACE, "TERR", "Create terrain probe for aircraft %u", aircraft); + Log::FormatLine(LOG_TRACE, "GETT", "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); - return; - } // transform probe location to world coordinates - // 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 origina 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); + double lat; + double lon; + double alt; + 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, "TERR", "Probe LLA %lf %lf %lf", lat, lon, alt); + Log::FormatLine(LOG_TRACE, "GETT", "Probe LLA %lf %lf %lf", lat, lon, alt); } else { @@ -713,35 +692,32 @@ namespace XPC lon = -998; alt = -998; - Log::FormatLine(LOG_TRACE, "TERR", "Probe failed. Return Value %u", rc); + Log::FormatLine(LOG_TRACE, "GETT", "Probe failed. Return Value %u", rc); } - + // keep probe for next query - // XPLMDestroyProbe(Terrain_probe); + // XPLMDestroyProbe(probe); // Assemble response message - unsigned char response[62] = "TERR"; + unsigned char response[50] = "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 normal vector + // terrain incidence 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 + 54, &probe_data.is_wet, 4); + memcpy(response + 42, &probe_data.is_wet, 4); // probe status - memcpy(response + 58, &rc, 4); - - sock->SendTo(response, 62, &connection.addr); + memcpy(response + 46, &rc, 4); + + sock->SendTo(response, 50, &connection.addr); } + void MessageHandlers::HandleSimu(const Message& msg) { // Update log From 70b4af90b21177e375ae153a1a5b1459851edb58 Mon Sep 17 00:00:00 2001 From: Norman Princen <63923352+NPrincen@users.noreply.github.com> Date: Sun, 19 Apr 2020 13:47:18 -0700 Subject: [PATCH 4/6] Improved accuracy of HandleGetT terrain probe 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 Also added the ground velocity vector to the output. This is used for carrier landing where "ground" is moving. --- xpcPlugin/MessageHandlers.cpp | 89 ++++++++++++++++++++++------------- 1 file changed, 57 insertions(+), 32 deletions(-) diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp index 5163edc..c8de4fd 100644 --- a/xpcPlugin/MessageHandlers.cpp +++ b/xpcPlugin/MessageHandlers.cpp @@ -642,49 +642,71 @@ 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) + { + 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); + return; + } // 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 origina 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 { @@ -692,31 +714,34 @@ namespace XPC 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) { From c49168d8156deb366304d90bfd62012e558dc6dd Mon Sep 17 00:00:00 2001 From: Norman Princen <63923352+NPrincen@users.noreply.github.com> Date: Fri, 1 May 2020 04:59:43 -0700 Subject: [PATCH 5/6] Improved accuracy of HandleGetT terrain probe and requested changes --- xpcPlugin/MessageHandlers.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp index c8de4fd..43e90af 100644 --- a/xpcPlugin/MessageHandlers.cpp +++ b/xpcPlugin/MessageHandlers.cpp @@ -685,6 +685,7 @@ namespace XPC { Log::FormatLine(LOG_ERROR, "TERR", "Probe failed. Return Value %u", rc); XPLMDestroyProbe(Terrain_probe); + Terrain_probe = nullptr; return; } @@ -694,7 +695,7 @@ namespace XPC Log::FormatLine(LOG_TRACE, "TERR", "Conv LLA=%f, %f, %f", lat, lon, alt); // transform probe location to local coordinates - // Step 4. NOW convert your origina lat/lon with the elevation from step 3 to XYZ + // 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 From 6dcbec89746e912097349b98f73e32190d6c6d07 Mon Sep 17 00:00:00 2001 From: Norman Princen <63923352+NPrincen@users.noreply.github.com> Date: Fri, 1 May 2020 05:04:05 -0700 Subject: [PATCH 6/6] Added SendTerr function --- xpcPlugin/MessageHandlers.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/xpcPlugin/MessageHandlers.h b/xpcPlugin/MessageHandlers.h index 096a4b6..76c8468 100644 --- a/xpcPlugin/MessageHandlers.h +++ b/xpcPlugin/MessageHandlers.h @@ -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