FlightSim-Streaming/XPlaneConnectCSP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactored message handling code into the MessageHandlers class.

Browse Source 
- Re-enabled CONF messages sent in response to CONN commands.
 - There was a bug in the construction of the CONF message.
 - Resolves nasa/XPlaneConnect#31
This commit is contained in:
Jason Watkins
2015-04-11 16:21:00 -07:00
parent 6c51fa476e
commit 52bf998632
8 changed files with 696 additions and 729 deletions
Download Patch File Download Diff File Expand all files Collapse all files

629
xpcPlugin/XPCPlugin.cpp
View File

@@ -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; i<lines; i++) //Execute changes one line at a time
{
short dataRef = (short) recValues[i][0];
if ( dataRef<0 || dataRef >134 )
{ // 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<sizeof(ind)/sizeof(ind[0]);j++)
{
if (recValues[i][ind[j]] != -998)
{
v = recValues[i][ind[j]];
XPC::DataManager::Set(XPC::DREF::LocalVX, v*cos((theta - alpha)*deg2rad)*sin(hpath*deg2rad));
XPC::DataManager::Set(XPC::DREF::LocalVY, v*sin((theta - alpha)*deg2rad));
XPC::DataManager::Set(XPC::DREF::LocalVZ, -v*cos((theta - alpha)*deg2rad)*cos(hpath*deg2rad));
}
}
break;}
{case 17: // Orientation
float orient[3]
{
recValues[i][1] == -998 ? XPC::DataManager::GetFloat(XPC::DREF::Pitch) : recValues[i][1],
recValues[i][2] == -998 ? XPC::DataManager::GetFloat(XPC::DREF::Roll) : recValues[i][2],
recValues[i][3] == -998 ? XPC::DataManager::GetFloat(XPC::DREF::HeadingTrue) : recValues[i][3]
};
XPC::DataManager::SetOrientation(orient);
break;}
{case 18: // Alpha, hpath etc.
if ( ( recValues[i][1] != recValues[i][1] ) && ( recValues[i][3] != recValues[i][3] ) ) // NaN Check
{
XPC::Log::WriteLine("[DATA] ERROR: Value must be a number (NaN received)");
break;
}
if (recValues[i][1] != -998)
savedAlpha = recValues[i][1];
if (recValues[i][3] != -998)
savedHPath = recValues[i][3];
break;}
{case 20: // Position
float pos[3]
{
recValues[i][2] == -998 ? XPC::DataManager::GetFloat(XPC::DREF::Latitude) : recValues[i][2],
recValues[i][3] == -998 ? XPC::DataManager::GetFloat(XPC::DREF::Longitude) : recValues[i][3],
recValues[i][4] == -998 ? XPC::DataManager::GetFloat(XPC::DREF::AGL) : recValues[i][4]
};
XPC::DataManager::SetPosition(pos);
break;}
{case 25: // Throttle
if ( recValues[i][1] != recValues[i][1] )
{
XPC::Log::WriteLine("[DATA] ERROR: Value must be a number (NaN received)");
break;
}
for (j=0; j<8; j++)
floatArray[j] = recValues[i][1];
XPC::DataManager::Set(XPC::DREF::ThrottleSet, floatArray, 8);
break;}
{default: // Non-Special dataRefs (everything else)
memcpy(floatArray,&recValues[i][1],8*sizeof(float));
for (j=0; j<8; j++)
{
if (LOG_VERBOSITY > 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
}