using System; using System.Collections.Generic; using System.ComponentModel; using System.ComponentModel.DataAnnotations; 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 = 115200; // open the comport serialPort1.Open(); // set timeout to 2 seconds serialPort1.ReadTimeout = 2000; BackgroundWorker bgw = new BackgroundWorker(); bgw.DoWork += bgw_DoWork; bgw.RunWorkerAsync(); } public static DisplayAttribute GetDisplayAttributesFrom(Enum enumValue, Type enumType) { return enumType.GetMember(enumValue.ToString()) .First() .GetCustomAttribute(); } int mav_messages_counter = 0; 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)) { mav_messages_counter++; if (mav_messages_counter > 0) mav_messages_Label.ForeColor = Color.Green; listBox1.Items.Add(packet.msgtypename); mav_messages_Label.Text = "Found " + mav_messages_counter + " messages."; } })); if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT){ var heartbeat = (MAVLink.mavlink_heartbeat_t)packet.data; richTextBox1.Invoke(new Action(() => { txt_vehicleType.Text = ((MAVLink.MAV_TYPE)heartbeat.autopilot).ToString(); txt_autopilot.Text = ((MAVLink.MAV_AUTOPILOT)heartbeat.autopilot).ToString(); txt_baseMode.Text = ((MAVLink.MAV_MODE_FLAG)heartbeat.base_mode).ToString(); txt_system.Text = ((MAVLink.MAV_STATE)heartbeat.system_status).ToString(); })); } } 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) { } } }