基于stm32的多旋翼無(wú)人機(jī)（Multi-rotor UAV based on stm32)（上）

一、多旋翼無(wú)人機(jī)飛行原理

四軸飛行器基本原理是通過(guò)飛控控制四個(gè)電機(jī)旋轉(zhuǎn)帶動(dòng)漿葉產(chǎn)生升力，分別控制每一個(gè)電機(jī)和漿葉產(chǎn)生不同的升力從而控制飛行器的姿態(tài)和位置。四軸在空中可以實(shí)現(xiàn)八種運(yùn)動(dòng)，分別為垂直上升、垂直下降、向前運(yùn)動(dòng)、向后運(yùn)動(dòng)、向左運(yùn)動(dòng)、向后運(yùn)動(dòng)、順時(shí)針改變航向、逆時(shí)針改變航向。

1.1 多旋翼無(wú)人機(jī)的組成

四軸飛行器主要是由電機(jī)、電調(diào)、電池、漿葉、機(jī)架、遙控器、飛控組成。下面以我們四軸及市場(chǎng)上常見(jiàn)的 DIY 大四軸來(lái)介紹這些部件。

1.1.1 電機(jī)

電機(jī)根據(jù)目前市場(chǎng)的供給以及本項(xiàng)目的需求，采用的是空心杯無(wú)刷電機(jī)8520，如下圖：

其轉(zhuǎn)速可達(dá)到12000~15000轉(zhuǎn)/min，并且其價(jià)格較低，性能較穩(wěn)定，更適合本項(xiàng)目的使用。

1.1.2 電調(diào)

電調(diào)即為電子調(diào)速器，控制電機(jī)轉(zhuǎn)動(dòng)、停止及轉(zhuǎn)速。有刷電機(jī)電調(diào)通常只需要一個(gè) MOS 管，飛控輸出 PWM 即可控制電機(jī)，電調(diào)所采用的MOS管如下圖所示：無(wú)刷電機(jī)電調(diào)模塊內(nèi)部通常由一個(gè) MCU 和三相橋電路組成，MCU 通過(guò)控制三相橋來(lái)實(shí)現(xiàn)無(wú)刷電機(jī)換相。同樣，無(wú)刷電機(jī)電調(diào)模塊也只需飛控輸出 PWM 即可控制電機(jī)

1.1.3 航模電池

航模所用電池為3.7v鋰電池（可充電），由于受到多旋翼無(wú)人機(jī)自身動(dòng)力的問(wèn)題，故采用800mh鋰電池來(lái)供電，遙控器也采用800mh供電，飛行時(shí)間大概20min左右。電池如下圖：

電池插座采用空對(duì)空接頭。

電池容量 mAh：鋰電池的容量，如 2000mAh 的電池，以 2000mA 放電，可持續(xù)放電 1 小時(shí)；以 1000mA 放電，可持續(xù)放電 2 小時(shí)。
電池節(jié)數(shù)：電池 2S、3S、4S 代表鋰電池節(jié)數(shù)。鋰電池 1 節(jié)的標(biāo)準(zhǔn)電壓為 3.7V，3S 代表有 3 節(jié) 3.7V 的電池在里面，電壓為 11.1V。

1.1.4 正反漿

漿液采用的是75mm直徑的正反漿，如下圖所示：

漿葉旋轉(zhuǎn)時(shí)會(huì)產(chǎn)生自旋力導(dǎo)致四軸自旋，為了抵消自旋力相隔電機(jī)的漿葉旋轉(zhuǎn)方向要不一樣，但是漿的風(fēng)都是要往下吹，這就出現(xiàn)了正反漿的說(shuō)法。通常順時(shí)針轉(zhuǎn)的叫正漿，逆時(shí)針轉(zhuǎn)的是反漿。

1.2 垂直上升及下降

當(dāng)四軸飛行在空中自穩(wěn)后，M1、M2、M3、M4 四個(gè)電機(jī)同時(shí)轉(zhuǎn)速增大或同時(shí)轉(zhuǎn)速減小，即可發(fā)生垂直上升運(yùn)動(dòng)或垂直下降運(yùn)動(dòng)，如下圖所示：

1.3 向前飛行及向后飛行

當(dāng)四軸飛行在空中自穩(wěn)后，M2、M3 轉(zhuǎn)速增大 M1、M4 轉(zhuǎn)速不變或減小即可實(shí)現(xiàn)向前運(yùn)動(dòng)。相反，M2、M3 轉(zhuǎn)速減小或不變 M1、M4 轉(zhuǎn)速增加，即可實(shí)現(xiàn)向后運(yùn)動(dòng)，如下圖所示：

向左和向右飛行同理可以實(shí)現(xiàn)，這里就不做介紹。

1.4 順時(shí)針改變航向和逆時(shí)針改變航向

當(dāng)四軸飛行在空中自穩(wěn)后，M1、M3 轉(zhuǎn)速增大 M2、M4 轉(zhuǎn)速不變或減小即可實(shí)現(xiàn)順時(shí)針改變航向。M1、M3 轉(zhuǎn)速減小或不變 M2、M4 轉(zhuǎn)速增加即可實(shí)現(xiàn)逆時(shí)針改變航向。如下圖所示：

二、多旋翼無(wú)人機(jī)飛控電路設(shè)計(jì)

2.1 系統(tǒng)框架設(shè)計(jì)

根據(jù)Minfly的系統(tǒng)框架圖，來(lái)進(jìn)行設(shè)計(jì)，主控芯片采用STM32103C8T6，三軸陀螺儀采用MPU6050芯片，收發(fā)無(wú)線(xiàn)模塊采用NRFL2401模塊（億百特）等。

2.2 主控MCU模塊

主控采用的是STM32103C8T6芯片，電路圖如下：

主控 MCU 為四軸飛行器的大腦，對(duì)飛行器穩(wěn)定飛行起著至關(guān)重要的作用。它同時(shí)承擔(dān)著多種責(zé)任，包括：傳感器數(shù)據(jù)讀取、數(shù)據(jù)融合、PID 控制、電機(jī)控制、無(wú)線(xiàn)通信和 USB通信等。
主控 MCU 連接了一個(gè) USB 接口，此接口可以用作與上位機(jī)通信，也可以用作固件升級(jí)，是一個(gè)非常方便適用的接口。

2.3 三軸加速陀螺儀模塊

三軸加速陀螺儀模塊采用的是MPU6050芯片作為主控，MPU6050 IMU 在單芯片上集成了一個(gè) 3 軸加速度計(jì)和一個(gè) 3 軸陀螺儀。以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器 DMP（Digital Motion Processor）。它也被稱(chēng)為六軸運(yùn)動(dòng)跟蹤設(shè)備或 6 DoF 設(shè)備，因?yàn)樗?6 個(gè)輸出，即 3 個(gè)加速度計(jì)輸出和 3 個(gè)陀螺儀輸出。以當(dāng)前地面為水平面檢測(cè)x,y,z三軸方向上的加速度，并轉(zhuǎn)換為電信號(hào)來(lái)進(jìn)行輸出。同時(shí)進(jìn)行三軸陀螺儀傳感器進(jìn)行使用，得到三軸方向上的傾角，即姿態(tài)角。如下圖所示：

MPU6050主控芯片的控制原理如圖：

設(shè)計(jì)電路圖如下：

2.4 電機(jī)驅(qū)動(dòng)模塊

本項(xiàng)目采用微型高速 8520空心杯電機(jī)，電機(jī)轉(zhuǎn)速高達(dá) 15000r/min，能夠?yàn)轱w行器提供充沛的動(dòng)力。電機(jī)采用 NMOS 管 SI2302，3V 門(mén)級(jí)驅(qū)動(dòng)電壓下，導(dǎo)通電阻只有幾十毫歐，驅(qū)動(dòng)電流高達(dá) 3A，輕松驅(qū)動(dòng) 8520空心杯電機(jī)，從而帶動(dòng)飛行器飛行。電路設(shè)計(jì)如下圖：

2.5 無(wú)線(xiàn)通信模塊

無(wú)線(xiàn)通信模塊所采用的是NRF24L01模塊來(lái)進(jìn)行通信,NRF24L01是一款新型單片射頻收發(fā)器件，工作與2.4GHz~2.5GHz ISM頻段。內(nèi)置頻率合成器、功率放大器、晶體振蕩器、調(diào)制器等功能模塊，并融合了增強(qiáng)型ShockBurst技術(shù)，其中輸出功率和通信頻道可通過(guò)程序進(jìn)行配置。
nRF24L01有工作模式有四種：

• 收發(fā)模式
• 配置模式
• 空閑模式
• 關(guān)機(jī)模式
  如圖所示（E01-ML01S）：

芯片方案：nRF24L01P
工作頻率：2.4~2.525GHz
發(fā)射功率：0dBm
通信距離：0.1km
接口類(lèi)型：SPI
產(chǎn)品重量：0.5±0.1g

產(chǎn)品簡(jiǎn)介：采用挪威NorDic公司原裝進(jìn)口nRF24L01P射頻芯片，收發(fā) 一體；全進(jìn)口工業(yè)級(jí)元器件，郵票孔貼片型，內(nèi)置PCB板載天線(xiàn)；體積小易嵌入，性能優(yōu)異，數(shù)據(jù)傳輸快，適合近距離傳輸。
要實(shí)現(xiàn)通信還需要E01-ML01D模塊來(lái)實(shí)現(xiàn)交互通信,模塊的圖片如下：

芯片方案：nRF24L01P
工作頻率：2.4~2.525GHz
發(fā)射功率：0dBm
通信距離：0.1km
通信接口：SPI
產(chǎn)品重量：0.9±0.1g

產(chǎn)品簡(jiǎn)介：挪威進(jìn)口nRF24L01+芯片，日本進(jìn)口阻容感，美國(guó)進(jìn)口晶振，無(wú)鉛焊接工藝； 目前已經(jīng)大量用于電力線(xiàn)，品質(zhì)相當(dāng)可靠，使用壽命長(zhǎng)。

無(wú)線(xiàn)通信模塊電路圖如下：（分別為接受端和發(fā)送端）

2.6 升壓、穩(wěn)壓電路模塊

考慮本項(xiàng)目所需8520電機(jī)供電為3.7v，而本項(xiàng)目還需要5v供電，電池的供電為3.7v,因此需要設(shè)計(jì)升壓電路來(lái)進(jìn)行從3.7v到5v的升壓，根據(jù)模擬電子技術(shù)以及電路原理自主設(shè)計(jì)了3.7v----5v的升壓電路。主控為DC–DC電源芯片，采用肖特基二極管來(lái)進(jìn)行外部穩(wěn)壓，具體電路如下圖所示：（篇幅影響不進(jìn)行詳細(xì)的講解，感興趣的粉們可以私信我討論相關(guān)知識(shí)）

最終得到5v的升壓電源，方便項(xiàng)目其他模塊的供電。
相反可以設(shè)計(jì)出穩(wěn)壓電路從5v—3.3v的降壓電路，具體原理不做說(shuō)明，具體見(jiàn)下圖所示：

2.7 LED電路模塊

此部分為L(zhǎng)ED指示燈電路設(shè)計(jì)，主要由3個(gè)LED直插式燈以及兩枚RGB貼片燈組成，具體i單路如下圖所示：

后續(xù)邏輯功能將在程序中進(jìn)行設(shè)計(jì)。

三、多旋翼無(wú)人機(jī)飛控算法設(shè)計(jì)

3.1 飛行姿態(tài)表示法

飛行器姿態(tài)有多種表示方式，常見(jiàn)的是四元數(shù)，歐拉角，矩陣和軸角。他們各自有其自身的優(yōu)點(diǎn)，在不同的領(lǐng)域使用不同的表示方式。在四軸飛行器中使用到了四元數(shù)和歐拉角。

3.1.1 歐拉角

用來(lái)確定定點(diǎn)轉(zhuǎn)動(dòng)剛體位置的 3 個(gè)一組獨(dú)立角參量，由章動(dòng)角 θ、旋進(jìn)角（即進(jìn)動(dòng)角）ψ 和自轉(zhuǎn)角 j 組成，為萊昂哈德·歐拉首先提出而得名。對(duì)于在三維空間里的一個(gè)參考系，任何坐標(biāo)系的取向，都可以用三個(gè)歐拉角來(lái)表現(xiàn)。參考系又稱(chēng)為實(shí)驗(yàn)室參考系，是靜止不動(dòng)的。而坐標(biāo)系則固定于剛體，隨著剛體的旋轉(zhuǎn)而旋轉(zhuǎn)。
如下圖所示：

設(shè)定 xyz-軸為參考系的參考軸。稱(chēng) xy-平面與 XY-平面的相交為交點(diǎn)
線(xiàn)，用英文字母（N）代表。
zxz 順規(guī)的歐拉角可以靜態(tài)地這樣定義：
α 是 x-軸與交點(diǎn)線(xiàn)的夾角，β 是 z-軸與 Z-軸的夾角，γ 是交點(diǎn)線(xiàn)與 X-軸的夾角。

3.1.2 四元數(shù)

四元數(shù)是復(fù)數(shù)的不可交換延伸。如把四元數(shù)的集合考慮成多維實(shí)數(shù)空間的話(huà)，四元數(shù)就代表著一個(gè)四維空間，相對(duì)于復(fù)數(shù)為二維空間。
四元數(shù)的計(jì)算，四元數(shù)可以理解為一個(gè)實(shí)數(shù)和一個(gè)向量的組合，也可以理解為四維的向量。

其中的q為一個(gè)四元數(shù)，其模的長(zhǎng)度為：

對(duì)四元數(shù)進(jìn)行單位化，與線(xiàn)性代數(shù)中的單位話(huà)相似，可得到：

再由創(chuàng)造出來(lái)一個(gè)變量q關(guān)于旋轉(zhuǎn)角得到的一個(gè)變量，即可表示為：

由于“四元數(shù)表示”轉(zhuǎn)“歐拉角表示”。（這個(gè)地方跳過(guò)了復(fù)雜的換算步驟，我也不太理解）

3.1.3 PID控制

當(dāng)今的閉環(huán)自動(dòng)控制技術(shù)都是基于反饋的概念以減少不確定性。反饋理論的要素包括三個(gè)部分：測(cè)量、比較和執(zhí)行。測(cè)量關(guān)鍵的是被控變量的實(shí)際值，與期望值相比較，用這個(gè)偏差來(lái)糾正系統(tǒng)的響應(yīng)，執(zhí)行調(diào)節(jié)控制。在工程實(shí)際中，應(yīng)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡(jiǎn)稱(chēng) PID 控制，又稱(chēng) PID 調(diào)節(jié)。
PID 控制器（比例-積分-微分控制器）是一個(gè)在工業(yè)控制應(yīng)用中常見(jiàn)的反饋回路部件，由比例單元 P、積分單元 I 和微分單元 D 組成。PID 控制的基礎(chǔ)是比例控制；積分控制可消除穩(wěn)態(tài)誤差，但可能增加超調(diào)；微分控制可加快大慣性系統(tǒng)響應(yīng)速度以及減弱超調(diào)趨勢(shì)。如下圖所示：

3.2 飛控軟件框架設(shè)計(jì)

主要程序設(shè)計(jì)框圖參考了Minfly的設(shè)計(jì)框圖：

主要任務(wù)關(guān)系如下：

說(shuō)明：此處不包含APP設(shè)計(jì)。

radiolinkTask：無(wú)線(xiàn)通信任務(wù)。該任務(wù)主要負(fù)責(zé)接收從 NRF51822 發(fā)送（串口方式）過(guò)來(lái)的數(shù)據(jù)，然后對(duì)數(shù)據(jù)進(jìn)行打包和校驗(yàn)，打包成 ATKP 格式并校驗(yàn)無(wú)誤后發(fā)送到atkpRxAnlTask 的接收隊(duì)列里，同時(shí)回傳一幀數(shù)據(jù)給 NRF51822。

usblinkRxTask：USB 通信接收任務(wù)。該任務(wù)主要負(fù)責(zé)接收上位機(jī)發(fā)下來(lái)（USB 虛擬串口方式）的數(shù)據(jù)，然后對(duì)數(shù)據(jù)進(jìn)行打包和校驗(yàn)，打包成 ATKP 格式并校驗(yàn)無(wú)誤后發(fā)送到atkpRxAnlTask 的接收隊(duì)列里。

atkpRxAnlTask：ATKP 數(shù)據(jù)包接收處理任務(wù)。該任務(wù)主要是處理遙控器和上位機(jī)發(fā)下來(lái)的數(shù)據(jù)包，解析到的控制指令則發(fā)送到 stabilizerTask 中去。

stabilizerTask：四軸平衡控制任務(wù)。該任務(wù)運(yùn)行的內(nèi)容比較多，也是比較關(guān)鍵的內(nèi)容。包括傳感器數(shù)據(jù)讀取，數(shù)據(jù)融合，獲取控制數(shù)據(jù)，空翻檢測(cè)，異常檢測(cè)，PID 控制，PWM輸出控制等。

wifilinkTask：手機(jī)控制任務(wù)。該任務(wù)主要是接收 WiFi 攝像頭模塊的串口數(shù)據(jù)，然后按照 WiFi 攝像頭模塊通訊協(xié)議解析成對(duì)應(yīng)的控制指令，并將控制指令發(fā)送到 stabilizerTask。

atkpTxTask：ATKP 數(shù)據(jù)包發(fā)送任務(wù)。該任務(wù)主要是獲取 stabilizerTask 中的傳感器數(shù)據(jù)、姿態(tài)數(shù)據(jù)、電機(jī) PWM 輸出數(shù)據(jù)等數(shù)據(jù)以定周期發(fā)送給 radiolinkTask 和 usblinkTxTask，由這兩個(gè)任務(wù)分別發(fā)送飛遙控器和上位機(jī)。

usblinkRxTask：USB 通信發(fā)送任務(wù)。該任務(wù)主要負(fù)責(zé)發(fā)送atkpTxTask 發(fā)送過(guò)來(lái)的數(shù)據(jù)包，這些數(shù)據(jù)包主要是傳感器數(shù)據(jù)、姿態(tài)數(shù)據(jù)等。

3.3 飛控軟件開(kāi)發(fā)

3.3.1 姿態(tài)解算與PID算法

算法流程圖如下：

關(guān)于姿態(tài)解算，采用互補(bǔ)濾波算法進(jìn)行姿態(tài)解算，更新周期 500Hz。MCU 通過(guò)IIC(模擬 IIC)讀取加速計(jì)和陀螺儀數(shù)據(jù)寄存器，然后對(duì)加速計(jì)數(shù)據(jù) IIR 低通濾波，對(duì)陀螺儀數(shù)據(jù)加偏置調(diào)整，然后對(duì)加計(jì)數(shù)據(jù)和陀螺數(shù)據(jù)進(jìn)行融合，輸出姿態(tài)數(shù)據(jù)（roll/pitch/yaw）。

角度環(huán) PID 控制器，更新周期 500Hz，Z 軸高度 PID 控制器，更新周期 250Hz。得到實(shí)際油門(mén)值和姿態(tài)控制量數(shù)據(jù)，我們就可以把油門(mén)值和姿態(tài)控制量數(shù)據(jù)整合，整合周期 1000Hz，然后通過(guò)控制 PWM 控制電機(jī)，從而控制四軸。

目前常見(jiàn)的飛控系統(tǒng)中只使用一個(gè)姿態(tài)傳感器芯片，這個(gè)芯片集成了加速度計(jì)、陀螺儀以及磁傳感器。MPU6050算法主要代碼如下：

#include "mpu6050.h"

#include "iic.h"

#include "systick.h"

#include "acc_cal.h"

S16_XYZ accRaw = {0};                  //加速度計(jì)原始數(shù)據(jù)

S16_XYZ gyroRaw = {0};                 //陀螺儀原始數(shù)據(jù)

SI_F_XYZ accButterworthData = {0};         //加速度計(jì)巴特沃斯低通濾波后的數(shù)據(jù)

SI_F_XYZ gyroButterworthData = {0};        //陀螺儀巴特沃斯低通濾波后的數(shù)據(jù)

SI_F_XYZ acc_att_lpf = {0};

SI_F_XYZ acc_fix_lpf = {0};

SI_F_XYZ acc_1_lpf = {0};

SI_F_XYZ acc_butter_lpf = {0};

SI_F_XYZ gyro_lpf = {0};

SI_F_XYZ gyro_offset = {0,0,0};            //陀螺儀零偏數(shù)據(jù)

_Mpu6050_data Mpu = {0};

//mpu初始化

void mpu6050_init(void)

{

IIC_Write_One_Byte(0xD0,PWR_MGMT_1, 0x80);

delay_ms(100);

IIC_Write_One_Byte(0xD0,PWR_MGMT_1, 0x00);         //喚醒mpu           

/* when DLPF is disabled( DLPF_CFG=0 or 7),陀螺儀輸出頻率 = 8kHz;

       when DLPFis enabled,陀螺儀輸出頻率 = 1KHz

       fs(采樣頻率) = 陀螺儀輸出頻率 / (1 + SMPLRT_DIV)*/

IIC_Write_One_Byte(0xD0,SMPLRT_DIV, 0x00);                 //sample rate.  Fsample= 1Khz/(<this value>+1) = 1000Hz   

IIC_Write_One_Byte(0xD0,MPU_CONFIG, 0x03);              //內(nèi)部低通  acc:44hz   gyro:42hz

IIC_Write_One_Byte(0xD0,GYRO_CONFIG, 0x18);                   // gyro scale  ：+-2000deg/s

IIC_Write_One_Byte(0xD0,ACCEL_CONFIG, 0x10);                     // Accel scale ：+-8g (65536/16=4096 LSB/g)                                            

}

//兩字節(jié)數(shù)據(jù)合成

static int GetData(unsigned char REG_Address)

{

unsigned char H,L;

H = IIC_Read_One_Byte(0xD0,REG_Address);

L = IIC_Read_One_Byte(0xD0,REG_Address+1);

return ((H<<8)+L);

}

//get id

uint8_t get_mpu_id(void)

{

u8 mpu_id;

mpu_id = IIC_Read_One_Byte(0xD0,WHO_AM_I);

return mpu_id

//讀取陀螺儀三軸數(shù)據(jù)量

void GetGyroRaw(void)

{

gyroRaw.x = GetData(GYRO_XOUT_H) - gyro_offset.x;              //原始數(shù)據(jù)

gyroRaw.y = GetData(GYRO_YOUT_H) - gyro_offset.y;

gyroRaw.z = GetData(GYRO_ZOUT_H) - gyro_offset.z;

gyroButterworthData.x = (float)butterworth_lpf(((float)gyroRaw.x),&gyroButterData[0],&gyro_30hz_parameter);              //巴特沃斯低通濾波后的數(shù)據(jù)

gyroButterworthData.y = (float)butterworth_lpf(((float)gyroRaw.y),&gyroButterData[1],&gyro_30hz_parameter);

gyroButterworthData.z = (float)butterworth_lpf(((float)gyroRaw.z),&gyroButterData[2],&gyro_30hz_parameter);

}

//求取IIR濾波因子

void get_iir_factor(float *out_factor,float Time, float Cut_Off)

{

*out_factor = Time /( Time + 1/(2.0f * PI * Cut_Off) );

}

//加速度IIR低通濾波

void acc_iir_lpf(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out,float lpf_factor)

{

acc_out->x = acc_out->x + lpf_factor*(acc_in->x - acc_out->x);

acc_out->y = acc_out->y + lpf_factor*(acc_in->y - acc_out->y);

acc_out->z = acc_out->z + lpf_factor*(acc_in->z - acc_out->z);

}

//加速度計(jì)濾波參數(shù) 

_Butterworth_parameter acc_5hz_parameter =

{

1,                  -1.778631777825,    0.8008026466657,

0.005542717210281,   0.01108543442056,  0.005542717210281

};

_Butterworth_data   acc_butter_data[3];

//加速度計(jì)巴特沃斯低通

void acc_butterworth_lpf(SI_F_XYZ *accIn,SI_F_XYZ *accOut)

{

accOut->x = butterworth_lpf(accIn->x,&acc_butter_data[0],&acc_5hz_parameter);

accOut->y = butterworth_lpf(accIn->y,&acc_butter_data[1],&acc_5hz_parameter);

accOut->z = butterworth_lpf(accIn->z,&acc_butter_data[2],&acc_5hz_parameter);

}

//原始加速度量轉(zhuǎn)為 g

void AccDataTransToG(SI_F_XYZ *accIn,SI_F_XYZ *accOut)

{

accOut->x = (float)(accIn->x * acc_raw_to_g);

accOut->y = (float)(accIn->y * acc_raw_to_g);

accOut->z = (float)(accIn->z * acc_raw_to_g);

}

//濾波后的數(shù)據(jù)轉(zhuǎn)成（弧度/秒）單位

void RadTransform(SI_F_XYZ *gyroIn,SI_F_XYZ *gyroRadOut)

{

gyroRadOut->x = (float)(gyroIn->x * gyro_raw_to_radian_s);

gyroRadOut->y = (float)(gyroIn->y * gyro_raw_to_radian_s);

gyroRadOut->z = (float)(gyroIn->z * gyro_raw_to_radian_s);

}

//濾波后的數(shù)據(jù)轉(zhuǎn)成（度/秒）單位

void DegTransform(SI_F_XYZ *gyroIn,SI_F_XYZ *gyroDegOut)

{

gyroDegOut->x = (float)(gyroIn->x * gyro_raw_to_deg_s);

gyroDegOut->y = (float)(gyroIn->y * gyro_raw_to_deg_s);

gyroDegOut->z = (float)(gyroIn->z * gyro_raw_to_deg_s);

}

3.3.2 無(wú)線(xiàn)通信軟件開(kāi)發(fā)

這里根據(jù)上面介紹的初始化程序然后根據(jù)需要發(fā)送的數(shù)據(jù)將數(shù)據(jù)傳送到發(fā)送和接受緩沖區(qū)進(jìn)行發(fā)送與接受。

#include "nrf24l01.h"

#include "spi.h"

#include "systick.h"

#include "led.h"

#include "imath.h"

#include "pair_freq.h"

const u8 TX_ADDRESS[TX_ADR_WIDTH]={0x1F,0x2E,0x3D,0x4C,0x5B};

const u8 RX_ADDRESS[RX_ADR_WIDTH]={0x1F,0x2E,0x3D,0x4C,0x5B};

void NRF24L01Init(void)

{

GPIO_InitTypeDef GPIO_InitStructure;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA, ENABLE);

//CE

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

GPIO_Init(GPIOB, &GPIO_InitStructure);

//IRQ

GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_4;

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

GPIO_Init(GPIOA, &GPIO_InitStructure);

NRF_CE_L;

SPI_CSN_H;

}

//無(wú)線(xiàn)是否在位檢測(cè)

u8 NRF24L01_Check(void)

{

u8 buf[5]={0X18,0X18,0X18,0X18,0X18};

u8 i;

SPI_Write_Buf(NRF_WRITE_REG+TX_ADDR,buf,5);

SPI_Read_Buf(TX_ADDR,buf,5);

for(i=0;i<5;i++){

if(buf[i]!=0X18)

break;

}

if(i!=5)

return 1;

return 0;

}

//向寄存器寫(xiě)入值

u8 SPI_Write_Reg(u8 reg,u8 value)

{

u8 status;

SPI_CSN_L;

status = Spi_RW_Byte(reg);

Spi_RW_Byte(value);

SPI_CSN_H;

return(status);

}

//讀取寄存器值

u8 SPI_Read_Reg(u8 reg)

{

u8 reg_val;

SPI_CSN_L;

Spi_RW_Byte(reg);

reg_val = Spi_RW_Byte(0XFF);

SPI_CSN_H;

return(reg_val);

}

//讀出寄存器中連續(xù)len個(gè)字節(jié)長(zhǎng)度的值

u8 SPI_Read_Buf(u8 reg,u8 *pBuf,u8 len)

{

u8 status,u8_ctr;

SPI_CSN_L;

status = Spi_RW_Byte(reg);

for(u8_ctr=0;u8_ctr<len;u8_ctr++)

pBuf[u8_ctr]=Spi_RW_Byte(0XFF);

SPI_CSN_H;

return status;

}

//向寄存器寫(xiě)入連續(xù)len個(gè)字節(jié)的值

u8 SPI_Write_Buf(u8 reg, u8 *pBuf, u8 len)

{

u8 status,u8_ctr;

SPI_CSN_L;

status = Spi_RW_Byte(reg);

for(u8_ctr=0; u8_ctr<len; u8_ctr++)

Spi_RW_Byte(*pBuf++);

SPI_CSN_H;

return status;

}

//接收模式        

void NRF24L01ReceiveMode(void)

{

NRF_CE_L;

SPI_Write_Reg(SETUP_AW, 0x03); // 設(shè)置地址寬度為 5bytes

SPI_Write_Buf(NRF_WRITE_REG+RX_ADDR_P0,(u8*)pair.addr,RX_ADR_WIDTH);//設(shè)置接收地址（RX）

SPI_Write_Reg( NRF_WRITE_REG+FEATURE, 0x06 );//使能動(dòng)態(tài)負(fù)載長(zhǎng)度及ACK應(yīng)答

SPI_Write_Reg(NRF_WRITE_REG+DYNPD, 0x01); //使能接收管道0動(dòng)態(tài)負(fù)載長(zhǎng)度

SPI_Write_Reg(NRF_WRITE_REG+EN_AA,0x01);      //使能通道0的自動(dòng)應(yīng)答

SPI_Write_Reg(NRF_WRITE_REG+EN_RXADDR,0x01);       //使能通道0的接收地址

SPI_Write_Reg(NRF_WRITE_REG+RF_CH,pair.freq_channel);          //設(shè)置頻點(diǎn)（RF通道）

SPI_Write_Reg(NRF_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);                            //設(shè)置接收數(shù)據(jù)通道0有效數(shù)據(jù)寬度為11

SPI_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x07);                                                         //設(shè)置射頻數(shù)據(jù)率為1MHZ，發(fā)射功率為7dBm

SPI_Write_Reg(NRF_WRITE_REG+CONFIG, 0x0f);                                                          //配置基本工作模式的參數(shù);開(kāi)啟CRC，配置為接收模式,開(kāi)啟所有中斷

NRF_CE_H;

}

//接收數(shù)據(jù)包

u8 NRF24L01_RxPacket(u8 *rxbuf)

{

u8 sta;

sta = SPI_Read_Reg(NRF_READ_REG+STATUS);                                            //狀態(tài)標(biāo)志位

SPI_Write_Reg(NRF_WRITE_REG+STATUS,sta);

if(sta&RX_OK)                                                                        //接收成功

{

SPI_Read_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);

SPI_Write_Reg(FLUSH_RX,0xff);

return 0;

}

return 1;

}

//該函數(shù)初始化NRF24L01到TX模式

//設(shè)置TX地址,寫(xiě)TX數(shù)據(jù)寬度,設(shè)置RX自動(dòng)應(yīng)答的地址,填充TX發(fā)送數(shù)據(jù),選擇RF頻道,波特率和LNA HCURR

//PWR_UP,CRC使能

//當(dāng)CE變高后,即進(jìn)入RX模式,并可以接收數(shù)據(jù)了              

//CE為高大于10us,則啟動(dòng)發(fā)送.  

void NRF24L01_TX_Mode(void)

{

NRF_CE_L;

SPI_Write_Reg(SETUP_AW, 0x03); // 設(shè)置地址寬度為 5bytes

SPI_Write_Buf(NRF_WRITE_REG+TX_ADDR,(uint8_t*)pair.addr,TX_ADR_WIDTH);    //寫(xiě)TX節(jié)點(diǎn)地址

SPI_Write_Buf(NRF_WRITE_REG+RX_ADDR_P0,(uint8_t*)pair.addr,RX_ADR_WIDTH); //設(shè)置TX節(jié)點(diǎn)地址,主要為了接收ACK        

//NRF24L01_Write_Reg(NRF_WRITE_REG+FEATURE, 0x02 );//使能動(dòng)態(tài)負(fù)載長(zhǎng)度及帶負(fù)載的ACK應(yīng)答

//NRF24L01_Write_Reg(NRF_WRITE_REG+DYNPD, 0x01); //使能接收管道0動(dòng)態(tài)負(fù)載長(zhǎng)度

SPI_Write_Reg(NRF_WRITE_REG+EN_AA,0x01);               //使能通道0的自動(dòng)應(yīng)答   

SPI_Write_Reg(NRF_WRITE_REG+EN_RXADDR,0x01);           //使能通道0的接收地址 

SPI_Write_Reg(NRF_WRITE_REG+RF_CH,pair.freq_channel);  //設(shè)置RF通道

SPI_Write_Reg(NRF_WRITE_REG+SETUP_RETR,0x1a);          //設(shè)置自動(dòng)重發(fā)間隔時(shí)間:500us;最大自動(dòng)重發(fā)次數(shù):10次

SPI_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x07);                                    //設(shè)置射頻數(shù)據(jù)率為1MHZ，發(fā)射功率為7dBm

SPI_Write_Reg(NRF_WRITE_REG+CONFIG,0x0e);              //配置基本工作模式的參數(shù);開(kāi)啟CRC，配置為發(fā)射模式,開(kāi)啟所有中斷

NRF_CE_H;                                          //CE為高,10us后啟動(dòng)發(fā)送

}

//啟動(dòng)NRF24L01發(fā)送一次數(shù)據(jù)

//sendBuff:待發(fā)送數(shù)據(jù)首地址

//返回值:發(fā)送完成狀況

uint8_t NRF24L01_TxPacket(uint8_t *sendBuff)

{

uint8_t state;

NRF_CE_L;

//NRF24L01_Write_Buf(SPI_WRITE_REG+RX_ADDR_P0,(uint8_t*)pair.addr,RX_ADR_WIDTH);

SPI_Write_Buf(WR_TX_PLOAD,sendBuff,TX_PLOAD_WIDTH);

NRF_CE_H;            //啟動(dòng)發(fā)送

while(NRF_IRQ!=0);           //等待發(fā)送完成

state=SPI_Read_Reg(NRF_WRITE_REG+STATUS);         //讀取狀態(tài)寄存器的值       

SPI_Write_Reg(NRF_WRITE_REG+STATUS,state);         //清除TX_DS或MAX_RT中斷標(biāo)志

if(state&MAX_TX){            //達(dá)到最大重發(fā)次數(shù)      

SPI_Write_Reg(FLUSH_TX,0xff);           //清除TX FIFO寄存器    

return MAX_TX;

}

if(state&TX_OK){             //發(fā)送完成

return TX_OK;

}

return 0xff;         //其他原因發(fā)送失敗

}

3.3.3 角度環(huán) PID 和角速度環(huán) PID

PID 更新函數(shù)，PID 采用的標(biāo)準(zhǔn) PID，其數(shù)學(xué)公式如下：

說(shuō)明：如何將該公式進(jìn)行轉(zhuǎn)換，我是得到了老師的幫助以及師兄的指點(diǎn)，再次感謝老師和師兄的幫助！??！具體轉(zhuǎn)換如下

將數(shù)學(xué)公式轉(zhuǎn)換為 C 代碼，PID 更新函數(shù)是這樣的：

float pidUpdate(PidObject* pid, const float error)

{

float output;

pid->error = error;

pid->integ += pid->error * pid->dt;

if (pid->integ > pid->iLimit)

{

pid->integ = pid->iLimit;

}

else if (pid->integ < pid->iLimitLow)

{

ALIENTEK

MiniFly

28 / 48

ATK-MiniFly 開(kāi)發(fā)指南

pid->integ = pid->iLimitLow;

}

pid->deriv = (pid->error - pid->prevError) / pid->dt;

pid->outP = pid->kp * pid->error;

pid->outI = pid->ki * pid->integ;

pid->outD = pid->kd * pid->deriv;

output = pid->outP + pid->outI + pid->outD;

pid->prevError = pid->error;

return output;

}

PidObject 為 PID 對(duì)象結(jié)構(gòu)體數(shù)據(jù)類(lèi)型，第一個(gè)參數(shù)為將被更新的 PID 結(jié)構(gòu)體對(duì)象，第二個(gè)參數(shù)則是偏差（期望值-測(cè)量值），積分項(xiàng)為偏差對(duì)時(shí)間的積分，微分項(xiàng)則是偏差對(duì)時(shí)間的微分，然后函數(shù)里面有三個(gè)參數(shù) pid->kp，pid->ki，pid->kd 分別指的是該 pid 對(duì)象的比例項(xiàng)，積分項(xiàng)和微分項(xiàng)系數(shù)，每個(gè) pid 對(duì)象都有屬于自己的 PID 系數(shù)，PID 初始化 pid 對(duì)象的時(shí)候會(huì)設(shè)定一組默認(rèn)的系數(shù)，同時(shí)這組系數(shù)是可以調(diào)整的，我們常說(shuō)的 PID 參數(shù)整定，其實(shí)就是調(diào)整這組系數(shù)，讓它滿(mǎn)足你的系統(tǒng)。

其函數(shù)原型如下：

void attitudeAnglePID(attitude_t *actualAngle, attitude_t *desiredAngle, attitude_t *outDesiredRate)

{/* 角度環(huán) PID */

outDesiredRate->roll = pidUpdate(&pidAngleRoll, desiredAngle->roll - actualAngle->roll);

outDesiredRate->pitch = pidUpdate(&pidAnglePitch, desiredAngle->pitch - actualAngle->pitch);

float yawError = desiredAngle->yaw - actualAngle->yaw ;

if (yawError > 180.0f)

yawError -= 360.0f;

else if (yawError < -180.0)

yawError += 360.0f;

outDesiredRate->yaw = pidUpdate(&pidAngleYaw, yawError);

}

attitude_t 是一個(gè)姿態(tài)數(shù)據(jù)結(jié)構(gòu)類(lèi)型，函數(shù)參數(shù) actualAngle 是一個(gè)結(jié)構(gòu)體指針，指向?qū)嶋H角度結(jié)構(gòu)體變量（數(shù)據(jù)融合輸出值）state->attitude, desiredAngle 指向期望角度結(jié)構(gòu)體變量（設(shè)置的角度）attitudeDesired，outDesiredRate 則是角度環(huán)的輸出，指向期望角速度結(jié)構(gòu)體變量 rateDesired。

然后是角速度環(huán) PID，其函數(shù)原型如下：

void attitudeRatePID(Axis3f *actualRate, attitude_t *desiredRate, control_t *output)

{/* 角速度環(huán) PID */

output->roll = pidOutLimit(pidUpdate(&pidRateRoll, desiredRate->roll - actualRate->x));

output->pitch = pidOutLimit(pidUpdate(&pidRatePitch, desiredRate->pitch - actualRate->y));

ALIENTEK

MiniFly

29 / 48

ATK-MiniFly 開(kāi)發(fā)指南

output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, desiredRate->yaw - actualRate->z));

}

3.3.4 姿態(tài)控制量和油門(mén)值整合

設(shè)置X 模式飛行，電機(jī)轉(zhuǎn)向和姿態(tài)解算正方向（箭頭指示正方向）：

control->thrust 為油門(mén)控制量，這個(gè)值增大四軸升高，減小則下降。control->roll，control->pitch，control->yaw 為 PID 輸出的姿態(tài)控制量。油門(mén)控制量和姿態(tài)控制量整合后控制電機(jī)，整合代碼在 power_control.c 文件的函數(shù) powerControl ()中實(shí)現(xiàn)，代碼如下：

void powerControl(control_t *control) /*功率輸出控制*/

{

s16 r = control->roll / 2.0f;

s16 p = control->pitch / 2.0f;

motorPWM.m1 = limitThrust(control->thrust - r - p + control->yaw);

motorPWM.m2 = limitThrust(control->thrust - r + p - control->yaw);

ALIENTEK

MiniFly

31 / 48

ATK-MiniFly 開(kāi)發(fā)指南

motorPWM.m3 = limitThrust(control->thrust + r + p + control->yaw);

motorPWM.m4 = limitThrust(control->thrust + r - p - control->yaw);

if (motorSetEnable)

{

motorPWM=motorPWMSet;

}

motorsSetRatio(MOTOR_M1, motorPWM.m1); /*控制電機(jī)輸出百分比*/

motorsSetRatio(MOTOR_M2, motorPWM.m2);

motorsSetRatio(MOTOR_M3, motorPWM.m3);

motorsSetRatio(MOTOR_M4, motorPWM.m4);

}

Roll 方向受外力向左旋轉(zhuǎn)（向右為正），為了恢復(fù)平衡，則 M3 和 M4 同側(cè)出力，M1和 M2 反向出力（m1 和 m2 的 Roll 為-，m3 和 m4 的 Roll 為+）;
Pitch 方向受外力向后旋轉(zhuǎn)（向前為正），為了恢復(fù)平衡，則 M2 和 M3 同側(cè)出力，M1和 M4 反向出力（m1 和 m4 的 Pitch 為-，m2 和 m3 的 Pitch 為+）;
Yaw 方向受外力順時(shí)針旋轉(zhuǎn)（逆時(shí)針為正），為了恢復(fù)平衡，則 M1 和 M3 同側(cè)出力，M2 和 M4 反向出力，（m2 和 m4 的 Yaw 為-，m1 和 m3 的 Yaw 為+）;
bool 型變量 motorSetEnable為 true，使能手動(dòng)設(shè)置電機(jī)占空比，這樣可以方便單獨(dú)調(diào)試某幾個(gè)電機(jī)，默認(rèn)不使能。
motorsSetRatio()當(dāng)然就是設(shè)定對(duì)應(yīng)電機(jī)定時(shí)器通道占空比的函數(shù)了，設(shè)定的占空比作用到 MOS 管，然后控制電機(jī)，從而控制四軸。

3.3.5 4D 空翻算法

4D 空翻實(shí)現(xiàn)原理是只使用內(nèi)環(huán) PID–角速度環(huán) PID 控制器，姿態(tài)角度期望值直接作為角速度環(huán)的期望值，測(cè)量值使用 3 軸陀螺儀數(shù)據(jù)，這樣我們控制的不是四軸的角度，而是四軸的轉(zhuǎn)動(dòng)角速度。知道了如何控制四軸轉(zhuǎn)動(dòng)，當(dāng)然就能控制翻滾了。
4D 空翻源碼比較多，我就不貼出來(lái)了，空翻具體實(shí)現(xiàn)過(guò)程自行去 flip.c 文件查看空翻實(shí)現(xiàn)函數(shù) flyerFlipCheck()，空翻過(guò)程有好幾個(gè)狀態(tài)，flipState 指示空翻的當(dāng)前狀態(tài)，其定義如下：

static enum

{

FLIP_IDLE = 0,

FLIP_SET,

FLIP_SPEED_UP,

FLIP_SLOW_DOWN,

ALIENTEK

MiniFly

32 / 48

ATK-MiniFly 開(kāi)發(fā)指南

FLIP_PERIOD,

FLIP_FINISHED,

REVER_SPEED_UP,

FLIP_ERROR,

}flipState = FLIP_IDLE;

FLIP_IDLE 為空翻空閑狀態(tài)，在此狀態(tài)下，四軸實(shí)時(shí)檢測(cè)是否要執(zhí)行空翻命令。如果檢測(cè)到空翻指令，則狀態(tài)切換到 FLIP_SET，在此狀態(tài)下，我們?cè)O(shè)置一些四軸翻滾用到的參數(shù)，參數(shù)設(shè)置完成后切換到加速上升狀態(tài) FLIP_SPEED_UP，因?yàn)榭辗丶紩?huì)有掉高問(wèn)題，所以我們?cè)谡嬲?4D 翻滾之前先加速一段時(shí)間，當(dāng) Z 軸速度達(dá)到一定值后，進(jìn)入減速狀態(tài)，為什么翻滾之前要這個(gè)減速狀態(tài)呢，答案是為了更好的空翻。減速到設(shè)定值后才進(jìn)入真正的翻滾狀態(tài) FLIP_PERIOD，前面狀態(tài)都是為空翻做準(zhǔn)備的。

3.4 限制于篇幅，其他模塊的算法不做說(shuō)明（太多了……）

第二部分：基于stm32的多旋翼無(wú)人機(jī)（Multi-rotor UAV based on stm32)（下）

博客主頁(yè)：https://blog.csdn.net/weixin_51141489，需要源碼或相關(guān)資料實(shí)物的友友請(qǐng)關(guān)注、點(diǎn)贊，私信吧！