はじめに

PicoMiteで3軸加速度センサー＋3軸ジャイロセンサーのMPU6050を使用できたことを元にPicoCalc版MachiKaniaで水準器的なものを作成したのでそれをまとめました。

MPU6050

MPU6050は3軸加速度センサー+3軸ジャイロセンサーです。加速度センサーは物体の直線的な動きである加速度を、ジャイロセンサーは物体の回転である角速度を検知できるセンサーです。

今回はAmazonで購入したREES52 MPU-6050 使用３軸ジャイロスコープ・３軸加速度センサーモジュールを使用しました。

このセンサの機能は次の通りです。

3軸MEMSジャイロスコープ（MPU-60X0）
- X・Y・Z 各軸独立の振動型コリオリ式ジャイロを搭載
- 回転によるコリオリ力で振動→静電容量検出→増幅・復調・フィルタリング→角速度に比例した電圧を生成
- 各軸ごとにオンチップ16ビットADCでデジタル化
- フルスケール範囲：±250／±500／±1,000／±2,000 °/s（プログラム選択）
- サンプリングレート：8,000 SPS～3.9 SPS（プログラム選択）
- 選択可能なローパスフィルタで多様なカットオフ周波数を設定可能
3軸MEMS加速度計（MPU-60X0）
- 各軸に独立したプルーフマス＋差動静電容量センサーを配置
- 製造ばらつき・熱ドリフトに強いアーキテクチャ
- 水平設置時：X/Y軸＝0 g、Z軸＝+1 g を測定
- 工場校正済みのスケール係数（電源電圧に依存しない）
- 各軸ごとに専用シグマデルタ16ビットADCでデジタル出力
- フルスケール範囲：±2／±4／±8／±16 g（プログラム選択）

X、Y、Z軸の方向は次の写真のとおりです。

MPU6050は設定後にレジスタに測定された加速度と角速度が保存されているのでそれを読み取り、必要な処理をする。

検証環境

今回は下記の環境で動作するプログラムを作成しました。

macOS : 15.6
PicoCalc : Raspberry Pi Pico 2 W搭載
MachiKania : Phyllosoma 1.6.0
REES52 MPU-6050 使用３軸ジャイロスコープ・３軸加速度センサーモジュール

配線

MPU6050とPicoCalcは表に示す接続をしています。

MPU6050	PicoCalc
VCC	VCC
GND	GND
SCL	GP5
SDA	GP4

実体配線図は次のとおりです。

MPU6050を載せたブレッドボードをUSBタイプA-C変換コネクタに両面テープで貼り付けてPicoCalcに差し込み、PicoCalcの傾斜に応じてMPU6050が傾くようにしています。

プログラム

プログラムは次の流れで実行している。

MPU6050の設定
以下を無限に繰り返す
1. MPU6050からのデータ読み込み
2. 読み込んだデータから加速、角速度を算出
3. ピッチとロールの計算
4. 算出したピッチとロールから水準器的な表示

アドレス、レジスタ設定

長文字変数でMPU6050のI²Cアドレスとレジスタアドレスを下記のように設定します。

変数名	値(16進）	説明
MPUADDR	$68	MPU6050のI²Cアドレス
RGYRCON	$1B	ジャイロメータ設定レジスタ
RACCCON	$1C	加速度計設定レジスタ
RPWMMGMT	$6B	電力管理1設定レジスタ
RACCXOUT	$3B	加速時計X軸データ上位8ビットの値格納レジスタ

USEVAR MPUADDR : MPUADDR  = $68
USEVAR RCON    : RCON     = $1A
USEVAR RGYRCON : RGYRCON  = $1B
USEVAR RACCCON : RACCCON  = $1C
USEVAR RACCXOUT: RACCXOUT = $3B
USEVAR RPWRMGMT: RPWRMGMT = $6B

I²Cチャンネルを開く

I2Cコマンドを使い、I²Cチャンネルを開きます。

REM Open I2C channel
I2C 400

I²Cチャネルを開いた後、電力管理、加速度計、ジャイロスコープを設定します。I²C通信でエラーが発生したら、そこでプログラムを停止するようにします。

デバイスへのデータ送信はI2CWRITE命令を使います。命令のパラメータはI²Cアドレス、レジスタアドレス、送信するデータです。

設定レジスタ(RCON)設定

ジャイロスコープと加速度センサーの両方について、外部フレーム同期（FSYNC）ピンのサンプリングとデジタルローパスフィルタ（DLPF）の設定するレジスタです。レジスタの構成は下記のとおりです。

レジスタ	Bit7	Bit6	Bit5	Bit4	Bit3	Bit2	Bit1	Bit0
1A	-	-	EXT_SYNC_SET[2:0]			DLPF_CFG[2:0]
00	0	0	0	0	0	0	0	0

ジャイロスコープと加速度計は外部のフレーム同期を使用せず(EXT_SYNC_SETを0)、ディジタルローバスフィルタの設定で加速度計は帯域幅260Hz、遅延0msで、ジャイロスコープは帯域幅256Hz、遅延0.98ms、サンプリング周波数8kHzにします（DLPF_CFGを0）。3ビットのDLPF_CFGは下の表の8とおりの値の組み合わせを示す。

DLPF_CFG	加速時計		ジャイロスコープ
	(Fs=1kHz)
	帯域幅(Hz)	遅延(ms)	帯域幅(Hz)	遅延(ms)	Fs(kHz)
0	260	0	256	0.98	8
1	184	2.0	188	1.9	1
2	94	3.0	98	2.8	1
3	44	4.9	42	4.8	1
4	21	8.5	20	8.3	1
5	10	13.8	10	13.4	1
6	5	19.0	5	18.6	1
7	予約済み		予約済み		8

設定コードを示します。

I2CWRITE MPUADDR, RCONF, $0
IF I2CERROR() != 0 THEN PRINT "I2C ERR (Conf)": END

加速度計、ジャイロスコープ感度

加速度計設定（RACCCON）

加速度計のセルフテストの有無、フルスケール範囲を設定するレジスタ。また、このレジスタはデジタルハイパスフィルタ（DHPF）も設定します。レジスタの構成は下記のとおりです。

レジスタ	Bit7	Bit6	Bit5	Bit4	Bit3	Bit2	Bit1	Bit0
1B	XG_ST	YG_ST	ZG_ST	FS_SEL[1:0]		-	-	-
10	0	0	0	1	0	0	0	0

加速度計のX、Y、Z軸のセルフテストは実行せず（Bit7〜Bit5を0）、フルスケールレンジを±8g（AFS_SEL[1:0]を2）にします。よってLSB感度は下表から4096LSB/gになります。

AFS_SEL	フルスケールレンジ	LSB感度(SENSA)
0	±2g	16384 LSB/g
1	±4g	8192 LSB/g
2	±8g	4096 LSB/g
3	±16g	2048 LSB/g

I2CWRITE MPUADDR, RACCCON, $10
IF I2CERROR() != 0 THEN PRINT "I2C ERR (AccelCfg)": END

ジャイロスコープ設定（RGYROCON）

ジャイロスコープのセルフテストの有無とフルスケールレンジを設定するレジスタ。レジスタの構成は下記のとおりです。

レジスタ	Bit7	Bit6	Bit5	Bit4	Bit3	Bit2	Bit1	Bit0
1B	XG_ST	YG_ST	ZG_ST	FS_SEL[1:0]		-	-	-
18	0	0	0	1	1	0	0	0

ジャイロスコープのX、Y、Z軸のセルフテストは実行せず（Bit7〜Bit5を0）、フルスケールレンジを±2000°/s（FS_SEL[1:0]を3）にします。よってLSB感度は下表から16.4LSB/°/sになります。

FS_SEL	フルスケールレンジ	LSB感度(SENSG)
0	±250°/s	131 LSB/°/s
1	±500°/s	65.5 LSB/°/s
2	±1000°/s	32.8 LSB/°/s
3	±2000°/s	16.4 LSB/°/s

I2CWRITE MPUADDR, RGYRCON, $18
IF I2CERROR() != 0 THEN PRINT "I2C ERR (Gyro)": END

電力管理1の設定（RPWRMGMT）

ユーザーが電力モードとクロックソースを設定するレジスタです。デバイス全体のリセット、温度センサーを無効にするためのビットも備えています。

レジスタ	Bit7	Bit6	Bit5	Bit4	Bit3	Bit2	Bit1	Bit0
6B	DEVICE_RESET	SLEEP	CYCLE	-	TEMP_DIS	CLKSEL[2:0]
00	0	0	0	0	0	0	0	0

クロックに内蔵8MHz発振器を使い（CLKSELを0）、スリープをせず（SLEEPを0）、温度センターを有効にします（TEMP_DISを0）。

CLKSEL	クロックソース
0	内蔵8MHz発振器
1	X軸ジャイロスコープリファレンス付きPLL
2	Y軸ジャイロスコープリファレンス付きPLL
3	Z軸ジャイロスコープリファレンス付きPLL
4	外部32.768MHzリファレンス付きPLL
5	外部19.2MHzリファレンス付きPLL
6	予約済み
7	クロックを停止し、タイミングジェネレータをリセット状態に保つ

I2CWRITE MPUADDR, RPWRMGMT, $0
IF I2CERROR() != 0 THEN PRINT "I2C ERR (Wake)": END

感度設定

レジスタの設定から加速時計、ジャイロスコープの感度を下記のように設定します。

変数名	値(16進）	説明
SENSA#	4096.0	加速度計LSB感度値
SENSG#	16.4	ジャイロスコープLSB感度値

USEVAR SENSA#  : SENSA#   = 4096.0
USEVAR SENSG#  : SENSG#   = 16.4

レジスタデータの読み取り

デーバスの設定完了後、X軸、Y軸、Z軸の加速度データをレジスタ0x3Bから上位8ビットと下位8ビットの順にバッファBに読み込みます。読み込むデータは全部で6バイトです。
読み込みに失敗した場合はプログラムを停止します。

I2CWRITE MPUADDR, RACCXOUT
I2CREADDATA MPUADDR, B, 6
IF I2CERROR() != 0 THEN PRINT "I2C ERR (Read)": END

バッファBに格納されるデータは次の順に並んでいる

B	B+1	B+2	B+3	B+4	B+5
X軸上位8ビット	X軸下位8ビット	Y軸上位8ビット	Y軸下位8ビット	Z軸上位8ビット	Z軸下位8ビット

サブルーチンTOSIGNEDに読み込むレジスタのオフセットの値を指定して符号付き実数を取得し、加速時計の感度で割り各軸の値を変数に格納します。

オフセット	取得するデータ
0	X軸データ
2	Y軸データ
4	Z軸データ

axf# = GOSUB#(TOSIGNED, 0) / SENSA#
ayf# = GOSUB#(TOSIGNED, 2) / SENSA#
azf# = GOSUB#(TOSIGNED, 4) / SENSA#

サブルーチンTOSIGNEDはバッファBからサブルーチンの引数の値で示す軸のデータを読み取みより、16ビットの値にします。このデータは2の補数表現なので負数の場合には負数に変換します。

LABEL TOSIGNED
  VAR V

  V = PEEK(B + ARGS(1)) * 256 + PEEK(B + ARGS(1) + 1)
  IF V >= 32768 THEN V = V - 65536

RETURN FLOAT#(V)

ピッチとロールの計算

X、Y、Zの3軸の加速度データが得られたので次の式でピッチとロールを計算します。$axf$、$ayf$、$azf$はX、Y、Z軸それぞれの加速度の値です。

$pitch=\arctan2({-axf},{\sqrt{ayf^2 + azf^2}})$
$roll=\arctan2(ayf, azf)$

MachiKaniにはATAN2関数があるのでそれを使います。関数の戻り値はラジアンなので度数に変換する係数を掛けます。

pitch# = ATAN2#(-axf#, SQRT#(ayf# * ayf# + azf# * azf#)) * RAD2DEG#
roll#  = ATAN2#( ayf#, azf#) * RAD2DEG#

描画

MPU6050で測定した加速度データから得られたピッチとロールの角度の値を使って水準器的な表示を作ります。

画面中央(159, 159)を原点として、$x=0$と$y=0$の軸線を描画します。
ピッチとロールの角度が−90度から＋90度の範囲のときに画面に半径10の水色の円を描画します。円の中心座標が0から319の範囲になるようにピッチとロールの角度から座標に変換します。
あわせてロールとピッチの値を表示します。
100ミリ秒後に黒い円を描いて表示をクリアします。
この描画を無限に繰り返します。
X軸方向の回転がロール、Y軸方向の回転がピッチになります。

DO
  X = 160 - INT(roll#  / 90 * 160)
  Y = 160 + INT(pitch# / 90 * 160)
  R = 10

  REM Draw lines & Circles
  LINE 159, 0, 159, 319, 7
  LINE 0, 159, 329, 159, 7
  CIRCLE 159, 159, 20, 7
  CIRCLE X, Y, R, 5

  REM Print Pitch, Roll
  CURSOR 0,3 :PRINT "Pitch:";pitch# * RAD2DEG#
  CURSOR 0,4 :PRINT "Roll :";roll#  * RAD2DEG#
  DELAYMS 100

  REM Clear circle
  CIRCLE X, Y, R, 0
LOOP

プログラム全体

以下にプログラム全体を示します。

MPU6050.BAS

USEGRAPHIC

CLS

REM Define address & register
USEVAR MPUADDR : MPUADDR  = $68
USEVAR RCONF   : RCONF    = $1A
USEVAR RACCCON : RACCCON  = $1C
USEVAR RGYRCON : RGYRCON  = $1B
USEVAR RACCXOUT: RACCXOUT = $3B
USEVAR RPWRMGMT: RPWRMGMT = $6B

REM Define Accelerometer 
REM Gyroscope LSB sensitivity value
USEVAR SENSA#  : SENSA#   = 4096.0
USEVAR SENSG#  : SENSG#   = 16.4

USEVAR RAD2DEG#: RAD2DEG# = 57.2957795

DIM b(13)
USEVAR yaw#, pitch#, roll#
USEVAR axf#, ayf#, azf#
USEVAR gxf#, gyf#, gzf#
USEVAR costh#, yd#

USETIMER 100
USEVAR t_prev : t_prev = TIMER()
USEVAR t_now
USEVAR dt

REM Open I2C channel
I2C 400

I2CWRITE MPUADDR, RCONF, 0
IF I2CERROR() != 0 THEN PRINT "I2C ERR (Conf)": END

I2CWRITE MPUADDR, RPWRMGMT, 0
IF I2CERROR() != 0 THEN PRINT "I2C ERR (Wake)": END

I2CWRITE MPUADDR, RACCCON, $10
IF I2CERROR() != 0 THEN PRINT "I2C ERR (AccelCfg)": END

I2CWRITE MPUADDR, RGYRCON, $18
IF I2Cerror() != 0 THEN PRINT "I2C ERR (Gyro)": END

PRINT "MPU-6050 Tilt-Meter"
PRINT "Pitch, Roll (deg)"

DO
  I2CWRITE MPUADDR, RACCXOUT
  I2CREADDATA MPUADDR, b, 6
  IF I2CERROR() != 0 THEN PRINT "I2C ERR (Read)": END : REM DELAYMS 100: CONTINUE

  axf# = GOSUB#(TOSIGNED,  0) / SENSA#
  ayf# = GOSUB#(TOSIGNED,  2) / SENSA#
  azf# = GOSUB#(TOSIGNED,  4) / SENSA#

  pitch# = ATAN2#(-axf#, SQRT#(ayf# * ayf# + azf# * azf#)) * RAD2DEG#
  roll#  = ATAN2#( ayf#, azf#) * RAD2DEG#

  X = 160 - INT(roll#  / 90 * 160)
  Y = 160 + INT(pitch# / 90 * 160)
  R = 10

  REM Draw lines & Circles
  LINE 159, 0, 159, 319, 7
  LINE 0, 159, 329, 159, 7
  CIRCLE 159, 159, 20, 7
  CIRCLE X, Y, R, 5

  REM Print Pitch, Roll
  CURSOR 0,3 :PRINT "Pitch:";SPRINTF$("%-5.0f",pitch#)
  CURSOR 0,4 :PRINT "Roll :";SPRINTF$("%-5.0f",roll#)
  DELAYMS 100

  REM Clear circle
  CIRCLE X, Y, R, 0
LOOP

END

LABEL TOSIGNED
VAR V

V = PEEK(B + ARGS(1)) * 256 + PEEK(B + ARGS(1) + 1)
IF V >= 32768 THEN V = V - 65536

RETURN FLOAT#(V)

最後に

PicoCalcへのMPU6050の取り付けが適当なものでしたがPicoCalc版MachiKaniaでMPU6050を使った水準器的なものが作れました。MachiKaniaがコンパイラなので円の描画のちらつきがありません。