先回でL298Nドライバモジュールを使うことで安定してDCモーターを回せるようになりました。
今回はジャイロセンサーMPU6050 モジュールを使ってみたいと思います。
このブログは長編になってしまったので、最初に成果を動画でご覧ください。
さて、本編に入りますよ。
MPU6050ジャイロセンサー
写真は、MPU6050チップ（中央）を搭載したブレークアウトモジュールです。MPU6050はInvenSense社が製造するICチップです。InvenSense社は2017年にTDKにより買収されてTDKの完全子会社になっていました。また、MPU6050チップは既にEOL（End Of Life）、製造終了になっていました。
ネット通販では、今も普通に購入できています。在庫品それとも代替品があるのかな？チップ上の型番をみようとしましたが判別不能でした。
MPU-6050は、3軸のジャイロスコープと3軸の加速度センサーを組み合わせた6軸の動き検出デバイスです。このようなセンサーはゲームコントローラやドローンやラジコンヘリコプターなどの姿勢制御に使われたりしています。
上の図は、センシング可能なX、Y、Z軸の方向と各軸の回転方向を示しています。図のピン識別子 (•) が基準になります。実際のMPU-6050ブレークアウトモジュールの向きを合わせて並べてみました。基板にもX、Ｙの矢印と回転の矢印が印刷されています。
MPU6050との通信は、 400kHzのI2Cを使用して実⾏されます。
このMPU6050ブレークアウトモジュールのピン端子の確認です。5㎜角の小さなMPU6050チップには24個のもの端子がありますが、モジュールでは8個にまで簡単化されています。さらに今回使用するのは4個のみです。この機会に8個のピン端子について確認しておきます。下表はMPU6050ブレークアウトモジュールのピン端子の説明です。なお、下表はMPU6050データシートをもとにまとめました。
表1.MPU6050モジュールのピン端子一覧
| ピン名 | 名称 | 内容 |
|---|---|---|
| VCC | 電源電圧 | 動作電圧範囲は2.375V ～ 3.46V。ラズパイでは3.3Vを入力。 |
| GND | 電源グランド | ラズパイのGPIOのグランドピンと接続。 |
| SCL | シリアルクロック | I2C通信におけるクロック信号。 |
| SDA | シリアルデータ | I2C通信におけるデータ信号。 |
| XDA | 補助シリアルデータ | 外部センサーとのデータ信号。自身がマスターになって外部センサーとI2C通信可能またはシステムプロセッサへ外部センサーの信号をパス可能。 |
| XCL | 補助シリアルクロック | 外部センサーとのデータ信号のためのクロック信号 |
| AD0 | I2Cスレーブアドレス | I2C通信バスに２つのMPU6050を接続するためのアドレッシング。MPU6050 のスレーブ アドレスは b110100Xで、XがAD0に割当てられています。AD0にGNDを接続するとb1101000（=0x68）になり、VCCに接続すると0x69になります。アドレスのbはバイナリ（二進数）を意味します。 |
| INT | 割り込みデジタル出力 | MPU6050内部や外部センサーなどの処理に関して、割り込み信号を発生させて、システムプロセッサ側に伝える信号。 FIFOバッファオーバー、I2Cマスター割り込み、データ準備完了など発生時に割り込み信号が１（HIGH）になります。 |
ジャイロセンサー情報は、どのように取得できるのか確認します。MPU6050のレジスタマップ説明書を見ると記載されていました。
このレジスタマップによると、レジスタ0x3Bの8ビットと0x3Cの8ビットにX方向加速度が格納されると記載あります。同様にY方向加速度はレジスタ0x3Dと0x3Eに、Z方向可読度はレジスタ0x3Fと0x40に格納されるとあります。
MPU6050のジャイロセンサーはサンプリングレートごとに測定されて上記のレジスタに格納されます。したがって、次の測定までにレジスタ0x3Bから0x40までを読み出す必要があります。
サンプリングレートは、以下の式で算出されるとあります。
サンプリングレート＝ジャイロスコープ出⼒レート / (1 + SMPLRT_DIV)
ジャイロスコープ出力レートは、以下の条件で決まるとあります。
・DLPF が無効の場合(DLPF_CFG = 0 または 7)、ジャイロスコープ出⼒レートは 8kHzになる
・DLPF が有効の場合 (レジスタ0x1Aを参照)、ジャイロスコープ出⼒レートは 1kHz になる
ただし、加速度計の出力レートは 1kHz 固定。
注意として、サンプリングレートが 1kHzを超える場合、次の測定までは加速度計の出力は同じ値であるということです。
このことから、Raspberry Pi からX、Y、Z方向の加速度を1kHz（周期0.001秒）より高速で取得する必要はないということです。
DLPFが有効かどうかについては、下表のレジスタ0x1Aのビット2～0の値を見ればよいことがわかります。
DLPF_CFGの値は下表のとおりです。見たところジャイロスコープの周波数も基本的には1kHzということがわかります。決めるところは、帯域幅を確保して遅延を小さくすることになります。MPU6050の初期設定がどうなっているか別途確認したいと思います。上記の注意点はDLPF_CFGが0の場合に限られることがわかりました。
DLPF_CFGの設定により以下の周波数になります。
また、SMPLRT_DIVは下表のようにレジスタ0x19に格納されている値です。説明書には初期値の記載は見当たりませんでした。
今回、使用するピンは、VCC、GND、SCL、SDAの４つです。4つを使用する上でもう少し知っておきたいことがあります。表内にも記載のある「I2C」通信です。
I2C通信とは
I2C（Inter-Integrated Circuit）通信は、Philips Semiconductors (現在の NXP Semiconductors)が開発した通信インターフェースで、デバイス同士を接続する双⽅向2線式バス（同期式シリアル）通信です。I2Cの仕様書は、UM10204 I2C-bus specification and user manualのドキュメントで公開されています。
必要なバスラインは、シリアルデータライン (SDA) とシリアルクロックライン (SCL) の2つだけです。シリアルの8ビット指向の双⽅向データ転送は、標準モードで最⼤100kbit/s、高速モードで最⼤ 400kbit/s、高速モードプラス(Fm+)で最⼤1Mbit/s、高速モードで最⼤3.4Mbit/s で実⾏できます。
MPU6050データシートによるとI2C動作周波数は高速モードで400kHz、標準モードで100kHzとなっていますのでI2C仕様通りです。
I2Cのシリアルバスの構成
I2C通信するためのシリアルバス構成は以下のようになります。I2C通信はコントローラとデバイス間で行います。構成では、複数のコントローラおよび複数のデバイスをシリアルバス上に接続することができます。SCL（シリアルクロック）信号線はコントローラおよびデバイスのSCLと接続します。また、SDA（シリアルデータ）信号線もコントローラおよびデバイスのSDAと接続します。SCLおよびSDAはどちらもプルアップ抵抗（Rp）（※）を介して電源電圧（VDD）に接続します。
デバイスにアドレスを持たせることで複数のデバイスを同じ回路上に接続させることができます。ただし、同じデバイスを接続する場合は、同じアドレスにならないようにアドレスを変更する必要があります。今回のMPU6050モジュールでは２つのアドレスをAD0端子によって選択できるので、同時に２つまで接続できます。
※プルアップは、信号線のレベルを電源電圧レベルにしておき、信号を安定に保つための働きがあります。反対にプルダウンは、信号線レベルをグランドレベルにします。働きは同じです。電子回路の仕様に合わせてどちらかにします。これによりノイズやサージへの影響を抑える役目もあります。
I2C通信の概要
I2C通信の概要を説明します。コントローラをRaspberry Pi に、デバイスをMPU6050と読み替えると対応がわかりやすいと思います。
１．コントローラ がデバイスに情報を送信する場合：
①コントローラは、デバイスのアドレスを指定する。
②コントローラは、デバイスにデータを送信する。
③コントローラは、通信を終了する。
２．コントローラがデバイスから情報を受信する場合：
①コントローラは、デバイスのアドレスを指定する。
②コントローラは、デバイスからデータを受信する。
③コントローラは、通信を終了する。
信号の決まり
I2C通信では、SCL（シリアルクロック）とSDA（シリアルデータ）の２つの信号しかありませんが、２つの信号には決まりがあります。ここでは基本的な決まりに絞ってまとめました。もっと詳しく知りたい場合は、上記の仕様書をご覧ください。
データの妥当性
SDA信号がデータとみなされるには、下図に示すように３つの決まりがあります。
・SCL信号がHIGHレベルの時はSDA信号のデータ（HIGHまたはLOW）が有効で安定していること
・SCL信号がLOWレベルの時はSDA信号のデータを変更（HIGH→LOWまたはLOW→HIGH）が可能
・SCL信号1クロックパルスに付きSDA信号１ビットデータ（HIGHまたはLOW）を送信する
開始（繰り返し開始）と終了の合図
SDA信号のデータの開始とデータの終了を相手先に知らせるには、下図に示すように条件があります。
・SCL信号がHIGHの時にSDA信号がHIGHからLOWに変わると開始条件が有効
・SCL信号がHIGHの時にSDA信号がLOWからHIGHに代わると終了条件が有効
・開始条件、終了条件の有効化は、コントローラが行う
・開始条件が有効になると、シリアルバスはビジー状態（使用中）になる
・終了条件が有効後、一定時間経過すると、シリアルバスはフリー状態（非使用中）になる
・繰り返し開始条件が有効になると、シリアルバスはビジー状態のまま
バイト形式
SDA信号のデータに関する決まりです。下図に示すように条件があります。
・SDA信号のデータの単位は8ビット長（１バイト）で、1回の通信で送信するバイト数に制限はなし
・受信側は各バイトの後（9パルス目）に確認応答ビット（ACK）または非確認応答ビット（NACK）を送信する。ACK（アック）としてLOW信号を、NACK（ナック）としてHIGH信号を送信する。
・データは最上位ビット(MSB(※))から送信する
・デバイス（例えばMPU6050）が別の処理を完了するまで次のデータを送信/受信できない場合、SCL信号をLOWに保持して、コントローラ（例えばRaspberry Pi ）を待機状態にすることが可能
・デバイスが次のデータの送信/受信の準備ができ、SCL信号を開放するとデータ転送が続行する
※MSBとは、Most Significant Bitの略で、コンピュータにおいて二進数で最も大きな値を意味するビット位置のことです。MSBは左端ビットともいわれます。逆に右端のビット位置をLSB（Least Significant Bit）といいます。この決まりはコントローラ側とデバイス側がデータの値を互いに正しく認識するための基本となる決まりです。例えば、コントローラ側はMSB順（10100111：10進数の167）でデータを送信し、デバイス側がLSB順（11100101：10進数の229）でデータを受信するとデータの値が正しく伝達できません。
アドレス指定と読み出し/書き込み指定
コントローラは、データを送信する前にどのデバイスと通信するかを決める必要があります。また、コントローラは、データを送信するのか受信するのかを決める必要があります。下図に示すように条件があります。
・開始の後、デバイスのアドレスを送信する。アドレスは7ビットの長さで、8番目のビットはデータの方向を示すビット（R/W）である。LOW信号は送信（WRITE）を示し、HIGH信号はデータの要求（READ）を示す。
・アドレスとデータ方向の確認応答（ACK）後、データの送信または受信を行い、コントローラが終了とするまでデータを転送する
MPU6050モジュールのアドレスの場合
ここで、ジャイロセンサーのMPU6050モジュールのアドレスの場合は、以下のようなSDA信号になります。アドレスは、前述の表１.MPU6050モジュールのピン端子一覧のAD0の欄に記載している0x68（b1101000）でした。ジャイロデータを受信するため8番目のビットはREAD（HIGH）になります。また、9番目のビットは、デバイスのMPU6050側からの確認応答でACK（LOW）を返します。
MPU6050モジュールのアドレスをMSBのビットから送信するので、アドレスは下図の並び順になります。
Raspberry Pi （コントローラ）がACKを確認すると、Raspberry Pi はMPU6050からのセンサー情報を受信します。Raspberry Pi は1バイト（8ビット）を受信するたびに9番目のビットにACKをMPU6050に返します。MPU6050はACKを確認して、次の1バイトデータを送信します。Raspberry Pi が終了にするまで、データ転送を繰り返します。実際には、MPU6050から情報取得するにはもう少し手順が必要ですがイメージと考えてください。
I2Cの説明で使ったデジタル波形は、自作のデジタル波形作成ツール（Digital Waveform Creater）で作成しました。ダウンロードもできます。エクセルVBAで作成したのでエクセルがあれば使えます。
I2Cの有効化
Raspberry PiにはI2Cインタフェースが標準で備わっています。ただし、初期設定（デフォルト）では無効になっている場合が多く、有効にする必要があります。
UI画面による設定（raspi-confg）の場合
ターミナルから以下を入力してraspi-config画面を開きます。
sudo raspai-config以下のRaspberry Pi のコンフィギュレーション画面が開きます。
・3 Interface Optionsを選択して、次の画面で I5 I2C を選択します。
・[はい] を選択し有効にします。再確認画面で [了解] を選択します。
・コンフィグレーション画面を閉じます。
・最後に再起動します。
直接 config.txtを編集する場合
ターミナルから以下を入力してconfig.txtを編集します。nanoはエディタツールのことです。
sudo nano /boot/firmware/config.txt以下のように「dtparam=i2c=on」に編集（なければ行を追加）します。
最後に保存して、再起動します。
sudo rebootI2Cインタフェースが有効になっていることを確認します。ターミナルから以下を入力して実行します。
ls /dev/ic2-*画面に「/dev/i2c-1」が表示されていればOKです。
/dev/i2c-1配線図
MPU6050をRaspberry Pi に配線します。4線しかないので配線はとても簡単です。
MPU6050の存在確認
実際に配線して電源を入れたところです。MPU6050モジュールのLEDが点灯しています。
Raspberry Pi にMPU6050が認識されているか確認します。
ターミナルから以下のコマンドを入力して、MPU6050のアドレスが検出されるか確認します。
ちなみに引数の「1」は、/dev/i2c-1のことです。
i2cdetect -y 1MPU6050のアドレス「0x68」が以下のように表示されます。前のL293Dのチップが不良品だったトラウマが横切りましたが、今のところ大丈夫そうです。
MPU6050のレジスタ読み出し
MPU6050の配線、デバイスの認識までできました。続いてMPU6050のレジスタ内容を読みだしてみたいと思います。
以下にレジスタを読み出すPythonプログラムを示します。
import smbus
import csv
# 電源管理のレジスタ
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
# MPU6050レジスタ読み出し
def read_mpu6050_registers(bus_number=1, device_address=0x68, start_register=0x00, end_register=0x75):
bus = smbus.SMBus(bus_number)
bus.write_byte_data(device_address,power_mgmt_1,0)
data = {}
try:
for reg in range(start_register, end_register + 1):
value = bus.read_byte_data(device_address, reg)
data[reg] = value
except Exception as e:
print(f"Error reading from MPU6050: {e}")
finally:
bus.close()
return data
# 読みだしたレジスタと値の表示
def display_registers(data):
print("Register Address | Value")
print("----------------|-------------------")
for reg, value in data.items():
print(f"0x{reg:02X} | {bin(value)[2:].zfill(8)}")
# CSVファイルへの保存
def save_to_csv(data, filename="mpu6050_registers.csv"):
with open(filename, mode='w', newline='') as file:
writer = csv.writer(file)
writer.writerow(["Register Address", "Value (Binary)"])
for reg, value in data.items():
writer.writerow([f"0x{reg:02X}", bin(value)[2:].zfill(8)])
print(f"Data saved to {filename}")
# メイン
if __name__ == "__main__":
registers = read_mpu6050_registers()
display_registers(registers)
save_to_csv(registers)Raspberry Pi で実行するとターミナルに以下のようにレジスタとその値を表示します。これは最初の方の抜粋です。これがレジスタの0x75まで表示されます。
下表はCSVファイルに保存したレジスタとその値の一覧で、整理したものです。
| レジスタ | 値(バイナリ) | 名称 | レジスタ | 値(バイナリ) | 名称 |
| 0x00 | 11111111 | 非公開 | 0x3B | 00000110 | X方向加速度H |
| 0x01 | 00000001 | 0x3C | 11001100 | X方向加速度L | |
| 0x02 | 10000011 | 0x3D | 11111110 | Y方向加速度H | |
| 0x03 | 11100011 | 0x3E | 00000000 | Y方向加速度L | |
| 0x04 | 00011100 | 0x3F | 01001010 | Z方向加速度H | |
| 0x05 | 10111100 | 0x40 | 11001000 | Z方向加速度L | |
| 0x06 | 11111100 | 0x41 | 11101011 | 温度出力H | |
| 0x07 | 00011000 | 0x42 | 11100000 | 温度出力L | |
| 0x08 | 00000000 | 0x43 | 00000100 | ジャイロX測定値H | |
| 0x09 | 10101011 | 0x44 | 10011101 | ジャイロX測定値L | |
| 0x0A | 00000010 | 0x45 | 11111111 | ジャイロY測定値H | |
| 0x0B | 01000010 | 0x46 | 11100101 | ジャイロY測定値L | |
| 0x0C | 00101000 | 0x47 | 00000000 | ジャイロZ測定値H | |
| 0x0D | 00110010 | 0x48 | 01001000 | ジャイロZ測定値L | |
| 0x0E | 01010000 | 0x49 | 00000000 | ||
| 0x0F | 10110111 | 0x4A | 00000000 | ||
| 0x10 | 00110001 | 0x4B | 00000000 | ||
| 0x11 | 00000000 | 0x4C | 00000000 | ||
| 0x12 | 00000000 | 0x4D | 00000000 | ||
| 0x13 | 00000000 | 0x4E | 00000000 | ||
| 0x14 | 00000000 | 0x4F | 00000000 | ||
| 0x15 | 00000000 | 0x50 | 00000000 | ||
| 0x16 | 00000000 | 0x51 | 00000000 | ||
| 0x17 | 00000000 | 0x52 | 00000000 | ||
| 0x18 | 00000000 | 0x53 | 00000000 | ||
| 0x19 | 00000000 | サンプルレート分周期 | 0x54 | 00000000 | |
| 0x1A | 00000000 | 構成 | 0x55 | 00000000 | |
| 0x1B | 00000000 | ジャイロスコープの設定 | 0x56 | 00000000 | |
| 0x1C | 00000000 | 加速度計の設定 | 0x57 | 00000000 | |
| 0x1D | 00000000 | 0x58 | 00000000 | ||
| 0x1E | 00000000 | 0x59 | 00000000 | ||
| 0x1F | 00000000 | 0x5A | 00000000 | ||
| 0x20 | 00000000 | 0x5B | 00000000 | ||
| 0x21 | 00000000 | 0x5C | 00000000 | ||
| 0x22 | 00000000 | 0x5D | 00000000 | ||
| 0x23 | 00000000 | 0x5E | 00000000 | ||
| 0x24 | 00000000 | I2Cマスター制御 | 0x5F | 00000000 | |
| 0x25 | 00000000 | 0x60 | 00000000 | ||
| 0x26 | 00000000 | I2Cスレーブ0制御 | 0x61 | 00000000 | |
| 0x27 | 00000000 | 0x62 | 00000000 | ||
| 0x28 | 00000000 | 0x63 | 00000000 | ||
| 0x29 | 00000000 | 0x64 | 00000000 | ||
| 0x2A | 00000000 | 0x65 | 00000000 | ||
| 0x2B | 00000000 | 0x66 | 00000000 | ||
| 0x2C | 00000000 | 0x67 | 00000000 | ||
| 0x2D | 00000000 | 0x68 | 00000000 | 信号パスのリセット | |
| 0x2E | 00000000 | 0x69 | 00000000 | 非公開 | |
| 0x2F | 00000000 | 0x6A | 00000000 | ユーザー制御 | |
| 0x30 | 00000000 | 0x6B | 00000000 | 電源管理１ | |
| 0x31 | 00000000 | 0x6C | 00000000 | 電源管理２ | |
| 0x32 | 00000000 | 0x6D | 00000000 | 非公開 | |
| 0x33 | 00000000 | 0x6E | 00000000 | ||
| 0x34 | 00000000 | 0x6F | 11111011 | ||
| 0x35 | 00000000 | 0x70 | 00000000 | ||
| 0x36 | 00000000 | I2Cマスターステータス | 0x71 | 00000000 | |
| 0x37 | 00000000 | 0x72 | 00000000 | ||
| 0x38 | 00000000 | 0x73 | 00000000 | ||
| 0x39 | 00000000 | 0x74 | 00000000 | ||
| 0x3A | 00000001 | 0x75 | 01101000 | Who Am I |
表を見ると、以下のことがわかります。
・レジスタ0x3B～0x40でX,Y,Z方向の加速度が取得されています。
・レジスタ0x43～0x48でジャイロX,Y,Z測定値が取得されています。
・レジスタ0x75でMPU6050自身のアドレスが取得されています。
・上述した以下の式の各設定値がわかります。
SMPLRT_DIV：０（レジスタ0x19の値）
ジャイロスコープ出力レート：8 kHz
DLPF_CFG[2:0]：0（レジスタ0x1Aの値）よりジャイロスコープ出力レート（周波数）は8 kHz。
上記より、サンプリングレート＝ジャイロスコープ出⼒レート / (1 + SMPLRT_DIV)
=８kHz / (1 + 0)
=８kHz
結果として、2つのサンプリングレートは以下ということがわかりました。
・加速度のサンプリングレートは１kHz
・ジャイロサンプリングレートは８kHz
MPU6050を起こせ！
もうひとつ、重要なことがあります。電源投入後ではMPU6050はスリープ状態になっており、加速度、ジャイロ測定値ともゼロのままでした。これはレジスタ0x6Bの電源管理１の値からわかりました。
以下は、電源投入後のレジスタ0x6Bの値です。6ビット目が１になっています。このビットはSLEEPの状態を示しています。1 の場合SLEEP状態になります。
この6ビット目を０にする必要があります。そこでPythonプログラムに以下の行を追加して、レジスタ0x6Bにゼロを書き込んでいます。上記のレジスタを読み出すPythonプログラムの朱書きのところです。
# 電源管理のレジスタ
power_mgmt_1 = 0x6b
bus.write_byte_data(device_address,power_mgmt_1,0) これを実行することでMPU6050はSLEEP状態から目覚めて、加速度、ジャイロ測定値を更新し始めます。
レジスタ0x6Bの値もゼロになっていることを確認できました。
ジャイロセンサー情報の値の換算
さて、ジャイロセンサーMPU6050のことが分かってきました。センサー情報もどこから取得すればよいかもわかりました。あとは、ジャイロセンサー情報を物理情報に変換して、理解できる情報にすることです。
MPU6050のレジスタマップ説明書をもう少し読み解く必要があります。
10進数に換算
レジスタ0x3B～0x40の説明によると、レジスタの値は16ビットの2の補数とあります。そこで、レジスタとその値の一覧の値を使って、X方向加速度について確認していきたいと思います。
| レジスタ | 値(バイナリ) | 名称 |
| 0x3B | 00000110 | X方向加速度H |
| 0x3C | 11001100 | X方向加速度L |
表から0x3Bの値が8ビットから15ビットに、0x3Cの値が0ビット～7ビットにあたります。全体で16ビットの値になります。
0x3Bの値を16ビットの８～15の箱に入れていくと、以下のようになります。
同様に0x3Cの値を０～７の箱に入れていくと以下になり、16ビットの値になります。10進数にすると1740になります。
16ビットの２の補数というのは、どういうものでしょうか？
2の補数は、負の数を2進数で表す際に用いられる方法です。コンピュータの内部では2進数で動作しています。つまり、0（OFF）と１（ON）の組み合わせで動くように作られています。この世界では負の数を表すマイナス（ー）という記号が用意されていません。その代わり最上位ビットMSBのビットを正の数と負の数を区別するためのビットに使うことにしています。
つまり、16ビットの数字を扱う場合、15ビット目の値が１の場合、負の数とし、０の場合、正の数としましょう、ということです。16ビットをすべて使って表せる数は0～65535（10進数）ですが、15ビット目を正/負の区別に使うため、表せる数は-32767～32767（10進数）になります。
最初に戻ると、16ビットの2の補数というのは、X方向加速度の値は-32767～32767の範囲であることを示しています。
あらためて以下の16ビットの値は、15ビット目が０であるため正の数であり、+1740（10進数）ということです。
これだと2の補数が実感できませんね。上記のレジスタとその値の一覧表をもう一度みると、Y方向加速度H/Lを見てみます。
| レジスタ | 値(バイナリ) | 名称 |
| 0x3D | 11111110 | Y方向加速度H |
| 0x3E | 00000000 | Y方向加速度L |
表から0x3Dの値が8ビットから15ビットに、0x3Eの値が0ビット～7ビットにあたります。全体で16ビットの値になります。
同じように16ビットの箱に詰めなおすと以下のようになります。15ビット目が１になりました。つまり、このY方向加速度の値は、負の数ということです。
0ビット目から14ビット目までの値111111000000000は10進数で32256です。15ビット目が１なのでマイナスを付けて-32256になります。
物理値に換算
次に10進数に換算したジャイロセンサー情報を物理値に換算します。ジャイロセンサー情報は2つあります。１つは加速度、もう１つは角速度です。レジスタの0x3B～0x40は加速度で、レジスタの0x43～0x48は角速度です。
ちょっと中学理科の復讐です。
加速度は、単位時間あたりの速度の変化率のことです。
地点Aを通過する速度をVa（ｍ/s）、地点Bを通過するときの速度をVb（ｍ/S）で、地点Aから地点Bへ移動する時間をt（s）とすると、加速度aは以下の式で求められます。
加速度a＝（Vb – Va）/ｔ（ｍ/s2)
高校物理で習う物体の加速度a(m/s2)は物体の慣性力F(N)と物体の質量ｍ(kg)からa=F/mと表せますね。
なお、1N＝1㎏・m/s2
角速度は、物体が回転運動する際に、単位時間あたりの回転角で表した量です。
時刻t(s)までに角度θだけ進んだ場合、t(s)で進んだ角度をθとして、角速度ωは1秒間に進んだ角度なので、θをtで割ることで求められます。
角速度ω＝θ/t（rad/s)
角速度の単位（rad/s)は国際単位系で定められていますが、ジャイロセンサーではしばしばdegree per second（°/s）が使われています。これは直感的にわかりやすい点やモーターなど1回転の分解能で制御する際に計算量が少なくてすむためと思われます。
X方向加速度の値から加速度への換算手順
X方向加速度の値から加速度に換算する手順を示します。
１．X方向加速度の測定値を取得:
ACCEL_XOUT_H（レジスタ0x3B）とACCEL_XOUT_L（レジスタ0x3C）から16ビットの2の補数値を読み取ります。これは上述の通りです。
２．フルスケール範囲とLSB感度を確認:
レジスタ0x1CのAFS_SELビットで設定されたフルスケール範囲を確認します。
レジスタ0x1CのAFS_SELビットで設定されたフルスケール範囲を確認し、フルスケール範囲に対するLSB感度を確認すると下表の通りです。表中の「g」は重力加速度を表し、1.0g＝9.8m/s2 です。
３．加速度に換算:
取得したX方向加速度の測定値をLSB感度で割って加速度を算出します。
ここで、AFS_SELビット値は下表の通り0でしたので、LSB感度は16384 LSB/gです。
| レジスタ | 値(バイナリ) | 名称 |
| 0x1C | 00000000 | 加速度の設定 |
X方向加速度の測定値が1740（＝0000011011001100）で、フルスケール範囲が±2gの場合:
加速度 = 1740 / 16384 = 0.1062g=1.0408m/s2
この手順でX方向加速度の測定値を加速度に換算できます。同様にY方向加速度、Z方向加速度から各軸方向の加速度を換算することができます。
ジャイロX測定値から角速度への換算手順
次に、ジャイロX測定値から角速度に換算する手順を示します。
１．ジャイロX測定値を取得:
GYRO_XOUT_H（レジスタ0x43）とGYRO_XOUT_L（レジスタ0x44）から16ビットの2の補数値を読み取ります。これは上述の通りです。
２．フルスケール範囲とLSB感度の確認:
レジスタ0x1BのFS_SELビットで設定されたフルスケール範囲を確認します。
FS_SELビットに対するフルスケール範囲とフルスケール範囲に対するLSB感度は以下の通りです。
３．角速度に換算:
取得したジャイロ測定値をLSB感度で割って角速度を算出します。
ここで、FS_SELビット値は下表の通り0でしたので、LSB感度は131 LSB/°/秒です。
| レジスタ | 値(バイナリ) | 名称 |
| 0x1B | 00000000 | ジャイロスコープの設定 |
ジャイロX測定値が1181（=0000010010011101）で、フルスケール範囲が±250°/秒の場合:
角速度 = 1181 / 131 = 9.0153 °/秒
この手順でジャイロX測定値を角速度に換算できます。同様にジャイロY測定値、ジャイロZ測定値から各軸の角速度を換算することができます。
プログラムの作成
今回、作成したのはジャイロセンサーMPU6050のセンサー情報を取得して表示するPythonプログラムです。取得した情報を数値表示だけでは面白くないので、ワイヤーフレームでオブジェクトも表示させてジャイロに同期してオブジェクトの姿勢を表示するようにしてみました。
センサー情報を取得するよりワイヤーフレームを3次元的に表示させる方がはるかに難しかったです。（笑）
本当は、OpenGLを使ってフル３Dモデルとして表示させたかったのですが、どうもRaspberry Pi 4 では使えなくなっているようです。＿|￣|○ ｶﾞｸｯ!
このプログラムのポイントは、以下です。
・MPU6050のI2C通信処理と受信結果の表示
・cursesライブラリを使用して見やすく表示
・センサー情報をロール、ピッチ、ヨーの姿勢角度に変換
・姿勢角に合わせてオブジェクトの座標を回転変換
・pygameライブラリを使用してワイヤーフレームでオブジェクトを表示
センサー情報を16ビットの2の補数に変換
MPU6050からセンサー情報を取得する部分は、以下の通りです。朱書きのところで16ビットの2の補数処理をしています。
def read_sensor_data(addr):
# MPU6050の指定アドレスからの読み出し
high = bus.read_byte_data(MPU6050_ADDR, addr)
low = bus.read_byte_data(MPU6050_ADDR, addr+1)
# 16ビットの2の補数に変換
value = (high << 8) | low
if value > 32768:
value -= 65536
return value16ビットの2の補数を物理値に変換
また、以下の朱書きの部分で16ビットの2の補数処理した値を物理値に換算しています。「16384」、「131」という値は、上述の「物理値に換算」のところに出てきましたね。
def get_mpu6050_data():
try:
# 正常読み出し時の不定値回避のための初期値設定
ax, ay, az, gx, gy, gz = 0, 0, 0, 0, 0, 0
# 読み出した値を±２Gモードにおける１G=16384によるG単位に変換
ax = read_sensor_data(ACCEL_XOUT_H) / 16384.0
ay = read_sensor_data(ACCEL_XOUT_H + 2) / 16384.0
az = read_sensor_data(ACCEL_XOUT_H + 4) / 16384.0
# 読み出した値を±250°/sモードにおける１°/s=131による°/s単位に変換
gx = read_sensor_data(GYRO_XOUT_H) / 131.0
gy = read_sensor_data(GYRO_XOUT_H + 2) / 131.0
gz = read_sensor_data(GYRO_XOUT_H + 4) / 131.0
except OSError as e:
print(f"Error reading from MPU6050: {e}")
return 0, 0, 0, 0, 0, 0
finally:
return ax, ay, az, gx, gy, gzジャイロセンサーの通信安定化対策
もうひとつ、このプログラムに注意点があります。MPU6050からセンサー値の受信を繰り返すと例外が発生してプログラムが止まってしまうトラブルがありました。真の原因ではないですが、状況からわかった原因として正常に読みだしたはずの値が、実は不定値になっているために、その不定値をリターンする際に例外が発生してしまう現象でした。
そこで対処療法的な対策をしました。上記プログラムの青文字部分のコードを追加することで例外が収まりました。不定値にならないように変数を初期化するようにしました。
ジャイロセンサーのキャリブレーション
センサー情報を数字にして表示するとわかりますが、センサーを動かしていなくても数字が目まぐるしく変化します。デリケートなセンサーのため温度や空気流れ、センサー自身の誤差など様々な外乱によってセンサー値が変化してしまいます。
そこで、キャリブレーションを実施して誤差を校正するようにプログラムしました。
キャリブレーションとは、計測器などの精度を維持するために、基準と比較して誤差を修正する作業です。校正や較正とも呼ばれます。ジャイロセンサーが計測器に相当します。
キャリブレーションのプログラムは以下です。センサー情報を500回取得して、センサー値の総和を500回で割ったものを誤差（バイアス）としています。誤差を求めるためキャリブレーション実施中はジャイロセンサーを動かしてはいけません。
# 角速度のバイアスを補正するためのキャリブレーション
def calibrate_gyro(samples=500):
gx_offset, gy_offset, gz_offset = 0, 0, 0
for _ in range(samples):
_, _, _, gx, gy, gz = get_mpu6050_data()
gx_offset += gx
gy_offset += gy
gz_offset += gz
return gx_offset / samples, gy_offset / samples, gz_offset / samples求めた誤差（バイアス）は、以下の朱書きコード部分でセンサー値に対して差し引いています。
def get_roll_pitch_yaw(dt, ax, ay, az, gx, gy, gz, prev_roll, prev_pitch, prev_yaw):
"""姿勢角ロール・ピッチ・ヨーの算出"""
# 測定値のバイアス補正
gx -= gx_bias
gy -= gy_bias
gz -= gz_bias移動平均フィルターの搭載
誤差を校正してもセンサー値の数字は変化します。そこでセンサー値のばらつきを抑えるために移動平均フィルターを入れて少しでも安定させようと思います。
移動平均フィルターとは、過去N回分のデータの平均を計算することで、ノイズやデータのばらつきを抑える手法です。
Nの回数を大きくすると、
・データの変化に対する反応が遅くなります。
・急激な変化を無視しやすくなります。
小さくすると、
・反応が速くなりますが、ノイズの影響を受けやすくなります。
・はらつきが大きくなります。
このあたり目的に合わせてチューニングします。
移動平均フィルターは、以下のコードです。collectionsというライブラリが便利です。データをキューに追加していくと決めた数を超えると古いものから順に削除されていくというものです。まさに移動平均フィルターにもってこいのライブラリです。ここでは、N＝６回としています。
# 移動平均フィルタ用バッファ
window_size = 6 # 観測データの範囲（調整可）
# キューに直近の最新データ(window_size)分を追加しながら古いデータを削除
roll_buffer = collections.deque([0] * window_size, maxlen=window_size)
pitch_buffer = collections.deque([0] * window_size, maxlen=window_size)以下の部分でセンサーで取得したロール、ピッチの値をキューに追加して、ロール、ピッチの平均値を算出して、この平均値を使います。
def get_roll_pitch_yaw(dt, ax, ay, az, gx, gy, gz, prev_roll, prev_pitch, prev_yaw):
"""姿勢角ロール・ピッチ・ヨーの算出"""
（中略）
# キューに追加
roll_buffer.append(roll)
pitch_buffer.append(pitch)
# 移動平均（ノイズ低減）によりロール・ピッチ角を算出
roll_avg = sum(roll_buffer) / len(roll_buffer)
pitch_avg = sum(pitch_buffer) / len(pitch_buffer)
return roll_avg, pitch_avg, yaw全体のプログラムは以下の通りで、プログラムは200行程度です。コメントが多いので実質は150行程度と思います。
import smbus2
import time
import math
import collections
import pygame
import curses
# MPU6050のI2Cのアドレス定義
MPU6050_ADDR = 0x68
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H = 0x43
# MPU6050オブジェクトモデルの頂点定義
vertices = [
[-1, -2, -0.2], [1, -2, -0.2], [1, 2, -0.2], [-1, 2, -0.2],
[-1, -2, 0.2], [1, -2, 0.2], [1, 2, 0.2], [-1, 2, 0.2]
]
# キューブオブジェクトモデルの頂点定義
#vertices = [
# [-1, -1, -1], [1, -1, -1], [1, 1, -1], [-1, 1, -1],
# [-1, -1, 1], [1, -1, 1], [1, 1, 1], [-1, 1, 1]
#]
# オブジェクトの辺の定義
edges = [
(0, 1), (1, 2), (2, 3), (3, 0),
(4, 5), (5, 6), (6, 7), (7, 4),
(0, 4), (1, 5), (2, 6), (3, 7)
]
def read_sensor_data(addr):
# MPU6050の指定アドレスからの読み出し
high = bus.read_byte_data(MPU6050_ADDR, addr)
low = bus.read_byte_data(MPU6050_ADDR, addr+1)
# 16ビットの2の補数に変換
value = (high << 8) | low
if value > 32768:
value -= 65536
return value
def get_mpu6050_data():
try:
# 正常読み出し時の不定値回避のための初期値設定
ax, ay, az, gx, gy, gz = 0, 0, 0, 0, 0, 0
# 読み出した値を±２Gモードにおける１G=16384によるG単位に変換
ax = read_sensor_data(ACCEL_XOUT_H) / 16384.0
ay = read_sensor_data(ACCEL_XOUT_H + 2) / 16384.0
az = read_sensor_data(ACCEL_XOUT_H + 4) / 16384.0
# 読み出した値を±250°/sモードにおける１°/s=131による°/s単位に変換
gx = read_sensor_data(GYRO_XOUT_H) / 131.0
gy = read_sensor_data(GYRO_XOUT_H + 2) / 131.0
gz = read_sensor_data(GYRO_XOUT_H + 4) / 131.0
except OSError as e:
print(f"Error reading from MPU6050: {e}")
return 0, 0, 0, 0, 0, 0
finally:
return ax, ay, az, gx, gy, gz
# 角速度のバイアスを補正するためのキャリブレーション
def calibrate_gyro(samples=500):
gx_offset, gy_offset, gz_offset = 0, 0, 0
for _ in range(samples):
_, _, _, gx, gy, gz = get_mpu6050_data()
gx_offset += gx
gy_offset += gy
gz_offset += gz
return gx_offset / samples, gy_offset / samples, gz_offset / samples
def get_roll_pitch_yaw(dt, ax, ay, az, gx, gy, gz, prev_roll, prev_pitch, prev_yaw):
"""姿勢角ロール・ピッチ・ヨーの算出"""
# 測定値のバイアス補正
gx -= gx_bias
gy -= gy_bias
gz -= gz_bias
# 加速度からロール、ピッチ角度を計算
roll_acc = math.degrees(math.atan2(ay, math.sqrt(ax**2 + az**2)))
pitch_acc = math.degrees(math.atan2(-ax, math.sqrt(ay**2 + az**2)))
# 相補フィルタ処理（ジャイロの角速度g系を時間dtで積分して角度を計算および角度と各重み付けでロール、ピッチを計算）
# 係数は用途に応じて調整。ただし、係数の総和は１であること。
roll = 0.98 * (prev_roll + gx * dt) + 0.02 * roll_acc
pitch = 0.98 * (prev_pitch + gy * dt) + 0.02 * pitch_acc
# ヨー角は加速度では正確に算出できないのでジャイロ測定値を積分して算出
yaw = prev_yaw + gz * dt
# キューに追加
roll_buffer.append(roll)
pitch_buffer.append(pitch)
# 移動平均（ノイズ低減）によりロール・ピッチ角を算出
roll_avg = sum(roll_buffer) / len(roll_buffer)
pitch_avg = sum(pitch_buffer) / len(pitch_buffer)
return roll_avg, pitch_avg, yaw
def rotateX(vertex, angle):
"""指定X,Y,Z座標のX軸回転処理"""
y = vertex[1] * math.cos(angle) - vertex[2] * math.sin(angle)
z = vertex[1] * math.sin(angle) + vertex[2] * math.cos(angle)
return [vertex[0], y, z]
def rotateY(vertex, angle):
"""指定X,Y,Z座標のY軸回転処理"""
x = vertex[0] * math.cos(angle) + vertex[2] * math.sin(angle)
z = -vertex[0] * math.sin(angle) + vertex[2] * math.cos(angle)
return [x, vertex[1], z]
def rotateZ(vertex, angle):
"""指定X,Y,Z座標のZ軸回転処理"""
x = vertex[0] * math.cos(angle) - vertex[1] * math.sin(angle)
y = vertex[0] * math.sin(angle) + vertex[1] * math.cos(angle)
return [x, y, vertex[2]]
def project(vertex):
"""X,Y,Z座標の2D描画エリアへの変換"""
scale = 80
x, y, z = vertex
return (int(200 - x * scale), int(200 - z * scale))
def display_cube(stdscr):
# 画面描画更新のためのクロックオブジェクトの作成
clock = pygame.time.Clock()
# ロール・ピッチ・ヨーの前回値の初期化
prev_roll, prev_pitch, prev_yaw = 0, 0, 0
# サンプリング時間の前回値の初期化
prev_time = time.time()
while True:
# サンプリング時間dtの算出（開始時）
current_time = time.time()
dt = current_time - prev_time
prev_time = current_time
# MPU6050から加速度、ジャイロ測定値を取得
ax, ay, az, gx, gy, gz = get_mpu6050_data()
# 姿勢角度 ロール、ピッチ、ヨーに変換
roll, pitch, yaw = get_roll_pitch_yaw(dt,
ax, ay, az, gx, gy ,gz,
prev_roll, prev_pitch, prev_yaw)
prev_roll, prev_pitch, prev_yaw = roll, pitch, yaw
# ターミナルへのデータの表示
stdscr.clear()
stdscr.addstr(1, 2, "MPU6050 Sensor Data")
stdscr.addstr(2, 2, f"Sampling dt:{dt:8.3f}")
stdscr.addstr(3, 2, f"Calibration: gx:{gx_bias:8.3f}, gy:{gy_bias:8.3f}, gz:{gz_bias:8.3f}")
stdscr.addstr(4, 2, f"Accel X:%8.3f, Y:%8.3f, Z:%8.3f" % (ax, ay, az))
stdscr.addstr(5, 2, f"Gyro X:%8.3f, Y:%8.3f, Z:%8.3f" % (gx, gy, gz))
stdscr.addstr(6, 2, f"roll: {roll:8.3f}, pitch: {pitch:8.3f}, yaw: {yaw:8.3f}")
# オブジェクト描画画面のクリア
screen.fill((0, 0, 0))
# オブジェクト描画画面終了のイベント処理
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
return
# 現在の姿勢角度におけるオブジェクトの座標計算
rotated_vertices = [rotateZ(rotateY(rotateX(v, math.radians(roll)), math.radians(pitch)),math.radians(yaw)) for v in vertices]
projected_vertices = [project(v) for v in rotated_vertices]
# オブジェクトの描画
for edge in edges:
pygame.draw.line(screen, (255, 255, 255), projected_vertices[edge[0]], projected_vertices[edge[1]], 2)
# 画面の描画更新
pygame.display.flip()
# サンプリング時間の表示
#stdscr.addstr(5, 2,f"dt: {dt:6.4f}")
# ターミナルの描画更新
stdscr.refresh()
# 画面更新調整時間
clock.tick(30)
if __name__ == "__main__":
# I2Cの初期化
bus = smbus2.SMBus(1)
# MPU6050のスリープ状態の解除
bus.write_byte_data(MPU6050_ADDR, PWR_MGMT_1, 0)
# キャリブレーションの実施
print(">>> Start calibration.")
gx_bias, gy_bias, gz_bias = calibrate_gyro()
print("<<< Completed calibration!")
# pygameの初期化
pygame.init()
# 画面サイズの設定
screen = pygame.display.set_mode((400, 400))
# 画面タイトルの設定
pygame.display.set_caption("Gyro-monitor")
# 移動平均フィルタ用バッファ
window_size = 6 # 観測データの範囲（調整可）
# キューに直近の最新データ(window_size)分を追加しながら古いデータを削除
roll_buffer = collections.deque([0] * window_size, maxlen=window_size)
pitch_buffer = collections.deque([0] * window_size, maxlen=window_size)
try:
# カーシスライブラリの初期化
curses.initscr()
# ジャイロセンサーに同期したオブジェクトの描画処理
curses.wrapper(display_cube)
except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the program will be executed.
pass
finally:
# pygameライブラリの終了
pygame.quit()
# カーシスライブラリの終了
curses.endwin()動作確認
さて、プログラムの作成もできましたので準備が整いました。ジャイロセンサーの動きに合わせてワイヤーフレームモデルが追従する様子をご覧ください。
まとめ
今回は、Raspberry Pi にジャイロセンサーMPU6050を付けて、センサー情報を取得してその情報をもとにワイヤーフレームモデルを追従させることができました。センサーを繋げることでI2C通信を知ることができました。わずか2本の線で通信でき、シンプルなプロトコルにも関心しました。
Raspberry Pi ではターミナルに位置を決めて文字列を表示させるライブラリやグラフィックを描画させるライブラリの使い方も学べました。
次は、ジャイロの使い方が分かったので、ジャイロでモーターを制御してみたいと思います。
コメント