今日も前回に続きMPU6050のネタです。
前回はMPU6050からのRAWデータ（生のデータ）をフィルタリングして使用しました。今回はMPU6050に搭載されているDMP (Digital Motion Processor)を利用したデモを行ってみます。DMPはオンボードのプロセッサであり、センサーのRAWデータから補正された姿勢データを計算し出力してくれます。
DMPを有効にするにはチップへの命令を行う必要がありますが、一からそれを行うのは大変なのでオープンにされているソースコードを借用して利用します。以下のコマンドで今回使用するソースコードをダウンロードします。
git clone https://github.com/richardghirst/PiBits.git
ダウンロードが完了したら同フォルダ内にcdコマンドで移動し、READMEファイルにある記載の通り、必要なパッケージのインストールとソースファイルのちょっとした編集およびコンパイルなどを行います。
まずPiBitsフォルダの中に複数のフォルダがあるかと思いますが、MPU6050-Pi-Demoフォルダ以外は今回は使用しないので削除して構いません。MPU6050-Pi-Demoフォルダ内に移動し、READEMEファイルを確認してください。
まずlibgtkmm-3.0-devのインストールを行います。
sudo apt-get update sudo apt-get install libgtkmm-3.0-dev
次に、すでにRaspberry Pi（以降ラズパイ）のGPIOピンにMPU6050を接続した状態（前回記事参照）で、以下のコマンドでデバイスファイルを確認します。
ls -l /Dev | grep i2c
/Dev/i2c0が無く/Dev/i2c1がある場合は、I2Cdev.cppファイルをテキストエディタで開き、/Dev/i2c0の箇所を/Dev/i2c1に全て書き換えてください。（３箇所あるはずです。）
以上が完了したら、makeします。
make
するとdemo_raw、demo_dmp、demo_3dの三つの実行ファイルが新たに生成されるはずです。demo_rawはセンサのRAWデータを、demo_dmpはDMPで処理したデータをそれぞれテキスト形式で標準出力するデモです。demo_3dは簡単な３Dモデルをリアルタイムで表示するデモです。その様子をYouTubeにアップしたのが以下です。DMP6050を傾けるとそれに対応して３Dモデルの姿勢も変わります。
では、今回もTeapotを動かしてみたいと思います。以下のPythonコードをMPU6050-Pi-Demoフォルダ内に保存し、同フォルダ内で実行します。
#!/usr/bin/python
from subprocess import call, Popen, PIPE
import pygame
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from pygame.locals import *
import sys
SCREEN_SIZE = (800, 600)
SCALAR = .5
SCALAR2 = 0.2
def resize(width, height):
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45.0, float(width) / height, 0.001, 10.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
gluLookAt(0.0, 1.0, -5.0,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0)
def init():
glEnable(GL_DEPTH_TEST)
glClearColor(0.0, 0.0, 0.0, 0.0)
glShadeModel(GL_SMOOTH)
glEnable(GL_BLEND)
glEnable(GL_POLYGON_SMOOTH)
glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)
glEnable(GL_COLOR_MATERIAL)
glEnable(GL_LIGHTING)
glEnable(GL_LIGHT0)
glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));
def run():
pygame.init()
screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)
resize(*SCREEN_SIZE)
init()
clock = pygame.time.Clock()
angle = 0
glutInit(sys.argv)
cmd0 = 'cd /home/pi/imu/PiBits/MPU6050-Pi-Demo'
call(cmd0, shell=True)
cmd1 = './demo_dmp'
p = Popen(cmd1, shell=True, stdout = PIPE, stderr = PIPE)
while True:
then = pygame.time.get_ticks()
for event in pygame.event.get():
if event.type == QUIT:
return
if event.type == KEYUP and event.key == K_ESCAPE:
return
lineCount = 0
for line in iter(p.stdout.readline,''):
lineCount += 1
if lineCount <= 15:
continue
else:
sys.stdout.write(line)
array = line.split()
x_angle = array[7]
y_angle = array[8]
z_angle = array[6]
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glColor((1.,1.,1.))
glLineWidth(1)
glPushMatrix()
glRotate(-float(x_angle), 1, 0, 0)
glRotate(float(y_angle), 0, 0, 1)
glRotate(-float(z_angle), 0, 1, 0)
glutWireTeapot(1)
#glutSolidTeapot(1)
glPopMatrix()
pygame.display.flip()
if __name__ == "__main__":
run()
するとTeapotがpygameのウィンドウ内に表示されます。MPU6050を動かすと４秒くらい遅れてTeapotも動くはずです。４秒くらい遅れるのはなんとかしたかったのですが、demo_dmpの標準出力をsubprocess経由で受け取って処理する過程でタイムラグがどうしても生じてしまいます。（改善策があれば教えていただきたく。）この動画もアップしましたのでご確認ください。
以上二回にわたりラズパイでDMP6050を動かしてみました。
今日のところはこの辺で。☕️