using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.IO.Ports; using System.Linq; using System.Reflection; using System.Text; using System.Threading.Tasks; using System.Windows.Forms; namespace Client { public partial class client : Form { MAVLink.MavlinkParse mavlink = new MAVLink.MavlinkParse(); // locking to prevent multiple reads on serial port object readlock = new object(); // our target sysid byte sysid; // our target compid byte compid; public client() { InitializeComponent(); } private void but_connect_Click(object sender, EventArgs e) { // if the port is open close it if (serialPort1.IsOpen) { serialPort1.Close(); return; } // set the comport options serialPort1.PortName = CMB_comport.Text; serialPort1.BaudRate = int.Parse(cmb_baudrate.Text); // open the comport serialPort1.Open(); // set timeout to 2 seconds serialPort1.ReadTimeout = 2000; BackgroundWorker bgw = new BackgroundWorker(); bgw.DoWork += bgw_DoWork; bgw.RunWorkerAsync(); } void bgw_DoWork(object sender, DoWorkEventArgs e) { while (serialPort1.IsOpen) { try { MAVLink.MAVLinkMessage packet; lock (readlock) { // read any valid packet from the port packet = mavlink.ReadPacket(serialPort1.BaseStream); // check its valid if (packet == null || packet.data == null) continue; } // check to see if its a hb packet from the comport if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t)) { var hb = (MAVLink.mavlink_heartbeat_t)packet.data; // save the sysid and compid of the seen MAV sysid = packet.sysid; compid = packet.compid; // request streams at 2 hz var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, new MAVLink.mavlink_request_data_stream_t() { req_message_rate = 2, req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL, start_stop = 1, target_component = compid, target_system = sysid }); serialPort1.Write(buffer, 0, buffer.Length); buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); serialPort1.Write(buffer, 0, buffer.Length); } // from here we should check the the message is addressed to us if (sysid != packet.sysid || compid != packet.compid) continue; Console.WriteLine(packet.msgtypename); listBox1.Invoke(new Action(() => { if (!listBox1.Items.Contains(packet.msgtypename)) { listBox1.Items.Add(packet.msgtypename); } })); if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) //or //if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t)) { var att = (MAVLink.mavlink_attitude_t)packet.data; //Console.WriteLine(packet.msgtypename + " => " + att.pitch*57.2958 + " " + att.roll*57.2958); } } catch (Exception ex) { MessageBox.Show(ex.ToString()); } System.Threading.Thread.Sleep(1); } } T readsomedata(byte sysid, byte compid, int timeout = 2000) { DateTime deadline = DateTime.Now.AddMilliseconds(timeout); lock (readlock) { // read the current buffered bytes while (DateTime.Now < deadline) { var packet = mavlink.ReadPacket(serialPort1.BaseStream); // check its not null, and its addressed to us if (packet == null || sysid != packet.sysid || compid != packet.compid) continue; //Console.WriteLine(packet); if (packet.data.GetType() == typeof(T)) { return (T)packet.data; } } } throw new Exception("No packet match found"); } private void CMB_comport_Click(object sender, EventArgs e) { CMB_comport.DataSource = SerialPort.GetPortNames(); } private void simpleexample_Load(object sender, EventArgs e) { } private void listBox1_SelectedIndexChanged(object sender, EventArgs e) { } private void listBox1_DoubleClick(object sender, EventArgs e) { } } }