private void SetupSLCanPort(ICommsSerial port) { //check if we started from within mavlink - if not get settings from menu and create port if (port == null || !port.IsOpen) { switch (MainV2._connectionControl.CMB_serialport.Text) { case "TCP": port = new TcpSerial(); break; case "UDP": port = new UdpSerial(); break; case "WS": port = new WebSocket(); break; case "UDPCl": port = new UdpSerialConnect(); break; default: port = new SerialPort() { PortName = MainV2._connectionControl.CMB_serialport.Text, BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text) }; break; } } if (can == null) { can = new DroneCAN.DroneCAN(); } can.SourceNode = 127; can.NodeAdded += (id, msg) => { this.BeginInvoke((Action) delegate { allnodes.Add(new DroneCANModel() { ID = id, Name = "?", Health = msg.health.ToString(), Mode = msg.mode.ToString(), Uptime = TimeSpan.FromSeconds(msg.uptime_sec), VSC = msg.vendor_specific_status_code }); uAVCANModelBindingSource.ResetBindings(false); }); }; if (!port.IsOpen) { try { port.Open(); } catch (Exception) { CustomMessageBox.Show(Strings.CheckPortSettingsOr); return; } } if (chk_log.Checked) { can.LogFile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".can"; } var prd = new ProgressReporterDialogue(); prd.UpdateProgressAndStatus(-1, "Trying to connect"); prd.DoWork += sender => can.StartSLCAN(port.BaseStream); prd.btnCancel.Click += (sender, args) => { prd.doWorkArgs.CancelAcknowledged = true; port.Close(); }; prd.RunBackgroundOperationAsync(); if (prd.doWorkArgs.CancelRequested || prd.doWorkArgs.ErrorMessage != null) { return; } can.SetupFileServer(); can.SetupDynamicNodeAllocator(); can.MessageReceived += (frame, msg, transferID) => { if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_NodeStatus)) { var ns = msg as DroneCAN.DroneCAN.uavcan_protocol_NodeStatus; var nodes = allnodes.Where((a) => a.ID == frame.SourceNode); if (nodes.Count() > 0 && nodes.First().Name == "?") { var statetracking = new DroneCAN.DroneCAN.statetracking(); // get node info DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req gnireq = new DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req() { }; gnireq.encode(DroneCAN.DroneCAN.dronecan_transmit_chunk_handler, statetracking); var slcan = can.PackageMessageSLCAN(frame.SourceNode, 30, 0, gnireq); can.WriteToStreamSLCAN(slcan); } foreach (var item in nodes) { switch (ns.health) { case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: item.Health = "OK"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: item.Health = "WARNING"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: item.Health = "ERROR"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: item.Health = "CRITICAL"; break; } switch (ns.mode) { case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: item.Mode = "OPERATIONAL"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: item.Mode = "INITIALIZATION"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: item.Mode = "MAINTENANCE"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: item.Mode = "SOFTWARE_UPDATE"; break; case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: item.Mode = "OFFLINE"; break; } item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec); } _updatePending = true; } else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res)) { var gnires = msg as DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res; var nodes = allnodes.Where((a) => a.ID == frame.SourceNode); foreach (var item in nodes) { item.Name = ASCIIEncoding.ASCII.GetString(gnires.name, 0, gnires.name_len); item.HardwareVersion = gnires.hardware_version.major + "." + gnires.hardware_version.minor; item.SoftwareVersion = gnires.software_version.major + "." + gnires.software_version.minor + "." + gnires.software_version.vcs_commit.ToString("X"); item.SoftwareCRC = gnires.software_version.image_crc; item.HardwareUID = gnires.hardware_version.unique_id.Select(a => a.ToString("X2")).Aggregate((a, b) => { return(a + " " + b); }); item.RawMsg = gnires; item.VSC = gnires.status.vendor_specific_status_code; } _updatePending = true; } else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_debug_LogMessage)) { var debug = msg as DroneCAN.DroneCAN.uavcan_protocol_debug_LogMessage; this.BeginInvoke((Action) delegate() { DGDebug.Rows.Insert(0, new object[] { frame.SourceNode, debug.level.value, ASCIIEncoding.ASCII.GetString(debug.source, 0, debug.source_len), ASCIIEncoding.ASCII.GetString(debug.text, 0, debug.text_len) }); if (DGDebug.Rows.Count > 100) { DGDebug.Rows.RemoveAt(DGDebug.Rows.Count - 1); } }); } }; }
static void Main(string[] args) { Console.CancelKeyPress += Console_CancelKeyPress; if (args.Length < 2) { Console.WriteLine("usage: UAVCANFlasher com1 flow-crc.bin"); Console.WriteLine("this program will flash the firmware to every connected uavcan node. DONT connect devices that its not intended for"); Console.ReadKey(); return; } var portname = args[0]; var firmware_name = args[1]; var port = new MissionPlanner.Comms.SerialPort(args[0], 115200); port.Open(); if (!File.Exists(firmware_name)) { Console.WriteLine("firmware file not found"); Console.ReadKey(); return; } MAVLinkInterface mav = new MAVLinkInterface(); mav.BaseStream = port; mav.getHeartBeat(); try { mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "CAN_SLCAN_TIMOUT"); mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "CAN_SLCAN_SERNUM"); mav.setParam("CAN_SLCAN_TIMOUT", 2, true); mav.setParam("CAN_SLCAN_SERNUM", 0, true); // usb } catch { } DroneCAN.DroneCAN can = new DroneCAN.DroneCAN(); can.SourceNode = 127; //can.Update(); // updater var firmware_namebytes = ASCIIEncoding.ASCII.GetBytes(Path.GetFileName(firmware_name.ToLower())); ulong firmware_crc = ulong.MaxValue; Exception exception = null; Dictionary <int, DateTime> lastseenDateTimes = new Dictionary <int, DateTime>(); DroneCAN.DroneCAN.MessageRecievedDel updatedelegate = (frame, msg, transferID) => { if (frame.IsServiceMsg && frame.SvcDestinationNode != can.SourceNode) { return; } if (frame.SourceNode == 0) { return; } lastseenDateTimes[frame.SourceNode] = DateTime.Now; if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_file_BeginFirmwareUpdate_res)) { var bfures = msg as DroneCAN.DroneCAN.uavcan_protocol_file_BeginFirmwareUpdate_res; if (bfures.error != 0) { exception = new Exception(frame.SourceNode + " Begin Firmware Update returned an error"); } } else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res)) { var gnires = msg as DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res; Console.WriteLine("GetNodeInfo: seen '{0}' from {1}", ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0'), frame.SourceNode); if (firmware_crc != gnires.software_version.image_crc || firmware_crc == ulong.MaxValue) { if (gnires.status.mode != DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE) { Console.WriteLine("Update node " + frame.SourceNode); var req_msg = new DroneCAN.DroneCAN.uavcan_protocol_file_BeginFirmwareUpdate_req() { image_file_remote_path = new DroneCAN.DroneCAN.uavcan_protocol_file_Path() { path = firmware_namebytes }, source_node_id = can.SourceNode }; req_msg.image_file_remote_path.path_len = (byte)firmware_namebytes.Length; var slcan = can.PackageMessageSLCAN(frame.SourceNode, frame.Priority, transferID, req_msg); can.WriteToStreamSLCAN(slcan); } else { //exception = new Exception("already in update mode"); return; } } else { Console.WriteLine(frame.SourceNode + " CRC matchs"); } } }; can.MessageReceived += updatedelegate; // getfile crc using (var stream = File.OpenRead(firmware_name)) { 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); } } can.NodeAdded += (id, status) => { Console.WriteLine(id + " Node status seen"); // get node info DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req gnireq = new DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req() { }; var slcan = can.PackageMessageSLCAN((byte)id, 30, 0, gnireq); can.WriteToStreamSLCAN(slcan); }; DroneCAN.DroneCAN.uavcan_protocol_NodeStatus node; can.FileSendComplete += (id, file) => { Console.WriteLine(id + " File send complete " + file); can.NodeList.TryRemove(id, out node); lastseenDateTimes.Remove(id); }; can.StartSLCAN(port.BaseStream); can.ServeFile(args[1]); can.SetupFileServer(); can.SetupDynamicNodeAllocator(); Console.WriteLine("Ready.. " + portname + " -> " + firmware_name); run = true; while (run) { Thread.Sleep(100); foreach (var lastseenDateTime in lastseenDateTimes.ToArray()) { if (lastseenDateTime.Value.AddSeconds(3) < DateTime.Now) { can.NodeList.TryRemove(lastseenDateTime.Key, out node); lastseenDateTimes.Remove(lastseenDateTime.Key); Console.WriteLine(lastseenDateTime.Key + " Remove old node - 3 seconds of no data"); } } if (exception != null) { Console.WriteLine(exception.ToString()); exception = null; } } }
public void startslcan(byte canport) { but_slcanmode1.Enabled = false; but_slcanmode2.Enabled = false; try { if (!MainV2.comPort.BaseStream.IsOpen) { if (CustomMessageBox.Show( "You are not currently connected via mavlink. Please make sure the device is already in slcan mode or this is the slcan serialport.", "SLCAN", CustomMessageBox.MessageBoxButtons.OKCancel) != CustomMessageBox.DialogResult.OK) { return; } } if (MainV2.comPort.BaseStream.IsOpen) { var cport = MainV2.comPort.MAV.param["CAN_SLCAN_CPORT"].Value; MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_SLCAN_CPORT", canport, true); if (cport == 0) { CustomMessageBox.Show("Reboot required" + " after setting CPORT. Please reboot!", Strings.ERROR); return; } MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_SLCAN_TIMOUT", 2, true); MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_P" + canport + "_DRIVER", 1); //MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_SLCAN_SERNUM", 0, true); // usb // blind send var paramname = "CAN_SLCAN_SERNUM"; var req = new MAVLink.mavlink_param_set_t { target_system = (byte)MainV2.comPort.sysidcurrent, target_component = (byte)MainV2.comPort.compidcurrent, param_type = (byte)MainV2.comPort .MAVlist[(byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent] .param_types[paramname], param_id = paramname.MakeBytesSize(16) }; MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); } } catch { } { // grab the connected port var port = MainV2.comPort.BaseStream; // place an invalid port in its place if (port != null) { MainV2.comPort.BaseStream = new Comms.SerialPort() { PortName = port.PortName, BaudRate = port.BaudRate } } ; //check if we started from within mavlink - if not get settings from menu and create port if (port == null || !port.IsOpen) { switch (MainV2._connectionControl.CMB_serialport.Text) { case "TCP": port = new TcpSerial(); break; case "UDP": port = new UdpSerial(); break; case "WS": port = new WebSocket(); break; case "UDPCl": port = new UdpSerialConnect(); break; default: port = new SerialPort() { PortName = MainV2._connectionControl.CMB_serialport.Text, BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text) }; break; } } if (can == null) { can = new DroneCAN.DroneCAN(); } can.SourceNode = 127; can.NodeAdded += (id, msg) => { this.BeginInvoke((Action) delegate { allnodes.Add(new DroneCANModel() { ID = id, Name = "?", Health = msg.health.ToString(), Mode = msg.mode.ToString(), Uptime = TimeSpan.FromSeconds(msg.uptime_sec), VSC = msg.vendor_specific_status_code }); uAVCANModelBindingSource.ResetBindings(false); }); }; if (!port.IsOpen) { try { port.Open(); } catch (Exception) { CustomMessageBox.Show(Strings.CheckPortSettingsOr); return; } } if (chk_log.Checked) { can.LogFile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".can"; } var prd = new ProgressReporterDialogue(); prd.UpdateProgressAndStatus(-1, "Trying to connect"); prd.DoWork += sender => can.StartSLCAN(port.BaseStream); prd.btnCancel.Click += (sender, args) => { prd.doWorkArgs.CancelAcknowledged = true; port.Close(); }; prd.RunBackgroundOperationAsync(); if (prd.doWorkArgs.CancelRequested || prd.doWorkArgs.ErrorMessage != null) { return; } can.SetupFileServer(); can.SetupDynamicNodeAllocator(); can.MessageReceived += (frame, msg, transferID) => { if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_NodeStatus)) { var ns = msg as DroneCAN.DroneCAN.uavcan_protocol_NodeStatus; var nodes = allnodes.Where((a) => a.ID == frame.SourceNode); if (nodes.Count() > 0 && nodes.First().Name == "?") { var statetracking = new DroneCAN.DroneCAN.statetracking(); // get node info DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req gnireq = new DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req() { }; gnireq.encode(DroneCAN.DroneCAN.dronecan_transmit_chunk_handler, statetracking); var slcan = can.PackageMessage(frame.SourceNode, 30, 0, gnireq); can.WriteToStream(slcan); } foreach (var item in nodes) { switch (ns.health) { case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: item.Health = "OK"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: item.Health = "WARNING"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: item.Health = "ERROR"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: item.Health = "CRITICAL"; break; } switch (ns.mode) { case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: item.Mode = "OPERATIONAL"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: item.Mode = "INITIALIZATION"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: item.Mode = "MAINTENANCE"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: item.Mode = "SOFTWARE_UPDATE"; break; case (byte)DroneCAN.DroneCAN.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: item.Mode = "OFFLINE"; break; } item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec); } _updatePending = true; } else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res)) { var gnires = msg as DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res; var nodes = allnodes.Where((a) => a.ID == frame.SourceNode); foreach (var item in nodes) { item.Name = ASCIIEncoding.ASCII.GetString(gnires.name, 0, gnires.name_len); item.HardwareVersion = gnires.hardware_version.major + "." + gnires.hardware_version.minor; item.SoftwareVersion = gnires.software_version.major + "." + gnires.software_version.minor + "." + gnires.software_version.vcs_commit.ToString("X"); item.SoftwareCRC = gnires.software_version.image_crc; item.HardwareUID = gnires.hardware_version.unique_id.Select(a => a.ToString("X2")).Aggregate((a, b) => { return(a + " " + b); }); item.RawMsg = gnires; item.VSC = gnires.status.vendor_specific_status_code; } _updatePending = true; } else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_debug_LogMessage)) { var debug = msg as DroneCAN.DroneCAN.uavcan_protocol_debug_LogMessage; this.BeginInvoke((Action) delegate() { DGDebug.Rows.Insert(0, new object[] { frame.SourceNode, debug.level.value, ASCIIEncoding.ASCII.GetString(debug.source, 0, debug.source_len), ASCIIEncoding.ASCII.GetString(debug.text, 0, debug.text_len) }); if (DGDebug.Rows.Count > 100) { DGDebug.Rows.RemoveAt(DGDebug.Rows.Count - 1); } }); } }; } } DroneCAN.DroneCAN can = new DroneCAN.DroneCAN();