cvl-robot's diary

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

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

一般にRS232Cシリアル通信は遅いので、マルチスレッド化してメインスレッドの処理の邪魔をしないようにします。SerialThreadというクラスにまとめています。マルチスレッドの書き方は、

of_v0.8.0_vs_release\examples\utils\threadExampleの下のサンプルコードと、

http://www.openframeworks.cc/documentation/utils/ofThread.html

を参照してください。

*1

 

SerialThread.h

#pragma once

#include "ofMain.h"

 

class SerialThread : public ofThread {

public:

  SerialThread();

  ~SerialThread();

 

  void init();

  void connect(int com_id, int baud);

  void connect(std::string com_name, int baud);

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

  void 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;

 

  string buf;

  int watchDogTimer;

 

  // thread

  void threadedFunction();

  bool is_updated();

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

  int last_id;

  std::string last_com;

  int last_baud;

  bool flag_updated;

protected:

  float roll, pitch, yaw;

};

SerialThread.cpp

#include "SerialThread.h"

 

static const int dafault_baud = 57600;

static const int timeout_count = 100000;

static const int timeout_wdt = 200;

static const int wait_ms = 1000/120;

 

SerialThread::SerialThread() {

  readTime = 0;

  nTimesRead = 0;

  watchDogTimer = 0;

 

  yaw = pitch = roll = 0.0f;

  last_id = 0;

  last_com = "";

}

 

SerialThread::~SerialThread() {

  serial.close();

}

 

void SerialThread::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 SerialThread::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;

  }

  last_id = com_id;

  last_baud = baud;

}

 

void SerialThread::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;

  }

  last_com = com_name;

  last_baud = baud;

}

 

void SerialThread::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;

  }

 

  lock(); // is_need?

  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

  }

  unlock();

}

 

void SerialThread::update(float &y, float &p, float &r) {

  watchDogTimer++;

  if(serial.isInitialized() ){

    if(serial.available() ){

      readypr(y, p, r); 

      readTime = ofGetElapsedTimef();

      watchDogTimer = 0;

    }

  }

 

  if(watchDogTimer>timeout_wdt){

    init();

    if(last_com != ""){

      connect(last_com, last_baud);

    }

    else {

      connect(last_id, last_baud);

    }

    watchDogTimer = 0;

  }

}

 

void SerialThread::threadedFunction() {

  flag_updated = false;

  while(isThreadRunning() ){

    update(yaw, pitch, roll);

    flag_updated = true;

    Sleep(wait_ms);

  }

}

 

bool SerialThread::is_updated() {

  return flag_updated;

}

 

void SerialThread::getYPR(float &y, float &p, float &r) {

  lock(); // is_need?

  r = roll;

  p = pitch;

  y = yaw;

  flag_updated = false;

  unlock();

}

 testApp.h

#pragma once

 

#include "ofMain.h"

#include "SerialThread.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);

 

  ofTrueTypeFont font;

 

  SerialThread *serial;

  float roll, pitch, yaw;

};

 testApp.cpp

#include "testApp.h"

 

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

void testApp::setup(){

  ofSetVerticalSync(true);

 

  ofBackground(255);

  ofSetLogLevel(OF_LOG_VERBOSE);

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

 

  // serial

  serial = new SerialThread();

  serial->init();

  serial->connect(0,57600);

 

  // thread

  roll = pitch = yaw = 0.0f;

  serial->startThread(true, false);

}

 

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

void testApp::update(){

  // non-threaded

  // serial->update(yaw, pitch, roll);

 

  // threaded

  if(serial->is_updated() ){

    serial->getYPR(yaw, pitch, roll);

  }

}

 

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

void testApp::draw(){

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

    ofSetColor(0);

  } else {

    ofSetColor(220);

  }

  string msg;

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

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

  msg += "(at time " + ofToString(serial->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){ }

 

*1:読むだけなので、mutex.lockは多分いらない。