Ejemplo n.º 1
0
 private void UDP_create_pkt(ref UDP_Data inD)
 {
     inD.frm_Id   = frameID;
     inD.frm_Idx  = frameIdx;
     inD.pkt_Id   = pktID;
     inD.pkt_cmd  = pktCmd;
     inD.pkt_leng = (byte)pktleng;
     inD.payload  = new byte[arrPktData.Length];
     arrPktData.CopyTo(inD.payload, 0);
     inD.pkt_cs = pktCS;
     inD.pkt_OK = true;
 }
Ejemplo n.º 2
0
        public void UDP_RcvHandle(byte[] rcvArr)
        {
            UDP_Data ret = UDP_initData();

            int  leng  = rcvArr.Length;
            int  add   = 0;
            byte u8Val = 0;


            while (leng != add)
            {
                switch (sts)
                {
                case Rcv_State.eWAIT_FRM_ID:
                    u8Val = rcvArr[add++];
                    switch (u8Val)
                    {
                    case cUDP_CMD.uFRM_ID_NORMAL:
                    case cUDP_CMD.uFRM_ID_DEBUG:
                        UDP_initRcvParam();
                        ret     = UDP_initData();
                        frameID = u8Val;
                        sts     = Rcv_State.eWAIT_FRM_IDX;
                        break;

                    default:
                        break;
                    }
                    break;

                case Rcv_State.eWAIT_FRM_IDX:
                    u8Val = rcvArr[add++];
                    switch (u8Val)
                    {
                    case cUDP_CMD.uFRM_IDX_NORMAL:
                    case cUDP_CMD.uFRM_IDX_RES1:
                        frameIdx = u8Val;
                        sts      = Rcv_State.eWAIT_PKT_ID;
                        break;

                    default:
                        sts = Rcv_State.eWAIT_FRM_ID;
                        break;
                    }
                    break;

                case Rcv_State.eWAIT_PKT_ID:
                    u8Val = rcvArr[add++];
                    // filtering packet id
                    pktID = u8Val;
                    sts   = Rcv_State.eWAIT_PKT_CMD;
                    break;

                case Rcv_State.eWAIT_PKT_CMD:
                    u8Val = rcvArr[add++];

                    switch (u8Val)
                    {
                    case cUDP_CMD.uCMD_PID_GET_PARAM:
                    case cUDP_CMD.uCMD_IMU_GET_VALUE:
                    case cUDP_CMD.uCMD_SYS_ARRIVE:
                        pktCmd = u8Val;
                        sts    = Rcv_State.eWAIT_PKT_LENG;
                        break;

                    default:
                        sts = Rcv_State.eWAIT_FRM_ID;
                        break;
                    }
                    break;

                case Rcv_State.eWAIT_PKT_LENG:
                    u8Val   = rcvArr[add++];
                    pktleng = u8Val;

                    if (pktleng == 0)
                    {
                        sts = Rcv_State.eWAIT_CS;
                    }
                    else
                    {
                        sts = Rcv_State.eONGOING_DATA;
                    }

                    arrPktData = new byte[pktleng];
                    idxData    = 0;
                    break;

                case Rcv_State.eONGOING_DATA:
                    arrPktData[idxData++] = rcvArr[add++];

                    if (idxData == pktleng)
                    {
                        sts = Rcv_State.eWAIT_CS;
                    }

                    break;

                case Rcv_State.eWAIT_CS:
                    byte cs = CS_Calculation(rcvArr, pktleng + 5);
                    pktCS = rcvArr[add++];


                    Console.WriteLine("cs1: " + pktCS + ", cs2: " + cs);
                    if (cs == pktCS)
                    {
                        UDP_create_pkt(ref ret);
                        Console.WriteLine(ret.pkt_cmd);
                        Console.WriteLine(ret.pkt_leng);
                        qPacket.Enqueue(ret);

                        if (qPacket.Count > 100)
                        {
                            initQueue();
                        }
                    }
                    sts = Rcv_State.eWAIT_FRM_ID;
                    break;
                } // end switch
            }     // end while
        }