DSP的MAVLink微型無人機(jī)通信協(xié)議移植與應(yīng)用

呂強(qiáng)，倪佩佩，王國勝，劉峰

（裝甲兵工程學(xué)院 控制工程系，北京100072）

引 言

無人機(jī)（Unmanned Air Vehicle，UAV）是一種搭載飛行控制器、空氣動(dòng)力承載、無人駕駛并且能夠重復(fù)使用、搭載多種設(shè)備、執(zhí)行多種任務(wù)的飛行器[1-2]。無人機(jī)一般包括固定翼飛行器和旋翼式飛行器。其中微小型旋翼機(jī)因其具有便攜、機(jī)動(dòng)性好、垂直起降、空中懸停的特點(diǎn)而廣泛應(yīng)用于災(zāi)害救援、交通巡查、航空攝影、軍事偵察等。無人機(jī)的地面控制系統(tǒng)可以完成對無人機(jī)的飛行操控、航路規(guī)劃、飛行狀態(tài)獲取及分析、前方圖像獲取等[3-4]；而一個(gè)完善的無人機(jī)通信協(xié)議則是連接無人機(jī)與地面站的重要橋梁。MAVLink是一個(gè)開源的基于C/C＋＋或者Python語言的能夠?qū)ξ⑿惋w行器（MAV）進(jìn)行數(shù)據(jù)封裝和解析的庫。國外許多開源的地面站控制軟件包括QGround-Control（QGC）、HK Ground Control Station、APM Planner、Copter GCS等，都是基于MAVLink開發(fā)并集成一套完整的可視化界面，包括地圖、儀表、實(shí)時(shí)參數(shù)曲線繪制和視頻顯示界面等。因此，基于MAVLink微型無人機(jī)通信協(xié)議傳輸無人機(jī)參數(shù)，可以幫助開發(fā)者節(jié)省單獨(dú)開發(fā)地面站軟件的時(shí)間。

由于TMS320F28335（簡稱F28335）的32位浮點(diǎn)運(yùn)算單元能夠大大提高控制算法的處理速度，故采用該DSP控制器作為飛行控制器，并提出一種基于該控制器的MAVLink微型無人機(jī)通信協(xié)議的移植與應(yīng)用方法。

1 MAVLink介紹

表1是MAVLink協(xié)議的數(shù)據(jù)結(jié)構(gòu)。雖然MAVLink只使用1字節(jié)FE作為數(shù)據(jù)包第0位的固定起始標(biāo)志，但是第1位的有效載荷長度和第（n＋7）～（n＋8）位的兩位校驗(yàn)和，能夠區(qū)別當(dāng)前接收到的FE是隨機(jī)噪聲還是數(shù)據(jù)包起始標(biāo)志。因此MAVLink不僅無需用多個(gè)字節(jié)作為起始標(biāo)志，還能保證數(shù)據(jù)長度簡短而又安全。

表1 MAVLink協(xié)議的數(shù)據(jù)結(jié)構(gòu)

2 MAVLink移植方法

F28335的外設(shè)寄存器為16位，而一般的微控制器都是8位。MAVLink默認(rèn)支持8位外設(shè)寄存器的微控制器，因此DSP并不兼容MAVLink，無法生成正確的數(shù)據(jù)。

F28335基于標(biāo)準(zhǔn)C語言使用CCS作為開發(fā)環(huán)境，具體的移植方法如下：

①在主程序中定義typedef unsigned char uint8＿t，否則程序無法識別uint8＿t。F28335的庫文件里面未定義uint8＿t數(shù)據(jù)類型，而unsigned char數(shù)據(jù)類型為16位，即使參數(shù)定義為8位，也是默認(rèn)以16位進(jìn)行存儲，不影響編程。

②在主程序中定義大端模式（＃define NATIVE＿BIG＿ENDIAN），并修改 MAVLink庫protocol.h頭文件中的byte＿swap函數(shù)。如修改byte＿swap＿4（）：

MAVLink默認(rèn)選擇小端模式，即數(shù)據(jù)的低位字節(jié)保存在內(nèi)存的高地址中，而數(shù)據(jù)的高位字節(jié)保存在內(nèi)存的低地址中。同時(shí)也有大端模式的函數(shù)代碼以滿足用戶的特殊要求。雖然F28335也是小端模式，但其16位的寄存器會使原本一個(gè)寄存器存儲1個(gè)字節(jié)的情況變成2個(gè)字節(jié)填滿一個(gè)寄存器，造成移位和串位的現(xiàn)象。因此可以選擇設(shè)置一個(gè)自定義的偽大端模式，即在大端模式代碼中對數(shù)據(jù)進(jìn)行移位和交換操作，實(shí)現(xiàn)我們希望的自定義小端模式，其他函數(shù)修改方法類似。

③修改發(fā)送序列的計(jì)數(shù)范圍。16位的寄存器導(dǎo)致生成序列current＿tx＿seq的計(jì)數(shù)范圍為0～65 535，而我們只需要0～255，因此對生成序列的高8位清零即可，即current＿tx＿seq＆0x00ff。

④修改校驗(yàn)和代碼。對MAVLink庫checksum.h頭文件中crc＿accumulate（）函數(shù)的參數(shù)temp也進(jìn)行高8位清零的操作。MAVLink是按照CRC-16-CCITT標(biāo)準(zhǔn)計(jì)算校驗(yàn)和的。其主要思想是對數(shù)據(jù)包的第1位至第n＋6位進(jìn)行一系列按位異或、移位和循環(huán)累加等操作，生成16位校驗(yàn)和。

⑤修改時(shí)間簽。部分類型的數(shù)據(jù)包內(nèi)的Data包含時(shí)間簽，以便實(shí)時(shí)繪制參數(shù)變化曲線；而CCS基本庫內(nèi)沒有生成時(shí)間簽的函數(shù)，故用戶需自定義。可在定時(shí)器0的中斷函數(shù)中定義一個(gè)時(shí)間累加器。注意區(qū)分不同數(shù)據(jù)包的時(shí)間簽單位是毫秒還是微秒。

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

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

3 四旋翼無人機(jī)與地面站的通信

引言中提到的開源的QGC是基于MAVLink協(xié)議編寫的一款用戶界面友好的地面站控制軟件。其連接方式可以選擇串口通信或者基于TCP/UDP協(xié)議的無線網(wǎng)絡(luò)。使用XBee模塊通過串行通信接口（SCI）連接發(fā)送的數(shù)據(jù)波特率最高只有115 200 bps，尤其在高頻率發(fā)送大量數(shù)據(jù)時(shí)會發(fā)生嚴(yán)重的數(shù)據(jù)丟失現(xiàn)象；而串行外設(shè)接口（SPI）是一個(gè)高速同步的串行輸入輸出接口，并且F28335支持一個(gè)16級深度的發(fā)送接收FIFO，可減少CPU開銷，故采用新力維WM001S V3.0高速SPI轉(zhuǎn)WiFi模塊進(jìn)行四旋翼無人機(jī)與地面站實(shí)時(shí)通信。

3.1 無線網(wǎng)絡(luò)的建立

配置SPI時(shí)有以下三點(diǎn)注意：

①SPI時(shí)鐘方案選擇有相位延時(shí)的下降沿，與WiFi模塊一致，否則WiFi模塊將停止工作。

②WiFi模塊最高支持50 MHz的時(shí)鐘頻率，且SPI波特率設(shè)置不能過大，否則會造成WiFi模塊無法及時(shí)響應(yīng)和數(shù)據(jù)丟失。

③發(fā)送數(shù)據(jù)以左對齊的方式存儲在串行輸出緩沖寄存器（SPITXBUF）中，接收數(shù)據(jù)以右對齊的方式存儲在串行輸入緩沖寄存器（SPIRXBUF）中。單次發(fā)送或接收小于16位數(shù)據(jù)時(shí)，尤其要注意這一點(diǎn)。本文選擇單次發(fā)送8位數(shù)據(jù)。

圖1是WM001S V3.0WiFi模塊發(fā)送數(shù)據(jù)時(shí)的流程。向WiFi模塊發(fā)送RBRS命令（0x40），準(zhǔn)備讀取WiFi模塊當(dāng)前可寫數(shù)據(jù)大小（VC）和可讀數(shù)據(jù)大?。≧C）；發(fā)送4次RSR命令（0x00），讀取4字節(jié)的VC和RC。如果VC≥lenth，lenth為待發(fā)送數(shù)據(jù)包長度，則先發(fā)送寫命令（0x D0），然后開始發(fā)送MAVLink數(shù)據(jù)包。

圖1 WiFi模塊發(fā)送數(shù)據(jù)的流程圖

F28335 SPIA主機(jī)向WiFi從機(jī)寫數(shù)據(jù)的代碼分三部分。

①SPIA初始化設(shè)置：

②自定義SPIA發(fā)送單個(gè)字節(jié)函數(shù)：

③定時(shí)器0中斷中發(fā)送數(shù)據(jù)代碼：

3.2 實(shí)驗(yàn)驗(yàn)證

MAVLink包含多達(dá)255種數(shù)據(jù)包類型，有心跳包、姿態(tài)角數(shù)據(jù)包、GPS信息包、光流數(shù)據(jù)包等等，用戶也可以自己定義數(shù)據(jù)包的封裝和解析方法。來自無人機(jī)的一套完整的數(shù)據(jù)包括一個(gè)心跳包和其他需要解析的參數(shù)包。地面站檢測是否與無人機(jī)失聯(lián)，主要依靠地面站能否以固定頻率接收到來自無人機(jī)的心跳包。建議心跳包以1 Hz頻率發(fā)送。在此基礎(chǔ)上，地面站方可接收并解析其他數(shù)據(jù)包。

首先通過 mavlink＿msg＿h(yuǎn)eartbeat＿pack（）函數(shù)生成心跳包，頻率為1 Hz。然后通過 mavlink＿msg＿attitude＿pack（）函數(shù)生成飛行器姿態(tài)數(shù)據(jù)包，頻率為20 Hz。其中包含姿態(tài)角信息和對應(yīng)的角速度信息。

打開QGC建立WiFi連接時(shí)，選擇TCP協(xié)議，端口號為5000，IP地址為192.168.2.3。以上均為 WiFi模塊默認(rèn)設(shè)置，用戶可自定義協(xié)議、端口號和IP地址。圖2為QGC獲取的橫滾角信息和橫滾角加速度信息導(dǎo)入到Matlab后繪制的曲線圖。其中圖2（a）為橫滾角曲線，（b）為橫滾角加速度曲線，橫滾通道控制算法選用PID控制算法。通過觀察可以發(fā)現(xiàn)，四旋翼無人機(jī)在有效數(shù)據(jù)接收范圍內(nèi)能夠保持與地面站的連接，并且以固定頻率發(fā)送飛行數(shù)據(jù)到地面站。

圖2 橫滾角和橫滾角加速度曲線圖

結(jié) 語

本文提出一種基于DSP的MAVLink微型無人機(jī)協(xié)議移植方法，實(shí)現(xiàn)通過WiFi與地面站控制軟件QGround-Control進(jìn)行通信的應(yīng)用。該方法能夠使MAVLink有效兼容DSP，并且通過SPI轉(zhuǎn)WiFi連接QGC地面站控制軟件，減小了CPU開銷，提高了數(shù)據(jù)傳輸速率，減少了數(shù)據(jù)的丟失情況，并且增強(qiáng)了數(shù)據(jù)的分析能力。下一步將研究DSP作為底層控制板與在板計(jì)算機(jī)的連接，由在板計(jì)算機(jī)實(shí)時(shí)獲取處理環(huán)境圖像，并通過WiFi傳輸飛行數(shù)據(jù)和圖像到地面站軟件。

[1]王冬來，呂強(qiáng)，劉峰.基于L1自適應(yīng)方法的四旋翼飛行器縱向控制[J].彈箭與制導(dǎo)學(xué)報(bào)，2011，31（6）：37-40.

[2]呂強(qiáng)，王冬來，劉峰，等.基于非線性指數(shù)趨近體的離散滑膜變結(jié)構(gòu)控制[J].彈箭與制導(dǎo)學(xué)報(bào)，2012，32（5）：50-54.

[3]寧金星，盧京潮，閆建國.基于VC＋＋ 的無人機(jī)飛控地面站軟件的開發(fā)[J].計(jì)算機(jī)測量與控制，2009，17（3）：596-598.

[4]詹鐳，賀人慶，謝陽，等.基于微型四旋翼無人機(jī)的智能導(dǎo)航系統(tǒng)[J].電子測量與計(jì)數(shù)，2011，34（6）：1-3.