MPU6050がDMP(Digital Motion Processor)モードで動いた

現行ではカルマンフィルタで適用している加速度センサとジャイロセンサの値が平均値ではないので、大きく外れた値で計算することがある。これがたまに走り出す原因だと考えられたのでMPU6050のFIFOバッファを使えないかとあれやこれやと調べていると、これまでテストしたmbedのMPU6050 DMPライブラリィ以外のものもあるのがわかった。
ばんとさんが知っているmbedのMPU6050 DMPライブラリィはコンパイルすると40K超えで32KとちっぽけなLPC1114FN28では使えなかったので諦めてたのだ。FIFOバッファを調べている内に、でぶちんのMPU6050 DMPライブラリィ以外にIan版というのがあるのを知った。
MPU6050のFIFOバッファの使い方がまだ分かってないのだが、試しにライブラリィを引っ張ってきてLPC1114用に修正して、ネット上でコンパイルしてみると22Kで収まった!! 💡
元々のMPU6050 DMPライブラリィはArduino版もあって32KのATMega328Pで収まっているので22Kは妥当なところだと思う。 😕

出来上がったバイナリをロボコン4号に入れてみると動いた。これがログだ。

MPU6050::initialize end
MPU6050::testConnection start
DeviceId = 52
MPU6050 test connection passed.
succeed in MPU6050 DMP Initializing.
succeed in restore config data.
P:0.25555, I:0.02222, D:0.35444
Init finish!
Roll:-38.80deg, Pitch: 35.85deg, Yaw:-170.71deg
Roll:-38.75deg, Pitch: 35.85deg, Yaw:-170.57deg
Roll:-38.79deg, Pitch: 35.83deg, Yaw:-170.43deg
Roll:-38.82deg, Pitch: 35.77deg, Yaw:-170.30deg
Roll:-38.92deg, Pitch: 35.54deg, Yaw:-170.30deg
Roll:-39.08deg, Pitch: 35.12deg, Yaw:-170.46deg
Roll:-39.20deg, Pitch: 34.87deg, Yaw:-170.61deg
Roll:-39.31deg, Pitch: 34.94deg, Yaw:-170.43deg
Roll:-39.36deg, Pitch: 35.09deg, Yaw:-170.22deg
Roll:-39.38deg, Pitch: 35.10deg, Yaw:-170.18deg
Roll:-39.38deg, Pitch: 35.14deg, Yaw:-170.11deg
Roll:-39.52deg, Pitch: 35.15deg, Yaw:-169.96deg
Roll:-39.78deg, Pitch: 35.25deg, Yaw:-169.42deg
Roll:-40.10deg, Pitch: 35.49deg, Yaw:-169.00deg
Roll:-40.18deg, Pitch: 35.59deg, Yaw:-168.94deg

ジャイロも加速度も共にキャリブレーションしてないのでYawデータのようにずるずると増加してるが、きちっとしたデータを放り込むと安定するはず。

不安定なのは起動直後だけのようで20~30秒動作させてみるとデータは収束(安定)する。つまりウォームアップが必要ということだ。

カルマンフィルタとDMP(Digital Motion Processor)

カルマンフィルタはソフトウェア、DMPはDigital Motion Processorというハードを利用して姿勢を割り出すらしいのだ。
DMPモードはMPU6050の性能をフルに引き出しているはずだ。使えるようになったのに使わない手はない。といってもカルマンフィルタ版を放棄するわけではない。平坦化したデータをカルマンフィルタに与えたらDMPモード並になると思うので、カルマンフィルタ版とブランチさせたDMP版、ふたつ作ることにする。

ただDMP(Digital Motion Processor)は情報がフリーで公開されていないのかプログラミングは不思議な呪文(数値列)を設定したりと謎だけらけである。

結構楽しめる

最近作ったものなら超音波距離計。これなんか超音波距離センサのデータシートが理解できれば完成したも同然だった。一方、倒立振子はデータシートはない。筐体を眺めながら考えるしかないのだ。これがハンダごてを握らなくても結構楽しめるところでもある。 😳