Raspberry pi 3でストロベリー・リナックス社製の「MPU-9250 ９軸センサモジュール (メーカー品番：MPU-9250)」を使うための奮闘記録です。Python2でトライしています。
GWが終わってもう時間が取れそうにないので、できたところまで...
せっかくセンサを大人買いしたのに、ほとんど手付かず。夏休み最終日の小学生の気分です。
（試行錯誤の過程をそのまま書いてますので長いです。いつかまとめたいですねぇ。）
2017-04-09 githubにupしました。
https://github.com/boyaki-machine/MPU-9250
物理配線
届いたセンサに、ピンヘッダをハンダ付けします。その後、配線します。
センサ1pin(VDD) -> Raspi 1番pin
センサ2pin(VDDIO) -> Raspi 1番pin
センサ3pin(SCL) -> Raspi 5番pin
センサ4pin(SDA) -> Raspi 3番pin
センサ5pin(GND or VDD) -> Raspi 6番pin (GNDかVDDかでI2Cアドレスが変わる)
センサ6pin(VDDIO) -> Raspi 1番pin
センサ10pin(GND) -> Raspi 6番pin
ゴチャゴチャですが、こんな感じです。
Raspberry piでI2Cを使えるようにする
OSのコンフィグメニューで設定。
$ sudo raspi-config
"9 Advenced Options” -> “A7 I2C” の順にメニューを選択。
「Would you like the ARM I2C interface to be enabled?」と聞いてくるのでyesを選択。
「Would you like the I2C kernel module to be loaded by default?」と聞いてくるのでyesを選択。
次に、/boot/config.txtを編集。
$ sudo vi /boot/config.txt
...以下の内容を追記
dtparam=i2c_arm=on
更に、/etc/modulesを編集。
$ sudo vi /etc/modules
...以下の内容を追記
snd-bcm2835
i2c-dev
設定終了後、Raspyを再起動。
再起動後にカーネルモジュールがロードされていることを確認。
$ lsmod
...
i2c_dev 6709 0
snd_bcm2835 21342 0
...
ツール類をインストールする
$ sudo apt-get install i2c-tools python-smbus
アドレス確認
センサのアドレスを確認します。
$ sudo i2cdetect -y 1 (5pinをGNDにつないだ時)
0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
アドレス0x68で認識。このMPU-9250は内部に旭化成エレクトロニクス製の磁気センサAK8963を内蔵しています。初期状態ではAK8963にアクセス出来ないのですが、幾つかのレジスタを操作するとI2Cでアクセスできるようになります。下記のpythonスクリプトでレジスタを設定します。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
channel = 1
bus = smbus.SMBus(channel)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# I2Cで磁気センサ機能(AK8963)へアクセスできるようにする(BYPASS_EN=1)
bus.write_i2c_block_data(address, 0x37, [0x02])
time.sleep(0.1)
スクリプト実行後再度アドレスを確認します。
$ sudo i2cdetect -y 1 (5pinをVDDにつないだ時)
0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- -- -- 0c -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
AK8963アクセス用のアドレス0x0Cが表示されました。
加速度の取得
生データの取得
こちらの記事を参考にさせていただきました。
ストロベリー・リナックスからの添付資料によると、
最初はスリープモードになっていてセンシングは行われていません。まずMPU-9250の内部アドレス0x6Bに0x00を書き込みます。更に内部アドレス0x37に0x02を書き込みます。これによって動作が開始し、磁気センサとI2C通信ができるようになります。加速度・ジャイロはこの状態で内部レジスタ0x3Bからの14バイトに加速度X,Y,ZジャイロX,Y,Zのデータが入ります。それぞれのデータは16ビットで上位８ビットが先に並んでいます。加速度はセンサを動かして重力加速度を見ることができますので簡単です。ジャイロは回転させる必要がありますので、面倒かもしれません。
16ビットで6軸なのに何故に14バイトなの？、と疑問でしたが、データシートを見ると温度も計測できるようで、それが2バイト分でした。
とりあえず加速度6バイトを取得、表示します。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
# レジスタをリセットする
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# 生データを取得する
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = data[0] << 8 | data[1] # 上位ビットが先
rawY = data[2] << 8 | data[3] # 上位ビットが先
rawZ = data[4] << 8 | data[5] # 上位ビットが先
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
これでデータが連続的に出力されることは確認できました。
gを算出する
生データから加速度を計算します。デフォルトの設定では±2gの範囲で出力されるようです。これを元に下記のように算出してみました。が、どうも表示される値が変です。
rawX = (2.0 / float(0x8000)) * (data[0] << 8 | data[1])
rawY = (2.0 / float(0x8000)) * (data[2] << 8 | data[3])
rawZ = (2.0 / float(0x8000)) * (data[4] << 8 | data[5])
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
いろいろ調べると、センサが出力する値はUnsigned型になっているので、マイナス値を表現するため、Signed型に変換する必要があるようです。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
# レジスタをリセットする
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# 生データを取得する
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = (2.0 / float(0x8000)) * u2s(data[0] << 8 | data[1])
rawY = (2.0 / float(0x8000)) * u2s(data[2] << 8 | data[3])
rawZ = (2.0 / float(0x8000)) * u2s(data[4] << 8 | data[5])
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
これで、Z軸方向はほぼ１gを出力するようになりました。
測定レンジを変える
センサを振り回すと意外とすぐに2gに達します。ので、測定レンジを変えてみます。データシートによると2,4,8,16のレンジを指定できるようです。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
# レジスタをリセットする
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# 加速度センサのレンジを±8gにする
bus.write_i2c_block_data(address, 0x1C, [0x08])
# 生データを取得する
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = (8.0 / float(0x8000)) * u2s(data[0] << 8 | data[1])
rawY = (8.0 / float(0x8000)) * u2s(data[2] << 8 | data[3])
rawZ = (8.0 / float(0x8000)) * u2s(data[4] << 8 | data[5])
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
ほほぅ～、これでかなり振り回しても値が取れるようになりました。
加速度を較正する
ストロベリー・リナックスからの添付資料によると、
センサにばらつきがありますので、加速度=0g、ジャイロ=0°sec、磁気=0μTでも観測値が 0にならず、少しずれた数値を示します。オフセットの許容範囲はデータシート上に範囲が示されていますので、その範囲内は正常品です。ソフトウェアの方でオフセットを差し引きするなどして調整することが必要になってきます。
とあります。確かにセンサを固定しておいてもX,Y軸の加速度は0になってくれません。データシートによるとレジスタ上にオフセット値を設定できそうですが、まずはpython側で較正を試みます。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
# レジスタをリセットする
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# 加速度センサのレンジを±8gにする
bus.write_i2c_block_data(address, 0x1C, [0x08])
# 較正値を算出する
print "Accel calibration start"
_sum = [0,0,0]
# 実データのサンプルを取る
for _i in range(1000):
data = bus.read_i2c_block_data(address, 0x3B ,6)
_sum[0] += (8.0 / float(0x8000)) * u2s(data[0] << 8 | data[1])
_sum[1] += (8.0 / float(0x8000)) * u2s(data[2] << 8 | data[3])
_sum[2] += (8.0 / float(0x8000)) * u2s(data[4] << 8 | data[5])
# 平均値をオフセットにする
offsetAccelX = -1.0 * _sum[0] / 1000
offsetAccelY = -1.0 * _sum[1] / 1000
# Z軸は重力分を差し引く 本当に1.0かは怪しい
offsetAccelZ = -1.0 * ((_sum[2] / 1000 ) - 1.0)
print "Accel calibration complete"
# 生データを取得する
while True:
data = bus.read_i2c_block_data(address, 0x3B ,6)
rawX = (8.0 / float(0x8000)) * u2s(data[0] << 8 | data[1]) + offsetAccelX
rawY = (8.0 / float(0x8000)) * u2s(data[2] << 8 | data[3]) + offsetAccelY
rawZ = (8.0 / float(0x8000)) * u2s(data[4] << 8 | data[5]) + offsetAccelZ
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ
time.sleep(1)
うーむ、ピッタリ0にはならないけど、最初の頃に比べればマシかな。平均値をオフセットに使ってよいか分かりませんが。
その他
較正しても、センサを固定して連続したセンシングを行うと値が微妙にぶれ続けています。未知の波動を検出しているのか!?、恐ろしい．．．
多分、センサが鋭敏すぎて電気ノイズを拾っているのではないかと思います。データシートのレジスタリストに「Accelerometer low pass filter setting」というのがあったので、これを弄くればもっと安定した値が得られるかもしれません。今回は時間切れで触れませんでした。
温度の取得
せっかく温度が取れるようなので、試してみました。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(channel)
# レジスタをリセットする
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# 生データを取得する
while True:
data = bus.read_i2c_block_data(address, 0x65 ,2)
raw = data[0] << 8 | data[1]
# 温度の算出式はデータシートから下記の通り
# ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity) + 21degC
temp = (raw / 333.87) + 21
print str(temp)
これで、室温らしい値が取れました。データシートに生データから温度を算出する式は書いてあるのですが、肝心のRoomTemp_OffsetやTemp_Sensitivityの値が載っておらず、こちらの記事を参考にさせてもらいました。
あまり追従性が良くないようで、手で握ってもなかなか値が変わりません。
角速度の取得
データの取得
やり方は加速度の時と同じですね。生データからdps値の算出、測定レンジの修正、較正値の算出も行います。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
address = 0x68
bus = smbus.SMBus(1)
# レジスタをリセットする
bus.write_i2c_block_data(address, 0x6B, [0x80])
time.sleep(0.1)
# PWR_MGMT_1をクリア
bus.write_i2c_block_data(address, 0x6B, [0x00])
time.sleep(0.1)
# 計測レンジを1000dpsに修正する
bus.write_i2c_block_data(address, 0x1B, [0x10])
# dpsを算出する係数
gyroCoefficient = 1000 / float(0x8000)
# 較正値を算出する
print "Gyro calibration start"
_sum = [0,0,0]
# 実データのサンプルを取る
for _i in range(1000):
data = bus.read_i2c_block_data(address, 0x43 ,6)
_sum[0] += gyroCoefficient * u2s(data[0] << 8 | data[1])
_sum[1] += gyroCoefficient * u2s(data[2] << 8 | data[3])
_sum[2] += gyroCoefficient * u2s(data[4] << 8 | data[5])
# 平均値をオフセットにする
offsetGyroX = -1.0 * _sum[0] / 1000
offsetGyroY = -1.0 * _sum[1] / 1000
offsetGyroZ = -1.0 * _sum[2] / 1000
print "Gyro calibration complete"
# データを取得する
while True:
data = bus.read_i2c_block_data(address, 0x43 ,6)
rawX = gyroCoefficient * u2s(data[0] << 8 | data[1]) + offsetGyroX
rawY = gyroCoefficient * u2s(data[2] << 8 | data[3]) + offsetGyroY
rawZ = gyroCoefficient * u2s(data[4] << 8 | data[5]) + offsetGyroZ
print "%+8.7f" % rawX + " ",
print "%+8.7f" % rawY + " ",
print "%+8.7f" % rawZ + " "
time.sleep(1)
その他
加速度センサの時と同様に、センサを固定していてもdps値がふらつきます。測定レンジから見れば誤差にすぎない値ですが、どうも落ち着きません．．．
こちらも、0x1AレジスタのDLPF_CFGを設定することで改善するのではないかと期待していますが、今回は試せませんでした。
磁気センサの取得
磁気センサは旭化成エレクトロニクス社製のAK8963です。日本語のデータシートが本当にありがたいです．．．
蛇足ですが、センサなんて日本メーカの独壇場なのかと思っていましたが、raspiにつなぐサンプルモジュールを調べても海外製ばかりなんですね。日本メーカに頑張って欲しいです。
それで、AK8963ですが、単純に一回データを取るだけだと簡単なのですが、連続したセンシングを行おうとすると色々手順が厄介でした。なんというか、マニアックな作りというか、妙に細かい設定ができるというか．．．
測定モードが単発、連続1(8Hz)、連続2(100Hz)、外部トリガと４種類あります。また、測定値を出力するbit数を、16bit/14bitで選択できます。単発モードだと、一回センシングするとPower downに遷移してしまうため、また単発モードを設定する必要があります。特に連続センシングモードの時は、前のデータが取り出されるまで、後の（新しい）データは破棄されていきます。取り出したデータは、オーバーフローしていないか毎回チェックする必要があります。オーバーフローすると正しい値が出ないそうな。
あと、地味にハマったのがレジスタからデータを読み出す際、下位ビットが先に取得できることです。加速度・角速度は上位ビットが先なので気をつけてください。
という感じで、磁気センサの部分だけ抜き出して書こうかと思ったのですが、結構大変なので、動作はクラス化したものを読んでください。
磁気センサは値が取れても正しいか判断が難しいですね。wikiによると、「経度50度における地磁気の強さ：58μT」らしいので、それほど外れた値ではないと思うのですが。
あと、各軸μTから方位を算出したかったのですが、そこも手が回りませんでした。
次の課題ですかね。
まとめてクラスにする
今までの結果をまとめてクラス化します。
#!/usr/bin/python -u
# -*- coding: utf-8 -*-
import smbus
import time
# Strawberry Linux社の「MPU-9250」からI2Cでデータを取得するクラス(python 2)
# https://strawberry-linux.com/catalog/items?code=12250
#
# 2016-05-03 Boyaki Machine
class SL_MPU9250:
# 定数宣言
REG_PWR_MGMT_1 = 0x6B
REG_INT_PIN_CFG = 0x37
REG_GYRO_CONFIG = 0x1B
REG_ACCEL_CONFIG1 = 0x1C
REG_ACCEL_CONFIG2 = 0x1D
MAG_MODE_POWERDOWN = 0 # 磁気センサpower down
MAG_MODE_SERIAL_1 = 1 # 磁気センサ8Hz連続測定モード
MAG_MODE_SERIAL_2 = 2 # 磁気センサ100Hz連続測定モード
MAG_MODE_SINGLE = 3 # 磁気センサ単発測定モード
MAG_MODE_EX_TRIGER = 4 # 磁気センサ外部トリガ測定モード
MAG_MODE_SELF_TEST = 5 # 磁気センサセルフテストモード
MAG_ACCESS = False # 磁気センサへのアクセス可否
MAG_MODE = 0 # 磁気センサモード
MAG_BIT = 14 # 磁気センサが出力するbit数
offsetRoomTemp = 0
tempSensitivity = 333.87
gyroRange = 250 # 'dps' 00:250, 01:500, 10:1000, 11:2000
accelRange = 2 # 'g' 00:±2, 01:±4, 10:±8, 11:±16
magRange = 4912 # 'μT'
offsetAccelX = 0.0
offsetAccelY = 0.0
offsetAccelZ = 0.0
offsetGyroX = 0.0
offsetGyroY = 0.0
offsetGyroZ = 0.0
# コンストラクタ
def __init__(self, address, channel):
self.address = address
self.channel = channel
self.bus = smbus.SMBus(self.channel)
self.addrAK8963 = 0x0C
# Sensor initialization
self.resetRegister()
self.powerWakeUp()
self.gyroCoefficient = self.gyroRange / float(0x8000) # センシングされたDecimal値をdpsに変換する係数
self.accelCoefficient = self.accelRange / float(0x8000) # センシングされたDecimal値をgに変換する係数
self.magCoefficient16 = self.magRange / 32760.0 # センシングされたDecimal値をμTに変換する係数(16bit時)
self.magCoefficient14 = self.magRange / 8190.0 # センシングされたDecimal値をμTに変換する係数(14bit時)
# レジスタを初期設定に戻します。
def resetRegister(self):
if self.MAG_ACCESS == True:
self.bus.write_i2c_block_data(self.addrAK8963, 0x0B, [0x01])
self.bus.write_i2c_block_data(self.address, 0x6B, [0x80])
self.MAG_ACCESS = False
time.sleep(0.1)
# レジスタをセンシング可能な状態にします。
def powerWakeUp(self):
# PWR_MGMT_1をクリア
self.bus.write_i2c_block_data(self.address, self.REG_PWR_MGMT_1, [0x00])
time.sleep(0.1)
# I2Cで磁気センサ機能(AK8963)へアクセスできるようにする(BYPASS_EN=1)
self.bus.write_i2c_block_data(self.address, self.REG_INT_PIN_CFG, [0x02])
self.MAG_ACCESS = True
time.sleep(0.1)
# 磁気センサのレジスタを設定する
def setMagRegister(self, _mode, _bit):
if self.MAG_ACCESS == False:
# 磁気センサへのアクセスが有効になっていないので例外を上げる
raise Exception('001 Access to a sensor is invalid.')
_writeData = 0x00
# 測定モードの設定
if _mode=='8Hz': # 連続測定モード１
_writeData = 0x02
self.MAG_MODE = self.MAG_MODE_SERIAL_1
elif _mode=='100Hz': # 連続測定モード２
_writeData = 0x06
self.MAG_MODE = self.MAG_MODE_SERIAL_2
elif _mode=='POWER_DOWN': # パワーダウンモード
_writeData = 0x00
self.MAG_MODE = self.MAG_MODE_POWERDOWN
elif _mode=='EX_TRIGER': # 外部トリガ測定モード
_writeData = 0x04
self.MAG_MODE = self.MAG_MODE_EX_TRIGER
elif _mode=='SELF_TEST': # セルフテストモード
_writeData = 0x08
self.MAG_MODE = self.MAG_MODE_SELF_TEST
else: # _mode='SINGLE' # 単発測定モード
_writeData = 0x01
self.MAG_MODE = self.MAG_MODE_SINGLE
# 出力するbit数
if _bit=='14bit': # 14bit出力
_writeData = _writeData | 0x00
self.MAG_BIT = 14
else: # _bit='16bit' # 16bit 出力
_writeData = _writeData | 0x10
self.MAG_BIT = 16
self.bus.write_i2c_block_data(self.addrAK8963, 0x0A, [_writeData])
# 加速度の測定レンジを設定します。広レンジでは測定粒度が荒くなります。
# val = 16, 8, 4, 2(default)
def setAccelRange(self, val, _calibration=False):
# ±2g (00), ±4g (01), ±8g (10), ±16g (11)
if val==16 :
self.accelRange = 16
_data = 0x18
elif val==8 :
self.accelRange = 8
_data = 0x10
elif val==4 :
self.accelRange = 4
_data = 0x08
else:
self.accelRange = 2
_data = 0x00
self.bus.write_i2c_block_data(self.address, self.REG_ACCEL_CONFIG1, [_data])
self.accelCoefficient = self.accelRange / float(0x8000)
time.sleep(0.1)
# オフセット値をリセット(過去のオフセット値が引き継がれないように)
self.offsetAccelX = 0
self.offsetAccelY = 0
self.offsetAccelZ = 0
# 本当はCalibrationしたほうが良いと思うけれど、時間もかかるし。
if _calibration == True:
self.calibAccel(1000)
return
# ジャイロの測定レンジを設定します。広レンジでは測定粒度が荒くなります。
# val= 2000, 1000, 500, 250(default)
def setGyroRange(self, val, _calibration=False):
if val==2000:
self.gyroRange = 2000
_data = 0x18
elif val==1000:
self.gyroRange = 1000
_data = 0x10
elif val==500:
self.gyroRange = 500
_data = 0x08
else:
self.gyroRange = 250
_data = 0x00
self.bus.write_i2c_block_data(self.address, self.REG_GYRO_CONFIG, [_data])
self.gyroCoefficient = self.gyroRange / float(0x8000)
time.sleep(0.1)
# オフセット値をリセット(過去のオフセット値が引き継がれないように)
self.offsetGyroX = 0
self.offsetGyroY = 0
self.offsetGyroZ = 0
# 本当はCalibrationしたほうが良いのだが、時間もかかるし。
if _calibration == True:
self.calibGyro(1000)
return
# 加速度センサのLowPassFilterを設定します。
# def setAccelLowPassFilter(self,val):
#センサからのデータはそのまま使おうとするとunsignedとして扱われるため、signedに変換(16ビット限定）
def u2s(self,unsigneddata):
if unsigneddata & (0x01 << 15) :
return -1 * ((unsigneddata ^ 0xffff) + 1)
return unsigneddata
# 加速度値を取得します
def getAccel(self):
data = self.bus.read_i2c_block_data(self.address, 0x3B ,6)
rawX = self.accelCoefficient * self.u2s(data[0] << 8 | data[1]) + self.offsetAccelX
rawY = self.accelCoefficient * self.u2s(data[2] << 8 | data[3]) + self.offsetAccelY
rawZ = self.accelCoefficient * self.u2s(data[4] << 8 | data[5]) + self.offsetAccelZ
return rawX, rawY, rawZ
# ジャイロ値を取得します。
def getGyro(self):
data = self.bus.read_i2c_block_data(self.address, 0x43 ,6)
rawX = self.gyroCoefficient * self.u2s(data[0] << 8 | data[1]) + self.offsetGyroX
rawY = self.gyroCoefficient * self.u2s(data[2] << 8 | data[3]) + self.offsetGyroY
rawZ = self.gyroCoefficient * self.u2s(data[4] << 8 | data[5]) + self.offsetGyroZ
return rawX, rawY, rawZ
def getMag(self):
if self.MAG_ACCESS == False:
# 磁気センサが有効ではない。
raise Exception('002 Access to a sensor is invalid.')
# 事前処理
if self.MAG_MODE==self.MAG_MODE_SINGLE:
# 単発測定モードは測定終了と同時にPower Downになるので、もう一度モードを変更する
if self.MAG_BIT==14: # 14bit出力
_writeData = 0x01
else: # 16bit 出力
_writeData = 0x11
self.bus.write_i2c_block_data(self.addrAK8963, 0x0A, [_writeData])
time.sleep(0.01)
elif self.MAG_MODE==self.MAG_MODE_SERIAL_1 or self.MAG_MODE==self.MAG_MODE_SERIAL_2:
status = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
if (status[0] & 0x02) == 0x02:
# データオーバーランがあるので再度センシング
self.bus.read_i2c_block_data(self.addrAK8963, 0x09 ,1)
elif self.MAG_MODE==self.MAG_MODE_EX_TRIGER:
# 未実装
return
elif self.MAG_MODE==self.MAG_MODE_POWERDOWN:
raise Exception('003 Mag sensor power down')
# ST1レジスタを確認してデータ読み出しが可能か確認する。
status = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
while (status[0] & 0x01) != 0x01:
# データレディ状態まで待つ
time.sleep(0.01)
status = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
# データ読み出し
data = self.bus.read_i2c_block_data(self.addrAK8963, 0x03 ,7)
rawX = self.u2s(data[1] << 8 | data[0]) # 下位bitが先
rawY = self.u2s(data[3] << 8 | data[2]) # 下位bitが先
rawZ = self.u2s(data[5] << 8 | data[4]) # 下位bitが先
st2 = data[6]
# オーバーフローチェック
if (st2 & 0x08) == 0x08:
# オーバーフローのため正しい値が得られていない
raise Exception('004 Mag sensor over flow')
# μTへの変換
if self.MAG_BIT==16: # 16bit出力の時
rawX = rawX * self.magCoefficient16
rawY = rawY * self.magCoefficient16
rawZ = rawZ * self.magCoefficient16
else: # 14bit出力の時
rawX = rawX * self.magCoefficient14
rawY = rawY * self.magCoefficient14
rawZ = rawZ * self.magCoefficient14
return rawX, rawY, rawZ
def getTemp(self):
data = self.bus.read_i2c_block_data(self.address, 0x65 ,2)
raw = data[0] << 8 | data[1]
return ((raw - self.offsetRoomTemp) / self.tempSensitivity) + 21
def selfTestMag(self):
print "start mag sensor self test"
self.setMagRegister('SELF_TEST','16bit')
self.bus.write_i2c_block_data(self.addrAK8963, 0x0C, [0x40])
time.sleep(1.0)
data = self.getMag()
print data
self.bus.write_i2c_block_data(self.addrAK8963, 0x0C, [0x00])
self.setMagRegister('POWER_DOWN','16bit')
time.sleep(1.0)
print "end mag sensor self test"
return
# 加速度センサを較正する
# 本当は緯度、高度、地形なども考慮する必要があるとは思うが、簡略で。
# z軸方向に正しく重力がかかっており、重力以外の加速度が発生していない前提
def calibAccel(self, _count=1000):
print "Accel calibration start"
_sum = [0,0,0]
# 実データのサンプルを取る
for _i in range(_count):
_data = self.getAccel()
_sum[0] += _data[0]
_sum[1] += _data[1]
_sum[2] += _data[2]
# 平均値をオフセットにする
self.offsetAccelX = -1.0 * _sum[0] / _count
self.offsetAccelY = -1.0 * _sum[1] / _count
self.offsetAccelZ = -1.0 * ((_sum[2] / _count ) - 1.0) # 重力分を差し引く
# オフセット値をレジスタに登録したいけれど、動作がわからないので実装保留
print "Accel calibration complete"
return self.offsetAccelX, self.offsetAccelY, self.offsetAccelZ
# ジャイロセンサを較正する
# 各軸に回転が発生していない前提
def calibGyro(self, _count=1000):
print "Gyro calibration start"
_sum = [0,0,0]
# 実データのサンプルを取る
for _i in range(_count):
_data = self.getGyro()
_sum[0] += _data[0]
_sum[1] += _data[1]
_sum[2] += _data[2]
# 平均値をオフセットにする
self.offsetGyroX = -1.0 * _sum[0] / _count
self.offsetGyroY = -1.0 * _sum[1] / _count
self.offsetGyroZ = -1.0 * _sum[2] / _count
# オフセット値をレジスタに登録したいけれど、動作がわからないので実装保留
print "Gyro calibration complete"
return self.offsetGyroX, self.offsetGyroY, self.offsetGyroZ
if __name__ == "__main__":
sensor = SL_MPU9250(0x68,1)
sensor.resetRegister()
sensor.powerWakeUp()
sensor.setAccelRange(8,True)
sensor.setGyroRange(1000,True)
sensor.setMagRegister('100Hz','16bit')
# sensor.selfTestMag()
while True:
now = time.time()
acc = sensor.getAccel()
gyr = sensor.getGyro()
mag = sensor.getMag()
print "%+8.7f" % acc[0] + " ",
print "%+8.7f" % acc[1] + " ",
print "%+8.7f" % acc[2] + " ",
print " | ",
print "%+8.7f" % gyr[0] + " ",
print "%+8.7f" % gyr[1] + " ",
print "%+8.7f" % gyr[2] + " ",
print " | ",
print "%+8.7f" % mag[0] + " ",
print "%+8.7f" % mag[1] + " ",
print "%+8.7f" % mag[2]
sleepTime = 0.1 - (time.time() - now)
if sleepTime < 0.0:
continue
time.sleep(sleepTime)
課題とか
上記クラスを使えばとりあえずデータを連続して取得することはできると思います。ただ、より高精度なセンシングを実現するには、チップについているオフセットや、Low pass filter機能などの活用が必要かと思います。今後時間が取れれば試してみたいです。
上記クラスを使ったサンプルソースで、加速度・角速度のみを連続センシングしたところ、100Hzは問題なく行えましたが、200Hzになると若干遅れがちでした。ここらへんも実装の工夫を行うことで、もう少し高頻度のセンシングを可能とできるのではないかと思います。
raspiで使う分にはあまり気にならないかもしれませんが、高頻度・高精度でセンシングを行うと電力消費も大きくなりますので、実際の利用に合わせて適切なモード選択が必要ですね。どのモードがどれくらいの電力消費か出してみたいですが..これは時間がかかりそうだからやらないかなぁ...
お役に立てば幸いです。