DSP的MAVLink微型无人机通信协议移植与应用

吕强，倪佩佩，王国胜，刘峰

（装甲兵工程学院 控制工程系，北京100072）

引 言

无人机（Unmanned Air Vehicle，UAV）是一种搭载飞行控制器、空气动力承载、无人驾驶并且能够重复使用、搭载多种设备、执行多种任务的飞行器[1-2]。无人机一般包括固定翼飞行器和旋翼式飞行器。其中微小型旋翼机因其具有便携、机动性好、垂直起降、空中悬停的特点而广泛应用于灾害救援、交通巡查、航空摄影、军事侦察等。无人机的地面控制系统可以完成对无人机的飞行操控、航路规划、飞行状态获取及分析、前方图像获取等[3-4]；而一个完善的无人机通信协议则是连接无人机与地面站的重要桥梁。MAVLink是一个开源的基于C/C＋＋或者Python语言的能够对微型飞行器（MAV）进行数据封装和解析的库。国外许多开源的地面站控制软件包括QGround-Control（QGC）、HK Ground Control Station、APM Planner、Copter GCS等，都是基于MAVLink开发并集成一套完整的可视化界面，包括地图、仪表、实时参数曲线绘制和视频显示界面等。因此，基于MAVLink微型无人机通信协议传输无人机参数，可以帮助开发者节省单独开发地面站软件的时间。

由于TMS320F28335（简称F28335）的32位浮点运算单元能够大大提高控制算法的处理速度，故采用该DSP控制器作为飞行控制器，并提出一种基于该控制器的MAVLink微型无人机通信协议的移植与应用方法。

1 MAVLink介绍

表1是MAVLink协议的数据结构。虽然MAVLink只使用1字节FE作为数据包第0位的固定起始标志，但是第1位的有效载荷长度和第（n＋7）～（n＋8）位的两位校验和，能够区别当前接收到的FE是随机噪声还是数据包起始标志。因此MAVLink不仅无需用多个字节作为起始标志，还能保证数据长度简短而又安全。

表1 MAVLink协议的数据结构

2 MAVLink移植方法

F28335的外设寄存器为16位，而一般的微控制器都是8位。MAVLink默认支持8位外设寄存器的微控制器，因此DSP并不兼容MAVLink，无法生成正确的数据。

F28335基于标准C语言使用CCS作为开发环境，具体的移植方法如下：

①在主程序中定义typedef unsigned char uint8＿t，否则程序无法识别uint8＿t。F28335的库文件里面未定义uint8＿t数据类型，而unsigned char数据类型为16位，即使参数定义为8位，也是默认以16位进行存储，不影响编程。

②在主程序中定义大端模式（＃define NATIVE＿BIG＿ENDIAN），并修改 MAVLink库protocol.h头文件中的byte＿swap函数。如修改byte＿swap＿4（）：

MAVLink默认选择小端模式，即数据的低位字节保存在内存的高地址中，而数据的高位字节保存在内存的低地址中。同时也有大端模式的函数代码以满足用户的特殊要求。虽然F28335也是小端模式，但其16位的寄存器会使原本一个寄存器存储1个字节的情况变成2个字节填满一个寄存器，造成移位和串位的现象。因此可以选择设置一个自定义的伪大端模式，即在大端模式代码中对数据进行移位和交换操作，实现我们希望的自定义小端模式，其他函数修改方法类似。

③修改发送序列的计数范围。16位的寄存器导致生成序列current＿tx＿seq的计数范围为0～65 535，而我们只需要0～255，因此对生成序列的高8位清零即可，即current＿tx＿seq＆0x00ff。

④修改校验和代码。对MAVLink库checksum.h头文件中crc＿accumulate（）函数的参数temp也进行高8位清零的操作。MAVLink是按照CRC-16-CCITT标准计算校验和的。其主要思想是对数据包的第1位至第n＋6位进行一系列按位异或、移位和循环累加等操作，生成16位校验和。

⑤修改时间签。部分类型的数据包内的Data包含时间签，以便实时绘制参数变化曲线；而CCS基本库内没有生成时间签的函数，故用户需自定义。可在定时器0的中断函数中定义一个时间累加器。注意区分不同数据包的时间签单位是毫秒还是微秒。

⑥由于CCS库的math.h未定义π/2，故在主程序中定义：

＃define M＿PI＿2 1.57079632679489661923。至此，MAVLink移植方法基本完成。

3 四旋翼无人机与地面站的通信

引言中提到的开源的QGC是基于MAVLink协议编写的一款用户界面友好的地面站控制软件。其连接方式可以选择串口通信或者基于TCP/UDP协议的无线网络。使用XBee模块通过串行通信接口（SCI）连接发送的数据波特率最高只有115 200 bps，尤其在高频率发送大量数据时会发生严重的数据丢失现象；而串行外设接口（SPI）是一个高速同步的串行输入输出接口，并且F28335支持一个16级深度的发送接收FIFO，可减少CPU开销，故采用新力维WM001S V3.0高速SPI转WiFi模块进行四旋翼无人机与地面站实时通信。

3.1 无线网络的建立

配置SPI时有以下三点注意：

①SPI时钟方案选择有相位延时的下降沿，与WiFi模块一致，否则WiFi模块将停止工作。

②WiFi模块最高支持50 MHz的时钟频率，且SPI波特率设置不能过大，否则会造成WiFi模块无法及时响应和数据丢失。

③发送数据以左对齐的方式存储在串行输出缓冲寄存器（SPITXBUF）中，接收数据以右对齐的方式存储在串行输入缓冲寄存器（SPIRXBUF）中。单次发送或接收小于16位数据时，尤其要注意这一点。本文选择单次发送8位数据。

图1是WM001S V3.0WiFi模块发送数据时的流程。向WiFi模块发送RBRS命令（0x40），准备读取WiFi模块当前可写数据大小（VC）和可读数据大小（RC）；发送4次RSR命令（0x00），读取4字节的VC和RC。如果VC≥lenth，lenth为待发送数据包长度，则先发送写命令（0x D0），然后开始发送MAVLink数据包。

图1 WiFi模块发送数据的流程图

F28335 SPIA主机向WiFi从机写数据的代码分三部分。

①SPIA初始化设置：

②自定义SPIA发送单个字节函数：

③定时器0中断中发送数据代码：

3.2 实验验证

MAVLink包含多达255种数据包类型，有心跳包、姿态角数据包、GPS信息包、光流数据包等等，用户也可以自己定义数据包的封装和解析方法。来自无人机的一套完整的数据包括一个心跳包和其他需要解析的参数包。地面站检测是否与无人机失联，主要依靠地面站能否以固定频率接收到来自无人机的心跳包。建议心跳包以1 Hz频率发送。在此基础上，地面站方可接收并解析其他数据包。

首先通过 mavlink＿msg＿heartbeat＿pack（）函数生成心跳包，频率为1 Hz。然后通过 mavlink＿msg＿attitude＿pack（）函数生成飞行器姿态数据包，频率为20 Hz。其中包含姿态角信息和对应的角速度信息。

打开QGC建立WiFi连接时，选择TCP协议，端口号为5000，IP地址为192.168.2.3。以上均为 WiFi模块默认设置，用户可自定义协议、端口号和IP地址。图2为QGC获取的横滚角信息和横滚角加速度信息导入到Matlab后绘制的曲线图。其中图2（a）为横滚角曲线，（b）为横滚角加速度曲线，横滚通道控制算法选用PID控制算法。通过观察可以发现，四旋翼无人机在有效数据接收范围内能够保持与地面站的连接，并且以固定频率发送飞行数据到地面站。

图2 横滚角和横滚角加速度曲线图

结 语

本文提出一种基于DSP的MAVLink微型无人机协议移植方法，实现通过WiFi与地面站控制软件QGround-Control进行通信的应用。该方法能够使MAVLink有效兼容DSP，并且通过SPI转WiFi连接QGC地面站控制软件，减小了CPU开销，提高了数据传输速率，减少了数据的丢失情况，并且增强了数据的分析能力。下一步将研究DSP作为底层控制板与在板计算机的连接，由在板计算机实时获取处理环境图像，并通过WiFi传输飞行数据和图像到地面站软件。

[1]王冬来，吕强，刘峰.基于L1自适应方法的四旋翼飞行器纵向控制[J].弹箭与制导学报，2011，31（6）：37-40.

[2]吕强，王冬来，刘峰，等.基于非线性指数趋近体的离散滑膜变结构控制[J].弹箭与制导学报，2012，32（5）：50-54.

[3]宁金星，卢京潮，闫建国.基于VC＋＋ 的无人机飞控地面站软件的开发[J].计算机测量与控制，2009，17（3）：596-598.

[4]詹镭，贺人庆，谢阳，等.基于微型四旋翼无人机的智能导航系统[J].电子测量与计数，2011，34（6）：1-3.