【免费开源】基于 STM32F4 的四轴飞行器设计与实现——从零开始到成功起飞（项目源码打包分享）

angle = α * (angle + gyro * dt) + (1 - α) * acc_angle;

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
  float norm;
  float vx, vy, vz;
  float ex, ey, ez;

  // 先把这些用得到的值算好
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
// float q0q3 = q0*q3;
  float q1q1 = q1*q1;
// float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;
	
	if(ax*ay*az==0)
 		return;
		
  norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
  ax = ax /norm;
  ay = ay / norm;
  az = az / norm;

  // estimated direction of gravity and flux (v and w)  估计重力方向和流量/变迁
  vx = 2*(q1q3 - q0q2);							//四元素中xyz的表示
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  // error is sum of cross product between reference direction of fields and direction measured by sensors
  ex = (ay*vz - az*vy) ;   			 //向量外积在相减得到差分就是误差
  ey = (az*vx - ax*vz) ;
  ez = (ax*vy - ay*vx) ;

  exInt = exInt + ex * Ki;		 //对误差进行积分
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;  	//将误差PI后补偿到陀螺仪，即补偿零点漂移
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

  // integrate quaternion rate and normalise			//四元素的微分方程
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + ( q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + ( q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + ( q0*gz + q1*gy - q2*gx)*halfT;

  // normalise quaternion
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  Q_ANGLE.Z = GYRO_I.Z;//atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
  Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

b)PID控制部分源码：
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;

    float rol = rol_tar + rol_now;
    float pit = pit_tar + pit_now;
    float yaw = yaw_tar + yaw_now;

    throttle = Rc_Get.THROTTLE - 1000;//油门 节流阀
    if (throttle < 0)  throttle = 0;

    PID_ROL.IMAX = throttle / 2;
    Get_MxMi(PID_ROL.IMAX, 1000, 0);
    PID_PIT.IMAX = PID_ROL.IMAX;

    PID_ROL.pout = PID_ROL.P * rol;
    PID_PIT.pout = PID_PIT.P * pit;

    if (rol_tar * rol_tar < 0.1 && pit_tar * pit_tar < 0.1 && rol_now * rol_now < 30 && pit_now * pit_now < 30 && 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;
    }

    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;
    vs16 yaw_d;
    if (1480 > Rc_Get.YAW || Rc_Get.YAW > 1520)
    {
        yaw_d = MPU6050_GYRO_LAST.Z + (Rc_Get.YAW - 1500) * 10;
        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;

		/*
		rol 翻滚
		pat 俯仰角
		yaw //偏航
		*/
    if (throttle > 200)
    {
			#define YAW_ADD (-30)
        moto1 = throttle + PID_ROL.OUT - PID_PIT.OUT +YAW_ADD+ PID_YAW.OUT/**/;
        moto2 = throttle - PID_ROL.OUT - PID_PIT.OUT -YAW_ADD- PID_YAW.OUT/**/;
        moto3 = throttle - PID_ROL.OUT + PID_PIT.OUT +YAW_ADD+ PID_YAW.OUT/**/;
        moto4 = throttle + PID_ROL.OUT + PID_PIT.OUT -YAW_ADD- PID_YAW.OUT/**/;
    }
    else
    {
        moto1 = 0;
        moto2 = 0;
        moto3 = 0;
        moto4 = 0;
    }
    if (ARMED)   Moto_PwmRflash(moto1, moto2, moto3, moto4);
    else         Moto_PwmRflash(0, 0, 0, 0);
}

typedef struct {
    float roll_target;
    float pitch_target;
    float yaw_target;
    float throttle;
} RC_Command;