MPU6050的初始化

　　MPU6050是運(yùn)動處理傳感器，它集成了3軸陀螺儀，3軸加速度計以及DMP，其中的DMP是一個可通過IIC接口擴(kuò)展的數(shù)字運(yùn)動處理器。

　　對于DMP可以用 InvenSense 公司提供的資料庫，使MPU6050可以解算出姿態(tài)，通過IIC接口直接輸出陀螺儀和加速度數(shù)據(jù)融合后的四元數(shù)，減輕了處理器的負(fù)荷，非常適合簡單的開發(fā)應(yīng)用。但是，使用DMP的這種硬件解算也存在問題，有時會無法讀出數(shù)據(jù)，因此，在四軸的應(yīng)用中通常都會采用軟件解算，常見的姿態(tài)解算方法有：非線性互補(bǔ)濾波算法，卡爾曼濾波算法，Mahony互補(bǔ)濾波算法（可參見Crazypony的開源項目）。

　　MPU6050通過IIC協(xié)議與處理器進(jìn)行通信。我使用STM32時通常采用軟件模擬IIC的方式。

　　寄存器的查閱

　　MPU6050的所有寄存器都可以在官方文檔“MPU-6000 and MPU-6050 Register Map and Descriptions”中找到，平時使用中最為重要的有以下幾種：電源管理寄存器1和2、陀螺儀配置寄存器、陀螺儀采樣率分頻寄存器、加速度傳感器配置寄存器、配置寄存器。

　　以電源管理寄存器為例：

　　寄存器地址：0x6B（Hex）或107（十進(jìn)制）。

　　表格后面的幾位Bit7~Bit0代表八位二進(jìn)制，給該寄存器賦值就是改變這幾位的值。各個位代表的意義可看表下方的說明：如DEVICE_RESET ：When set to 1， this bit resets all internal registers to their default values.The bit automatically clears to 0 once the reset is done.表明DEVICE_RESET被置1時芯片就會將所有內(nèi)部寄存器復(fù)位。

　　-

　　驅(qū)動程序

　　對MPU6050的初始化驅(qū)動就是通過IIC的協(xié)議，對MPU6050的寄存器進(jìn)行初始化配置，我選擇配置的有：

　　設(shè)置電源管理寄存器1（0X6B），復(fù)位MPU6050 （下面舉例）

　　設(shè)置陀螺儀配置寄存器（0X1B），將量程設(shè)置為 2000dps

　　設(shè)置加速度計配置寄存器（0X1C），將量程設(shè)置為 2g

　　設(shè)置采樣頻率分頻器（0X19），將采樣率設(shè)置為50Hz

　　設(shè)置中斷使能寄存器（0X38），關(guān)閉中斷

　　設(shè)置電源管理寄存器2（0X6C），使加速度陀螺儀都工作

　　//以下函數(shù)通過IIC協(xié)議，修改MPU6050的電源管理寄存器，實現(xiàn)復(fù)位

　　//其中的（IIC_……）函數(shù)為IIC通信函數(shù)，可從名字中了解大致功能，具體應(yīng)用可參見IIC通信協(xié)議的內(nèi)容

　　char MPU_Reset（）

　　{

　　IIC_Start（）;

　　IIC_Send_Byte（（0x68《《1）|0）; //發(fā)送器件地址+寫命令

　　if（IIC_Wait_Ack（））

　　{

　　IIC_Stop（）;

　　return 1;

　　}

　　IIC_Send_Byte（0x6B）; //寫寄存器地址，選擇電源管理寄存器1

　　IIC_Wait_Ack（）;

　　IIC_Send_Byte（0x80）; //發(fā)送數(shù)據(jù)（1000 0000）第七位為1，復(fù)位

　　if（IIC_Wait_Ack（））

　　{

　　IIC_Stop（）;

　　return 1;

　　}

　　IIC_Stop（）;

　　return 0;

　　}

　　按照相同的方法對其余的寄存器進(jìn)行配置之后，MPU6050就可以正常工作了。接下來的任務(wù)就是不斷讀取它的數(shù)據(jù)，計算出芯片的姿態(tài)了。。。

　　讀取原始數(shù)據(jù)

　　使用IIC協(xié)議讀取以上寄存器的值，每個軸的值由16位二進(jìn)制表示（0–65535），以X軸為例：ACCEL_XOUT［15：8］、ACCEL_XOUT［7：0］分別為X軸加速度的高八位和低八位，每次讀取八位再將它們拼起來即可。

　　ax=（ （unsigned int）buffer［0］《《8 ） | buffer［1］;

　　此時就獲得了MPU6050輸出的ADC值了，它是以LSB為單位的，而不是以實際值的 g 為單位，它對應(yīng)的實際值與你在初始化的時候設(shè)置的量程有關(guān)。比如說我們設(shè)置的量程是+-2g，那對應(yīng)的靈敏度=65536/4 LSB/g ， 那么實際的加速度值=ADC的值LSB / 16384 LSB/g

　　通過這些我們就可以得到原始數(shù)據(jù)了，順便附上初始化MPU6050源代碼：

　　itStructure）;

　　GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;

　　GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; GPIO_Init（GPIOB， &GPIO_InitStructure）;

　　if（Single_Read（MPU6050_Addr，WHO_AM_I）==0x68） {

　　Single_Write（MPU6050_Addr，PWR_MGMT_1， 0x00）;

　　//電源管理1，解除休眠狀態(tài)，時鐘為內(nèi)部8MHz

　　Single_Write（MPU6050_Addr，SMPLRT_DIV， 0x07）;//采樣速率125Hz Single_Write（MPU6050_Addr，CONFIG，0x06）;

　　//不使能FSYNC，不使用外同步采樣速率；DLPF_CFG［2~0］，設(shè)置任意軸是否通過DLPF，

　　//典型值：0x06（5Hz）低通濾波器帶寬5Hz，

　　//對加速度和陀螺儀都有效，輸出頻率為1kHz，決定SMPLRT_DIV的頻率基準(zhǔn)

　　Single_Write（MPU6050_Addr，GYRO_CONFIG， 0x08）;//不自測，量程設(shè)置500°/s /*？GYRO 量程單位系數(shù)

　　+-250 deg/s 131 LSB/deg/s 初始化hex 0x00 +-500 deg/s 65.5 LSB/deg/s 0x08 +-1000 deg/s 32.8 LSB/deg/s 0x10 +-2000 deg/s 16.4 LSB/deg/s 0x18 */

　　Single_Write（MPU6050_Addr，ACCEL_CONFIG， 0x00）;//不自測，量程設(shè)置2g

　　/* Accle any axe

　　+-2 g 16384 LSB/g +-4 g 8192 LSB/g +-8 g 4096 LSB/g +-16 g 2048 LSB/g

　　*/

　　return 0; } return 1; }

　　//******讀取MPU6050數(shù)據(jù)**************************************** //**************************************

　　//讀取mpu6050內(nèi)部數(shù)據(jù)，兩個字節(jié)，合成數(shù)據(jù) //************************************** s16

　　GetData（u8

　　REG_Address）

　　//返回值為有符號的整形，16位 {

　　s16 H=0，L=0;

　　H = Single_Read（MPU6050_Addr，REG_Address）; //先讀高字節(jié)，再讀低字節(jié)

　　L = Single_Read（MPU6050_Addr，REG_Address+1）; return

　　（H《《8）+L;

　　//合成數(shù)據(jù)，為有符號整形數(shù) }

　　//-------------加速度部分解算角度------------------

　　s32 Read_Acc（void） {

　　s32 Accel_x; //mpo6050讀出的X軸加速度 s32 Accel_z; //mpu6050讀出的z軸加速度 //-------------加速度部分解算------------------

　　/*使用是加速度軸x軸正向朝向小車行徑方向，y軸陀螺儀的正向 逆時針方向。 加速度計的量程范圍見配置 不自測，量程設(shè)置4g scal系數(shù)為8192 Accle any axe

　　+-2 g 16384 LSB/g +-4 g 8192 LSB/g +-8 g 4096 LSB/g +-16 g 2048 LSB/g */

　　Accel_x = GetData（ACCEL_XOUT_H）; //從mpu6050讀取X軸加速度 Accel_z = GetData（ACCEL_ZOUT_H）; //從mpu6050讀取z軸加速度

　　if（Accel_x》0） {

　　Angle_accel = atan2（（float）Accel_x，（float）Accel_z）*（180/3.14159265）;//反正切計算rad

　　/* atan2（y，x）是表示X-Y平面上所對應(yīng)的（x，y）坐標(biāo)的角度， 它的值域范圍是（-Pi，Pi） 用數(shù)學(xué)表示就是：atan2（y，x）=arg（y/x）-Pi 當(dāng)y《0時，其值為負(fù)，

　　當(dāng)y》0時，其值為正。 atan2*180/Pi可以計算出角度值 */

　　}

　　else {

　　s32 read_gyro_y; s32 Angle_gyro; //-------角速度解算-------------------------

　　//角速度量程見配置 本處使用1000 deg/s。scal系數(shù)為32.8 LSB /*？GYRO 量程單位系數(shù)

　　+-250 deg/s 131 LSB/deg/s offset 44.38188277*2 +-500 deg/s 65.5 LSB/deg/s offset 44.38188277 +-1000 deg/s 32.8 LSB/deg/s ok offset 44.38188277/2 +-2000 deg/s 16.4 LSB/deg/s offset 44.38188277/4 */

　　read_gyro_y= GetData（GYRO_YOUT_H）+Gyro_y_offset; //靜止時角速度Y軸輸出 //Gyro_y_offset計算方法gyro靜止時候N多個數(shù)據(jù)的算術(shù)均值

　　Angle_gyro= -read_gyro_y/65.5; //去除零點偏移，計算角速度值，負(fù)號為方向處理 //Angle_gyro測量值的單位是 deg/s.測量的物理量是角速度。 return Angle_gyro; }

　　Angle_accel

　　=

　　atan2（（float）Accel_z，（float）Accel_x）*（180/3.14159265）-90;//反正切計算 Angle_accel = -Angle_accel; }

　　//angle_accel物理量單位是角度 deg！ return Angle_accel; }

　　//陀螺儀計算Y軸的角速度 s32 Read_Gry（void） {