Merge pull request #204 from NPrincen/getTERR

Improved MessageHandler HandleGetT and created getTERR C function

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

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 sock   The socket to use to send the command.
-/// \param dref  The name of the dataref to set.
+/// \param dref   The name of the dataref to set.
-/// \param value An array of values representing the data to set.
+/// \param values An array of values representing the data to set.
-/// \param size  The number of elements in values.
+/// \param size   The number of elements in values.
-/// \returns     0 if successful, otherwise a negative value.
+/// \returns      0 if successful, otherwise a negative value.
-int sendDREF(XPCSocket sock, const char* dref, float value[], int size);
+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);
 

xpcPlugin/MessageHandlers.cpp

@@ -642,49 +642,72 @@ namespace XPC
 		unsigned char aircraft = buffer[5];
 		Log::FormatLine(LOG_TRACE, "GETT", "Getting terrain information for aircraft %u", aircraft);
 		
-		double loc[3];
+		double pos[3];
-		double X;
+		memcpy(pos, buffer + 6, 24);
-		double Y;
-		double Z;
-		memcpy(loc, 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...
+			pos[0] = DataManager::GetDouble(DREF_Latitude, aircraft);
-			X = DataManager::GetDouble(DREF_LocalX, aircraft);
+			pos[1] = DataManager::GetDouble(DREF_Longitude, aircraft);
-			Z = DataManager::GetDouble(DREF_LocalZ, aircraft);
+			pos[2] = 0.0;
-			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
-		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;
+		}
 		
 		// transform probe location to world coordinates
-		double lat;
+		// Step 3. Convert that new XYZ back to LLE
-		double lon;
+		XPLMLocalToWorld(probe_data.locationX, probe_data.locationY, probe_data.locationZ, &lat, &lon, &alt);
-		double 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
 		{
@@ -692,31 +715,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);
+		memcpy(response + 58, &rc, 4);
-		
-		sock->SendTo(response, 50, &connection.addr);
-	}
 
+		sock->SendTo(response, 62, &connection.addr);
+	}
 
 	void MessageHandlers::HandleSimu(const Message& msg)
 	{

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