/// <summary>
/// Sends a packet to the arduino.
/// </summary>
/// <param name="cmd">The command number.</param>
/// <param name="buffer">The bytearray to be sent.</param>
public void Send(int cmd, ArduinoDataType dataType, byte[] buffer)
{
// build serial packet
int checksum = Crc16.ComputeChecksum(buffer);
byte[] serialPacket = new byte[buffer.Length + headerSize];
serialPacket[0] = (byte)(buffer.Length & 0xff);
serialPacket[1] = (byte)((buffer.Length >> 8) & 0xff);
serialPacket[2] = (byte)(checksum & 0xff);
serialPacket[3] = (byte)((checksum >> 8) & 0xff);
serialPacket[4] = (byte)cmd;
serialPacket[5] = (byte)dataType;
Array.Copy(buffer, 0, serialPacket, headerSize, buffer.Length);
// encode packet
byte[] encodedBuffer = new byte[COBS.GetEncodedBufferSize(serialPacket.Length)];
int encodedBufferSize = COBS.Encode(ref serialPacket, serialPacket.Length, ref encodedBuffer);
Array.Resize(ref encodedBuffer, encodedBufferSize + 1);
encodedBuffer[encodedBufferSize] = 0;
// add msg to outgoing queue
lock (sendLockObject)
{
sendPackets.Enqueue(encodedBuffer);
}
}