public void Send(int packetType, int firstByte, byte[] packet) {
if (!serial.IsOpen)
return;
int count = packet.Length;
Escaper outBuff = new Escaper(count + 7);
outBuff.nextByte(packetType);
outBuff.nextByte(firstByte);
for (int i = 0; i < count; i++) {
outBuff.nextByte(packet[i]);
}
int crc = outBuff.crc;
outBuff.nextByte(crc & 0xff);
outBuff.nextByte(crc >> 8);
outBuff.terminate();
byte[] frame = new byte[outBuff.escapePtr];
Array.Copy(outBuff.escaped, 0, frame, 0, outBuff.escapePtr);
serial.Write(frame, 0, frame.Length);
//Console.Write("\nSENT: " + BitConverter.ToString(frame, 0, frame.Length) + "\n");
}