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
}