1 MPU6050初始化

①初始化IIC接口。
②初始化MPU6050。由电源管理寄存器1(0X6B)控制。
③设置角速度传感器和加速度传感器的满量程范围。由陀螺仪配置寄存器(0X1B)和加速度传感器配置寄存器(0X1C)设置 。
④设置其他参数。配置中断，由中断使能寄存器(0X38)控制；设置AUX IIC接口，由户控制寄存器(0X6A)控制；设置FIFO，由FIFO使能寄存器(0X23)控制；陀螺仪采样率 ，由采样率分频寄存器(0X19)控制；设置数字低通滤波器，由配置寄存器(0X1A)控制。
⑤设置系统时钟。由电源管理寄存器1(0X6B)控制。一般选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟。
⑥**使能角速度传感器(陀螺仪)和加速度传感器。**由电源管理寄存器2(0X6C)控制

注:如果是使用MPU6050的标准函数,只需要依次调用这三个函数即可

IIC_Init();                      //=====IIC初始化
MPU6050_initialize();           //=====MPU6050初始化
DMP_Init();                     //=====初始化DMP

下面主要解释这三个函数的配置:

2 IIC_Init()函数的使用

2.1 IIC是什么

IIC串行总线一般有两根信号线，一根是双向的数据线SDA，另一根是时钟线SCL。所有接到I2C总线设备上的串行数据SDA都接到总线的SDA上，各设备的时钟线SCL接到总线的SCL上。
　　设备上的串行数据线SDA接口电路应该是双向的，输出电路用于向总线上发送数据，输入电路用于接收总线上的数据。而串行时钟线也应是双向的，作为控制总线数据传送的主机，一方面要通过SCL输出电路发送时钟信号，另一方面还要检测总线上的SCL电平，以决定什么时候发送下一个时钟脉冲电平；作为接受主机命令的从机，要按总线上的SCL信号发出或接收SDA上的信号，也可以向SCL线发出低电平信号以延长总线时钟信号周期。
　　总线空闲时，因各设备都是开漏输出，上拉电阻Rp使SDA和SCL线都保持高电平。任一设备输出的低电平都将使相应的总线信号线变低，也就是说：各设备的SDA是“与”关系，SCL也是“与”关系。

2.2 IIC在stm32中的初始化

void IIC_Init(void)
{			
  GPIO_InitTypeDef  GPIO_InitStructure;
	
 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD,ENABLE);//先使能外设IO PORTB时钟 
		
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13;	 // 端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		    //推挽输出
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		    //IO口速度为50MHz
  GPIO_Init(GPIOD, &GPIO_InitStructure);					//根据设定参数初始化GPIO 
	
  GPIO_SetBits(GPIOD,GPIO_Pin_12|GPIO_Pin_13);						 
}

在.h文件中最好将SDA与SCL进行预处理方便移植和使用:

#define IIC_SCL    PDout(12) //SCL
#define IIC_SDA    PDout(13) //SDA	 
#define READ_SDA   PDin(13)  //输入SDA 

其他的IIC读取和写入函数在例程中已完成,我们主要的工作是将任意两端口配置为IIC的SDA和SCL.

3 MPU6050初始化

/**************************实现函数********************************************
*函数原型:		void MPU6050_initialize(void)
*功　　能:	    初始化 	MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void)
{
    MPU6050_setClockSource(MPU6050_CLOCK_PLL_ZGYRO); //设置时钟
    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_1000);//陀螺仪最大量程 +-1000度每秒
    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);	//加速度度最大量程 +-2G
    MPU6050_setSleepEnabled(0); //进入工作状态
    MPU6050_setI2CMasterModeEnabled(0);	 //不让MPU6050 控制AUXI2C
    MPU6050_setI2CBypassEnabled(0);	 //主控制器的I2C与	MPU6050的AUXI2C	直通。控制器可以直接访问HMC5883L
}

此配置的相关函数在mpu6050.h和iic.h中.

4 重点:DMP初始化

DMP是MPU6050官方给出的一种直接读取四元数的方法,上手十分方便,作为初学MPU6050,我们以此为例程.

void DMP_Init(void)
{
    u8 temp[1] = {0};
    i2cRead(0x68, 0x75, 1, temp);
    printf("mpu_set_sensor complete ......\r\n");
    if(temp[0] != 0x68)
		NVIC_SystemReset();
    if(!mpu_init())
    {
        if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
            printf("mpu_set_sensor complete ......\r\n")
        if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
           printf("mpu_configure_fifo complete ......\r\n");
        if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
           printf("mpu_set_sample_rate complete ......\r\n");
        if(!dmp_load_motion_driver_firmware())
           printf("dmp_load_motion_driver_firmware complete ......\r\n");
        if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
            printf("dmp_set_orientation complete ......\r\n");
        if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                               DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                               DMP_FEATURE_GYRO_CAL));
            printf("dmp_enable_feature complete ......\r\n");
        if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
           printf("dmp_set_fifo_rate complete ......\r\n");
		run_self_test();
        if(!mpu_set_dmp_state(1))
         printf("mpu_set_dmp_state complete ......\r\n");
    }
}

此函数主要是对DMP进行初始化,如果想要了解每个函数具体是初始化的什么内容,请看printf函数输出的内容即可.

我们主要讲:陀螺仪是如何进行开机校准,或者设置开机不校准.
将目光放到**run_self_test();**这个函数,这是用来设置校准的函数.
static void run_self_test(void)
{
int result;
long gyro[3], accel[3];

result = mpu_run_self_test(gyro, accel);				//返回值是使用陀螺仪的型号,MPU6050对应的是0x03
if (result == 0x03)											
{
    /* Test passed. We can trust the gyro data here, so let's push it down
     * to the DMP.
     */
    float sens;
    unsigned short accel_sens;
    mpu_get_gyro_sens(&sens);					//读取当前陀螺仪的状态
  	//sens=0;
    gyro[0] = (long)(gyro[0] * sens);				
    gyro[1] = (long)(gyro[1] * sens);
    gyro[2] = (long)(gyro[2] * sens);
    dmp_set_gyro_bias(gyro);				    //根据读取的状态进行校准
    mpu_get_accel_sens(&accel_sens);			//读取当前加速度计的状态
    accel[0] *= accel_sens;								
    accel[1] *= accel_sens;
    accel[2] *= accel_sens;
    dmp_set_accel_bias(accel);					//根据读取的状态进行校准
   printf("setting bias succesfully ......\r\n");
}

}
这样的话,mpu6050校准的过程就明确了.
如果不需要开机校准,可以是if(result==0x03)这个条件不满足,不进入处理,或者将陀螺仪 加速度计的基准设置函数set_bias不执行即可.
如果需要开机校准,保证整个校准流程正常执行就行.

4 陀螺仪漂移的解决方法

横滚角和俯仰角的偏差可以通过卡尔曼滤波或者一阶互补滤波即可消除漂移量.

但是**航向角存在的偏差很难由自身来校准,**这是由于:
Z轴在静止状态下没有漂移，运动情况下有累积误差。因为Z轴角度是通过对角速度积分计算出来的，没有观测量滤波，所以漂移是不可避免的。X、Y轴的角度是根据重力加速度分量计算，X，Y轴是有重力场滤波，所以不会有漂移。Z轴只能用短时间内的相对测量量。 就是相邻两次的角度差来计算转过的角度。

可以采用带地磁的9轴陀螺仪来降低漂移量,但是仍然无法完全解决这个问题.而且地磁计对周围磁场的变化敏感,多磁场和多电机的工作环境都会导致地磁计不准.