public NativeCAN()
{
handle = FRC_CAN_Reader_Native_Create();
readThread = new Thread(() =>
{
var eventHandle = FRC_CAN_Reader_Native_GetEventHandle(handle);
while (true)
{
int dataLen = 0;
if (WPI_WaitForObjectTimeout(eventHandle, 1, &dataLen) != 0)
{
dataLen = 0;
CANData *messages = FRC_CAN_Reader_Native_ReadMessages(handle, &dataLen);
if (messages == null)
{
continue;
}
for (int i = 0; i < dataLen; i++)
{
CANData data = messages[i];
ulong ulongData = Unsafe.ReadUnaligned <ulong>(data.data);
CANMessageReceived?.Invoke((uint)data.id, ulongData, data.length, (uint)data.timestamp);
}
}
}
});
readThread.IsBackground = true;
readThread.Name = "CAN Managed Reader";
readThread.Start();
}
private void ParseCANMessage(string dataLine)
{
if (string.IsNullOrEmpty(dataLine))
{
throw new ArgumentException();
}
switch (dataLine[0])
{
case 'T':
case 'R':
//Extended frame
CANMessage msg = new CANMessage();
if (dataLine.Length >= 10)
{
//parse address (8 HEX characters)
msg.IsExtended = true;
msg.IsRTR = dataLine[0] == 'R';
msg.Id = UInt32.Parse(dataLine.Substring(1, 8), System.Globalization.NumberStyles.HexNumber);
int dataLength = UInt16.Parse(dataLine.Substring(9, 1), System.Globalization.NumberStyles.HexNumber);
if (!msg.IsRTR && dataLength > 0)
{
//parse data
if (dataLine.Length == 10 + dataLength * 2)
{
msg.Data = dataLine.Substring(10, dataLength * 2).HexStringToByteArray();
}
else
{
//expected more or less data than present
throw new FormatException();
}
}
else
{
msg.Data = new byte[0];
}
if (Verbose)
{
Console.WriteLine("--> {0}", msg.ToString());
}
CANMessageReceived?.Invoke(this, new CANMessageReceivedEventArgs()
{
CANMessage = msg
});
}
else
{
//Ignore illegal format
}
break;
case 't':
case 'r':
//Basic frame
msg = new CANMessage();
if (dataLine.Length >= 5)
{
//parse address (3 HEX characters)
msg.IsExtended = false;
msg.IsRTR = dataLine[0] == 'r';
msg.Id = UInt32.Parse(dataLine.Substring(1, 3), System.Globalization.NumberStyles.HexNumber);
int dataLength = UInt16.Parse(dataLine.Substring(4, 1), System.Globalization.NumberStyles.HexNumber);
if (!msg.IsRTR && dataLength > 0)
{
//parse data
if (dataLine.Length == 5 + dataLength * 2)
{
msg.Data = dataLine.Substring(5, dataLength * 2).HexStringToByteArray();
}
else
{
//expected more or less data than present
throw new FormatException();
}
}
else
{
msg.Data = new byte[0];
}
}
else
{
throw new FormatException();
}
CANMessageReceived?.Invoke(this, new CANMessageReceivedEventArgs()
{
CANMessage = msg
});
break;
default:
throw new ArgumentException();
}
}
