Exemplo n.º 1
0
        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;
                }
            }
        }
Exemplo n.º 2
0
        private void menu_passthrough_Click(object sender, EventArgs e)
        {
            if (listener != null)
            {
                menu_passthrough.Checked = false;
                listener.Stop();
                CustomMessageBox.Show("Stop", "Disabled forwarding");
                listener = null;
                return;
            }

            var port = 500;

            if (InputBox.Show("Enter TCP Port", "Enter TCP Port", ref port) == DialogResult.OK)
            {
                menu_passthrough.Checked = true;

                Task.Run(() =>
                {
                    try
                    {
                        listener = new TcpListener(IPAddress.Any, port);
                        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();

                            DroneCAN.DroneCAN.MessageRecievedDel mrd = (frame, msg, id) =>
                            {
                                combps += frame.SizeofEntireMsg;
                                if (frame.MsgTypeID == DroneCAN.DroneCAN.uavcan_equipment_gnss_RTCMStream.UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_DT_ID)
                                {
                                    var data = msg as DroneCAN.DroneCAN.uavcan_equipment_gnss_RTCMStream;
                                    try
                                    {
                                        rtcmbps += data.data_len;
                                        st.Write(data.data, 0, data.data_len);
                                        st.Flush();
                                    }
                                    catch
                                    {
                                        client = null;
                                    }
                                }

                                if (frame.MsgTypeID == DroneCAN.DroneCAN.ardupilot_gnss_MovingBaselineData.ARDUPILOT_GNSS_MOVINGBASELINEDATA_DT_ID)
                                {
                                    var data = msg as DroneCAN.DroneCAN.ardupilot_gnss_MovingBaselineData;
                                    try
                                    {
                                        rtcmbps += data.data_len;
                                        st.Write(data.data, 0, data.data_len);
                                        st.Flush();
                                    }
                                    catch
                                    {
                                        client = null;
                                    }
                                }
                            };

                            can.MessageReceived -= mrd;
                            can.MessageReceived += mrd;

                            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.PackageMessageSLCAN(0, 30, 0,
                                                                            new DroneCAN.DroneCAN.uavcan_equipment_gnss_RTCMStream()
                                        {
                                            protocol_id = 3, data = buffer, data_len = (byte)read
                                        });
                                        can.WriteToStreamSLCAN(slcan);
                                    }

                                    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)
                            {
                                can.MessageReceived -= mrd;
                                Console.WriteLine(ex);
                            }
                        }
                    }
                    catch (Exception exception)
                    {
                        CustomMessageBox.Show(Strings.ERROR, "Forwarder problem " + exception.ToString());
                        if (listener != null)
                        {
                            listener.Stop();
                        }
                        listener = null;
                        this.InvokeIfRequired(() => { menu_passthrough.Checked = true; });
                    }
                });
            }
        }