|
|
@ -10,12 +10,11 @@ using System.Text; |
|
|
|
using System.Threading.Tasks; |
|
|
|
using System.Windows.Forms; |
|
|
|
|
|
|
|
namespace SimpleExample |
|
|
|
namespace Client |
|
|
|
{ |
|
|
|
public partial class simpleexample : Form |
|
|
|
public partial class client : 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
|
|
|
@ -23,11 +22,7 @@ namespace SimpleExample |
|
|
|
// our target compid
|
|
|
|
byte compid; |
|
|
|
|
|
|
|
// Giak stuff
|
|
|
|
List<uint> listItemIds = new List<uint>(); |
|
|
|
Dictionary<int, object> droneDataDictionary = new Dictionary<int, object>(); |
|
|
|
|
|
|
|
public simpleexample() |
|
|
|
public client() |
|
|
|
{ |
|
|
|
InitializeComponent(); |
|
|
|
} |
|
|
@ -69,7 +64,7 @@ namespace SimpleExample |
|
|
|
{ |
|
|
|
// read any valid packet from the port
|
|
|
|
packet = mavlink.ReadPacket(serialPort1.BaseStream); |
|
|
|
|
|
|
|
|
|
|
|
// check its valid
|
|
|
|
if (packet == null || packet.data == null) |
|
|
|
continue; |
|
|
@ -78,9 +73,9 @@ namespace SimpleExample |
|
|
|
// 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; |
|
|
@ -109,17 +104,12 @@ namespace SimpleExample |
|
|
|
|
|
|
|
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); |
|
|
|
} |
|
|
|
|
|
|
|
})); |
|
|
@ -132,11 +122,8 @@ namespace SimpleExample |
|
|
|
//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) |
|
|
|
catch (Exception ex) |
|
|
|
{ |
|
|
|
MessageBox.Show(ex.ToString()); |
|
|
|
} |
|
|
@ -145,150 +132,10 @@ namespace SimpleExample |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
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) |
|
|
|
T readsomedata<T>(byte sysid, byte compid, int timeout = 2000) |
|
|
|
{ |
|
|
|
DateTime deadline = DateTime.Now.AddMilliseconds(timeout); |
|
|
|
|
|
|
|
|
|
|
|
lock (readlock) |
|
|
|
{ |
|
|
|
// read the current buffered bytes
|
|
|
@ -302,9 +149,9 @@ namespace SimpleExample |
|
|
|
|
|
|
|
//Console.WriteLine(packet);
|
|
|
|
|
|
|
|
if (packet.data.GetType() == typeof (T)) |
|
|
|
if (packet.data.GetType() == typeof(T)) |
|
|
|
{ |
|
|
|
return (T) packet.data; |
|
|
|
return (T)packet.data; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
@ -312,16 +159,26 @@ namespace SimpleExample |
|
|
|
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) |
|
|
|
{ |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|