private static unsafe bool HandlePacket(byte *offset, byte *end, out int increment)
{
Packet.Header *header = (Packet.Header *)offset;
increment = -1;
UInt16 checksum = CRC16.Generate(offset + Marshal.SizeOf(header->HeaderChecksum), Marshal.SizeOf(*header) - Marshal.SizeOf(header->HeaderChecksum));
DebugTools.Print($"Header: HeaderChecksum={header->HeaderChecksum.ToString("X4")}, DataChecksum={header->DataChecksum.ToString("X4")}, Type={header->Type}, Size={header->Size} (Checksum={checksum.ToString("X4")}, Hex={ToHexString(offset, Marshal.SizeOf(*header))})");
if (!Packet.VerifyHeader(header))
{
DebugTools.Print($"Header checksum failed: {header->HeaderChecksum.ToString("X4")} / {checksum.ToString("X4")}");
return(false);
}
if (offset + (Marshal.SizeOf(*header) + header->Size) > end)
{
increment = 0;
return(false);
}
checksum = CRC16.Generate(offset + Marshal.SizeOf(*header), header->Size);
DebugTools.Print($"Data: DataChecksum={header->DataChecksum.ToString("X4")}, Size={header->Size} (Checksum={checksum.ToString("X4")}, Hex={ToHexString(offset + Marshal.SizeOf(*header), header->Size)})");
if (!Packet.VerifyData(header))
{
DebugTools.Print($"Content checksum failed: {header->DataChecksum.ToString("X4")} / {checksum.ToString("X4")}");
return(false);
}
increment = Marshal.SizeOf(*header) + header->Size;
OnPacketReceivedEvent?.Invoke(header);
return(true);
}
private unsafe void PacketReceived(Packet.Header *header)
{
switch (header->Type)
{
case (byte)CustomPackets.Type.OrientationData:
{
Orientation = *((CustomPackets.OrientationDataPacket *)header);
DebugTools.Print($"DATA: {Orientation}");
break;
}
case (byte)CustomPackets.Type.ProximityData:
{
ProximityData data = *((CustomPackets.ProximityDataPacket *)header);
DebugTools.Print($"DATA: {data}");
if (data.ID < 0 || data.ID >= ProximityContent.Length)
{
DebugTools.Print($"Invalid sensor index ID: {data.ID}");
return;
}
int count = ProximityContent[data.ID].Count;
int cap = m_ContentCapacity - 1;
for (; count-- > cap;)
{
ProximityContent[data.ID].RemoveAt(0);
}
ProximityContent[data.ID].Add(data);
break;
}
default:
{
DebugTools.Print($"Unknown packet type: {header->Type}");
break;
}
}
}
