public static bool testconversion <T>(T input, byte bitlength, bool signed, byte bit_offset = 0) where T : struct
{
var buf = new byte[16];
var ctx = new statetracking()
{
bit = bit_offset
};
T ans = input;
T ans2 = input;
uavcan.canardEncodeScalar(buf, bit_offset, bitlength, input);
uavcan_transmit_chunk_handler(buf, bitlength, ctx);
uavcan.canardEncodeScalar(buf, bit_offset, bitlength, input);
uavcan_transmit_chunk_handler(buf, bitlength, ctx);
uavcan.canardDecodeScalar(new uavcan.CanardRxTransfer(ctx.ToBytes()), bit_offset, bitlength, signed, ref ans);
uavcan.canardDecodeScalar(new uavcan.CanardRxTransfer(ctx.ToBytes()), bit_offset + (uint)bitlength, bitlength, signed, ref ans2);
if (input.ToString() != ans.ToString())
{
throw new Exception();
}
if (input.ToString() != ans2.ToString())
{
throw new Exception();
}
return(true);
}
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);
}
public async void Update(string devicename, double hwversion, string firmware_name)
{
ServeFile(firmware_name);
var firmware_namebytes = ASCIIEncoding.ASCII.GetBytes(Path.GetFileName(firmware_name.ToLower()));
ulong firmware_crc = ulong.MaxValue;
MessageRecievedDel updatedelegate = (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_file_BeginFirmwareUpdate_res))
{
var bfures = msg as uavcan.uavcan_protocol_file_BeginFirmwareUpdate_res;
if (bfures.error != 0)
{
throw new Exception("Begin Firmware Update returned an error");
}
}
else if (msg.GetType() == typeof(uavcan.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as uavcan.uavcan_protocol_GetNodeInfo_res;
Console.WriteLine("GetNodeInfo: seen '{0}' from {1}", ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0'), frame.SourceNode);
if (devicename == ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0') || devicename == ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0') + "-BL")
{
if (firmware_crc != gnires.software_version.image_crc)
{
if (hwversion == double.Parse(gnires.hardware_version.major + "." + gnires.hardware_version.minor, CultureInfo.InvariantCulture) || hwversion == 0)
{
if (gnires.status.mode != uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE)
{
var req_msg =
new uavcan.uavcan_protocol_file_BeginFirmwareUpdate_req()
{
image_file_remote_path = new uavcan.uavcan_protocol_file_Path()
{
path = firmware_namebytes
},
source_node_id = SourceNode
};
req_msg.image_file_remote_path.path_len = (byte)firmware_namebytes.Length;
var slcan = PackageMessage(frame.SourceNode, frame.Priority, transferID, req_msg);
lock (sr_lock)
WriteToStream(slcan);
}
else
{
Console.WriteLine("already in update mode");
}
}
else
{
Console.WriteLine("hwversion does not match");
}
}
else
{
throw new Exception(String.Format("{0} - No need to upload, crc matchs", frame.SourceNode));
}
}
else
{
Console.WriteLine("device name does not match");
}
}
};
MessageReceived += updatedelegate;
// getfile crc
using (var stream = File.OpenRead(fileServerList[Path.GetFileName(firmware_name.ToLower())]))
{
string app_descriptor_fmt = "<8cQI";
var SHARED_APP_DESCRIPTOR_SIGNATURES = new byte[][] {
new byte[] { 0xd7, 0xe4, 0xf7, 0xba, 0xd0, 0x0f, 0x9b, 0xee },
new byte[] { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 }
};
var app_descriptor_len = 8 * 1 + 8 + 4;
var location = GetPatternPositions(stream, SHARED_APP_DESCRIPTOR_SIGNATURES[0]);
stream.Seek(0, SeekOrigin.Begin);
var location2 = GetPatternPositions(stream, SHARED_APP_DESCRIPTOR_SIGNATURES[1]);
if (location.Count > 0 || location2.Count > 0)
{
var offset = location.Count > 0 ? location[0] : location2[0];
stream.Seek(offset, SeekOrigin.Begin);
byte[] buf = new byte[app_descriptor_len];
stream.Read(buf, 0, app_descriptor_len);
firmware_crc = BitConverter.ToUInt64(buf, 8);
}
}
NodeAdded += (NodeID, msg) =>
{
var statetracking = new statetracking();
// get node info
uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(uavcan_transmit_chunk_handler, statetracking);
var slcan = PackageMessage(NodeID, 30, transferID++, gnireq);
lock (sr_lock)
WriteToStream(slcan);
};
foreach (var i in nodeList.Keys.ToArray())
{
var statetracking = new statetracking();
// get node info
uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(uavcan_transmit_chunk_handler, statetracking);
var slcan = PackageMessage((byte)i, 30, transferID++, gnireq);
lock (sr_lock)
WriteToStream(slcan);
}
int b = 0;
while (true)
{
await Task.Delay(1000);
if (nodeList.Values.Any(a => a.mode == uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE))
{
}
else
{
b++;
if (b > 100)
{
break;
}
}
}
MessageReceived -= updatedelegate;
}
