STM32F401VE 四轴飞行器程序框架与核心代码

STM32F401VE四轴飞行器程序框架与核心代码一、程序整体架构STM32F401VE四轴飞行器程序采用模块化设计，分为硬件驱动层、传感器数据处理层、姿态解算层、控制算法层、遥控器通信层和电机驱动层，各模块通过函数调用实现数据交互，确保程序逻辑清晰、可维护性强。c运行//程序核心模块依赖关系硬件驱动层→传感器数据处理层→姿态解算层→控制算法层→电机驱动层↗遥控器通信层↗二、核心模块代码实现1.硬件初始化模块（main.c）负责STM32F401VE芯片外设初始化，包括时钟、串口、定时器、SPI等，为传感器和电机驱动提供硬件支持。c运行#include"stm32f4xx.h"#include"delay.h"#include"mpu6050.h"#include"nrf24l01.h"#include"motor.h"#include"pid.h"intmain(void){//1.系统时钟初始化（168MHz）SystemInit();//2.延时函数初始化delay_init(168);//3.串口初始化（用于调试，波特率115200）uart_init(115200);//4.定时器初始化（用于PWM输出，控制电机）TIM3_PWM_Init(8399,0);//分频8400，频率20kHz//5.传感器初始化（MPU6050陀螺仪+加速度计）MPU6050_Init();//6.遥控器通信初始化（NRF24L01无线模块）NRF24L01_Init();//7.PID控制器初始化PID_Init();//8.电机初始化Motor_Init();while(1){//传感器数据读取与处理Sensor_Data_Update();//姿态解算（四元数法）Attitude_Solve();//遥控器数据接收Remote_Control_Receive();//飞行控制算法Flight_Control();//电机转速更新Motor_Update();//延时控制循环频率delay_ms(5);}}2.传感器数据处理模块（sensor.c）读取MPU6050的加速度计和陀螺仪数据，进行滤波处理，去除噪声干扰，为姿态解算提供稳定数据。c运行#include"sensor.h"#include"mpu6050.h"#include"math.h"//传感器原始数据int16_taccel_x,accel_y,accel_z;int16_tgyro_x,gyro_y,gyro_z;//滤波后数据floataccel_x_filt,accel_y_filt,accel_z_filt;floatgyro_x_filt,gyro_y_filt,gyro_z_filt;//一阶低通滤波系数#defineFILTER_K0.2voidSensor_Data_Update(void){//读取MPU6050原始数据MPU6050_Read_Data(&accel_x,&accel_y,&accel_z,&gyro_x,&gyro_y,&gyro_z);//单位转换：原始数据→实际物理量floatax=accel_x/16384.0f;//加速度计量程±2g，灵敏度16384LSB/gfloatay=accel_y/16384.0f;floataz=accel_z/16384.0f;floatgx=gyro_x/131.0f;//陀螺仪量程±250°/s，灵敏度131LSB/(°/s)floatgy=gyro_y/131.0f;floatgz=gyro_z/131.0f;//一阶低通滤波，去除高频噪声accel_x_filt=accel_x_filt*(1-FILTER_K)+ax*FILTER_K;accel_y_filt=accel_y_filt*(1-FILTER_K)+ay*FILTER_K;accel_z_filt=accel_z_filt*(1-FILTER_K)+az*FILTER_K;gyro_x_filt=gyro_x_filt*(1-FILTER_K)+gx*FILTER_K;gyro_y_filt=gyro_y_filt*(1-FILTER_K)+gy*FILTER_K;gyro_z_filt=gyro_z_filt*(1-FILTER_K)+gz*FILTER_K;}3.姿态解算模块（attitude.c）采用四元数法融合加速度计和陀螺仪数据，计算飞行器的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw），避免欧拉角的万向锁问题。c运行#include"attitude.h"#include"sensor.h"#include"math.h"//四元数floatq0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//姿态角（单位：°）floatroll,pitch,yaw;//陀螺仪积分时间（与主循环频率一致，5ms=0.005s）#defineDT0.005f//四元数更新函数voidAttitude_Solve(void){floatgx=gyro_x_filt*PI/180.0f;//转换为弧度floatgy=gyro_y_filt*PI/180.0f;floatgz=gyro_z_filt*PI/180.0f;//四元数微分方程floatq0_dot=0.5f*(-q1*gx-q2*gy-q3*gz);floatq1_dot=0.5f*(q0*gx+q2*gz-q3*gy);floatq2_dot=0.5f*(q0*gy-q1*gz+q3*gx);floatq3_dot=0.5f*(q0*gz+q1*gy-q2*gx);//积分更新四元数q0+=q0_dot*DT;q1+=q1_dot*DT;q2+=q2_dot*DT;q3+=q3_dot*DT;//四元数归一化，避免漂移floatnorm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0/=norm;q1/=norm;q2/=norm;q3/=norm;//四元数→欧拉角转换roll=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))*180.0f/PI;pitch=asin(2*(q0*q2-q3*q1))*180.0f/PI;yaw=atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))*180.0f/PI;}4.PID控制模块（pid.c）实现串级PID控制，内环控制姿态角速率，外环控制姿态角，确保飞行器姿态稳定，响应快速。c运行#include"pid.h"#include"attitude.h"//PID参数结构体typedefstruct{floatkp;//比例系数floatki;//积分系数floatkd;//微分系数floaterror;//当前误差floatlast_error;//上一次误差floatintegral;//积分项floatoutput;//输出}PID_TypeDef;//姿态角PID（外环）PID_TypeDefpid_roll,pid_pitch,pid_yaw;//姿态角速率PID（内环）PID_TypeDefpid_roll_rate,pid_pitch_rate,pid_yaw_rate;//PID初始化voidPID_Init(void){//姿态角PID参数pid_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;//姿态角速率PID参数pid_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;}//PID计算函数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;}5.飞行控制与电机驱动模块（flight_control.c）根据遥控器指令和姿态解算结果，通过PID控制器计算电机输出，实现飞行器的升降、前后、左右和偏航控制。c运行#include"flight_control.h"#include"pid.h"#include"motor.h"#include"remote.h"//遥控器通道值（0-1000，500为中值）uint16_tremote_throttle,remote_roll,remote_pitch,remote_yaw;//电机输出值（1000-2000，对应PWM占空比）uint16_tmotor1,motor2,motor3,motor4;voidFlight_Control(void){//外环PID：姿态角控制（遥控器指令→目标姿态角）floatroll_rate_set=PID_Calculate(&pid_roll,remote_roll-500,roll);floatpitch_rate_set=PID_Calculate(&pid_pitch,remote_pitch-500,pitch);floatyaw_rate_set=PID_Calculate(&pid_yaw,remote_yaw-500,yaw);//内环PID：姿态角速率控制（目标姿态角→电机输出）floatroll_output=PID_Calculate(&pid_roll_rate,roll_rate_set,gyro_x_filt);floatpitch_output=PID_Calculate(&pid_pitch_rate,pitch_rate_set,gyro_y_filt);floatyaw_output=PID_Calculate(&pid_yaw_rate,yaw_rate_set,gyro_z_filt);//电机混合计算（X型四轴布局）motor1=remote_throttle+pitch_output-roll_output+yaw_output;motor2=remote_throttle-pitch_output-roll_output-yaw_output;motor3=remote_throttle-pitch_output+roll_output+yaw_output;motor4=remote_throttle+pitch_output+roll_output-yaw_output;//电机输出限幅，避免超出安全范围motor1=motor1<1000?1000:(motor1>2000?2000:motor1);motor2=motor2<1000?1000:(motor2>2000?2000:motor2);motor3=motor3<1000?1000:(motor3>2000?2000:motor3);motor4=motor4<1000?1000:(motor4>2000?2000:motor4);}//电机PWM输出更新voidMotor_Update(void){TIM_SetCompare1(TIM3,motor1);TIM_SetCompare2(TIM3,motor2);TIM_SetCompare3(TIM3,motor3);TIM_SetCompare4(TIM3,motor4);}6.