private void OnNewByte(byte b)
{
switch (rxState)
{
//wait for start byte 0
case 0:
if (b == 0x24)
{
curMsgTimestamp = timingProvider.GetCurrentTime();
rxState = 1;
}
else
{
Console.WriteLine("Septentrio Serial Parser: Resync");
}
break;
//wait for start byte 1
case 1:
if (b == 0x40)
{
headerByteCount = 0;
rxState = 2;
}
else if (b == 0x24)
{
rxState = 1;
}
else
{
rxState = 0;
}
break;
//the next 6 bytes are part of the fixed header.
case 2:
headerBytes[headerByteCount] = b;
headerByteCount++;
if (headerByteCount == 6)
{
BinaryReader br = new BinaryReader(new MemoryStream(headerBytes));
curMsgCRC = br.ReadUInt16();
UInt16 temp = br.ReadUInt16();
curMsgLen = br.ReadUInt16();
curMsgID = (UInt16)(temp & 0x1FFF);
curMsgIDRev = (Byte)(temp >> 13);
//for the unique case of a 0 length message
if (curMsgLen > 0)
{
//allocate the message and copy the header bytes
curMsg = new byte[curMsgLen];
curMsg[0] = 0x24; curMsg[1] = 0x40;
Array.Copy(headerBytes, 0, curMsg, 2, 6);
curMsgByteNum = 8;
rxState = 3;
}
else
{
Console.WriteLine("Septentrio Serial Parser: Warning: zero length message!");
rxState = 0;
}
}
break;
//we have the complete header, now begin receiving bytes until we reach message length
case 3:
curMsg[curMsgByteNum] = b;
curMsgByteNum++;
if (curMsgByteNum == curMsgLen)
{
if (CheckCRC(curMsg, curMsgCRC))
{
ProcessPacket(curMsgID, curMsgIDRev, curMsg, curMsgTimestamp);
}
else
{
Console.WriteLine("Septentrio Serial Parser: CRC Failure. Message Dropped!");
}
rxState = 0;
}
break;
default:
throw new InvalidOperationException("Septentrio Serial Parser: Fell threw state machine");
break;
}
}