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