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; }