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; } }