IMU(9DOF Razor)のシリアル出力を読み込む

全天球画像をヘッドマウントディスプレイやタブレットPCなどで任意の方向に臨場感たっぷり見たい場合、ユーザが見ている方向を知る必要があります。そのためにIMUが搭載されているデバイスもありますが、搭載されていないものも多いので、汎用に使えるデバイスを用意しておくと便利です。ヘッドホンの磁石に気を付ければ、HRTFヘッドホンにも使えます。

1.IMU - 9DOF Razor

いろいろ試してみて一番安定に使えたのがコレsparkfunの9DOF Razorです。

超薄型IMU「9DOF Razor」

出版社/メーカー: スイッチサイエンス
メディア: エレクトロニクス
この商品を含むブログを見る

http://www.switch-science.com/catalog/679/

http://www.sparkfun.com/products/10736

IMUは高いものだと100万円を超える物も多いので、価格性能比はいい方だと思います。*1数世代の更新を経て、安定に動作するデバイスに洗練されています。地磁気にアラインされた方位が取れます。

デバイスのセットアップやキャリブレーションの方法は、

https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial

を参考にしてください。

2.シリアルデータの読み込み

#YPR=0.000, 0.000, 0.000

の書式で文字列として流されてくるシリアルデータから、yaw, pitch, rollの値を読み取るプログラムを作ります。いつも通り、openFrameworksのserialExampleを改造します。

バッファに溜まっているシリアルデータを毎回すべて吐き出して、最新の１つだけを読み取ります。読み取り方は簡単にsscanfです。

とくに難しいところはありませんが、センサーの接続が切断されたことを検出するウォッチドッグタイマーを入れて、切れていたら再接続を試みるという処理が入っています。

testApp.h

#pragma once

#include "ofMain.h"

class testApp : public ofBaseApp{

public:

void setup();

void update();

void draw();

void keyPressed (int key);

void keyReleased(int key);

void mouseMoved(int x, int y );

void mouseDragged(int x, int y, int button);

void mousePressed(int x, int y, int button);

void mouseReleased(int x, int y, int button);

void windowResized(int w, int h);

void dragEvent(ofDragInfo dragInfo);

void gotMessage(ofMessage msg);
ofTrueTypeFontfont;

void serial_init();

void serial_connect(int com_id, int baud);

void serial_connect(string com_name, int baud);

void serial_readypr(float &y, float &p, float &r);

int nTimesRead;// how many times did we read?

float readTime;// when did we last read?

ofSerial serial;

std::vector <ofSerialDeviceInfo> deviceList;

float roll, pitch, yaw;

string buf;

int watchDogTimer;

};

testApp.cpp

#include "testApp.h"

static const int dafault_baud = 57600;

static const int timeout_count = 100000;

static const int timeout_wdt = 200;

//--------------------------------------------------------------

void testApp::setup(){

ofSetVerticalSync(true);

ofBackground(255);

ofSetLogLevel(OF_LOG_VERBOSE);

font.loadFont("DIN.otf", 64);

roll = pitch = yaw = 0.0f;

readTime = 0;

nTimesRead = 0;

watchDogTimer = 0;

serial_init();

serial_connect(0,dafault_baud);

}

void testApp::serial_init(){

serial.close();

serial.listDevices();

deviceList = serial.getDeviceList();

for(int i=0; i<deviceList.size(); i++){

cout << i << ": "

<< deviceList[i].getDeviceID() << ", "

<< deviceList[i].getDeviceName() << ", "

<< deviceList[i].getDevicePath() << endl;

}

}

void testApp::serial_connect(int com_id, int baud = dafault_baud){

serial.setup(com_id,baud);// 0 is to open the first device

nTimesRead = 0;

while(!serial.available() ){

nTimesRead++;

Sleep(10);

if(nTimesRead>timeout_count) return;

}

}

void testApp::serial_connect(string com_name, int baud = dafault_baud) {

serial.setup(com_name.data(),baud);

//serial.setup("COM4", baud); // windows example

//serial.setup("/dev/tty.usbserial-A4001JEC", baud); // mac osx example

//serial.setup("/dev/ttyUSB0", baud); //linux example

nTimesRead = 0;

while(!serial.available()){

nTimesRead++;

Sleep(10);

if(nTimesRead>timeout_count) return;

}

}

void testApp::serial_readypr(float &y, float &p, float &r) {

int nRead = 0; // a temp variable to keep count per read

serial.drain();

buf.clear();

// header align

nTimesRead = 0;

while( (nRead = serial.readByte()) != '#'){

nTimesRead++;

if(nTimesRead>timeout_count) return;

}

// readline

nTimesRead = 0;

while( (nRead = serial.readByte()) != '\n'){

nTimesRead++;

if(nRead > 0){

buf.push_back(nRead);

}

if( nTimesRead>timeout_count) return;

}

if( (sscanf_s(buf.data(), "YPR=%f,%f,%f", &y, &p, &r)) == 3){

#if _DEBUG

cerr << buf.data() << endl;

//cerr << y << ", " << p << ", " << r << endl;

#endif

}

}

//--------------------------------------------------------------

void testApp::update(){

watchDogTimer++;

if(serial.isInitialized()){

if(serial.available()){

serial_readypr(yaw, pitch, roll);

readTime = ofGetElapsedTimef();

watchDogTimer = 0;

}

}

if(watchDogTimer>timeout_wdt){

serial_init();

serial_connect(0, dafault_baud);

watchDogTimer = 0;

}

}

//--------------------------------------------------------------

void testApp::draw(){

if ( ( (ofGetElapsedTimef() - readTime) < 0.5f)){

ofSetColor(0);

} else {

ofSetColor(220);

}

string msg;

msg += "nTimes read " + ofToString(nTimesRead) + "\n";

msg += "read: " + ofToString(buf.data()) + "\n";

msg += "(at time " + ofToString(readTime, 3) + ")\n";

msg += "roll: " + ofToString(roll) + "\n";

msg += "pitch: " + ofToString(pitch) + "\n";

msg += "yaw: " + ofToString(yaw) + "\n";

font.drawString(msg, 50, 100);

}

//--------------------------------------------------------------

void testApp::keyPressed (int key){ }

//--------------------------------------------------------------

void testApp::keyReleased(int key){}

//--------------------------------------------------------------

void testApp::mouseMoved(int x, int y){}

//--------------------------------------------------------------

void testApp::mouseDragged(int x, int y, int button){}

//--------------------------------------------------------------

void testApp::mousePressed(int x, int y, int button){}

//--------------------------------------------------------------

void testApp::mouseReleased(int x, int y, int button){}

//--------------------------------------------------------------

void testApp::windowResized(int w, int h){}

//--------------------------------------------------------------

void testApp::gotMessage(ofMessage msg){}

//--------------------------------------------------------------

void testApp::dragEvent(ofDragInfo dragInfo){}