public void ParseData(string packetStr)
{
Debug.WriteLine("New packet:\t string:" + packetStr);
for (int i = 0; i < packetStr.Length; i++)
{
//Simple state-machine
if (packetStr[i] == 'T')
{
incomingPacket = new Packet { RawString = packetStr };
currentField = PacketFields.Type;
}
else if (packetStr[i] == 'N')
currentField = PacketFields.NodeID;
else if (packetStr[i] == 'I')
{
if (incomingPacket.PacketType == PacketTypes.Command || incomingPacket.PacketType == PacketTypes.Command_Reply)
currentField = PacketFields.CommandID;
else if (incomingPacket.PacketType == PacketTypes.Data_Array_Request || incomingPacket.PacketType == PacketTypes.Data_Int || incomingPacket.PacketType == PacketTypes.Data_Request)
{
currentField = PacketFields.SensorID;
}
}
else if (packetStr[i] == 'P')
currentField = PacketFields.Payload;
else if (packetStr[i] == 'Q')
currentField = PacketFields.Parity;
else
{
switch (currentField)
{
case PacketFields.Type:
incomingPacket.PacketType = (PacketTypes)packetStr.Substring(i, 2).FromHexStringToInt();
i++;
break;
case PacketFields.NodeID:
incomingPacket.NodeID = packetStr.Substring(i, 2).FromHexStringToInt();
i++;
break;
case PacketFields.SensorID:
incomingPacket.SensorID = packetStr.Substring(i, 2).FromHexStringToInt();
i++;
break;
case PacketFields.CommandID:
incomingPacket.CommandID = (Commands)packetStr.Substring(i, 2).FromHexStringToInt();
i++;
break;
case PacketFields.Payload:
incomingPacket.Payload = packetStr.Substring(i, 2).FromHexStringToInt();
i++;
break;
case PacketFields.Parity:
incomingPacket.Parity = packetStr.Substring(i, 2).FromHexStringToInt();
i = packetStr.Length; //we're done with this packet
if (SerialMessageReceived != null && ComputeParity() == incomingPacket.Parity) //&& parity klopt
SerialMessageReceived(this, new SerialArduinoMessageEventArgs(incomingPacket));
else
{
Debug.WriteLine("Parity failed");
}
break;
default:
throw new ArgumentOutOfRangeException();
}
}
}
}