MPU6050 6軸陀螺儀的使用與校準

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軸陀螺儀來降低漂移量,但是仍然無法完全解決這個問題.而且地磁計對周圍磁場的變化敏感,多磁場和多電機的工作環境都會導緻地磁計不準.