Raspberry Pi Zero Wで加速度センサーの情報を読み出す(MPU6050)

前回までで車両からのCAN情報を読み出せるようになりました。

今度は加速度センサーも購入してより詳しく車両の挙動を把握できるようにしていきます。

加速度センサーの準備

今回購入した加速度センサーはこちらです。



購入したものには加速度センサーだけでなく、ジャイロセンサーもついているみたいです。
そもそもジャイロセンサーが何物か知らなかったため下記サイトで勉強しました。

ジャイロセンサーがあれば角度の変化量を求められるので、車に取り付ければヨー/ロール/ピッチが分かりそうです。

ハードウェア接続構成

今回はCAN HATを接続している状態での接続です。

図1:CAN HATを接続した状態でのピン配[1]

CAN HATを付けることで一部ピン配が異なっている状態なので、CAN HATを付けてない人は注意してください。

今回はラズパイの3.3Vの電源/GND/SDA/SCLをMPU6050と接続します。
図2:接続図
最初ブレッドボード経由でMPU6050とラズパイを接続していたのですが、ブレッドボードを経由するとブレッドボードの寄生容量の影響か通信が不安定になってしまいました。

直接ジャンパー線で接続するようにしたら、通信が安定するようになったのでブレッドボードは使わないほうがいいです。

ソースコード

初めてこのようなセンサーを使用するのですがレジスタ設定とかがよくわかりませんでした。。。
今回のセンサーのレジスタなどについてはこちらの記事で分かりやすく書いてあったため、参考にしながら勉強しました。

また、参考記事に記載されているレジスターマップはこちらから確認できます。
I2C通信で使用したpythonモジュールの使い方はこちらを参照しました。

今回使用したコードは以下の通りです。

import smbus
import math
from time import sleep
import time
import sys

DEV_ADDR = 0x68
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
ACCEL_CONFIG = 0x1C

ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
TEMP_OUT = 0x41
GYRO_XOUT = 0x43
GYRO_YOUT = 0x45
GYRO_ZOUT = 0x47

PWR_MGMT_1 = 0x6b
PWR_MGMT_2 = 0x6c   

bus = smbus.SMBus(1)

#デバイスの初期化処理
#----------------------------------------------------------------------------------
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x80)#write_byte_data(int addr,char cmd,char val) 1:I2C通信対象のアドレス。今回は加速度センサのみなので68固定,2:レジスタのアドレス,3:値)0x80はデバイスリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x00)
bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x07)#FIFO/I2C_MST/SIG_CONDのリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x00)
#----------------------------------------------------------------------------------

#センサーの設定
#----------------------------------------------------------------------------------
bus.write_byte_data(DEV_ADDR, CONFIG, 0x00)#CONFIG
bus.write_byte_data(DEV_ADDR, GYRO_CONFIG, 0x00)#250°/s
bus.write_byte_data(DEV_ADDR, ACCEL_CONFIG, 0x00)#2g
#----------------------------------------------------------------------------------


def read_word(adr):#センサーの値が2バイトであらわされるため、2バイト分のデータを結合する関数
    high = bus.read_byte_data(DEV_ADDR, adr)#上位バイトのデータ
    low = bus.read_byte_data(DEV_ADDR, adr+1)#下位バイトのデータ
    val = (high << 8) + low#上位バイトのデータをビット1バイト分(8bit)左にシフトして、下位バイトのデータと足し合わせて1つのデータにしている。
    return val

def read_word_sensor(adr):
    val = read_word(adr)
    if (val >= 0x8000):  return -((65535 - val) + 1)#0x8000が最上位ビットが立っている状態なので、マイナス。もし、マイナス値だった場合は符号を反転させてる。
    else:  return val

def get_temp():
    temp = read_word_sensor(TEMP_OUT)
    x = temp / 340 + 36.53      # data sheet(register map)記載の計算式.
    return x

def getGyro():
    x = read_word_sensor(GYRO_XOUT)/ 131.0# data sheet(register map)記載の計算式.(250°を選択した場合)
    y = read_word_sensor(GYRO_YOUT)/ 131.0
    z = read_word_sensor(GYRO_ZOUT)/ 131.0
    return [x, y, z]


def getAccel(): 
    x = read_word_sensor(ACCEL_XOUT)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
    y= read_word_sensor(ACCEL_YOUT)/ 16384.0
    z= read_word_sensor(ACCEL_ZOUT)/ 16384.0
    return [x, y, z]


while True:
    try:
        ax, ay, az = getAccel()#加速度センサーの情報読み出し
        gx, gy, gz = getGyro()#ジャイロセンサーの情報読み出し
        magnitude_a = math.sqrt(ax**2+ay**2+az**2)#加速度の大きさを算出
        temp = get_temp()#温度センサーの情報読み出し
        print ('GYRO:{:4.3f},{:4.3f},{:4.3f},' .format(gx, gy, gz)) 
        print ('ACC:{:4.3f},{:4.3f},{:4.3f},' .format(ax, ay, az))    
        print ('ACC_MAG:{:4.3f}'.format(magnitude_a))
        print ('TEMP:{:4.3f}'.format(temp))
    except:
        bus.close()
        print("Error Occur")
        sys.exit()


もしプログラム実行時に下記エラーが起きた場合はsudoで実行してみてください。
PermissionError: [Errno 13] Permission denied

もし毎回sudoで実行するのが面倒でしたら下記サイトで紹介されている設定を試してください。

また、今回は加速度のレンジを±2Gとしたが、おそらく一般の車でサーキット走る程度なら十分なレンジだと思ってます。
理由:NSXの減速Gが1G、加速Gが0.8G、横Gが1Gとなっているため
(F1とかだと4.5G程度かかるっぽいのでレンジを広げる必要がありますが。。。)

実行結果

実行結果は次のようになりました。
図3:実行結果


こちらのデータはセンサー静止時のデータで、加速度の三軸合計が0.9 G程度で温度も室温相当を示しているので、おそらく問題なく読み出せてると思います。

※個人的には加速度の3軸合計が1G程度になると思っていたのですが、センサーを上下逆にしたら1.1 Gになったため、私が購入したセンサー少しオフセットが掛かっている気がします。。。

まとめ

MPU6050を使って加速度や角速度情報を読み出せるようになりました。
加速度情報やCAN情報を元にサーキットなどで使えるロガーが作れればと思ってます。

また、私が購入したセンサーにオフセットが掛かっているため、キャリブレーション方法も調べて追記したいと思います。

その他参考文献

【Arduino】MPU6050とI2C通信して、加速度とジャイロのデータを取得
(関数read_word_sensorの負のデータの扱いについてはこの記事が分かりやすかったです。)



0 件のコメント :

コメントを投稿