由于一直在調試本項目，好久沒有發文章，最近本項目的PID調試初見成效！開始正文前首先感謝各位粉絲的支持，以及對本項目技術上支持的老師以及師兄，謝謝你們！

對應源碼及文件：源碼及文件下載

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

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

1.1 多旋翼無人機的組成

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

1.1.1 電機

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

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

1.1.2 電調

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

1.1.3 航模電池

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

電池插座采用空對空接頭。
電池容量 mAh：鋰電池的容量，如 2000mAh 的電池，以 2000mA 放電，可持續放電 1
小時；以 1000mA 放電，可持續放電 2 小時。
電池節數：電池 2S、3S、4S 代表鋰電池節數。鋰電池 1 節的標準電壓為 3.7V，3S 代
表有 3 節 3.7V 的電池在里面，電壓為 11.1V。

1.1.4 正反漿

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

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

1.2 垂直上升及下降

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

1.3 向前飛行及向后飛行

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

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

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

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

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

2.1 系統框架設計

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

2.2 主控MCU模塊

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

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

2.3 三軸加速陀螺儀模塊

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

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

設計電路圖如下：

2.4 電機驅動模塊

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

2.5 無線通信模塊

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

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

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

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

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

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

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

2.6 升壓、穩壓電路模塊

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

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

2.7 LED電路模塊

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

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

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

3.1 飛行姿態表示法

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

3.1.1 歐拉角

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

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

3.1.2 四元數

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

其中的q為一個四元數，其模的長度為：

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

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

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

3.1.3 PID控制

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

3.2 飛控軟件框架設計

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

主要任務關系如下：

說明：此處不包含APP設計。
radiolinkTask：無線通信任務。該任務主要負責接收從 NRF51822 發送（串口方式）過來的數據，然后對數據進行打包和校驗，打包成 ATKP 格式并校驗無誤后發送到atkpRxAnlTask 的接收隊列里，同時回傳一幀數據給 NRF51822。

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

atkpRxAnlTask：ATKP 數據包接收處理任務。該任務主要是處理遙控器和上位機發下來的數據包，解析到的控制指令則發送到 stabilizerTask 中去。
stabilizerTask：四軸平衡控制任務。該任務運行的內容比較多，也是比較關鍵的內容。包括傳感器數據讀取，數據融合，獲取控制數據，空翻檢測，異常檢測，PID 控制，PWM輸出控制等。

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

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

usblinkRxTask：USB 通信發送任務。該任務主要負責發送atkpTxTask 發送過來的數據包，這些數據包主要是傳感器數據、姿態數據等。

3.3 飛控軟件開發

3.3.1 姿態解算與PID算法

算法流程圖如下：

關于姿態解算，采用互補濾波算法進行姿態解算，更新周期 500Hz。MCU 通過IIC(模擬 IIC)讀取加速計和陀螺儀數據寄存器，然后對加速計數據 IIR 低通濾波，對陀螺儀數據加偏置調整，然后對加計數據和陀螺數據進行融合，輸出姿態數據（roll/pitch/yaw）。
角度環 PID 控制器，更新周期 500Hz，Z 軸高度 PID 控制器，更新周期 250Hz。得到實際油門值和姿態控制量數據，我們就可以把油門值和姿態控制量數據整合，整合周期 1000Hz，然后通過控制 PWM 控制電機，從而控制四軸。
目前常見的飛控系統中只使用一個姿態傳感器芯片，這個芯片集成了加速度計、陀螺儀以及磁傳感器。MPU6050算法主要代碼如下：

#include "mpu6050.h"
#include "iic.h"
#include "systick.h"
#include "acc_cal.h"S16_XYZ accRaw = {0};                  //加速度計原始數據
S16_XYZ gyroRaw = {0};                 //陀螺儀原始數據
SI_F_XYZ accButterworthData = {0};		//加速度計巴特沃斯低通濾波后的數據
SI_F_XYZ gyroButterworthData = {0};		//陀螺儀巴特沃斯低通濾波后的數據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};		//陀螺儀零偏數據_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:42hzIIC_Write_One_Byte(0xD0,GYRO_CONFIG, 0x18);			    // gyro scale  ：+-2000deg/sIIC_Write_One_Byte(0xD0,ACCEL_CONFIG, 0x10);			// Accel scale ：+-8g (65536/16=4096 LSB/g)    			   			
}//兩字節數據合成
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//讀取陀螺儀三軸數據量
void GetGyroRaw(void)
{gyroRaw.x = GetData(GYRO_XOUT_H) - gyro_offset.x;		//原始數據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);		//巴特沃斯低通濾波后的數據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); 
}//加速度計濾波參數  
_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);
}//濾波后的數據轉成（弧度/秒）單位
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);
}//濾波后的數據轉成（度/秒）單位
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 無線通信軟件開發

這里根據上面介紹的初始化程序然后根據需要發送的數據將數據傳送到發送和接受緩沖區進行發送與接受。

#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);		//CEGPIO_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);//IRQGPIO_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);        
}	//讀出寄存器中連續len個字節長度的值
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;        
}//向寄存器寫入連續len個字節的值
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); // 設置地址寬度為 5bytesSPI_Write_Buf(NRF_WRITE_REG+RX_ADDR_P0,(u8*)pair.addr,RX_ADR_WIDTH);//設置接收地址（RX）SPI_Write_Reg( NRF_WRITE_REG+FEATURE, 0x06 );//使能動態負載長度及ACK應答SPI_Write_Reg(NRF_WRITE_REG+DYNPD, 0x01); //使能接收管道0動態負載長度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);				//設置接收數據通道0有效數據寬度為11SPI_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x07);									//設置射頻數據率為1MHZ，發射功率為7dBmSPI_Write_Reg(NRF_WRITE_REG+CONFIG, 0x0f);									//配置基本工作模式的參數;開啟CRC，配置為接收模式,開啟所有中斷NRF_CE_H; 
}						 //接收數據包
u8 NRF24L01_RxPacket(u8 *rxbuf)
{u8 sta;		    				sta = SPI_Read_Reg(NRF_READ_REG+STATUS); 	 					    //狀態標志位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;
}					    //該函數初始化NRF24L01到TX模式
//設置TX地址,寫TX數據寬度,設置RX自動應答的地址,填充TX發送數據,選擇RF頻道,波特率和LNA HCURR
//PWR_UP,CRC使能
//當CE變高后,即進入RX模式,并可以接收數據了		   
//CE為高大于10us,則啟動發送.	 
void NRF24L01_TX_Mode(void)
{														 NRF_CE_L;		 SPI_Write_Reg(SETUP_AW, 0x03); // 設置地址寬度為 5bytesSPI_Write_Buf(NRF_WRITE_REG+TX_ADDR,(uint8_t*)pair.addr,TX_ADR_WIDTH);    //寫TX節點地址 SPI_Write_Buf(NRF_WRITE_REG+RX_ADDR_P0,(uint8_t*)pair.addr,RX_ADR_WIDTH); //設置TX節點地址,主要為了接收ACK	  //NRF24L01_Write_Reg(NRF_WRITE_REG+FEATURE, 0x02 );//使能動態負載長度及帶負載的ACK應答//NRF24L01_Write_Reg(NRF_WRITE_REG+DYNPD, 0x01); //使能接收管道0動態負載長度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);          //設置自動重發間隔時間:500us;最大自動重發次數:10次SPI_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x07);						//設置射頻數據率為1MHZ，發射功率為7dBmSPI_Write_Reg(NRF_WRITE_REG+CONFIG,0x0e);              //配置基本工作模式的參數;開啟CRC，配置為發射模式,開啟所有中斷NRF_CE_H;                                          //CE為高,10us后啟動發送
}		  //啟動NRF24L01發送一次數據
//sendBuff:待發送數據首地址
//返回值:發送完成狀況
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;		//啟動發送while(NRF_IRQ!=0);		//等待發送完成state=SPI_Read_Reg(NRF_WRITE_REG+STATUS);		//讀取狀態寄存器的值	   SPI_Write_Reg(NRF_WRITE_REG+STATUS,state);		//清除TX_DS或MAX_RT中斷標志if(state&MAX_TX){		//達到最大重發次數	SPI_Write_Reg(FLUSH_TX,0xff);		//清除TX FIFO寄存器 	return MAX_TX; }if(state&TX_OK){		//發送完成return TX_OK;}return 0xff;		//其他原因發送失敗
}

3.3.3 角度環 PID 和角速度環 PID

PID 更新函數，PID 采用的標準 PID，其數學公式如下：

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

將數學公式轉換為 C 代碼，PID 更新函數是這樣的：

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 開發指南
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 對象結構體數據類型，第一個參數為將被更新的 PID 結構體對象，第二個參數則是偏差（期望值-測量值），積分項為偏差對時間的積分，微分項則是偏差對時間的微分，然后函數里面有三個參數 pid->kp，pid->ki，pid->kd 分別指的是該 pid 對象的比例項，積分項和微分項系數，每個 pid 對象都有屬于自己的 PID 系數，PID 初始化 pid 對象的時候會設定一組默認的系數，同時這組系數是可以調整的，我們常說的 PID 參數整定，其實就是調整這組系數，讓它滿足你的系統。
其函數原型如下：

void attitudeAnglePID(attitude_t *actualAngle, attitude_t *desiredAngle, attitude_t *outDesiredRate)
{/* 角度環 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 是一個姿態數據結構類型，函數參數 actualAngle 是一個結構體指針，指向實際角度結構體變量（數據融合輸出值）state->attitude, desiredAngle 指向期望角度結構體變量（設置的角度）attitudeDesired，outDesiredRate 則是角度環的輸出，指向期望角速度結構體變量 rateDesired。
然后是角速度環 PID，其函數原型如下：

void attitudeRatePID(Axis3f *actualRate, attitude_t *desiredRate, control_t *output)
{/* 角速度環 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 開發指南
output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, desiredRate->yaw - actualRate->z));
}

3.3.4 姿態控制量和油門值整合

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

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

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 開發指南
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()當然就是設定對應電機定時器通道占空比的函數了，設定的占空比作用到 MOS 管，然后控制電機，從而控制四軸。

3.3.5 4D 空翻算法

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

static enum
{
FLIP_IDLE = 0,
FLIP_SET,
FLIP_SPEED_UP,
FLIP_SLOW_DOWN,
ALIENTEK
MiniFly
32 / 48
ATK-MiniFly 開發指南
FLIP_PERIOD,
FLIP_FINISHED,
REVER_SPEED_UP,
FLIP_ERROR,
}flipState = FLIP_IDLE;

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

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

四、飛控的制作

制作之前需要將原理圖導入PCB并設計出板子形狀。

4.1 PCB板圖

為方便后續封裝的配置，將原理圖導入到AD軟甲中進行設計，板圖如下：分別為飛機和遙控器板圖：


將設計圖紙發給廠家，最終得到上面板子的板子：

4.2 貼片器件的焊接

因為以前沒有焊接貼片式器件的經驗，所以去工作室練習了芯片燈復雜器件的焊接。完成這些之前，你需要有自己的加熱臺，有條件的可以買個熱風槍。
將器件按照原理圖中的位置進行擺放，我采購的器件的封裝為0603封裝（僅限電阻電感），如下圖：

4.3 實物圖

最終可以得到實物如下：

五、飛控程序燒錄及調試

焊接好器件就可以進行程序的燒錄以及+
器件的調試工作了。
在這里我想說一下我遇到的一些問題，以及解決的方法。在調試的過程，我重復了上面3次的焊接步驟，具體原因如下：

• 芯片焊接出現問題，有了連錫的現象

• MPU6050封裝加熱時出現，由于加熱時間過長，導致焊盤脫落

• 遙控器焊接完畢，OLED顯示屏不能顯示，最終發現是程序出現了問題，最終導致芯片燒壞，被迫換板

如下圖，分別是上述三種原因導致的焊接失敗的板子：



經過三次的失敗，我得到了自己的焊接芯片的技術方法，最終焊接成功。如圖：


程序的燒錄采用的是keil軟件進行燒錄：

此處顯示編譯無誤，即可調試進行燒錄程序，如果芯片焊接無誤這里顯示的是這樣的：

或者采用STM32 unity進行驗證，單擊這里：

如果出現如圖所示，證明芯片焊接沒有問題：

之后就在keil中進行燒錄程序：單機load進行燒錄，等待完成。遙控器也是同樣的操作，完成步驟后如圖：


然后成品就完成了，后面我根據需求，自己加裝了天線。

六、 總結

裝箱后的飛機就是這樣，目前還沒有試飛，有空時候去外邊飛一飛看，整機花銷100以內，還是很親民的，哈哈哈哈哈

創作不易，這個飛機花費了太多的時間，希望粉絲朋友們喜歡，謝謝大家！！！