[STM32] MPU6050 陀螺儀/加速器換算方式

圖一(圖片來至google)

最近在研究Quadcopter，需要使用加速度計和陀螺儀來做為平衡的依據，而選用的晶片是MPU6050(如圖一)，首先先簡易了解一下晶片的一些特性

1. 三軸陀螺儀(Gyroscope)

•  數位輸出X,Y,Z的角速度(°C)
•  Range : ±250, ±500, ±1000, and ±2000°/sec 

2.三軸加速度計(Accelerometer)

•  數位輸出X,Y,Z的重力(g)
•  Range : ±2g, ±4g, ±8g, and ±16g 
  

3.其他特性

• 使用I2C與MCU溝通
• 可以外加地磁計(Magnetometer)，組成九軸偵測

4. Block Diagram

• X/Y/Z  Accel、X/Y/Z-Gyro 經過ADC轉換，儲存Sensor Registers，最後再透過I2C介面提供data給MCU。

看完基本特性之後，開始研究程式碼，雖然官方有提供8051跟arduino的Source Code 不過我採用是由Harinadha Reddy Chintalapalli所提供的MP6050lib for STM32，

#include "mpu6050.h"
int main()
{
     s16 Buffer[6]={0x00};
     MPU6050_Initialize();

    while ( 1 ) {
        // index:0~2 acceleration
        // 3~5 Angular rate
        MPU6050_GetRawAccelGyro(Buffer);  
    }
}

從Debug Mode，確定已經可以取到加速度和腳速度的值(圖二)。

圖二

但問題又來了，這些值，到底代表甚麼含義呢??還有要如何測試呢??
首先從MPU6050 Doucment得到以下幾個資訊(圖三)

圖三

• 使用二的補數來表示正負號 
• 正號 : 0x0000~0x7FFF ( 0 ~ 32767)
• 負號 : 0x8000~0xFFFF ( 32768 ~ 65535 )

• Gyroscope (AFS_SEL =  0)
• 測量角速度
• 131 LSB/°:表示一度使用131表示
• 正號 : 0x0001~0x7FEE (  1 ~ 32750 )
• 負號 : 0xFFFF~0x80C7 ( -1~-32750 )
•  測試方式
• 理論上，在靜止時，角速度的值應該都要為0，但從圖二可得知，實際上並不為0，會有一個誤差值，不過我測試的結果，大概誤差在正負1~2度左右，還在可接受範圍內。
• 針對X/Y/Z軸，做順時針與逆時針旋轉，可得知值變化狀態
  
  
• Accelerometer (AFS_SEL =  0)
• 測量加速度
• 16384 LSB/mg :表示1g使用16384表示
• 正號: 0x0001~0x7FFF ( 0~ 32767 )
• 負號: 0xFFFF~0x8000 (-1~-32768 )
  
• 測試方式
• 如果因為某些因素讓你無法得知，你的晶片方向，那你可以使加速度計來得知，因為只要是水平的放，那三軸中必有一軸須承受地心引力(1g)，以圖三index[2](Z軸)的值為16700，而約等於16384，差不多為1G，而在90度的翻轉向X或Y軸(圖四)，在觀察16384的值會落在哪個index上，就可以得知X,Y,Z三軸的位置。

  

  圖四

•  Reference
  [1] 以陀螺儀 (HiTechnic Gyro)推算物體的旋轉角度的實作測試
  [2] MPU6050lib for STM32 
  [3] Product mpu6050 for invensense 
  [4] 陀螺儀和方向感應器