static void BuildTcpUdpNet()
{
lockobj = new object();
packetBuilder = new ATCDataPacketBuilder();
comConfig = JsonFileConfig.Instance.ComConfig;
Console.WriteLine($"Me is {WswHelper.GetFlightKindFromIp(comConfig.SelfIp)}");
udpClient = new UdpClient(comConfig.SelfPort);
tcpClient = new TcpClient();
sendInterval = comConfig.SendDataIntervalMs;
isTest = comConfig.IsTCPTest;
}
static void Main(string[] args)
{
ShowHeaderInfo();
SetConsoleCtrlHandler(cancelHandler, true);
BuildTcpUdpNet();
UdpTask();
DoTcpConnect();
TcpTask();
Console.WriteLine("Press enter to exit");
while (true)
{
var line = Console.ReadLine();
if (line == "exit")
{
break;
}
if (line == "udp")
{
Console.WriteLine($"udp state is {isRevcUdp}, count is {udpCount} and error is {isUdpError}");
}
if (line == "tcp")
{
Console.WriteLine($"tcp state is {tcpClient.Connected} and error is {isTcpError}");
}
if (line == "me" || line == "self")
{
Console.WriteLine($"Me is {WswHelper.GetFlightKindFromIp(comConfig.SelfIp)}");
}
if (line == "run udp" && isUdpError == true)
{
UdpTask();
Console.WriteLine("udp run success");
}
if (line == "run tcp" && isUseTCP == true && isTcpError == true)
{
DoTcpConnect();
TcpTask();
Console.WriteLine("tcp run success");
}
if (line == "config")
{
Process.Start(JsonFileConfig.PathAndFileName);
}
if (line == "save config")
{
JsonFileConfig.Instance.WriteToFile();
Console.WriteLine("save file success");
}
}
CloseTcpUdpNet();
}
static void UdpTask()
{
Task.Run(() =>
{
try
{
while (true)
{
// 从WswTHUSim接收飞行模拟器姿态和经纬度坐标
var ipEndPoint = new IPEndPoint(IPAddress.Any, 0);
var recieveBytes = udpClient.Receive(ref ipEndPoint);
var ip = ipEndPoint.Address.ToString();
var length = recieveBytes.Length;
if (length == StructHelper.GetStructSize <AngleWithLocation>() &&
(ip.StartsWith(comConfig.SelfIp) == true || ip.StartsWith("127.0.0.1")))
{
// 地球坐标系坐标 x y z roll pitch yaw
var angleWithLocation = StructHelper.BytesToStruct <AngleWithLocation>(recieveBytes);
var lon = PositionHelper.XYZToLon(angleWithLocation.X, angleWithLocation.Y, angleWithLocation.Z);
var lat = PositionHelper.XYZToLat(angleWithLocation.X, angleWithLocation.Y, angleWithLocation.Z);
var height = PositionHelper.XYZToHeight(angleWithLocation.X, angleWithLocation.Y, angleWithLocation.Z);
var rolldeg = (angleWithLocation.Roll);
var pitchdeg = (angleWithLocation.Pitch);
var yawdeg = (angleWithLocation.Yaw);
var kind = WswHelper.GetFlightKindFromIp(ip);
if (kind == WswModelKind.EH101)
{
rolldeg = -rolldeg; pitchdeg = -pitchdeg; yawdeg = -yawdeg;
}
udpCount++;
isRevcUdp = true;
lock (lockobj)
{
packetBuilder.SetAngles(rolldeg, pitchdeg, yawdeg)
.SetPositions(lon, lat, height)
.SetStatus(true, false, true, 80)
.SetFlightSimulatorKind(kind);
}
}
}
}
catch (Exception ex)
{
Console.WriteLine("udp error as follows:");
Console.WriteLine(ex.ToString());
isUdpError = true;
}
});
}
