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);
}
private void OnReadBytesComplete(IAsyncResult AsyncResult)
{
int bytesRead = tcpClient.GetStream().EndRead(AsyncResult);
//THE BRIAN STATE MACHINE
switch (syncState)
{
case 0:
if (rxbuf[0] == MAGIC_WORD1)
{
syncState++;
tempMsg.Position = 0;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
if (GotBadPacket != null)
{
GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
break;
case 1:
if (rxbuf[0] == MAGIC_WORD2)
{
syncState++;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
syncState = 0;
if (GotBadPacket != null)
{
GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
break;
case 2:
if (rxbuf[0] == MAGIC_WORD3)
{
syncState++;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
syncState = 0;
if (GotBadPacket != null)
{
GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
break;
case 3:
if (rxbuf[0] == MAGIC_WORD4)
{
syncState++;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
syncState = 0;
if (GotBadPacket != null)
{
GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 12, new AsyncCallback(OnReadBytesComplete), null);
break;
case 4:
tempMsg.Write(rxbuf, 0, bytesRead);
if (tempMsg.Position == HEADER_SIZE) //got everything
{
syncState++;
BinaryReader br = new BinaryReader(new MemoryStream(tempMsgBuf, 0, HEADER_SIZE));
//check to see the header....
BigToLittle(br.ReadUInt32()); //read the header
msgSize = Convert.ToInt32(BigToLittle(br.ReadUInt32()));
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, msgSize, new AsyncCallback(OnReadBytesComplete), null);
}
else //read the rest....
{
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 12 - bytesRead, new AsyncCallback(OnReadBytesComplete), null);
}
break;
case 5:
tempMsg.Write(rxbuf, 0, bytesRead);
if (tempMsg.Position >= (msgSize + HEADER_SIZE)) //got everything
{
syncState = 0;
if (tempMsg.Position > (msgSize + HEADER_SIZE))
{
if (GotBadPacket != null)
{
GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
}
if (GotDataPacket != null)
{
IbeoPacket p = new IbeoPacket();
TimeStampPacket tsp;
if (tsPacketBufferList.Count != 0)
{
tsp = tsPacketBufferList.Peek();
}
else
{
tsp = new TimeStampPacket();
tsp.CarTimeSeconds = 0;
tsp.CarTimeTicks = 0;
tsp.SeqNumber = 0;
}
totalPackets++;
totalPacketsResetable++;
DataPoint[] d = processIbeoPacket(ref p, tempMsg);
if (d != null)
{
GotDataPacket(this, new DrawDataPacketEventArgs(d, this, tsp));
}
if (p.DataType == FILE_TYPE_COMPRESSED_SCAN) //we got a data packet
{
//Console.WriteLine("SCAN");
//if (tsPacketBuffer.SeqNumber == 0)
TimeStampPacket ts;
if (tsPacketBufferList.Count > 0)
{
ts = tsPacketBufferList.Dequeue();
}
else
{
System.Console.WriteLine("WARNING: LOST SEQ NUMBER!");
}
}
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
}
else
{
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, msgSize - bytesRead, new AsyncCallback(OnReadBytesComplete), null);
}
break;
}
}
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;
}
private void OnReadBytesComplete(IAsyncResult AsyncResult)
{
int bytesRead = tcpClient.GetStream().EndRead(AsyncResult);
//THE BRIAN STATE MACHINE
switch (syncState)
{
case 0:
if (rxbuf[0] == MAGIC_WORD1)
{
syncState++;
tempMsg.Position = 0;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
if (GotBadPacket != null) GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
break;
case 1:
if (rxbuf[0] == MAGIC_WORD2)
{
syncState++;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
syncState = 0;
if (GotBadPacket != null) GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
break;
case 2:
if (rxbuf[0] == MAGIC_WORD3)
{
syncState++;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
syncState = 0;
if (GotBadPacket != null) GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
break;
case 3:
if (rxbuf[0] == MAGIC_WORD4)
{
syncState++;
tempMsg.WriteByte(rxbuf[0]);
}
else
{
syncState = 0;
if (GotBadPacket != null) GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 12, new AsyncCallback(OnReadBytesComplete), null);
break;
case 4:
tempMsg.Write(rxbuf, 0, bytesRead);
if (tempMsg.Position == HEADER_SIZE) //got everything
{
syncState++;
BinaryReader br = new BinaryReader(new MemoryStream (tempMsgBuf,0,HEADER_SIZE));
//check to see the header....
BigToLittle(br.ReadUInt32()); //read the header
msgSize = Convert.ToInt32(BigToLittle(br.ReadUInt32()));
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, msgSize, new AsyncCallback(OnReadBytesComplete), null);
}
else //read the rest....
{
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 12 - bytesRead, new AsyncCallback(OnReadBytesComplete), null);
}
break;
case 5:
tempMsg.Write(rxbuf, 0, bytesRead);
if (tempMsg.Position >= (msgSize + HEADER_SIZE)) //got everything
{
syncState=0;
if (tempMsg.Position > (msgSize + HEADER_SIZE))
if (GotBadPacket != null) GotBadPacket(this, new DrawDataPacketErrEventArgs(0, this));
if (GotDataPacket != null)
{
IbeoPacket p = new IbeoPacket();
TimeStampPacket tsp;
if (tsPacketBufferList.Count !=0 )
tsp = tsPacketBufferList.Peek();
else
{
tsp = new TimeStampPacket ();
tsp.CarTimeSeconds = 0;
tsp.CarTimeTicks = 0;
tsp.SeqNumber = 0;
}
totalPackets++;
totalPacketsResetable++;
DataPoint[] d = processIbeoPacket(ref p, tempMsg);
if (d!=null)
GotDataPacket(this, new DrawDataPacketEventArgs(d, this, tsp));
if (p.DataType == FILE_TYPE_COMPRESSED_SCAN) //we got a data packet
{
//Console.WriteLine("SCAN");
//if (tsPacketBuffer.SeqNumber == 0)
TimeStampPacket ts;
if (tsPacketBufferList.Count>0)
ts = tsPacketBufferList.Dequeue();
else
System.Console.WriteLine("WARNING: LOST SEQ NUMBER!");
}
}
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, 1, new AsyncCallback(OnReadBytesComplete), null);
}
else
{
AsyncReadResult = tcpClient.GetStream().BeginRead(rxbuf, 0, msgSize - bytesRead, new AsyncCallback(OnReadBytesComplete), null);
}
break;
}
}
