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();
