在静止状态下根据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)；

/*通过内积法，求两个向量的夹角*/
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.01f >= mod_of_vec_a) || (0.01f >= mod_of_vec_b)){/*无法计算某向量与零向量的夹角*/
    angle = 0.0f;/*如果俯仰角是90deg,那么滚转角的计算会运行到这里*/
  }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};

  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;
      }
  }
  pitch = 90.0f - makekam_vec_ang(flight_x_acc,gravity_acc);
  *ret_roll = roll;
  *ret_pitch = pitch;
}