四轴飞行器（STM32）

想要更多项目私wo!!!

一、系统简介

四轴飞行器是一种通过四个旋翼产生的升力实现飞行的无人机，其核心控制原理基于欧拉角动力学模型。四轴飞行器通过改变四个电机的转速来实现六自由度控制（前后、左右、上下移动和俯仰、横滚、偏航旋转）

核心控制原理：

姿态感知：通过MPU6050/MPU9250等惯性测量单元获取飞行器的三轴加速度和三轴角速度数据
姿态解算：使用互补滤波或卡尔曼滤波算法将传感器数据转换为欧拉角（俯仰、横滚、偏航）
控制算法：采用串级PID控制，内环控制角速度，外环控制角度
电机混控：将控制量分配到四个电机，实现稳定飞行

二、系统总体框图

飞控原理图

遥控器原理图

三、部分代码

#include "control.h"
#include "BSP.H"
#include "rc.h"
#include "imu.h"
#include "uart1.h"
PID PID_ROL,PID_PIT,PID_YAW;
u8 ARMED = 0;
extern vs16 QH,ZY,XZ;

float Get_MxMi(float num,float max,float min)
{
        if(num>max)
                return max;
        else if(num<min)
                return min;
        else
                return num;
}

void CONTROL(float rol_now, float pit_now, float yaw_now, float rol_tar, float pit_tar, float yaw_tar)        
{                                                //当前姿态角，，，目标姿态角
        u16 moto1=0,moto2=0,moto3=0,moto4=0;
        vs16 throttle;
//        u8 moto[8];
        
        float rol = rol_tar + rol_now;
        float pit = pit_tar + pit_now;
        float yaw = yaw_tar + yaw_now;
        
        throttle = Rc_Get.THROTTLE - 1000;        //1000<遥控油门值<2000
        if(throttle<0)        throttle=0;
        
        PID_ROL.IMAX = throttle/10;                        //积分限幅，积分值不超过当前油门值的一半
        PID_ROL.IMAX = Get_MxMi(PID_ROL.IMAX,1000,0);        //限制积分结果为，0到1000        
        PID_PIT.IMAX = PID_ROL.IMAX;
        
        PID_ROL.pout = PID_ROL.P * rol;
        PID_PIT.pout = PID_PIT.P * pit;
//、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、        
        /0.1  0.1   30  30
        if(rol_tar*rol_tar<1 && pit_tar*pit_tar<1 && rol_now*rol_now<100 && pit_now*pit_now<100 && throttle>300)
        {                        //防止角度大了，积分超调//目标姿态角水平，姿态角几乎水平，油门值不太低
                PID_ROL.iout += PID_ROL.I * rol;
                PID_PIT.iout += PID_PIT.I * pit;
                PID_ROL.iout = Get_MxMi(PID_ROL.iout,PID_ROL.IMAX,-PID_ROL.IMAX);                //对输出的积分限幅
                PID_PIT.iout = Get_MxMi(PID_PIT.iout,PID_PIT.IMAX,-PID_PIT.IMAX);
        }
        else if(throttle<200)
        {                        //油门值较小时，积分项清零
                PID_ROL.iout = 0;
                PID_PIT.iout = 0;
        }
//、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、        

//        rc_roll_d = rol_tar - getlast_roll;
//        getlast_roll = rol_tar;
//        PID_ROL.dout = PID_ROL.D * (MPU6050_GYRO_LAST.X+rc_roll_d*300);//角速度+控制误差微分
//        
//        rc_pitch_d = pit_tar - getlast_pitch;
//        getlast_pitch = pit_tar;
//        PID_PIT.dout = PID_PIT.D * (MPU6050_GYRO_LAST.Y+rc_pitch_d*300);//角速度+控制误差微分
        
        PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;               
        PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;
/
        PID_YAW.pout = PID_YAW.P * yaw;
        //若 *yaw_now；锁尾模式！！！！
        //若 *yaw；  非锁尾模式！！！！
/
        vs16 yaw_d;
        /
        if(1480>Rc_Get.YAW || Rc_Get.YAW>1520)        //给遥控加死区（偏航角的死区）
        {
                yaw_d = MPU6050_GYRO_LAST.Z + (Rc_Get.YAW-1500)*10;        //用Z轴角速度及目标偏航角值进行四轴运动的预判
                GYRO_I.Z = 0;
        }
        else
                yaw_d =  MPU6050_GYRO_LAST.Z;
        
        PID_YAW.dout = PID_YAW.D * yaw_d;
/        
        PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout + PID_ROL.dout;
        PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
        PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
        
        if(throttle>200)        //油门值大于200才起飞        （遥控油门值大于1200）
        {               
//                moto1 = throttle  - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
//                moto2 = throttle  - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
//                moto3 = throttle  + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
//                moto4 = throttle  + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;

//                moto4 = throttle  - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
//                moto3 = throttle  - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
//                moto2 = throttle  + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
//                moto1 = throttle  + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;               
               
               
//                moto1 = throttle -  25 + QH + ZY - XZ - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
//                moto2 = throttle -  25 + QH - ZY + XZ - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
//                moto3 = throttle -  25 - QH + ZY + XZ + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
//                moto4 = throttle +  75 - QH - ZY - XZ + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
               
                moto4 = throttle + QH + ZY - XZ - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
                moto3 = throttle + QH - ZY + XZ - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
                moto2 = throttle - QH + ZY + XZ + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
                moto1 = throttle - QH - ZY - XZ + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
               
//                moto1 = throttle;
//                moto2 = throttle;
//                moto3 = throttle;
//                moto4 = throttle;
        }
        else
        {
                moto1 = 0;
                moto2 = 0;
                moto3 = 0;
                moto4 = 0;
        }
        
//                moto[1] = moto1 & 0xFF;
//          moto[0] = (moto1>>8) & 0xFF;
//                moto[3] = moto2 & 0xFF;
//          moto[2] = (moto2>>8) & 0xFF;
//                moto[5] = moto3 & 0xFF;
//          moto[4] = (moto3>>8) & 0xFF;
//                moto[7] = moto4 & 0xFF;
//          moto[6] = (moto4>>8) & 0xFF;        
    //Uart1_Send_Buf(moto,8);               
        if(ARMED)        Moto_PwmRflash(moto1,moto2,moto3,moto4);        //未解锁则空心杯不转        ARMED=1则解锁
        else                        Moto_PwmRflash(0,0,0,0);
}