public DirectoryInfo(string dir, DroneCAN.DroneCAN can, byte nodeid) { _can = can; _nodeid = nodeid; FullPath = dir; }
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); } }); } }; }
private void StartMavlinkCAN(byte bus = 1) { BusInUse = bus; but_slcanmode1.Enabled = false; but_mavlinkcanmode2.Enabled = true; but_mavlinkcanmode2_2.Enabled = true; but_filter.Enabled = true; Task.Run(() => { // allows old instance to exit Thread.Sleep(1000); mavlinkCANRun = true; // send every second, timeout is in 5 seconds while (mavlinkCANRun) { try { // setup forwarding on can port 1 var ans = MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, MAVLink.MAV_CMD.CAN_FORWARD, bus, 0, 0, 0, 0, 0, 0, false); if (ans == false) // MAVLink.MAV_RESULT.UNSUPPORTED) { //return; } } catch (Exception ex) { } if (mavlinkCANRun) { Thread.Sleep(1000); } } }); var port = new CommsInjection(); var can = new DroneCAN.DroneCAN(); can.FrameReceived += (frame, payload) => { //https://github.com/dronecan/pydronecan/blob/master/dronecan/driver/mavcan.py#L114 //if frame.extended: // message_id |= 1 << 31 if (payload.packet_data.Length > 8) { MainV2.comPort.sendPacket(new MAVLink.mavlink_canfd_frame_t(BitConverter.ToUInt32(frame.packet_data, 0) + (frame.Extended ? 0x80000000: 0), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, (byte)(bus - 1), (byte)DroneCAN.DroneCAN.dataLengthToDlc(payload.packet_data.Length), payload.packet_data), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); } else { var frame2 = new MAVLink.mavlink_can_frame_t(BitConverter.ToUInt32(frame.packet_data, 0) + (frame.Extended ? 0x80000000 : 0), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, (byte)(bus - 1), (byte)DroneCAN.DroneCAN.dataLengthToDlc(payload.packet_data.Length), payload.packet_data); MainV2.comPort.sendPacket(frame2, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); } }; port.ReadBufferUpdate += (o, i) => { }; port.WriteCallback += (o, bytes) => { var lines = ASCIIEncoding.ASCII.GetString(bytes.ToArray()) .Split(new[] { '\r' }, StringSplitOptions.RemoveEmptyEntries); foreach (var line in lines) { can.ReadMessageSLCAN(line); } }; // mavlink to slcan MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.CAN_FRAME, (m) => { if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAN_FRAME) { var canfd = false; var pkt = (MAVLink.mavlink_can_frame_t)m.data; var cf = new CANFrame(BitConverter.GetBytes(pkt.id)); var length = pkt.len; var payload = new CANPayload(pkt.data); var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X") , payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length))); port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2)); } else if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CANFD_FRAME) { var canfd = true; var pkt = (MAVLink.mavlink_canfd_frame_t)m.data; var cf = new CANFrame(BitConverter.GetBytes(pkt.id)); var length = pkt.len; var payload = new CANPayload(pkt.data); var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X") , payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length))); port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2)); } return(true); }, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, true); SetupSLCanPort(port); }
private void but_Click(object sender, EventArgs e) { OpenFileDialog ofd = new OpenFileDialog(); ofd.Filter = "can file|*.can"; if (ofd.ShowDialog() == DialogResult.OK) { SaveFileDialog sfd = new SaveFileDialog(); sfd.DefaultExt = "txt"; sfd.Filter = "txt|*.txt"; sfd.FileName = Path.GetFileNameWithoutExtension(ofd.FileName) + ".txt"; sfd.InitialDirectory = Path.GetDirectoryName(ofd.FileName); if (sfd.ShowDialog() == DialogResult.OK) { DroneCAN.DroneCAN can = new DroneCAN.DroneCAN(); using (var stream = sfd.OpenFile()) { var data = File.ReadAllBytes(ofd.FileName); can.MessageReceived += (frame, msg, id) => { var str = ASCIIEncoding.ASCII.GetBytes("\n" + msg.GetType().Name + "=" + msg.ToJSON()); stream .Write(str, 0, str.Length); }; can.FrameReceived += (frame, payload) => { var str = ASCIIEncoding.ASCII.GetBytes("\nFrame " + frame.ToJSON(Formatting.None) + payload.ToJSON(Formatting.None)); stream .Write(str, 0, str.Length); }; can.FrameError += (frame, payload) => { var str = ASCIIEncoding.ASCII.GetBytes("\nError"); stream .Write(str, 0, str.Length); }; data.ForEach(b => { try { can.Read((byte)b); stream.WriteByte((byte)b); } catch (Exception ex) { var str = ASCIIEncoding.ASCII.GetBytes(ex.ToString()); stream .Write(str, 0, str.Length); } }); } } } }
static void Main(string[] args) { Console.WriteLine("Usage: Ntrip.exe comport"); Console.WriteLine("Usage: Ntrip.exe comport lat lng alt"); Console.CancelKeyPress += Console_CancelKeyPress; AppDomain.CurrentDomain.DomainUnload += (sender, eventArgs) => { stop = true; Thread.Sleep(1000); }; var tcp = TcpListener.Create(2101); Console.WriteLine("Listerning on 2101"); tcp.Start(); tcp.BeginAcceptTcpClient(processclient, tcp); var ubx = new Ubx(); var rtcm = new rtcm3(); var can = new DroneCAN.DroneCAN(); Stream file = null; DateTime filetime = DateTime.MinValue; SerialPort port = null; everyminute = DateTime.Now.Minute; Directory.SetCurrentDirectory(Path.GetDirectoryName(Assembly.GetExecutingAssembly().Location)); // setup allocator can.SourceNode = 127; can.SetupDynamicNodeAllocator(); // feed the rtcm data into the rtcm parser if we get a can message can.MessageReceived += (frame, msg, id) => { if (frame.MsgTypeID == (ushort)DroneCAN.DroneCAN.uavcan_equipment_gnss_RTCMStream.UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_DT_ID) { var rtcmcan = (DroneCAN.DroneCAN.uavcan_equipment_gnss_RTCMStream)msg; for (int a = 0; a < rtcmcan.data_len; a++) { int seenmsg = -1; if ((seenmsg = rtcm.Read(rtcmcan.data[a])) > 0) { Console.WriteLine("CANRTCM" + seenmsg); gotRTCMData?.Invoke(rtcm.packet, rtcm.length); file.Write(rtcm.packet, 0, rtcm.length); } } } }; while (!stop) { try { if (port == null || !port.IsOpen) { var comport = args[0]; if (comport.Contains("/dev/")) { comport = UnixPath.GetRealPath(comport); } Console.WriteLine("Port: " + comport); port = new SerialPort(comport, 115200); port.Open(); ubx.SetupM8P(port, true, false); if (args.Length == 4) { ubx.SetupBasePos(port, new PointLatLngAlt(double.Parse(args[1]), double.Parse(args[2]), double.Parse(args[3])), 60, 2); } else { ubx.SetupBasePos(port, PointLatLngAlt.Zero, 60, 2); } } if (file == null || !file.CanWrite || DateTime.UtcNow.Day != filetime.Day) { if (file != null) { file.Close(); } string fn = ""; int no = 0; while (File.Exists((fn = DateTime.UtcNow.ToString("yyyy-MM-dd") + "-" + no + ".rtcm.Z"))) { no++; } file = new GZipStream(new BufferedStream(new FileStream(fn, FileMode.OpenOrCreate)), CompressionMode.Compress); filetime = DateTime.UtcNow; } var btr = port.BytesToRead; if (btr > 0) { totalread += btr; var buffer = new byte[btr]; btr = port.Read(buffer, 0, btr); foreach (byte by in buffer) { btr--; if (ubx.Read((byte)by) > 0) { rtcm.resetParser(); //Console.WriteLine(DateTime.Now + " new ubx message"); } if (by >= 0 && rtcm.Read((byte)by) > 0) { ubx.resetParser(); //Console.WriteLine(DateTime.Now + " new rtcm message"); gotRTCMData?.Invoke(rtcm.packet, rtcm.length); file.Write(rtcm.packet, 0, rtcm.length); } if ((by >= 0 && can.ReadSLCAN(by) > 0))// can_rtcm { ubx.resetParser(); } } } if (DateTime.Now.Minute != everyminute) { Console.WriteLine("{0} bps", totalread / 60); if (totalread == 0) { try { port.Close(); port = null; } catch (Exception) { port = null; } } totalread = 0; everyminute = DateTime.Now.Minute; } } catch (UnauthorizedAccessException ex) { Console.WriteLine(ex); Thread.Sleep(5000); } catch (IOException ex) { Console.WriteLine(ex); Thread.Sleep(1000); } catch (Exception ex) { Console.WriteLine(ex); } Thread.Sleep(10); } if (file != null) { file.Close(); } }
private void but_slcanmavlink_Click(object sender, EventArgs e) { byte bus = 1; Task.Run(() => { mavlinkCANRun = true; // send every second, timeout is in 5 seconds while (mavlinkCANRun) { // setup forwarding on can port 1 var ans = MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, MAVLink.MAV_CMD.CAN_FORWARD, bus, 0, 0, 0, 0, 0, 0); if (ans == false) // MAVLink.MAV_RESULT.UNSUPPORTED) { return; } Thread.Sleep(1000); } }); var port = new CommsInjection(); var can = new DroneCAN.DroneCAN(); can.FrameReceived += (frame, payload) => { if (payload.packet_data.Length > 8) { MainV2.comPort.sendPacket(new MAVLink.mavlink_canfd_frame_t(BitConverter.ToUInt32(frame.packet_data, 0), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, (byte)(bus), (byte)payload.packet_data.Length, payload.packet_data), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); } else { MainV2.comPort.sendPacket(new MAVLink.mavlink_can_frame_t(BitConverter.ToUInt32(frame.packet_data, 0), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, (byte)(bus), (byte)payload.packet_data.Length, payload.packet_data), (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); } }; port.ReadBufferUpdate += (o, i) => { }; port.WriteCallback += (o, bytes) => { var lines = ASCIIEncoding.ASCII.GetString(bytes.ToArray()) .Split(new[] { '\r' }, StringSplitOptions.RemoveEmptyEntries); foreach (var line in lines) { can.ReadMessageSLCAN(line); } }; // mavlink to slcan MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.CAN_FRAME, (m) => { if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAN_FRAME) { var canfd = false; var pkt = (MAVLink.mavlink_can_frame_t)m.data; var cf = new CANFrame(BitConverter.GetBytes(pkt.id)); var length = pkt.len; var payload = new CANPayload(pkt.data); var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X") , payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length))); port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2)); } else if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CANFD_FRAME) { var canfd = true; var pkt = (MAVLink.mavlink_canfd_frame_t)m.data; var cf = new CANFrame(BitConverter.GetBytes(pkt.id)); var length = pkt.len; var payload = new CANPayload(pkt.data); var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X") , payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length))); port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2)); } return(true); }, true); SetupSLCanPort(port); }
public DroneCANSubscriber(DroneCAN.DroneCAN can, string selectedmsgid) { this.selectedmsgid = selectedmsgid; this.can = can; InitializeComponent(); }
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();