Processing math: 5%
ホーム / MPU6050 / python / プログラミング / ラズパイ /

MPU6050で現在の姿勢を推定する~プログラムの実装編~

2022/03/08 最終更新日:2022/11/05

MPU6050 python プログラミング ラズパイ

 前回のブログでMPU6050の加速度センサーのあたいから現在の角度を求めるための式を導出しました。(前回：MPU6050で現在の姿勢を推定する~3次元の回転についての勉強編~)

今回は前回の導出した式を元に実際に角度を求めていき、センサーの座標系をグローバル座標系に変換していきます。

目次[非表示]

現在の角度を求める

前回の記事でx軸周りの回転角\varphi及びy軸周りの回転角度\thetaは以下の式で求められることが分かりました。
(z軸周りの回転角\psiは加速度センサーの値から算出することはできないことに注意)
\begin{equation}\varphi = \arctan \left(\frac{a_y}{a_z}\right ) \end{equation}

\begin{equation}\theta = \arctan \left( \frac{-a_x}{\sqrt{a^2_y+a^2_z}} \right ) \end{equation}

この式を元に加速度から角度を求める関数を作成します。
今回はセンサーの値の平均値を求める関数と算出した平均値を元に角度を算出する関数を作りました。
def calc_average():#センサーの値の平均値を求めグローバル変数に入れる。
    global ax_average
    global ay_average
    global az_average
    
    for i in range(CAL_SIZE):
        if i>IGNORE_DATA_SIZE:#最初のデータは信憑性が無いため、無視
            x,y,z = getAccel_offset()
            ax_average += x
            ay_average += y
            az_average += z 
 
        else:#念のため空読み
            x,y,z = getAccel()
 
    ax_average = ax_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    ay_average = ay_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    az_average = az_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    print("ax_average")
    print(ax_average)
    print("ay_average")
    print(ay_average)
    print("az_average")
    print(az_average)
 
def calc_initial_angle():#センサーの平均値から現在の角度を求める(ラジアンで返す)
    global initial_phi
    global initial_theta
    if (ax_average == 0 ) and (ay_average == 0) and (az_average == 0):#もし平均値の計算が出来てなければ平均値を計算し、グローバル変数に値を格納する
        calc_average()  
    initial_phi = np.arctan(ay_average/az_average)
    initial_theta = np.arctan(-ax_average/(np.sqrt(ay_average**2+az_average**2)))
    print("phi")
    print( initial_phi)
    print("theta")
    print(initial_theta)
    print("phi_deg")
    print( np.rad2deg(initial_phi))
    print("theta_deg")
    print( np.rad2deg(initial_theta))
全体のコードは別途書きますが、こちらの関数だけ実行するとグローバル変数の定義やimportなど書いてないためエラーを吐いてしまうので注意して下さい。(もしこのまま使うなら適宜グローバル変数の定義を記載して下さい。)

動作確認結果

先ほどのコードを早速実行してみました。
水平に置いた場合の実行結果は以下の通りになりおおよそ合ってそうです。
水平方向においてセンサーの角度を求めた結果
次に\varphi方向に傾けた場合の実行結果も見てみます。
ラズパイZeroのカメラコネクタがあるせいできちんと90度作れなかったですが、だいたい80度くらいだったのであってそうです。
\varphi方向に90度傾けた結果

今度は\theta方向に立てた場合です。おおよそ90度くらいだったので問題なさそうです。
\theta方向に90度傾けた場合

補足になりますが最初センサーの読み値(1Gとかに変換する前の値)を使って算出していたのですが、うまいこと計算できませんでした。。。あとから考えれば当たり前ですが、前回角度の算出式を求めた際にZ軸方向に1Gをかかった場合で求めてたからですね。。。

角度の算出にセンサーの読み値を使った場合


求めた角度をつかってセンサ値を回転させる。

次に求めた角度を使ってセンサー値を回転させていきます。
計算式は前回の記事の(１３)式の逆行列を左からかけていったものになります。

\begin{equation}\begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix} = \begin{bmatrix}\cos\psi & -\sin\psi & 0\\ \sin\psi & \cos \psi & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}\cos\theta & 0 & \sin\theta \\  0 & 1 & 0 \\ -\sin\theta & 0 & \cos \theta\end{bmatrix}  \begin{bmatrix}1 & 0 & 0 \\  0 & \cos\varphi & -\sin\varphi \\ 0 & \sin\varphi& \cos \varphi\end{bmatrix}\begin{bmatrix}a_x\\ a_y \\a_z\end{bmatrix}\end{equation}

こちらもこのままでは動かないので適宜グローバル変数の定義などを追加してください。
  
def calc_rotation(ax, ay, az):
    global initial_Rxyz
    a_initial = np.array([[ax], [ay], [az]])#回転前の加速度の列ベクトルを作成
 
    Cp = np.cos(initial_phi)#phiのCos値(初期値の角度分回転)
    Sp = np.sin(initial_phi)#phiのSin値(初期値の角度分回転)
    Ct = np.cos(initial_theta)#thetaのcos値(初期値の角度分回転)
    St = np.sin(initial_theta)#thetaのsin値(初期値の角度分回転)
    Cps = 1 #psi方向のCos値(psi方向は初期値が求められないため、0度として計算)
    Sps = 0 #psi方向のSin値(psi方向は初期値が求められないため、0度として計算)
    Rx = np.matrix([[1,0,0],[0,Cp,-Sp],[0,Sp,Cp]])#x方向の回転行列
    Ry = np.matrix([[Ct,0,St],[0,1,0],[-St,0,Ct]])#y方向の回転行列
    Rz = np.matrix([[Cps,-Sps,0],[Sps,Cps,0],[0,0,1]])#z方向の回転行列
    initial_Rxyz = Rz@Ry@Rx#@で行列の演算が出来るとの事。X->Y->Zの順で回転させる
    
    a_rotate = initial_Rxyz@a_initial#初期状態の角度回転させた場合の値
    
    print("initial_Rxyz")
    print(initial_Rxyz)
    print("a_rotate")
    print(a_rotate)
#    print(a_rotate[0]*10**10)#テスト用。これを記述して一度実行したら何故か2回目実行時も問題無く表示されるように。。。
下記図に水平に配置した場合のセンサーの回転値を示します。
配列なので見ずらいですが、initial_Rxyzが回転行列で、a_rotateが回転後の加速度の配列となります。
こちらの値を見るとa_rotateの3行目(a_z)が1G近い値を示しているため問題なく値が出せてそうです。

水平の時の回転

ただ、x軸方向の値とy軸方向の値が何故か正しく表示されません。。。計算してみたところ
e^-12となっており、非常に小さい値のため表示されなかったのではないかと思います。
回転後のaxとayの値(上からax,ayの順です。)

(補足)Numpyで表示形式を指定する

調べてみたら恐らく表示形式がうまくいってないように思えたので、下記記事を元に表示形式を指数形式にする設定をプログラムの一番初めに記載しました。
np.set_printoptions(formatter={'float': '{:.8e}'.format})
(参考：NumPy配列ndarrayの表示形式（桁数や指数表記、0埋めなど）を指定)

表示形式を変えたら問題なく表示できてそうです。
表示形式を変えた場合


話が少しそれてしまいましたが、次に\varphi方向に90度傾けた場合の結果を見てみます。
90度傾けた場合でもa_rotateの値はz方向が1G近い値になっているため問題なさそうです。
varphi方向に90度傾けた場合


補足になりますが、傾けすぎてtheta方向にマイナス側で回転してしまった場合は
出力される値の符号が逆になってしまいます。設置する際はこのことを意識して設置したいと思います。(将来的にはソフトウェアで処理したいと思います。)

\varphi方向に90度傾けた場合(theta方向にプラスで傾いていた場合)

最後に\theta方向に立てた場合です。
こちらもa_rotateの値はz方向が1G近い値になってる為、計算自体はうまくできてそうです。


今回実装したソースコード

今回実装したソースコードを以下に示します。(不要な箇所はコメントアウトしてますが、整理できてないため後ほど整えます。。。)
import smbus
import math
from time import sleep
import time
import sys
import traceback
import matplotlib.pyplot as plt
import datetime
import numpy as np
 
np.set_printoptions(formatter={'float': '{:.8e}'.format})#値が小さい場合に上手く表示されない事象があったので、指数形式で表示するよう
#各種レジスタのアドレス
#----------------------------------------------------------------------------------
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#キャリブレーションを行う際に最初の方のデータは信頼性が無いため、何個のデータを無視するか設定
X_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
Y_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
Z_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
PLLOT_TIME = 0.01#何秒間隔でプロットするか
X_OFFSET = -703#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は-703を入れる)
Y_OFFSET = 394#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は394を入れる)
Z_OFFSET = 1867#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は1867を入れる)
#----------------------------------------------------------------------------------
 
#変数/配列の初期化処理
#----------------------------------------------------------------------------------
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)
 
ax_average = 0#x加速度の平均値を格納するglobal変数
ay_average = 0#y加速度の平均値を格納するglobal変数
az_average = 0#z加速度の平均値を格納するglobal変数
 
initial_theta = 0#初期状態のθ
initial_phi = 0#初期状態のφ
initial_Rxyz = 0#初期値を元に算出した回転行列
#----------------------------------------------------------------------------------
 
#リアルタイム描画する際に必要な処理
#----------------------------------------------------------------------------------
fig,axes = plt.subplots(1,1)
lines, = axes.plot(0,0,"-o",lw=5)#何かプロットしないといけないとのことなので、空プロット
plt.grid()
axes.set_xlabel("X_AXIS[G]")
axes.set_ylabel("Y_AXIS[G]")
 
buf_x =[]
buf_y =[]
buf_z =[]
#----------------------------------------------------------------------------------
 
# x_offset = -703#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は-703を入れる)
# y_offset = 394#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は394を入れる)
# z_offset = 1867#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は1867を入れる)
#----------------------------------------------------------------------------------
 
#デバイスの初期化処理
#----------------------------------------------------------------------------------
# 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")
 
def calc_average():#センサーの値の平均値を求めグローバル変数に入れる。
    global ax_average
    global ay_average
    global az_average
    
    for i in range(CAL_SIZE):
        if i>IGNORE_DATA_SIZE:#最初のデータは信憑性が無いため、無視
            x,y,z = getAccel_offset()
            ax_average += x
            ay_average += y
            az_average += z 
 
        else:#念のため空読み
            x,y,z = getAccel()
 
    ax_average = ax_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    ay_average = ay_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    az_average = az_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    print("ax_average")
    print(ax_average)
    print("ay_average")
    print(ay_average)
    print("az_average")
    print(az_average)
 
def calc_initial_angle():#センサーの平均値から現在の角度を求める(ラジアンで返す)
    global initial_phi
    global initial_theta
    if (ax_average == 0 ) and (ay_average == 0) and (az_average == 0):#もし平均値の計算が出来てなければ平均値を計算し、グローバル変数に値を格納する
        calc_average()  
    initial_phi = np.arctan(ay_average/az_average)
    initial_theta = np.arctan(-ax_average/(np.sqrt(ay_average**2+az_average**2)))
    print("phi")
    print( initial_phi)
    print("theta")
    print(initial_theta)
    print("phi_deg")
    print( np.rad2deg(initial_phi))
    print("theta_deg")
    print( np.rad2deg(initial_theta))
def calc_rotation(ax, ay, az):
    global initial_Rxyz
    a_initial = np.array([[ax], [ay], [az]])#回転前の加速度の列ベクトルを作成
 
    Cp = np.cos(initial_phi)#phiのCos値(初期値の角度分回転)
    Sp = np.sin(initial_phi)#phiのSin値(初期値の角度分回転)
    Ct = np.cos(initial_theta)#thetaのcos値(初期値の角度分回転)
    St = np.sin(initial_theta)#thetaのsin値(初期値の角度分回転)
    Cps = 1 #psi方向のCos値(psi方向は初期値が求められないため、0度として計算)
    Sps = 0 #psi方向のSin値(psi方向は初期値が求められないため、0度として計算)
    Rx = np.matrix([[1,0,0],[0,Cp,-Sp],[0,Sp,Cp]])#x方向の回転行列
    Ry = np.matrix([[Ct,0,St],[0,1,0],[-St,0,Ct]])#y方向の回転行列
    Rz = np.matrix([[Cps,-Sps,0],[Sps,Cps,0],[0,0,1]])#z方向の回転行列
    initial_Rxyz = Rz@Ry@Rx#@で行列の演算が出来るとの事。X->Y->Zの順で回転させる
    
    a_rotate = initial_Rxyz@a_initial#初期状態の角度回転させた場合の値
    
    print("initial_Rxyz")
    print(initial_Rxyz)
    print("a_rotate")
    print(a_rotate)
    #print(initial_Rxyz*10**10)
    #print(a_rotate[0]*10**10)#テスト用。これを記述して一度実行したら何故か2回目実行時も問題無く表示されるように。。。
def real_time_plot():
    global lines
    global ax
    global buf_x
    global buf_y
    global buf_z
 
    x,y,z = getAccel_offset()
    buf_x.append(x)
    buf_y.append(y)
    buf_z.append(z)
    #何個の点を描画するか
    if len(buf_x) > X_BUF_SIZE:
        del buf_x[0]
    if len(buf_y) > Y_BUF_SIZE:
        del buf_y[0]
    if len(buf_z) > Z_BUF_SIZE:
        del buf_z[0]
    lines.set_data(buf_x,buf_y)
    # print("bus_x,bus_y")
    # print(buf_x)
    # print(buf_y)
    axes.set_xlim(-2.1,2.1)#今回は2gのセンサーを使っている為、範囲は2g+マージンで0.1の範囲で描画
    axes.set_ylim(-2.1,2.1)#今回は2gのセンサーを使っている為、範囲は2g+マージンで0.1の範囲で描画
    draw_circle = plt.Circle(xy=(0, 0), radius=0.2,fill=False,ec = "b")
    axes.add_artist(draw_circle)
    draw_circle2 = plt.Circle(xy=(0, 0), radius=0.5,fill=False,ec = "b")
    axes.add_artist(draw_circle2)
    draw_circle3 = plt.Circle(xy=(0, 0), radius=1.0,fill=False,ec = "b")
    axes.add_artist(draw_circle3)
    draw_circle4 = plt.Circle(xy=(0, 0), radius=1.5,fill=False,ec = "b")
    plt.pause(PLLOT_TIME)
def file_output():
    dt_now = datetime.datetime.now()
    file = open(str(dt_now)+"calc_data.txt","w",encoding="utf-8")#統計データ格納用のファイル
    #統計情報をファイルに出力
    #----------------------------------------------------------------------------------
    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()
    #----------------------------------------------------------------------------------
 
# 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_offset = math.sqrt(ax_offset**2+ay_offset**2+az_offset**2)#加速度の大きさを算出
        temp = get_temp()#温度センサーの情報読み出し
        raw_ax = read_word(ACCEL_XOUT)
        raw_ay = read_word(ACCEL_YOUT)
        raw_az = read_word(ACCEL_ZOUT)
        if (ax_average == 0 ) and (ay_average == 0) and (az_average == 0):#初期状態の角度を計算してない場合に初期状態の角度を求める
            print("初期状態の角度を求めます")
            calc_initial_angle()
            calc_rotation(ax_average,ay_average,az_average)
        else:#センサーの値をリアルタイムで回転させていく処理
            calc_rotation(ax,ay,az)
 
        # real_time_plot()#リアルタイム描画の関数
        # 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_offset))
        # 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())
        sys.exit()#後ほど削除
        #----------------------------------------------------------------------------------
        
        # file_output()#ファイル下記だし用の処理 
 
 
        #ロギングしたデータをプロット
        #----------------------------------------------------------------------------------
        # 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()
        #----------------------------------------------------------------------------------

まとめ

今回初期値状態の角度を算出してセンサー値をグローバル座標系に変換することが出来ました。

また、初期状態を求めた後に、センサー値をリアルタイムで回転する処理を書きました。
実行した結果そこまで重たい処理ではないように見受けられましたが、今後使ってみて動作が重そうでしたら初期状態を求めた後に回転前のセンサー値をログとして保存して後処理としてまとめて回転するようにしたいと思います。





自己紹介

はじめまして 社会人になってからバイクやプログラミングなどを始めました。 プログラミングや整備の記事を書いていますが、独学なので間違った情報が多いかもしれません。 間違っている情報や改善点がありましたらコメントしていただけると幸いです。

X(旧Twitter)

フォローお願いします！

ラベル

整備 (48) プログラミング (40) python (32) 車 (29) バイク (26) 雑記 (23) VTR (22) ラズパイ (17) 設定 (14) FIT2 (12) GE8 (12) GK5 (11) FIT3 (10) Blogger (9) 環境構築 (7) 修理 (6) 競技プログラミング (6) 組み込み (6) MPU6050 (5) その他 (4) セキュリティ (4) 統計 (4) scikit-learn (3) Android (2) CTF (2) CustomTkinter (2) C言語 (2) ESP32 (2) Ruby (2) Adsense (1) DIO (1) UML (1) kotlin (1) その他/修理 (1) ゲーム (1) 原付 (1) 回帰分析 (1) 数学 (1) 自転車 (1)

QooQ