ESP32-C6（RISC-V）とMPU9250でI2C通信による姿勢角取得を試してみた。

1.はじめに

近年、RISC-Vを採用したマイコンやSoC (System on Chip) が増えており、FPGA上でRISC-V SoCを構築する事例も増えている。
私自身、将来的にFPGA上やASICでRISC-VベースのSoC開発を行いたいと考えている。しかし、いきなりRTL (Register Transfer Lebel) 設計やSoC構築に取り組むのはハードルが高いため、まずは既存のRISC-Vチップを用いてセンサ制御を体験してみることにした。
今回は、RISC-Vコアを搭載した ESP32-C6 DevKit と、9軸IMUセンサである MPU9250 をI2C接続し、加速度データから姿勢角（Roll/Pitch）を取得してみる。

2.なぜESP32-C6なのか？

ESP32シリーズといえばWi-FiやBluetoothで有名ですが、ESP32-C6は従来のXtensaコアではなく、RISC-Vコアを採用しています。

今回の目的は単なるセンサ制御ではなく、

RISC-V ベースCPU
↓
I2C通信
↓
センサデータ取得
↓
角度計算

という組込みシステムの基本構造を体験することである。

3.使用機材

機材	用途
XIAO ESP32-C6	RISC-Vマイコン
MPU9250	9軸IMU
USB Type-Cケーブル	書込み・給電
ブレッドボード	回路作成
ジャンパワイヤ	配線

4.開発環境

・Windows 11 Home, Version: 25H2

・Arduino IDE 2.3.9

上記からインストール可能

・ESP32 Board Package

Arduino IDE を開き

【Tools】 → 【Board】 → 【Boards Manager...】

BOARDS MANAGERでesp32を入力して

esp32 by Espressif System

を【INSTALL】してESP32を追加する。
※ESP32-C6(RISC-V)をArduino IDEで使おうとしたところ、ESP32ボードパッケージのインストールに失敗した。原因を調査した結果、楽天ポケットWiFi経由ではGitHub Releasesからのダウンロードが途中で切断され、スターバックスWiFiへ切り替えることで解消した。自分だけ？？

設定するボードは

【Tools】 → 【Board】 → 【esp32】　→ 【XIAO_ESP32C6】

である。

5.配線

今回はI2C通信を使用します。

ESP32-C6	MPU9250
3.3V →	VCC
GND →	GND
D4 →	SDA
D5 →	SCL

配線すると以下のようになる

6.I2Cデバイスを検出する

MPU9250が正しく接続されているか確認するためにI2Cスキャナを実行します。

#include <Wire.h>


void setup() {
  Serial.begin(115200);
  Wire.begin(D4, D5);

  Serial.println("I2C Scan");

  for (byte addr = 1; addr < 127; addr++) {
  
    Wire.beginTransmission(addr);

    if (Wire.endTransmission() == 0) {
      Serial.print("Found: 0x");
      Serial.println(addr, HEX);
    }
  }
}

void loop() {
}

シリアルモニタで確認すると、

Found: 0x68

と表示されていた。これでMPU9250が正しく認識されていることが分かる。

7.MPU9250から加速度を取得する

MPU9250の動作確認として、加速度センサの値を取得する。
MPU9250では加速度データがレジスタ 0x3B ～ 0x40 に格納されている。

レジスタ	内容
0x3B	ACCEL_XOUT_H
0x3C	ACCEL_XOUT_L
0x3D	ACCEL_YOUT_H
0x3E	ACCEL_YOUT_L
0x3F	ACCEL_ZOUT_H
0x40	ACCEL_ZOUT_L

各軸は16bitの符号付き整数として格納されているため、上位バイト(H)と下位バイト(L)を結合して取得する。

加速度読み出し関数

まずは加速度レジスタから6バイトを読み出す関数を作成する。

#include <Wire.h>

int16_t read16()
{
  int16_t high = Wire.read();
  int16_t low  = Wire.read();

  return (high << 8) | low;
}

void readAccel(int16_t &ax,
               int16_t &ay,
               int16_t &az)
{
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU9250_ADDR, 6);

  ax = read16();
  ay = read16();
  az = read16();
}

I２C通信の初期初期セットアップをする。

    void setup()
{
  Serial.begin(115200);
  delay(1000);

  Wire.begin(D4, D5);
  Wire.setClock(100000);

  Serial.println("MPU9250 Initialize");

  // Sleep解除
  writeReg(0x6B, 0x00);

  // 加速度レンジ ±2g
  writeReg(0x1C, 0x00);

  Serial.println("Initialization Complete");
}

シリアルモニタへ出力

取得した加速度データをシリアルモニタへ表示する。

void loop()
{
  int16_t ax_raw;
  int16_t ay_raw;
  int16_t az_raw;

  readAccel(ax_raw,
            ay_raw,
            az_raw);

  Serial.print("ax = ");
  Serial.print(ax_raw);

  Serial.print(", ay = ");
  Serial.print(ay_raw);

  Serial.print(", az = ");
  Serial.println(az_raw);

  delay(500);
}

G単位への変換

今回は加速度レンジを ±2g に設定している。

writeReg(0x1C, 0x00);

MPU9250のデータシートによると、±2g設定時の感度は16384 LSB/gである。

したがって加速度は次式で変換できる。

加速度[g] = 生データ / 16384

例えば、ax = 16384の場合は1.0 gを意味する。
コードでは以下のように変換する。

float ax = ax_raw / 16384.0;
float ay = ay_raw / 16384.0;
float az = az_raw / 16384.0;

8.加速度センサのみを利用して姿勢角を計算。

前章では、MPU9250から加速度 ax, ay, az を取得した。
ここでは、加速度センサの値だけを使って、基板の傾きであるRoll角 と Pitch角 を計算する。

Roll/Pitchの計算式

Roll角は、Y軸とZ軸の加速度から計算する。

Roll = atan2(ay, az)

Pitch角は、X軸、Y軸、Z軸の加速度から計算する。

Pitch = atan2(-ax, sqrt(ay * ay + az * az))

Arduinoでは atan2() の戻り値はラジアンなので、度数法に変換する。

float roll = atan2(ay, az) * 180.0 / PI;
float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;

プログラム

前章の加速度取得プログラムに、Roll/Pitchの計算を追加する。

#include <Wire.h>

#define MPU9250_ADDR 0x68

void writeReg(uint8_t reg, uint8_t data)
{
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

int16_t read16()
{
  int16_t high = Wire.read();
  int16_t low  = Wire.read();

  return (high << 8) | low;
}

void readAccel(int16_t &ax, int16_t &ay, int16_t &az)
{
  Wire.beginTransmission(MPU9250_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU9250_ADDR, 6);

  ax = read16();
  ay = read16();
  az = read16();
}

void setup()
{
  Serial.begin(115200);
  delay(1000);

  Wire.begin(D4, D5);
  Wire.setClock(100000);

  Serial.println("MPU9250 Initialize");

  // Sleep解除
  writeReg(0x6B, 0x00);

  // 加速度レンジ ±2g
  writeReg(0x1C, 0x00);

  Serial.println("Initialization Complete");
}

void loop()
{
  int16_t ax_raw, ay_raw, az_raw;

  readAccel(ax_raw, ay_raw, az_raw);

  float ax = ax_raw / 16384.0;
  float ay = ay_raw / 16384.0;
  float az = az_raw / 16384.0;

  float roll =
    atan2(ay, az) * 180.0 / PI;

  float pitch =
    atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;

  Serial.print("Roll = ");
  Serial.print(roll, 2);
  Serial.print(" deg, Pitch = ");
  Serial.print(pitch, 2);
  Serial.println(" deg");

  delay(500);
}

実行結果の例

基板を水平に置いた場合は、おおよそ以下のようになる。

基板を傾けると、RollまたはPitchの値が変化する。

※注意点

この方法は加速度センサのみを使うため、静止状態の傾き推定には有効である。
一方で、基板を動かしたときには、重力加速度だけでなく運動による加速度も加わる。そのため、動作中の姿勢角は不安定になる。より安定した姿勢推定を行うには、次の段階でジャイロセンサを使い、相補フィルタやカルマンフィルタを導入する。

9.RISC-Vについて

Arduino IDEでもRISC-Vを使っている。Arduino IDEを使うと、RISC-Vを意識する機会はあまりない。

しかし実際には、

Arduinoコード
↓
ESP32 Arduino Core
↓
RISC-V GCC
↓
ESP32-C6

という流れでコンパイルされている。つまり今回の実験は、

RISC-V CPU
↓
I2Cコントローラ
↓
MPU9250
↓
角度演算

を実際に動かしていることになる。

10.おわりに

今回はESP32-C6とMPU9250を接続し、I2C通信による加速度取得と姿勢角計算を試してみた。FPGA上でRISC-V SoCを構築する等の応用に移る前に

RISC-Vとは何か
I2C通信とは何か
センサデータはどのように扱うのか

を実機で体験できる良い教材であった。

次回は、ESP32-C6で
ジャイロセンサの利用、相補フィルタ、倒立振子
などにも挑戦してみたいと思う。