/// <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="recevierByte"></param>
private void ParseNMEA(byte recevierByte)
{
switch (_nmeaState)
{
case NMEAState.Start:
_parserMessage.SetLength(0);
_nmeaChecksum = 0;
_nmeaCrc = 0;
_nmeaState = NMEAState.Body;
break;
case NMEAState.Body:
if (recevierByte == '*')
{
_nmeaState = NMEAState.ChecksumA;
}
else if (recevierByte == '\r')
{
Debug.WriteLine("NMEA Message Resetting: Body terminated without checksum");
_nmeaState = NMEAState.LF;
}
else
{
_nmeaChecksum ^= recevierByte;
}
break;
case NMEAState.ChecksumA:
_nmeaCrc = (byte)(ByteToHex(recevierByte) << 4);
_nmeaState = NMEAState.ChecksumB;
break;
case NMEAState.ChecksumB:
_nmeaCrc |= ByteToHex(recevierByte);
if (_nmeaChecksum == _nmeaCrc)
{
_nmeaState = NMEAState.CR;
}
else
{
Debug.WriteLine("NMEA Message Resetting: Checksum failed");
_nmeaState = NMEAState.Start;
return;
}
break;
case NMEAState.CR:
if (recevierByte == '\r')
{
_nmeaState = NMEAState.LF;
}
else
{
Debug.WriteLine("NMEA Message Resetting: CR failed");
_nmeaState = NMEAState.Start;
return;
}
break;
case NMEAState.LF:
if (recevierByte == '\n')
{
_nmeaState = NMEAState.End;
}
else
{
Debug.WriteLine("NMEA Message Resetting: LF failed");
_nmeaState = NMEAState.Start;
return;
}
break;
}
_parserMessage.WriteByte(recevierByte);
if (_nmeaState == NMEAState.End)
{
// Sending the received message after getting the last line feed
MessageReceived?.Invoke(this, new MessageReceivedEventArgs(_parserMessage.ToArray(), null, MessageProtocol.NEMA));
_nmeaState = NMEAState.Start;
}
}
public void ProcessNMEA(byte btData)
{
switch(nState)
{
///////////////////////////////////////////////////////////////////////
// Search for start of message '$'
case NMEAState.NP_STATE_SOM :
if(btData == '$')
{
checksum = 0; // reset checksum
index = 0; // reset index
nState = NMEAState.NP_STATE_CMD;
}
break;
///////////////////////////////////////////////////////////////////////
// Retrieve command (NMEA Address)
case NMEAState.NP_STATE_CMD :
if(btData != ',' && btData != '*')
{
command[index++] = btData;
checksum ^= btData;
// Check for command overflow
if(index >= NP_MAX_CMD_LEN)
{
nState = NMEAState.NP_STATE_SOM;
}
}
else
{
command[index] = (byte)'\0'; // terminate command
checksum ^= btData;
index = 0;
nState = NMEAState.NP_STATE_DATA; // goto get data state
}
break;
///////////////////////////////////////////////////////////////////////
// Store data and check for end of sentence or checksum flag
case NMEAState.NP_STATE_DATA:
if(btData == '*') // checksum flag?
{
data[index] = (byte)'\0';
nState = NMEAState.NP_STATE_CHECKSUM_1;
}
else // no checksum flag, store data
{
//
// Check for end of sentence with no checksum
//
if(btData == '\r')
{
data[index] = (byte) '\0';
ProcessCommand(EncodeToString( command), data);
nState = NMEAState.NP_STATE_SOM;
return;
}
//
// Store data and calculate checksum
//
checksum ^= btData;
data[index] = btData;
if(++index >= NP_MAX_DATA_LEN) // Check for buffer overflow
{
nState = NMEAState.NP_STATE_SOM;
}
}
break;
///////////////////////////////////////////////////////////////////////
case NMEAState.NP_STATE_CHECKSUM_1:
if( (btData - '0') <= 9)
{
receivedChecksum = (byte)((btData - '0') << 4);
}
else
{
receivedChecksum = (byte)((btData - 'A' + 10) << 4);
}
nState = NMEAState.NP_STATE_CHECKSUM_2;
break;
///////////////////////////////////////////////////////////////////////
case NMEAState.NP_STATE_CHECKSUM_2:
if( (btData - '0') <= 9)
{
receivedChecksum |= (byte)((btData - (byte)'0'));
}
else
{
receivedChecksum |= (byte)((btData - (byte)'A' + 10));
}
if(checksum == receivedChecksum)
{
ProcessCommand(EncodeToString(command), data);
}
nState = NMEAState.NP_STATE_SOM;
break;
///////////////////////////////////////////////////////////////////////
default :
nState = NMEAState.NP_STATE_SOM;
break;
}
}
