/// <summary>
///
/// </summary>
private void MessageParser()
{
foreach (byte receiverByte in ReadReceiver(MininumMessageSize))
{
if (_ubxState != UBXState.Start)
{
ParseUBX(receiverByte);
}
else if (receiverByte == 0xB5)
{
_ubxState = UBXState.Start;
ParseUBX(receiverByte);
}
else if (_nmeaState != NMEAState.Start)
{
ParseNMEA(receiverByte);
}
else if (receiverByte == '$')
{
_nmeaState = NMEAState.Start;
ParseNMEA(receiverByte);
}
else
{
// Delay if bytes (0xFF) are being disguarded
Task.Delay(PollingDelay);
}
}
}
/// <summary>
///
/// </summary>
/// <param name="receiverByte"></param>
private void ParseUBX(byte receiverByte)
{
switch (_ubxState)
{
case UBXState.Start:
_parserMessage.SetLength(0);
_ubxCrcA = 0x00;
_ubxCrcB = 0x00;
_ubxState = UBXState.Sync2;
break;
case UBXState.Sync2:
if (receiverByte == 0x62)
{
_ubxState = UBXState.Class;
}
else
{
Debug.WriteLine("UBX Message Resetting: Sync2 failed");
_ubxState = UBXState.Start;
return;
}
break;
case UBXState.Class:
_ubxCrcA += receiverByte;
_ubxCrcB += _ubxCrcA;
_ubxState = UBXState.ID;
break;
case UBXState.ID:
_ubxCrcA += receiverByte;
_ubxCrcB += _ubxCrcA;
_ubxState = UBXState.Length1;
break;
case UBXState.Length1:
_ubxCrcA += receiverByte;
_ubxCrcB += _ubxCrcA;
_ubxLength = receiverByte;
_ubxState = UBXState.Length2;
break;
case UBXState.Length2:
_ubxCrcA += receiverByte;
_ubxCrcB += _ubxCrcA;
_ubxLength += (ushort)(receiverByte << 8);
_ubxState = UBXState.Payload;
break;
case UBXState.Payload:
_ubxCrcA += receiverByte;
_ubxCrcB += _ubxCrcA;
if (_ubxLength + 5 == _parserMessage.Position)
{
_ubxState = UBXState.ChecksumA;
}
if (_parserMessage.Position >= 1022)
{
Debug.WriteLine("UBX Message Resetting: Payload is too large");
_ubxState = UBXState.Start;
return;
}
break;
case UBXState.ChecksumA:
if (_ubxCrcA == receiverByte)
{
_ubxState = UBXState.ChecksumB;
}
else
{
Debug.WriteLine("UBX Message Resetting: Checksum A failed");
_ubxState = UBXState.Start;
return;
}
break;
case UBXState.ChecksumB:
if (_ubxCrcB == receiverByte)
{
_ubxState = UBXState.End;
}
else
{
Debug.WriteLine("UBX Message Resetting: Checksum B failed");
_ubxState = UBXState.Start;
return;
}
break;
}
_parserMessage.WriteByte(receiverByte);
if (_ubxState == UBXState.End)
{
// Sending the received message after getting the last checksum
byte[] message = _parserMessage.ToArray();
IMessageResult messageObject = _messageFactory.Invoke(message);
messageObject.TryParse(message);
MessageReceived?.Invoke(this, new MessageReceivedEventArgs(message, messageObject, MessageProtocol.UBX));
_ubxState = UBXState.Start;
}
}
