在静止状态下根据IMU(加速度计)计算姿态角

   一、 姿态角定义：

   1. 滚转角的定义:机体Z轴与过机体x轴的垂面的夹角。向右滚转为正，范围【-180deg-180deg】。

    2.俯仰角的定义:机体x轴与水平面的夹角。向上为正，范围【-90deg-90deg】。

   二、根据重力加速度 计算姿态角的原理:

    1.滚转角计算，首先通过机体x轴与重力加速度的外积，计算出“过机体x轴的垂面”的法向量。然后通过计算内积的方法，求该法向量与机体z轴的夹角，这个夹角与滚转角互余。

    2.俯仰角计算:通过计算内积的方法，求机体x轴与重力加速度的夹角，这个夹角与俯仰角互余。

    三、代码实现：

    本人亲自测试，计算结果合格。使用方法是调用makekam_gacc_to_att函数，第一个参数是三轴重力加速度，第二和第三个参数是计算出的滚转俯仰角。

extern void makekam_gacc_to_att(float gravity_acc[3]/*输入静止状态下的机体系加速度*/,float *ret_roll/*计算结果*/,float *ret_pitch)；

/*计算向量的长度，用于判定pitch为90deg时的特殊情况*/
float get_vector_mod(const float vector[3])
{
    return sqrtf(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
}

/*通过内积法，求两个向量的夹角*/
float makekam_vec_ang(const float vec_a[3],const float vec_b[3])/*return 0~180deg*/
{
    float angle = 0.0f;
    float mod_of_vec_a = 0.0f;
    float mod_of_vec_b = 0.0f;
    mod_of_vec_a = get_vector_mod(vec_a);
    mod_of_vec_b = get_vector_mod(vec_b);
    if((0.001f >= mod_of_vec_a) || (0.001f >= mod_of_vec_b)){
        angle = 0.0f;/*无法计算某向量与零向量的夹角，返回0*/
    }else{
        float cos_value = (vec_a[0]*vec_b[0] + vec_a[1]*vec_b[1] + vec_a[2]*vec_b[2]) / (mod_of_vec_a * mod_of_vec_b);
        if(cos_value > 1.0f){
            cos_value=1.0f;
        }
        if(cos_value < -1.0f){
            cos_value=-1.0f;
        }
        angle = acos(cos_value);
        angle = angle * 180.0f / 3.1415f;
    }
    return angle;
}

/*通过外积法，求平面两个非同向向量所在平面的法向量*/
inline void makekam_outer_product(const float vec1[3],const float vec2[3],float vecDst[3])
{
    vecDst[0] = vec1[1]*vec2[2] - vec2[1]*vec1[2];/*y1z2-y2z1*/
    vecDst[1] = vec2[0]*vec1[2] - vec1[0]*vec2[2];/*x2z1-x1z2*/
    vecDst[2] = vec1[0]*vec2[1] - vec2[0]*vec1[1];/*x1y2-x2y1*/
}

/*根据重力加速度，计算姿态角*/
void makekam_gacc_to_att(float gravity_acc[3],float *ret_roll,float *ret_pitch)
{
    float roll = 0.0f;
    float pitch = 0.0f;
    float flight_z_acc[3] = {0.0f,0.0f,10.0f};
    float flight_x_acc[3] = {10.0f,0.0f,0.0f};
    float plane_xog_normal_vec[3] ={0};

    pitch = 90.0f - makekam_vec_ang(flight_x_acc,gravity_acc);/*pitch处于-90°到+90°之间*/
    if((90.0f - fabsf(pitch)) > 0.1f){/*pitch与±90度相差大于0.1°*/
        makekam_outer_product(flight_x_acc,gravity_acc,plane_xog_normal_vec);
        roll = makekam_vec_ang(flight_z_acc,plane_xog_normal_vec) - 90.0f;
        if(gravity_acc[2] > 0.0f){
            if(roll < 0.0f){
                roll = -180.0f - roll;
            }else{
                roll = 180 - roll;
            }
        }
    }else{
        roll = 0.0f;/*判定x轴与重力方向处于一条直线，此为特殊情况，滚转角设定为0°*/
    }
    *ret_roll = roll;
    *ret_pitch = pitch;
}