public static void Stop()
{
DataBrokerLeft.Esc();
DataBrokerRight.Esc();
}
private static void ParsePacket(object str)
{
List <SpatialPoint> spList = new List <SpatialPoint>();
var indices = (str as Tuple <int, string>).Item1;
var datagram = (str as Tuple <int, string>).Item2;
if (datagram.IndexOf("sRA LMDscandata") < 0)
{
return;
}
datagram = datagram.Substring(datagram.IndexOf("sRA LMDscandata"));
//分解数据
String[] parts = datagram.Split(' ');
var ts = (DateTime.Now - TimeStamp).TotalSeconds;
var y = ts * Speed;
try
{
String test1 = parts[23];
String test2 = parts[24];
String test3 = parts[25];
}
catch (Exception)
{
// TODO: handle exception
return;
}
string startAngelStr = parts[23];
string stepStr = parts[24];
string numStr = parts[25];
int startAngel1 = int.Parse(startAngelStr, System.Globalization.NumberStyles.HexNumber) / 10000;
//BigInteger startAngel = new BigInteger(startAngelStr, 16).divide(new BigInteger(10000 + ""));//6DDD0//FFFF3CB0
float step = int.Parse(stepStr, NumberStyles.HexNumber) / 10000f;
int num = int.Parse(numStr, NumberStyles.HexNumber);
for (int index = 26; index < (26 + num) && index < parts.Length - 1; index++)
{
String aDataItem = parts[index];
float distance = int.Parse(aDataItem, NumberStyles.HexNumber) / 1000f;
float angle = startAngel1 + (index - 26) * step;
if (distance == 0.0)
{
continue;
}
//pointList.Add(new OriginPoint(distance, angel));
spList.Add(Format(indices, distance, angle, y));
}
if (spList.Count < 30)
{
return;
}
else
{
foreach (var n in spList)
{
if (n.X > 1.82 && n.X < 2.57)
{
//LEFT
DataBrokerLeft.Enqueue(n);
}
//else if (n.X > 4.58 && n.X < 5.88)
else if (n.X > 3.58 && n.X < 6.88)
{
//RIGHT
DataBrokerRight.Enqueue(n);
}
}
}
}
