Beispiel #1
0
        private void StartMavlinkCAN(byte bus = 1)
        {
            BusInUse = bus;
            but_slcanmode1.Enabled        = false;
            but_mavlinkcanmode2.Enabled   = true;
            but_mavlinkcanmode2_2.Enabled = true;
            but_filter.Enabled            = true;

            Task.Run(() =>
            {
                // allows old instance to exit
                Thread.Sleep(1000);
                mavlinkCANRun = true;
                // send every second, timeout is in 5 seconds
                while (mavlinkCANRun)
                {
                    try
                    {
                        // setup forwarding on can port 1
                        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);

                        if (ans == false) // MAVLink.MAV_RESULT.UNSUPPORTED)
                        {
                            //return;
                        }
                    }
                    catch (Exception ex) {  }

                    if (mavlinkCANRun)
                    {
                        Thread.Sleep(1000);
                    }
                }
            });

            var port = new CommsInjection();

            var can = new DroneCAN.DroneCAN();

            can.FrameReceived += (frame, payload) =>
            {
                //https://github.com/dronecan/pydronecan/blob/master/dronecan/driver/mavcan.py#L114
                //if frame.extended:
                //  message_id |= 1 << 31

                if (payload.packet_data.Length > 8)
                {
                    MainV2.comPort.sendPacket(new MAVLink.mavlink_canfd_frame_t(BitConverter.ToUInt32(frame.packet_data, 0) + (frame.Extended ? 0x80000000: 0),
                                                                                (byte)MainV2.comPort.sysidcurrent,
                                                                                (byte)MainV2.comPort.compidcurrent, (byte)(bus - 1), (byte)DroneCAN.DroneCAN.dataLengthToDlc(payload.packet_data.Length),
                                                                                payload.packet_data),
                                              (byte)MainV2.comPort.sysidcurrent,
                                              (byte)MainV2.comPort.compidcurrent);
                }
                else
                {
                    var frame2 = new MAVLink.mavlink_can_frame_t(BitConverter.ToUInt32(frame.packet_data, 0) + (frame.Extended ? 0x80000000 : 0),
                                                                 (byte)MainV2.comPort.sysidcurrent,
                                                                 (byte)MainV2.comPort.compidcurrent, (byte)(bus - 1), (byte)DroneCAN.DroneCAN.dataLengthToDlc(payload.packet_data.Length),
                                                                 payload.packet_data);
                    MainV2.comPort.sendPacket(frame2,
                                              (byte)MainV2.comPort.sysidcurrent,
                                              (byte)MainV2.comPort.compidcurrent);
                }
            };

            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
            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);

            SetupSLCanPort(port);
        }
        private void but_slcanmavlink_Click(object sender, EventArgs e)
        {
            byte bus = 1;

            Task.Run(() =>
            {
                mavlinkCANRun = true;
                // send every second, timeout is in 5 seconds
                while (mavlinkCANRun)
                {
                    // setup forwarding on can port 1
                    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);

                    if (ans == false) // MAVLink.MAV_RESULT.UNSUPPORTED)
                    {
                        return;
                    }

                    Thread.Sleep(1000);
                }
            });

            var port = new CommsInjection();

            var can = new DroneCAN.DroneCAN();

            can.FrameReceived += (frame, payload) =>
            {
                if (payload.packet_data.Length > 8)
                {
                    MainV2.comPort.sendPacket(new MAVLink.mavlink_canfd_frame_t(BitConverter.ToUInt32(frame.packet_data, 0),
                                                                                (byte)MainV2.comPort.sysidcurrent,
                                                                                (byte)MainV2.comPort.compidcurrent, (byte)(bus), (byte)payload.packet_data.Length,
                                                                                payload.packet_data),
                                              (byte)MainV2.comPort.sysidcurrent,
                                              (byte)MainV2.comPort.compidcurrent);
                }
                else
                {
                    MainV2.comPort.sendPacket(new MAVLink.mavlink_can_frame_t(BitConverter.ToUInt32(frame.packet_data, 0),
                                                                              (byte)MainV2.comPort.sysidcurrent,
                                                                              (byte)MainV2.comPort.compidcurrent, (byte)(bus), (byte)payload.packet_data.Length,
                                                                              payload.packet_data),
                                              (byte)MainV2.comPort.sysidcurrent,
                                              (byte)MainV2.comPort.compidcurrent);
                }
            };

            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
            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);
            }, true);

            SetupSLCanPort(port);
        }