XNSim/XNServices/XNUDPService/XNUDPService.cpp

#include "XNUDPService.h"
#include "XNUDPService_p.h"
#include <XNCore/XNServiceManager.h>
#include <XNCore/XNDDSManager.h>
#include <fstream>
#include <sstream>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <XNCore/XNByteArray.h>

XN_SERVICE_INITIALIZE(XNUDPService)

XNUDPService::XNUDPService() : XNServiceObject(new XNUDPServicePrivate())
{
}

XNUDPService::~XNUDPService()
{
	T_D();
	if (d->udpSocket >= 0) {
		close(d->udpSocket);
		d->udpSocket = -1;
	}
};

XNUDPService::XNUDPService(PrivateType *p) : XNServiceObject(p)
{
}

void XNUDPService::Initialize()
{
	XNServiceObject::Initialize();
	T_D();
	// 读取配置文件
	std::ifstream file(GetXmlPath());
	if (!file.is_open()) {
		LOG_WARNING("无法打开配置文件:%s", GetXmlPath().c_str());
		return;
	}

	std::stringstream buffer;
	buffer << file.rdbuf();
	std::string content = buffer.str();
	file.close();

	// 简单的XML解析
	size_t udpPos = content.find("<UDP>");
	if (udpPos != std::string::npos) {
		size_t localPortPos	 = content.find("<LocalPort>", udpPos);
		size_t targetHostPos = content.find("<TargetHost>", udpPos);
		size_t targetPortPos = content.find("<TargetPort>", udpPos);

		if (localPortPos != std::string::npos) {
			size_t endPos = content.find("</LocalPort>", localPortPos);
			d->localPort = std::stoi(content.substr(localPortPos + 11, endPos - localPortPos - 11));
		}
		if (targetHostPos != std::string::npos) {
			size_t endPos = content.find("</TargetHost>", targetHostPos);
			d->targetHost = content.substr(targetHostPos + 12, endPos - targetHostPos - 12);
		}
		if (targetPortPos != std::string::npos) {
			size_t endPos = content.find("</TargetPort>", targetPortPos);
			d->targetPort =
				std::stoi(content.substr(targetPortPos + 12, endPos - targetPortPos - 12));
		}
	} else {
		LOG_WARNING("未找到UDP配置, 使用默认值");
		d->localPort  = 12345;
		d->targetHost = "127.0.0.1";
		d->targetPort = 54321;
	}
}

void XNUDPService::PrepareForExecute()
{
	XNServiceObject::PrepareForExecute();
	T_D();
	// 初始化UDP socket
	d->udpSocket = socket(AF_INET, SOCK_DGRAM, 0);
	if (d->udpSocket < 0) {
		LOG_WARNING("无法创建UDP socket");
		return;
	}

	// 设置非阻塞模式
	int flags = fcntl(d->udpSocket, F_GETFL, 0);
	fcntl(d->udpSocket, F_SETFL, flags | O_NONBLOCK);

	// 绑定本地端口
	struct sockaddr_in localAddr;
	memset(&localAddr, 0, sizeof(localAddr));
	localAddr.sin_family	  = AF_INET;
	localAddr.sin_addr.s_addr = INADDR_ANY;
	localAddr.sin_port		  = htons(d->localPort);

	if (bind(d->udpSocket, (struct sockaddr *)&localAddr, sizeof(localAddr)) < 0) {
		LOG_WARNING("UDP socket 绑定失败, 端口:%d", d->localPort);
		close(d->udpSocket);
		d->udpSocket = -1;
		return;
	}

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

void XNUDPService::HandleIncomingData()
{
	T_D();
	if (d->udpSocket < 0)
		return;

	char buffer[65536];
	struct sockaddr_in senderAddr;
	socklen_t senderLen = sizeof(senderAddr);

	while (true) {
		ssize_t bytesRead = recvfrom(d->udpSocket, buffer, sizeof(buffer), 0,
									 (struct sockaddr *)&senderAddr, &senderLen);

		if (bytesRead <= 0) {
			if (errno == EAGAIN || errno == EWOULDBLOCK) {
				break; // 没有更多数据
			}
			LOG_WARNING("读取UDP socket失败");
			break;
		}

		if (bytesRead <= 8) {
			LOG_WARNING("UDP 数据包小于8字节");
			continue;
		}

		// 将数据转换为XNByteArray
		XNByteArray datagram;
		datagram.resize(bytesRead);
		memcpy(datagram.data(), buffer, bytesRead);
		if (datagram[0] != 0xa6) {
			LOG_WARNING("UDP 数据包头无效, 头:%1", datagram[0]);
			continue;
		}

		if (datagram[5] != 0x00) {
			LOG_WARNING("UDP 数据包传输方向错误, 方向:%1", datagram[5]);
			continue;
		}

		uint16_t dataSize = ((uint16_t)datagram[6] << 8) | (uint16_t)datagram[7];
		if (dataSize != bytesRead) {
			LOG_WARNING("UDP 数据包大小无效, 大小:%d, 实际大小:%zd", dataSize, bytesRead - 8);
			continue;
		}

		uint8_t planeHeader	 = datagram[1];
		uint8_t ataHeader	 = datagram[2];
		uint8_t modelHeader	 = datagram[3];
		uint8_t structHeader = datagram[4];

		if (planeHeader == 0xc0) {								   //C909数据
			if (ataHeader == 0x04) {							   //ATA04章节数据
				if (modelHeader == 0x00 && structHeader == 0x00) { //气动输入数据
					TriggerRTEvent("C909::ATA04::AeroInput", datagram);
					continue;
				} else if (modelHeader == 0x01 && structHeader == 0x00) { //地操输入数据
					TriggerRTEvent("C909::ATA04::GhInput", datagram);
					continue;
				} else if (modelHeader == 0x02 && structHeader == 0x00) { //质量输入数据
					TriggerRTEvent("C909::ATA04::WbInput", datagram);
					continue;
				}
			}
		} else {
			TriggerRTEvent("ReceiveUDPData", datagram);
		}
	}
}

void XNUDPService::SendData(const std::any &data)
{
	T_D();
	if (d->udpSocket < 0) {
		LOG_WARNING("UDP socket not initialized");
		return;
	}

	struct sockaddr_in targetAddr;
	memset(&targetAddr, 0, sizeof(targetAddr));
	targetAddr.sin_family = AF_INET;
	targetAddr.sin_port	  = htons(d->targetPort);
	inet_pton(AF_INET, d->targetHost.c_str(), &targetAddr.sin_addr);

	XNByteArray packet = std::any_cast<XNByteArray>(data);
	ssize_t bytesSent  = sendto(d->udpSocket, packet.data(), packet.size(), 0,
								(struct sockaddr *)&targetAddr, sizeof(targetAddr));

	if (bytesSent < 0) {
		LOG_WARNING("Failed to send UDP datagram: %s", strerror(errno));
	}
}