Raspberry Pi 筆記(26)：MPU-6050 加速度計與陀螺儀感測器

目前正流行的空拍機，主要是機上安裝了加速度計與陀螺儀感測器，使得四軸的飛行器能夠漂浮在空中。MPU-6050是一個價格不高的六軸感測器，模組載有加速度計與陀螺儀的功能，我們可以透過 I2C 讀取感測器的值，作為改變飛行的方向與速度。以下就來看看如何利用Raspberry Pi 讀取 MPU-6050 六軸感測器的資料。

加速規，又稱加速計、加速針、加速度傳感器、重力加速度傳感器等，是測量加速度的裝置。相對於遠距感測的裝置，它測量的是自身的運動。加速規的應用之一是測量重力，特別是使用於重量測定法的加速規上，這樣的裝置稱為重力計。[維基百科]

陀螺儀（英文：gyroscope），是一種用來感測與維持方向的裝置，基於角動量守恆的理論設計出來的。陀螺儀主要是由一個位於軸心且可旋轉的轉子構成。 陀螺儀一旦開始旋轉，由於轉子的角動量，陀螺儀有抗拒方向改變的趨向。陀螺儀多用於導航、定位等系統。[維基百科]

[材料]

• Raspberry Pi 2 Model B x 1
• MPU-6050模組 x 1
• 連接線 x 4條

[接線]

將 Raspberry Pi 的第 1 pin (GPIO4)接到MPU-6050的VCC，第 3 pin (GPIO4)接到MPU-6050的SDA。其他 pin 連接如下表：

MPU-6050模組	Raspberry pi接腳
VCC	1
SDA	3
SCL	5
GND	6

[設定步驟]

進入 root 模式執行設定，並開啟 SPI：

# raspi-config

選項如下：7.Advanced Options -> A7 I2C -> Enable -> YES -> OK -> <Finish>

檢查是否MPU-6050是否可以被偵測到，先安裝I2C工具：

$ sudo apt-get install i2c-tools

使用 Raspberry Pi A版的要使用以下指令：

$ sudo i2cdetect -y 0

使用 Raspberry Pi B版的要使用以下指令：

$ sudo i2cdetect -y 1

這時如果正常，會出現以下畫面：

為了要使用 python 讀取I2C，我們需要安裝 smbus 模組：

$ sudo apt-get install python-smbus

讀取加速度計及陀螺儀資料的程式（取自Bitify）：

#!/usr/bin/python
import smbus
import math

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

bus = smbus.SMBus(0) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

print "gyro data"
print "---------"

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131)
print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)

print
print "accelerometer data"
print "------------------"

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

print "accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled
print "accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled
print "accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled

print "x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
print "y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

結果畫面如下：

[參考資料]

• Bitify : Interfacing Raspberry Pi and MPU-6050
•             Reading data from the MPU-6050 on the Raspberry Pi