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