読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

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

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

1.IMU - 9DOF Razor

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

 

超薄型IMU「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を改造します。

バッファに溜まっているシリアルデータを毎回すべて吐き出して、最新の1つだけを読み取ります。読み取り方は簡単に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){}

 

実行結果の画面はこんな感じ。

f:id:cvl-robot:20131225183401p:plain

*1:注意点は、10個に1個ぐらいどんなにキャリブしてもずれてしまうハズレがあることです。