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

一、多旋翼无人机飞行原理

四轴飞行器基本原理是通过飞控控制四个电机旋转带动浆叶产生升力，分别控制每一个电机和浆叶产生不同的升力从而控制飞行器的姿态和位置。四轴在空中可以实现八种运动，分别为垂直上升、垂直下降、向前运动、向后运动、向左运动、向后运动、顺时针改变航向、逆时针改变航向。

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

}

//两字节数据合成

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

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

}

//读出寄存器中连续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); // 设置地址宽度为 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 );//使能动态负载长度及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有效数据宽度为11

SPI_Write_Reg(NRF_WRITE_REG+RF_SETUP,0x07);                                                         //设置射频数据率为1MHZ，发射功率为7dBm

SPI_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); // 设置地址宽度为 5bytes

SPI_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，发射功率为7dBm

SPI_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 限制于篇幅，其他模块的算法不做说明（太多了……）

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

博客主页：https://blog.csdn.net/weixin_51141489，需要源码或相关资料实物的友友请关注、点赞，私信吧！