XNSim/XNServices/XNUDPService/XNUDPService.cpp

#include "XNUDPService.h"
#include "XNUDPService_p.h"
#include <XNCore/XNServiceManager.h>
#include <XNCore/XNDDSManager.h>
#include <QFile>
#include <QDomDocument>

XN_DLL_INITIALIZE(XNUDPService)

XN_REGISTER_SERVICE_BEGIN_SERVICE(XNUDPService)
XN_REGISTER_SERVICE_END_SERVICE(XNUDPService)

XNUDPService::XNUDPService(QObject *parent)
	: XNServiceObject(*new XNUDPServicePrivate(this), parent)
{
}

XNUDPService::~XNUDPService()
{
}

XNUDPService::XNUDPService(XNUDPServicePrivate &dd, QObject *parent) : XNServiceObject(dd, parent)
{
}

void XNUDPService::OnInitialize()
{
	Q_D(XNUDPService);
	XNServiceObject::OnInitialize();

	// 读取配置文件
	QFile file(GetXmlPath());
	if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
		LOG_WARNING("Failed to open config file:%1", GetXmlPath());
		return;
	}

	QDomDocument doc;
	if (!doc.setContent(&file)) {
		file.close();
		LOG_WARNING("Failed to parse config file:%1", GetXmlPath());
		return;
	}
	file.close();

	// 读取UDP配置
	QDomElement udpElement = doc.documentElement().firstChildElement("UDP");
	if (!udpElement.isNull()) {
		d->localPort  = udpElement.firstChildElement("LocalPort").text().toUInt();
		d->targetHost = udpElement.firstChildElement("TargetHost").text();
		d->targetPort = udpElement.firstChildElement("TargetPort").text().toUInt();
	} else {
		LOG_WARNING("UDP configuration not found, using default values");
		d->localPort  = 12345;
		d->targetHost = "127.0.0.1";
		d->targetPort = 54321;
	}
}

void XNUDPService::OnPrepareForExecute()
{
	Q_D(XNUDPService);
	XNServiceObject::OnPrepareForExecute();

	// 初始化UDP socket
	d->udpSocket = new QUdpSocket(this);

	// 绑定本地端口
	if (!d->udpSocket->bind(QHostAddress::Any, d->localPort)) {
		LOG_WARNING("UDP socket bind failed on port:%1", d->localPort);
		return;
	}

	// 连接UDP socket的信号
	connect(d->udpSocket, &QUdpSocket::readyRead, this, &XNUDPService::HandleIncomingData);

	RegisterRTEventHandler("SendUDPData",
						   std::bind(&XNUDPService::SendData, this, std::placeholders::_1));
}

void XNUDPService::HandleIncomingData()
{
	Q_D(XNUDPService);
	while (d->udpSocket->hasPendingDatagrams()) {
		QByteArray datagram;
		datagram.resize(d->udpSocket->pendingDatagramSize());
		QHostAddress sender;
		quint16 senderPort;

		d->udpSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);

		// 数据包头在第9个位置，直接跳过前9个字节
		if (datagram.size() <= 9) {
			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;
		}

		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) {
			LOG_WARNING("Invalid size of UDP datagram received, size:%1, expected:%2",
						actualData.size(), size);
			continue;
		}

		if (type == 0x04) {
			if (subType1 == 0x00 && subType2 == 0x00) {
				TriggerRTEvent("ATA04AeroInput", actualData);
				continue;
			} else if (subType1 == 0x01 && subType2 == 0x00) {
				TriggerRTEvent("ATA04GhInput", actualData);
				continue;
			} else if (subType1 == 0x02 && subType2 == 0x00) {
				TriggerRTEvent("ATA04WbInput", actualData);
				continue;
			}
		}
		// TODO: 根据具体需求处理其它数据
		TriggerRTEvent("ReceiveUDPData", actualData);
	}
}

void XNUDPService::SendData(const QVariant &data)
{
	Q_D(XNUDPService);
	if (!d->udpSocket) {
		LOG_WARNING("UDP socket not initialized");
		return;
	}

	// 将QVariant转换为字节数组
	QByteArray datagram;
	QDataStream stream(&datagram, QIODevice::WriteOnly);
	stream << data;

	// 发送数据
	qint64 bytesSent =
		d->udpSocket->writeDatagram(datagram, QHostAddress(d->targetHost), d->targetPort);
	if (bytesSent == -1) {
		LOG_WARNING("Failed to send UDP datagram:%1", d->udpSocket->errorString());
	}
}