public void Init()
{
TriangleLocate locate = new TriangleLocate();
var lpoint1 = new Locate2D(DateTime.UtcNow, 116.3187, 39.98544, 41.09);
locate.Buoys.Add(1, lpoint1);
var lpoint2 = new Locate2D(DateTime.UtcNow, 116.3184, 39.98542, 22.23);
locate.Buoys.Add(2, lpoint2);
var lpoint3 = new Locate2D(DateTime.UtcNow, 116.3184, 39.98556, 23.64);
locate.Buoys.Add(4, lpoint3);
var center = new PointLatLng((locate.Buoys.Values[0].Lat + locate.Buoys.Values[1].Lat + locate.Buoys.Values[2].Lat) / 3,
(locate.Buoys.Values[0].Lng + locate.Buoys.Values[1].Lng + locate.Buoys.Values[2].Lng) / 3);
var buoy1 = new PointLatLng(locate.Buoys.Values[0].Lat, locate.Buoys.Values[0].Lng);
Utility.Util.GetReltXY(buoy1, center, out x1, out y1);
z1 = 0;
var buoy2 = new PointLatLng(locate.Buoys.Values[1].Lat, locate.Buoys.Values[1].Lng);
Utility.Util.GetReltXY(buoy2, center, out x2, out y2);
z2 = 0;
var buoy3 = new PointLatLng(locate.Buoys.Values[2].Lat, locate.Buoys.Values[2].Lng);
Utility.Util.GetReltXY(buoy3, center, out x3, out y3);
z3 = 0;
range1 = locate.Buoys.Values[0].Range;
range2 = locate.Buoys.Values[1].Range;
range3 = locate.Buoys.Values[2].Range;
m = new Matrix();
i1 = new Input()
{
x = x1,
y = y1,
z = z1,
r = range1,
};
i2 = new Input()
{
x = x2,
y = y2,
z = z2,
r = range2,
};
i3 = new Input()
{
x = x3,
y = y3,
z = z3,
r = range3,
};
D = new double[3];
MatrixLocate.InitMatrix(ref m, i1, i2, i3, ref D);
}
public void Handle(object sender, CustomEventArgs e)
{
if (e.ParseOK)
{
//
try
{
var buf = new byte[e.DataBuffer.Length + 2];
var ip = e.CallSrc as string;
int id = 0;
id = Int16.Parse(ip.Substring(ip.Length - 1));
if (ip != null && !UnitCore.Instance.IsReplay)
{
//save
Buffer.BlockCopy(e.DataBuffer, 0, buf, 2, e.DataBuffer.Length);
Buffer.BlockCopy(BitConverter.GetBytes(id), 0, buf, 0, 2);
UnitCore.Instance.MonTraceService.Save("ALL", buf);
switch (id)
{
case 1:
UnitCore.Instance.MonTraceService.Save("Buoy1", e.DataBuffer);
break;
case 2:
UnitCore.Instance.MonTraceService.Save("Buoy2", e.DataBuffer);
break;
case 3:
UnitCore.Instance.MonTraceService.Save("Buoy3", e.DataBuffer);
break;
case 4:
UnitCore.Instance.MonTraceService.Save("Buoy4", e.DataBuffer);
break;
default:
break;
}
}
Buoy buoy = null;
if (UnitCore.Instance.Buoy.ContainsKey(id - 1))
{
buoy = ((Buoy)UnitCore.Instance.Buoy[id - 1]);
}
if (buoy == null)
{
return;
}
//类型标志
if (e.Mode == CallMode.UDPAns)
{
App.Current.Dispatcher.Invoke(new Action(() =>
{
UnitCore.Instance.EventAggregator.PublishMessage(new LogEvent("浮标命令发送成功", LogType.OnlyInfoandClose));
}));
return;
}
else if (e.Mode == CallMode.GPS) //gps
{
var gpsbuf = new byte[1030];
Buffer.BlockCopy(e.DataBuffer, 2, gpsbuf, 0, 1030);
var info = MonProtocol.MonProtocol.ParseGps(gpsbuf);
if (info != null)
{
buoy.gps = info;
}
}
else if (e.Mode == CallMode.Range)
{
var litebuf = new byte[204];
Buffer.BlockCopy(e.DataBuffer, 2, litebuf, 0, 204);
var range = MonProtocol.MonProtocol.ParsePulseRange(litebuf, false);
var gpsbuf = new byte[1030];
Buffer.BlockCopy(e.DataBuffer, 16, gpsbuf, 0, 200 - 16);
var info = MonProtocol.MonProtocol.ParseGps(gpsbuf);
if (info != null)
{
buoy.gps = info;
}
if (range.ID == UnitCore.Instance.TargetObj1.ID)
{
buoy.liteRange1 = range;
buoy.Range1 = MonProtocol.MonProtocol.CalDistanceByLite(range);
}
if (range.ID == UnitCore.Instance.TargetObj2.ID)
{
buoy.liteRange2 = range;
buoy.Range2 = MonProtocol.MonProtocol.CalDistanceByLite(range);
}
}
else if (e.Mode == CallMode.TeleRange)
{
var litebuf = new byte[14];
Buffer.BlockCopy(e.DataBuffer, 2, litebuf, 0, 14);
var range = MonProtocol.MonProtocol.ParsePulseRange(litebuf, true);
var length = BitConverter.ToUInt16(e.DataBuffer, 31);
var combuf = new byte[242];
Buffer.BlockCopy(e.DataBuffer, 16, combuf, 0, 242);
var telerange = MonProtocol.MonProtocol.ParseTeleRange(combuf, length);
var gpsbuf = new byte[1030];
Buffer.BlockCopy(e.DataBuffer, 33 + length, gpsbuf, 0, 256 - 33 - length);
var info = MonProtocol.MonProtocol.ParseGps(gpsbuf);
if (telerange.ID == UnitCore.Instance.TargetObj1.ID)
{
buoy.teleRange1 = telerange;
buoy.liteRange1 = range;
buoy.Range1 = MonProtocol.MonProtocol.CalDistanceByTele(range, telerange);
}
if (telerange.ID == UnitCore.Instance.TargetObj2.ID)
{
buoy.teleRange2 = telerange;
buoy.liteRange2 = range;
buoy.Range2 = MonProtocol.MonProtocol.CalDistanceByTele(range, telerange);
}
if (info != null)
{
buoy.gps = info;
}
}
if (buoy.gps == null)
{
return;
}
if (buoy.Range1 > 0.5)
{
var lpoint = new Locate2D(buoy.RangeTime1, buoy.gps.Longitude, buoy.gps.Latitude, buoy.Range1);
//remove possible duplicate data
UnitCore.Instance.Locate1.Buoys.Remove(id);
UnitCore.Instance.Locate1.Buoys.Add(id, lpoint);
}
if (buoy.Range2 > 0.5)
{
var lpoint = new Locate2D(buoy.RangeTime2, buoy.gps.Longitude, buoy.gps.Latitude, buoy.Range2);
//remove possible duplicate data
UnitCore.Instance.Locate2.Buoys.Remove(id);
UnitCore.Instance.Locate2.Buoys.Add(id, lpoint);
}
var point = new PointLatLng(buoy.gps.Latitude, buoy.gps.Longitude);
point.Offset(UnitCore.Instance.MainMapCfg.MapOffset.Lat,
UnitCore.Instance.MainMapCfg.MapOffset.Lng);
UnitCore.Instance.EventAggregator.PublishMessage(new RefreshBuoyInfoEvent(id - 1));
App.Current.Dispatcher.Invoke(new Action(() =>
{
if (UnitCore.Instance.mainMap != null)
{
UnitCore.Instance.BuoyLock.WaitOne();
var itor = UnitCore.Instance.mainMap.Markers.GetEnumerator();
while (itor.MoveNext())
{
var marker = itor.Current;
if ((int)marker.Tag == id)
{
if (marker.Shape is BuoyMarker buoymarker)
{
buoymarker.Refresh(buoy);
marker.Position = point;
}
}
}
UnitCore.Instance.BuoyLock.ReleaseMutex();
}
}));
}
catch (Exception ex)
{
if (UnitCore.Instance.BuoyLock.WaitOne(100) == true)
{
UnitCore.Instance.BuoyLock.ReleaseMutex();
}
App.Current.Dispatcher.Invoke(new Action(() =>
{
UnitCore.Instance.EventAggregator.PublishMessage(new ErrorEvent(ex, LogType.Both));
}));
}
}
else
{
if (e.Mode == CallMode.ErrMode)
{
App.Current.Dispatcher.Invoke(new Action(() =>
{
UnitCore.Instance.NetCore.StopUDPService();
// UnitCore.Instance.EventAggregator.PublishMessage(new ErrorEvent(e.Ex, LogType.Both));
}));
}
}
}