InvenSense製慣性センサでのDigital Motion Processorの使い方II

InvenSenseからDigital Motion Processor(DMP)ライブラリが公開されました。
Developer's Cornerからダウンロードできます。
I2Cdevlibのライブラリではquaternion出力のみが利用可能でしたが、今回公開されたライブラリでは、歩数計、タップ検出、Android端末用方向検出も利用可能になっています。

これまで公開されていたライブラリとは違い、コードはすべてANSI Cで記述されているのでほとんどのマイコンで使えるはずです。
ダウンロードできるサンプルコードはMSP430向けのものですが、I2C周りの関数の追加すればDMPを利用することができます。

以下にPSoC 5LPに移植する上で気になった点をまとめておきます。

1. i2c_write, i2c_read関数

ドキュメント類に特に指定はありませんでしたが、MSP430のコードを参考にすると符号付き16ビット整数の戻り値が必要なようです。
0で正常終了、それ以外(サンプルコードでは-1)でエラーありを表しているようです。
この部分をSPIに書き換えればMPU6000等をSPI接続で使うこともできます。
SPI接続を行う場合には、読み込みを行う場合にレジスタアドレスのMSBを1にしておく必要があり注意が必要です。

2. 時間管理関数

ディレイ用の関数delay_msが必要ですが、必要なのは50 msまででした。
今回はCyDelayを利用しました。
また、内部時刻取得用の関数get_msも要求されていますが、データ取得のタイムスタンプ用なので用意しなくても問題はないかもしれません。
ここにはFreeRTOSのTick取得関数を使いました。

3. 数学関数

labs, fabsf, minが必要です。
math.h, stdlib.hで定義済みでないかを確認し、必要に応じて追加します。
GCCを使っている場合、fabsf, labsの追加は不要です。

4. ログ出力関数

log_i(...), log_e(...)というログ出力関数がありますが、利用しない場合は空関数をdefineしておきます。

5. 割り込み制御関数

これが最もプラットホームに依存する部分だと思います。
割り込みを利用するポート、ピン、コールバック関数がreg_int_cbの引数です。
PSoC 5LPとは相性が悪いので空関数として、割り込み処理は他の部分に記述しました。

テストプログラムmotion-driver-clientを動作させた動画を以下に示します。

以前に示したピッチ角表示と同じようなものですが、きちんと3軸が動作しているのがわかります。

実際に動作させていくつか気になった点があったのでまとめておきます。

1. SPIバスのスピード

DMP出力は1024バイトのFIFOから読み出しますが、SPIクロックが高くなると読み出しが不安定になる傾向がありました。
また、内蔵プロセッサにコードを流し込む際にも、クロックが高いと失敗することがありました。
動作が不安定な場合はクロックを下げてみるとよいかもしれません。

2. FIFOリセット関数

内部に50 msのソフトウエアディレイが入っているので、FIFOリセット関数mpu_reset_fifoが頻繁に呼び出されるとサンプルレートを上げることができません。
FIFOがオーバーフローしないように必要最小限のDMP出力レートをdmp_set_fifo_rateで設定する必要があります。

3. ジャイロのフルスケールレンジ

DMPは2000 dpsのレンジを仮定して計算を行うようで、それ以外のレンジだと異常なquaternionが出力されます。
加速度のレンジは2, 4, 8, 16 gのどれでも構わないようです。

ライブラリの公開でDMPはかなり利用しやすくなりました。
現在作製している電装基板にはquaternionから求めた姿勢角のリアルタイム出力、ログ機能も追加し、機体に搭載してどの程度使えるものなのかを調べてみるつもりです。