MPU6050的初始化

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

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

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

　　寄存器的查閱

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

　　以電源管理寄存器為例：

　　寄存器地址：0x6B（Hex）或107（十進制）。

　　表格后面的幾位Bit7~Bit0代表八位二進制，給該寄存器賦值就是改變這幾位的值。各個位代表的意義可看表下方的說明：如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時芯片就會將所有內部寄存器復位。

　　-

　　驅動程序

　　對MPU6050的初始化驅動就是通過IIC的協議，對MPU6050的寄存器進行初始化配置，我選擇配置的有：

　　設置電源管理寄存器1（0X6B），復位MPU6050 （下面舉例）

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

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

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

　　設置中斷使能寄存器（0X38），關閉中斷

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

　　//以下函數通過IIC協議，修改MPU6050的電源管理寄存器，實現復位

　　//其中的（IIC_……）函數為IIC通信函數，可從名字中了解大致功能，具體應用可參見IIC通信協議的內容

　　char MPU_Reset（）

　　{

　　IIC_Start（）;

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

　　if（IIC_Wait_Ack（））

　　{

　　IIC_Stop（）;

　　return 1;

　　}

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

　　IIC_Wait_Ack（）;

　　IIC_Send_Byte（0x80）; //發送數據（1000 0000）第七位為1，復位

　　if（IIC_Wait_Ack（））

　　{

　　IIC_Stop（）;

　　return 1;

　　}

　　IIC_Stop（）;

　　return 0;

　　}

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

　　讀取原始數據

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

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

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

　　通過這些我們就可以得到原始數據了，順便附上初始化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，解除休眠狀態，時鐘為內部8MHz

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

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

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

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

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

　　+-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）;//不自測，量程設置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數據**************************************** //**************************************

　　//讀取mpu6050內部數據，兩個字節，合成數據 //************************************** s16

　　GetData（u8

　　REG_Address）

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

　　s16 H=0，L=0;

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

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

　?。℉《《8）+L;

　　//合成數據，為有符號整形數 }

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

　　s32 Read_Acc（void） {

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

　　/*使用是加速度軸x軸正向朝向小車行徑方向，y軸陀螺儀的正向 逆時針方向。 加速度計的量程范圍見配置 不自測，量程設置4g scal系數為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平面上所對應的（x，y）坐標的角度， 它的值域范圍是（-Pi，Pi） 用數學表示就是：atan2（y，x）=arg（y/x）-Pi 當y《0時，其值為負，

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

　　}

　　else {

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

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

　　+-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多個數據的算術均值

　　Angle_gyro= -read_gyro_y/65.5; //去除零點偏移，計算角速度值，負號為方向處理 //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） {