public static ushort compute(byte[] bytes, int len)
{
var temp = new TransferCRC();
var total = len;
while (len > 0)
{
temp.add(bytes[total - len]);
len--;
}
return(temp.get());
}
public string PackageMessage(byte destNode, byte priority, byte transferID, IUAVCANSerialize msg)
{
var state = new statetracking();
msg.encode(uavcan_transmit_chunk_handler, state);
var msgtype = uavcan.MSG_INFO.First(a => a.Item1 == msg.GetType());
CANFrame cf = new CANFrame(new byte[4]);
cf.SourceNode = SourceNode;
cf.Priority = priority;
if (msg.GetType().FullName.EndsWith("_res") || msg.GetType().FullName.EndsWith("_req"))
{
// service
cf.IsServiceMsg = true;
cf.SvcDestinationNode = destNode;
cf.SvcIsRequest = msg.GetType().FullName.EndsWith("_req") ? true : false;
cf.SvcTypeID = (byte)msgtype.Item2;
}
else
{
// message
cf.MsgTypeID = (ushort)msgtype.Item2;
}
string ans = "";
var payloaddata = state.ToBytes();
if (payloaddata.Length > 7)
{
var dt_sig = BitConverter.GetBytes(msgtype.Item3);
var crcprocess = new TransferCRC();
crcprocess.add(dt_sig, 8);
crcprocess.add(payloaddata, payloaddata.Length);
var crc = crcprocess.get();
var buffer = new byte[8];
var toogle = false;
var size = 7;
for (int a = 0; a < payloaddata.Length; a += size)
{
if (a == 0)
{
buffer[0] = (byte)(crc & 0xff);
buffer[1] = (byte)(crc >> 8);
size = 5;
Array.ConstrainedCopy(payloaddata, a, buffer, 2, 5);
}
else
{
size = payloaddata.Length - a <= 7 ? payloaddata.Length - a : 7;
Array.ConstrainedCopy(payloaddata, a, buffer, 0, size);
if (buffer.Length != size + 1)
{
Array.Resize(ref buffer, size + 1);
}
}
CANPayload payload = new CANPayload(buffer);
payload.SOT = a == 0 ? true : false;
payload.EOT = a + size == payloaddata.Length ? true : false;
payload.TransferID = (byte)transferID;
payload.Toggle = toogle;
toogle = !toogle;
ans += String.Format("T{0}{1}{2}\r", cf.ToHex(), a == 0 ? 8 : size + 1, payload.ToHex());
}
}
else
{
var buffer = new byte[payloaddata.Length + 1];
Array.Copy(payloaddata, buffer, payloaddata.Length);
CANPayload payload = new CANPayload(buffer);
payload.SOT = payload.EOT = true;
payload.TransferID = (byte)transferID;
ans = String.Format("T{0}{1}{2}\r", cf.ToHex(), buffer.Length, payload.ToHex());
}
//Console.Write("TX " + ans.Replace("\r", "\r\n"));
//Console.WriteLine("TX " + msgtype.Item1 + " " + JsonConvert.SerializeObject(msg));
return(ans);
}