IMU(9DOF Razor)のシリアル出力を読み込む(その2)
一般にRS232Cシリアル通信は遅いので、マルチスレッド化してメインスレッドの処理の邪魔をしないようにします。SerialThreadというクラスにまとめています。マルチスレッドの書き方は、
of_v0.8.0_vs_release\examples\utils\threadExampleの下のサンプルコードと、
http://www.openframeworks.cc/documentation/utils/ofThread.html
を参照してください。
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は多分いらない。