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.

328 lines
14 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 SimpleExample
{
public partial class simpleexample : Form
{
MAVLink.MavlinkParse mavlink = new MAVLink.MavlinkParse();
bool armed = false;
// locking to prevent multiple reads on serial port
object readlock = new object();
// our target sysid
byte sysid;
// our target compid
byte compid;
// Giak stuff
List<uint> listItemIds = new List<uint>();
Dictionary<int, object> droneDataDictionary = new Dictionary<int, object>();
public simpleexample()
{
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);
// Update the data in the dictionary.
//
droneDataDictionary[(int)packet.msgid] = packet.data;
listBox1.Invoke(new Action(() =>
{
if (!listBox1.Items.Contains(packet.msgtypename))
{
listBox1.Items.Add(packet.msgtypename);
listItemIds.Add(packet.msgid);
}
}));
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);
}
string d = GetData((int)packet.msgid, packet.data);
BeginInvoke(new Action(() => richTextBox1.Text = d));
}
catch(Exception ex)
{
MessageBox.Show(ex.ToString());
}
System.Threading.Thread.Sleep(1);
}
}
private string GetData(int id, object droneData)
{
string ret = "";
if (id == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
MAVLink.mavlink_heartbeat_t data = (MAVLink.mavlink_heartbeat_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_heartbeat_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.POWER_STATUS)
{
MAVLink.mavlink_power_status_t data = (MAVLink.mavlink_power_status_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_power_status_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.LOCAL_POSITION_NED)
{
MAVLink.mavlink_local_position_ned_t data = (MAVLink.mavlink_local_position_ned_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_local_position_ned_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.VIBRATION)
{
MAVLink.mavlink_vibration_t data = (MAVLink.mavlink_vibration_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_vibration_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.VFR_HUD)
{
MAVLink.mavlink_vfr_hud_t data = (MAVLink.mavlink_vfr_hud_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_vfr_hud_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.SIMSTATE)
{
MAVLink.mavlink_simstate_t data = (MAVLink.mavlink_simstate_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_simstate_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.AHRS2)
{
MAVLink.mavlink_ahrs2_t data = (MAVLink.mavlink_ahrs2_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_ahrs2_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.AHRS3)
{
MAVLink.mavlink_ahrs3_t data = (MAVLink.mavlink_ahrs3_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_ahrs3_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
{
MAVLink.mavlink_attitude_t data = (MAVLink.mavlink_attitude_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_attitude_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT)
{
MAVLink.mavlink_mission_current_t data = (MAVLink.mavlink_mission_current_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_mission_current_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW)
{
MAVLink.mavlink_servo_output_raw_t data = (MAVLink.mavlink_servo_output_raw_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_servo_output_raw_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
else if (id == (int)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW)
{
MAVLink.mavlink_servo_output_raw_t data = (MAVLink.mavlink_servo_output_raw_t)droneDataDictionary[id];
foreach (var field in typeof(MAVLink.mavlink_servo_output_raw_t).GetFields(BindingFlags.Instance |
BindingFlags.NonPublic |
BindingFlags.Public))
{
ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
}
}
return ret == "" ? "" : ret.Substring(0, ret.Length-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)
{
}
}
}