Refactored message handling code into the MessageHandlers class.
- 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
627
xpcPlugin/MessageHandlers.cpp
Normal file
627
xpcPlugin/MessageHandlers.cpp
Normal file
@@ -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<std::string, MessageHandlers::ConnectionInfo> MessageHandlers::connections;
|
||||
std::unordered_map<std::string, MessageHandler> 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<sockaddr_in*>(sa);
|
||||
inet_ntop(AF_INET, &sin->sin_addr, ip, INET6_ADDRSTRLEN);
|
||||
break;
|
||||
}
|
||||
case AF_INET6:
|
||||
{
|
||||
sockaddr_in6* sin = reinterpret_cast<sockaddr_in6*>(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<sockaddr_in*>(sa);
|
||||
return ntohs((*sin).sin_port);
|
||||
}
|
||||
case AF_INET6:
|
||||
{
|
||||
sockaddr_in6 *sin = reinterpret_cast<sockaddr_in6*>(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
|
||||
}
|
||||
}
|
||||
53
xpcPlugin/MessageHandlers.h
Normal file
53
xpcPlugin/MessageHandlers.h
Normal file
@@ -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 <string>
|
||||
#include <unordered_map>
|
||||
|
||||
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<std::string, ConnectionInfo> connections;
|
||||
static std::unordered_map<std::string, MessageHandler> handlers;
|
||||
static std::string connectionKey;
|
||||
static ConnectionInfo connection;
|
||||
static UDPSocket* sock;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -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
|
||||
}
|
||||
Binary file not shown.
@@ -104,18 +104,19 @@
|
||||
<ClInclude Include="..\Drawing.h" />
|
||||
<ClInclude Include="..\Log.h" />
|
||||
<ClInclude Include="..\Message.h" />
|
||||
<ClInclude Include="..\MessageHandlers.h" />
|
||||
<ClInclude Include="..\UDPSocket.h" />
|
||||
<ClInclude Include="..\xpcPluginTools.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\..\C\src\xplaneConnect.c" />
|
||||
<ClCompile Include="..\DataManager.cpp" />
|
||||
<ClCompile Include="..\DataMaps.cpp" />
|
||||
<ClCompile Include="..\Drawing.cpp" />
|
||||
<ClCompile Include="..\Log.cpp" />
|
||||
<ClCompile Include="..\Message.cpp" />
|
||||
<ClCompile Include="..\MessageHandlers.cpp" />
|
||||
<ClCompile Include="..\UDPSocket.cpp" />
|
||||
<ClCompile Include="..\XPCPlugin.cpp" />
|
||||
<ClCompile Include="..\xpcPluginTools.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Library Include="..\SDK\Libraries\Win\XPLM.lib" />
|
||||
|
||||
@@ -36,7 +36,7 @@
|
||||
<ClInclude Include="..\DataMaps.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\xpcPluginTools.h">
|
||||
<ClInclude Include="..\MessageHandlers.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
@@ -62,7 +62,10 @@
|
||||
<ClCompile Include="..\DataManager.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\xpcPluginTools.cpp">
|
||||
<ClCompile Include="..\MessageHandlers.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\DataMaps.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
|
||||
@@ -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 <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
#include <math.h>
|
||||
#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)<tol);
|
||||
}
|
||||
|
||||
unsigned short getIP(struct sockaddr recvaddr, char *IP)
|
||||
{
|
||||
// Gets the IP Address from sockaddr
|
||||
struct sockaddr_in *sendaddr;
|
||||
|
||||
sendaddr = (struct sockaddr_in *) &recvaddr;
|
||||
sprintf(IP,"%d.%d.%d.%d",(int) (*sendaddr).sin_addr.s_addr&0xFF, (int) ((*sendaddr).sin_addr.s_addr&0xFF00)>>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;
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
//
|
||||
// xpcPluginTools.pch
|
||||
// xpcPlugin
|
||||
//
|
||||
// Created by Chris Teubert on 4/14/14.
|
||||
//
|
||||
//
|
||||
|
||||
#ifndef xpcPlugin_xpcPluginTools_h
|
||||
#define xpcPlugin_xpcPluginTools_h
|
||||
|
||||
#include <time.h>
|
||||
#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
|
||||
Reference in New Issue
Block a user