Mavlink地面站编写之一--协议分析

阿木社区:转载注意出处 http://www.amovauto.com

        Pixhawk/APM都是采用MAVLINK协议实现的飞控的数据链路传输。先简单介绍下mavlink协议。Mavlink协议最早由 苏黎世联邦理工学院 计算机视觉与几何实验组 的 Lorenz Meier于2009年发布，并遵循LGPL开源协议。Mavlink协议是在串口通讯基础上的一种更高层的开源通讯协议，主要应用在微型飞行器（micro aerial vehicle）的通讯上。Mavlink是为小型飞行器和地面站（或者其他飞行器）通讯时常常用到的那些数据制定一种发送和接收的规则并加入了校验（checksum）功能。据说亿航四轴的初期版本就是参考的MAVLINK协议或者说参考的APM飞控系统。
下面是mavlink的消息简介和官方指导连接：

https://pixhawk.ethz.ch/mavlink/
http://qgroundcontrol.org/mavlink/start
1.1 mavlink消息结构分析：
MAVLINK的传输基本单位是消息帧，一帧的数据长度8bytes到263bytes不等

                                                                   图1.1

如图所示的消息帧结构，除了灰色外，其他格子都代表一个字节的数据，灰色格子里面的数据长度是不固定的。下面是每个消息帧的详细解释：

图1.2

上文中已经提到了在mavlink消息帧里最重要的两个东西，一个是msgid；一个是payload，前者是payload中内容的编号，后者则存放了消息。消息有许多种类型，在官网的网页中中以蓝色的“#”加数字的方式来表示消息的编号如 “#0”（这样的表示方法应该是为了方便在网页中查找相应编号消息的定义）。在官网介绍网页里往下拉，大概拉到二分之一的位置处，开始出现“MAVLink Messages”的介绍，往下看是各种消息的数据组成说明。下面将以几个消息为例，讲解mavlink消息

图1.2

如图所示这是一个APM2.8的控制板输出的一帧心跳包数据，其固件是无人车的固件，可以详细的看到包开始标志，有效载荷长度，消息ID等等。从结构来讲还是比较简单的。这是一个心跳包的一帧数据，因为他的消息ID是00就代表这是一帧心跳包。

心跳包结构如下:

图1.3

Type表示设备类型在MAV_TYPE有定义可以在https://pixhawk.ethz.ch/mavlink/ 的网页中搜索到，从图1.2可以看到，10是第一位的数据值，那么10在MAV_TYPE代表Ground rover(地面车辆)，这个数据就是从地面车辆的固件里面发出的。同理分析这9个字节所代表的含义可以知道心跳包所代表的含义。在上面网站的文档中可以分析到蓝色(#0,#1,#2)

这样的数据就是代表数据包。在文档的1/2处往后都可以看到，一共有254条消息类型。

第二个参数是自驾仪（即通常所说的飞控）类型，比如apm，ppz，Pixhawk等飞控，具体定义查找和之前查找飞行器类型时的方法一样。同样的，对于发送心跳包的飞行器来说代表了自己的飞控类性，对地面站发出的心跳包来说意义不大。

第三个参数是基本模式（base mode），是指飞控现在处在哪个基本模式，对于发心跳包的地面站来说没有意义，对于发送心跳包的飞控来说是有意义的。这个参数要看各个飞控自己的定义方式，mavlink介绍网页并不会给出具体的模式。在Pixhawk中基本模式可以分为使用用户模式（custom mode）还是基本模式（这里有点绕，其实是就是是否使用用户模式）。使用用户模式将在讲下个参数时说明，使用基本模式又会分为自动模式（auto），位置控制模式（posctl）和手动模式（manual）。一般情况下都会使用用户模式，普通用户不用关心这个参数。

第四个参数是用户模式（custom mode），大概说一下Pixhawk的用户模式。以多轴为例。它分为主模式（main mode）和子模式（sub mode），两种模式组合在一起成为最终的模式，主模式分为3种，手动（manual），辅助（assist），自动（auto）。手动模式类似apm的姿态模式。在辅助模式中，又分为高度控制模式（altctl）和位置控制模式（posctl）两个子模式，高度控制模式就类似apm的定高模式，油门对应到飞行器高度控制上。位置模式控制飞行器相对地面的速度，油门和高度控制模式一样，yaw轴控制和手动模式一样。自动模式里又分为3个子模式，任务模式（mission），留待模式（loiter），返航模式（return），任务模式就是执行设定好的航点任务，留待模式就是gps悬停模式，返航模式就是直线返回home点并自动降落。在apm里这个参数貌似是没有用的，注意这个数据占了4个字节，在Pixhawk中，前两个字节（低位）是保留的，没有用，第三个字节是主模式，第四个字节是子模式。

以上是心跳包的分析，还有很多其他的数据包的分析类似，请自行翻阅官方指导文件。

1.2消息包封装过程

以上各区域信息存在关联，当使用MavLink协议提供的方法封装消息包时，会根据所使用的MSG获取到该类别MSG消息的LEN信息，同时软件（地面站或飞行控制软件）会根据自身状态信息填写SYS、COMP。信息填写完毕生成数据包时，封装方法会自动添加STX，并在上一次发送消息包所使用的SEQ上加1作为本次发送的SEQ写入，当SEQ超过255时，会回到0并重新开始计数。CKA、CKB会在PAYLOAD信息写入后、封装完成之前，根据CRC[Fe1] （CyclicRedundancy Check）循环冗余校验码算法计算得出并自动写入包内。

也就是说，设定SYS和COMP并且正确调用MavLink所提供方法后，整个消息包的生成过程中仅有MSG和PAYLOAD两项内容需要用户关心，消息包封装过程如活动图所示。

1.3消息包示例

本文开始提到MavLink使用消息库的形式定义传输规则，用户可以在在源码中查阅消息库的内容，此处使用Java语言下的消息库作为实例，以便更清晰地展示包结构（MavLink源码自带了多语言的生成器，可从源码中的xml文件转换为对应C，C++，Java等语言的MavLink协议包）。以下表格中，SEQ为计算得出，数值不固定，故用X代替，SYS，COMP两项为笔者使用的Mission Planner地面站设定的系统ID和组件ID，MSG项0代表HEARTBEAT消息的ID，PAYLOAD内存储详细信息，最后的CKA CKB为封包后计算得出，以Y代替。

msg_heartbeat：最基本的心跳信号包，周期性发送，用于确认地面站与无人机之间的连接是否有效。

STX	LEN	SEQ	SYS	COMP	MSG	PAYLOAD	CKA B
254	9	X	255	190	0	……	Y

msg_request_data_stream：数据流请求包，地面站使用该消息包向飞行控制软件提交数据流申请，飞行控制软件收到该消息后将按照设定的参数周期性返回消息包。

STX	LEN	SEQ	SYS	COMP	MSG	PAYLOAD	CKA B
254	6	X	255	190	66	……	Y

大概知道了mavlink协议之后呢，我们就可以用MAVLINK结合APM2.6来实现一个APM的地面站。参考Mission Planner的地面站，Mission Planner是采用C#言语，基于C#的易用性，我们也采用C#语言。不过在编写一个简版的地面站以前。我们可以参考Mission Planner的构架，我们可以安装好VS2013，编译一下Mission Planner源码，如果有能力，可以修改Mission Planner源码。MP地面站的编译方法已经在另一篇文章中详细讲述过，编译完了之后有个SimpleExample的简单代码例程在MP的源码中，我们可以分析下这简单的代码，这个简单的代码是引用了MAVLINK的库，具体位置在MissionPlanner-master\ExtLibs\Mavlink中，如果我们要用只要在新建的工程里面引用即可，知道MAVLINK是怎么解析的。

MAVLINK解析示例

主代码如下：

namespace SimpleExample

{
public partial class simpleexample : Form
{
MAVLink.MavlinkParse mavlink = new MAVLink.MavlinkParse();//MAVLINK解析包函数，通过这个函数从串口得到一帧的数据
bool armed = false;
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;
// 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
});//设置mavlink数据缓冲区格式
while (serialPort1.IsOpen)
{
try
{
// try read a hb packet from the compo
var hb = readsomedata();//读取一帧心跳数据包
var att = readsomedata();//读取一帧姿态包
Console.WriteLine(att.pitch*57.2958 + " " + att.roll*57.2958);//写入命令行
}
catch{
}
System.Threading.Thread.Sleep(1);
Application.DoEvents();
}
}
T readsomedata(int timeout = 2000)//读取数据函数
{
DateTime deadline = DateTime.Now.AddMilliseconds(timeout);
// read the current buffered bytes
while (DateTime.Now < deadline)
{
var packet = mavlink.ReadPacketObj(serialPort1.BaseStream);//读出一帧数据
if (packet == null)
continue;
Console.WriteLine(packet);//打印在控制台
if (packet.GetType() == typeof(T))
{
return (T)packet;//从一帧MAVLINK数据中返回T类型的数据，T类型是在调用处指定的可以是心跳包，或者姿态包等等。
}
}
throw new Exception("No packet match found");
}
MAVLink.MavlinkParse mavlink 最重要的是这个类，这个类是解析一帧数据包用的，我们来分析下
public partial class MAVLink
{
public static void ReadWithTimeout
public byte[] ReadPacket
public object ReadPacketObj//读取一包数据
public byte[] GenerateMAVLinkPacket//生成一个MAVLINK包，可以同个这个函数来生成读取数据包或者写命令数据包，在解析数据和读取MAVLINK数据之前都要先调用这个函数，生成一个空包，以便放入数据或者写入数据，相当于开辟一个MAVLINK数据缓冲区。
}
以上的程序运行如下：

图1.4 读取MAVLINK数据包

可以看到在
var hb = readsomedata();
var att = readsomedata();
在mavlink_heartbeat_t这个类中有心跳包的成员变量六个。
在MAVLink.mavlink_attitude_t这类中有姿态成员变量。我们可以在VS里面清楚的看到。所以如果我们想读取心跳包和姿态包，就可以使用上面的方法来获取心跳和姿态，很方便。通过上面的代码，我们就可以从APM的串口MAVLINK数据流中解析出想要的数据。我们想读取其他数据，在readsomedata中写入不同的形参即可，具体还有其他什么形参可以在VS里面查看。如果我们要更复杂的界面，做出专业的地面站，就去布局控件即可，不过还是比较复杂的，可以参考MP地面站。

更多有关咨询在：http://www.amovauto.com 阿木社区

QQ群:526221258