最近再弄创新无人机项目, 半期前终于把硬件配套弄好了，最近开始弄飞控了。由于我是第一次接触这一块，也只能看着网上的公开的算法，自己弄明白后写写欸。最近写了获取姿态角的代码，献丑了 ~

imu.c

#include "mymath.h"
#include "imu.h"
#include <math.h>
//绕X轴旋转角度为roll，绕Y轴旋转角度为pitch，绕Z轴旋转角度为yaw
//对于角速度进行PID控制
const float EA_kpf=0.8;// 角速度融合加速度比例补偿值系数
static Quaternion NumQ = {1, 0, 0, 0};//初始四元数
void imu_Getangle(T_gyro gyro,T_Acc Acc,T_angle *angle,float dt)
{
float HalfTime = dt * 0.5f;
float q0_t,q1_t,q2_t,q3_t;
	struct V{
        float x;
        float y;
        float z;
        } Gravity,EA;
    //加速度计进行姿态解算
//float A_roll=arctan(Acc.Ay/Acc.Az);
//float A_pitch=-arctan(Acc.Ax*Q_rsqrt((Acc.Ay)*(Acc.Ay)+(Acc.Az)*(Acc.Az)));
float R_Gx=Rad(gyro.Gx),R_Gy=Rad(gyro.Gy),R_Gz=Rad(gyro.Gz);
//加速度归一化
float recipNorm= Q_rsqrt((Acc.Ay)*(Acc.Ay)+(Acc.Az)*(Acc.Az)+(Acc.Ax)*(Acc.Ax));
Acc.Az*=recipNorm;
Acc.Ay*=recipNorm;
Acc.Ax*=recipNorm;
//提取重力分量
 Gravity.x=2*(NumQ.q1*NumQ.q3-NumQ.q0*NumQ.q2) ;
Gravity.y=2*(NumQ.q0*NumQ.q1+NumQ.q2*NumQ.q3);
Gravity.z=1-2*NumQ.q1*NumQ.q1-2*NumQ.q2*NumQ.q2;
//求姿态误差
EA.x=(Acc.Ay*Gravity.z-Acc.Az*Gravity.y);
EA.y=(Acc.Az*Gravity.x-Acc.Ax*Gravity.z);
EA.z=(Acc.Ax*Gravity.y-Acc.Ay*Gravity.x);
//角速度融合加速度比例补偿值得到矫正后的角速度值
 R_Gx=R_Gx+EA.x*EA_kpf;
 R_Gy=R_Gy+EA.y*EA_kpf;
R_Gz=R_Gz+EA.z*EA_kpf;
//一阶龙格库塔法, 更新四元数
 q0_t = (-NumQ.q1 * R_Gx - NumQ.q2 * R_Gy - NumQ.q3 * R_Gz) * HalfTime;
    q1_t = ( NumQ.q0 * R_Gx - NumQ.q3 * R_Gy + NumQ.q2 * R_Gz) * HalfTime;
    q2_t = ( NumQ.q3 * R_Gx + NumQ.q0 * R_Gy - NumQ.q1 * R_Gz) * HalfTime;
    q3_t = (-NumQ.q2 * R_Gx + NumQ.q1 * R_Gy + NumQ.q0 * R_Gz) * HalfTime;
    NumQ.q0 += q0_t;
    NumQ.q1 += q1_t;
    NumQ.q2 += q2_t;
    NumQ.q3 += q3_t;	
 //四元数归一化
 recipNorm= Q_rsqrt((NumQ.q0)*(NumQ.q0)+(NumQ.q1)*(NumQ.q1)+(NumQ.q2)*(NumQ.q2)+(NumQ.q3)*(NumQ.q3));
 NumQ.q0*=recipNorm;
 NumQ.q1*=recipNorm;
 NumQ.q2*=recipNorm;
 NumQ.q3*=recipNorm;
 //计算姿态角
 float g1=2.0f*(NumQ.q1*NumQ.q3-NumQ.q0*NumQ.q2);
 float g2=2.0f*(NumQ.q2*NumQ.q3+NumQ.q0*NumQ.q1);
 float g3=NumQ.q0*NumQ.q0-NumQ.q1*NumQ.q1-NumQ.q2*NumQ.q2+NumQ.q3*NumQ.q3;
 float g4=2.0f*(NumQ.q1*NumQ.q2+NumQ.q0*NumQ.q3);
 float g5=NumQ.q0*NumQ.q0+NumQ.q1*NumQ.q1-NumQ.q2*NumQ.q2-NumQ.q3*NumQ.q3;
angle->pitch=-AT(arcsin(g1));
angle->roll=AT(arctan(g2/g3));
angle->yaw=AT(atan2f(g4,g5));
}

imu.h

#ifndef  __IMU_H__
#define __IMU_H__
typedef volatile struct 
{
  float q0;
  float q1;
  float q2;
  float q3;
} Quaternion;
typedef struct 
{
float Ax;
float Ay;
float Az;
}T_Acc;

typedef struct 
{
float Gx;
float Gy;
float Gz;
}T_gyro;

typedef struct 
{
float roll;
float yaw;
float pitch;
}T_angle;

void imu_Getangle(T_gyro gyro,T_Acc Acc,T_angle *angle,float dt);
#endif

等期末结束后再专心写飞控吧，之后测试完毕后我会把所有代码分享出来！