private void ReadOnThread()
{
while (ReadEnable)
{
// Read
Tuple <uint, byte[]> CanRead = CANBus.Read();
byte priority = Convert.ToByte(((CanRead.Item1) >> 8) & 0x01);
byte sender = Convert.ToByte(((CanRead.Item1) >> 4) & 0x0F);
byte receiver = Convert.ToByte((CanRead.Item1) & 0x0F);
ArmPacket newPack = new ArmPacket
{
TargetDeviceID = (Device)sender,
Priority = priority == 0 ? false : true,
PacketType = (CANPacket)CanRead.Item2[0],
Payload = CanRead.Item2
};
lock (ReceiveQueue)
{
ReceiveQueue.Enqueue(newPack);
ReceiveQueue.TrimExcess();
}
Thread.Sleep(Constants.DEFAULT_MIN_THREAD_SLEEP);
}
}
public void Send(ArmPacket SendPacket)
{
byte[] id = ConstructCanID(SendPacket);
uint constructedID = (uint)(id[2] << 8) | (uint)(id[1] << 4) | id[0];
CANBus.Write(constructedID, SendPacket.Payload);
}
private static byte[] ConstructCanID(ArmPacket packet)
{
byte[] canID = new byte[3];
canID[2] = (byte)(packet.Priority ? 0x01 : 0x00);
canID[1] = DEVICE_ADDR;
canID[0] = (byte)packet.TargetDeviceID;
return(canID);
}
