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