一、手册代码以及图示

二、流程说明

1、角度计算函数说明

//============================================================================
//函数名称：void AngleCalculate(void)
//函数返回：无
//参数说明：无
//功能概要：车身角度计算。
//============================================================================
void AngleCalculate(void)
{
  //读取加速度计
  MMA8451_Read();

  //读取陀螺仪
  LS3G4200D_Read();

  //取陀螺仪的X轴数据
  VOLTAGE_GYRO = Gyro_X;

  //取加速度读计的Y轴数据
  VOLTAGE_GRAVITY = Gray_Y;

  //陀螺仪零偏跟随
  Gyro_Offset_Calculate();

  if(VOLTAGE_GRAVITY > 1050)                              //限制加速度计读数-1050 ~1050
  {
    VOLTAGE_GRAVITY = 1050;
  }

  if(VOLTAGE_GRAVITY <=  -1050)
  {
    VOLTAGE_GRAVITY = - 1050;
  }

  //加速度计读取的Y轴数值转换为角度
  g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;

  //陀螺仪提取角速度
  g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;

  //车身角度计算
  g_fCarAngle = g_fGyroscopeAngleIntegral;
  fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
  g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
}

首先是通过加速度计测得重力加速度，然后通过归一化（因为g和角度成正比），计算出角度。然后由于车子运动过程中，车子加速度会产生一定的影响，导致不是正比关系，所以使用陀螺仪进行修正，陀螺仪测得是角速度，积分之后就是角度，然后再和加速度计测得角度进行融合，然后反馈回去，使得系统平衡。

2、陀螺仪零点跟随

/============================================================================
//函数名称：void Gyro_Offset_Calculate()
//函数返回：无
//参数说明：无
//功能概要：//陀螺仪三个轴的零点跟随  Cut_Gyro_X为经过处理后的值， Gyro_X为实际的读值，Gyro_X_Offset 为零点
//============================================================================
void Gyro_Offset_Calculate()
{
  float Cut_Gyro_X;

  //减去偏移值得到的实时值
  Cut_Gyro_X = Gyro_X - GYROSCOPE_OFFSET;

  //根据实时值，修改零偏置
  if(Cut_Gyro_X>=2)
  {
    GYROSCOPE_OFFSET += 0.001;
  }
  else if(Cut_Gyro_X<=-2)
  {
    GYROSCOPE_OFFSET -= 0.001;
  }
  else
  {
    GYROSCOPE_OFFSET = GYROSCOPE_OFFSET * 0.99 + 0.01 * Gyro_X;
  }

  //开机2秒进行跟随
  if (Clocks_Gyro <= 400)
  {
    GYROSCOPE_OFFSET = GYROSCOPE_OFFSET * 0.5 + 0.5 * Gyro_X;
  }

}

根据实时值，修改零偏值。

3、其中有一些参数设置：

/***************直立控制*****************/

float g_fGravityAngle = 0;                                              //加速度计转换来的角度
float g_fGyroscopeAngleSpeed  = 0;                                      //陀螺仪转换来的角速度
float g_fCarAngle = 0;                                                  //车身角度

float VOLTAGE_GYRO = 0;                                                 //定义陀螺仪（选X,Y,Z）
float VOLTAGE_GRAVITY = 0;                                              //定义加速度计（选X,Y,Z）

float GRAVITY_OFFSET = 27;                                               //加速度计零偏
float GRAVITY_ANGLE_RATIO = (90 /(1050.0 - 27)) ;                       //加速度计归一化因子

float GYROSCOPE_OFFSET = 0  ;                                            //陀螺仪零偏移
float GYROSCOPE_ANGLE_RATIO = 1.780;                                       //归一化因子

float GRAVITY_ADJUST_TIME_CONSTANT = 2;                                  //陀螺仪加速度计加和的比例因子
float GYROSCOPE_ANGLE_SIGMA_FREQUENCY = 200;                             //积分的频率

float Clocks_Gyro = 0;                                                   //开机2s内进行陀螺仪零点比对