You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

184 lines
5.6 KiB

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<T>(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)
{
}
}
}