private void ResetConn()
{
if (connEstablished)
{
Logger.Log("Invalidating Connection");
connEstablished = false;
OnConnStateChanged?.Invoke(false);
}
}
private void HandlePacket(BinaryReader r)
{
var id = (PacketId)r.ReadByte();
if (!connEstablished && id != PacketId.KeepAlive)
{
return;
}
switch (id)
{
case PacketId.KeepAlive:
int value = r.ReadInt32() ^ -1;
if (value != keepAlive)
{
if (connEstablished)
{
Logger.Log($"Keep Alive Mismatch. {value:X8} {keepAlive:X8}");
ResetConn();
}
return;
}
if (!connEstablished)
{
connEstablished = true;
Logger.Log("Connection Established.");
OnConnStateChanged?.Invoke(true);
}
keepAliveResponded = true;
SendLatencyResp();
break;
case PacketId.CommsReset:
ResetConn();
break;
case PacketId.Latency:
Latency = r.ReadUInt16();
OnLatencyChanged?.Invoke();
break;
case PacketId.SetServos:
SnekTime = r.ReadInt32() / 1000d;
ServoReading?.Invoke((sbyte)r.ReadByte(), (sbyte)r.ReadByte(), (sbyte)r.ReadByte(), (sbyte)r.ReadByte());
break;
case PacketId.SetServos2:
SnekTime = r.ReadInt32() / 1000d;
ServoReading?.Invoke(r.ReadInt16(), r.ReadInt16(), r.ReadInt16(), r.ReadInt16());
break;
case PacketId.StrainReadings:
SnekTime = r.ReadInt32() / 1000d;
TensionReading?.Invoke(r.ReadInt32() / 1000d, r.ReadInt32() / 1000d, r.ReadInt32() / 1000d, r.ReadInt32() / 1000d);
break;
default:
if (connEstablished)
{
Logger.Log($"Unknown Packet Id ({id})");
}
break;
}
}
