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