public byte[] GetBytes(ButtonMessage message)
{
//set the sequence
message.SequenceID = (byte)_sequenceID++;
//set checksum
message.Checksum = CalculateChecksum(message);
return(ConvertToByte(message));
}
ushort CalculateChecksum(ButtonMessage message)
{
//payload_len: The actual length of the packet
//subtracting 2 here, to remove checksum from len
//else we will end up computing checksum of the checksum
ushort checksum = 0;
checksum += (ushort)message.PacketID;
checksum += (ushort)message.ErrorID;
foreach (var d in message.Payload)
{
checksum += (ushort)d;
}
return(checksum);
}
ButtonMessage ParseMessage()
{
var result = new ButtonMessage();
int index = 0;
result.StartOfFrame = packet_buffer[index++];
if (result.StartOfFrame != SOP)
{
return(null); //Invalid SOP
}
result.PacketID = packet_buffer[index++];
result.ErrorID = packet_buffer[index++];
result.SequenceID = packet_buffer[index++];
int index2 = 0;
result.Payload = new byte[packet_size - index - 3];
for (; index < packet_size - 3; index++)
{
result.Payload[index2++] = packet_buffer[index];
}
result.EndOfFrame = packet_buffer[index + 2];
if (result.EndOfFrame != EOP)
{
return(null); //Invalid EOP
}
//correct checksum (to big endian)
result.Checksum = BitConverter.ToUInt16(new byte[] { packet_buffer[index + 1], packet_buffer[index] }, 0);
//verify checksum
var check = CalculateChecksum(result);
if (check != result.Checksum)
{
return(null); //Invalid Checksum
}
return(result);
}
Byte[] ConvertToByte(ButtonMessage message)
{
var index = 0;
var result = new byte[message.Payload.Length + 7];
result[index++] = message.StartOfFrame;
result[index++] = message.PacketID;
result[index++] = message.ErrorID;
result[index++] = message.SequenceID;
foreach (var data in message.Payload)
{
result[index++] = data;
}
var checksum = BitConverter.GetBytes(message.Checksum);
result[index++] = checksum[1];
result[index++] = checksum[0];
result[index++] = message.EndOfFrame;
return(result);
}
