public static void RequestDataStream(int streamId, int messageRate, int startStop)
{
// This is from the MissionPlanner. Don't change this code as this is written in BLOOD
MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
req.target_system = 0;
req.target_component = 0;
req.req_message_rate = (ushort)messageRate;
req.start_stop = (byte)startStop;
req.req_stream_id = (byte)streamId;
byte[] bytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
SendPacket(bytes);
}
public static void SetNoUpdates()
{
byte dataSteamType = 0; // ALL data streams
ushort updateRate = 1; // 1Hz
MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
req.target_system = sysID;
req.target_component = compID;
req.req_message_rate = updateRate;
req.start_stop = 0; // start
req.req_stream_id = dataSteamType; // id
byte[] bytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
SendPacket(bytes);
Thread.Sleep(100);
SendPacket(bytes);
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try//首先需要申请全部数据包，否则只能收到心跳包！*****
{
//只有此处申请66号信息包才能获得全部参数！
//放在监听中无法实时触发！
MAVLink.mavlink_request_data_stream_t r = new MAVLink.mavlink_request_data_stream_t();
r.req_message_rate = 2;
r.req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL;
r.start_stop = 1;
r.target_component = compid;
r.target_system = sysid;
byte[] packetRequest = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, r);
serialPort1.Write(packetRequest, 0, packetRequest.Length);
MAVLink.MAVLinkMessage packet; //定义一个mavlink消息解析类
lock (readlock) //锁定读取端口的当前数据当前数据
{
//读取端口的数据流
packet = mavlink.ReadPacket(serialPort1.BaseStream);
if (packet == null || packet.data == null)
{
continue;
}
}
//判断是否为心跳包，如果是向向飞控申请数据流
//
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// 保存包sysid和compid参数
sysid = packet.sysid;
compid = packet.compid;
Console.WriteLine("飞控数据接收成功：");
//向飞控申请数据流
/* mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
* //飞控数据流的格式要求
* new MAVLink.mavlink_request_data_stream_t()
* {//Mavlink的一个结构体数组
* req_message_rate = 2,//信息频率2hz
* //数据流ID，ALL枚举=0，代表获得全部信息
* req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
*
* start_stop = 1,//1开始发送，0停止发送
* target_component = compid,
* target_system = sysid
* });*/
}
// 核对发送的信息
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
// Console.WriteLine("packet.msgid = " + packet.msgid);
if (packet.msgid == 0)
{
var mode1 = (MAVLink.mavlink_heartbeat_t)packet.data;
Console.WriteLine("当前模式" + mode1.base_mode);
string s = mode1.base_mode.ToString();
int n = Convert.ToInt32(s);
/* MAV_MODE ****
* 0 MAV_MODE_PREFLIGHT
* 80 MAV_MODE_STABILIZE_DISARMED
* 208 MAV_MODE_STABILIZE_ARMED
* 88 MAV_MODE_GUIDED_DISARMED
* 216 MAV_MODE_GUIDED_ARMED
*/
string str = null;
switch (n)
{
case 1: str = "设备故障"; break;
case 81: str = "自稳锁定"; break;
case 209: str = "自稳解锁"; break;
case 89: str = "引导锁定"; break;
case 217: str = "引导解锁"; break;
default: str = "其他模式"; break;
}
if (fly.Text.Equals("已起飞") && str.Equals("自稳锁定"))
{
fly.Text = "等待起飞..";
}
textBox1.Text = str;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ALTITUDE)
//#141号信息包
{
var alti = (MAVLink.mavlink_altitude_t)packet.data;
Console.WriteLine("当前高度" + alti.altitude_relative);
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT)
//#24号信息包
{
var sta = (MAVLink.mavlink_gps_raw_int_t)packet.data;
Console.WriteLine("地速度" + sta.vel / 100.0);
Console.WriteLine("卫星数量" + sta.satellites_visible);
int n = (int)(sta.satellites_visible & 0xFF);
gps.Text = Convert.ToString(n);
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
//#33号信息包
{
var glo = (MAVLink.mavlink_global_position_int_t)packet.data;
Console.WriteLine("垂直速度" + glo.vz / 100.0);
Console.WriteLine("相对高度" + glo.relative_alt / 1000.0);
}
if (packet.msgid == 62)
//#62号信息包 NAV_CONTROLLER_OUTPUT
{
var nav = (MAVLink.mavlink_nav_controller_output_t)packet.data;
Console.WriteLine("高度误差" + nav.alt_error);
error.Text = nav.alt_error.ToString("f2");
}
if (packet.msgid == 74)
//#74号信息包 VFR_HUD
{
var vfr = (MAVLink.mavlink_vfr_hud_t)packet.data;
Console.WriteLine("空速" + vfr.airspeed);
airv.Text = Convert.ToString(vfr.airspeed);
Console.WriteLine("地速" + vfr.groundspeed);
grov.Text = Convert.ToString(vfr.groundspeed);
Console.WriteLine("高度" + vfr.alt);
alt.Text = Convert.ToString(vfr.alt);
Console.WriteLine("升降速度" + vfr.climb);
clime.Text = Convert.ToString(vfr.climb);
}
}
catch
{
}
System.Threading.Thread.Sleep(1);//while循环通常加这句，防止堵死
}
}
