基于 STM32 的多旋翼无人机设计与实现

1.3 向前飞行及向后飞行

当四轴飞行在空中自稳后，M2、M3 转速增大 M1、M4 转速不变或减小即可实现向前运动。相反，M2、M3 转速减小或不变 M1、M4 转速增加，即可实现向后运动，如下图所示：

向左和向右飞行同理可以实现，这里就不做介绍。

1.4 顺时针改变航向和逆时针改变航向

当四轴飞行在空中自稳后，M1、M3 转速增大 M2、M4 转速不变或减小即可实现顺时针改变航向。M1、M3 转速减小或不变 M2、M4 转速增加即可实现逆时针改变航向。如下图所示：

二、多旋翼无人机飞控电路设计

2.1 系统框架设计

根据 MiniFly 的系统框架图，来进行设计，主控芯片采用 STM32F103C8T6，三轴陀螺仪采用 MPU6050 芯片，收发无线模块采用 NRF24L01 模块等。

2.2 主控 MCU 模块

主控采用的是 STM32F103C8T6 芯片，电路图如下：

主控 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

要实现通信还需要 E01-ML01D 模块来实现交互通信，模块的图片如下：

芯片方案：nRF24L01P 工作频率：2.4~2.525GHz 发射功率：0dBm 通信距离：0.1km 通信接口：SPI 产品重量：0.9±0.1g

无线通信模块电路图如下：（分别为接受端和发送端）

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 贴片灯组成，具体单路如下图所示：

后续逻辑功能将在程序中进行设计。

三、多旋翼无人机飞控算法设计

3.1 飞行姿态表示法

飞行器姿态有多种表示方式，常见的是四元数，欧拉角，矩阵和轴角。他们各自有其自身的优点，在不同的领域使用不同的表示方式。在四轴飞行器中使用到了四元数和欧拉角。

3.1.1 欧拉角

用来确定定点转动刚体位置的 3 个一组独立角参量，由章动角 θ、旋进角（即进动角）ψ 和自转角 φ 组成，为莱昂哈德·欧拉首先提出而得名。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。 如下图所示：

设定 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 飞控软件框架设计

主要程序设计框图参考了 MiniFly 的设计框图：

主要任务关系如下：

说明：此处不包含 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，由这两个任务分别发送飞遥控器和上位机。

usblinkTxTask：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){
        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));
    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);
    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,
    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 以内，还是很亲民的。