Implemented plugin support for handling WYPT commands.

C/src/xplaneConnect.c

@@ -434,6 +434,32 @@ short sendTEXT(struct xpcSocket sendfd, char* msg, int x, int y)
     return 0;
 }
 
+short sendWYPT(struct xpcSocket sendfd, WYPT_OP op, float points[], int numPoints)
+{
+	char buf[255] = "WYPT";
+	//Preconditions
+	//Validate operation
+	if (op < xpc_WYPT_ADD || op > xpc_WYPT_CLR)
+	{
+		return -1;
+	}
+	//Validate number of points
+	else if (numPoints > 19)
+	{
+		return -2;
+	}
+	//Everything checks out; send the message
+	else
+	{
+		buf[5] = op;
+		buf[6] = numPoints;
+		size_t len = sizeof(float) * numPoints;
+		memcpy(buf + 7, points, len);
+		sendUDP(sendfd, buf, len + 7);
+		return 0;
+	}
+}
+
 //READ
 //----------------------------------------
 short readUDP(struct xpcSocket recfd, char *dataRef, struct sockaddr *recvaddr)
@@ -669,3 +695,40 @@ xpcCtrl parseCTRL(const char data[])
     }
     return result;
 }
+
+xpcWypt parseWYPT(const char data[])
+{
+	xpcWypt result;
+	unsigned char len = data[4];
+	//Preconditions
+	//Validate message prefix to ensure we are looking at the right kind of packet.
+	if (strncmp(data, "WYPT", 4) != 0)
+	{
+		result.op = -1;
+	}
+	//Validate operation
+	else if (data[5] < xpc_WYPT_ADD || data[5] > xpc_WYPT_CLR)
+	{
+		result.op = -1;
+	}
+	//Validate number of points
+	else if (data[6] > 19)
+	{
+		result.op = -2;
+	}
+	//Everything checks out; copy the points into result
+	else
+	{
+		result.op = data[5];
+		result.numPoints = data[6];
+		char* ptr = data[7];
+		for (size_t i = 0; i < result.numPoints; ++i)
+		{
+			result.points[i].latitude = *((float*)ptr);
+			result.points[i].longitude = *((float*)(ptr + 4));
+			result.points[i].altitude = *((float*)(ptr + 8));
+			ptr += 12;
+		}
+	}
+	return result;
+}

C/src/xplaneConnect.h

@@ -47,6 +47,27 @@
 		float flaps;
 		char aircraft;
 	} xpcCtrl;
+
+	typedef struct
+	{
+		double latitude;
+		double longitude;
+		double altitude;
+	} Waypoint;
+
+	typedef enum
+	{
+		xpc_WYPT_ADD = 1,
+		xpc_WYPT_DEL = 2,
+		xpc_WYPT_CLR = 3
+	} WYPT_OP;
+
+	typedef struct
+	{
+		WYPT_OP op;
+		Waypoint points[20];
+		size_t numPoints;
+	} xpcWypt;
         
     // Basic Functions
         struct xpcSocket openUDP(unsigned short port, const char *xpIP, unsigned short xpPort);
@@ -83,6 +104,10 @@
         short parseRequest(const char my_message[], float *resultArray[], short arraySizes[]);
 		short readRequest(struct xpcSocket recfd, float *dataRef[], short arraySizes[], struct sockaddr *recvaddr);
 
+	// Waypoints
+		xpcWypt parseWYPT(const char data[]);
+		short sendWYPT(struct xpcSocket sendfd, WYPT_OP op, float points[], int numPoints);
+
 	// Screen Text
         short sendTEXT(struct xpcSocket sendfd, char* msg, int x, int y);
 

xpcPlugin/XPCPlugin.cpp

@@ -120,7 +120,7 @@ int handleSIMU(char buf[]);
 int handleCONN(char buf[]);
 int handlePOSI(char buf[]);
 int handleCTRL(char buf[]);
-int handleWYPT();
+int handleWYPT(char buf[], int len);
 int handleGETD(char *buf);
 int handleDREF(char *buf);
 int handleVIEW();
@@ -357,7 +357,7 @@ short handleInput(struct XPCMessage * theMessage)
 		}
 		else if (strncmp(theMessage->head,"WYPT",4)==0) // Header = WYPT (Waypoint Draw)
 		{
-			handleWYPT();
+			handleWYPT(theMessage->msg, theMessage->msglen);
 		}
 		else if (strncmp(theMessage->head,"GETD",4)==0) // Header = GETD (Data Request)
 		{
@@ -830,14 +830,35 @@ int handleCTRL(char buf[])
 	return 0;
 }
 
-int handleWYPT()
+int handleWYPT(char buf[], int len)
 {
-	char logmsg[100];
-	
 	// UPDATE LOG
-	sprintf(logmsg,"[WYPT] Message Received (Conn %i)- WAYPOINT DRAWING FEATURE UNDER CONSTRUCTION", current_connection+1);
+	char logmsg[100];
+	sprintf(logmsg,"[WYPT] Message Received (Conn %i)", current_connection+1);
 	updateLog(logmsg, strlen(logmsg));
-	
+
+	xpcWypt wypt = parseWYPT(buf);
+	if (wypt.op < 0)
+	{
+		sprintf(logmsg, "[WYPT] Failed to parse command. ERR:%i", wypt.op);
+		updateLog(logmsg, strlen(logmsg));
+		return -1;
+	}
+
+	switch (wypt.op)
+	{
+	case xpc_WYPT_ADD:
+		XPCAddWaypoints(wypt.points, wypt.numPoints);
+		break;
+	case xpc_WYPT_DEL:
+		XPCRemoveWaypoints(wypt.points, wypt.numPoints);
+		break;
+	case xpc_WYPT_CLR:
+		XPCClearWaypoints();
+		break;
+	default: //If parseWYPT is doing its job, we shouldn't ever hit this.
+		return -2;
+	}
 	return 0;
 }
 

xpcPlugin/xpcDrawing.cpp

@@ -113,7 +113,7 @@ static int RouteDrawCallback(XPLMDrawingPhase inPhase, int inIsBefore, void * in
 	{
 		g = &waypoints[i];
 		l = &localPoints[i];
-		XPLMWorldToLocal(g->lattitude, g->longitude, g->altitude,
+		XPLMWorldToLocal(g->latitude, g->longitude, g->altitude,
 			&l->x, &l->y, &l->z);
 	}
 
@@ -147,7 +147,7 @@ static int RouteDrawCallback(XPLMDrawingPhase inPhase, int inIsBefore, void * in
 		float xoff = (float)l->x - px;
 		float yoff = (float)l->y - py;
 		float zoff = (float)l->z - pz;
-		float d = sqrtf(xoff*xoff + zoff*zoff);
+		float d = sqrtf(xoff*xoff + yoff*yoff + zoff*zoff);
 		gl_drawCube((float)l->x, (float)l->y, (float)l->z, d);
 	}
 	return 1;
@@ -239,7 +239,7 @@ void XPCRemoveWaypoints(Waypoint points[], size_t numPoints)
 		for (size_t j = 0; j < numWaypoints; ++j)
 		{
 			Waypoint q = waypoints[j];
-			if (p.lattitude == q.lattitude &&
+			if (p.latitude == q.latitude &&
 				p.longitude == q.longitude &&
 				p.altitude == q.altitude)
 			{
@@ -255,7 +255,7 @@ void XPCRemoveWaypoints(Waypoint points[], size_t numPoints)
 	size_t copyCur = 0;
 	size_t count = delPointsCur;
 	delPointsCur = 0;
-	for (size_t i = 0; i < count; ++i)
+	for (size_t i = 0; i < numWaypoints; ++i)
 	{
 		if (i == delPoints[delPointsCur])
 		{

xpcPlugin/xpcDrawing.h

@@ -2,13 +2,7 @@
 #define xpcDrawing_h
 
 #include <stdlib.h>
-
+#include "xplaneConnect.h"
-typedef struct
-{
-	double lattitude;
-	double longitude;
-	double altitude;
-} Waypoint;
 
 void XPCClearMessage();