Skip to content

Commit

Permalink
SimpleExample: update
Browse files Browse the repository at this point in the history
  • Loading branch information
@meee1
meee1 committed Apr 1, 2016
1 parent 3370fc4 commit cb7ebaf
Show file tree
Hide file tree
Showing 4 changed files with 116 additions and 42 deletions.
1 change: 1 addition & 0 deletions ExtLibs/Mavlink/MAVLink.csproj
View file
Expand Up @@ -43,6 +43,7 @@
<ItemGroup>
<Compile Include="MavlinkCRC.cs" />
<Compile Include="Mavlink.cs" />
<Compile Include="MAVLinkMessage.cs" />
<Compile Include="MAVLinkParam.cs" />
<Compile Include="MAVLinkParamList.cs" />
<Compile Include="MavlinkParse.cs" />
Expand Down
29 changes: 29 additions & 0 deletions ExtLibs/Mavlink/MAVLinkMessage.cs
View file
@@ -0,0 +1,29 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

public partial class MAVLink
{
public class MAVLinkMessage
{
public byte header;
public byte length;
public byte seq;
public byte sysid;
public byte compid;
public byte messid;
public object data;

public MAVLinkMessage(byte header1, byte length1, byte seq1, byte sysid1, byte compid1, byte messid1, object data)
{
this.header = header1;
this.length = length1;
this.seq = seq1;
this.sysid = sysid1;
this.compid = compid1;
this.messid = messid1;
this.data = data;
}
}
}
4 changes: 2 additions & 2 deletions ExtLibs/Mavlink/MavlinkParse.cs
View file
Expand Up @@ -107,7 +107,7 @@ public byte[] ReadPacket(Stream BaseStream)
return buffer;
}

public object ReadPacketObj(Stream BaseStream)
public MAVLinkMessage ReadPacketObj(Stream BaseStream)
{
byte[] buffer = ReadPacket(BaseStream);

Expand All @@ -124,7 +124,7 @@ public object ReadPacketObj(Stream BaseStream)
// fill in the data of the object
MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6);

return data;
return new MAVLinkMessage(header, length, seq, sysid, compid, messid, data);
}

public byte[] GenerateMAVLinkPacket(MAVLINK_MSG_ID messageType, object indata)
Expand Down
124 changes: 84 additions & 40 deletions ExtLibs/SimpleExample/simpleexample.cs
View file
Expand Up @@ -15,6 +15,12 @@ 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;

public simpleexample()
{
Expand All @@ -40,60 +46,98 @@ private void but_connect_Click(object sender, EventArgs e)
// set timeout to 2 seconds
serialPort1.ReadTimeout = 2000;

// request streams - asume target is at 1,1
mavlink.GenerateMAVLinkPacket(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 = 1,
target_system = 1
});

while (serialPort1.IsOpen)
{
BackgroundWorker bgw = new BackgroundWorker();

bgw.DoWork += bgw_DoWork;

bgw.RunWorkerAsync();
}

void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
// try read a hb packet from the comport
var hb = readsomedata<MAVLink.mavlink_heartbeat_t>();

var att = readsomedata<MAVLink.mavlink_attitude_t>();

Console.WriteLine(att.pitch*57.2958 + " " + att.roll*57.2958);
}
catch
{
}

System.Threading.Thread.Sleep(1);
Application.DoEvents();

}
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
packet = mavlink.ReadPacketObj(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
mavlink.GenerateMAVLinkPacket(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
});
}

// from here we should check the the message is addressed to us
if (sysid != packet.sysid || compid != packet.compid)
continue;

if (packet.messid == (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(att.pitch*57.2958 + " " + att.roll*57.2958);
}
}
catch
{
}

System.Threading.Thread.Sleep(1);
}
}

T readsomedata<T>(int timeout = 2000)
T readsomedata<T>(byte sysid,byte compid,int timeout = 2000)
{
DateTime deadline = DateTime.Now.AddMilliseconds(timeout);

// read the current buffered bytes
while (DateTime.Now < deadline)
lock (readlock)
{
var packet = mavlink.ReadPacketObj(serialPort1.BaseStream);
// read the current buffered bytes
while (DateTime.Now < deadline)
{
var packet = mavlink.ReadPacketObj(serialPort1.BaseStream);

if (packet == null)
continue;
// check its not null, and its addressed to us
if (packet == null || sysid != packet.sysid || compid != packet.compid)
continue;

Console.WriteLine(packet);
Console.WriteLine(packet);

if (packet.GetType() == typeof(T))
{
return (T)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)
{
Expand Down Expand Up @@ -121,7 +165,7 @@ private void but_armdisarm_Click(object sender, EventArgs e)

try
{
var ack = readsomedata<MAVLink.mavlink_command_ack_t>();
var ack = readsomedata<MAVLink.mavlink_command_ack_t>(sysid, compid);
if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
{

Expand Down

0 comments on commit cb7ebaf

Please sign in to comment.