private void TSReceiveCallback(IAsyncResult ar)
{
try
{
IPEndPoint e = new IPEndPoint(IPAddress.Any, 0);
Byte[] receiveBytes = udpClientRX.EndReceive(ar, ref e);
udpClientRX.BeginReceive(new AsyncCallback(TSReceiveCallback), ar.AsyncState);
//we got a new timing packet, throw it on the stack
BinaryReader br = new BinaryReader(new MemoryStream(receiveBytes));
UInt16 seconds = BigToLittle(br.ReadUInt16());
UInt32 ticks = BigToLittle(br.ReadUInt32());
UInt32 seq = BigToLittle(br.ReadUInt32());
TimeStampPacket tsp = new TimeStampPacket();
tsp.SeqNumber = seq;
tsp.CarTimeTicks = ticks;
tsp.CarTimeSeconds = seconds;
//tsPacketBuffer = tsp;
tsPacketBufferList.Enqueue(tsp);
//Console.WriteLine("UDP");
if (seqNumExp == -1) //firstpacket
{
seqNumExp = (int)seq;
Console.WriteLine("INIT: Seq Num! GOT: " + seq + " SET: " + seqNumExp);
}
else if (seqNumExp != seq)
{
Console.WriteLine("WARNING: Seq mismatch! GOT: " + seq + " EXP: " + seqNumExp);
seqNumExp = (int)seq;
}
seqNumExp++;
}
catch (SocketException e)
{
Console.WriteLine("SOCKET EXCEPTION IN IBEO: " + e.Message);
}
}
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;
}
}
public DrawDataPacketEventArgs(DataPoint[] data, ISensor lidar, TimeStampPacket tsp)
{
this.data = data;
this.lidar = lidar;
this.tsp = tsp;
}
public DrawDataPacketEventArgs(DataPoint[] data, ISensor lidar, TimeStampPacket tsp)
{
this.data = data;
this.lidar = lidar;
this.tsp = tsp;
}
private void TSReceiveCallback(IAsyncResult ar)
{
try
{
IPEndPoint e = new IPEndPoint(IPAddress.Any, 0);
Byte[] receiveBytes = udpClientRX.EndReceive(ar, ref e);
udpClientRX.BeginReceive(new AsyncCallback(TSReceiveCallback), ar.AsyncState);
//we got a new timing packet, throw it on the stack
BinaryReader br = new BinaryReader(new MemoryStream(receiveBytes));
UInt16 seconds = BigToLittle(br.ReadUInt16());
UInt32 ticks = BigToLittle(br.ReadUInt32());
UInt32 seq = BigToLittle(br.ReadUInt32());
TimeStampPacket tsp = new TimeStampPacket();
tsp.SeqNumber = seq;
tsp.CarTimeTicks = ticks;
tsp.CarTimeSeconds = seconds;
//tsPacketBuffer = tsp;
tsPacketBufferList.Enqueue(tsp);
//Console.WriteLine("UDP");
if (seqNumExp == -1) //firstpacket
{
seqNumExp = (int)seq;
Console.WriteLine("INIT: Seq Num! GOT: " + seq + " SET: " + seqNumExp);
}
else if (seqNumExp != seq)
{
Console.WriteLine("WARNING: Seq mismatch! GOT: " + seq + " EXP: " + seqNumExp);
seqNumExp = (int)seq;
}
seqNumExp++;
}
catch (SocketException e)
{
Console.WriteLine("SOCKET EXCEPTION IN IBEO: " + e.Message);
}
}
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;
}
}