public override void Parse(byte data)
{
switch (parseState)
{
case PARSE_STATES.IDLE:
if (data == 0xFE)
{
parseState = PARSE_STATES.PREAMBLE;
}
break;
case PARSE_STATES.PREAMBLE:
parseBuffer.Enqueue(data);
pBytes = data & 0x0F;
msgLength = pBytes + 3;
parseState = PARSE_STATES.LENGTH;
break;
case PARSE_STATES.LENGTH:
parseBuffer.Enqueue(data);
if (pBytes > 0)
{
parseState = PARSE_STATES.MESSAGEID;
}
else
{
parseState = PARSE_STATES.PAYLOAD;
}
break;
case PARSE_STATES.MESSAGEID:
parseBuffer.Enqueue(data);
pBytes--;
if (pBytes < 1)
{
parseState = PARSE_STATES.PAYLOAD;
}
break;
case PARSE_STATES.PAYLOAD:
parseBuffer.Enqueue(data);
parseState = PARSE_STATES.IDLE;
byte[] packetBytes = parseBuffer.ToArray();
byte crc_calc = CalculateCRC8(packetBytes, 0, msgLength - 1);
if (crc_calc == data)
{
NotifyNewPacketReceived(packetBytes);
}
else
{
// Some error
}
ResetParser();
break;
default:
break;
}
}
void ResetParser()
{
parseState = PARSE_STATES.IDLE;
parseBuffer.Clear();
}
