季宏祥 耿相銘

?

基于S698和MPU9150的無(wú)人機(jī)姿態(tài)識(shí)別

季宏祥 耿相銘

上海交通大學(xué)電子信息與電氣工程學(xué)院，上海200240

S698是一款由珠海歐比特控制工程股份有限公司研制出的32位嵌入式處理器。采用S698的主控芯片平臺(tái)，調(diào)用Motion- Fusion，其輸出的四元數(shù)轉(zhuǎn)換后的姿態(tài)角與無(wú)人機(jī)的姿態(tài)相符。介紹如何結(jié)合S698平臺(tái)重構(gòu)關(guān)鍵函數(shù)及底層驅(qū)動(dòng)來(lái)實(shí)現(xiàn)調(diào)用MotionFusion來(lái)識(shí)別無(wú)人機(jī)的姿態(tài)。

S698；MPU9150；姿態(tài)；MotionFusion；四元素

引言

珠海歐比特工程研發(fā)的S698處理器，是國(guó)內(nèi)首款基于SPARK V8架構(gòu)的32為嵌入式處理器。由于國(guó)防、航空等領(lǐng)域的特殊性，國(guó)產(chǎn)的S698已被廣泛的應(yīng)用于這些領(lǐng)域[1]。

MPU9150的MotionFusion專利能夠幫助客戶減少在運(yùn)動(dòng)數(shù)據(jù)融合方面的工作，其目前主要支持TI-MSP430和ARM平臺(tái)，對(duì)于其他平臺(tái)，使用者需要結(jié)合Invensense官網(wǎng)提供的Motion Driver 5.1.2版本，重構(gòu)部分函數(shù)進(jìn)而實(shí)現(xiàn)運(yùn)動(dòng)數(shù)據(jù)的融合及四元數(shù)姿態(tài)輸出。Motion Driver 5.1.2的限制在于它不能融合磁力計(jì)的三軸數(shù)據(jù)，但由于Motion Driver平臺(tái)本身要求，若需要融合磁力計(jì)的數(shù)據(jù)，必須使用TI-MSP430或者ARM平臺(tái)，本文中所討論的是基于Motion Driver 5.1.2版本，而融合的數(shù)據(jù)也是六軸數(shù)據(jù)[2]。

1 Invensense的Motion Driver概述

Motion Driver是Invensense公司提供的傳感器驅(qū)動(dòng)層，以便用戶能夠配置并啟動(dòng)Invensense公司運(yùn)動(dòng)監(jiān)測(cè)芯片的數(shù)字融合處理功能。Motion Driver的源代碼是以C語(yǔ)言的方式編寫(xiě)的，方便進(jìn)行移植。其融合的運(yùn)動(dòng)數(shù)據(jù)是可以根據(jù)用戶的需求進(jìn)行配置的，關(guān)于Motion Driver中各個(gè)函數(shù)具體的接口定義及含義參見(jiàn)Embeded Motion Driver v5.1.1 APIs Specification。Motion Driver的移植需要根據(jù)Invensense提供的Motion Driver 5.1.1 Tutorial進(jìn)行。本指導(dǎo)書(shū)中關(guān)于具體的哪些函數(shù)需要重構(gòu)，并不明確，需要使用者去閱讀Motion Driver的源代碼，并且需要結(jié)合所需要移植的控制平臺(tái)對(duì)接口及部分底層驅(qū)動(dòng)函數(shù)根據(jù)需要進(jìn)行重構(gòu)[3]。

2 S698中關(guān)鍵函數(shù)及驅(qū)動(dòng)的重構(gòu)

2.1 I2C總線驅(qū)動(dòng)及接口函數(shù)重構(gòu)

S698內(nèi)部沒(méi)有I2C總線接口，而使用S698的多功能口模擬I2C總線的關(guān)鍵在于模擬時(shí)鐘SCL和根據(jù)SCL時(shí)鐘。模擬SCL時(shí)鐘有兩種方式，一種是使用空指令，等待延時(shí)。另外一種非阻塞式的實(shí)現(xiàn)方式是通過(guò)定時(shí)器來(lái)實(shí)現(xiàn)。結(jié)合Motion Driver中的源代碼，所有的涉及I2C的驅(qū)動(dòng)調(diào)用均是阻塞式的方式，所以在S698平臺(tái)上采用阻塞是方式的I2C。下面以I2C總線寫(xiě)操作為例說(shuō)明實(shí)現(xiàn)過(guò)程[4]。

圖1 I2C總線寫(xiě)操作為例說(shuō)明實(shí)現(xiàn)過(guò)程

3.2 中斷函數(shù)重構(gòu)

MPU9150的中斷輸出功能在Motion Driver中被多次調(diào)用，其可被用于提醒主芯片其所需的數(shù)據(jù)或者運(yùn)動(dòng)數(shù)據(jù)的融合已經(jīng)完成等。以上的功能的實(shí)現(xiàn)需要結(jié)合硬件平臺(tái)上MPU9150中斷引腳與S698的連接，以及軟件對(duì)MPU9150的配置。由于在Motion Driver中具有專門(mén)的中斷注冊(cè)函數(shù)接口，用于關(guān)聯(lián)具體的中斷源于中斷處理函數(shù)，在移植的過(guò)程中，需要根據(jù)S698平臺(tái)重構(gòu)此中斷注冊(cè)函數(shù)。主要的中斷相關(guān)函數(shù)重構(gòu)如下。

2.3 時(shí)鐘函數(shù)重構(gòu)

根據(jù)Motion Driver的要求，對(duì)于部分時(shí)鐘函數(shù)也需要重構(gòu)。采用S698通用定時(shí)器1來(lái)執(zhí)行延時(shí)[5]。

圖3 采用S698通用定時(shí)器1來(lái)執(zhí)行延時(shí)

4 S698平臺(tái)上配置并調(diào)用Motion Driver

配置和調(diào)用的主要步驟包括以下幾步。（1）S698及MPU9150的初始化及自測(cè)。（2）配置加速度計(jì)及陀螺儀的量程。（3）加載數(shù)字運(yùn)動(dòng)處理程序（DMP）的鏡像文件到MPU9150中。（4）將陀螺儀和加速度計(jì)的初始方向矩陣輸入到DMP中。（5）配置具體需要DMP反饋的數(shù)據(jù)。（6）啟動(dòng)DMP。（7）配置DMP的數(shù)據(jù)輸出的速率。

測(cè)試使用的是S698自帶的開(kāi)發(fā)平臺(tái)Orion 4.0，使用的硬件測(cè)試平臺(tái)為基于S698的旋翼無(wú)人機(jī)的飛行控制板。在測(cè)試過(guò)程中，根據(jù)以上七步，成功調(diào)用MPU9150內(nèi)部的數(shù)字運(yùn)算處理模塊，輸出六軸融合數(shù)據(jù)及傳感器原始數(shù)據(jù)，同時(shí)將四元數(shù)轉(zhuǎn)換成歐拉角，結(jié)果顯示歐拉角的輸出與無(wú)人機(jī)機(jī)體的姿態(tài)相符。

5 總結(jié)

本文給出了基于S698平臺(tái)如何調(diào)用MPU9150內(nèi)部的運(yùn)動(dòng)處理單元，并詳述了其關(guān)鍵函數(shù)重構(gòu)的方法。通過(guò)將MPU9150其輸出的四元數(shù)轉(zhuǎn)換后的姿態(tài)角與無(wú)人機(jī)的姿態(tài)相比較驗(yàn)證了該移植的有效性。需要指出的是，由于Invensense公司自身運(yùn)動(dòng)庫(kù)的限制，若需要在S698平臺(tái)上實(shí)現(xiàn)九軸數(shù)據(jù)的融合，則需要完全拋開(kāi)Motion Driver，而這就不可以避免的涉及到各種融合算法。關(guān)于此項(xiàng)工作的研究，有待后續(xù)開(kāi)展。

[1]冀亮，錢正洪，白茹.基于四元數(shù)的四軸無(wú)人機(jī)姿態(tài)的估計(jì)和控制[J].現(xiàn)代電子技術(shù)，2015（11）：112-116.

[2]任志明，李永樹(shù)，何敬，等.基于姿態(tài)數(shù)據(jù)的無(wú)人機(jī)影像自動(dòng)刺點(diǎn)功能研究[J].測(cè)繪通報(bào)，2011（4）：44-46.

[3]宋宇，翁新武，郭昕剛.基于四元數(shù)EKF算法的小型無(wú)人機(jī)姿態(tài)估計(jì)[J].吉林大學(xué)學(xué)報(bào)：理學(xué)版，2015（3）：511-518.

[4]何永勃，賈輝，姜坤，等.基于Android終端陀螺儀傳感器的無(wú)人機(jī)飛行姿態(tài)控制[J].傳感技術(shù)學(xué)報(bào)，2015（4）：474-478.

[5]趙亞鵬，曹彬才，吳帥.基于飛行姿態(tài)校正的無(wú)人機(jī)影像快速拼接[J].測(cè)繪科學(xué)與工程，2015（1）：47-54.

V249.12

A

1009-6434（2016）02-0018-02