LoginSignup
2
2

More than 1 year has passed since last update.

@pond-e(pond 6814)

ラズパイで9軸センサmpu9250を使う方法

Last updated at Posted at 2021-11-21

目次

開発環境

RaspberryPi Zero W
OS : 2021-05-07-raspios-buster-armhf
python 3.7

ラズパイへの接続方法

Raspberry PiはI2C通信用のピンを備えています
I2Cとは、マイコン間で主に使用される通信方式の一つです
MPU9250はI2C通信で情報をやり取りできるので、Raspberry PiとMPU9250をI2C通信できるように接続します

2021-11-21 (3).png

I2Cの有効化

Raspberry PiのI2Cはデフォルトでは無効になっているので、設定を変更して有効にします
以下のコマンドを使用してラズパイの設定を変更します

raspi-config

Screenshot from 2018-04-29 11-56-31.png

Interfacing Options -> I2C -> Yes を選択してI2Cを有効化します
設定できたら、左右矢印でFinishを選択して、エンターキーを押して、raspi-configを終わります

コードを動かすための前準備

PythonからI2Cをコントロールするためのライブラリ「wiringpi2」のインストール
$ sudo pip install wiringpi2
データをファイルに書出すので「mpu9250_logs.csv」の空ファイルを準備する
※この例ではユーザーネームがpiであることが前提ですが適宜自分のユーザー名に読み替えてください

$ mkdir /home/pi/data
$ sudo touch /home/pi/data/mpu9250_logs.csv
$ sudo chown  pi /home/pi/data/mpu9250_logs.csv

コード

関数ごとの役割はコード上にコメントアウトで書いてあるので適宜見てください

mpu9250.py
#!/usr/bin/python3 -u
# -*- coding: utf-8 -*-

############################################################
# Written by pond-e
#
# Environment:  Python 3.4.2
#               OS: Raspi OS
#               on Raspberry Pi Zero W
# Used sensor: MPU-9250(Strawberry Linux)
#
# PythonからI2C をコントロールするためのライブラリ「wiringpi2」のインストール必要。
#    $ sudo pip install wiringpi2
#
# データをファイルに書出すので「mpu9250_logs.csv」の空ファイルを準備する
# pi@raspberrypi ~ $ sudo touch mpu9250_logs.csv
# pi@raspberrypi ~ $ sudo chown  pi mpu9250_logs.csv
############################################################
import sys  # sysモジュールの呼び出し
import wiringpi as wi  # wiringPiモジュールの呼び出し
import time  # timeライブラリの呼び出し
import datetime  # datetimeモジュールの呼び出し
import os

# データ計測時間は　SAMPLING_TIME x TIMES
SAMPLING_TIME = 0.1  # データ取得の時間間隔[sec]
TIMES = 100  # データの計測回数

wi.wiringPiSetup()  # wiringPiの初期化
i2c = wi.I2C()  # i2cの初期化

address = 0x68
addrAK8963 = 0x0C  # 磁気センサAK8963 アドレス
mpu9250 = i2c.setup(address)  # i2cアドレス0x68番地をmpu9250として設定(アドレスは$sudo i2cdetect 1で見られる)
AK8963 = i2c.setup(addrAK8963)
gyroRange = 1000  # 250, 500, 1000, 2000　'dps'から選択
accelRange = 8  # +-2, +-4, +-8, +-16 'g'から選択
magRange = 4912  # 'μT'

# センサ定数
REG_PWR_MGMT_1 = 0x6B
REG_INT_PIN_CFG = 0x37
REG_ACCEL_CONFIG1 = 0x1C
REG_ACCEL_CONFIG2 = 0x1D
REG_GYRO_CONFIG = 0x1B

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 = 16  # 磁気センサが出力するbit数

# オフセット用変数
offsetAccelX = 0
offsetAccelY = 0
offsetAccelZ = 0
offsetGyroX = 0
offsetGyroY = 0
offsetGyroZ = 0


# レジスタを初期設定に戻す。
def resetRegister():
    global MAG_ACCESS
    if MAG_ACCESS == True:
        i2c.writeReg8(AK8963, 0x0B, 0x01)
    i2c.writeReg8(mpu9250, 0x6B, 0x80)
    MAG_ACCESS = False
    time.sleep(0.1)


# センシング可能な状態にする。
def powerWakeUp():
    # PWR_MGMT_1をクリア
    i2c.writeReg8(mpu9250, REG_PWR_MGMT_1, 0x00)
    time.sleep(0.1)
    # I2Cで磁気センサ機能(AK8963)へアクセスできるようにする(BYPASS_EN=1)
    i2c.writeReg8(mpu9250, REG_INT_PIN_CFG, 0x02)
    global MAG_ACCESS
    MAG_ACCESS = True
    time.sleep(0.1)


# 加速度の測定レンジを設定
# val = 16, 8, 4, 2(default)
val = 8


def setAccelRange(val, _calibration=False):
    # +-2g (00), +-4g (01), +-8g (10), +-16g (11)
    if val == 16:
        accelRange = 16
        _data = 0x18
    elif val == 8:
        accelRange = 8
        _data = 0x10
    elif val == 4:
        accelRange = 4
        _data = 0x08
    else:
        accelRange = 2
        _data = 0x00
    print("set accelRange=%d [g]" % accelRange)
    i2c.writeReg8(mpu9250, REG_ACCEL_CONFIG1, _data)
    accelCoefficient = accelRange / float(0x8000)
    time.sleep(0.1)

    # オフセット値をリセット
    # offsetAccelX       = 0
    # offsetAccelY       = 0
    # offsetAccelZ       = 0

    # Calibration
    if _calibration == True:
        calibAccel(1000)
    return

    # ジャイロの測定レンジを設定します。
    # val= 2000, 1000, 500, 250(default)


def setGyroRange(val, _calibration=False):
    if val == 2000:
        gyroRange = 2000
        _data = 0x18
    elif val == 1000:
        gyroRange = 1000
        _data = 0x10
    elif val == 500:
        gyroRange = 500
        _data = 0x08
    else:
        gyroRange = 250
        _data = 0x00
    print("set gyroRange=%d [dps]" % gyroRange)
    i2c.writeReg8(mpu9250, REG_GYRO_CONFIG, _data)
    gyroCoefficient = gyroRange / float(0x8000)
    time.sleep(0.1)

    # Reset offset value (so that the past offset value is not inherited)
    # offsetGyroX        = 0
    # offsetGyroY        = 0
    # offsetGyroZ        = 0

    # Calibration
    if _calibration == True:
        calibGyro(1000)
    return


# 磁気センサのレジスタを設定する
def setMagRegister(_mode, _bit):
    global MAG_ACCESS
    global MAG_MODE
    if MAG_ACCESS == False:
        # 磁気センサへのアクセスが有効になっていない場合は例外
        raise Exception('001 Access to a sensor is invalid.')

    _writeData = 0x00
    # 測定モードの設定
    if _mode == '8Hz':  # Continuous measurement mode 1
        _writeData = 0x02
        MAG_MODE = MAG_MODE_SERIAL_1
    elif _mode == '100Hz':  # Continuous measurement mode 2
        _writeData = 0x06
        MAG_MODE = MAG_MODE_SERIAL_2
    elif _mode == 'POWER_DOWN':  # Power down mode
        _writeData = 0x00
        MAG_MODE = MAG_MODE_POWERDOWN
    elif _mode == 'EX_TRIGER':  # Trigger measurement mode
        _writeData = 0x04
        MAG_MODE = MAG_MODE_EX_TRIGER
    elif _mode == 'SELF_TEST':  # self test mode
        _writeData = 0x08
        MAG_MODE = MAG_MODE_SELF_TEST
    else:  # _mode='SINGLE'    # single measurment mode
        _writeData = 0x01
        MAG_MODE = MAG_MODE_SINGLE

    # 出力するbit数
    if _bit == '14bit':  # output 14bit
        _writeData = _writeData | 0x00
        MAG_BIT = 14
    else:  # _bit='16bit'      # output 16bit
        _writeData = _writeData | 0x10
        MAG_BIT = 16
    print("set MAG_MODE=%s, %d bit" % (_mode, MAG_BIT))
    i2c.writeReg8(AK8963, 0x0A, _writeData)


# センサからのデータはそのまま使おうとするとunsignedとして扱われるため、signedに変換(16ビット限定）
def u2s(unsigneddata):
    if unsigneddata & (0x01 << 15):
        return -1 * ((unsigneddata ^ 0xffff) + 1)
    return unsigneddata


# 加速度値を取得
def getAccel():
    ACCEL_XOUT_H = i2c.readReg8(mpu9250, 0x3B)
    ACCEL_XOUT_L = i2c.readReg8(mpu9250, 0x3C)
    ACCEL_YOUT_H = i2c.readReg8(mpu9250, 0x3D)
    ACCEL_YOUT_L = i2c.readReg8(mpu9250, 0x3E)
    ACCEL_ZOUT_H = i2c.readReg8(mpu9250, 0x3F)
    ACCEL_ZOUT_L = i2c.readReg8(mpu9250, 0x40)
    rawX = accelCoefficient * u2s(ACCEL_XOUT_H << 8 | ACCEL_XOUT_L) + offsetAccelX
    rawY = accelCoefficient * u2s(ACCEL_YOUT_H << 8 | ACCEL_YOUT_L) + offsetAccelY
    rawZ = accelCoefficient * u2s(ACCEL_ZOUT_H << 8 | ACCEL_ZOUT_L) + offsetAccelZ
    # data    = i2c.readReg8(address, 0x3B )
    # print "getaccell data=%d"%data
    # rawX    = accelCoefficient * u2s(data[0] << 8 | data[1]) + offsetAccelX
    # rawY    = accelCoefficient * u2s(data[2] << 8 | data[3]) + offsetAccelY
    # rawZ    = accelCoefficient * u2s(data[4] << 8 | data[5]) + offsetAccelZ
    return rawX, rawY, rawZ


# ジャイロ値を取得
def getGyro():
    GYRO_XOUT_H = i2c.readReg8(mpu9250, 0x43)
    GYRO_XOUT_L = i2c.readReg8(mpu9250, 0x44)
    GYRO_YOUT_H = i2c.readReg8(mpu9250, 0x45)
    GYRO_YOUT_L = i2c.readReg8(mpu9250, 0x46)
    GYRO_ZOUT_H = i2c.readReg8(mpu9250, 0x47)
    GYRO_ZOUT_L = i2c.readReg8(mpu9250, 0x48)
    rawX = gyroCoefficient * u2s(GYRO_XOUT_H << 8 | GYRO_XOUT_L) + offsetGyroX
    rawY = gyroCoefficient * u2s(GYRO_YOUT_H << 8 | GYRO_YOUT_L) + offsetGyroY
    rawZ = gyroCoefficient * u2s(GYRO_ZOUT_H << 8 | GYRO_ZOUT_L) + offsetGyroZ
    # data    =  i2c.readReg8(address, 0x43 )
    # 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
    return rawX, rawY, rawZ


# 磁気値を取得
def getMag():
    global MAG_ACCESS
    if MAG_ACCESS == False:
        # 磁気センサへのアクセスが有効になっていない場合は例外
        raise Exception('002 Access to a sensor is invalid.')

    # 事前処理
    global MAG_MODE
    if MAG_MODE == MAG_MODE_SINGLE:
        # 単発測定モードは測定終了と同時にPower Downになるので、もう一度モードを変更する
        if MAG_BIT == 14:  # output 14bit
            _writeData = 0x01
        else:  # output 16bit
            _writeData = 0x11
        i2c.writeReg8(AK8963, 0x0A, _writeData)
        time.sleep(0.01)

    elif MAG_MODE == MAG_MODE_SERIAL_1 or MAG_MODE == MAG_MODE_SERIAL_2:
        status = i2c.readReg8(AK8963, 0x02)
        if (status & 0x02) == 0x02:
            # if (status[0] & 0x02) == 0x02:
            # データオーバーランがあるので再度センシング
            i2c.readReg8(AK8963, 0x09)

    elif MAG_MODE == MAG_MODE_EX_TRIGER:
        # 未実装
        return

    elif MAG_MODE == MAG_MODE_POWERDOWN:
        raise Exception('003 Mag sensor power down')

    # ST1レジスタを確認してデータ読み出しが可能か確認する
    status = i2c.readReg8(AK8963, 0x02)
    while (status & 0x01) != 0x01:
        # while (status[0] & 0x01) != 0x01:
        # Wait until data ready state.
        time.sleep(0.01)
        status = i2c.readReg8(AK8963, 0x02)

    # データ読み出し
    MAG_XOUT_L = i2c.readReg8(AK8963, 0x03)
    MAG_XOUT_H = i2c.readReg8(AK8963, 0x04)
    MAG_YOUT_L = i2c.readReg8(AK8963, 0x05)
    MAG_YOUT_H = i2c.readReg8(AK8963, 0x06)
    MAG_ZOUT_L = i2c.readReg8(AK8963, 0x07)
    MAG_ZOUT_H = i2c.readReg8(AK8963, 0x08)
    MAG_OF = i2c.readReg8(AK8963, 0x09)
    rawX = u2s(MAG_XOUT_H << 8 | MAG_XOUT_L)
    rawY = u2s(MAG_YOUT_H << 8 | MAG_YOUT_L)
    rawZ = u2s(MAG_ZOUT_H << 8 | MAG_ZOUT_L)
    st2 = MAG_OF
    # data    = i2c.readReg8(addrAK8963, 0x03 ,7)
    # rawX    = u2s(data[1] << 8 | data[0])  # Lower bit is ahead.
    # rawY    = u2s(data[3] << 8 | data[2])  # Lower bit is ahead.
    # rawZ    = u2s(data[5] << 8 | data[4])  # Lower bit is ahead.
    # st2     = data[6]

    # オーバーフローチェック
    if (st2 & 0x08) == 0x08:
        # オーバーフローのため正しい値が得られていない
        raise Exception('004 Mag sensor over flow')

    # μTへの変換
    if MAG_BIT == 16:  # output 16bit
        rawX = rawX * magCoefficient16
        rawY = rawY * magCoefficient16
        rawZ = rawZ * magCoefficient16
    else:  # output 14bit
        rawX = rawX * magCoefficient14
        rawY = rawY * magCoefficient14
        rawZ = rawZ * magCoefficient14

    return rawX, rawY, rawZ


# 加速度センサを較正する
# 本当は緯度、高度、地形なども考慮する必要があるとは思うが、簡略で。
# z軸方向に正しく重力がかかっており、重力以外の加速度が発生していない前提
def calibAccel(_count=1000):
    print("Accel calibration start")
    _sum = [0, 0, 0]

    # データのサンプルを取る
    for _i in range(_count):
        _data = getAccel()
        _sum[0] += _data[0]
        _sum[1] += _data[1]
        _sum[2] += _data[2]

        # 平均値をオフセットにする
    global offsetAccelX, offsetAccelY, offsetAccelZ
    offsetAccelX = -1.0 * _sum[0] / _count
    offsetAccelY = -1.0 * _sum[1] / _count
    offsetAccelZ = -1.0 * ((_sum[2] / _count) - 1.0)  # 重力分を差し引く

    # I want to register an offset value in a register. But I do not know the behavior, so I will put it on hold.
    print("Accel calibration complete")
    return offsetAccelX, offsetAccelY, offsetAccelZ


# ジャイロセンサを較正する
# 各軸に回転が発生していない前提
def calibGyro(_count=1000):
    print("Gyro calibration start")
    _sum = [0, 0, 0]

    # データのサンプルを取る
    for _i in range(_count):
        _data = getGyro()
        _sum[0] += _data[0]
        _sum[1] += _data[1]
        _sum[2] += _data[2]

    # 平均値をオフセットにする
    global offsetGyroX, offsetGyroY, offsetGyroZ
    offsetGyroX = -1.0 * _sum[0] / _count
    offsetGyroY = -1.0 * _sum[1] / _count
    offsetGyroZ = -1.0 * _sum[2] / _count

    # I want to register an offset value in a register. But I do not know the behavior, so I will put it on hold.
    print("Gyro calibration complete")
    return offsetGyroX, offsetGyroY, offsetGyroZ


if __name__ == '__main__':

    # bus     = smbus.SMBus(1)
    resetRegister()
    powerWakeUp()
    gyroCoefficient = gyroRange / float(0x8000)  # coefficient : sensed decimal val to dps val.
    accelCoefficient = accelRange / float(0x8000)  # coefficient : sensed decimal val to g val.
    magCoefficient16 = magRange / 32760.0  # confficient : sensed decimal val to μT val (16bit)
    magCoefficient14 = magRange / 8190.0  # confficient : sensed decimal val to μT val (14bit)
    setAccelRange(accelRange, False) # Trueにすると補正できる
    setGyroRange(gyroRange, False) # Trueにすると補正できる
    setMagRegister('100Hz', '16bit')
    # ファイルへ書出し準備
    now = datetime.datetime.now()
    # 現在時刻を織り込んだファイル名を生成
    fmt_name = "/home/pi/data/mpu9250wpi_logs_{0:%Y%m%d-%H%M%S}.csv".format(now)
    f_mpu9250 = open(fmt_name, 'w')  # 書き込みファイル
    # f_mpu9250= open('home/pi/data/mpu9250wpi_logs.csv', 'w')    #書き込みファイル
    value = "yyyy-mm-dd hh:mm:ss.mmmmmm, x[g],y[g],z[g],x[dps],y[dps],z[dps],x[uT],y[uT],z[uT]"  # header行への書き込み内容
    f_mpu9250.write(value + "\n")  # header行をファイル出力
    # while True:
    for _i in range(TIMES):
        try:
            date = datetime.datetime.now()  # now()メソッドで現在日付・時刻のdatetime型データの変数を取得 世界時：UTCnow
            now = time.time()  # 現在時刻の取得
            acc = getAccel()  # 加速度値の取得
            gyr = getGyro()  # ジャイロ値の取得
            mag = getMag()  # 磁気値の取得
            # データの表示
            # ファイルへ書出し
            value = "%s,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f" % (
            date, acc[0], acc[1], acc[2], gyr[0], gyr[1], gyr[2], mag[0], mag[1], mag[2])  # 時間、xyz軸回りの加速度
            print(value)
            f_mpu9250.write(value + "\n")  # ファイルを出力
            # 指定秒数の一時停止
            sleepTime = SAMPLING_TIME - (time.time() - now)
            if sleepTime < 0.0:
                continue
            time.sleep(sleepTime)
        except KeyboardInterrupt:
            break
    f_mpu9250.close()  # 書き込みファイルを閉じる

main関数の解説

  • まず初期設定をする
  • /home/pi/data/ にmpu9250wpi_logs_{日時}.csvというファイルを生成してそこにデータを書き込んでいきます
  • データは加速度(x, y, z)、角速度(x, y, z)、地磁気(x, y, z)の順に書き込まれます
  • それぞれ、acc、gyr、magという名前の配列に取得した値が入っているので9軸センサの値を使って何かをする場合はこの配列を用いましょう

応用

　9軸センサは物体の姿勢推定に使用できます
　まず、角速度の値を用いることで物体の大まかな角度を測定し、次に加速度センサでピッチ角とロール角の補正をします。最後に地磁気センサでヨー角の補正をする。といった感じです
　ただし、加速度センサは物体がランダムに動いていたら加速度の方向がぶれるので使いにくいし、地磁気は室内では機械や金属の影響を受けるので補正する際は測定環境に注意しましょう
　ここのサイトが参考になります https://qiita.com/Tanba28/items/5092c3e5e2c631b804f3

2
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
2