void Start()
{
TeachIp = new IPEndPoint(IPAddress.Parse(File.ReadAllText("ip.txt")), 16000);
this.clientTeach = new UdpClient(12000);
ip = new IPEndPoint(IPAddress.Any, 0);
angle = DefaultAngle;
lastAngle = DefaultAngle;
otherAngle = DefaultAngle;
command = default(TeachingCommand);
angleWithLocation = default(AngleWithLocation);
otherAngleWithLocation = default(AngleWithLocation);
SendThreading = new Thread(new ThreadStart(SendRotation));
SendThreading.Start();
SendThreading.Name = "SendEvent" + base.transform.name;
RecieveThread = new Thread(new ThreadStart(RecieveData));
RecieveThread.Start();
RecieveThread.Name = "RecieveEvent";
RecieveFromTeachThread = new Thread(new ThreadStart(RecieveDataFromTeach));
RecieveFromTeachThread.Start();
RecieveFromTeachThread.Name = "RecieveFromTeachEvent";
//UdpPlatform.Instance.SendString("Udp Start");
Task.Run(() =>
{
while (true)
{
//var ipRe = new IPEndPoint(IPAddress.Parse("192.168.0.135"), 16000);
//var bytes = this.clientTeach.Receive(ref ipRe);
var bytes = this.clientTeach.Receive(ref TeachIp);
var length = bytes.Length;
if (length == StructHelper.GetStructSize <TeachingCommand>())
{
command = StructHelper.BytesToStruct <TeachingCommand>(bytes);
this.IsSendFromTeachingPlatform = command.IsStart == 1;
}
}
});
Task.Run(() =>
{
while (true)
{
UdpPlatform.Instance.SendData(this.angleWithLocation, new IPEndPoint(IPAddress.Broadcast, 16000));
Thread.Sleep(20);
}
});
if (this.MemDB.Init(this.memName) == 0 || this.MemDB.Init("GAME_SHARED_MEM_0000") == 0)
{
Debug.Log("Run");
}
else
{
Debug.Log("no Run");
Time.timeScale = 0f;
}
}
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;
}
});
}
private void RecieveDataFromTeach()
{
while (true)
{
var ipRe = new IPEndPoint(IPAddress.Parse("192.168.0.135"), 16000);
var bytes = this.clientTeach.Receive(ref ipRe);
//var bytes = this.clientTeach.Receive(ref TeachIp);
var length = bytes.Length;
if (length == StructHelper.GetStructSize <TeachingCommand>())
{
command = StructHelper.BytesToStruct <TeachingCommand>(bytes);
this.IsSendFromTeachingPlatform = command.IsStart == 1;
}
}
}
private void RecieveData()
{
while (true)
{
ip = new IPEndPoint(IPAddress.Any, 0);
var bytes = UdpPlatform.Instance.RecieveData(ref ip);
var length = bytes.Length;
if (length == StructHelper.GetStructSize <AngleWithLocation>())
{
var angleAndLocation = StructHelper.BytesToStruct <AngleWithLocation>(bytes);
var pitchRad = StructHelper.Deg2Rad((float)angleAndLocation.Pitch);
var rollRad = StructHelper.Deg2Rad((float)angleAndLocation.Roll);
var yawRad = StructHelper.Deg2Rad((float)angleAndLocation.Yaw);
var ipstr = ip.ToString();
if (ipstr.StartsWith("192.168.0.131") == false && ipstr.StartsWith("192.168.0.136") == false)
{
this.angleWithLocation = angleAndLocation;
this.angle.Pitch = -pitchRad;
this.angle.Roll = -rollRad;
this.angle.Yaw = yawRad;
if (this.IsSend == true)
{
UdpPlatform.Instance.SendData(this.angleWithLocation, TeachIp);
}
}
else
{
this.otherAngleWithLocation = angleAndLocation;
this.otherAngle.Pitch = pitchRad;
this.otherAngle.Roll = rollRad;
this.otherAngle.Yaw = yawRad;
}
}
if (length == StructHelper.GetStructSize <TeachingCommand>())
{
command = StructHelper.BytesToStruct <TeachingCommand>(bytes);
this.IsSendFromTeachingPlatform = command.IsStart == 1;
}
}
}
