From 1a83517beef87f4fae0e312141989414bd6d71b9 Mon Sep 17 00:00:00 2001 From: Jason Watkins Date: Thu, 9 Apr 2015 09:42:26 -0700 Subject: [PATCH] Implemented plugin support for handling WYPT commands. --- C/src/xplaneConnect.c | 63 ++++++++++++++++++++++++++++++++++++++++ C/src/xplaneConnect.h | 25 ++++++++++++++++ xpcPlugin/XPCPlugin.cpp | 35 +++++++++++++++++----- xpcPlugin/xpcDrawing.cpp | 8 ++--- xpcPlugin/xpcDrawing.h | 8 +---- 5 files changed, 121 insertions(+), 18 deletions(-) diff --git a/C/src/xplaneConnect.c b/C/src/xplaneConnect.c index 313d9ef..de0991f 100755 --- a/C/src/xplaneConnect.c +++ b/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; +} diff --git a/C/src/xplaneConnect.h b/C/src/xplaneConnect.h index 38f18bf..86b1615 100644 --- a/C/src/xplaneConnect.h +++ b/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); diff --git a/xpcPlugin/XPCPlugin.cpp b/xpcPlugin/XPCPlugin.cpp index 7c54828..d6fee5e 100755 --- a/xpcPlugin/XPCPlugin.cpp +++ b/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; } diff --git a/xpcPlugin/xpcDrawing.cpp b/xpcPlugin/xpcDrawing.cpp index 4d73552..d770d27 100644 --- a/xpcPlugin/xpcDrawing.cpp +++ b/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]) { diff --git a/xpcPlugin/xpcDrawing.h b/xpcPlugin/xpcDrawing.h index e6c01ee..4d79275 100644 --- a/xpcPlugin/xpcDrawing.h +++ b/xpcPlugin/xpcDrawing.h @@ -2,13 +2,7 @@ #define xpcDrawing_h #include - -typedef struct -{ - double lattitude; - double longitude; - double altitude; -} Waypoint; +#include "xplaneConnect.h" void XPCClearMessage();