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

一、多旋翼無人機飛行原理

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

1.1 多旋翼無人機的組成

四軸飛行器主要是由電機、電調、電池、漿葉、機架、遙控器、飛控組成。下面以我們四軸及市場上常見的 DIY 大四軸來介紹這些部件。

1.1.1 電機

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

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

1.1.2 電調

電調即為電子調速器，控制電機轉動、停止及轉速。有刷電機電調通常只需要一個 MOS 管，飛控輸出 PWM 即可控制電機，電調所采用的MOS管如下圖所示：無刷電機電調模塊內部通常由一個 MCU 和三相橋電路組成，MCU 通過控制三相橋來實現(xiàn)無刷電機換相。同樣，無刷電機電調模塊也只需飛控輸出 PWM 即可控制電機

1.1.3 航模電池

航模所用電池為3.7v鋰電池（可充電），由于受到多旋翼無人機自身動力的問題，故采用800mh鋰電池來供電，遙控器也采用800mh供電，飛行時間大概20min左右。電池如下圖：

電池插座采用空對空接頭。

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

1.1.4 正反漿

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

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

1.2 垂直上升及下降

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

1.3 向前飛行及向后飛行

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

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

1.4 順時針改變航向和逆時針改變航向

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

二、多旋翼無人機飛控電路設計

2.1 系統(tǒng)框架設計

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

2.2 主控MCU模塊

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

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

2.3 三軸加速陀螺儀模塊

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

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

設計電路圖如下：

2.4 電機驅動模塊

本項目采用微型高速 8520空心杯電機，電機轉速高達 15000r/min，能夠為飛行器提供充沛的動力。電機采用 NMOS 管 SI2302，3V 門級驅動電壓下，導通電阻只有幾十毫歐，驅動電流高達 3A，輕松驅動 8520空心杯電機，從而帶動飛行器飛行。電路設計如下圖：

2.5 無線通信模塊

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

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

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

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

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

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

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

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

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

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

2.7 LED電路模塊

此部分為LED指示燈電路設計，主要由3個LED直插式燈以及兩枚RGB貼片燈組成，具體i單路如下圖所示：

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

三、多旋翼無人機飛控算法設計

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

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

3.1.1 歐拉角

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

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

3.1.2 四元數(shù)

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

其中的q為一個四元數(shù)，其模的長度為：

對四元數(shù)進行單位化，與線性代數(shù)中的單位話相似，可得到：

再由創(chuàng)造出來一個變量q關于旋轉角得到的一個變量，即可表示為：

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

3.1.3 PID控制

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

3.2 飛控軟件框架設計

主要程序設計框圖參考了Minfly的設計框圖：

主要任務關系如下：

說明：此處不包含APP設計。

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

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

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

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

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

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

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

3.3 飛控軟件開發(fā)

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

算法流程圖如下：

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

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

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

#include "mpu6050.h"

#include "iic.h"

#include "systick.h"

#include "acc_cal.h"

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

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

SI_F_XYZ accButterworthData = {0};         //加速度計巴特沃斯低通濾波后的數(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);              //內部低通  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);

}

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

_Butterworth_parameter acc_5hz_parameter =

{

1,                  -1.778631777825,    0.8008026466657,

0.005542717210281,   0.01108543442056,  0.005542717210281

};

_Butterworth_data   acc_butter_data[3];

//加速度計巴特沃斯低通

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);

}

//原始加速度量轉為 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ù)轉成（弧度/秒）單位

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ù)轉成（度/秒）單位

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 無線通信軟件開發(fā)

這里根據(jù)上面介紹的初始化程序然后根據(jù)需要發(fā)送的數(shù)據(jù)將數(shù)據(jù)傳送到發(fā)送和接受緩沖區(qū)進行發(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;

}

//無線是否在位檢測

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;

}

//向寄存器寫入值

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個字節(jié)長度的值

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;

}

//向寄存器寫入連續(xù)len個字節(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); // 設置地址寬度為 5bytes

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

SPI_Write_Reg( NRF_WRITE_REG+FEATURE, 0x06 );//使能動態(tài)負載長度及ACK應答

SPI_Write_Reg(NRF_WRITE_REG+DYNPD, 0x01); //使能接收管道0動態(tài)負載長度

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

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

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

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

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

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

NRF_CE_H;

}

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

u8 NRF24L01_RxPacket(u8 *rxbuf)

{

u8 sta;

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

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模式

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

//PWR_UP,CRC使能

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

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

void NRF24L01_TX_Mode(void)

{

NRF_CE_L;

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

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

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

//NRF24L01_Write_Reg(NRF_WRITE_REG+FEATURE, 0x02 );//使能動態(tài)負載長度及帶負載的ACK應答

//NRF24L01_Write_Reg(NRF_WRITE_REG+DYNPD, 0x01); //使能接收管道0動態(tài)負載長度

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

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

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

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

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

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

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

}

//啟動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;            //啟動發(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中斷標志

if(state&MAX_TX){            //達到最大重發(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 采用的標準 PID，其數(shù)學公式如下：

說明：如何將該公式進行轉換，我是得到了老師的幫助以及師兄的指點，再次感謝老師和師兄的幫助?。?！具體轉換如下

將數(shù)學公式轉換為 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 開發(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 對象結構體數(shù)據(jù)類型，第一個參數(shù)為將被更新的 PID 結構體對象，第二個參數(shù)則是偏差（期望值-測量值），積分項為偏差對時間的積分，微分項則是偏差對時間的微分，然后函數(shù)里面有三個參數(shù) pid->kp，pid->ki，pid->kd 分別指的是該 pid 對象的比例項，積分項和微分項系數(shù)，每個 pid 對象都有屬于自己的 PID 系數(shù)，PID 初始化 pid 對象的時候會設定一組默認的系數(shù)，同時這組系數(shù)是可以調整的，我們常說的 PID 參數(shù)整定，其實就是調整這組系數(shù)，讓它滿足你的系統(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 是一個姿態(tài)數(shù)據(jù)結構類型，函數(shù)參數(shù) actualAngle 是一個結構體指針，指向實際角度結構體變量（數(shù)據(jù)融合輸出值）state->attitude, desiredAngle 指向期望角度結構體變量（設置的角度）attitudeDesired，outDesiredRate 則是角度環(huán)的輸出，指向期望角速度結構體變量 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 開發(fā)指南

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

}

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

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

control->thrust 為油門控制量，這個值增大四軸升高，減小則下降。control->roll，control->pitch，control->yaw 為 PID 輸出的姿態(tài)控制量。油門控制量和姿態(tài)控制量整合后控制電機，整合代碼在 power_control.c 文件的函數(shù) powerControl ()中實現(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 開發(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); /*控制電機輸出百分比*/

motorsSetRatio(MOTOR_M2, motorPWM.m2);

motorsSetRatio(MOTOR_M3, motorPWM.m3);

motorsSetRatio(MOTOR_M4, motorPWM.m4);

}

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

3.3.5 4D 空翻算法

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

static enum

{

FLIP_IDLE = 0,

FLIP_SET,

FLIP_SPEED_UP,

FLIP_SLOW_DOWN,

ALIENTEK

MiniFly

32 / 48

ATK-MiniFly 開發(fā)指南

FLIP_PERIOD,

FLIP_FINISHED,

REVER_SPEED_UP,

FLIP_ERROR,

}flipState = FLIP_IDLE;

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

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

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

博客主頁：https://blog.csdn.net/weixin_51141489，需要源碼或相關資料實物的友友請關注、點贊，私信吧！