From f8852ba0b443fc2ff2cd250848bd5a30de4a1e20 Mon Sep 17 00:00:00 2001 From: jinchao <383321154@qq.com> Date: Tue, 29 Apr 2025 11:55:56 +0800 Subject: [PATCH] =?UTF-8?q?UDP=E6=B5=8B=E8=AF=95=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../XNATA04DataProcessor.cpp | 371 ++++++++++-------- XNServices/XNUDPService/XNUDPService.cpp | 42 +- .../XNUDPTestService/XNUDPTestService.cpp | 6 +- 3 files changed, 226 insertions(+), 193 deletions(-) diff --git a/XNModels/XNATA04DataProcessor/XNATA04DataProcessor.cpp b/XNModels/XNATA04DataProcessor/XNATA04DataProcessor.cpp index 0a46310..80902f8 100755 --- a/XNModels/XNATA04DataProcessor/XNATA04DataProcessor.cpp +++ b/XNModels/XNATA04DataProcessor/XNATA04DataProcessor.cpp @@ -3,6 +3,7 @@ #include #include #include +#include XN_DLL_INITIALIZE(XNATA04DataProcessor) XN_REGISTER_PARTICIPANT_BEGIN(XNATA04DataProcessor) @@ -75,12 +76,8 @@ void XNATA04DataProcessor::SendAeroOutput() QByteArray outputData; QDataStream outputStream(&outputData, QIODevice::WriteOnly); outputStream.setByteOrder(QDataStream::LittleEndian); - quint8 header[5] = {0x0a, 0x04, 0x00, 0x01, 0x00}; - outputStream << header[0]; //报头1，表示由系统发出 - outputStream << header[1]; //报头2，表示气动，0x04表示ATA04 - outputStream << header[2]; //报头3，表示模型，0x00表示气动 - outputStream << header[3]; //报头4，表示数据类型，0x01表示输出 - outputStream << header[4]; //报头5，表示数据长度 + quint8 header[6] = {0x0a, 0x04, 0x00, 0x01, 0x00, 0x00}; // 最后两个字节用于大小 + outputStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5]; //outputData.append(getQByteArray(d->_aeroOutput.l_04_o_aerocomac_fxb_f8())); if (d->_aeroOutput.l_04_o_aerocomac_fxb_f8()) { outputStream << d->_aeroOutput.l_04_o_aerocomac_fxb_f8().value(); @@ -148,7 +145,11 @@ void XNATA04DataProcessor::SendAeroOutput() } else { outputStream << (double)0; } - outputData[4] = outputData.size(); + // 更新数据包大小（使用两个字节） + auto size = outputData.size(); + outputData[4] = (size >> 8) & 0xFF; // 高字节 + outputData[5] = size & 0xFF; // 低字节 + TriggerRTEvent("SendUDPData", QVariant::fromValue(outputData)); } @@ -159,12 +160,8 @@ void XNATA04DataProcessor::SendGhOutput() QByteArray outputData; QDataStream outputStream(&outputData, QIODevice::WriteOnly); outputStream.setByteOrder(QDataStream::LittleEndian); - quint8 header[5] = {0x0a, 0x04, 0x01, 0x01, 0x00}; - outputStream << header[0]; //报头1，表示由系统发出 - outputStream << header[1]; //报头2，表示气动，0x04表示ATA04 - outputStream << header[2]; //报头3，表示模型，0x01表示地操模型 - outputStream << header[3]; //报头4，表示数据类型，0x01表示输出 - outputStream << header[4]; //报头5，表示数据长度 + quint8 header[6] = {0x0a, 0x04, 0x01, 0x01, 0x00, 0x00}; // 最后两个字节用于大小 + outputStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5]; if (d->_ghOutput.l_04_o_gdcomac_frz_l1()) { outputStream << d->_ghOutput.l_04_o_gdcomac_frz_l1().value(); } else { @@ -400,7 +397,11 @@ void XNATA04DataProcessor::SendGhOutput() } else { outputStream << (double)0 << (double)0 << (double)0 << (double)0 << (double)0 << (double)0; } - outputData[4] = outputData.size(); + // 更新数据包大小（使用两个字节） + auto size = outputData.size(); + outputData[4] = (size >> 8) & 0xFF; // 高字节 + outputData[5] = size & 0xFF; // 低字节 + TriggerRTEvent("SendUDPData", QVariant::fromValue(outputData)); } @@ -411,12 +412,8 @@ void XNATA04DataProcessor::SendWbOutput() QByteArray outputData; QDataStream outputStream(&outputData, QIODevice::WriteOnly); outputStream.setByteOrder(QDataStream::LittleEndian); - quint8 header[5] = {0x0a, 0x04, 0x02, 0x01, 0x00}; - outputStream << header[0]; //报头1，表示由系统发出 - outputStream << header[1]; //报头2，表示气动，0x04表示ATA04 - outputStream << header[2]; //报头3，表示模型，0x02表示质量模型 - outputStream << header[3]; //报头4，表示数据类型，0x01表示输出 - outputStream << header[4]; //报头5，表示数据长度 + quint8 header[6] = {0x0a, 0x04, 0x02, 0x01, 0x00, 0x00}; // 最后两个字节用于大小 + outputStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5]; if (d->_wbOutput.l_04_o_wbcomac_gw_f4()) { outputStream << d->_wbOutput.l_04_o_wbcomac_gw_f4().value(); } else { @@ -701,7 +698,11 @@ void XNATA04DataProcessor::SendWbOutput() } else { outputStream << (double)0; } - outputData[4] = outputData.size(); + // 更新数据包大小（使用两个字节） + auto size = outputData.size(); + outputData[4] = (size >> 8) & 0xFF; // 高字节 + outputData[5] = size & 0xFF; // 低字节 + TriggerRTEvent("SendUDPData", QVariant::fromValue(outputData)); } @@ -712,14 +713,14 @@ void XNATA04DataProcessor::SendAeroHeartbeat() QByteArray heartbeatData; QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly); heartbeatStream.setByteOrder(QDataStream::LittleEndian); - quint8 header[5] = {0x0a, 0x04, 0x00, 0x02, 0x00}; - heartbeatStream << header[0]; //报头1，表示由系统发出 - heartbeatStream << header[1]; //报头2，表示气动，0x04表示ATA04 - heartbeatStream << header[2]; //报头3，表示模型，0x00表示气动 - heartbeatStream << header[3]; //报头4，表示数据类型，0x02表示心跳 - heartbeatStream << header[4]; //报头5，表示数据长度 + quint8 header[6] = {0x0a, 0x04, 0x00, 0x02, 0x00, 0x00}; // 最后两个字节用于大小 + heartbeatStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5]; heartbeatStream << d->_aeroHeartbeat.aero_model_heartbeat(); - heartbeatData[4] = heartbeatData.size(); + // 更新数据包大小（使用两个字节） + auto size = heartbeatData.size(); + heartbeatData[4] = (size >> 8) & 0xFF; // 高字节 + heartbeatData[5] = size & 0xFF; // 低字节 + TriggerRTEvent("SendUDPData", QVariant::fromValue(heartbeatData)); } @@ -730,14 +731,14 @@ void XNATA04DataProcessor::SendGhHeartbeat() QByteArray heartbeatData; QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly); heartbeatStream.setByteOrder(QDataStream::LittleEndian); - quint8 header[5] = {0x0a, 0x04, 0x01, 0x02, 0x00}; - heartbeatStream << header[0]; //报头1，表示由系统发出 - heartbeatStream << header[1]; //报头2，表示气动，0x04表示ATA04 - heartbeatStream << header[2]; //报头3，表示模型，0x01表示地操 - heartbeatStream << header[3]; //报头4，表示数据类型，0x02表示心跳 - heartbeatStream << header[4]; //报头5，表示数据长度 + quint8 header[6] = {0x0a, 0x04, 0x01, 0x02, 0x00, 0x00}; // 最后两个字节用于大小 + heartbeatStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5]; heartbeatStream << d->_ghHeartbeat.groundhandling_model_heartbeat(); - heartbeatData[4] = heartbeatData.size(); + // 更新数据包大小（使用两个字节） + auto size = heartbeatData.size(); + heartbeatData[4] = (size >> 8) & 0xFF; // 高字节 + heartbeatData[5] = size & 0xFF; // 低字节 + TriggerRTEvent("SendUDPData", QVariant::fromValue(heartbeatData)); } @@ -748,14 +749,14 @@ void XNATA04DataProcessor::SendWbHeartbeat() QByteArray heartbeatData; QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly); heartbeatStream.setByteOrder(QDataStream::LittleEndian); - quint8 header[5] = {0x0a, 0x04, 0x02, 0x02, 0x00}; - heartbeatStream << header[0]; //报头1，表示由系统发出 - heartbeatStream << header[1]; //报头2，表示气动，0x04表示ATA04 - heartbeatStream << header[2]; //报头3，表示模型，0x02表示质量 - heartbeatStream << header[3]; //报头4，表示数据类型，0x02表示心跳 - heartbeatStream << header[4]; //报头5，表示数据长度 + quint8 header[6] = {0x0a, 0x04, 0x02, 0x02, 0x00, 0x00}; // 最后两个字节用于大小 + heartbeatStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5]; heartbeatStream << d->_wbHeartbeat.weightbody_model_heartbeat(); - heartbeatData[4] = heartbeatData.size(); + // 更新数据包大小（使用两个字节） + auto size = heartbeatData.size(); + heartbeatData[4] = (size >> 8) & 0xFF; // 高字节 + heartbeatData[5] = size & 0xFF; // 低字节 + TriggerRTEvent("SendUDPData", QVariant::fromValue(heartbeatData)); } @@ -763,17 +764,33 @@ void XNATA04DataProcessor::OnWbInput(const QVariant &data) { Q_D(XNATA04DataProcessor); QByteArray inputData = data.toByteArray(); - if (inputData.size() <= 5 || inputData[0] != 0x0b || inputData[1] != 0x04 - || inputData[2] != 0x02 || inputData[3] != 0x00 || inputData[4] != inputData.size()) { - LOG_WARNING("ATA04DataProcessor::OnWbInput: invalid input data"); + + // 检查数据包头 + if (inputData[0] != 0x0b || inputData[1] != 0x04 || inputData[2] != 0x02 + || inputData[3] != 0x00) { + LOG_WARNING("ATA04DataProcessor::OnWbInput: invalid input data header"); return; } + + // 读取两个字节的大小字段 + quint16 size = ((quint16)(quint8)inputData[4] << 8) | (quint16)(quint8)inputData[5]; + if (size != inputData.size()) { + LOG_WARNING( + "ATA04DataProcessor::OnWbInput: invalid data size. Expected %d bytes, got %d bytes", + size, inputData.size()); + return; + } + QDataStream inputStream(&inputData, QIODevice::ReadOnly); inputStream.setByteOrder(QDataStream::LittleEndian); - for (int i = 0; i < 5; i++) { + inputStream.setVersion(QDataStream::Qt_6_0); + + // 跳过6字节的头 + for (int i = 0; i < 6; i++) { char tmp; inputStream >> tmp; } + QMutexLocker locker(&d->_wbInputMutex); double dTmp; inputStream >> dTmp; @@ -841,17 +858,33 @@ void XNATA04DataProcessor::OnGhInput(const QVariant &data) { Q_D(XNATA04DataProcessor); QByteArray inputData = data.toByteArray(); - if (inputData.size() <= 5 || inputData[0] != 0x0b || inputData[1] != 0x04 - || inputData[2] != 0x01 || inputData[3] != 0x00 || inputData[4] != inputData.size()) { - LOG_WARNING("ATA04DataProcessor::OnGhInput: invalid input data"); + + // 检查数据包头 + if (inputData[0] != 0x0b || inputData[1] != 0x04 || inputData[2] != 0x01 + || inputData[3] != 0x00) { + LOG_WARNING("ATA04DataProcessor::OnGhInput: invalid input data header"); return; } + + // 读取两个字节的大小字段 + quint16 size = ((quint16)(quint8)inputData[4] << 8) | (quint16)(quint8)inputData[5]; + if (size != inputData.size()) { + LOG_WARNING( + "ATA04DataProcessor::OnGhInput: invalid data size. Expected %d bytes, got %d bytes", + size, inputData.size()); + return; + } + QDataStream inputStream(&inputData, QIODevice::ReadOnly); inputStream.setByteOrder(QDataStream::LittleEndian); - for (int i = 0; i < 5; i++) { + inputStream.setVersion(QDataStream::Qt_6_0); + + // 跳过6字节的头 + for (int i = 0; i < 6; i++) { char tmp; inputStream >> tmp; } + QMutexLocker locker(&d->_ghInputMutex); quint8 uchartmp; inputStream >> uchartmp; @@ -983,19 +1016,19 @@ void XNATA04DataProcessor::OnAeroInput(const QVariant &data) Q_D(XNATA04DataProcessor); QByteArray inputData = data.toByteArray(); - // 基本验证：长度和包头前四个字节 - if (inputData.size() <= 5) { - LOG_WARNING("ATA04DataProcessor::OnAeroInput: data too short, size=%d", inputData.size()); + // 现在我们知道数据包确实以0b 04 00开头 + if (inputData[0] != 0x0b || inputData[1] != 0x04 || inputData[2] != 0x00 + || inputData[3] != 0x00) { + LOG_WARNING("ATA04DataProcessor::OnAeroInput: invalid input data"); return; } - // 现在我们知道数据包确实以0b 04 00 00开头 - if (inputData[0] != 0x0b || inputData[1] != 0x04 || inputData[2] != 0x00 - || inputData[3] != 0x00) { - LOG_WARNING("ATA04DataProcessor::OnAeroInput: invalid header, expected 0b 04 00 00, got " - "%02x %02x %02x %02x", - (quint8)inputData[0], (quint8)inputData[1], (quint8)inputData[2], - (quint8)inputData[3]); + // 读取两个字节的大小字段 + quint16 size = ((quint16)(quint8)inputData[4] << 8) | (quint16)(quint8)inputData[5]; + if (size != inputData.size()) { + LOG_WARNING( + "ATA04DataProcessor::OnAeroInput: invalid data size. Expected %1 bytes, got %2 bytes", + size, inputData.size()); return; } @@ -1004,8 +1037,8 @@ void XNATA04DataProcessor::OnAeroInput(const QVariant &data) inputStream.setByteOrder(QDataStream::LittleEndian); inputStream.setVersion(QDataStream::Qt_6_0); - // 跳过5字节的头 - for (int i = 0; i < 5; i++) { + // 跳过6字节的头 + for (int i = 0; i < 6; i++) { char tmp; inputStream >> tmp; } @@ -1049,49 +1082,73 @@ void XNATA04DataProcessor::OnAeroInput(const QVariant &data) d->_aeroInput.l_04_i_aerocomac_wlcg_f8(tmp); inputStream >> tmp; d->_aeroInput.l_04_i_aerocomac_stab_f8(tmp); - std::array tmpArray; + + // ail_f8 (10个double) + std::array ailArray; for (int i = 0; i < 10; i++) { - inputStream >> tmpArray[i]; + inputStream >> ailArray[i]; } - d->_aeroInput.l_04_i_aerocomac_ail_f8(tmpArray); - std::array tmpArray2; + d->_aeroInput.l_04_i_aerocomac_ail_f8(ailArray); + + // elv_f8 (4个double) + std::array elvArray; for (int i = 0; i < 4; i++) { - inputStream >> tmpArray2[i]; + inputStream >> elvArray[i]; } - d->_aeroInput.l_04_i_aerocomac_elv_f8(tmpArray2); - std::array tmpArray3; + d->_aeroInput.l_04_i_aerocomac_elv_f8(elvArray); + + // rud_f8 (2个double) + std::array rudArray; for (int i = 0; i < 2; i++) { - inputStream >> tmpArray3[i]; + inputStream >> rudArray[i]; } - d->_aeroInput.l_04_i_aerocomac_rud_f8(tmpArray3); - std::array tmpArray4; + d->_aeroInput.l_04_i_aerocomac_rud_f8(rudArray); + + // gear_f8 (7个double) + std::array gearArray; for (int i = 0; i < 7; i++) { - inputStream >> tmpArray4[i]; + inputStream >> gearArray[i]; } - d->_aeroInput.l_04_i_aerocomac_gear_f8(tmpArray4); + d->_aeroInput.l_04_i_aerocomac_gear_f8(gearArray); + + // flap_f8 (10个double) + std::array flapArray; for (int i = 0; i < 10; i++) { - inputStream >> tmpArray[i]; + inputStream >> flapArray[i]; } - d->_aeroInput.l_04_i_aerocomac_flap_f8(tmpArray); - std::array tmpArray5; + d->_aeroInput.l_04_i_aerocomac_flap_f8(flapArray); + + // slat_f8 (20个double) + std::array slatArray; for (int i = 0; i < 20; i++) { - inputStream >> tmpArray5[i]; + inputStream >> slatArray[i]; } - d->_aeroInput.l_04_i_aerocomac_slat_f8(tmpArray5); + d->_aeroInput.l_04_i_aerocomac_slat_f8(slatArray); + + // spl_f8 (20个double) + std::array splArray; for (int i = 0; i < 20; i++) { - inputStream >> tmpArray5[i]; + inputStream >> splArray[i]; } - d->_aeroInput.l_04_i_aerocomac_spl_f8(tmpArray5); + d->_aeroInput.l_04_i_aerocomac_spl_f8(splArray); + + // tnet_f8 (4个double) + std::array tnetArray; for (int i = 0; i < 4; i++) { - inputStream >> tmpArray2[i]; + inputStream >> tnetArray[i]; } - d->_aeroInput.l_04_i_aerocomac_tnet_f8(tmpArray2); + d->_aeroInput.l_04_i_aerocomac_tnet_f8(tnetArray); + + // kice_f8 (20个double) + std::array kiceArray; for (int i = 0; i < 20; i++) { - inputStream >> tmpArray5[i]; + inputStream >> kiceArray[i]; } - d->_aeroInput.l_04_i_aerocomac_kice_f8(tmpArray5); + d->_aeroInput.l_04_i_aerocomac_kice_f8(kiceArray); + inputStream >> tmp; d->_aeroInput.l_04_i_aerocomac_alt_agl_f8(tmp); + d->_dataWriters["XNSim::ATA04::Aerodynamics_input"]->write(&d->_aeroInput); } catch (const std::exception &e) { LOG_WARNING("ATA04DataProcessor::OnAeroInput: exception during data parsing: %s", e.what()); @@ -1155,27 +1212,28 @@ void XNATA04DataProcessor::SendUdpTestData() aeroStream.setVersion(QDataStream::Qt_6_0); // 气动输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x00=气动模型, 0x00=输入数据) - quint8 aeroHeader[5] = {0x0b, 0x04, 0x00, 0x00, 0x00}; - aeroStream << aeroHeader[0] << aeroHeader[1] << aeroHeader[2] << aeroHeader[3] << aeroHeader[4]; + quint8 aeroHeader[6] = {0x0b, 0x04, 0x00, 0x00, 0x00, 0x00}; // 最后两个字节用于大小 + aeroStream << aeroHeader[0] << aeroHeader[1] << aeroHeader[2] << aeroHeader[3] << aeroHeader[4] + << aeroHeader[5]; // 气动数据字段 - aeroStream << (double)5.0; // alpha - aeroStream << (double)0.5; // alpdot - aeroStream << (double)1.0; // beta + aeroStream << (double)-2.0; // alpha + aeroStream << (double)0.0; // alpdot + aeroStream << (double)0.0; // beta aeroStream << (double)10000.0; // press_alt - aeroStream << (double)250.0; // tas - aeroStream << (double)0.8; // mach - aeroStream << (double)0.2; // nx + aeroStream << (double)360.0; // tas + aeroStream << (double)0.3; // mach + aeroStream << (double)0.0; // nx aeroStream << (double)0.0; // ny aeroStream << (double)1.0; // nz aeroStream << (double)0.0; // p aeroStream << (double)0.0; // q aeroStream << (double)0.0; // r - aeroStream << (double)12000.0; // qbar + aeroStream << (double)110.0; // qbar aeroStream << (double)0.0; // blcg - aeroStream << (double)0.0; // bscg - aeroStream << (double)0.0; // wlcg - aeroStream << (double)0.0; // stab + aeroStream << (double)650.0; // bscg + aeroStream << (double)7.0; // wlcg + aeroStream << (double)-2.0; // stab // ail_f8 数组，需要10个double for (int i = 0; i < 10; i++) { @@ -1194,17 +1252,17 @@ void XNATA04DataProcessor::SendUdpTestData() // gear_f8 数组，需要7个double for (int i = 0; i < 7; i++) { - aeroStream << (double)0.0; + aeroStream << (double)1.0; } // flap_f8 数组，需要10个double for (int i = 0; i < 10; i++) { - aeroStream << (double)0.0; + aeroStream << (double)41.5; } // slat_f8 数组，需要20个double for (int i = 0; i < 20; i++) { - aeroStream << (double)0.0; + aeroStream << (double)20.855; } // spl_f8 数组，需要20个double @@ -1214,7 +1272,7 @@ void XNATA04DataProcessor::SendUdpTestData() // tnet_f8 数组，需要4个double for (int i = 0; i < 4; i++) { - aeroStream << (double)20000.0; + aeroStream << (double)8000.0; } // kice_f8 数组，需要20个double @@ -1223,40 +1281,38 @@ void XNATA04DataProcessor::SendUdpTestData() } // alt_agl - aeroStream << (double)1000.0; + aeroStream << (double)2500.0; - // 更新数据包大小，确保不会溢出 - if (aeroData.size() <= 255) { - aeroData[4] = aeroData.size(); - } else { - LOG_WARNING("ATA04DataProcessor::SendUdpTestData: aeroData size exceeds byte limit: %d", - aeroData.size()); - aeroData[4] = 255; // 设置为最大值 - } + // 更新数据包大小（使用两个字节） + quint16 size = aeroData.size(); + aeroData[4] = (size >> 8) & 0xFF; // 高字节 + aeroData[5] = size & 0xFF; // 低字节 // 创建地面操作输入(GhInput)数据包 QByteArray ghData; QDataStream ghStream(&ghData, QIODevice::WriteOnly); ghStream.setByteOrder(QDataStream::LittleEndian); ghStream.setVersion(QDataStream::Qt_6_0); + // 地面操作输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x01=地面操作模型, 0x00=输入数据) - quint8 ghHeader[5] = {0x0b, 0x04, 0x01, 0x00, 0x00}; - ghStream << ghHeader[0] << ghHeader[1] << ghHeader[2] << ghHeader[3] << ghHeader[4]; + quint8 ghHeader[6] = {0x0b, 0x04, 0x01, 0x00, 0x00, 0x00}; // 最后两个字节用于大小 + ghStream << ghHeader[0] << ghHeader[1] << ghHeader[2] << ghHeader[3] << ghHeader[4] + << ghHeader[5]; // 地面操作数据字段 ghStream << (quint8)0; // frz_l1 ghStream << (quint8)0; // chocks_l1 - ghStream << (double)100.0; // alt_agl_f8 + ghStream << (double)8.0; // alt_agl_f8 ghStream << (quint8)0; // frzflt_l1 ghStream << (double)0.0; // p_f8 ghStream << (double)0.0; // q_f8 ghStream << (double)0.0; // r_f8 - ghStream << (double)0.0; // ug_f8 + ghStream << (double)50.0; // ug_f8 ghStream << (double)0.0; // vg_f8 ghStream << (double)0.0; // wg_f8 ghStream << (double)0.0; // blcg_f8 - ghStream << (double)0.0; // bscg_f8 - ghStream << (double)0.0; // wlcg_f8 + ghStream << (double)649.3; // bscg_f8 + ghStream << (double)10.0; // wlcg_f8 ghStream << (quint8)0; // pb_active_l1 ghStream << (double)0.0; // pb_towforce_f8 @@ -1269,12 +1325,7 @@ void XNATA04DataProcessor::SendUdpTestData() // gear_f8 数组，需要3个double for (int i = 0; i < 3; i++) { - ghStream << (double)0.0; - } - - // 另一个3个double的数组 - for (int i = 0; i < 3; i++) { - ghStream << (double)0.0; + ghStream << (double)1.0; } // gsteer_f8 数组，需要10个double @@ -1285,7 +1336,11 @@ void XNATA04DataProcessor::SendUdpTestData() // tire_pres_f8 数组 3x2 for (int i = 0; i < 3; i++) { for (int j = 0; j < 2; j++) { - ghStream << (double)30.0; // 30 psi的胎压 + if (i == 0) { + ghStream << (double)98.0; // 98 kpa的胎压 + } else { + ghStream << (double)140.0; // 140 kpa的胎压 + } } } @@ -1297,7 +1352,8 @@ void XNATA04DataProcessor::SendUdpTestData() ghStream << (quint8)0; // onjax_l1 // contdep_f8 数组，需要7个double - for (int i = 0; i < 7; i++) { + ghStream << (double)1.0; + for (int i = 1; i < 7; i++) { ghStream << (double)0.0; } @@ -1307,12 +1363,12 @@ void XNATA04DataProcessor::SendUdpTestData() ghStream << (double)0.0; // rwyhdg_f8 ghStream << (quint8)0; // reset_braketemp_l1 ghStream << (quint8)0; // reset_tirepress_l1 - ghStream << (double)20.0; // temp_c_f8 + ghStream << (double)16.0; // temp_c_f8 // brake_temp_f8 数组 3x2 for (int i = 0; i < 3; i++) { for (int j = 0; j < 2; j++) { - ghStream << (double)50.0; // 50度的刹车温度 + ghStream << (double)16.0; // 16度的刹车温度 } } @@ -1333,51 +1389,50 @@ void XNATA04DataProcessor::SendUdpTestData() ghStream << (quint8)0; // brk_reset_tpres_l1 // rcon_ci_f8 数组，需要14个double - for (int i = 0; i < 14; i++) { + ghStream << (double)1.0; + for (int i = 1; i < 14; i++) { ghStream << (double)0.0; } - ghStream << (qint32)0; // gsteer_state_i4 - ghStream << (quint8)0; // trim_active_l1 + ghStream << (qint32)2; // gsteer_state_i4 + ghStream << (quint8)1; // trim_active_l1 ghStream << (double)0.0; // phi_deg_f8 ghStream << (double)0.0; // theta_deg_f8 ghStream << (double)0.0; // psi_deg_f8 - ghStream << (quint8)0; // resetint_l1 + ghStream << (quint8)1; // resetint_l1 - // 更新数据包大小，确保不会溢出 - if (ghData.size() <= 255) { - ghData[4] = ghData.size(); - } else { - LOG_WARNING("ATA04DataProcessor::SendUdpTestData: ghData size exceeds byte limit: %d", - ghData.size()); - ghData[4] = 255; // 设置为最大值 - } + // 更新数据包大小（使用两个字节） + size = ghData.size(); + ghData[4] = (size >> 8) & 0xFF; // 高字节 + ghData[5] = size & 0xFF; // 低字节 // 创建重量平衡输入(WbInput)数据包 QByteArray wbData; QDataStream wbStream(&wbData, QIODevice::WriteOnly); wbStream.setByteOrder(QDataStream::LittleEndian); wbStream.setVersion(QDataStream::Qt_6_0); + // 重量平衡输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x02=重量平衡模型, 0x00=输入数据) - quint8 wbHeader[5] = {0x0b, 0x04, 0x02, 0x00, 0x00}; - wbStream << wbHeader[0] << wbHeader[1] << wbHeader[2] << wbHeader[3] << wbHeader[4]; + quint8 wbHeader[6] = {0x0b, 0x04, 0x02, 0x00, 0x00, 0x00}; // 最后两个字节用于大小 + wbStream << wbHeader[0] << wbHeader[1] << wbHeader[2] << wbHeader[3] << wbHeader[4] + << wbHeader[5]; // 重量平衡数据字段 wbStream << (double)0.0; // theta_deg_f8 wbStream << (double)0.0; // phi_deg_f8 wbStream << (double)0.0; // psi_deg_f8 wbStream << (quint8)0; // gear_mode_l1 - wbStream << (double)60000.0; // acset_gw_f8 (60000kg飞机重量) - wbStream << (double)25.0; // acset_cg_f8 (25%MAC重心位置) + wbStream << (double)82574.0; // acset_gw_f8 (60000kg飞机重量) + wbStream << (double)0.1215; // acset_cg_f8 (25%MAC重心位置) // acset_tankfuel_f4 数组，需要20个float for (int i = 0; i < 20; i++) { - wbStream << (float)(i < 10 ? 1000.0 : 0.0); // 前10个油箱各1000kg + wbStream << (double)(i < 2 ? 4541.0 : 0.0); // 前2个油箱各4541kg } - wbStream << (double)10000.0; // acset_totfuel_f8 (总油量10000kg) - wbStream << (double)50000.0; // acset_zfw_f8 (零油重50000kg) - wbStream << (double)24.0; // acset_zfwcg_f8 (零油重重心24%MAC) + wbStream << (double)9082.0; // acset_totfuel_f8 (总油量9082kg) + wbStream << (double)73492.0; // acset_zfw_f8 (零油重73492kg) + wbStream << (double)0.12; // acset_zfwcg_f8 (零油重重心24%MAC) // eng_efsep_l1 数组，需要4个char for (int i = 0; i < 4; i++) { @@ -1386,34 +1441,30 @@ void XNATA04DataProcessor::SendUdpTestData() // fuel_f8 数组，需要20个double for (int i = 0; i < 20; i++) { - wbStream << (double)(i < 10 ? 1000.0 : 0.0); // 前10个油箱各1000kg + wbStream << (double)(i < 2 ? 4541.0 : 0.0); // 前2个油箱各4541kg } - wbStream << (double)0.0; // gear_avg_f8 + wbStream << (double)1.0; // gear_avg_f8 // kice_f8 数组，需要20个double for (int i = 0; i < 20; i++) { wbStream << (double)0.0; } - wbStream << (quint8)0; // bycglim_l1 - wbStream << (quint8)0; // bygwlim_l1 + wbStream << (quint8)1; // bycglim_l1 + wbStream << (quint8)1; // bygwlim_l1 wbStream << (quint8)0; // frz_l1 wbStream << (quint8)0; // zcgfrz_l1 wbStream << (quint8)0; // zcgfrz_grfx_l1 wbStream << (quint8)0; // ycgfrz_l1 wbStream << (quint8)0; // inertfrz_l1 - wbStream << (double)60000.0; // potreq_gw_f8 - wbStream << (double)25.0; // potreq_gwcg_f8 + wbStream << (double)80000.0; // potreq_gw_f8 + wbStream << (double)0.2; // potreq_gwcg_f8 - // 更新数据包大小，确保不会溢出 - if (wbData.size() <= 255) { - wbData[4] = wbData.size(); - } else { - LOG_WARNING("ATA04DataProcessor::SendUdpTestData: wbData size exceeds byte limit: %d", - wbData.size()); - wbData[4] = 255; // 设置为最大值 - } + // 更新数据包大小（使用两个字节） + size = wbData.size(); + wbData[4] = (size >> 8) & 0xFF; // 高字节 + wbData[5] = size & 0xFF; // 低字节 // 发送测试数据包 TriggerRTEvent("SendTestUDPData", QVariant::fromValue(aeroData)); diff --git a/XNServices/XNUDPService/XNUDPService.cpp b/XNServices/XNUDPService/XNUDPService.cpp index 628ea0a..11d2003 100755 --- a/XNServices/XNUDPService/XNUDPService.cpp +++ b/XNServices/XNUDPService/XNUDPService.cpp @@ -89,56 +89,42 @@ void XNUDPService::HandleIncomingData() d->udpSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); - // 数据包头在第9个位置，直接跳过前9个字节 - if (datagram.size() <= 9) { + if (datagram.size() <= 6) { LOG_WARNING("Invalid size of UDP datagram received, size:%1", datagram.size()); continue; } - // 创建一个新的数据包，去除前面的额外头信息 - QByteArray actualData = datagram.mid(9); - // 使用处理后的数据 - quint8 header = (quint8)actualData[0]; - quint8 type = (quint8)actualData[1]; - quint8 subType1 = (quint8)actualData[2]; - quint8 subType2 = (quint8)actualData[3]; - quint8 size = (quint8)actualData[4]; - - // 处理接收到的数据 - if (actualData.size() <= 5) { - LOG_WARNING("Invalid size of UDP datagram received, size:%1", actualData.size()); - continue; - } - + quint8 header = (quint8)datagram[0]; + quint8 type = (quint8)datagram[1]; + quint8 subType1 = (quint8)datagram[2]; + quint8 subType2 = (quint8)datagram[3]; + quint16 size = (quint8)datagram[4] << 8 | (quint8)datagram[5]; if (header != 0x0b) { LOG_WARNING("Invalid header of UDP datagram received, header:%1", header); continue; } - //如果数据包大小大于size,则截取size大小的数据 - if (actualData.size() > size) { - actualData = actualData.mid(0, size); - } else if (actualData.size() < size) { + if (datagram.size() < size) { LOG_WARNING("Invalid size of UDP datagram received, size:%1, expected:%2", - actualData.size(), size); + datagram.size(), size); continue; } if (type == 0x04) { if (subType1 == 0x00 && subType2 == 0x00) { - TriggerRTEvent("ATA04AeroInput", actualData); + TriggerRTEvent("ATA04AeroInput", datagram); continue; } else if (subType1 == 0x01 && subType2 == 0x00) { - TriggerRTEvent("ATA04GhInput", actualData); + TriggerRTEvent("ATA04GhInput", datagram); continue; } else if (subType1 == 0x02 && subType2 == 0x00) { - TriggerRTEvent("ATA04WbInput", actualData); + TriggerRTEvent("ATA04WbInput", datagram); continue; } } // TODO: 根据具体需求处理其它数据 - TriggerRTEvent("ReceiveUDPData", actualData); + TriggerRTEvent("ReceiveUDPData", datagram); } } @@ -151,9 +137,7 @@ void XNUDPService::SendData(const QVariant &data) } // 将QVariant转换为字节数组 - QByteArray datagram; - QDataStream stream(&datagram, QIODevice::WriteOnly); - stream << data; + QByteArray datagram = data.toByteArray(); // 发送数据 qint64 bytesSent = diff --git a/XNServices/XNUDPTestService/XNUDPTestService.cpp b/XNServices/XNUDPTestService/XNUDPTestService.cpp index 8bad458..72a533a 100755 --- a/XNServices/XNUDPTestService/XNUDPTestService.cpp +++ b/XNServices/XNUDPTestService/XNUDPTestService.cpp @@ -73,7 +73,7 @@ void XNUDPTestService::OnPrepareForExecute() } // 连接UDP socket的信号 - connect(d->udpSocket, &QUdpSocket::readyRead, this, &XNUDPTestService::HandleIncomingData); + //connect(d->udpSocket, &QUdpSocket::readyRead, this, &XNUDPTestService::HandleIncomingData); RegisterRTEventHandler("SendTestUDPData", std::bind(&XNUDPTestService::SendData, this, std::placeholders::_1)); @@ -122,9 +122,7 @@ void XNUDPTestService::SendData(const QVariant &data) } // 将QVariant转换为字节数组 - QByteArray datagram; - QDataStream stream(&datagram, QIODevice::WriteOnly); - stream << data; + QByteArray datagram = data.toByteArray(); // 发送数据 qint64 bytesSent =