EEPW论坛

四轴的ROLL，PITCH，YAW的部分算法程序；

//二阶毕卡法
void IMUupdate(float* Roll, float* Pitch, float* Yaw,
                float gx, float gy, float gz, 
                float ax, float ay, float az, float* fusionDt)
{
#ifdef IMUMpu_Set

    float delta_2=0;
//    float gx, gy, gz, ax, ay, az;
//    float Fgx, Fgy, Fgz; // estimated gravity direction
    float norm = 0.0f;
//    float halfT;
    float vx, vy, vz;
    float ex, ey, ez;
    const static float FACTOR = 0.002;
// 先把这些用得到的值算好
    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;

//    getInitMpu(&gx, &gy, &gz, &ax, &ay, &az);

//    gx = Gyrox;
//    gy = Gyroy;
//    gz = Gyroz;
//    ax = Accelx;
//    ay = Accely;
//    az = Accelz;

    /*归一化测量值，加速度计和磁力计的单位是什么都无所谓，因为它们在此被作了归一化处理*/        
    //normalise the measurements
//    norm = invSqrt(ax*ax + ay*ay + az*az);   
    norm = sqrt(ax*ax + ay*ay + az*az);
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;

    vx = 2*(q1q3 - q0q2);
    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);

/*
    // integral error scaled integral gain
    exInt = exInt + ex*Ki;
    eyInt = eyInt + ey*Ki;
    ezInt = ezInt + ez*Ki;

    // adjusted gyroscope measurements
    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;
*/

//    halfT = SysTickUser / 2000;
//    *fusionDt = SysTickUser / 1000;
    *fusionDt = 0.002;

    gx = gx + ex*FACTOR/halfT;                     //校正陀螺仪测量值       用叉积误差来做PI修正陀螺零偏                            
    gy = gy + ey*FACTOR/halfT; 
    gz = gz + ez*FACTOR/halfT;     

    delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);    

    q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;            // 整合四元数率     四元数微分方程    四元数更新算法，二阶毕卡法
    q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT;             

/*
    // 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 = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / norm;       //w
    q1 = q1 / norm;       //x
    q2 = q2 / norm;       //y
    q3 = q3 / norm;       //z

#endif

//    由四元数计算出Pitch  Roll  Yaw,乘以57.3是为了将弧度转化为角度，陀螺仪x轴为前进方向
//    *Pitch = asin(2 * q2 * q3 + 2 * q0 * q1) * 57.295780; //俯仰角，绕x轴转动     
//    *Roll  = -atan2(2 * q1 * q3 - 2 * q0 * q2, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.295780; //滚动角，绕y轴转动
//    *Yaw   = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.295780;  //偏航角，绕z轴转动

    *Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)*57.295780; // pitch
    *Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.295780; // roll         
    *Yaw   = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1)*57.295780;  //偏航角，绕z轴转动
//    *Yaw = 0;

    if(*Yaw < -180 ){*Yaw = *Yaw + 360;}
    if(*Yaw > 180 ){*Yaw = *Yaw - 360;}

}