はじめに
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の設定
- 以下を無限に繰り返す
- MPU6050からのデータ読み込み
- 読み込んだデータから加速、角速度を算出
- ピッチとロールの計算
- 算出したピッチとロールから水準器的な表示
アドレス、レジスタ設定
長文字変数でMPU6050のI2Cアドレスとレジスタアドレスを下記のように設定します。
| 変数名 | 値(16進） | 説明 |
|---|---|---|
| MPUADDR | $68 | MPU6050のI2Cアドレス |
| 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
I2Cチャンネルを開く
I2Cコマンドを使い、I2Cチャンネルを開きます。
REM Open I2C channel
I2C 400
I2Cチャネルを開いた後、電力管理、加速度計、ジャイロスコープを設定します。I2C通信でエラーが発生したら、そこでプログラムを停止するようにします。
デバイスへのデータ送信はI2CWRITE命令を使います。命令のパラメータはI2Cアドレス、レジスタアドレス、送信するデータです。
設定レジスタ(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 |
| 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
プログラム全体
以下にプログラム全体を示します。
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がコンパイラなので円の描画のちらつきがありません。