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;
 }
Ejemplo n.º 4
0
 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;
            }
        }