MainV2: add periph version checking at connect.

MainV2.cs

@@ -38,6 +38,8 @@
 using MissionPlanner.Joystick;
 using System.Net;
 using Newtonsoft.Json;
+using DroneCAN;
+using Accord.Math;
 
 namespace MissionPlanner
 {
@@ -1767,6 +1769,128 @@ public void doConnect(MAVLinkInterface comPort, string portname, string baud, bo
                         }
                     });
 
+                // check for newer firmware - can peripheral
+                if (showui)
+                    Task.Run(() =>
+                    {
+                        try
+                        {
+                            List<int> buses = new List<int> { 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, true);
+
+                                    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);
+                                    };
+
+                                    // 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
+
+                                        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");
+                                        }
+                                    }
+
+                                    MainV2.comPort.UnSubscribeToPacketType(canref);
+                                }
+                            }
+                        }
+                        catch (Exception ex)
+                        {
+                            log.Error(ex);
+                        }
+                    });
+
                 this.BeginInvokeIfRequired(() =>
                 {
                     _connectionControl.UpdateSysIDS();