STM32F401VE 四轴飞行器完整程序（含驱动与注释）

STM32F401VE四轴飞行器完整程序（含驱动与注释）一、头文件汇总（common.h）c运行#ifndef__COMMON_H#define__COMMON_H#include"stm32f4xx.h"#include"math.h"//系统时钟频率#defineSYSCLK_FREQ168000000UL//数学常量#definePI3.14159265358979323846f#defineRAD_TO_DEG(180.0f/PI)#defineDEG_TO_RAD(PI/180.0f)//主循环周期（5ms）#defineDT0.005f//函数声明voidSystem_Init(void);voidDelay_ms(uint32_tms);#endif二、MPU6050驱动（mpu6050.c/h）1.头文件（mpu6050.h）c运行#ifndef__MPU6050_H#define__MPU6050_H#include"common.h"//MPU6050寄存器地址#defineMPU6050_ADDR0xD0#defineMPU6050_PWR_MGMT_10x6B#defineMPU6050_SMPLRT_DIV0x19#defineMPU6050_CONFIG0x1A#defineMPU6050_GYRO_CONFIG0x1B#defineMPU6050_ACCEL_CONFIG0x1C#defineMPU6050_ACCEL_XOUT_H0x3B#defineMPU6050_GYRO_XOUT_H0x43//函数声明voidMPU6050_Init(void);voidMPU6050_Read_Data(int16_t*accel_x,int16_t*accel_y,int16_t*accel_z,int16_t*gyro_x,int16_t*gyro_y,int16_t*gyro_z);#endif2.驱动实现（mpu6050.c）c运行#include"mpu6050.h"#include"i2c.h"voidMPU6050_Init(void){//唤醒MPU6050I2C_Start();I2C_Send_Byte(MPU6050_ADDR);I2C_Wait_Ack();I2C_Send_Byte(MPU6050_PWR_MGMT_1);I2C_Wait_Ack();I2C_Send_Byte(0x00);I2C_Wait_Ack();I2C_Stop();//设置采样率分频器（1000Hz采样率）I2C_Start();I2C_Send_Byte(MPU6050_ADDR);I2C_Wait_Ack();I2C_Send_Byte(MPU6050_SMPLRT_DIV);I2C_Wait_Ack();I2C_Send_Byte(0x07);I2C_Wait_Ack();I2C_Stop();//配置陀螺仪量程±250°/sI2C_Start();I2C_Send_Byte(MPU6050_ADDR);I2C_Wait_Ack();I2C_Send_Byte(MPU6050_GYRO_CONFIG);I2C_Wait_Ack();I2C_Send_Byte(0x00);I2C_Wait_Ack();I2C_Stop();//配置加速度计量程±2gI2C_Start();I2C_Send_Byte(MPU6050_ADDR);I2C_Wait_Ack();I2C_Send_Byte(MPU6050_ACCEL_CONFIG);I2C_Wait_Ack();I2C_Send_Byte(0x00);I2C_Wait_Ack();I2C_Stop();}voidMPU6050_Read_Data(int16_t*accel_x,int16_t*accel_y,int16_t*accel_z,int16_t*gyro_x,int16_t*gyro_y,int16_t*gyro_z){uint8_tbuf[14];I2C_Start();I2C_Send_Byte(MPU6050_ADDR);I2C_Wait_Ack();I2C_Send_Byte(MPU6050_ACCEL_XOUT_H);I2C_Wait_Ack();I2C_Start();I2C_Send_Byte(MPU6050_ADDR|0x01);I2C_Wait_Ack();for(uint8_ti=0;i<14;i++){buf[i]=I2C_Receive_Byte();if(i<13)I2C_Send_Ack();elseI2C_Send_Nack();}I2C_Stop();*accel_x=(buf[0]<<8)|buf[1];*accel_y=(buf[2]<<8)|buf[3];*accel_z=(buf[4]<<8)|buf[5];*gyro_x=(buf[8]<<8)|buf[9];*gyro_y=(buf[10]<<8)|buf[11];*gyro_z=(buf[12]<<8)|buf[13];}三、NRF24L01驱动（nrf24l01.c/h）1.头文件（nrf24l01.h）c运行#ifndef__NRF24L01_H#define__NRF24L01_H#include"common.h"//NRF24L01寄存器地址#defineNRF24L01_CONFIG0x00#defineNRF24L01_EN_AA0x01#defineNRF24L01_EN_RXADDR0x02#defineNRF24L01_SETUP_AW0x03#defineNRF24L01_SETUP_RETR0x04#defineNRF24L01_RF_CH0x05#defineNRF24L01_RF_SETUP0x06#defineNRF24L01_STATUS0x07#defineNRF24L01_RX_ADDR_P00x0A#defineNRF24L01_TX_ADDR0x10#defineNRF24L01_RX_PW_P00x11#defineNRF24L01_FIFO_STATUS0x17//函数声明voidNRF24L01_Init(void);uint8_tNRF24L01_RxPacket(uint8_t*buf);voidNRF24L01_TxPacket(uint8_t*buf,uint8_tlen);#endif2.驱动实现（nrf24l01.c）c运行#include"nrf24l01.h"#include"spi.h"#include"gpio.h"//NRF24L01片选引脚#defineNRF_CE_HIGHGPIO_SetBits(GPIOA,GPIO_Pin_4)#defineNRF_CE_LOWGPIO_ResetBits(GPIOA,GPIO_Pin_4)#defineNRF_CS_HIGHGPIO_SetBits(GPIOA,GPIO_Pin_5)#defineNRF_CS_LOWGPIO_ResetBits(GPIOA,GPIO_Pin_5)voidNRF24L01_Init(void){NRF_CS_HIGH;NRF_CE_LOW;//配置寄存器SPI_Write_Byte(NRF24L01_CONFIG,0x0F);//使能CRC，16位CRC，接收模式SPI_Write_Byte(NRF24L01_EN_AA,0x01);//使能通道0自动应答SPI_Write_Byte(NRF24L01_EN_RXADDR,0x01);//使能通道0接收地址SPI_Write_Byte(NRF24L01_SETUP_AW,0x03);//地址宽度5字节SPI_Write_Byte(NRF24L01_SETUP_RETR,0x1A);//自动重传10次，间隔250usSPI_Write_Byte(NRF24L01_RF_CH,0x02);//射频通道2SPI_Write_Byte(NRF24L01_RF_SETUP,0x07);//1Mbps，0dBm功率SPI_Write_Byte(NRF24L01_RX_PW_P0,0x08);//通道0接收数据长度8字节//设置接收地址uint8_trx_addr[5]={0x34,0x43,0x10,0x10,0x01};SPI_Write_Buf(NRF24L01_RX_ADDR_P0,rx_addr,5);SPI_Write_Buf(NRF24L01_TX_ADDR,rx_addr,5);NRF_CE_HIGH;}uint8_tNRF24L01_RxPacket(uint8_t*buf){uint8_tstatus=SPI_Read_Byte(NRF24L01_STATUS);if(status&0x40){SPI_Read_Buf(0x61,buf,8);SPI_Write_Byte(NRF24L01_STATUS,0x40);return1;}return0;}四、PID控制模块（pid.c/h）1.头文件（pid.h）c运行#ifndef__PID_H#define__PID_H#include"common.h"typedefstruct{floatkp;floatki;floatkd;floaterror;floatlast_error;floatintegral;floatoutput;}PID_TypeDef;externPID_TypeDefpid_roll,pid_pitch,pid_yaw;externPID_TypeDefpid_roll_rate,pid_pitch_rate,pid_yaw_rate;voidPID_Init(void);floatPID_Calculate(PID_TypeDef*pid,floatsetpoint,floatactual);#endif2.控制实现（pid.c）c运行#include"pid.h"PID_TypeDefpid_roll,pid_pitch,pid_yaw;PID_TypeDefpid_roll_rate,pid_pitch_rate,pid_yaw_rate;voidPID_Init(void){//姿态角PIDpid_roll.kp=2.0f;pid_roll.ki=0.1f;pid_roll.kd=0.05f;pid_pitch.kp=2.0f;pid_pitch.ki=0.1f;pid_pitch.kd=0.05f;pid_yaw.kp=1.5f;pid_yaw.ki=0.05f;pid_yaw.kd=0.02f;//姿态角速率PIDpid_roll_rate.kp=1.0f;pid_roll_rate.ki=0.05f;pid_roll_rate.kd=0.01f;pid_pitch_rate.kp=1.0f;pid_pitch_rate.ki=0.05f;pid_pitch_rate.kd=0.01f;pid_yaw_rate.kp=0.8f;pid_yaw_rate.ki=0.03f;pid_yaw_rate.kd=0.01f;}floatPID_Calculate(PID_TypeDef*pid,floatsetpoint,floatactual){pid->error=setpoint-actual;pid->integral+=pid->error*DT;//积分限幅if(pid->integral>500)pid->integral=500;if(pid->integral<-500)pid->integral=-500;pid->output=pid->kp*pid->error+pid->ki*pid->integral+pid->kd*(pid->error-pid->last_error)/DT;pid->last_error=pid->error;returnpid->output;}五、电机驱动模块（motor.c/h）1.头文件（motor.h）c运行#ifndef__MOTOR_H#define__MOTOR_H#include"common.h"//电机输出范围#defineMIN_THROTTLE1000#defineMAX_THROTTLE2000externuint16_tmotor1,motor2,motor3,motor4;voidMotor_Init(void);voidMotor_Update(void);#endif2.驱动实现（motor.c）c运行#include"motor.h"#include"tim.h"uint16_tmotor1,motor2,motor3,motor4;voidMotor_Init(void){//TIM3PWM初始化（CH1-CH4对应4个电机）TIM_TimeBaseInitTypeDefTIM_TimeBaseInitStruct;TIM_OCInitTypeDefTIM_OCInitStruct;RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);TIM_TimeBaseInitStruct.TIM_Prescaler=83;TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;TIM_TimeBaseInitStruct.TIM_Period=19999;TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;TIM_OCInitStruct.TIM_Pulse=1000;TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;TIM_OC1Init(TIM3,&TIM_OCInitStruct);TIM_OC2Init(TIM3,&TIM_OCInitStruct);TIM_OC3Init(TIM3,&TIM_OCInitStruct);TIM_OC4Init(TIM3,&TIM_OCInitStruct);TIM_Cmd(TIM3,ENABLE);TIM_CtrlPWMOutputs(TIM3,ENABLE);}voidMotor_Update(void){TIM_SetCompare1(TIM3,motor1);TIM_SetCompare2(TIM3,motor2);TIM_SetCompare3(TIM3,motor3);TIM_SetCompare4(TIM3,motor4);}六、主程序（main.c）c运行#include"common.h"#include"mpu6050.h"#include"nrf24l01.h"#include"pid.h"#include"motor.h"#include"sensor.h"#include"attitude.h"#include"remote.h"#include"flight_control.h"intmain(void){System_Init();Delay_ms(100);//外设初始化MPU6050_Init();NRF24L01_Init();PID_Init();Motor_Init();while(1){//传感器数据更新Sensor_Data_Update();//姿态解算Attitude_Solve();//遥控器数据接收Remote_Control_Receive();//飞行控制计算Flight_Control();//电机输出更新Motor_Update();Delay_ms(5);}}voidSystem_Init(void){//系统时钟初始化RCC_DeInit();RCC_HSEConfig(RCC_HSE_ON);while(RCC_GetFlagStatus(RCC_FLAG_HSERDY)==RESET);RCC_PLLConfig(RCC_PLLSource_HSE,8,336,2,7);