こんにちは、shotaです。
前回の記事ではSPI機能を使ってモーショントラッキングセンサICM-20648と通信しました。ESP32のプログラムをC++でコーディングする方法も説明しています。
今回はその続きで、C++でICM-20648のライブラリを作成します。と言っても加速度と角速度を受け取るための最低限の機能しか実装しません。
ブロクで書いたサンプルコードはGitHubに公開しています。
まずは動かしてみる
作成したライブラリを説明する前に、まずは動かしてみます。
モーショントラッキングサンプルのディレクトリに移動し、プログラムを書き込みます。
$ cd ~/esp/especial/examples/6_motion_tracking $ make flash monitor
実際に動かしてみた動画がこちらです。
画面にはセンサから得られた加速度が表示されています。
マウスの向きを変えて重力加速度（1.0 g）がかかる方向（xyz軸）を変化させています。
センサから得られる加速度の単位は重力加速度のgです。
1.0 gは約\(9.8m/s^2\)です。
角速度の値を読み取るためにはmain.cppを書き換えます。
$ cd ~/esp/especial/examples/6_motion_tracking/ $ vim main/main.cpp
while(1){
// std::cout << std::setw(12) << std::left << driver.getAccelX() << ",";
// std::cout << std::setw(12) << std::left << driver.getAccelY() << ",";
// std::cout << std::setw(12) << std::left << driver.getAccelZ() << ",";
// std::cout << std::endl;
std::cout << std::setw(12) << std::left << driver.getGyroX() << ",";
std::cout << std::setw(12) << std::left << driver.getGyroY() << ",";
std::cout << std::setw(12) << std::left << driver.getGyroZ() << ",";
std::cout << std::endl;
vTaskDelay(10 / portTICK_PERIOD_MS);
}
実行した結果がこちらです。
画面には各軸（xyz）まわりの角速度が表示されています。
マウスを回転させたときに角速度が±250 dps（degrees per second）に達していることがわかります。
(±250 dpsについては後ほど説明します）
それでは、作成したライブラリについて説明します。
ICM-20648ライブラリの説明
まずはライブラリに実装した機能の一覧を紹介して、そのあと個別に説明します。
ライブラリに実装したの機能一覧
このブログシリーズで作成しているのはあくまでサンプルプログラムなので、
誰でもどこでも使えるよう万能なライブラリは作成しません。
実装した機能は以下の通りです。
- ESP-IDFのSPI初期設定
- 通信ポート設定
- 通信速度設定
- ICM-20648の初期設定
- 加速度・角速度センシングのON/OFF
- 加速度・角速度センシングの感度設定
- 加速度（x,y,z軸）の取得
- 角速度（x,y,z軸）の取得
Especialの回路図やESP32のSPI機能の使い方については前回の記事を参照してください。
app_main関数の確認
まずはICM-20648ライブラリを呼び出しているapp_main関数を見てみましょう。
app_main関数はmain.cppに実装されています。
extern "C" void app_main(){
icm20648 driver(GPIO_MOSI, GPIO_MISO, GPIO_SCLK, GPIO_CS);
std::cout<< "WHO AM I:" << std::hex << driver.readWhoAmI() << std::endl;
vTaskDelay(3000 / portTICK_PERIOD_MS);
while(1){
std::cout << std::setw(12) << std::left << driver.getAccelX() << ",";
std::cout << std::setw(12) << std::left << driver.getAccelY() << ",";
std::cout << std::setw(12) << std::left << driver.getAccelZ() << ",";
std::cout << std::endl;
// std::cout << std::setw(12) << std::left << driver.getGyroX() << ",";
// std::cout << std::setw(12) << std::left << driver.getGyroY() << ",";
// std::cout << std::setw(12) << std::left << driver.getGyroZ() << ",";
// std::cout << std::endl;
vTaskDelay(10 / portTICK_PERIOD_MS);
}
}
app_main関数の最初で、icm20648クラスのインスタンスを生成しています。
このとき、SPI通信に使うGPIOピンを引数に渡しています。
ICM-20648の設定と、ESP32のSPI通信の設定はインスタンス生成時（icm20648クラスのコンストラクタ）に完了します。簡単ですね。
その後、readWhoAmI関数でICM-20648のWHO_AM_Iレジスタの値を取得しています。
また、getAccel*やgetGyro*関数で各軸のセンサ値を取得しています。
SPI通信とICM-20648の初期設定
さて、icm20648クラスのコンストラクタで何を実行しているのか確認します。
icm20648.cppを開きます。
icm20648::icm20648(const int mosi_io_num, const int miso_io_num,
const int sclk_io_num, const int cs_io_num){
spidevInit(mosi_io_num, miso_io_num, sclk_io_num, cs_io_num);
pwr_mgmt_t mgmt;
// 設定を初期化
mgmt.reset_device = true;
writePwrMgmt(&mgmt);
mgmt.reset_device =false;
mgmt.enable_sleep_mode = false;
mgmt.enable_low_power = false;
mgmt.disable_temp_sensor = false;
mgmt.clock_source = 1; // auto select
mgmt.disable_accel = 0b111; // disable all
mgmt.disable_gyro = 0b111; // disable all
writePwrMgmt(&mgmt);
writeAccelConfig(1, false, 0);
writeGyroConfig(0, false, 0);
mgmt.disable_accel = 0b000; // enable all
mgmt.disable_gyro = 0b000; // enable all
writePwrMgmt(&mgmt);
}
コンストラクタではspidevInit関数を実行し、ESP32のSPI通信を設定します。
spidevInit関数の中身は前回の記事で紹介したものとほぼ同じです。
通信ポートや通信速度がこの関数で決まります。
次に、pwr_mgmt_t構造体変数を使ってICM-20648のPWR_MGMT_1、PWR_MGMT_2レジスタを設定します。
writePwrMgmt関数にpwr_mgmt_t構造体変数を渡すことで、レジスタに値を書き込むことができます。
レジスタの詳細についてはICM-20648のデータシートを参照してください。
レジスタの設定項目を構造体のパラメータ名にしているので、ソースコードを読めば何となく理解できると思います。
コンストラクタの後半では、writeAccelConfig関数とwriteGyroConfig関数で加速度センサとジャイロセンサを設定します。
これらの関数については次に説明します。
コンストラクタの最後で、加速度・角速度センシングをONしています。
加速度・角速度センシングの感度設定
つづいて、writeAccelConfig関数を見ます。
bool icm20648::writeAccelConfig(const uint8_t fssel,
const bool enableLPF=false, const uint8_t configLPF=0){
const uint8_t ADDR_ACCEL_CONFIG_1 = 0x14;
if (writeAccelGyroConfig(ADDR_ACCEL_CONFIG_1,
fssel, enableLPF, configLPF)){
mAccelFSSel = fssel;
return true;
}else{
ESP_LOGE(TAG, "writeAccelConfig failed.\
fssel:%x, enableLPF:%x, configLPF:%x",
int(fssel), int(enableLPF), int(configLPF));
return false;
}
}
この関数ではICM-20648のACCEL_CONFIGレジスタに値を書き込みます。
fsselは加速度センサの感度を設定するパラメータです。
データシートより、加速度センシングの感度は±2g、±4g、±8g、±16gの4段階あり、fssel=0で±2gが設定されます。
注意点として、感度にかかわらず加速度は16ビットで表現されるため、感度が±16gのときはセンサ値が荒くなり、細かい値を検出できません。
大きな加速度を検出しない場合は最小の±2gを設定すればよいと思います。
enableLPFとconfigLPFはローパスフィルタ（LPF）を設定するパラメータです。
例えば、enableLPFをtrueにし、configLPFを5にすると、11.5Hzで3dB減衰するLPFを設定できます。
ICM-20648データシート 67ページより、ACCEL_DLPFCFGについて
先程のコンストラクタを次のように書き換えると、LPFが設定され、定常時の値のブレが減少することを確認できます。
writeAccelConfig(1, true, 5); // enableLPF=true, configLPF=5
writeGyroConfig(0, false, 0);
writeGyroConfig関数も同じように実装しているため説明は省略します。
この記事の冒頭で触れた±250 dpsという値は角速度センサの感度です。
手で回転させただけで250 dpsに達しているので、感度を変えたほうが良さそうですね。
加速度（x,y,z軸）の取得
app_main関数で使用したgetAccelX, Y, Z関数は、内部でgetAccel関数を呼び出しています。
float icm20648::getAccelX(void){
return getAccel(AXIS_X);
}
float icm20648::getAccel(const AXIS axis){
const uint8_t ADDR_ACCEL_OUT_H[AXIS_SIZE] = {0x2d, 0x2f, 0x31};
const uint8_t ADDR_ACCEL_OUT_L[AXIS_SIZE] = {0x2e, 0x30, 0x32};
// 符号をつけるため、int16_t型の変数に格納する
int16_t rawData = readRegister2Byte(
ADDR_ACCEL_OUT_H[axis],
ADDR_ACCEL_OUT_L[axis]
);
return float(rawData) / ACCEL_SENSITIVITY[mAccelFSSel];
}
加速度のセンサ値は各軸ごとに2バイト（16ビット）で表現されます。
レジスタには1バイトずつ値が格納されているため、それらをreadRegister2Byte関数内で結合しています。
getGyro関数も同じような構成です。
その他説明していない関数がいくつかありますが、それらについてはGitHubのソースコードを参照してください。
次回の記事
今回でEspecialの周辺装置の使い方を全て説明しました。
これまでに執筆した記事は次のとおりです。
- GPIOによるLEDの点灯消灯
- AD変換によるバッテリー電圧取得
- GPIOとAD変換による物体検出センサの信号取得
- MCPWMによるモータ制御
- SPI通信によるエンコーダの値取得
- C++とSPI通信によるモーショントラッキングセンサの値取得
記事の一覧はこちらをご覧ください。
次回以降でESP32の内部フラッシュメモリの使い方と、マウスプログラムを紹介して、このESP32マウスシリーズを終えたいと思います。