From a337345ef3617dd0b12d648eb861f8bb3d854c21 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 24 Sep 2024 21:15:40 +1000 Subject: [PATCH] MainV2: remove periph version checking at connect CAN_FORWARD requests are not always safe to perform, e.g., if there is more traffic on the bus than the telemetry link can handle. We should only forward CAN messages if the user has explicitly asked for it by going to the DroneCAN setup tab. --- MainV2.cs | 126 ------------------------------------------------------ 1 file changed, 126 deletions(-) diff --git a/MainV2.cs b/MainV2.cs index 139e8926e9..f2c86b6e79 100644 --- a/MainV2.cs +++ b/MainV2.cs @@ -1703,132 +1703,6 @@ public void doConnect(MAVLinkInterface comPort, string portname, string baud, bo } }); - // check for newer firmware - can peripheral - if (showui) - Task.Run(() => - { - try - { - List buses = new List { 1, 2 }; - foreach (var bus in buses) - { - using (var port = new CommsInjection()) - { - var can = new DroneCAN.DroneCAN(); - can.SourceNode = 127; - - 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 - var canref = 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, false); - - can.NodeAdded += (id, status) => - { - Console.WriteLine(id + " Node status seen - request Node Info"); - // 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); - }; - - // be invisible - can.NodeStatus = false; - can.StartSLCAN(port.BaseStream); - - //start on bus - 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); - - Thread.Sleep(5000); - - // stop - MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, - (byte)MainV2.comPort.compidcurrent, MAVLink.MAV_CMD.CAN_FORWARD, 0, 0, 0, 0, 0, 0, 0, - false); - - foreach (var node in can.NodeInfo) - { - var devicename = can.GetNodeName((byte)node.Key); - var githash = can.NodeInfo[node.Key].software_version.vcs_commit.ToString("X"); - //Version and githash - - log.Info(node.ToJSON()); - - var option = APFirmware.Manifest.Firmware.Where(a => - a.MavFirmwareVersionType == APFirmware.RELEASE_TYPES.OFFICIAL.ToString() && - a.VehicleType == "AP_Periph" && - a.Format == "bin" && - a.MavType == "CAN_PERIPHERAL" && - a.MavFirmwareVersionMajor >= node.Value.software_version.major && - a.MavFirmwareVersionMinor >= node.Value.software_version.minor && - node.Value.software_version.major != 0 && - node.Value.software_version.minor != 0 && - devicename.EndsWith(a.Platform) && - !a.GitSha.StartsWith(githash, StringComparison.InvariantCultureIgnoreCase) - ).FirstOrDefault(); - if (option != default(APFirmware.FirmwareInfo)) - { - Common.MessageShowAgain("New firmware", "New firmware for " + devicename + " " + option.MavFirmwareVersion + " " + option.GitSha + "\nUpdate via the dronecan screen"); - } - } - - can.Stop(); - - MainV2.comPort.UnSubscribeToPacketType(canref); - } - } - } - catch (Exception ex) - { - log.Error(ex); - } - }); - this.BeginInvokeIfRequired(() => { _connectionControl.UpdateSysIDS();