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.

439 lines
17 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 but_armdisarm_Click(object sender, EventArgs e)
{
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
req.target_system = 1;
req.target_component = 1;
req.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;
req.param1 = armed ? 0 : 1;
armed = !armed;
/*
req.param2 = p2;
req.param3 = p3;
req.param4 = p4;
req.param5 = p5;
req.param6 = p6;
req.param7 = p7;
*/
byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);
serialPort1.Write(packet, 0, packet.Length);
try
{
var ack = readsomedata<MAVLink.mavlink_command_ack_t>(sysid, compid);
if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
{
}
}
catch
{
}
}
private void CMB_comport_Click(object sender, EventArgs e)
{
CMB_comport.DataSource = SerialPort.GetPortNames();
}
private void but_mission_Click(object sender, EventArgs e)
{
MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t();
req.target_system = 1;
req.target_component = 1;
// set wp count
req.count = 1;
byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_COUNT, req);
//Console.WriteLine("MISSION_COUNT send");
serialPort1.Write(packet, 0, packet.Length);
var ack = readsomedata<MAVLink.mavlink_mission_request_t>(sysid, compid);
if (ack.seq == 0)
{
MAVLink.mavlink_mission_item_int_t req2 = new MAVLink.mavlink_mission_item_int_t();
req2.target_system = sysid;
req2.target_component = compid;
req2.command = (byte)MAVLink.MAV_CMD.WAYPOINT;
req2.current = 1;
req2.autocontinue = 0;
req2.frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
req2.y = (int) (115 * 1.0e7);
req2.x = (int) (-35 * 1.0e7);
req2.z = (float) (2.34);
req2.param1 = 0;
req2.param2 = 0;
req2.param3 = 0;
req2.param4 = 0;
req2.seq = 0;
packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT, req2);
//Console.WriteLine("MISSION_ITEM_INT send");
lock (readlock)
{
serialPort1.Write(packet, 0, packet.Length);
var ack2 = readsomedata<MAVLink.mavlink_mission_ack_t>(sysid, compid);
if ((MAVLink.MAV_MISSION_RESULT) ack2.type != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
}
}
MAVLink.mavlink_mission_ack_t req3 = new MAVLink.mavlink_mission_ack_t();
req3.target_system = 1;
req3.target_component = 1;
req3.type = 0;
packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, req3);
//Console.WriteLine("MISSION_ACK send");
serialPort1.Write(packet, 0, packet.Length);
}
}
private void simpleexample_Load(object sender, EventArgs e)
{
}
private void listBox1_SelectedIndexChanged(object sender, EventArgs e)
{
}
private void listBox1_DoubleClick(object sender, EventArgs e)
{
}
}
}