Raspberry Pi Zero WでMPU6050の加速度センサーの補正を行う

オフセットがかかっているのを補正する。

ソースコード

import smbus
import math
from time import sleep
import time
import sys
import traceback
import matplotlib.pyplot as plt
import datetime


#各種レジスタのアドレス
#----------------------------------------------------------------------------------
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
#----------------------------------------------------------------------------------

CAL_SIZE = 5000#キャリブレーションに使うデータ数
IGNORE_DATA_SIZE = 50#キャリブレーションを行う際に最初の方のデータは信頼性が無いため、何個のデータを無視するか設定

#変数/配列の初期化処理
#----------------------------------------------------------------------------------
data_x_list = []#データ格納用の配列作成
data_y_list = []
data_z_list = []

data_x_offset_list = []#データ格納用の配列作成(オフセット後)
data_y_offset_list = []
data_z_offset_list = []

bus = smbus.SMBus(1)
x_offset = 0#オフセット値の計算結果を入れる(計算前は0を入れる)
y_offset = 0#オフセット値の計算結果を入れる(計算前は0を入れる)
z_offset = 0#オフセット値の計算結果を入れる(計算前は0を入れる)

dt_now = datetime.datetime.now()
file = open(str(dt_now)+"calc_data.txt","w",encoding="utf-8")#統計データ格納用のファイル
#----------------------------------------------------------------------------------

#デバイスの初期化処理
#----------------------------------------------------------------------------------
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, CONFIG, 0x06)#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]

def getAccel_offset(): #オフセット分の考慮した場合のデータ
    x = (read_word_sensor(ACCEL_XOUT)+x_offset)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
    y = (read_word_sensor(ACCEL_YOUT)+y_offset)/ 16384.0
    z = (read_word_sensor(ACCEL_ZOUT)+z_offset)/ 16384.0
    return [x, y, z]

def calc_offset_acc():#平面に置いた場合のオフセット計算処理
    global x_offset
    global y_offset
    global z_offset
    for i in range(CAL_SIZE):
        if i>IGNORE_DATA_SIZE:#最初のデータは信憑性が無いため、無視
            x_offset += -read_word_sensor(ACCEL_XOUT)#x,yは0が基準なので、0は省略している。
            y_offset += -read_word_sensor(ACCEL_YOUT)
            z_offset += 16384-read_word_sensor(ACCEL_ZOUT)#変換前の値で16384が1なので、16384を基準に計算する。
        else:#捨てデータは空読み
            read_word_sensor(ACCEL_XOUT)
            read_word_sensor(ACCEL_YOUT)
            read_word_sensor(ACCEL_ZOUT)

    x_offset =int(x_offset/(CAL_SIZE-IGNORE_DATA_SIZE)) #データの個数(無視したデータも考慮)で割ってオフセット分の平均を算出
    y_offset =int(y_offset/(CAL_SIZE-IGNORE_DATA_SIZE)) 
    z_offset =int(z_offset/(CAL_SIZE-IGNORE_DATA_SIZE)) 
    print("offset_calc_finish")        
    
calc_offset_acc()#オフセットの計算をする場合にこの関数をコールする。(一度計算すれば問題無し。計算が終わったらコメントアウト)
print("lecoding")
while True:
    try:
        ax, ay, az = getAccel()#加速度センサーの情報読み出し
        gx, gy, gz = getGyro()#ジャイロセンサーの情報読み出し
        ax_offset,ay_offset,az_offset = getAccel_offset()
        magnitude_a = math.sqrt(ax**2+ay**2+az**2)#加速度の大きさを算出
        temp = get_temp()#温度センサーの情報読み出し
        raw_ax = read_word(ACCEL_XOUT)
        raw_ay = read_word(ACCEL_YOUT)
        raw_az = read_word(ACCEL_ZOUT)
        
        # 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_RAW:{:4.3f},{:4.3f},{:4.3f},' .format(raw_ax, raw_ay, raw_az))    
        # print ('ACC_MAG:{:4.3f}'.format(magnitude_a))
        # print ('TEMP:{:4.3f}'.format(temp))
        data_x_list.append(ax)
        data_y_list.append(ay)
        data_z_list.append(az)
        data_x_offset_list.append(ax_offset)
        data_y_offset_list.append(ay_offset)
        data_z_offset_list.append(az_offset)


    except:#ctrl+Cでコードの実行を停止した場合にこちらの処理が実行(将来的にはCAN途絶したらこの処理に入るように変更予定)
        bus.close()
        del data_x_list[:50]#最初の方のデータが0なので除外
        del data_y_list[:50]
        del data_z_list[:50]
        del data_x_offset_list[:50]#最初の方のデータが0なので除外
        del data_y_offset_list[:50]
        del data_z_offset_list[:50]
        
        #現状ctrl+cでこの処理に入るので本当にエラーが起きた時用にエラーメッセージを出力
        #----------------------------------------------------------------------------------        
        print("Error Occur")
        print(traceback.format_exc())
        #----------------------------------------------------------------------------------
        
        
        #統計情報をファイルに出力
        #----------------------------------------------------------------------------------
        print("x_max",file=file)
        print(max(data_x_list),file=file)
        print("x_min",file=file)
        print(min(data_x_list),file=file)
        print("x_ave",file=file)
        print(sum(data_x_list)/len(data_x_list),file=file)
        print("y_max",file=file)
        print(max(data_y_list),file=file)
        print("y_min",file=file)
        print(min(data_y_list),file=file)
        print("y_ave",file=file)
        print(sum(data_y_list)/len(data_y_list),file=file)
        print("z_max",file=file)
        print(max(data_z_list),file=file)
        print("z_min",file=file)
        print(min(data_z_list),file=file)
        print("z_ave",file=file)
        print(sum(data_z_list)/len(data_z_list),file=file)

        print("x_offset_max",file=file)
        print(max(data_x_offset_list),file=file)
        print("x_offset_min",file=file)
        print(min(data_x_offset_list),file=file)
        print("x_offset_ave",file=file)
        print(sum(data_x_offset_list)/len(data_x_offset_list),file=file)
        print("y_offset_max",file=file)
        print(max(data_y_offset_list),file=file)
        print("y_offset_min",file=file)
        print(min(data_y_offset_list),file=file)
        print("y_offset_ave",file=file)
        print(sum(data_y_offset_list)/len(data_y_offset_list),file=file)
        print("z_offset_max",file=file)
        print(max(data_z_offset_list),file=file)
        print("z_offset_min",file=file)
        print(min(data_z_offset_list),file=file)
        print("z_offset_ave",file=file)
        print(sum(data_z_offset_list)/len(data_z_offset_list),file=file)

        print("OFFSET_X",file=file)
        print(x_offset,file=file)
        print("OFFSET_Y",file=file)
        print(y_offset,file=file)
        print("OFFSET_Z",file=file)
        print(z_offset,file=file)
        print("OFFSET_Z_cal",file=file)
        print(z_offset/16384,file=file)
        file.close()
        #----------------------------------------------------------------------------------

        #ロギングしたデータをプロット
        #----------------------------------------------------------------------------------
        time_list = range(len(data_x_list))
        time_list_offset = range(len(data_x_offset_list))        
        plt.plot(time_list,data_x_list,label="x_not_offset")
        plt.plot(time_list,data_y_list,label="y_not_offset")
        plt.plot(time_list_offset,data_x_offset_list,label="x_offset")
        plt.plot(time_list_offset,data_y_offset_list,label="y_offset")
        plt.legend()
        # plt.ylim(-0.015,0.015)
        plt.show()
        plt.plot(time_list,data_z_list,label="z_not_offset")
        plt.plot(time_list_offset,data_z_offset_list,label="z_offset")
        plt.legend()
        # plt.ylim(0.9,1.1)
        plt.show()
        sys.exit()
        #----------------------------------------------------------------------------------    
 

まとめ

関連記事