基于ARM11+CPLD的小型無(wú)人機(jī)飛行控制器設(shè)計(jì)

本文設(shè)計(jì)了9路PWM脈寬數(shù)據(jù)采集,該模塊輸入0.4us的基頻,將外部PWM波形信息轉(zhuǎn)換為數(shù)值信息,再由SPI傳輸模塊,傳入MCU。SPI數(shù)據(jù)傳送模塊,實(shí)現(xiàn)上模擬從SPI設(shè)備。該設(shè)計(jì)有效解決端口不足,釋放了MCU資源,也提高系統(tǒng)實(shí)時(shí)性。

3 軟件系統(tǒng)設(shè)計(jì)
在上述飛行控制器硬件平臺(tái)上,移植Linux操作系統(tǒng)(Linux 2.6內(nèi)核),并對(duì)Linux內(nèi)核作裁剪,對(duì)內(nèi)核加上Xenomai補(bǔ)丁[8],Xenomei技術(shù)使得操作系統(tǒng)運(yùn)行在雙內(nèi)核下,多了一個(gè)強(qiáng)實(shí)時(shí)的微內(nèi)核,能滿足飛控的實(shí)時(shí)性要求。在此基礎(chǔ)上進(jìn)行飛行控制器軟件設(shè)計(jì)。飛行控制器軟件主要由硬件接口驅(qū)動(dòng)、導(dǎo)航控制程序、姿態(tài)控制程序組成。
3.1 接口驅(qū)動(dòng)程序設(shè)計(jì)
(1) UART接口驅(qū)動(dòng):GPS與MCU的連接通過(guò)UART串口,所以GPS驅(qū)動(dòng)需要串口驅(qū)動(dòng)的支持。本文的GPS接口驅(qū)動(dòng),沒(méi)有采用linux內(nèi)核里自帶的串口驅(qū)動(dòng)程序,而是通過(guò)內(nèi)存映射以及內(nèi)核函數(shù)直接對(duì)MCU內(nèi)部UART寄存器進(jìn)行讀寫(xiě)操作,從而實(shí)現(xiàn)了高效的GPS數(shù)據(jù)讀取操作。由于對(duì)GPS數(shù)據(jù)信息實(shí)時(shí)性要求不高,驅(qū)動(dòng)采用了查詢模式[9]。接收的數(shù)據(jù)量大小為一組完整GPS NMEA協(xié)議定義的數(shù)據(jù)的大小,本設(shè)計(jì)使用了NMEA協(xié)議中的GGA,RMC語(yǔ)句,最后通過(guò)底層的內(nèi)核數(shù)據(jù)傳遞函數(shù)將GPS數(shù)據(jù)從內(nèi)核層傳輸?shù)綉?yīng)用層。
(2) 姿態(tài)傳感器驅(qū)動(dòng)程序:ADIS16365通過(guò)SPI接口與MCU進(jìn)行數(shù)據(jù)通信,所以在ADIS16365驅(qū)動(dòng)模塊中要實(shí)現(xiàn)SPI的讀寫(xiě)驅(qū)動(dòng)。方法是通過(guò)內(nèi)核函數(shù)對(duì)SPI寄存器讀寫(xiě)賦值操作,從而實(shí)現(xiàn)數(shù)據(jù)的收發(fā)。除了SPI數(shù)據(jù)收發(fā)驅(qū)動(dòng)外還得構(gòu)造出ADIS16365規(guī)定的數(shù)據(jù)通信時(shí)序,只有正常的時(shí)序才能實(shí)現(xiàn)數(shù)據(jù)的正確利用。ADIS16365一次完整的數(shù)據(jù)讀取流程需要32個(gè)CLK脈沖,1秒內(nèi)平均采集數(shù)據(jù)408次,滿足精度要求。
(3) CPLD驅(qū)動(dòng)設(shè)計(jì):本文飛控MCU與CPLD的通信采用的是SPI通信方式,驅(qū)動(dòng)程序的實(shí)現(xiàn)也是通過(guò)內(nèi)核函數(shù)對(duì)SPI寄存器讀寫(xiě)賦值操作,從而實(shí)現(xiàn)數(shù)據(jù)的讀寫(xiě)。CPLD為自定義的硬件模塊,其通信的數(shù)據(jù)格式也是自己定義的,CPLD內(nèi)部通訊數(shù)據(jù)位為32位,定義前二位為路數(shù)控制地址信號(hào)。
3.3 導(dǎo)航控制程序設(shè)計(jì)
導(dǎo)航控制是指控制無(wú)人機(jī)按照存儲(chǔ)在飛行控制器中的航點(diǎn)數(shù)據(jù)自主飛行,航點(diǎn)依次兩兩連接成直線段即構(gòu)成航線。每個(gè)航點(diǎn)包括航點(diǎn)序號(hào)、經(jīng)度、緯度、高度值信息。而航點(diǎn)、航線的輸入是通過(guò)地面控制站軟件中繪制并通過(guò)數(shù)傳電臺(tái)上傳到飛控中,如圖5所示,在為本文飛控配套開(kāi)發(fā)的地面控制軟件中繪制航線。

導(dǎo)航控制中的高度控制主要通過(guò)計(jì)算無(wú)人機(jī)的高度誤差,進(jìn)而計(jì)算輸出給升降舵機(jī)的PWM值來(lái)控制高度。高度可由MS5540氣壓傳感器測(cè)得的高度與GPS模塊的高度數(shù)據(jù)融合計(jì)算后獲得。
導(dǎo)航控制中的航向控制流程如圖6所示。該流程先讀入FLASH中的航點(diǎn)數(shù)據(jù),再由GPS獲取當(dāng)前位置信息,進(jìn)而計(jì)算航向角、偏航距、偏航角。航向角的計(jì)算是在GPS讀出的航向字段基礎(chǔ)上,加上前后兩時(shí)刻的位置差進(jìn)行修正獲得。

偏航距和偏航角的計(jì)算如圖7所示。

設(shè)A點(diǎn)為無(wú)人機(jī)當(dāng)前位置,B為第n個(gè)航點(diǎn),C為第n+1個(gè)航點(diǎn)。圖7中,以B為坐標(biāo)原點(diǎn),X軸表示經(jīng)度,Y軸表示緯度構(gòu)建直角坐標(biāo)系。在地球表面,任意兩點(diǎn)

離即線段AB的值,無(wú)人機(jī)與終點(diǎn)距離即線段AC的值,航線起點(diǎn)與終點(diǎn)距離即線段BC的值,再根據(jù)點(diǎn)到直線距離公式,可求得線段AD即偏航距,根據(jù)三角形余弦定理可求出∠ABD, ∠ABD即為偏航角。
無(wú)人機(jī)飛行過(guò)程中,判斷是否過(guò)航點(diǎn)是讀取下一個(gè)航點(diǎn)的必要條件。當(dāng)無(wú)人機(jī)飛過(guò)航線的垂直線EF既為過(guò)航點(diǎn)。判斷方法是,過(guò)C點(diǎn)繪出Y軸平行的線段CG,求線段BC與CG的夾角減線段AC與CG夾角的差值,計(jì)算該差值的絕對(duì)值,若|a1-a2|>90則判定為過(guò)點(diǎn)。
3.4 姿態(tài)控制程序設(shè)計(jì)
姿態(tài)控制主要由俯仰姿態(tài)、滾轉(zhuǎn)姿態(tài)控制構(gòu)成,以俯仰姿態(tài)控制為例,其控制結(jié)構(gòu)如圖8所示:

本文設(shè)計(jì)了PID控制器實(shí)現(xiàn)姿態(tài)控制,而俯仰角由姿態(tài)傳感器ADIS16365測(cè)量并經(jīng)姿態(tài)計(jì)算而得。ADIS16365測(cè)量的是機(jī)體坐標(biāo)系下的數(shù)值,而要表達(dá)機(jī)體相對(duì)于參考坐標(biāo)系的姿態(tài),需進(jìn)行姿態(tài)計(jì)算。姿態(tài)計(jì)算以角速度測(cè)量值和運(yùn)載體的初始條件為輸入,姿態(tài)計(jì)算后,輸出飛機(jī)的姿態(tài)信息和比力坐標(biāo)變換所需的姿態(tài)矩陣。在姿態(tài)計(jì)算過(guò)程中,因四元數(shù)法在實(shí)際求解中需更新的參數(shù)少、精度較高的特點(diǎn),本文采用四元數(shù)法求解。