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.

205 lines
6.4 KiB

using System;
using System.Collections.Generic;
using System.ComponentModel;
5 years ago
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
{
5 years ago
public partial class client : Form
{
5 years ago
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();
}
5 years ago
public static DisplayAttribute GetDisplayAttributesFrom(Enum enumValue, Type enumType)
{
return enumType.GetMember(enumValue.ToString())
.First()
.GetCustomAttribute<DisplayAttribute>();
}
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;
5 years ago
//Console.WriteLine(packet.msgtypename);
listBox1.Invoke(new Action(() =>
{
if (!listBox1.Items.Contains(packet.msgtypename))
{
listBox1.Items.Add(packet.msgtypename);
}
}));
5 years ago
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT){
var heartbeat = (MAVLink.mavlink_heartbeat_t)packet.data;
var heartbeat_string = "Vehicle type: " + "" +
"\n Autopilot: " + "" +
"\n Base mode: " + heartbeat.base_mode +
"\n Custom mode: " + heartbeat.custom_mode +
"\n Mavlink Version: " + heartbeat.mavlink_version +
"\n System status: " + heartbeat.system_status;
richTextBox1.Invoke(new Action(() =>
{
richTextBox1.Text = heartbeat_string;
}));
}
}
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)
{
}
}
}