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