diff --git a/xpcPlugin/DataManager.cpp b/xpcPlugin/DataManager.cpp index 42048e5..eccb8c2 100644 --- a/xpcPlugin/DataManager.cpp +++ b/xpcPlugin/DataManager.cpp @@ -169,8 +169,8 @@ namespace XPC sprintf(multi, "sim/multiplayer/position/plane%i_gear_deploy", i); mdrefs[i][DREF_GearDeploy] = XPLMFindDataRef(multi); sprintf(multi, "sim/multiplayer/position/plane%i_flap_ratio", i); - mdrefs[i][DREF_FlapSetting] = XPLMFindDataRef(multi); // Can't set the actual flap setting on npc aircraft mdrefs[i][DREF_FlapActual] = XPLMFindDataRef(multi); + mdrefs[i][DREF_FlapSetting] = mdrefs[i][DREF_FlapActual]; // Can't set the actual flap setting on npc aircraft sprintf(multi, "sim/multiplayer/position/plane%i_flap_ratio2", i); mdrefs[i][DREF_FlapActual2] = XPLMFindDataRef(multi); sprintf(multi, "sim/multiplayer/position/plane%i_spoiler_ratio", i); @@ -183,6 +183,7 @@ namespace XPC mdrefs[i][DREF_Sweep] = XPLMFindDataRef(multi); sprintf(multi, "sim/multiplayer/position/plane%i_throttle", i); mdrefs[i][DREF_ThrottleActual] = XPLMFindDataRef(multi); + mdrefs[i][DREF_ThrottleSet] = mdrefs[i][DREF_ThrottleActual]; // No throttle set for multiplayer planes. sprintf(multi, "sim/multiplayer/position/plane%i_yolk_pitch", i); mdrefs[i][DREF_YokePitch] = XPLMFindDataRef(multi); sprintf(multi, "sim/multiplayer/position/plane%i_yolk_roll", i); @@ -729,13 +730,13 @@ namespace XPC // http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf float q[4]; float halfRad = 0.00872664625997F; - orient[2] = halfRad * orient[2]; - orient[0] = halfRad * orient[0]; - orient[1] = halfRad * orient[1]; - q[0] = cos(orient[2]) * cos(orient[0]) * cos(orient[1]) + sin(orient[2]) * sin(orient[0]) * sin(orient[1]); - q[1] = cos(orient[2]) * cos(orient[0]) * sin(orient[1]) - sin(orient[2]) * sin(orient[0]) * cos(orient[1]); - q[2] = cos(orient[2]) * sin(orient[0]) * cos(orient[1]) + sin(orient[2]) * cos(orient[0]) * sin(orient[1]); - q[3] = sin(orient[2]) * cos(orient[0]) * cos(orient[1]) - cos(orient[2]) * sin(orient[0]) * sin(orient[1]); + float theta = halfRad * orient[0]; + float phi = halfRad * orient[1]; + float psi = halfRad * orient[2]; + q[0] = cos(phi) * cos(theta) * cos(psi) + sin(phi) * sin(theta) * sin(psi); + q[1] = sin(phi) * cos(theta) * cos(psi) - cos(phi) * sin(theta) * sin(psi); + q[2] = cos(phi) * sin(theta) * cos(psi) + sin(phi) * cos(theta) * sin(psi); + q[3] = cos(phi) * cos(theta) * sin(psi) - sin(phi) * sin(theta) * cos(psi); // If the sim is un-paused, this will overwrite the pitch/roll/yaw // values set above. diff --git a/xpcPlugin/MessageHandlers.cpp b/xpcPlugin/MessageHandlers.cpp index d72b8a8..4709165 100644 --- a/xpcPlugin/MessageHandlers.cpp +++ b/xpcPlugin/MessageHandlers.cpp @@ -55,6 +55,8 @@ namespace XPC handlers.insert(std::make_pair("TEXT", MessageHandlers::HandleText)); handlers.insert(std::make_pair("WYPT", MessageHandlers::HandleWypt)); handlers.insert(std::make_pair("VIEW", MessageHandlers::HandleView)); + handlers.insert(std::make_pair("GETC", MessageHandlers::HandleGetC)); + handlers.insert(std::make_pair("GETP", MessageHandlers::HandleGetP)); // X-Plane data messages handlers.insert(std::make_pair("DSEL", MessageHandlers::HandleXPlaneData)); handlers.insert(std::make_pair("USEL", MessageHandlers::HandleXPlaneData)); @@ -425,6 +427,43 @@ namespace XPC } } + void MessageHandlers::HandleGetC(const Message& msg) + { + const unsigned char* buffer = msg.GetBuffer(); + std::size_t size = msg.GetSize(); + if (size != 6) + { + Log::FormatLine(LOG_ERROR, "GCTL", "Unexpected message length: %u", size); + return; + } + unsigned char aircraft = buffer[5]; + // TODO(jason-watkins): Get proper printf specifier for unsigned char + Log::FormatLine(LOG_TRACE, "GCTL", "Getting control information for aircraft %u", aircraft); + + float throttle[8]; + unsigned char response[31] = "CTRL"; + *((float*)(response + 5)) = DataManager::GetFloat(DREF_Elevator, aircraft); + *((float*)(response + 9)) = DataManager::GetFloat(DREF_Aileron, aircraft); + *((float*)(response + 13)) = DataManager::GetFloat(DREF_Rudder, aircraft); + DataManager::GetFloatArray(DREF_ThrottleSet, throttle, 8, aircraft); + *((float*)(response + 17)) = throttle[0]; + if (aircraft == 0) + { + response[21] = (char)DataManager::GetInt(DREF_GearHandle, aircraft); + } + else + { + float mpGear[10]; + DataManager::GetFloatArray(DREF_GearDeploy, mpGear, 10, aircraft); + response[21] = mpGear[0] > 0.5 ? 1 : 0; + } + *((float*)(response + 22)) = DataManager::GetFloat(DREF_FlapSetting, aircraft); + response[26] = aircraft; + *((float*)(response + 27)) = DataManager::GetFloat(DREF_SpeedBrakeSet, aircraft); + + sock->SendTo(response, 31, &connection.addr); + } + void MessageHandlers::HandleGetD(const Message& msg) { const unsigned char* buffer = msg.GetBuffer(); @@ -471,6 +510,31 @@ namespace XPC sock->SendTo(response, cur, &connection.addr); } + void MessageHandlers::HandleGetP(const Message& msg) + { + const unsigned char* buffer = msg.GetBuffer(); + std::size_t size = msg.GetSize(); + if (size != 6) + { + Log::FormatLine(LOG_ERROR, "GPOS", "Unexpected message length: %u", size); + return; + } + unsigned char aircraft = buffer[5]; + Log::FormatLine(LOG_TRACE, "GPOS", "Getting position information for aircraft %u", aircraft); + + unsigned char response[34] = "POSI"; + response[5] = (char)DataManager::GetInt(DREF_GearHandle, aircraft); + *((float*)(response + 6)) = (float)DataManager::GetDouble(DREF_Latitude, aircraft); + *((float*)(response + 10)) = (float)DataManager::GetDouble(DREF_Longitude, aircraft); + *((float*)(response + 14)) = (float)DataManager::GetDouble(DREF_Elevation, aircraft); + *((float*)(response + 18)) = DataManager::GetFloat(DREF_Pitch, aircraft); + *((float*)(response + 22)) = DataManager::GetFloat(DREF_Roll, aircraft); + *((float*)(response + 26)) = DataManager::GetFloat(DREF_HeadingTrue, aircraft); + *((float*)(response + 30)) = (float)DataManager::GetInt(DREF_GearHandle, aircraft); + + sock->SendTo(response, 34, &connection.addr); + } + void MessageHandlers::HandlePosi(const Message& msg) { // Update log diff --git a/xpcPlugin/MessageHandlers.h b/xpcPlugin/MessageHandlers.h index 13742ee..d7f317d 100644 --- a/xpcPlugin/MessageHandlers.h +++ b/xpcPlugin/MessageHandlers.h @@ -43,7 +43,9 @@ namespace XPC static void HandleCtrl(const Message& msg); static void HandleData(const Message& msg); static void HandleDref(const Message& msg); + static void HandleGetC(const Message& msg); static void HandleGetD(const Message& msg); + static void HandleGetP(const Message& msg); static void HandlePosi(const Message& msg); static void HandleSimu(const Message& msg); static void HandleText(const Message& msg);