3.2 MAVLink通信解析

本节导航

3.2.1 MAVLink介绍

MAVLink (Micro Air Vehicle Link）是一种用于小型无人载具的通信协议，于2009年首次发布。该协议广泛应用于地面站（Ground Control Station,GCS）与无人载具（Unmanned vehicles）之间的通信，同时也应用在载具上机载计算机与Pixhawk之间的内部通信中，协议以消息库的形式定义了参数传输的规则。MAVLink协议支持无人固定翼飞行器、无人旋翼飞行器、无人车辆等多种载具。

官方使用文档网站： https://mavlink.io/en/messages/common.html
MAVLink源码： https://github.com/mavlink/mavlink
基于MAVLink的QGroundControl地面站源码：https://github.com/mavlink/qgroundcontrol

3.2.2 MAVLink本质

MAVLink的本质就是字节流的封装与解析协议

MAVLink 1的数据包格式如下：
MAVLink 2的数据包如下：

3.2.3 解析原理

读取：所有字节流存入buffer，依次读取buffer中的字节数据，遇到STX标志位（MAVLink v1的标志位是0xFE，v2的标志位为0xFD）开始识别一条消息直到消息尾部，如果消息校验正确则将消息发送给处理程序
发送：按上一页将消息转换成字节流

3.2.4 接收解析源码分析

给定一定长度的字节流buffer，长度为length，通过下列脚本解析，每解析出一个mavlink数据包就执行onMavLinkMessage函数

for(int i = 0 ; i < length ; ++i){
      msgReceived = mavlink_parse_char(MAVLINK_COMM_1, (uint8_t)buffer[i], &message, &status);
if(msgReceived){
      emit onMavLinkMessage(message);
 }
}

其中： void onMavLinkMessage(mavlink_message_t message);是得到一个MAVLink消息包后的处理函数，需要根据这个消息的ID来识别当前包的用途（心跳包，GPS位置，姿态等），并提取出感兴趣的数据。

解析函数实现如下，根据message.msgid跳到对应的_decode函数，解码出数据

void onMavLinkMessage(mavlink_message_t message){
  switch (message.msgid){
    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:{
      mavlink_global_position_int_t gp;
      mavlink_msg_global_position_int_decode(&message, &gp);
      outHilData.time_boot_ms = m_LastReceiveMavMsg;
      outHilData.GpsPos[0]=gp.lat;
      outHilData.GpsPos[1]=gp.lon;
      outHilData.GpsPos[2]=gp.alt;
      outHilData.relative_alt = gp.relative_alt;
      outHilData.GpsVel[0]=gp.vx;
      outHilData.GpsVel[1]=gp.vy;
      outHilData.GpsVel[2]=gp.vz;
      outHilData.hdg = gp.hdg;
      break;
     }
    } 
  }

3.2.5 发送源码解析 — 发送一条mavlink_hil_actuator_controls消息

void sendHILCtrlMessage(uint8_t modes, uint64_t flags, float ctrl[])

{
mavlink_hil_actuator_controls_t hilctrl;
hilctrl.mode = modes;
hilctrl.flags = flags;
for(int i=0;i<16;i++){
hilctrl.controls[i]=ctrl[i];
}
mavlink_message_t mess;
mavlink_msg_hil_actuator_controls_encode(SystemID, TargetCompID, &mess, &hilctrl);
char buffer[500];
memset(buffer,0,500);
unsigned int length = mavlink_msg_to_send_buffer((uint8_t*)buffer, & mess);
udp.writeDatagram(buffer,length);//通过UDP或者串口将buffer发送出去即可
}

3.2.6 MAVLink消息包的ID列表

https://mavlink.io/en/messages/common.html

3.2.7 QGC地面站查看MAVLink消息

在QGC的MAVLinkInspector页面中可以浏览Pixhawk发送的所有MAVLink包，查看各个包的频率以及具体数值

3.2.8 MAVLink 2的源码

打开文件夹“RflySimAPIs\SimulinkControlAPI\MavlinkDemo\mavlink\v2.0\common” 可以看到MAVLink的C++源代码，里面包含了所有消息包的定义

3.2.9 MAVLink协议的Simulink封装与解析实现

打开例程“RflySimAPIs\SimulinkControlAPI\MavlinkDemo\MavlinkCodeDecode.slx”

点击运行之后，可以看到我们在Simulink中将数据封装成了字节流data（uint8字节流）和len（字节流长度），然后又经过一个解析函数，将字节流解析成了发送数据。

本例子通过S-Function Builder实现，它在运行时会自动调用mavlink的头文件，并编译成下图所示的.c/.mexw64/.tlc等文件

本例程可以教会大家如何在Simulink中调用外部C/C++头文件来实现自己算法。

3.2.10 Simulink S-Function编写方法

打开.slx文件，并从Simulink-User-Defined Functions中拖入一个S-Function Builder模块，双击它可以得到图

下面我们展示用这个模块生成前文提到的Mavlink消息：MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS

对模块命名，并在DataProperties页面设置输入输出参数名称、维度、数据类型

引入Mavlink头文件库

在Libraries标签页的includes框内加入下列代码 #define inline __inline #include ".\mavlink\v2.0\common\mavlink.h"

在Outputs标签页中，添加获取输入数据，并打包成Mavlink消息的C/C++代码，放到输出口data和len中。其中data是uint8的矩阵，len是数据有效长度。

注意：Simulink S-function的输入输出信号没有标量的概念，所有信号都是向量。因此假设一个输出len是一维标量，像“len=***”这种赋值语句是错误的，要用“len[0]=***”的形式。

勾选生成TLC和MEX-file的选项，再点击编译按钮，就可以得到右图所示Simulink可调用的文件。

下面我们来构建一个Mavlink消息的解码模块
• 命名为“mavlink_msg_receiver” • 输入输出口，和刚才的模块完全相反

如下图所示，在Outputs标签页中设置解码字节流并解析出Mavlink消息的代码。

同样的道理，在库文件页面，导入Mavlink库文件

设置好编译选项后，点击“build”按钮，查看是否可以正确生成tlc和mex文件。

3.2.11 通过Simulink/MAVLink向飞控发送解锁指令（仅限RflySim高级版）

插上Pixhawk，打开CopterSim，设置HITL仿真，UDP_Mode设置为Mavlink_FULL（仅限RflySim高级版），然后点击“开始仿真”按钮

打开例程“MavlinkDemo\MavSfunTest_Arm.slx”并运行，可以在CopterSim的消息框中看到“Command ARM/DISARMACCECPTED”说明实验成功

3.2.12 通过MAVLink模拟发送遥控器数据（仅限RflySim高级版）

和上一步相同，打开CopterSim硬件在环，并运行“MavlinkDemo\ MavSfunTest_controlRC.slx”例程，可以控制Pixhawk的解锁，并发送遥控器数据，控制飞机起降、飞行等。

3.2.13 Simulink通过串口收发MAVLink消息

连接Pixhawk到电脑，用CopterSim开始硬件在环仿真（不需要设置UDP_Mode因此RflySim基础版也适用），用一个数传模块（分别连飞控的TELEM端口和电脑的USB串口）连接Pixhawk到电脑，记住数传模块的串口号（如果没有数传，可以关闭CopterSim，直接插上Pixhawk，这里输入Pixhawk的串口号也行）。

打开“MavlinkDemo\MavSfunTest_SerialCom.slx”，双击“Mavlink SerialInput&Output”，在其中输入数传（或Pixhawk飞控）串口号，并配置波特率，其中Pixhawk使用115200的波特率，数传通常使用56700的波特率

在本例中可以通过串口获取Pixhawk的数据，并发送控制指令。本例子可以直接用于Pixhawk多旋翼的真机实时（通过数传）控制。

3.2 MAVLink通信解析