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.