private void ReadPacket(SerialControl serial)
{
byte idx = serial.ReadByte();
if (connEstablished)
{
if (idx != ++recvIdx)
{
Logger.Log($"Packet Drop. Got {idx}, Expected {recvIdx}");
ResetConn();
return;
}
}
int len = serial.ReadByte();
if (!connEstablished)
{
if (len != KeepAliveLength)
{
return;
}
recvIdx = idx;
}
var arr = new byte[len];
serial.ReadBytes(arr);
byte crcRecv = serial.ReadByte();
byte crcCheck = CRC.crc_update(0, idx);
crcCheck = CRC.crc_update(crcCheck, (byte)len);
crcCheck = CRC.crc_update(crcCheck, arr, 0, arr.Length);
if (crcRecv != crcCheck)
{
if (connEstablished)
{
Logger.Log($"CRC Failed. Recieved {crcRecv:02X}, Calculated {crcCheck:02X}");
ResetConn();
}
return;
}
HandlePacket(new BinaryReader(new MemoryStream(arr)));
}
private void SerialProcessor(SerialControl serial)
{
#if SerialPortStream
byte[] buf = new byte[2048];
serial.port.InvokeEventsOnThreadPool = false;
serial.port.DataReceived += (s, args) => {
if (args.EventType == SerialData.Chars)
{
}
int len = serial.port.Read(buf, 0, buf.Length);
for (int i = 0; i < len; i++)
{
ReadByte(buf[i]);
}
};
#else
try {
var sb = new StringBuilder();
while (true)
{
byte b = serial.ReadByte();
if (b == '!')
{
ReadPacket(serial);
}
else if (!connEstablished)
{
}
else if (b == '\r')
{
}
else if (b == '\n')
{
var s = sb.ToString();
sb.Clear();
Logger.LogDirect(s);
}
else
{
sb.Append((char)b);
}
}
}
catch (OperationCanceledException) {}
#endif
}