public UAVCANParams(UAVCAN.uavcan can, byte node, List <UAVCAN.uavcan.uavcan_protocol_param_GetSet_res> paramlist)
{
_can = can;
_paramlist = paramlist;
_node = node;
InitializeComponent();
}
public UAVCANParams(UAVCAN.uavcan can, byte node, List <UAVCAN.uavcan.uavcan_protocol_param_GetSet_res> paramlist)
{
_can = can;
_paramlist = paramlist;
_node = node;
InitializeComponent();
this.Text = "UAVCAN Params - " + node;
Params.CellValidating += CellValidatingEvtHdlr;
}
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)
{
port = new Comms.SerialPort()
{
PortName = MainV2._connectionControl.CMB_serialport.Text,
BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text)
};
}
if (can == null)
{
can = new uavcan();
}
can.SourceNode = 127;
can.NodeAdded += (id, msg) =>
{
this.BeginInvoke((Action) delegate
{
allnodes.Add(new UAVCANModel()
{
ID = id,
Name = "?",
Health = msg.health.ToString(),
Mode = msg.mode.ToString(),
Uptime = TimeSpan.FromSeconds(msg.uptime_sec),
VSC = msg.vendor_specific_status_code
});
});
};
if (!port.IsOpen)
{
try
{
port.Open();
}
catch (Exception e)
{
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(UAVCAN.uavcan.uavcan_protocol_NodeStatus))
{
var ns = msg as UAVCAN.uavcan.uavcan_protocol_NodeStatus;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
if (nodes.Count() > 0 && nodes.First().Name == "?")
{
var statetracking = new UAVCAN.uavcan.statetracking();
// get node info
UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(UAVCAN.uavcan.uavcan_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)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK:
item.Health = "OK";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING:
item.Health = "WARNING";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR:
item.Health = "ERROR";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL:
item.Health = "CRITICAL";
break;
}
switch (ns.mode)
{
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL:
item.Mode = "OPERATIONAL";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION:
item.Mode = "INITIALIZATION";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE:
item.Mode = "MAINTENANCE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:
item.Mode = "SOFTWARE_UPDATE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE:
item.Mode = "OFFLINE";
break;
}
item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec);
}
_updatePending = true;
this.BeginInvoke((Action) delegate
{
if (_updatePending)
{
_updatePending = false;
uAVCANModelBindingSource.ResetBindings(false);
}
});
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as UAVCAN.uavcan.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;
this.BeginInvoke((Action) delegate
{
if (_updatePending)
{
_updatePending = false;
uAVCANModelBindingSource.ResetBindings(false);
}
});
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_debug_LogMessage))
{
var debug = msg as UAVCAN.uavcan.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);
}
});
}
};
}
}
UAVCAN.uavcan can = new UAVCAN.uavcan();
public void Deactivate()
{
can?.Stop();
can = null;
}
static void Main(string[] args)
{
Console.WriteLine("Usage: CanPassThrough.exe serialport tcpport");
var comport = args[0];
var tcpport = args[1];
var serial = new SerialPort(comport, 115200);
serial.Open();
var can = new UAVCAN.uavcan();
can.SetupDynamicNodeAllocator();
can.PrintDebugToConsole();
can.StartSLCAN(serial.BaseStream);
TcpListener listener = new TcpListener(int.Parse(tcpport));
listener.Start();
int tcpbps = 0;
int rtcmbps = 0;
int combps = 0;
int second = 0;
while (true)
{
var client = listener.AcceptTcpClient();
client.NoDelay = true;
var st = client.GetStream();
can.MessageReceived += (frame, msg, id) =>
{
combps += frame.SizeofEntireMsg;
if (frame.MsgTypeID == UAVCAN.uavcan.UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_DT_ID)
{
var data = msg as uavcan.uavcan_equipment_gnss_RTCMStream;
try
{
rtcmbps += data.data_len;
st.Write(data.data, 0, data.data_len);
st.Flush();
}
catch
{
client = null;
}
}
};
try
{
while (true)
{
if (client.Available > 0)
{
var toread = Math.Min(client.Available, 128);
byte[] buffer = new byte[toread];
var read = st.Read(buffer, 0, toread);
foreach (var b in buffer)
{
Console.Write("0x{0:X} ", b);
}
Console.WriteLine();
tcpbps += read;
var slcan = can.PackageMessage(0, 0, 0,
new uavcan.uavcan_equipment_gnss_RTCMStream()
{
protocol_id = 3, data = buffer, data_len = (byte)read
});
can.WriteToStream(slcan);
serial.BaseStream.Flush();
}
Thread.Sleep(1);
if (second != DateTime.Now.Second)
{
Console.WriteLine("tcp:{0} can:{1} data:{2} avail:{3}", tcpbps, combps, rtcmbps, client.Available);
tcpbps = combps = rtcmbps = 0;
second = DateTime.Now.Second;
}
}
}
catch (Exception ex)
{
Console.WriteLine(ex);
}
}
}
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 UAVCAN.uavcan();
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)uavcan.UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_DT_ID)
{
var rtcmcan = (uavcan.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.Read(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 ex)
{
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();
}
}
public UAVCANSubscriber(UAVCAN.uavcan 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)
{
var cport = MainV2.comPort.MAV.param["CAN_SLCAN_CPORT"].Value;
MainV2.comPort.setParam("CAN_SLCAN_CPORT", canport, true);
if (cport == 0)
{
CustomMessageBox.Show("Reboot required" + " after setting CPORT. Please reboot!", Strings.ERROR);
return;
}
MainV2.comPort.setParam("CAN_SLCAN_TIMOUT", 2, true);
MainV2.comPort.setParam("CAN_P" + canport + "_DRIVER", 1);
MainV2.comPort.setParam("CAN_SLCAN_SERNUM", 0, true); // usb
}
}
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)
{
port = new Comms.SerialPort()
{
PortName = MainV2._connectionControl.CMB_serialport.Text,
BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text)
};
}
can.SourceNode = 127;
can.NodeAdded += (id, msg) =>
{
this.BeginInvoke((Action) delegate
{
allnodes.Add(new UAVCANModel()
{
ID = id,
Name = "?",
Health = msg.health.ToString(),
Mode = msg.mode.ToString(),
Uptime = TimeSpan.FromSeconds(msg.uptime_sec)
});
});
};
if (!port.IsOpen)
{
try
{
port.Open();
}
catch (Exception e)
{
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";
}
can.StartSLCAN(port.BaseStream);
can.SetupFileServer();
can.SetupDynamicNodeAllocator();
can.MessageReceived += (frame, msg, transferID) =>
{
if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_NodeStatus))
{
var ns = msg as UAVCAN.uavcan.uavcan_protocol_NodeStatus;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
if (nodes.Count() > 0 && nodes.First().Name == "?")
{
var statetracking = new UAVCAN.uavcan.statetracking();
// get node info
UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(UAVCAN.uavcan.uavcan_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)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK:
item.Health = "OK";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING:
item.Health = "WARNING";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR:
item.Health = "ERROR";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL:
item.Health = "CRITICAL";
break;
}
switch (ns.mode)
{
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL:
item.Mode = "OPERATIONAL";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION:
item.Mode = "INITIALIZATION";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE:
item.Mode = "MAINTENANCE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:
item.Mode = "SOFTWARE_UPDATE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE:
item.Mode = "OFFLINE";
break;
}
item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec);
}
this.BeginInvoke((Action) delegate
{
uAVCANModelBindingSource.ResetBindings(false);
});
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as UAVCAN.uavcan.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;
}
this.BeginInvoke((Action) delegate
{
uAVCANModelBindingSource.ResetBindings(false);
});
}
};
}
}
UAVCAN.uavcan can = new UAVCAN.uavcan();
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 { }
UAVCAN.uavcan can = new UAVCAN.uavcan();
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>();
UAVCAN.uavcan.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(uavcan.uavcan_protocol_file_BeginFirmwareUpdate_res))
{
var bfures = msg as uavcan.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(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 (firmware_crc != gnires.software_version.image_crc || firmware_crc == ulong.MaxValue)
{
if (gnires.status.mode != uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE)
{
Console.WriteLine("Update node " + frame.SourceNode);
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 = can.SourceNode
};
req_msg.image_file_remote_path.path_len = (byte)firmware_namebytes.Length;
var slcan = can.PackageMessage(frame.SourceNode, frame.Priority, transferID, req_msg);
can.WriteToStream(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
uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
var slcan = can.PackageMessage((byte)id, 30, 0, gnireq);
can.WriteToStream(slcan);
};
can.FileSendComplete += (id, file) =>
{
Console.WriteLine(id + " File send complete " + file);
can.NodeList.Remove(id);
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.Remove(lastseenDateTime.Key);
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;
}
}
}
