代码收藏家 基于STM32F103单片机的小四轴飞行器开发指南
序言
本文采用STM32F103C8T6做主控芯片，整体控制思路分为以下四步：
1、获取飞行器六轴数据：MPU6050采集飞行器原始六轴数据（三轴加速度、三轴角速度），通过卡尔曼滤波算法对加速度进行滤波、角速度采用一阶低通滤波。
2、进行姿态解算：对滤波后的数据采用四元数姿态解算，得到飞行器姿态：欧拉角（翻滚角、俯仰角和偏航角）。
3、获取手柄控制数据（期望值）：通过NRF24L01无线模块，获取遥控手柄数据，包括：油门、翻滚角、俯仰角和偏航角。
4、通过PID调节控制电机转速：对获取到的期望值与实际值，采用PID算法，控制定时器输出，调节电机转速。
硬件准备：
1）3.3V稳压：MIC5219
为IC元器件提供3.3V驱动电压
2）主控芯片：STM32F103C8T6
进行数据处理及运算，最高频率72Mhz.
3）姿态检测：MPU6050
采用IIC总线通讯方式进行数据交互，读取六轴数据（三轴加速度、三轴角速度），根据四元数姿态解算，获取飞行器实际位置（翻滚角、俯仰角和偏航角实际值）。
4）无线传输：NRF24L01
采用SPI通讯方式进行数据交互，接收遥控手柄数据（油门、翻转角、俯仰角和偏航角期望值）。
5）空心杯电机驱动：SI2302
使用定时器输出PWM波实现电机调速。
一、获取飞行器六轴数据：
1）模拟IIC——MPU6050读写数据底层驱动
#include "iic.h"
void iic_gpio_config(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
//GPIO时钟使能
IIC_GPIO_CLK_ENABLE();
//IIC_SCL输入输出模式配置
GPIO_InitStructure.Pin=IIC_SCL_BIN;
GPIO_InitStructure.Mode=GPIO_MODE_OUTPUT_OD;
GPIO_InitStructure.Speed=GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(IIC_SCL_GPIO_PORT,&GPIO_InitStructure);
//IIC_SDA输入输出模式配置
GPIO_InitStructure.Pin=IIC_SDA_PIN;
HAL_GPIO_Init(IIC_SDA_GPIO_PORT,&GPIO_InitStructure);
iic_stop();
}
static void iic_delay (void)
{
uint8_t i;
for(i=10;i<=0;i--);
}
//iic启动信号
void iic_start(void)
{
IIC_SDA_H;
IIC_SCL_H;
iic_delay();
IIC_SDA_L;
iic_delay();
IIC_SCL_L;
iic_delay();
}
//iic停止信号
void iic_stop(void)
{
IIC_SDA_L;
IIC_SCL_H;
iic_delay();
IIC_SDA_H;
iic_delay();
}
//iic发送数据
void iic_SendByte(uint8_t Sendbyte)
{
uint8_t i;
for(i=0;i<8;i++)
{
if(Sendbyte & 0x80)
{
IIC_SDA_H;
}
else
{
IIC_SDA_L;
}
iic_delay();
IIC_SCL_H;
iic_delay();
IIC_SCL_L;
if(i==7)
{
IIC_SDA_H; //发送结束后释放数据总线，输出高阻态
}
Sendbyte<<=1;
iic_delay();
}
}
//iic接收数据
uint8_t iic_ReceiveByte(void)
{
uint8_t Result=0;
uint8_t i;
for(i=0;i<8;i++)
{
Result<<=1;
IIC_SCL_H;
iic_delay();
if(IIC_SDA_READ())
{
Result++;
}
IIC_SCL_L;
iic_delay();
}
return Result;
}
//等待响应
//接收响应成功返回1，失败返回0
uint8_t iic_WaitAck(void)
{
uint8_t value;
IIC_SDA_H;
iic_delay();
IIC_SCL_H;
iic_delay();
if(IIC_SDA_READ())
{
value = 0;
}
else
{
value = 1;
}
IIC_SCL_L;
iic_delay();
return value;
}
//发送Ack信号
void iic_Ack(void)
{
IIC_SDA_L;
iic_delay();
IIC_SCL_H;
iic_delay();
IIC_SCL_L;
iic_delay();
IIC_SDA_H; //释放SDA总线
iic_delay();
}
//发送NAck信号
void iic_NAck(void)
{
IIC_SDA_H;
iic_delay();
IIC_SCL_H;
iic_delay();
IIC_SCL_L;
iic_delay();
}2）MPU6050数据读写：
#include "mpu6050.h"
//写单字节数据
static void iic_mpu6050_write_one_byte(uint8_t slave_adr,
uint8_t reg_adr,
uint8_t data)
{
//发送起始信号
iic_start();
//发送读写设备地址+读写信号
iic_SendByte(slave_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送读写寄存器地址
iic_SendByte(reg_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送数据
iic_SendByte(data);
//等待从站相应
while(iic_WaitAck()==0);
//发送停止信号
iic_stop();
}
//写多字节数据
static void iic_mpu6050_write_bytes(uint8_t slave_adr,
uint8_t reg_adr,
uint8_t* buffer,
uint8_t len)
{
uint8_t i;
//发送起始信号
iic_start();
//发送读写设备地址+读写信号
iic_SendByte(slave_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送读写寄存器地址
iic_SendByte(reg_adr);
//等待从站相应
while(iic_WaitAck()==0);
//循环发送字节
for(i=0;i<len;i++)
{
iic_SendByte(buffer[i]);
while(iic_WaitAck()==0);
}
iic_stop();
}
//读单字节数据
static uint8_t iic_mpu6050_read_one_byte(uint8_t slave_adr,
uint8_t reg_adr)
{
uint8_t result;
//发送起始信号
iic_start();
//发送读写设备地址+读写信号
iic_SendByte(slave_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送读写寄存器地址
iic_SendByte(reg_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送起始信号
iic_start();
//发送读写设备地址+读写信号
iic_SendByte(slave_adr+1);
//等待从站相应
while(iic_WaitAck()==0);
//读取从站数据
result=iic_ReceiveByte();
//发送非相应信号
iic_NAck();
//发送停止信号
iic_stop();
return result;
}
//读多字节数据
static void iic_mpu6050_read_bytes(uint8_t slave_adr,
uint8_t reg_adr,
uint8_t* buffer,
uint8_t len)
{
uint8_t i;
//发送起始信号
iic_start();
//发送读写设备地址+读写信号
iic_SendByte(slave_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送读写寄存器地址
iic_SendByte(reg_adr);
//等待从站相应
while(iic_WaitAck()==0);
//发送起始信号
iic_start();
//发送读写设备地址+读写信号
iic_SendByte(slave_adr+1);
//等待从站相应
while(iic_WaitAck()==0);
//连续读出数据
for(i=0;i<len;i++)
{
buffer[i]=iic_ReceiveByte();
//判断是否是最后一个字节
(i==len-1) ? iic_NAck() : iic_Ack();
}
iic_stop();
}
3）获取六轴数据并进行滤波：
#define Acc_read() iic_mpu6050_read_bytes(MPU6050_ADR,MPU6050_ACC_ADR,buf,6)
#define Gyro_read() iic_mpu6050_read_bytes(MPU6050_ADR,MPU6050_ACC_ADR,&buf[6],6)
//获取MPU6050数据并滤波
void mpu6050_getdate(uint16_t* mpudate)
{
uint8_t i;
uint8_t buf[12];
Acc_read();
Gyro_read();
for(i=0;i<6;i++)
{
mpudate[i]=(((uint16_t)buf[i<<1]<<8) + buf[(i<<1)+1]);
//加速度进行一阶卡尔曼滤波
if(i<3)
{
struct _1_ekf_filter ekf[3]={{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543}};
kalman_1(&ekf[i],(float)mpudate[i]);
mpudate[i]=(uint16_t)ekf[i].out;
}
//角速度进行一阶低通滤波
else
{
uint8_t k=i-3;
const float factor =0.25f;
static float tbuf[3];
mpudate[i]=tbuf[k]=(1-factor)*tbuf[k]+factor*mpudate[i];
}
}
}
//一维卡尔曼滤波算法
void kalman_1(struct _1_ekf_filter *ekf,float input)
{
ekf->Now_P = ekf->LastP + ekf->Q;
ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}
二、进行姿态解算：
采用四元数姿态解获取欧拉角
void GetAngle(const st_mpu *pMpu,st_ange *pAngE, float dt)
{
volatile struct V Gravity,Acc,Gyro,AccGravity;
static float KpDef = 0.5f ;
static float KiDef = 0.0001f;
// static Quaternion NumQ = {1, 0, 0, 0};
float q0_t,q1_t,q2_t,q3_t;
//float NormAcc;
float NormQuat;
float HalfTime = dt * 0.5f;
// 提取等效旋转矩阵中的重力分量
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
// 加速度归一化
NormAcc = 1/sqrt(squa(mpu6050_data.acc_x)+ squa(mpu6050_data.acc_y) +squa(mpu6050_data.acc_z));
Acc.x = pMpu->acc_x * NormAcc;
Acc.y = pMpu->acc_y * NormAcc;
Acc.z = pMpu->acc_z * NormAcc;
//向量差乘得出的值
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
//再做加速度积分补偿角速度的补偿值
GyroIntegError.x += AccGravity.x * KiDef;
GyroIntegError.y += AccGravity.y * KiDef;
GyroIntegError.z += AccGravity.z * KiDef;
//角速度融合加速度积分补偿值
Gyro.x = pMpu->gyro_x * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//弧度制
Gyro.y = pMpu->gyro_y * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
Gyro.z = pMpu->gyro_z * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
// 一阶龙格库塔法, 更新四元数
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
NumQ.q0 += q0_t;
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;
// 四元数归一化
NormQuat = 1/sqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
NumQ.q0 *= NormQuat;
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;
// 四元数转欧拉角
{
#ifdef YAW_GYRO
*(float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
#else
float yaw_G = pMpu->gyro_z * Gyro_G;
if((yaw_G > 1.2f) || (yaw_G < -1.2f))
{
pAngE->yaw += yaw_G * dt;
}
#endif
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA; //PITCH
}
}三、获取手柄控制数据：
1）模拟SPI——NRF24L01读写数据底层驱动
#include "spi.h"
//SPI初始化GPIO配置
void spi_gpio_congig(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
//GPIO时钟使能
SPI_GPIO_CLK_ENABLE();
//输入输出模式配置
GPIO_InitStructure.Pin = SPI_MOSI_PIN | SPI_SCK_PIN | SPI_CSN_PIN ;
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(SPI_GPIO_PORT,&GPIO_InitStructure);
GPIO_InitStructure.Pin = SPI_MISO_PIN ;
GPIO_InitStructure.Mode = GPIO_MODE_INPUT;
GPIO_InitStructure.Pull = GPIO_NOPULL;
HAL_GPIO_Init(SPI_GPIO_PORT,&GPIO_InitStructure);
SPI_SCK_0;
}
//SPI发送、接收1byte
uint8_t spi_wr(uint8_t t_data)
{
SPI_SCK_0;
uint8_t result;
for(uint8_t i=0;i<8;i++)
{
if(t_data&0x80)
{
SPI_MOSI_1;
}
else
{
SPI_MOSI_0;
}
t_data<<=1;
result<<=1;
SPI_SCK_1;
if(SPI_MISO_READ())
{
result++;
}
SPI_SCK_0;
}
return result;
}
//spi写寄存器
void spi_w_reg(uint8_t reg,uint8_t value)
{
SPI_CSN_0;
spi_wr(reg);
spi_wr(value);
SPI_CSN_1;
}
//spi读寄存器
uint8_t spi_r_reg(uint8_t reg)
{
uint8_t result;
SPI_CSN_0;
spi_wr(reg);
result=spi_wr(0XFF);
SPI_CSN_1;
return result;
}
//spi写缓存区
void spi_w_buffer(uint8_t reg,uint8_t* buffer_adr,uint8_t len)
{
uint8_t i;
SPI_CSN_0;
spi_wr(reg);
for(i=0;i<len;i++)
spi_wr(buffer_adr[i]);
SPI_CSN_1;
}
//spi读缓存区
void spi_r_buffer(uint8_t reg,uint8_t* buffer_adr,uint8_t len)
{
uint8_t i;
SPI_CSN_0;
spi_wr(reg);
for(i=0;i<len;i++)
buffer_adr[i]=spi_wr(0xFF);
SPI_CSN_1;
}2）获取遥控数据
#include "remote.h"
uint8_t rc_temp[32];
void RC_Angle(void)
{
static uint16_t cnt_15s=0;
if(nrf_rx_packet(rc_temp)==1)
{
cnt_15s=0;
uint8_t i;
uint8_t checksum;
for(i=0;i<31;i++)
{
checksum += rc_temp[i];
}
if(checksum==rc_temp[31] && rc_temp[0]==0xAA && rc_temp[1]==0xAF)
{
Remote.thr=(uint16_t)rc_temp[4]<<8 | rc_temp[5];
Remote.roll=(uint16_t)rc_temp[6]<<8 | rc_temp[7];
Remote.pitch=(uint16_t)rc_temp[8]<<8 | rc_temp[9];
Remote.yaw=(uint16_t)rc_temp[10]<<8 | rc_temp[11];
{
//处理遥控数据
float roll_pitch_ratio = 0.04f;
pidRoll.desired=-(Remote.roll-1500)*roll_pitch_ratio;
pidPitch.desired=-(Remote.pitch-1500)*roll_pitch_ratio;
if(Remote.yaw>1820)
{
pidYaw.desired -= 0.45f;
}
else if(Remote.yaw<1180)
{
pidYaw.desired += 0.45f;
}
//限制油门不要太大
Remote.thr=LIMIT(Remote.thr,1000,2000);
}
}
}
}
//NRF接收数据包
uint8_t nrf_rx_packet(uint8_t* rxbuffer)
{
uint8_t sta;
NRF_CE_1;
//获取STATUS状态寄存器的值
sta = spi_r_reg(STATUS);
//写状态寄存器，清除中断标志
spi_w_reg(NRF_WRITE_REG+STATUS,sta);
if(sta&RX_OK)
{
//从接收缓冲区读取数据
spi_r_buffer(RD_RX_PLOAD,rxbuffer,0x32);
//清空数据缓冲区
spi_w_reg(FLUSH_RX,0xff);
return 1;
}
return 0;
}四、通过PID调节控制电机：
1）定时器输出PWM波
#include "timer.h"
TIM_HandleTypeDef htimer2;
//定时器2初始化，输出PWM波控制电机,周期500ms
void timer2_config(void)
{
TIM_OC_InitTypeDef TIM_OC_InitStructure;
//定时器时基配置
htimer2.Instance = TIM2;
htimer2.Init.Prescaler = 7199;
htimer2.Init.CounterMode = TIM_COUNTERMODE_UP;
htimer2.Init.Period = 499;
htimer2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
HAL_TIM_Base_Init(&htimer2);
//定时器输出比较模式配置
HAL_TIM_PWM_Init(&htimer2);
TIM_OC_InitStructure.OCMode = TIM_OCMODE_PWM1;
TIM_OC_InitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OC_InitStructure.OCFastMode = TIM_OCFAST_DISABLE;
TIM_OC_InitStructure.Pulse = 250;
HAL_TIM_PWM_ConfigChannel(&htimer2,&TIM_OC_InitStructure,TIM_CHANNEL_2);
//定时器启动
HAL_TIM_Base_Start(&htimer2);
HAL_TIM_PWM_Start(&htimer2,TIM_CHANNEL_2);
}
void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim)
{
if(htim == &htimer2)
{
//定时器时钟使能
TIM2_CLK_ENABLE();
//GPIO外设时钟使能
TIM2_GPIO_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStructure;
//GPIO输出模式配置
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
GPIO_InitStructure.Speed = GPIO_SPEED_LOW;
GPIO_InitStructure.Pin = (TIM2_CH1_PIN | TIM2_CH2_PIN | TIM2_CH3_PIN | TIM2_CH4_PIN);
HAL_GPIO_Init(TIM2_GPIO_PORT,&GPIO_InitStructure);
}
}2）对电机转速进行PID调节
#include "motor.h"
PidObject* (pPidObject[]) = {&pidRateX,&pidRateY,&pidRateZ,&pidRoll,&pidPitch,&pidYaw};
void FlyPidControl(float dt)
{
volatile static uint8_t status=WAITING_1;
switch(status)
{
case WAITING_1 :
if(ALL_flag.unlock)
{
status=READY_11;
}
break;
case READY_11 :
//PID参数复位
pidreset(pPidObject,6);
status=PROCESS_31;
break;
case PROCESS_31:
//三轴角速度实测值
pidRateX.measured=mpu6050_data.gyro_x*Gyro_G;
pidRateY.measured=mpu6050_data.gyro_y*Gyro_G;
pidRateZ.measured=mpu6050_data.gyro_z*Gyro_G;
//欧拉角转换值（实际值）
pidRoll.measured=angle.roll;
pidPitch.measured=angle.pitch;
pidYaw.measured=angle.yaw;
//翻滚角PID调节
Cascadepid(&pidRoll,&pidRateX,dt);
//俯仰角PID调节
Cascadepid(&pidPitch,&pidRateY,dt);
//偏航角PID调节
Cascadepid(&pidYaw,&pidRateZ,dt);
break;
case EXIT_255 :
pidreset(pPidObject,6);
status=WAITING_1;
break;
default:
status=EXIT_255;
break;
}
}
uint16_t motor[4];
uint16_t low_thr_cnt;
void MotorControl(void)
{
volatile static uint8_t status=WAITING_1;
switch (status)
{
case WAITING_1:
if(ALL_flag.unlock)
{
motor[0]=motor[1]=motor[2]=motor[3]=0;
status=WAITING_2;
}
break;
case WAITING_2:
if(Remote.thr>1100)
{
pidreset(pPidObject,6);
low_thr_cnt=0;
status=PROCESS_31;
}
break;
case PROCESS_31:
uint16_t temp;
temp=Remote.thr-1000;
if(temp<10)
{
low_thr_cnt++;
if(low_thr_cnt>500)
{
motor[0]=motor[1]=motor[2]=motor[3]=0;
pidreset(pPidObject,6);
status=WAITING_2;
}
}
low_thr_cnt=0;
motor[0]=motor[1]=motor[2]=motor[3]=LIMIT(temp,0,800);
motor[0]+= + pidRateX.out + pidRateY.out + pidRateZ.out;//; 姿态输出分配给各个电机的控制量
motor[1]+= - pidRateX.out + pidRateY.out - pidRateZ.out ;//;
motor[2]+= + pidRateX.out - pidRateY.out - pidRateZ.out;
motor[3]+= - pidRateX.out - pidRateY.out + pidRateZ.out;//;
break;
case EXIT_255:
motor[0]=motor[1]=motor[2]=motor[3]=LIMIT(temp,0,800);
status=WAITING_1;
break;
default:
break;
}
}五、结尾
整体的小四轴控制思路大概就是以上几步，调控阶段可以增加USB库，使用USB通讯与匿名上位机链接，实时监测小四轴位置数据，调试参数可以增加EEPROM通过IIC通讯存储。
作者：FrankFeng01
