public DrawDataPacketEventArgs(DataPoint[] data, ISensor lidar, TimeStampPacket tsp)
{
this.data = data;
this.lidar = lidar;
this.tsp = tsp;
}
private DataPoint[] processIbeoPacket(ref IbeoPacket p, MemoryStream ms)
{
ms.Position = 0;
BinaryReader br = new BinaryReader(ms);
DataPoint[] points = null;
p.MagicWord = BigToLittle(br.ReadUInt32());
if (p.MagicWord != MAGIC_WORD) throw new Exception("FATAL BAD PACKET IN PROCESS!!!!");
p.Size = BigToLittle(br.ReadUInt32());
p.DataType = BigToLittle(br.ReadUInt32());
p.IbeoTimeStamp = BigToLittle(br.ReadUInt32());
if (p.DataType == FILE_TYPE_COMPRESSED_SCAN) //FILE TYPE COMPRESSED SCAN
{
p.Version = br.ReadByte();
p.ScannerType = br.ReadByte();
p.ECUID = br.ReadByte();
p.PAD = br.ReadByte();
p.IbeoTimeStamp = BigToLittle(br.ReadUInt32());
p.StartAngle = BigToLittle(br.ReadInt16());
p.StopAngle = BigToLittle(br.ReadInt16());
p.ScanCount = BigToLittle(br.ReadUInt16());
//System.Console.WriteLine("Num stuffs:" + p.ScanCount);
p.NumberOfPoints = BigToLittle(br.ReadUInt16());
p.ScanPoints = new IbeoPoint[p.NumberOfPoints];
points = new DataPoint[p.NumberOfPoints];
for (int i = 0; i < p.NumberOfPoints; i++)
{
p.ScanPoints[i] = new IbeoPoint();
p.ScanPoints[i].ScannerID = br.ReadByte();
p.ScanPoints[i].Channel = br.ReadByte();
p.ScanPoints[i].SubChannel = br.ReadByte();
p.ScanPoints[i].PointStatus = br.ReadByte();
p.ScanPoints[i].X = BigToLittle(br.ReadInt16());
p.ScanPoints[i].Y = BigToLittle(br.ReadInt16());
p.ScanPoints[i].Z = BigToLittle(br.ReadInt16());
p.ScanPoints[i].EchoPulseWidth = BigToLittle(br.ReadUInt16());
points[i] = new DataPoint(IbeoPointConverter(p.ScanPoints[i].X),
IbeoPointConverter(p.ScanPoints[i].Y),
IbeoPointConverter(p.ScanPoints[i].Z),
p.ScanPoints[i].Channel,
p.ScanPoints[i].SubChannel,
p.ScanPoints[i].PointStatus);
}
}
return points;
}
