jinchao/XNSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

UDP测试完成

Browse Source
This commit is contained in:
jinchao 2025-04-29 11:55:56 +08:00
parent 49f139f508
commit f8852ba0b4
3 changed files with 226 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

371
XNModels/XNATA04DataProcessor/XNATA04DataProcessor.cpp
View File

@ -3,6 +3,7 @@
#include <XNCore/XNModelManager.h> #include <XNCore/XNModelManager.h>
#include <XNCore/XNDDSManager.h> #include <XNCore/XNDDSManager.h>
#include <QMutexLocker> #include <QMutexLocker>
#include <iostream>
XN_DLL_INITIALIZE(XNATA04DataProcessor) XN_DLL_INITIALIZE(XNATA04DataProcessor)
XN_REGISTER_PARTICIPANT_BEGIN(XNATA04DataProcessor) XN_REGISTER_PARTICIPANT_BEGIN(XNATA04DataProcessor)
@ -75,12 +76,8 @@ void XNATA04DataProcessor::SendAeroOutput()
QByteArray outputData; QByteArray outputData;
QDataStream outputStream(&outputData, QIODevice::WriteOnly); QDataStream outputStream(&outputData, QIODevice::WriteOnly);
outputStream.setByteOrder(QDataStream::LittleEndian); outputStream.setByteOrder(QDataStream::LittleEndian);
quint8 header[5] = {0x0a, 0x04, 0x00, 0x01, 0x00}; quint8 header[6] = {0x0a, 0x04, 0x00, 0x01, 0x00, 0x00}; // 最后两个字节用于大小
outputStream << header[0]; //报头1，表示由系统发出 outputStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5];
outputStream << header[1]; //报头2，表示气动，0x04表示ATA04
outputStream << header[2]; //报头3，表示模型，0x00表示气动
outputStream << header[3]; //报头4，表示数据类型，0x01表示输出
outputStream << header[4]; //报头5，表示数据长度
//outputData.append(getQByteArray(d->_aeroOutput.l_04_o_aerocomac_fxb_f8())); //outputData.append(getQByteArray(d->_aeroOutput.l_04_o_aerocomac_fxb_f8()));
if (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(); outputStream << d->_aeroOutput.l_04_o_aerocomac_fxb_f8().value();
@ -148,7 +145,11 @@ void XNATA04DataProcessor::SendAeroOutput()
} else { } else {
outputStream << (double)0; 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)); TriggerRTEvent("SendUDPData", QVariant::fromValue(outputData));
} }
@ -159,12 +160,8 @@ void XNATA04DataProcessor::SendGhOutput()
QByteArray outputData; QByteArray outputData;
QDataStream outputStream(&outputData, QIODevice::WriteOnly); QDataStream outputStream(&outputData, QIODevice::WriteOnly);
outputStream.setByteOrder(QDataStream::LittleEndian); outputStream.setByteOrder(QDataStream::LittleEndian);
quint8 header[5] = {0x0a, 0x04, 0x01, 0x01, 0x00}; quint8 header[6] = {0x0a, 0x04, 0x01, 0x01, 0x00, 0x00}; // 最后两个字节用于大小
outputStream << header[0]; //报头1，表示由系统发出 outputStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5];
outputStream << header[1]; //报头2，表示气动，0x04表示ATA04
outputStream << header[2]; //报头3，表示模型，0x01表示地操模型
outputStream << header[3]; //报头4，表示数据类型，0x01表示输出
outputStream << header[4]; //报头5，表示数据长度
if (d->_ghOutput.l_04_o_gdcomac_frz_l1()) { if (d->_ghOutput.l_04_o_gdcomac_frz_l1()) {
outputStream << d->_ghOutput.l_04_o_gdcomac_frz_l1().value(); outputStream << d->_ghOutput.l_04_o_gdcomac_frz_l1().value();
} else { } else {
@ -400,7 +397,11 @@ void XNATA04DataProcessor::SendGhOutput()
} else { } else {
outputStream << (double)0 << (double)0 << (double)0 << (double)0 << (double)0 << (double)0; 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)); TriggerRTEvent("SendUDPData", QVariant::fromValue(outputData));
} }
@ -411,12 +412,8 @@ void XNATA04DataProcessor::SendWbOutput()
QByteArray outputData; QByteArray outputData;
QDataStream outputStream(&outputData, QIODevice::WriteOnly); QDataStream outputStream(&outputData, QIODevice::WriteOnly);
outputStream.setByteOrder(QDataStream::LittleEndian); outputStream.setByteOrder(QDataStream::LittleEndian);
quint8 header[5] = {0x0a, 0x04, 0x02, 0x01, 0x00}; quint8 header[6] = {0x0a, 0x04, 0x02, 0x01, 0x00, 0x00}; // 最后两个字节用于大小
outputStream << header[0]; //报头1，表示由系统发出 outputStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5];
outputStream << header[1]; //报头2，表示气动，0x04表示ATA04
outputStream << header[2]; //报头3，表示模型，0x02表示质量模型
outputStream << header[3]; //报头4，表示数据类型，0x01表示输出
outputStream << header[4]; //报头5，表示数据长度
if (d->_wbOutput.l_04_o_wbcomac_gw_f4()) { if (d->_wbOutput.l_04_o_wbcomac_gw_f4()) {
outputStream << d->_wbOutput.l_04_o_wbcomac_gw_f4().value(); outputStream << d->_wbOutput.l_04_o_wbcomac_gw_f4().value();
} else { } else {
@ -701,7 +698,11 @@ void XNATA04DataProcessor::SendWbOutput()
} else { } else {
outputStream << (double)0; 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)); TriggerRTEvent("SendUDPData", QVariant::fromValue(outputData));
} }
@ -712,14 +713,14 @@ void XNATA04DataProcessor::SendAeroHeartbeat()
QByteArray heartbeatData; QByteArray heartbeatData;
QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly); QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly);
heartbeatStream.setByteOrder(QDataStream::LittleEndian); heartbeatStream.setByteOrder(QDataStream::LittleEndian);
quint8 header[5] = {0x0a, 0x04, 0x00, 0x02, 0x00}; quint8 header[6] = {0x0a, 0x04, 0x00, 0x02, 0x00, 0x00}; // 最后两个字节用于大小
heartbeatStream << header[0]; //报头1，表示由系统发出 heartbeatStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5];
heartbeatStream << header[1]; //报头2，表示气动，0x04表示ATA04
heartbeatStream << header[2]; //报头3，表示模型，0x00表示气动
heartbeatStream << header[3]; //报头4，表示数据类型，0x02表示心跳
heartbeatStream << header[4]; //报头5，表示数据长度
heartbeatStream << d->_aeroHeartbeat.aero_model_heartbeat(); 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)); TriggerRTEvent("SendUDPData", QVariant::fromValue(heartbeatData));
} }
@ -730,14 +731,14 @@ void XNATA04DataProcessor::SendGhHeartbeat()
QByteArray heartbeatData; QByteArray heartbeatData;
QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly); QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly);
heartbeatStream.setByteOrder(QDataStream::LittleEndian); heartbeatStream.setByteOrder(QDataStream::LittleEndian);
quint8 header[5] = {0x0a, 0x04, 0x01, 0x02, 0x00}; quint8 header[6] = {0x0a, 0x04, 0x01, 0x02, 0x00, 0x00}; // 最后两个字节用于大小
heartbeatStream << header[0]; //报头1，表示由系统发出 heartbeatStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5];
heartbeatStream << header[1]; //报头2，表示气动，0x04表示ATA04
heartbeatStream << header[2]; //报头3，表示模型，0x01表示地操
heartbeatStream << header[3]; //报头4，表示数据类型，0x02表示心跳
heartbeatStream << header[4]; //报头5，表示数据长度
heartbeatStream << d->_ghHeartbeat.groundhandling_model_heartbeat(); 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)); TriggerRTEvent("SendUDPData", QVariant::fromValue(heartbeatData));
} }
@ -748,14 +749,14 @@ void XNATA04DataProcessor::SendWbHeartbeat()
QByteArray heartbeatData; QByteArray heartbeatData;
QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly); QDataStream heartbeatStream(&heartbeatData, QIODevice::WriteOnly);
heartbeatStream.setByteOrder(QDataStream::LittleEndian); heartbeatStream.setByteOrder(QDataStream::LittleEndian);
quint8 header[5] = {0x0a, 0x04, 0x02, 0x02, 0x00}; quint8 header[6] = {0x0a, 0x04, 0x02, 0x02, 0x00, 0x00}; // 最后两个字节用于大小
heartbeatStream << header[0]; //报头1，表示由系统发出 heartbeatStream << header[0] << header[1] << header[2] << header[3] << header[4] << header[5];
heartbeatStream << header[1]; //报头2，表示气动，0x04表示ATA04
heartbeatStream << header[2]; //报头3，表示模型，0x02表示质量
heartbeatStream << header[3]; //报头4，表示数据类型，0x02表示心跳
heartbeatStream << header[4]; //报头5，表示数据长度
heartbeatStream << d->_wbHeartbeat.weightbody_model_heartbeat(); 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)); TriggerRTEvent("SendUDPData", QVariant::fromValue(heartbeatData));
} }
@ -763,17 +764,33 @@ void XNATA04DataProcessor::OnWbInput(const QVariant &data)
{ {
Q_D(XNATA04DataProcessor); Q_D(XNATA04DataProcessor);
QByteArray inputData = data.toByteArray(); 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; 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); QDataStream inputStream(&inputData, QIODevice::ReadOnly);
inputStream.setByteOrder(QDataStream::LittleEndian); 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; char tmp;
inputStream >> tmp; inputStream >> tmp;
} }
QMutexLocker locker(&d->_wbInputMutex); QMutexLocker locker(&d->_wbInputMutex);
double dTmp; double dTmp;
inputStream >> dTmp; inputStream >> dTmp;
@ -841,17 +858,33 @@ void XNATA04DataProcessor::OnGhInput(const QVariant &data)
{ {
Q_D(XNATA04DataProcessor); Q_D(XNATA04DataProcessor);
QByteArray inputData = data.toByteArray(); 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; 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); QDataStream inputStream(&inputData, QIODevice::ReadOnly);
inputStream.setByteOrder(QDataStream::LittleEndian); 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; char tmp;
inputStream >> tmp; inputStream >> tmp;
} }
QMutexLocker locker(&d->_ghInputMutex); QMutexLocker locker(&d->_ghInputMutex);
quint8 uchartmp; quint8 uchartmp;
inputStream >> uchartmp; inputStream >> uchartmp;
@ -983,19 +1016,19 @@ void XNATA04DataProcessor::OnAeroInput(const QVariant &data)
Q_D(XNATA04DataProcessor); Q_D(XNATA04DataProcessor);
QByteArray inputData = data.toByteArray(); QByteArray inputData = data.toByteArray();
// 基本验证：长度和包头前四个字节 // 现在我们知道数据包确实以0b 04 00开头
if (inputData.size() <= 5) { if (inputData[0] != 0x0b || inputData[1] != 0x04 || inputData[2] != 0x00
LOG_WARNING("ATA04DataProcessor::OnAeroInput: data too short, size=%d", inputData.size()); || inputData[3] != 0x00) {
LOG_WARNING("ATA04DataProcessor::OnAeroInput: invalid input data");
return; return;
} }
// 现在我们知道数据包确实以0b 04 00 00开头 // 读取两个字节的大小字段
if (inputData[0] != 0x0b || inputData[1] != 0x04 || inputData[2] != 0x00 quint16 size = ((quint16)(quint8)inputData[4] << 8) | (quint16)(quint8)inputData[5];
|| inputData[3] != 0x00) { if (size != inputData.size()) {
LOG_WARNING("ATA04DataProcessor::OnAeroInput: invalid header, expected 0b 04 00 00, got " LOG_WARNING(
"%02x %02x %02x %02x", "ATA04DataProcessor::OnAeroInput: invalid data size. Expected %1 bytes, got %2 bytes",
(quint8)inputData[0], (quint8)inputData[1], (quint8)inputData[2], size, inputData.size());
(quint8)inputData[3]);
return; return;
} }
@ -1004,8 +1037,8 @@ void XNATA04DataProcessor::OnAeroInput(const QVariant &data)
inputStream.setByteOrder(QDataStream::LittleEndian); inputStream.setByteOrder(QDataStream::LittleEndian);
inputStream.setVersion(QDataStream::Qt_6_0); inputStream.setVersion(QDataStream::Qt_6_0);
// 跳过5字节的头 // 跳过6字节的头
for (int i = 0; i < 5; i++) { for (int i = 0; i < 6; i++) {
char tmp; char tmp;
inputStream >> tmp; inputStream >> tmp;
} }
@ -1049,49 +1082,73 @@ void XNATA04DataProcessor::OnAeroInput(const QVariant &data)
d->_aeroInput.l_04_i_aerocomac_wlcg_f8(tmp); d->_aeroInput.l_04_i_aerocomac_wlcg_f8(tmp);
inputStream >> tmp; inputStream >> tmp;
d->_aeroInput.l_04_i_aerocomac_stab_f8(tmp); d->_aeroInput.l_04_i_aerocomac_stab_f8(tmp);
std::array<double, 10> tmpArray;
// ail_f8 (10个double)
std::array<double, 10> ailArray;
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
inputStream >> tmpArray[i]; inputStream >> ailArray[i];
} }
d->_aeroInput.l_04_i_aerocomac_ail_f8(tmpArray); d->_aeroInput.l_04_i_aerocomac_ail_f8(ailArray);
std::array<double, 4> tmpArray2;
// elv_f8 (4个double)
std::array<double, 4> elvArray;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
inputStream >> tmpArray2[i]; inputStream >> elvArray[i];
} }
d->_aeroInput.l_04_i_aerocomac_elv_f8(tmpArray2); d->_aeroInput.l_04_i_aerocomac_elv_f8(elvArray);
std::array<double, 2> tmpArray3;
// rud_f8 (2个double)
std::array<double, 2> rudArray;
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
inputStream >> tmpArray3[i]; inputStream >> rudArray[i];
} }
d->_aeroInput.l_04_i_aerocomac_rud_f8(tmpArray3); d->_aeroInput.l_04_i_aerocomac_rud_f8(rudArray);
std::array<double, 7> tmpArray4;
// gear_f8 (7个double)
std::array<double, 7> gearArray;
for (int i = 0; i < 7; i++) { 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<double, 10> flapArray;
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
inputStream >> tmpArray[i]; inputStream >> flapArray[i];
} }
d->_aeroInput.l_04_i_aerocomac_flap_f8(tmpArray); d->_aeroInput.l_04_i_aerocomac_flap_f8(flapArray);
std::array<double, 20> tmpArray5;
// slat_f8 (20个double)
std::array<double, 20> slatArray;
for (int i = 0; i < 20; i++) { 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<double, 20> splArray;
for (int i = 0; i < 20; i++) { 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<double, 4> tnetArray;
for (int i = 0; i < 4; i++) { 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<double, 20> kiceArray;
for (int i = 0; i < 20; i++) { 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; inputStream >> tmp;
d->_aeroInput.l_04_i_aerocomac_alt_agl_f8(tmp); d->_aeroInput.l_04_i_aerocomac_alt_agl_f8(tmp);
d->_dataWriters["XNSim::ATA04::Aerodynamics_input"]->write(&d->_aeroInput); d->_dataWriters["XNSim::ATA04::Aerodynamics_input"]->write(&d->_aeroInput);
} catch (const std::exception &e) { } catch (const std::exception &e) {
LOG_WARNING("ATA04DataProcessor::OnAeroInput: exception during data parsing: %s", e.what()); LOG_WARNING("ATA04DataProcessor::OnAeroInput: exception during data parsing: %s", e.what());
@ -1155,27 +1212,28 @@ void XNATA04DataProcessor::SendUdpTestData()
aeroStream.setVersion(QDataStream::Qt_6_0); aeroStream.setVersion(QDataStream::Qt_6_0);
// 气动输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x00=气动模型, 0x00=输入数据) // 气动输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x00=气动模型, 0x00=输入数据)
quint8 aeroHeader[5] = {0x0b, 0x04, 0x00, 0x00, 0x00}; quint8 aeroHeader[6] = {0x0b, 0x04, 0x00, 0x00, 0x00, 0x00}; // 最后两个字节用于大小
aeroStream << aeroHeader[0] << aeroHeader[1] << aeroHeader[2] << aeroHeader[3] << aeroHeader[4]; aeroStream << aeroHeader[0] << aeroHeader[1] << aeroHeader[2] << aeroHeader[3] << aeroHeader[4]
<< aeroHeader[5];
// 气动数据字段 // 气动数据字段
aeroStream << (double)5.0; // alpha aeroStream << (double)-2.0; // alpha
aeroStream << (double)0.5; // alpdot aeroStream << (double)0.0; // alpdot
aeroStream << (double)1.0; // beta aeroStream << (double)0.0; // beta
aeroStream << (double)10000.0; // press_alt aeroStream << (double)10000.0; // press_alt
aeroStream << (double)250.0; // tas aeroStream << (double)360.0; // tas
aeroStream << (double)0.8; // mach aeroStream << (double)0.3; // mach
aeroStream << (double)0.2; // nx aeroStream << (double)0.0; // nx
aeroStream << (double)0.0; // ny aeroStream << (double)0.0; // ny
aeroStream << (double)1.0; // nz aeroStream << (double)1.0; // nz
aeroStream << (double)0.0; // p aeroStream << (double)0.0; // p
aeroStream << (double)0.0; // q aeroStream << (double)0.0; // q
aeroStream << (double)0.0; // r aeroStream << (double)0.0; // r
aeroStream << (double)12000.0; // qbar aeroStream << (double)110.0; // qbar
aeroStream << (double)0.0; // blcg aeroStream << (double)0.0; // blcg
aeroStream << (double)0.0; // bscg aeroStream << (double)650.0; // bscg
aeroStream << (double)0.0; // wlcg aeroStream << (double)7.0; // wlcg
aeroStream << (double)0.0; // stab aeroStream << (double)-2.0; // stab
// ail_f8 数组，需要10个double // ail_f8 数组，需要10个double
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
@ -1194,17 +1252,17 @@ void XNATA04DataProcessor::SendUdpTestData()
// gear_f8 数组，需要7个double // gear_f8 数组，需要7个double
for (int i = 0; i < 7; i++) { for (int i = 0; i < 7; i++) {
aeroStream << (double)0.0; aeroStream << (double)1.0;
} }
// flap_f8 数组，需要10个double // flap_f8 数组，需要10个double
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
aeroStream << (double)0.0; aeroStream << (double)41.5;
} }
// slat_f8 数组，需要20个double // slat_f8 数组，需要20个double
for (int i = 0; i < 20; i++) { for (int i = 0; i < 20; i++) {
aeroStream << (double)0.0; aeroStream << (double)20.855;
} }
// spl_f8 数组，需要20个double // spl_f8 数组，需要20个double
@ -1214,7 +1272,7 @@ void XNATA04DataProcessor::SendUdpTestData()
// tnet_f8 数组，需要4个double // tnet_f8 数组，需要4个double
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
aeroStream << (double)20000.0; aeroStream << (double)8000.0;
} }
// kice_f8 数组，需要20个double // kice_f8 数组，需要20个double
@ -1223,40 +1281,38 @@ void XNATA04DataProcessor::SendUdpTestData()
} }
// alt_agl // alt_agl
aeroStream << (double)1000.0; aeroStream << (double)2500.0;
// 更新数据包大小，确保不会溢出 // 更新数据包大小（使用两个字节）
if (aeroData.size() <= 255) { quint16 size = aeroData.size();
aeroData[4] = aeroData.size(); aeroData[4] = (size >> 8) & 0xFF; // 高字节
} else { aeroData[5] = size & 0xFF; // 低字节
LOG_WARNING("ATA04DataProcessor::SendUdpTestData: aeroData size exceeds byte limit: %d",
aeroData.size());
aeroData[4] = 255; // 设置为最大值
}
// 创建地面操作输入(GhInput)数据包 // 创建地面操作输入(GhInput)数据包
QByteArray ghData; QByteArray ghData;
QDataStream ghStream(&ghData, QIODevice::WriteOnly); QDataStream ghStream(&ghData, QIODevice::WriteOnly);
ghStream.setByteOrder(QDataStream::LittleEndian); ghStream.setByteOrder(QDataStream::LittleEndian);
ghStream.setVersion(QDataStream::Qt_6_0); ghStream.setVersion(QDataStream::Qt_6_0);
// 地面操作输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x01=地面操作模型, 0x00=输入数据) // 地面操作输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x01=地面操作模型, 0x00=输入数据)
quint8 ghHeader[5] = {0x0b, 0x04, 0x01, 0x00, 0x00}; quint8 ghHeader[6] = {0x0b, 0x04, 0x01, 0x00, 0x00, 0x00}; // 最后两个字节用于大小
ghStream << ghHeader[0] << ghHeader[1] << ghHeader[2] << ghHeader[3] << ghHeader[4]; ghStream << ghHeader[0] << ghHeader[1] << ghHeader[2] << ghHeader[3] << ghHeader[4]
<< ghHeader[5];
// 地面操作数据字段 // 地面操作数据字段
ghStream << (quint8)0; // frz_l1 ghStream << (quint8)0; // frz_l1
ghStream << (quint8)0; // chocks_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 << (quint8)0; // frzflt_l1
ghStream << (double)0.0; // p_f8 ghStream << (double)0.0; // p_f8
ghStream << (double)0.0; // q_f8 ghStream << (double)0.0; // q_f8
ghStream << (double)0.0; // r_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; // vg_f8
ghStream << (double)0.0; // wg_f8 ghStream << (double)0.0; // wg_f8
ghStream << (double)0.0; // blcg_f8 ghStream << (double)0.0; // blcg_f8
ghStream << (double)0.0; // bscg_f8 ghStream << (double)649.3; // bscg_f8
ghStream << (double)0.0; // wlcg_f8 ghStream << (double)10.0; // wlcg_f8
ghStream << (quint8)0; // pb_active_l1 ghStream << (quint8)0; // pb_active_l1
ghStream << (double)0.0; // pb_towforce_f8 ghStream << (double)0.0; // pb_towforce_f8
@ -1269,12 +1325,7 @@ void XNATA04DataProcessor::SendUdpTestData()
// gear_f8 数组，需要3个double // gear_f8 数组，需要3个double
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
ghStream << (double)0.0; ghStream << (double)1.0;
}
// 另一个3个double的数组
for (int i = 0; i < 3; i++) {
ghStream << (double)0.0;
} }
// gsteer_f8 数组，需要10个double // gsteer_f8 数组，需要10个double
@ -1285,7 +1336,11 @@ void XNATA04DataProcessor::SendUdpTestData()
// tire_pres_f8 数组 3x2 // tire_pres_f8 数组 3x2
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
for (int j = 0; j < 2; j++) { 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 ghStream << (quint8)0; // onjax_l1
// contdep_f8 数组，需要7个double // 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; ghStream << (double)0.0;
} }
@ -1307,12 +1363,12 @@ void XNATA04DataProcessor::SendUdpTestData()
ghStream << (double)0.0; // rwyhdg_f8 ghStream << (double)0.0; // rwyhdg_f8
ghStream << (quint8)0; // reset_braketemp_l1 ghStream << (quint8)0; // reset_braketemp_l1
ghStream << (quint8)0; // reset_tirepress_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 // brake_temp_f8 数组 3x2
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
for (int j = 0; j < 2; j++) { 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 ghStream << (quint8)0; // brk_reset_tpres_l1
// rcon_ci_f8 数组，需要14个double // 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 << (double)0.0;
} }
ghStream << (qint32)0; // gsteer_state_i4 ghStream << (qint32)2; // gsteer_state_i4
ghStream << (quint8)0; // trim_active_l1 ghStream << (quint8)1; // trim_active_l1
ghStream << (double)0.0; // phi_deg_f8 ghStream << (double)0.0; // phi_deg_f8
ghStream << (double)0.0; // theta_deg_f8 ghStream << (double)0.0; // theta_deg_f8
ghStream << (double)0.0; // psi_deg_f8 ghStream << (double)0.0; // psi_deg_f8
ghStream << (quint8)0; // resetint_l1 ghStream << (quint8)1; // resetint_l1
// 更新数据包大小，确保不会溢出 // 更新数据包大小（使用两个字节）
if (ghData.size() <= 255) { size = ghData.size();
ghData[4] = ghData.size(); ghData[4] = (size >> 8) & 0xFF; // 高字节
} else { ghData[5] = size & 0xFF; // 低字节
LOG_WARNING("ATA04DataProcessor::SendUdpTestData: ghData size exceeds byte limit: %d",
ghData.size());
ghData[4] = 255; // 设置为最大值
}
// 创建重量平衡输入(WbInput)数据包 // 创建重量平衡输入(WbInput)数据包
QByteArray wbData; QByteArray wbData;
QDataStream wbStream(&wbData, QIODevice::WriteOnly); QDataStream wbStream(&wbData, QIODevice::WriteOnly);
wbStream.setByteOrder(QDataStream::LittleEndian); wbStream.setByteOrder(QDataStream::LittleEndian);
wbStream.setVersion(QDataStream::Qt_6_0); wbStream.setVersion(QDataStream::Qt_6_0);
// 重量平衡输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x02=重量平衡模型, 0x00=输入数据) // 重量平衡输入数据包头 (0x0b=从外部输入, 0x04=ATA04, 0x02=重量平衡模型, 0x00=输入数据)
quint8 wbHeader[5] = {0x0b, 0x04, 0x02, 0x00, 0x00}; quint8 wbHeader[6] = {0x0b, 0x04, 0x02, 0x00, 0x00, 0x00}; // 最后两个字节用于大小
wbStream << wbHeader[0] << wbHeader[1] << wbHeader[2] << wbHeader[3] << wbHeader[4]; wbStream << wbHeader[0] << wbHeader[1] << wbHeader[2] << wbHeader[3] << wbHeader[4]
<< wbHeader[5];
// 重量平衡数据字段 // 重量平衡数据字段
wbStream << (double)0.0; // theta_deg_f8 wbStream << (double)0.0; // theta_deg_f8
wbStream << (double)0.0; // phi_deg_f8 wbStream << (double)0.0; // phi_deg_f8
wbStream << (double)0.0; // psi_deg_f8 wbStream << (double)0.0; // psi_deg_f8
wbStream << (quint8)0; // gear_mode_l1 wbStream << (quint8)0; // gear_mode_l1
wbStream << (double)60000.0; // acset_gw_f8 (60000kg飞机重量) wbStream << (double)82574.0; // acset_gw_f8 (60000kg飞机重量)
wbStream << (double)25.0; // acset_cg_f8 (25%MAC重心位置) wbStream << (double)0.1215; // acset_cg_f8 (25%MAC重心位置)
// acset_tankfuel_f4 数组，需要20个float // acset_tankfuel_f4 数组，需要20个float
for (int i = 0; i < 20; i++) { 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)9082.0; // acset_totfuel_f8 (总油量9082kg)
wbStream << (double)50000.0; // acset_zfw_f8 (零油重50000kg) wbStream << (double)73492.0; // acset_zfw_f8 (零油重73492kg)
wbStream << (double)24.0; // acset_zfwcg_f8 (零油重重心24%MAC) wbStream << (double)0.12; // acset_zfwcg_f8 (零油重重心24%MAC)
// eng_efsep_l1 数组，需要4个char // eng_efsep_l1 数组，需要4个char
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
@ -1386,34 +1441,30 @@ void XNATA04DataProcessor::SendUdpTestData()
// fuel_f8 数组，需要20个double // fuel_f8 数组，需要20个double
for (int i = 0; i < 20; i++) { 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 // kice_f8 数组，需要20个double
for (int i = 0; i < 20; i++) { for (int i = 0; i < 20; i++) {
wbStream << (double)0.0; wbStream << (double)0.0;
} }
wbStream << (quint8)0; // bycglim_l1 wbStream << (quint8)1; // bycglim_l1
wbStream << (quint8)0; // bygwlim_l1 wbStream << (quint8)1; // bygwlim_l1
wbStream << (quint8)0; // frz_l1 wbStream << (quint8)0; // frz_l1
wbStream << (quint8)0; // zcgfrz_l1 wbStream << (quint8)0; // zcgfrz_l1
wbStream << (quint8)0; // zcgfrz_grfx_l1 wbStream << (quint8)0; // zcgfrz_grfx_l1
wbStream << (quint8)0; // ycgfrz_l1 wbStream << (quint8)0; // ycgfrz_l1
wbStream << (quint8)0; // inertfrz_l1 wbStream << (quint8)0; // inertfrz_l1
wbStream << (double)60000.0; // potreq_gw_f8 wbStream << (double)80000.0; // potreq_gw_f8
wbStream << (double)25.0; // potreq_gwcg_f8 wbStream << (double)0.2; // potreq_gwcg_f8
// 更新数据包大小，确保不会溢出 // 更新数据包大小（使用两个字节）
if (wbData.size() <= 255) { size = wbData.size();
wbData[4] = wbData.size(); wbData[4] = (size >> 8) & 0xFF; // 高字节
} else { wbData[5] = size & 0xFF; // 低字节
LOG_WARNING("ATA04DataProcessor::SendUdpTestData: wbData size exceeds byte limit: %d",
wbData.size());
wbData[4] = 255; // 设置为最大值
}
// 发送测试数据包 // 发送测试数据包
TriggerRTEvent("SendTestUDPData", QVariant::fromValue(aeroData)); TriggerRTEvent("SendTestUDPData", QVariant::fromValue(aeroData));

42
XNServices/XNUDPService/XNUDPService.cpp
View File

@ -89,56 +89,42 @@ void XNUDPService::HandleIncomingData()
d->udpSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); d->udpSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
// 数据包头在第9个位置，直接跳过前9个字节 if (datagram.size() <= 6) {
if (datagram.size() <= 9) {
LOG_WARNING("Invalid size of UDP datagram received, size:%1", datagram.size()); LOG_WARNING("Invalid size of UDP datagram received, size:%1", datagram.size());
continue; continue;
} }
// 创建一个新的数据包，去除前面的额外头信息
QByteArray actualData = datagram.mid(9);
// 使用处理后的数据 // 使用处理后的数据
quint8 header = (quint8)actualData[0]; quint8 header = (quint8)datagram[0];
quint8 type = (quint8)actualData[1]; quint8 type = (quint8)datagram[1];
quint8 subType1 = (quint8)actualData[2]; quint8 subType1 = (quint8)datagram[2];
quint8 subType2 = (quint8)actualData[3]; quint8 subType2 = (quint8)datagram[3];
quint8 size = (quint8)actualData[4]; quint16 size = (quint8)datagram[4] << 8 | (quint8)datagram[5];
// 处理接收到的数据
if (actualData.size() <= 5) {
LOG_WARNING("Invalid size of UDP datagram received, size:%1", actualData.size());
continue;
}
if (header != 0x0b) { if (header != 0x0b) {
LOG_WARNING("Invalid header of UDP datagram received, header:%1", header); LOG_WARNING("Invalid header of UDP datagram received, header:%1", header);
continue; continue;
} }
//如果数据包大小大于size,则截取size大小的数据 if (datagram.size() < size) {
if (actualData.size() > size) {
actualData = actualData.mid(0, size);
} else if (actualData.size() < size) {
LOG_WARNING("Invalid size of UDP datagram received, size:%1, expected:%2", LOG_WARNING("Invalid size of UDP datagram received, size:%1, expected:%2",
actualData.size(), size); datagram.size(), size);
continue; continue;
} }
if (type == 0x04) { if (type == 0x04) {
if (subType1 == 0x00 && subType2 == 0x00) { if (subType1 == 0x00 && subType2 == 0x00) {
TriggerRTEvent("ATA04AeroInput", actualData); TriggerRTEvent("ATA04AeroInput", datagram);
continue; continue;
} else if (subType1 == 0x01 && subType2 == 0x00) { } else if (subType1 == 0x01 && subType2 == 0x00) {
TriggerRTEvent("ATA04GhInput", actualData); TriggerRTEvent("ATA04GhInput", datagram);
continue; continue;
} else if (subType1 == 0x02 && subType2 == 0x00) { } else if (subType1 == 0x02 && subType2 == 0x00) {
TriggerRTEvent("ATA04WbInput", actualData); TriggerRTEvent("ATA04WbInput", datagram);
continue; continue;
} }
} }
// TODO: 根据具体需求处理其它数据 // TODO: 根据具体需求处理其它数据
TriggerRTEvent("ReceiveUDPData", actualData); TriggerRTEvent("ReceiveUDPData", datagram);
} }
} }
@ -151,9 +137,7 @@ void XNUDPService::SendData(const QVariant &data)
} }
// 将QVariant转换为字节数组 // 将QVariant转换为字节数组
QByteArray datagram; QByteArray datagram = data.toByteArray();
QDataStream stream(&datagram, QIODevice::WriteOnly);
stream << data;
// 发送数据 // 发送数据
qint64 bytesSent = qint64 bytesSent =

6
XNServices/XNUDPTestService/XNUDPTestService.cpp
View File

@ -73,7 +73,7 @@ void XNUDPTestService::OnPrepareForExecute()
} }
// 连接UDP socket的信号 // 连接UDP socket的信号
connect(d->udpSocket, &QUdpSocket::readyRead, this, &XNUDPTestService::HandleIncomingData); //connect(d->udpSocket, &QUdpSocket::readyRead, this, &XNUDPTestService::HandleIncomingData);
RegisterRTEventHandler("SendTestUDPData", RegisterRTEventHandler("SendTestUDPData",
std::bind(&XNUDPTestService::SendData, this, std::placeholders::_1)); std::bind(&XNUDPTestService::SendData, this, std::placeholders::_1));
@ -122,9 +122,7 @@ void XNUDPTestService::SendData(const QVariant &data)
} }
// 将QVariant转换为字节数组 // 将QVariant转换为字节数组
QByteArray datagram; QByteArray datagram = data.toByteArray();
QDataStream stream(&datagram, QIODevice::WriteOnly);
stream << data;
// 发送数据 // 发送数据
qint64 bytesSent = qint64 bytesSent =