单片机设计_四轴飞行器(STM32)

四轴飞行器(STM32)

想要更多项目私wo!!!

一、系统简介

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

核心控制原理:

姿态感知:通过MPU6050/MPU9250等惯性测量单元获取飞行器的三轴加速度和三轴角速度数据

姿态解算:使用互补滤波或卡尔曼滤波算法将传感器数据转换为欧拉角(俯仰、横滚、偏航)

控制算法:采用串级PID控制,内环控制角速度,外环控制角度

电机混控:将控制量分配到四个电机,实现稳定飞行

二、系统总体框图

飞控原理图

遥控器原理图

三、部分代码

cpp 复制代码
#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);
}
相关推荐
智者知已应修善业31 分钟前
【51单片机用数码管显示流水灯的种类是按钮控制数码管加一和流水灯】2022-6-14
c语言·经验分享·笔记·单片机·嵌入式硬件·51单片机
智商偏低7 小时前
单片机之helloworld
单片机·嵌入式硬件
青牛科技-Allen8 小时前
GC3910S:一款高性能双通道直流电机驱动芯片
stm32·单片机·嵌入式硬件·机器人·医疗器械·水泵、
森焱森10 小时前
无人机三轴稳定控制(2)____根据目标俯仰角,实现俯仰稳定化控制,计算出升降舵输出
c语言·单片机·算法·架构·无人机
白鱼不小白10 小时前
stm32 USART串口协议与外设(程序)——江协教程踩坑经验分享
stm32·单片机·嵌入式硬件
S,D11 小时前
MCU引脚的漏电流、灌电流、拉电流区别是什么
驱动开发·stm32·单片机·嵌入式硬件·mcu·物联网·硬件工程
芯岭技术14 小时前
PY32F002A单片机 低成本控制器解决方案,提供多种封装
单片机·嵌入式硬件
youmdt14 小时前
Arduino IDE ESP8266连接0.96寸SSD1306 IIC单色屏显示北京时间
单片机·嵌入式硬件
嘿·嘘14 小时前
第七章 STM32内部FLASH读写
stm32·单片机·嵌入式硬件
Meraki.Zhang14 小时前
【STM32实践篇】:I2C驱动编写
stm32·单片机·iic·驱动·i2c