diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp new file mode 100644 index 0000000..1d93cec --- /dev/null +++ b/xpcPlugin/MessageHandlers.cpp @@ -0,0 +1,627 @@ +//Copyright (c) 2013-2015 United States Government as represented by the Administrator of the +//National Aeronautics and Space Administration. All Rights Reserved. +#include "MessageHandlers.h" +#include "DataManager.h" +#include "DataMaps.h" +#include "Drawing.h" +#include "Log.h" + +namespace XPC +{ + std::unordered_map MessageHandlers::connections; + std::unordered_map MessageHandlers::handlers( + { + // Common messages + { "CONN", MessageHandlers::HandleConn }, + { "CTRL", MessageHandlers::HandleCtrl }, + { "DATA", MessageHandlers::HandleData }, + { "DREF", MessageHandlers::HandleDref }, + { "GETD", MessageHandlers::HandleGetD }, + { "POSI", MessageHandlers::HandlePosi }, + { "SIMU", MessageHandlers::HandleSimu }, + { "TEXT", MessageHandlers::HandleText }, + { "WYPT", MessageHandlers::HandleWypt }, + // Not implemented messages + { "VIEW", MessageHandlers::HandleUnknown }, + // X-Plane data messages + { "DSEL", MessageHandlers::HandleXPlaneData }, + { "USEL", MessageHandlers::HandleXPlaneData }, + { "DCOC", MessageHandlers::HandleXPlaneData }, + { "UCOC", MessageHandlers::HandleXPlaneData }, + { "MOUS", MessageHandlers::HandleXPlaneData }, + { "CHAR", MessageHandlers::HandleXPlaneData }, + { "MENU", MessageHandlers::HandleXPlaneData }, + { "SOUN", MessageHandlers::HandleXPlaneData }, + { "FAIL", MessageHandlers::HandleXPlaneData }, + { "RECO", MessageHandlers::HandleXPlaneData }, + { "PAPT", MessageHandlers::HandleXPlaneData }, + { "VEHN", MessageHandlers::HandleXPlaneData }, + { "VEH1", MessageHandlers::HandleXPlaneData }, + { "VEHA", MessageHandlers::HandleXPlaneData }, + { "GSET", MessageHandlers::HandleXPlaneData }, + { "OBJN", MessageHandlers::HandleXPlaneData }, + { "OBJL", MessageHandlers::HandleXPlaneData }, + { "GSET", MessageHandlers::HandleXPlaneData }, + { "ISET", MessageHandlers::HandleXPlaneData }, + { "BOAT", MessageHandlers::HandleXPlaneData }, + }); + + std::string MessageHandlers::connectionKey; + MessageHandlers::ConnectionInfo MessageHandlers::connection; + UDPSocket* MessageHandlers::sock; + + void MessageHandlers::SetSocket(UDPSocket* socket) + { + MessageHandlers::sock = socket; + } + + static std::string getIP(sockaddr* sa) + { + char ip[INET6_ADDRSTRLEN + 6] = { 0 }; + switch (sa->sa_family) + { + case AF_INET: + { + sockaddr_in* sin = reinterpret_cast(sa); + inet_ntop(AF_INET, &sin->sin_addr, ip, INET6_ADDRSTRLEN); + break; + } + case AF_INET6: + { + sockaddr_in6* sin = reinterpret_cast(sa); + inet_ntop(AF_INET6, &sin->sin6_addr, ip, INET6_ADDRSTRLEN); + break; + } + default: + return "UNKNOWN"; + } + return std::string(ip); + } + + static std::uint16_t getPort(sockaddr* sa) + { + switch (sa->sa_family) + { + case AF_INET: + { + sockaddr_in *sin = reinterpret_cast(sa); + return ntohs((*sin).sin_port); + } + case AF_INET6: + { + sockaddr_in6 *sin = reinterpret_cast(sa); + return ntohs((*sin).sin6_port); + } + default: + return ~0; + } + } + + void MessageHandlers::HandleMessage(Message& msg) + { + // Make sure we really have a message to handle. + std::string head = msg.GetHead(); + if (head == "") + { + return; + } + msg.PrintToLog(); + + // Set current connection + sockaddr sourceaddr = msg.GetSource(); + std::string ip = getIP(&sourceaddr); + std::uint16_t port = getPort(&sourceaddr); + connectionKey = ip + ":" + std::to_string(port); +#if LOG_VERBOSITY > 4 + Log::FormatLine("[MSGH] Handling message from %s", connectionKey.c_str()); +#endif + auto conn = connections.find(connectionKey); + if (conn == connections.end()) // New connection + { + connection = MessageHandlers::ConnectionInfo + { + // If this is a new connection, that means we just added an elment + // to connections. As long as we never remove elements, the size of + // connections will serve as a unique id. + connections.size(), + ip, + 49008, // By default, send information to the client on this port. + port, + 0 + }; + connections[connectionKey] = connection; +#if LOG_VERBOSITY > 4 + Log::FormatLine("[MSGH] New connection. ID=%u, Remote=%s:%u", connection.id, ip.c_str(), port); +#endif + } + else + { + connection = (*conn).second; +#if LOG_VERBOSITY > 4 + Log::FormatLine("[MSGH] Existing connection. ID=%u, Remote=%s:%u", + connection.id, connection.ip.c_str(), connection.srcPort); +#endif + } + + // Check if there is a handler for this message type. If so, execute + // that handler. Othewwise, execute the unknown message handler. + auto iter = handlers.find(head); + if (iter != handlers.end()) + { + MessageHandler handler = (*iter).second; + handler(msg); + } + else + { + MessageHandlers::HandleUnknown(msg); + } + } + + void MessageHandlers::HandleConn(Message& msg) + { + const std::uint8_t* buffer = msg.GetBuffer(); + + // Store new port + connection.dstPort = *((std::uint16_t*)(buffer + 5)); + connections[connectionKey] = connection; + + // Create response + std::uint8_t response[6] = "CONF"; + response[5] = connection.id; + + // Update log +#if LOG_VERBOSITY > 0 + Log::FormatLine("[CONN] ID: %u New destination port: %u", + connection.id, connection.dstPort); +#endif + + // Send response + sock->SendTo(response, 6, connection.ip, connection.dstPort); + } + + void MessageHandlers::HandleCtrl(Message& msg) + { + // Update Log +#if LOG_VERBOSITY > 0 + Log::FormatLine("[CTRL] Message Received (Conn %i)", connection.id); +#endif + + const std::uint8_t* buffer = msg.GetBuffer(); + std::size_t size = msg.GetSize(); + //Legacy packets that don't specify an aircraft number should be 26 bytes long. + //Packets specifying an A/C num should be 27 bytes. + if (size != 26 && size != 27) + { +#if LOG_VERBOSITY > 1 + Log::FormatLine("[CTRL] ERROR: Unexpected message length (%i)", size); +#endif + return; + } + + // Parse message data + float pitch = *((float*)(buffer + 5)); + float roll = *((float*)(buffer + 9)); + float yaw = *((float*)(buffer + 13)); + float thr = *((float*)(buffer + 17)); + std::int8_t gear = buffer[21]; + float flaps = *((float*)(buffer + 22)); + float aircraft = 0; + if (size == 27) + { + aircraft = buffer[26]; + } + + float thrArray[8]; + for (int i = 0; i < 8; ++i) + { + thrArray[i] = thr; + } + + DataManager::Set(DREF::YokePitch, pitch, aircraft); + DataManager::Set(DREF::YokeRoll, roll, aircraft); + DataManager::Set(DREF::YokeHeading, yaw, aircraft); + DataManager::Set(XPC::DREF::ThrottleSet, thrArray, 8, aircraft); + DataManager::Set(XPC::DREF::ThrottleActual, thrArray, 8, aircraft); + if (aircraft == 0) + { + DataManager::Set("sim/flightmodel/engine/ENGN_thro_override", thrArray, 1); + } + if (gear != -1) + { + DataManager::SetGear(gear, false, aircraft); + } + if (flaps < -999.5 || flaps > -997.5) + { + DataManager::Set(DREF::FlapSetting, flaps, aircraft); + } + } + + void MessageHandlers::HandleData(Message& msg) + { + // Parse data + const std::uint8_t* buffer = msg.GetBuffer(); + std::size_t size = msg.GetSize(); + std::uint8_t numCols = (size - 5) / 36; + float values[32][9]; + for (int i = 0; i < numCols; ++i) + { + values[i][0] = buffer[5 + 36 * i]; + memcpy(values[i] + 1, buffer + 9 + 36 * i, 9 * sizeof(float)); + } + + // Update log +#if LOG_VERBOSITY > 1 + if (numCols > 0) + { + // UPDATE LOG + Log::FormatLine("[DATA] Message Received (Conn %i)", connection.id); + } + else + { + // UPDATE LOG + Log::FormatLine("[DATA] WARNING: Empty data packet received (Conn %i)", connection.id); + return; + } +#endif + + float savedAlpha = -998; + float savedHPath = -998; + for (int i = 0; i < numCols; ++i) + { + std::uint8_t dataRef = (std::uint8_t)values[i][0]; + if (dataRef > 134) + { + Log::FormatLine("[DATA] ERROR: DataRef # must be between 0 - 134 (Received: %hi)", (int)dataRef); + } + + switch (dataRef) + { + case 3: // Velocity + { + float theta = DataManager::GetFloat(DREF::Pitch); + float alpha = savedAlpha != -998 ? savedAlpha : DataManager::GetFloat(DREF::AngleOfAttack); + float hpath = savedHPath != -998 ? savedHPath : DataManager::GetFloat(DREF::HPath); + if (alpha != alpha || hpath != hpath) + { +#if LOG_VERBOSITY > 0 + Log::WriteLine("[DATA] ERROR: Value must be a number (NaN received)"); +#endif + break; + } + const float deg2rad = (float) 0.0174532925; + int ind[3] = { 1, 3, 4 }; + for (int j = 0; j < 3; ++j) + { + float v = values[i][ind[j]]; + if (v != -998) + { + DataManager::Set(DREF::LocalVX, v*cos((theta - alpha)*deg2rad)*sin(hpath*deg2rad)); + DataManager::Set(DREF::LocalVY, v*sin((theta - alpha)*deg2rad)); + DataManager::Set(DREF::LocalVZ, -v*cos((theta - alpha)*deg2rad)*cos(hpath*deg2rad)); + } + } + break; + } + case 17: // Orientation + { + float orient[3] + { + values[i][1] == -998 ? DataManager::GetFloat(DREF::Pitch) : values[i][1], + values[i][2] == -998 ? DataManager::GetFloat(DREF::Roll) : values[i][2], + values[i][3] == -998 ? DataManager::GetFloat(DREF::HeadingTrue) : values[i][3] + }; + DataManager::SetOrientation(orient); + break; + } + case 18: // Alpha, hpath etc. + { + if (values[i][1] != values[i][1] || values[i][3] != values[i][3]) + { +#if LOG_VERBOSITY > 0 + Log::WriteLine("[DATA] ERROR: Value must be a number (NaN received)"); +#endif + break; + } + if (values[i][1] != -998) + { + savedAlpha = values[i][1]; + } + if (values[i][3] != -998) + { + savedHPath = values[i][3]; + } + break; + } + case 20: // Position + { + float pos[3] + { + values[i][2] == -998 ? DataManager::GetFloat(DREF::Latitude) : values[i][2], + values[i][3] == -998 ? DataManager::GetFloat(DREF::Longitude) : values[i][3], + values[i][4] == -998 ? DataManager::GetFloat(DREF::AGL) : values[i][4] + }; + DataManager::SetPosition(pos); + break; + } + case 25: // Throttle + { + if (values[i][1] != values[i][1]) + { +#if LOG_VERBOSITY > 0 + Log::WriteLine("[DATA] ERROR: Value must be a number (NaN received)"); +#endif + break; + } + float thr[8]; + for (int j = 0; j < 8; ++j) + { + thr[j] = values[i][1]; + } + DataManager::Set(DREF::ThrottleSet, thr, 8); + break; + } + default: // Non-Special dataRefs + { + float line[8]; + memcpy(line, values[i] + 1, 8 * sizeof(float)); + for (int j = 0; j < 8; ++j) + { +#if LOG_VERBOSITY > 1 + Log::FormatLine("Setting Dataref %i.%i to %f", dataRef, j, line[j]); +#endif + + if (dataRef == 14 && j == 0) + { + DataManager::SetGear(line[0], true); + continue; + } + + XPC::DREF dref = XPC::XPData[dataRef][j]; + if (dref == XPC::DREF::None) + { + HandleXPlaneData(msg); + } + else + { + XPC::DataManager::Set(dref, line, 8); + } + } + } + } + } + } + + void MessageHandlers::HandleDref(Message& msg) + { + const std::uint8_t* buffer = msg.GetBuffer(); + std::uint8_t len = buffer[5]; + std::string dref = std::string((char*)buffer + 6, len); + + std::uint8_t valueCount = buffer[6 + len]; + float values[40]; + memcpy(values, buffer + len + 7, valueCount * sizeof(float)); + +#if LOG_VERBOSITY > 1 + Log::FormatLine("[DREF] Request to set DREF value received (Conn %i): %s", connection.id, dref.c_str()); +#endif + + DataManager::Set(dref, values, valueCount); + } + + void MessageHandlers::HandleGetD(Message& msg) + { + const std::uint8_t* buffer = msg.GetBuffer(); + std::uint8_t drefCount = buffer[5]; + if (drefCount == 0) // Use last request + { +#if LOG_VERBOSITY > 0 + Log::FormatLine("[GETD] DATA Requested: Repeat last request from connection %i (%i data refs)", + connection.id, connection.getdCount); +#endif + if (connection.getdCount == 0) // No previous request to use + { +#if LOG_VERBOSITY > 1 + Log::FormatLine("[GETD] ERROR: No previous requests from connection %i.", connection.id); +#endif + return; + } + } + else // New request + { +#if LOG_VERBOSITY > 0 + Log::FormatLine("[GETD] DATA Requested: New Request for connection %i (%i data refs)", + connection.id, drefCount); +#endif + std::size_t ptr = 6; + for (int i = 0; i < drefCount; ++i) + { + std::uint8_t len = buffer[ptr]; + connection.getdRequest[i] = std::string((char*)buffer + 1 + ptr, len); + ptr += 1 + len; + } + connection.getdCount = drefCount; + connections[connectionKey] = connection; + } + + std::uint8_t response[4096] = "RESP"; + response[5] = drefCount; + std::size_t cur = 6; + for (int i = 0; i < drefCount; ++i) + { + float values[255]; + std::size_t count = DataManager::Get(connection.getdRequest[i], values, 255); + response[cur++] = count; + memcpy(response + cur, values, count * sizeof(float)); + cur += count * sizeof(float); + } + + sock->SendTo(response, cur, connection.ip, connection.dstPort); + } + + void MessageHandlers::HandlePosi(Message& msg) + { + // Update log +#if LOG_VERBOSITY > 0 + Log::FormatLine("[POSI] Message Received (Conn %i)", connection.id); +#endif + + const std::uint8_t* buffer = msg.GetBuffer(); + const std::size_t size = msg.GetSize(); + if (size < 34) + { +#if LOG_VERBOSITY > 1 + Log::FormatLine("[POSI] ERROR: Unexpected size: %i (Expected at least 34)", size); +#endif + return; + } + + std::int8_t aircraft = buffer[5]; + float gear = *((float*)(buffer + 30)); + float pos[3]; + float orient[3]; + memcpy(pos, buffer + 6, 12); + memcpy(orient, buffer + 18, 12); + + if (aircraft > 0) + { + // Enable AI for the aircraft we are setting + float ai[20]; + std::size_t result = DataManager::GetFloatArray(DREF::PauseAI, ai, 20); + if (result == 20) // Only set values if they were retrieved successfully. + { + ai[aircraft] = 1; + DataManager::Set(DREF::PauseAI, ai, 0, 20); + } + } + + DataManager::SetPosition(pos, aircraft); + DataManager::SetOrientation(orient, aircraft); + if (gear != -1) + { + DataManager::SetGear(gear, true, aircraft); + } + } + + void MessageHandlers::HandleSimu(Message& msg) + { + // Update log +#if LOG_VERBOSITY > 0 + Log::FormatLine("[SIMU] Message Received (Conn %i)", connection.id); +#endif + + const std::uint8_t* buffer = msg.GetBuffer(); + + // Set DREF + int value = buffer[5]; + DataManager::Set(DREF::Pause, &value, 1); + +#if LOG_VERBOSITY > 2 + if (buffer[5] == 0) + { + Log::FormatLine("[SIMU] Simulation Resumed (Conn %i)", connection.id); + } + else + { + Log::FormatLine("[SIMU] Simulation Paused (Conn %i)", connection.id); + } +#endif + } + + void MessageHandlers::HandleText(Message& msg) + { + // Update Log +#if LOG_VERBOSITY > 0 + Log::FormatLine("[TEXT] Message Received (Conn %i)", connection.id); +#endif + + std::size_t len = msg.GetSize(); + const std::uint8_t* buffer = msg.GetBuffer(); + + char text[256] = { 0 }; + if (len < 14) + { +#if LOG_VERBOSITY > 1 + Log::WriteLine("[TEXT] ERROR: Length less than 14 bytes"); +#endif + return; + } + size_t msgLen = (unsigned char)buffer[13]; + if (msgLen == 0) + { + Drawing::ClearMessage(); +#if LOG_VERBOSITY > 2 + Log::WriteLine("[TEXT] Text cleared"); +#endif + } + else + { + int x = *((int*)(buffer + 5)); + int y = *((int*)(buffer + 9)); + strncpy(text, (char*)buffer + 14, msgLen); + Drawing::SetMessage(x, y, text); +#if LOG_VERBOSITY > 2 + Log::WriteLine("[TEXT] Text set"); +#endif + } + } + + void MessageHandlers::HandleWypt(Message& msg) + { + // Update Log +#if LOG_VERBOSITY > 0 + Log::FormatLine("[WYPT] Message Received (Conn %i)", connection.id); +#endif + + // Parse data + const std::uint8_t* buffer = msg.GetBuffer(); + std::uint8_t op = buffer[5]; + std::uint8_t count = buffer[6]; + Waypoint points[255]; + const std::uint8_t* ptr = buffer + 7; + for (size_t i = 0; i < count; ++i) + { + points[i].latitude = *((float*)ptr); + points[i].longitude = *((float*)(ptr + 4)); + points[i].altitude = *((float*)(ptr + 8)); + ptr += 12; + } + + // Perform operation +#if LOG_VERBOSITY > 2 + Log::FormatLine("[WYPT] Performing operation %i", op); +#endif + switch (op) + { + case xpc_WYPT_ADD: + Drawing::AddWaypoints(points, count); + break; + case xpc_WYPT_DEL: + Drawing::RemoveWaypoints(points, count); + break; + case xpc_WYPT_CLR: + Drawing::ClearWaypoints(); + break; + default: +#if LOG_VERBOSITY > 1 + Log::FormatLine("[WYPT] ERROR: %i is not a valid operation.", op); +#endif + break; + } + } + + void MessageHandlers::HandleXPlaneData(Message& msg) + { +#if LOG_VERBOSITY > 1 + Log::WriteLine("[MSGH] Sending raw data to X-Plane"); +#endif + sock->SendTo((std::uint8_t*)msg.GetBuffer(), msg.GetSize(), "127.0.0.1", 49000); + } + + void MessageHandlers::HandleUnknown(Message& msg) + { + // UPDATE LOG +#if LOG_VERBOSITY > 0 + XPC::Log::FormatLine("[EXEC] ERROR: Unknown packet type %s", msg.GetHead().c_str()); +#endif + } +} \ No newline at end of file diff --git a/xpcPlugin/MessageHandlers.h b/xpcPlugin/MessageHandlers.h new file mode 100644 index 0000000..544023a --- /dev/null +++ b/xpcPlugin/MessageHandlers.h @@ -0,0 +1,53 @@ +//Copyright (c) 2013-2015 United States Government as represented by the Administrator of the +//National Aeronautics and Space Administration. All Rights Reserved. +#ifndef XPC_MESSAGEHANDLERS_H +#define XPC_MESSAGEHANDLERS_H +#include "Message.h" +#include "UDPSocket.h" + +#include +#include + +namespace XPC +{ + typedef void(*MessageHandler)(Message&); + + class MessageHandlers + { + public: + static void HandleMessage(Message& msg); + + static void SetSocket(UDPSocket* socket); + + private: + static void HandleConn(Message& msg); + static void HandleCtrl(Message& msg); + static void HandleData(Message& msg); + static void HandleDref(Message& msg); + static void HandleGetD(Message& msg); + static void HandlePosi(Message& msg); + static void HandleSimu(Message& msg); + static void HandleText(Message& msg); + static void HandleWypt(Message& msg); + static void HandleXPlaneData(Message& msg); + static void HandleUnknown(Message& msg); + + typedef struct + { + std::uint8_t id; + std::string ip; + std::uint16_t dstPort; + std::uint16_t srcPort; + std::uint8_t getdCount; + std::string getdRequest[255]; + } ConnectionInfo; + + static std::unordered_map connections; + static std::unordered_map handlers; + static std::string connectionKey; + static ConnectionInfo connection; + static UDPSocket* sock; + }; +} + +#endif \ No newline at end of file diff --git a/xpcPlugin/XPCPlugin.cpp b/xpcPlugin/XPCPlugin.cpp index e718108..a9de952 100755 --- a/xpcPlugin/XPCPlugin.cpp +++ b/xpcPlugin/XPCPlugin.cpp @@ -66,8 +66,8 @@ #include "DataMaps.h" #include "Drawing.h" #include "Message.h" +#include "MessageHandlers.h" #include "UDPSocket.h" -#include "xpcPluginTools.h" // XPLM Includes //#include "XPLMPlanes.h" @@ -93,28 +93,12 @@ XPC::UDPSocket* recvSocket = nullptr; XPC::UDPSocket* sendSocket = nullptr; -short number_of_connections = 0; -short current_connection = -1; - double start,lap; static double timeConvert = 0.0; int benchmarkingSwitch = 0; // 1 = time for operations, 2 = time for op + cycle; int cyclesToClear = -1; // Clear message bus every n cycles. -1 == dont clear int counter = 0; -struct connectionHistory -{ - short connectionID; - - char IP[16]; - unsigned short recPort; - unsigned short fromPort; - short requestLength; - std::string XPLMRequestedDRefs[100]; -}; - -connectionHistory connectionList[MAXCONN]; - PLUGIN_API int XPluginStart(char * outName, char * outSig, char * outDesc); static float MyFlightLoopCallback(float, float, int inCounter, void * inRefcon); PLUGIN_API void XPluginStop(void); @@ -122,20 +106,6 @@ PLUGIN_API void XPluginDisable(void); PLUGIN_API int XPluginEnable(void); PLUGIN_API void XPluginReceiveMessage(XPLMPluginID inFromWho, int inMessage, void * inParam); -int handleSIMU(char buf[]); -int handleCONN(char buf[]); -int handlePOSI(char buf[]); -int handleCTRL(char buf[]); -int handleWYPT(char buf[], int len); -int handleGETD(char *buf); -int handleDREF(char *buf); -int handleVIEW(); -int handleDATA(char *buf, int buflen); -int handleTEXT(char *buf, int len); -short handleInput(XPC::Message& msg); - -void sendBUF(char buf[], int buflen); - PLUGIN_API int XPluginStart( char * outName, char * outSig, char * outDesc) @@ -202,6 +172,7 @@ PLUGIN_API int XPluginEnable(void) // Open sockets recvSocket = new XPC::UDPSocket(RECVPORT); sendSocket = new XPC::UDPSocket(SENDPORT); + XPC::MessageHandlers::SetSocket(sendSocket); XPC::Log::WriteLine("[EXEC] xpcPlugin Enabled, sockets opened"); if (benchmarkingSwitch > 0) @@ -227,7 +198,6 @@ float MyFlightLoopCallback( float inElapsedSinceLastCall, void * inRefcon) { int i; - short result; #if (__APPLE__) double diff_t; #endif @@ -247,7 +217,11 @@ float MyFlightLoopCallback( float inElapsedSinceLastCall, #endif } XPC::Message msg = XPC::Message::ReadFrom(*recvSocket); - result = handleInput(msg); + if (msg.GetHead() == "") + { + break; + } + XPC::MessageHandlers::HandleMessage(msg); if (benchmarkingSwitch > 0) { @@ -257,8 +231,6 @@ float MyFlightLoopCallback( float inElapsedSinceLastCall, XPC::Log::FormatLine("Runtime %.6f",diff_t); #endif } - - if (result<=0) break; // No Signal } if (cyclesToClear != -1) @@ -271,589 +243,4 @@ float MyFlightLoopCallback( float inElapsedSinceLastCall, } } return -1; -} - -short handleInput(XPC::Message& msg) -{ - int i; - char IP[16] = { 0 }; - unsigned short port = 0; - - current_connection = -1; - - if (msg.GetSize() == 0) - { - // No message received - return 0; - } - - msg.PrintToLog(); - - // Check for existing connection - port = getIP(msg.GetSource(), IP); - for (i = 0; i < number_of_connections; i++) - { - if (strcmp(connectionList[i].IP, IP) == 0 && port == connectionList[i].fromPort && port > 0) - { - current_connection = i; - break; - } - } - - if (current_connection == -1) //SETUP NEW CONNECTION - { - if (number_of_connections >= MAXCONN) // Handle hitting MAXCONN (COME UP WITH A BETTER SOLUTION) - { - XPC::Log::WriteLine("[EXEC] Hit maximum number of connections-Removing old ones"); - number_of_connections = 0; - } - - memcpy(connectionList[number_of_connections].IP, IP, 16); - connectionList[number_of_connections].recPort = 49008; - connectionList[number_of_connections].fromPort = port; - current_connection = number_of_connections; - number_of_connections++; - - // Log Connection - XPC::Log::FormatLine("[EXEC] New Connection [%i]. IP=%s, port=%i", number_of_connections, IP, port); - } - - std::string head = msg.GetHead(); - if (head == "CONN") // Header = CONN (Connection) - { - handleCONN((char*)msg.GetBuffer()); - } - else if (head == "SIMU") // Header = SIMU - { - handleSIMU((char*)msg.GetBuffer()); - } - else if (head == "POSI") // Header = POSI (Position) - { - handlePOSI((char*)msg.GetBuffer()); - } - else if (head == "CTRL") // Header = CTRL (Control) - { - handleCTRL((char*)msg.GetBuffer()); - } - else if (head == "WYPT") // Header = WYPT (Waypoint Draw) - { - handleWYPT((char*)msg.GetBuffer(), msg.GetSize()); - } - else if (head == "GETD") // Header = GETD (Data Request) - { - handleGETD((char*)msg.GetBuffer()); - } - else if (head == "DREF") // Header = DREF (By Data Ref) (this is slower than DATA) - { - handleDREF((char*)msg.GetBuffer()); - } - else if (head == "VIEW") // Header = VIEW (Change View) - { - handleVIEW(); - } - else if (head == "DATA") // Header = DATA (UDP Data) - { - handleDATA((char*)msg.GetBuffer(), msg.GetSize()); - } - else if (head == "TEXT") // Header = TEXT (Screen message) - { - handleTEXT((char*)msg.GetBuffer(), msg.GetSize()); - } - else if ((head == "DSEL") || (head == "USEL")) // Header = DSEL/USEL (Select UDP Send) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if ((head == "DCOC") || (head == "UCOC")) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if ((head == "MOUS") || (head == "CHAR")) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if (head == "MENU") - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if (head == "SOUN") - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if ((head == "FAIL") || (head == "RECO")) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if (head == "PAPT") - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if ((head == "VEHN") || head == "VEH1" || (head == "VEHA")) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if ((head == "OBJN") || (head == "OBJL")) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if ((head == "GSET") || (head == "ISET")) - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else if (head == "BOAT") - { - sendBUF((char*)msg.GetBuffer(), msg.GetSize()); // Send to UDP - } - else - { //unrecognized header - XPC::Log::FormatLine("[EXEC] ERROR: Command %s not recognized", head); - } - current_connection = -1; - - return 0; -} - -void sendBUF( char buf[], int buflen) -{ - sendSocket->SendTo((std::uint8_t*)buf, buflen, "127.0.0.1", 49000); -} - -int handleCONN(char buf[]) -{ - char the_message[7]; - char header[5]; - - strncpy(header,"CONF",4); - memcpy(&the_message,&header,4); - the_message[4] = (char) current_connection; - - // COPY PORT - memcpy(&(connectionList[current_connection].recPort),&buf[5],sizeof(connectionList[current_connection].recPort)); - - if (connectionList[current_connection].recPort <= 1) // Is not valid port - { - connectionList[current_connection].recPort = 49008; - XPC::Log::WriteLine("[CONN] ERROR: Port Number must be a number (NaN received)"); - return 1; - } - - - // UPDATE LOG - XPC::Log::FormatLine("[CONN] Update Connection %i- Sending to port: %i", - current_connection + 1, - connectionList[current_connection].recPort); - - // SEND CONFIRMATION - // TODO: Ivestigate why sending confirmation causes crashes on Windows 8 - //memcpy(sendSocketet.xpIP,connectionList[current_connection].IP, sizeof(connectionList[current_connection].IP)); - //sendSocketet.xpPort = connectionList[current_connection].recPort; - //sendUDP(sendSocketet, the_message, 5); - - return 0; -} - -int handleSIMU(char buf[]) -{ - int SIMUArray[1] = {buf[5]}; - - if (SIMUArray[0] != SIMUArray[0]) // Is NaN - { - XPC::Log::WriteLine("[SIMU] ERROR: Value must be a number (NaN received)"); - return 1; - } - - int value = buf[5]; - XPC::DataManager::Set(XPC::DREF::Pause, &value, 1); - - if (buf[5] == 0) - { - XPC::Log::FormatLine("[SIMU] Simulation Resumed (Conn %i)", current_connection + 1); - } - else - { - XPC::Log::FormatLine("[SIMU] Simulation Paused (Conn %i)", current_connection + 1); - } - - return 0; -} - -int handleTEXT(char *buf, int len) -{ - char msg[256] = { 0 }; - if (len < 14) - { - XPC::Log::WriteLine("[TEXT] ERROR: Length less than 14 bytes"); - return -1; - } - size_t msgLen = (unsigned char)buf[13]; - if (msgLen == 0) - { - XPC::Drawing::ClearMessage(); - XPC::Log::WriteLine("[TEXT] Text cleared"); - } - else - { - int x = *((int*)(buf + 5)); - int y = *((int*)(buf + 9)); - strncpy(msg, buf + 14, msgLen); - XPC::Drawing::SetMessage(x, y, msg); - XPC::Log::WriteLine("[TEXT] Text set"); - } - return 0; -} - -int handlePOSI(char buf[]) -{ - float position[8] = {0.0}; - float pos[3],orient[3]; - short aircraft = 0; - float gear = -1.0; - int autopilot[20] = {0}; - - // UPDATE LOG - XPC::Log::FormatLine("[POSI] Message Received (Conn %i)", current_connection + 1); - - aircraft = fmini(parsePOSI(buf,position,6, &gear),19); - - if (aircraft > 0) - { - // Enable AI for the aircraft we are setting - float ai[20]; - std::size_t result = XPC::DataManager::GetFloatArray(XPC::DREF::PauseAI, ai, 20); - if (result == 20) // Only set values if they were retrieved successfully. - { - ai[aircraft] = 1; - XPC::DataManager::Set(XPC::DREF::PauseAI, ai, 0, 20); - } - } - - // Position - memcpy(pos,position,3*sizeof(float)); - XPC::DataManager::SetPosition(pos, aircraft); - - // Orientation - memcpy(orient,&position[3],3*sizeof(float)); - XPC::DataManager::SetOrientation(orient, aircraft); - - //Landing Gear - if (gear != -1) - { - XPC::DataManager::SetGear(gear, false, aircraft); - } - - return 0; -} - -int handleCTRL(char buf[]) -{ - xpcCtrl ctrl; - float thr[8] = { 0 }; - short i; - - // UPDATE LOG - XPC::Log::FormatLine("[CTRL] Message Received (Conn %i)", current_connection + 1); - - ctrl = parseCTRL(buf); - if (ctrl.aircraft < 0) //parseCTRL failed - { - return 1; - } - if (ctrl.aircraft > 19) //Can only handle 19 non-player aircraft right now - { - return 2; - } - // SET CONTROLS - XPC::DataManager::Set(XPC::DREF::YokePitch, ctrl.pitch, ctrl.aircraft); - XPC::DataManager::Set(XPC::DREF::YokeRoll, ctrl.roll, ctrl.aircraft); - XPC::DataManager::Set(XPC::DREF::YokeHeading, ctrl.yaw, ctrl.aircraft); - - // SET Throttle - for (i = 0; i<8; i++) - { - thr[i] = ctrl.throttle; - } - XPC::DataManager::Set(XPC::DREF::ThrottleSet, thr, 8, ctrl.aircraft); - XPC::DataManager::Set(XPC::DREF::ThrottleActual, thr, 8, ctrl.aircraft); - if (ctrl.aircraft == 0) - { - XPC::DataManager::Set("sim/flightmodel/engine/ENGN_thro_override", thr, 1); - } - - // SET Gear/Flaps - if (ctrl.gear != -1) - { - XPC::DataManager::SetGear(ctrl.gear, false, ctrl.aircraft); - } - if (ctrl.flaps < -999.5 || ctrl.flaps > -997.5) // Flaps - { - XPC::DataManager::Set(XPC::DREF::FlapSetting, ctrl.flaps, ctrl.aircraft); - } - return 0; -} - -int handleWYPT(char buf[], int len) -{ - // UPDATE LOG - XPC::Log::FormatLine("[WYPT] Message Received (Conn %i)", current_connection + 1); - - xpcWypt wypt = parseWYPT(buf); - if (wypt.op < 0) - { - XPC::Log::FormatLine("[WYPT] Failed to parse command. ERR:%i", wypt.op); - return -1; - } - else - { - XPC::Log::FormatLine("[WYPT] Performing operation %i", wypt.op); - } - - switch (wypt.op) - { - case xpc_WYPT_ADD: - XPC::Drawing::AddWaypoints(wypt.points, wypt.numPoints); - break; - case xpc_WYPT_DEL: - XPC::Drawing::RemoveWaypoints(wypt.points, wypt.numPoints); - break; - case xpc_WYPT_CLR: - XPC::Drawing::ClearWaypoints(); - break; - default: //If parseWYPT is doing its job, we shouldn't ever hit this. - return -2; - } - return 0; -} - -int handleGETD(char buf[]) -{ - int DREFSizes[100] = { 0 }; - char *DREFArray[100]; - std::uint8_t drefCount = buf[5]; - if (drefCount == 0) // USE LAST REQUEST - { - XPC::Log::FormatLine("[GETD] DATA Requested- repeat last request from connection %i (%i data refs)", - current_connection + 1, - connectionList[current_connection].requestLength); - - if (connectionList[current_connection].requestLength < 0) // No past requests - { - XPC::Log::FormatLine("[GETD] ERROR- No previous requests from connection %i.", current_connection + 1); - return 1; - } - } - else if (drefCount > 0) // NEW REQUEST - { - connectionList[current_connection].requestLength = (short)drefCount; - - for (int i = 0; i < connectionList[current_connection].requestLength; i++) - { - DREFArray[i] = (char *)malloc(100); - memset(DREFArray[i], 0, 100); - } - - parseGETD(buf, DREFArray, DREFSizes); - XPC::Log::FormatLine("[GETD] DATA Requested- New Request for connection %i [%i]:", - current_connection + 1, - drefCount); - } - else - { - return -1; - } - - std::uint8_t response[4096] = "RESP"; - response[5] = drefCount; - std::size_t cur = 6; - for (int i = 0; i < drefCount; ++i) - { - float values[255]; - std::size_t count = XPC::DataManager::Get(DREFArray[i], values, 255); - response[cur] = count; - memcpy(response + 1 + cur, values, count * sizeof(float)); - cur += 1 + count * sizeof(float); - } - - char* host = connectionList[current_connection].IP; - std::uint16_t port = connectionList[current_connection].recPort; - sendSocket->SendTo(response, cur, host, port); - - return 0; -} - -int handleDREF(char buf[]) -{ - char DREF[100] = {0}; - unsigned short lenDREF = 0; - unsigned short lenVALUE = 0; - float values[40] = {0.0}; - - parseDREF(buf, DREF, &lenDREF,values,&lenVALUE); - - // Input Varification - if (lenDREF <= 0) - { - return 1; - } - - // Handle DREF - XPC::Log::FormatLine("[DREF] Request to set DREF value received (Conn %i): %s", current_connection + 1, DREF); - - XPC::DataManager::Set(DREF, values, lenVALUE); - - return 0; -} - -int handleVIEW() -{ - XPC::Log::FormatLine("[VIEW] Message Received (Conn %i)- VIEW FEATURE UNDER CONSTRUCTION", - current_connection + 1); - return 0; -} - -int handleDATA(char buf[], int buflen) -{ - int i,j,lines; - float floatArray[10] = {0}; - float recValues[20][9] = {0}; - const float deg2rad = (float) 0.0174532925; - float savedAlpha=-998; - float savedHPath=-998; - - lines = parseDATA(buf, buflen, recValues); //Parse Data - if (lines > 0) - { - // UPDATE LOG - XPC::Log::FormatLine("[DATA] Message Received (Conn %i)", current_connection + 1); - } - else - { - // UPDATE LOG - XPC::Log::FormatLine("[DATA] WARNING: Empty data packet received (Conn %i)", current_connection + 1); - } - - for (i = 0; i134 ) - { // DREF Check 1: This ensures that the received dataRef is in the range of 0-134 - XPC::Log::FormatLine("[DATA] ERROR: DataRef # must be between 0 - 134 (Rec: %hi)",dataRef); - continue; - } - - switch (dataRef) - { - {case 3: // Velocity - // Velocity has to be broken into individual components in X-Plane World Coordinate System where x=East, y=Up. - //Note: Add Sideslip - - float theta, alpha, hpath,v; - int ind[3] = {1,3,4}; - theta = XPC::DataManager::GetFloat(XPC::DREF::Pitch); - - if (savedAlpha != -998) - alpha = savedAlpha; - else - alpha = XPC::DataManager::GetFloat(XPC::DREF::AngleOfAttack); - - if (savedHPath != -998) - hpath = savedHPath; - else - hpath = XPC::DataManager::GetFloat(XPC::DREF::HPath); //Velocity Heading - - if ( ( hpath != hpath ) && ( alpha != alpha ) && ( theta != theta ) ) // NaN Check - { - XPC::Log::WriteLine("[DATA] ERROR: Value must be a number (NaN received)"); - break; - } - - for (j=0;j 0) - { - XPC::Log::FormatLine("Setting Dataref %i.%i to %f", dataRef, j, floatArray[j]); - } - - if (dataRef==14 && j==0) - { - XPC::DataManager::SetGear(recValues[i][j + 1], false); - continue; - } - - XPC::DREF dref = XPC::XPData[dataRef][j]; - if (dref == XPC::DREF::None) - { - sendBUF(buf, buflen); - } - else - { - XPC::DataManager::Set(dref, floatArray, 8); - } - } //End for j=1:8 - break;} - } // End switch(dataRef) - }// End for(all lines) - - return 0; -} // End handleDATA +} \ No newline at end of file diff --git a/xpcPlugin/XPlaneConnect/win.xpl b/xpcPlugin/XPlaneConnect/win.xpl index 1e49218..87898db 100644 Binary files a/xpcPlugin/XPlaneConnect/win.xpl and b/xpcPlugin/XPlaneConnect/win.xpl differ diff --git a/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj b/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj index 3117f1c..2c91d4e 100644 --- a/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj +++ b/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj @@ -104,18 +104,19 @@ + - + + - diff --git a/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj.filters b/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj.filters index aecbf36..21d7598 100644 --- a/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj.filters +++ b/xpcPlugin/xpcPlugin/xpcPlugin.vcxproj.filters @@ -36,7 +36,7 @@ Header Files - + Header Files @@ -62,7 +62,10 @@ Source Files - + + Source Files + + Source Files diff --git a/xpcPlugin/xpcPluginTools.cpp b/xpcPlugin/xpcPluginTools.cpp deleted file mode 100644 index bd6e7ad..0000000 --- a/xpcPlugin/xpcPluginTools.cpp +++ /dev/null @@ -1,78 +0,0 @@ -// xpcPluginTools functions to support xpcPlugin -// -// FUNCTIONS -// buildXPLMDataRefs -// fmini -// almostequal -// updateLog -// handleDREFSIM -// getIP -// printBuffertoLog -// test -// testi -// -// CONTACT -// For questions email Christopher Teubert (christopher.a.teubert@nasa.gov) -// -// CONTRIBUTORS -// CT: Christopher Teubert (christopher.a.teubert@nasa.gov) -// -// TO DO: -// 1. Support sending -1 length to updateLog for it to calculate intself (look for /0) -// 2. Have printbuffertolog run parse function -// 3. Builddatarefs: Fill out & test options -// -// BEGIN CODE - -#include "Log.h" -#include -#include -#include -#include -#include -#include "xpcPluginTools.h" - -int fmini(int a, int b) -{ - // Returns the minimum value of two integers - return ((((a)-(b))&0x80000000) >> 31)? (a) : (b); -} - -int almostequal(float arg1, float arg2, float tol) -{ - // Compares arg1 and arg 2. If they are within tol returns true - return (abs(arg1-arg2)>8, (int) ((*sendaddr).sin_addr.s_addr&0xFF0000)>>16, (int) ((*sendaddr).sin_addr.s_addr&0xFF000000)>>24); - - return ntohs((*sendaddr).sin_port); -} - -// DEBUGGING TOOLS -// -------------------------------- - -int test(const char *buffer) -{ - // Prints "test buffer" to log (for debugging) - char buffer2[95] = {0}; - strncpy(buffer2, buffer, 95); - - XPC::Log::FormatLine("[TEST] %s", buffer2); - - return 0; -} - -int test(int buffer) -{ - // Prints "test #[buffer]" to log (for debugging) - XPC::Log::FormatLine("[TEST] #%i", buffer); - - return 0; -} diff --git a/xpcPlugin/xpcPluginTools.h b/xpcPlugin/xpcPluginTools.h deleted file mode 100644 index 5b549dc..0000000 --- a/xpcPlugin/xpcPluginTools.h +++ /dev/null @@ -1,26 +0,0 @@ -// -// xpcPluginTools.pch -// xpcPlugin -// -// Created by Chris Teubert on 4/14/14. -// -// - -#ifndef xpcPlugin_xpcPluginTools_h -#define xpcPlugin_xpcPluginTools_h - -#include -#include "xplaneConnect.h" - - int almostequal(float arg1, float arg2, float tol); - - int test(const char *buffer); - - int test(int buffer); - - unsigned short getIP(struct sockaddr recvaddr, char *IP); - - int fmini(int a, int b); - - -#endif