センサの核心部分は MPU6050 というチップで、GY-521 という刻印のある製品がメジャーなようです。X-Y-Z 3 軸方向の加速度と、3 軸まわりの回転速度、それと温度を計測できます。
RaspberryPi とは I2C で接続でき、I2C で必要な 3 本とは別に電源が必要で、計 4 本で接続します。基盤上に 3.3V のレギュレータが載っており、5V 電源で動作しますが、3.3V でも動くみたいです*1。
まずは、シェルからポチポチとコマンドを叩いて実験してみます。念のため、MPU6050 のデータシートを見ながら進めますが、このチップのデータシートがまた非常に読みにくい
### バスに接続されたスレーブを一覧して、センサのアドレスを調べます $ i2cdetect -y 1 ⇒ センサのアドレスは 0x68 ... ### レジスタの値を読み書きしてみます $ i2cget -y 1 0x68 0x1b 0x00 ← Gyroscope Configuration を読む。ジャイロ センサの感度は ±250deg/s $ i2cget -y 1 0x68 0x1c 0x00 ← Accelerometer Configuration を読む。加速度センサの感度は ±2G $ i2cget -y 1 0x68 0x6b 0x40 ← Power Management 1 を読む。デバイスは sleep 状態 $ i2cset -y 1 0x68 0x6b 0x00 ← sleep を解除して計測開始 ### 計測値を読出します $ i2cget -y 1 0x68 0x3b w ← X 軸方向の加速度 0x78fc ← 上位バイト(MSB)と下位バイト(LSB)が入れ替わっているので注意
センサをあっちこっちに傾けながら i2cget コマンドを叩くと、取得でき値が変化していることが判ります。
とりあえず、センサを正しく結線できて、何等かのデータを読み出せているようなので、連続してデータを取得できるよう python スクリプトを書いてみます。
#!/usr/bin/python
# -*- coding: utf-8 -*-
import smbus # use I2C
import math
from time import sleep # time module
### define #############################################################
DEV_ADDR = 0x68 # device address
PWR_MGMT_1 = 0x6b # Power Management 1
ACCEL_XOUT = 0x3b # Axel X-axis
ACCEL_YOUT = 0x3d # Axel Y-axis
ACCEL_ZOUT = 0x3f # Axel Z-axis
TEMP_OUT = 0x41 # Temperature
GYRO_XOUT = 0x43 # Gyro X-axis
GYRO_YOUT = 0x45 # Gyro Y-axis
GYRO_ZOUT = 0x47 # Gyro Z-axis
# 1byte read
def read_byte( addr ):
return bus.read_byte_data( DEV_ADDR, addr )
# 2byte read
def read_word( addr ):
high = read_byte( addr )
low = read_byte( addr+1 )
return (high << 8) + low
# Sensor data read
def read_word_sensor( addr ):
val = read_word( addr )
if( val < 0x8000 ):
return val # positive value
else:
return val - 65536 # negative value
# Get Temperature
def get_temp():
temp = read_word_sensor( TEMP_OUT )
# offset = -521 @ 35℃
return ( temp + 521 ) / 340.0 + 35.0
# Get Gyro data (raw value)
def get_gyro_data_lsb():
x = read_word_sensor( GYRO_XOUT )
y = read_word_sensor( GYRO_YOUT )
z = read_word_sensor( GYRO_ZOUT )
return [ x, y, z ]
# Get Gyro data (deg/s)
def get_gyro_data_deg():
x,y,z = get_gyro_data_lsb()
# Sensitivity = 131 LSB/(deg/s), @cf datasheet
x = x / 131.0
y = y / 131.0
z = z / 131.0
return [ x, y, z ]
# Get Axel data (raw value)
def get_accel_data_lsb():
x = read_word_sensor( ACCEL_XOUT )
y = read_word_sensor( ACCEL_YOUT )
z = read_word_sensor( ACCEL_ZOUT )
return [ x, y, z ]
# Get Axel data (G)
def get_accel_data_g():
x,y,z = get_accel_data_lsb()
# Sensitivity = 16384 LSB/G, @cf datasheet
x = x / 16384.0
y = y / 16384.0
z = z / 16384.0
return [x, y, z]
### Main function ######################################################
bus = smbus.SMBus( 1 )
bus.write_byte_data( DEV_ADDR, PWR_MGMT_1, 0 )
while 1:
temp = get_temp()
print 't= %.2f' % temp, '\t',
gyro_x,gyro_y,gyro_z = get_gyro_data_deg()
print 'Gx= %.3f' % gyro_x, '\t',
print 'Gy= %.3f' % gyro_y, '\t',
print 'Gz= %.3f' % gyro_z, '\t',
accel_x,accel_y,accel_z = get_accel_data_g()
print 'Ax= %.3f' % accel_x, '\t',
print 'Ay= %.3f' % accel_y, '\t',
print 'Az= %.3f' % accel_z, '\t',
print # 改行
sleep( 1 )
センサ モジュールをだいたい水平なテーブル上に静置した状態で、上記のプログラムを利用して、加速度センサとジャイロ センサの計測値 60 点分をグラフにしてみました(Fig.1 , Fig.2 )。
加速度センサの計測値(A*)を見ると、おおよそ安定しており、計測値をそのまま姿勢制御に使うことができそうです。Y 軸(赤)が -0.2 程度と少し大き目の値を示しているのは、テーブルが微妙に傾いていることに加えて、ブレッドボードに刺したセンサ自体が傾いていたからです。sin-1(-0.2) = 11.5°と計算できますが、まぁ、だいたいそれくらは傾いていた感じ。また、Z 軸(緑)は、地面に鉛直方向なので、地球重力の 1G を概ね計測できているようです。
一方、センサを静置していたにも関わらず、ジャイロ センサの計測値(G*)には大きな変動がみられます。また、オフセットも大きく、計測値を使うには骨が折れそうです。実際、高価なジャイロ センサにおいても、このような計測値の変動があります。得られた計測値について、カルマン フィルタなどを利用してやれば、それなりの値を求められると思います*2。
次に、センサの Z 軸を水平に保ち、センサを Z 軸周りに回転させ、X 軸および Y 軸の計測値を X-Y プロットしてみました(Fig.3 )。微妙に軸がずれている気もしますが*3、概ね半径 1G の円を描けており、加速度センサとしては、無調整でもそのまま使えそうな感じです。
