基于msp430f5529驱动mpu6050，通过原始数据求出欧拉角

求一份使用msp430f5529驱动mpu6050得到欧拉角的代码，使用官方dmp库的。已经通过i2c通信拿到mpu6050的6个原始数据了，将原始数据转换成欧拉角的代码也行

Read_MPU6050(tp);    //**读取6060,6个数据在tp里,这个是个例子,到时候根据你6个数据进行替换**

Angle_ax = ((float)(((int *)&tp)[0])) / 4096.0;    //加速度处理    结果单位是 +- g
Angle_ay = ((float)(((int *)&tp)[1])) / 4096.0;    //转换关系    8192 LSB/g, 1g对应读数8192
Angle_az = ((float)(((int *)&tp)[2])) / 4096.0;    //加速度量程 +-4g/S
Angle_gx = ((float)(((int *)&tp)[4])) / 65.5;    //陀螺仪处理    结果单位是 +-度
Angle_gy = ((float)(((int *)&tp)[5])) / 65.5;    //陀螺仪量程 +-500度/S, 1度/秒 对应读数 65.536
Angle_gz = ((float)(((int *)&tp)[6])) / 65.5;    //转换关系65.5 LSB/度

IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);//前面加速度乘以0.0174533f是为了转换为重力加速度

void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az)
{
    const float Kp = 3.5, Ki = 0.05;
    static float exInt=0, eyInt=0, ezInt=0;
    float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
    float haftT=0.004f ;
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q1q1 = q1*q1;
    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); 
    ax = ax / norm; 
    ay = ay / norm; 
    az = az / norm; 

    // 第二步：DCM矩阵旋转
    vx = 2*(q1q3 - q0q2); 
    vy = 2*(q0q1 + q2q3); 
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    // 第三步：在机体坐标系下做向量叉积得到补偿数据
    ex = ay*vz - az*vy ;
    ey = az*vx - ax*vz ;
    ez = ax*vy - ay*vx ;

    // 第四步：对误差进行PI计算，补偿角速度
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;

    // 第五步：按照四元数微分公式进行四元数更新
    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;

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

    roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;     
    pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
    yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
}