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列表
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
多旋翼的真机实时(通过数传)控制。