示例#1
0
        public void DecodePacket(object sender, DataReceivedArgs e)
        {
            foreach (byte b in e.Data)
            {
                switch (rcvState)
                {
                case ReceptionStates.Waiting:
                    if (b == 0xFF)
                    {
                        rcvState = ReceptionStates.sof2;
                    }
                    break;

                case ReceptionStates.sof2:
                    if (b == 0xFF)
                    {
                        rcvState = ReceptionStates.packetSize;
                    }
                    break;

                case ReceptionStates.packetSize:
                    packetSize = b;
                    packetData = new byte[packetSize - 7];     //init to the data size only, -(status error, detail)
                    rcvState   = ReceptionStates.pID;
                    break;

                case ReceptionStates.pID:
                    pID      = b;
                    rcvState = ReceptionStates.CMD;
                    break;

                case ReceptionStates.CMD:
                    cmd      = b;
                    rcvState = ReceptionStates.checkSum1;
                    break;

                case ReceptionStates.checkSum1:
                    checkSum1 = b;
                    rcvState  = ReceptionStates.checkSum2;
                    break;

                case ReceptionStates.checkSum2:
                    checkSum2 = b;
                    rcvState  = ReceptionStates.data;
                    break;

                case ReceptionStates.data:
                    if (packetDataByteIndex < packetData.Length)
                    {
                        packetData[packetDataByteIndex] = b;
                        packetDataByteIndex++;
                    }

                    if (!(packetDataByteIndex < packetData.Length))
                    {
                        packetDataByteIndex = 0;

                        byte clcChksum1 = CommonMethods.CheckSum1(packetSize, (byte)pID, cmd, packetData);
                        byte clcChksum2 = CommonMethods.CheckSum2(clcChksum1);

                        if (checkSum1 == clcChksum1 && checkSum2 == clcChksum2)
                        {
                            byte statusError  = packetData[packetData.Length - 2];
                            byte statusDetail = packetData[packetData.Length - 1];

                            //Console.WriteLine("Fin decodage Packet : " + DateTime.Now.Second + "." + DateTime.Now.Millisecond);
                            ProcessPacket(packetSize, (ServoId)pID, cmd, checkSum1, checkSum2, packetData, statusError, statusDetail);
                            OnAnyAckReveived((ServoId)pID, statusError, statusDetail);
                        }
                        rcvState = ReceptionStates.Waiting;
                    }
                    break;
                }
            }
        }
示例#2
0
        public void DecodePacket(object sender, DataReceivedArgs e)
        {
            foreach (byte b in e.Data)
            {
                switch (rcvState)
                {
                case ReceptionStates.Waiting:
                    if (b == 0xFF)
                    {
                        rcvState = ReceptionStates.sof2;
                    }
                    break;

                case ReceptionStates.sof2:
                    if (b == 0xFF)
                    {
                        rcvState = ReceptionStates.packetSize;
                    }
                    break;

                case ReceptionStates.packetSize:
                    packetSize = b;
                    packetData = new byte[packetSize - 7];     //init to the data size only, -(status error, detail)
                    rcvState   = ReceptionStates.pID;
                    break;

                case ReceptionStates.pID:
                    pID      = b;
                    rcvState = ReceptionStates.CMD;
                    break;

                case ReceptionStates.CMD:
                    cmd      = b;
                    rcvState = ReceptionStates.checkSum1;
                    break;

                case ReceptionStates.checkSum1:
                    checkSum1 = b;
                    rcvState  = ReceptionStates.checkSum2;
                    break;

                case ReceptionStates.checkSum2:
                    checkSum2 = b;
                    rcvState  = ReceptionStates.data;
                    break;

                case ReceptionStates.data:
                    if (packetDataByteIndex < packetData.Length)
                    {
                        packetData[packetDataByteIndex] = b;
                        packetDataByteIndex++;
                    }

                    if (!(packetDataByteIndex < packetData.Length))
                    //if (packetDataByteIndex == packetData.Length)
                    {
                        packetDataByteIndex = 0;

                        byte clcChksum1 = CommonMethods.CheckSum1(packetSize, pID, cmd, packetData);
                        byte clcChksum2 = CommonMethods.CheckSum2(clcChksum1);

                        if (checkSum1 == clcChksum1 && checkSum2 == clcChksum2)
                        {
                            byte statusError  = packetData[packetData.Length - 2];
                            byte statusDetail = packetData[packetData.Length - 1];
                            ProcessPacket(packetSize, pID, cmd, checkSum1, checkSum2, packetData, statusError, statusDetail);
                        }
                        else
                        {
                            OnCheckSumErrorOccured(pID, checkSum1, checkSum2);
                        }

                        rcvState = ReceptionStates.Waiting;
                    }
                    break;
                }
            }
        }