Contents
1.目的
preMSMに搭載されている9軸モーションセンサBMX160の出力を使って、センサの姿勢計算（オイラー角の計算）をリアルタイムに行い、グラフで表示させる。
2. 取り組み内容
2-1. オイラー角のリアルタイム計算
6軸センサの出力MPU-6050用のオイラー角出力用Arduinoのライブラリが、GitHubにあり、それが地磁気にも対応している*1。これを用いて、preMSMのBMX160の出力からオイラー角をリアルタイムに計算させるC++のプログラムを作成した*2。
誤差抑制のフィルタは、一般的に使われているカルマンフィルタではなく、Madgwickフィルタを使用した。
*1 arduino-libraries / MadgwickAHRS
*2 【Arduino 】MPU-6050を使って姿勢角を算出 （MadgwickFilterを使用）
2-2. オイラー角から直方体のリアルタイム姿勢表示
BMX055の9軸センサのオイラー角から、Processingを使って、リアルタイムに直方体の姿勢表示をさせるプログラムが公開されていた*3。
上記のC++プログラムのインターフェースをProcessingに合わせ、多少表示を変更することで、preMSMを振るとそれに追随して画面に表示させた。
*3 BMX055の9軸データをProcessingで可視化させる
3. 構成
3-1. ハード構成
ハードの構成を図１に示す。ハードとしては、9軸センサBMX160を搭載したpreMSM、センサ出力を受信しオイラー角の計算を行うGatewayとオイラー角を元に姿勢表示を行うPCの3つで構成される。
図1. ハード構成図
3-2. ソフト構成
ソフト構成とデータの流れをまとめた図を、図2に示す。
- preMSM上のArduinoの環境で動作するBMX160のライブラリから出力されたデータは、BluetoothでGatewayに送られる。
- GatewayではNode-REDで、rfcomm0からのシリアル信号として受ける。その後、UDPでオイラー角計算プログラムに送られる。
- C++で書かれたオイラー角計算プログラムは、Gateway上でコンパイルされ実行される。UDPで逐次送られてくるデータをもとにオイラー角を計算し、結果をUDPでNode-REDに返す。
- オイラー角計算プログラムから返されたオイラー角をGatewayは、MQTTを使ってPublishする。MQTTのブローカーは、GatewayのNode-RED上に設置した。
- PCは、GatewayのMQTTからSubscribeしたオイラー角を、processingで受け図形表示する。
Gateway上で動作するNode-REDのフローとC++のプログラム、ならびにPC上で動作するProcessingのプログラムは、Appendixを参照のこと。
図2.ソフト構成とデータの流れ
4. 結果
リアルタイムにpreMSMの振れに反応して、図形が動く表示ができた。図3に動画画面を示す。
5. Appendix
5.1. Gateway上のNode-REDフロー
図A-1にNode-REDのフローと再利用時に変更が必要な個所を示す。
5.2. オイラー角計算プログラム(C++)
オイラー角計算プリグラムは、以下の【準備項目】で入手したライブラリと【ソースコード】を同じフォルダに置いて、コンパイルすることで実行ファイルが出来上がる。
【準備項目】
- Madgwickフィルタのライブラリ(MadgwickAHRS.hとMadgwickAHRS.cpp)を下記HPより入手
arduino-libraries / MadgwickAHRS - UDPのライブラリ(simple_UDP.h)を下記HPより入手
メカトロ講座03 c++でudp通信をする方法
【ソースコード】
//
// 姿勢算出プログラム
// Ver 0.01: 2021.11.27 first version
// Ver 0.02: 2021.11.28 corporation with Processing
// Ver 0.03: 2021.11.28 corporation with BMX160 on preMSM
// Ver UDP : 2021.12.12 enable to send data via UDP
// Ver 2UDP: 2021.12.12 enable to send and receive data via UDP
//
// $ g++ -o acs_2udp -Wall acs_2UDP.cpp MadgwickAHRS.cpp -lm
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h> // for usleep()
#include <time.h>
#include <sys/time.h>
#include "MadgwickAHRS.h"
#include "simple_udp.h"
#define MAXNUM 256
#define GYRO 1
#define ACCEL 2
#define MAG 3
#define XYZ 4
simple_udp udp0("127.0.0.1",4001); // send data
simple_udp udp1("0.0.0.0" ,4002); // receive data
Madgwick MadgwickFilter;
void getData(char*, float*, int);
void strSrch(char*, char*);
int main(int argc, char**argv){
float axRaw, ayRaw, azRaw; // 加速度
float gxRaw, gyRaw, gzRaw; // ジャイロ
float mxRaw, myRaw, mzRaw; // 地磁気
float ax, ay, az; // 加速度
float gx, gy, gz; // ジャイロ
float mx, my, mz; // 地磁気
float pitch, roll, yaw; // 計算結果
float data[3];
int i;
char buf[MAXNUM];
// 初期化処理
/* Madwick Filter setting */
MadgwickFilter.begin(100); // 100Hz
/* UDP ポート オープン */
udp1.udp_bind();
while (1){
/* 受信処理 */
std::string rdata=udp1.udp_recv();
strcpy(buf, rdata.c_str());
i = strlen(buf);
if (i<=3){
continue;
}
// Accel data
getData(buf, data, ACCEL);
axRaw = data[0]; ayRaw=data[1]; azRaw=data[2];
// Gyro data
getData(buf, data, GYRO);
gxRaw = data[0]; gyRaw=data[1]; gzRaw=data[2];
// Mag data
getData(buf, data, MAG);
mxRaw = data[0]; myRaw=data[1]; mzRaw=data[2];
/* データの変換 */
// 加速度値を分解能で割って加速度(G)に変換する
// Full Scall 16,384 LSB /g
ax = axRaw; // ax = axRaw / 16384.0;
ay = ayRaw; // ay = ayRaw / 16384.0;
az = azRaw; // az = azRaw / 16384.0;
// 角加速度値を分解能で割って角速度(degrees per sec)に変換する
gx = gxRaw; // gx = gxRaw / 131;
gy = gyRaw; // gy = gyRaw / 131;
gz = gzRaw; // gz = gzRaw / 131;
mx = mxRaw;
my = myRaw;
mz = mzRaw;
/* Madwick Filter処理 */
MadgwickFilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
pitch = MadgwickFilter.getPitch();
roll = MadgwickFilter.getRoll();
yaw = MadgwickFilter.getYaw();
// Serial Transfer
for (i=1;i<5;i++){
if (i == GYRO){
sprintf(buf,"%d,%f,%f,%f",GYRO,gx,gy,gz);
} else if (i == ACCEL){
sprintf(buf,"%d,%f,%f,%f",ACCEL,ax,ay,az);
} else if (i == MAG){
sprintf(buf,"%d,%f,%f,%f",MAG,mx,my,mz);
} else if (i == XYZ){
sprintf(buf,"%d,%f,%f,%f",XYZ,roll,pitch,yaw);
}
udp0.udp_send(buf);
}
}
// 後処理
//serialClose(fd);
return 0;
}
void getData(char buf[], float data[], int name){
char key1[]="gyro";
char key2[]="acc";
char key3[]="mag";
char key4[]="val";
char key[MAXNUM];
char tmp[MAXNUM];
char delim[] = "[],";
char *token;
int i = 0;
// 検索文字の設定
if (name == GYRO){strcpy(key, key1);}
else if (name == ACCEL){strcpy(key, key2);}
else if (name == MAG){strcpy(key, key3);}
// 検索文字の探索
strcpy(tmp, buf);
strSrch(tmp, key);
strSrch(tmp, key4); //val
strcpy(buf, tmp);
// データの取得
token = strtok(tmp, delim);
while(token != NULL){
if (i == 1){
data[0] = atof(token);
} else if (i == 2){
data[1] = atof(token);
} else if (i == 3){
data[2] = atof(token);
break;
}
token = strtok(NULL, delim);
i++;
}
}
void strSrch(char buf[], char key[]){
int i;
int j = 0;
int n = strlen(buf);
int m = strlen(key);
char tmp[MAXNUM];
strcpy(tmp, buf);
for (i=0;i<n;i++){
if (tmp[i] != key[j]){
j=0;
continue;
} else {
j++;
if (j>=m){
i++;
strcpy(buf,&tmp[i]);
break;
}
}
}
}
5.3. 姿勢表示プログラム(processing)
下記のソースコードをprocessingで表示することで、姿勢表示ができた。
表示のプログラムは、 BMX055の9軸データをProcessingで可視化させるをベースにさせていただいた。
変更箇所が分かるようにコメントアウトはそのまま残した。また、MQTTに対応するために、MQTT関係のコードを追加した(added for MQTTのコメント有)。
import processing.serial.*;
import processing.opengl.*;
import mqtt.*; // added for MQTT
static final char GYRO=1;
static final char ACCEL=2;
static final char MAG=3;
static final char XYZ_rotations=4;
static final int graph_width=300, graph_depth=150;
static final int axis_color[][]={{200,50,50},{60,120,200},{200,200,80}};//x_axis(1)is RED,y_axis(2)is BLUE,z_axis(3)is YELLOW
static final int x_color=0,y_color=1,z_color=2; //1is red 2is blue 3is yellow
static final int text_size=30;
static final int box_width=100,box_height=50,box_depth=160;
static final int box_Xpotision=1100,box_Ypotision=850;
//static final int box_Xpotision=1600,box_Ypotision=850;
static final int box_textpotision=-180;
static final int circle_R=270;
static final int Diamond=100;
static final int Rectangle_width=160,Rectangle_height=35;
char data;//data
Serial myPort;
float gx, gy, gz;
float ax, ay, az;
float mx, my, mz;
float roll=0,pitch=0,yaw=0;
int a=1;//回転方向
graphMonitor x_GyroGraph;
graphMonitor y_GyroGraph;
graphMonitor z_GyroGraph;
graphMonitor x_AccelGraph;
graphMonitor y_AccelGraph;
graphMonitor z_AccelGraph;
graphMonitor x_MagGraph;
graphMonitor y_MagGraph;
graphMonitor z_MagGraph;
graphMonitor x_rotations;
graphMonitor y_rotations;
graphMonitor z_rotations;
circleMonitor roll_circle;
circleMonitor pitch_circle;
circleMonitor yaw_circle;//未実装
MQTTClient client; // added for MQTT
void setup(){
size(1800,1000,P3D);//3D space specific
// myPort = new Serial(this,"COM17",1000000);//COM4 serial 115200
client = new MQTTClient(this); // added for MQTT
client.connect("mqtt://192.168.11.43:1883"); // added for MQTT
frameRate(50);
smooth();
//myPort.bufferUntil('\n');//\nでデータ分割
x_GyroGraph = new graphMonitor("x_gyro", 100, 40, graph_width, graph_depth,x_color);//String _TITLE,_X_POSITION,_Y_POSITION,_,_Y_LENGTH
y_GyroGraph = new graphMonitor("y_gyro", 100, 240, graph_width, graph_depth,y_color);
z_GyroGraph = new graphMonitor("z_gyro", 100, 440, graph_width, graph_depth,z_color);
x_AccelGraph= new graphMonitor("x_accel",520, 40, graph_width, graph_depth,x_color);
y_AccelGraph= new graphMonitor("y_accel",520, 240, graph_width, graph_depth,y_color);
z_AccelGraph= new graphMonitor("z_accel",520, 440, graph_width, graph_depth,z_color);
x_MagGraph = new graphMonitor("x_mag", 940, 40, graph_width, graph_depth,x_color);
y_MagGraph = new graphMonitor("y_mag", 940, 240, graph_width, graph_depth,y_color);
z_MagGraph = new graphMonitor("z_mag", 940, 440, graph_width, graph_depth,z_color);
x_rotations = new graphMonitor("roll", 1360, 40, graph_width, graph_depth,x_color);
y_rotations = new graphMonitor("pitch", 1360, 240, graph_width, graph_depth,y_color);
z_rotations = new graphMonitor("yaw", 1360, 440, graph_width, graph_depth,z_color);
roll_circle = new circleMonitor("roll",150,800,x_color);
pitch_circle= new circleMonitor("pitch",450,800,y_color);
}
void draw(){
background(25);
x_GyroGraph.graphDraw(gx);
y_GyroGraph.graphDraw(gy);
z_GyroGraph.graphDraw(gz);
x_AccelGraph.graphDraw(ax);
y_AccelGraph.graphDraw(ay);
z_AccelGraph.graphDraw(az);
x_MagGraph.graphDraw(mx);
y_MagGraph.graphDraw(my);
z_MagGraph.graphDraw(mz);
x_rotations.graphDraw(roll);
y_rotations.graphDraw(pitch);
z_rotations.graphDraw(yaw);
roll_circle.graphDraw(roll);
pitch_circle.graphDraw(pitch);
yaw_circle(yaw);
ypr_box(roll,pitch,yaw);
ryp_box(roll,pitch,yaw);
}
// added for MQTT
void clientConnected() {
println("client connected");
client.subscribe("/position");
}
// added for MQTT
void messageReceived(String topic, byte[] payload) {
String myString = new String( payload );
// println("new message: " + topic + " - " + myString);
// println(myString);
// println("---------");
if (myString != null) {
myString = trim(myString);//trimで空白消去
float sensors[] = float(split(myString, ','));// ,cut output
if (sensors.length > 2) {
switch(int(sensors[0])){
case GYRO:
gx = sensors[1];
gy = sensors[2];
gz = sensors[3];
break;
case ACCEL:
ax = sensors[1];
ay = sensors[2];
az = sensors[3];
break;
case MAG:
mx = sensors[1];
my = sensors[2];
mz = sensors[3];
break;
case XYZ_rotations:
roll =sensors[1];
pitch=sensors[2];
yaw =sensors[3];
}
}
}
}
// added for MQTT
void connectionLost() {
println("connection lost");
}
/* comment out for MQTT instead of UDP
void receive(byte[] data, String ip, int port){
String myString = new String( data );
println(myString);
// println("---------");
if (myString != null) {
myString = trim(myString);//trimで空白消去
float sensors[] = float(split(myString, ','));// ,cut output
if (sensors.length > 2) {
switch(int(sensors[0])){
case GYRO:
gx = sensors[1];
gy = sensors[2];
gz = sensors[3];
break;
case ACCEL:
ax = sensors[1];
ay = sensors[2];
az = sensors[3];
break;
case MAG:
mx = sensors[1];
my = sensors[2];
mz = sensors[3];
break;
case XYZ_rotations:
roll =sensors[1];
pitch=sensors[2];
yaw =sensors[3];
}
}
}
}
*/
/* commented out for Web Socket
void serialEvent(Serial myPort){
String myString = myPort.readStringUntil('\n');
if (myString != null) {
myString = trim(myString);//trimで空白消去
float sensors[] = float(split(myString, ','));// ,cut output
if (sensors.length > 2) {
switch(int(sensors[0])){
case GYRO:
gx = sensors[1];
gy = sensors[2];
gz = sensors[3];
break;
case ACCEL:
ax = sensors[1];
ay = sensors[2];
az = sensors[3];
break;
case MAG:
mx = sensors[1];
my = sensors[2];
mz = sensors[3];
break;
case XYZ_rotations:
roll =sensors[1];
pitch=sensors[2];
yaw =sensors[3];
}
}
}
}
*/
void ypr_box(float r,float p,float y ){//YPR
ambientLight(200,200,200);
pointLight(200,200,200,box_Xpotision,box_Ypotision,70);//light potison
lightFalloff(1,0,0);
lightSpecular(0,0,0);
pushMatrix();
translate(box_Xpotision,box_Ypotision);
textSize(text_size);// title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text("YPR", box_textpotision, box_textpotision);
rotateY(radians(y));//Y_rotations is changed raw(Z_rotation)
rotateZ(radians(a*r));
rotateX(radians(-a*-p));
fill(70, 180, 230);
stroke(200,255,230);
box(box_width,box_height,box_depth);
stroke(60,120,200);
line( 0, 0, 0, 200, 0, 0);//X rotation(pitch) blue
stroke(200,200,80);
line(0, 0, 0, 0, -200, 0);//Y rotation(yaw)yellow
stroke(200,50,50);
line(0, 0, 0, 0, 0, 200);//Z rotation(roll) red
popMatrix();
}
void ryp_box(float r ,float p ,float y){//RYP
ambientLight(100,100,100);
pointLight(100,100,100,box_Xpotision,box_Ypotision,70);//light potison
lightFalloff(1,0,0);
lightSpecular(0,0,0);
pushMatrix();
translate(box_Xpotision+350,box_Ypotision);
// translate(box_Xpotision+400,box_Ypotision);
textSize(text_size);// title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text("RYP", box_textpotision, box_textpotision);
rotateX(radians(a*p));
rotateY(radians(y));//Y_rotations is changed raw(Z_rotation)
rotateZ(radians(-a*-r));
fill(200, 60, 50);
stroke(200,130,80);
box(box_width,box_height,box_depth);
stroke(60,120,200);
line( 0, 0, 0, 200, 0, 0);//X rotation(pitch) blue
stroke(200,200,80);
line(0, 0, 0, 0, -200, 0);//Y rotation(yaw)yellow
stroke(200,50,50);
line(0, 0, 0, 0, 0, 200);//Z rotation(roll) red
popMatrix();
}
class circleMonitor{
String TITLE;
int X_POSITION,Y_POSITION;
int X_LENGTH,Y_LENGTH;
int c;
circleMonitor(String _TITLE, int _X_POSITION, int _Y_POSITION,int _COLOR){
TITLE=_TITLE;
X_POSITION=_X_POSITION;
Y_POSITION=_Y_POSITION;
c=_COLOR;
}
void graphDraw(float ang){
pushMatrix();
translate(X_POSITION,Y_POSITION);
textSize(text_size);// title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text(TITLE, -circle_R/2, -circle_R/2);
strokeWeight(1);
stroke(255,255,255);
fill(25);
ellipse(0,0,circle_R,circle_R);
fill(axis_color[c][0],axis_color[c][1],axis_color[c][2]);
textSize(18);
// text(ang,-circle_R/2-60,0);
text(ang,-40,circle_R/2+30);
line(-circle_R/2,0,circle_R/2,0);
line(0,-circle_R/2,0,circle_R/2);
strokeWeight(1);
stroke(220);
fill(axis_color[c][0],axis_color[c][1],axis_color[c][2]);
translate(0,0);
rotate(radians(ang));
rect(-Rectangle_width/2,-Rectangle_height/2,Rectangle_width,Rectangle_height);
popMatrix();
}
}/*
void roll_circle(float r){//roll circle
pushMatrix();
translate(circle_Xpotision,circle_Ypotision);
textSize(text_size);// title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text("roll", -circle_R/2, -circle_R/2);
strokeWeight(1);
stroke(255,255,255);
fill(25);
ellipse(0,0,circle_R,circle_R);
line(-circle_R/2,0,circle_R/2,0);
line(0,-circle_R/2,0,circle_R/2);
fill(200,50,50);
textSize(18);
text(r,-circle_R/2-60,0);
strokeWeight(2);
stroke(20,50,50);
fill(200,50,50);
translate(0,0);
rotate(radians(r));
rect(0,0,Rectangle_width,Rectangle_height);
popMatrix();
}
void pitch_circle(float p){// pitch circle
pushMatrix();
translate(circle_Xpotision+350,circle_Ypotision);
textSize(text_size);// title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text("pitch", -circle_R/2, -circle_R/2);
strokeWeight(1);
stroke(255,255,255);
fill(25);
ellipse(0,0,circle_R,circle_R);
fill(60,120,200);
textSize(18);
text(p,-circle_R/2-60,0);
line(-circle_R/2,0,circle_R/2,0);
line(0,-circle_R/2,0,circle_R/2);
strokeWeight(1);
stroke(220);
fill(60,120,200);
stroke(60,120,200);
translate(0,0);
rotate(radians(-p));
rect(0,0,Rectangle_width,Rectangle_height);
popMatrix();
}*/
void yaw_circle(float ang){// yaw circle
pushMatrix();
translate(750,800);//picthの円の描写位置から(+700,+0)
// translate(700+350,800);//picthの円の描写位置から(+700,+0)
textSize(text_size);// title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text("yaw", -circle_R/2, -circle_R/2);
strokeWeight(1);
stroke(230,230,230);
fill(25);
ellipse(0,0,circle_R,circle_R);
fill(200,200,80);
textSize(18);
// text(ang,-circle_R/2-60,0);
text(ang,-40,circle_R/2+30);
strokeWeight(1);
stroke(220);
line(-circle_R/2,0,circle_R/2,0);
line(0,-circle_R/2,0,circle_R/2);
fill(200,200,80);
stroke(200,200,90);
translate(0,0);
rotate(radians(-ang));
beginShape();
for (int i = 0; i < 4; i++) {
int R;
if (i % 2 == 0) {
R =Diamond/3 ;
} else {
R =Diamond;
}
vertex(R*cos(radians(90*i)), R*sin(radians(90*i)));
} endShape(CLOSE);
popMatrix();
}
class graphMonitor {
String TITLE;
int X_POSITION, Y_POSITION;
int X_LENGTH, Y_LENGTH;
float [] y1;
float maxRange;
int c;
graphMonitor(String _TITLE, int _X_POSITION, int _Y_POSITION, int _X_LENGTH, int _Y_LENGTH ,int _COLOR) {
TITLE = _TITLE;
X_POSITION = _X_POSITION;
Y_POSITION = _Y_POSITION;
X_LENGTH = _X_LENGTH;
Y_LENGTH = _Y_LENGTH;
c=_COLOR;
y1 = new float[X_LENGTH];
for (int i = 0; i < X_LENGTH; i++) {
y1[i] = 0;//rotations
}
}
void graphDraw(float _y1) {
y1[X_LENGTH - 1] = _y1;
for (int i = 0; i < X_LENGTH - 1; i++) {
y1[i] = y1[i + 1];
}
maxRange = 1.0;
for (int i = 0; i < X_LENGTH - 1; i++) {// maxRange change
maxRange = (abs(y1[i]) > maxRange ? abs(y1[i]) : maxRange);//range change
}
pushMatrix();
translate(X_POSITION, Y_POSITION);//graph position
fill(25);//in fill
stroke(160);//out fill
strokeWeight(1);//line weight
rect(0,0, X_LENGTH, Y_LENGTH);//rect potison
line(0, Y_LENGTH / 2, X_LENGTH, Y_LENGTH / 2);//center line
textSize(25);//graph title
fill(80,200,200);
textAlign(LEFT, BOTTOM);
text(TITLE, 20, -5);
textSize(22);//origin
fill(220);//white
textAlign(RIGHT);
text(0, -5, Y_LENGTH / 2 + 7);
textSize(20);
text(nf(maxRange, 0, 1), -5, 18);//range
text(nf(-1 * maxRange, 0, 1), -5, Y_LENGTH);
translate(0, Y_LENGTH / 2);
text(nf(y1[X_LENGTH - 1],0,2), X_LENGTH+80,-3);//data minute
scale(1, -1);
strokeWeight(1);
for (int i = 0; i < X_LENGTH - 1; i++) {
stroke(axis_color[c][0],axis_color[c][1],axis_color[c][2]);//color change
line(i, y1[i] * (Y_LENGTH / 2) / maxRange, i + 1, y1[i + 1] * (Y_LENGTH / 2) / maxRange);// graph data output
}
popMatrix();
}
}
