5
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

Day 2

八戸高専 Advent Calendar 2025

@amino-3

BMX055とMagdwickフィルタでジャイロポインタ的なものを作った話

5
Posted at

読んで字の如くです。加速度センサ1つだけを使って、ジャイロで動くポインタのようなものを作ったので記録として残します。

概要

任天堂のWiiリモコンをぶんぶん振り回したときにそれと連動して、例えばWiiのホーム画面で青いポインタが動くみたいなものを想像していただけるといいかと思います。
ハードウェアの中にセンサが仕込んであって、そのセンサを動かすと画面上のポインタが連動して動くみたいな感じ。

使ったものたち

ハードウェア

・BMX055
BOSCH社の9軸(加速度3軸+ジャイロ3軸+磁気コンパス3軸)センサ
その辺に落ちていたので今回は上記の加速度センサを使いました。
秋月のサイトにピンの付け方や、Arduinoボードとの接続方法が記載されているのでリンク貼っときます。
BMX055センサ

数値が取れればそれでいいので9軸センサなら何でもいいし、なんなら磁気を用いない6軸のものでも機能すると思われます。

・Arduino Uno
今回のハードウェアの頭脳です。後述のスクラッチを実装して、センサを接続して使います。

フィルタ

・Magdwickフィルタ
自分も理解が曖昧なのですごくざっくりとしか書けないのですが、取得した加速度や角速度などの値を用いて、そのセンサの姿勢を推定するためのフィルタのようです。
ロール、ピッチ、ヨーの三方向に対する回転角が出力として得られます。

フィルタ内部でどのような計算をしているかなどの詳しい解説をされている記事があったのでリンクを載せておきます。動作原理が知りたい方は読んでみるといいかもしれません。
Madgwick Filterを読んでみた | @fumiya_sato

スクラッチ

作ったものを張ります。

xy_axis.ino
//madgwickフィルタのロールピッチ使って二次元の座標を求める
//画面の中心の座標は(400,300)
#include <Arduino.h>
#include <Wire.h>
#include<MadgwickAHRS.h>
#include <math.h>

// BMX055　加速度センサのI2Cアドレス
#define Addr_Accl 0x19  // (JP1,JP2,JP3 = Openの時)
// BMX055　ジャイロセンサのI2Cアドレス
#define Addr_Gyro 0x69  // (JP1,JP2,JP3 = Openの時)
// BMX055　磁気センサのI2Cアドレス
#define Addr_Mag 0x13   // (JP1,JP2,JP3 = Openの時)

#define hori 500//横軸の0からの幅（実際はこれの二倍）
#define vart 450//縦軸の幅

// センサーの値を保存するグローバル関数
float xAccl = 0.00, yAccl = 0.00, zAccl = 0.00;
float xGyro = 0.00, yGyro = 0.00, zGyro = 0.00;
float xMag  = 0.00, yMag  = 0.00, zMag  = 0.00;

float axrc = 0.00, ayrc = 0.00, azrc = 0.00;
float gxrc = 0.00, gyrc = 0.00, gzrc = 0.00;
int mxrc = 0, myrc = 0, mzrc = 0;

float ROLL, PITCH, YAW;

float a[3] = {0.0, 1.0, 0.0}; // 回転前のベクトル
float b[3] ;                    // 回転後のベクトル
float cur[3];//角度などが正しかった時のベクトルを保存

int x, y;

Madgwick MadgwickFilter;

bool sendflag;
char buf[16];

void setup() {
  
  Serial.begin(9600);

  Wire.begin();
  BMX055_Init();

  delay(100);
  MadgwickFilter.begin(10);
  
  sendflag = true;
 
}
void loop() {

  for(size_t i=0;i<min(Serial.available(),sizeof(buf));i++){
    char c =  Serial.read();
    buf[i]=c;
    constexpr uint8_t begin = 0x63;
    if(c==begin){
      sendflag=true;
    }
    constexpr char end = 0x64;
    if(c==end){
      sendflag=false;
    }
  }

  // bにaの値をコピーして、計算に使用
  float temp[3];

  BMX055_Gyro();
  BMX055_Accl();
  BMX055_Mag();

  MadgwickFilter.updateIMU(xGyro,yGyro,zGyro,xAccl,yAccl,zAccl);

  ROLL = MadgwickFilter.getRollRadians();
  PITCH = MadgwickFilter.getPitchRadians();
  YAW = MadgwickFilter.getYawRadians();//フィルタのずれを補正（若干ドリフトはする)

  float cr = cos(ROLL);
  float sr = sin(ROLL);
  float cp = cos(PITCH);
  float sp = sin(PITCH);
  float cy = cos(YAW);
  float sy = sin(YAW);

  // 回転行列
  float rx[3][3] = {{1.0f, 0.0f, 0.0f}, {0.0f, cr, -sr}, {0.0f, sr, cr}};
  float ry[3][3] = {{cp, 0.0f, sp}, {0.0f, 1.0f, 0.0f}, {-sp, 0.0f, cp}};
  float rz[3][3] = {{cy, -sy, 0.0f}, {sy, cy, 0.0f}, {0.0f, 0.0f, 1.0f}};

  // Yaw回転
  multiplyMatrixVector(temp, rz, a);
  for (int i = 0; i < 3; i++) b[i] = temp[i];

  // Pitch回転
  multiplyMatrixVector(temp, ry, b);
  for (int i = 0; i < 3; i++) b[i] = temp[i];

  // Roll回転
  multiplyMatrixVector(temp, rx, b);
  for (int i = 0; i < 3; i++) b[i] = temp[i];

  x = int((b[0]) *hori)+400;//ベクトルのx成分を画面のx座標
  y = int((-b[2]) *vart)+300;//z成分をy座標

  if (x < 0 ) x = 0;
  else if (x > 800) x = 800; 
  if (y < 0) y = 0;
  else if (y > 600) y = 600;

  if(sendflag){
    Serial.print("(");
    Serial.print(x);
    Serial.print(",");
    Serial.print(y);
    Serial.println(")");
  }

  delay(20);


}

// 行列とベクトルの掛け算を行う関数
void multiplyMatrixVector(float result[3], float matrix[3][3], float vector[3]) {
  for (int i = 0; i < 3; i++) {
    result[i] = matrix[i][0] * vector[0] + matrix[i][1] * vector[1] + matrix[i][2] * vector[2];
  }
}

//=====================================================================================//
void BMX055_Init()
{
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x0F); // Select PMU_Range register
  Wire.write(0x03);   // Range = +/- 2g
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x10);  // Select PMU_BW register
  Wire.write(0x08);  // Bandwidth = 7.81 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x11);  // Select PMU_LPW register
  Wire.write(0x00);  // Normal mode, Sleep duration = 0.5ms
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x0F);  // Select Range register
  Wire.write(0x04);  // Full scale = +/- 125 degree/s
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x10);  // Select Bandwidth register
  Wire.write(0x07);  // ODR = 100 Hz
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x11);  // Select LPM1 register
  Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x83);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x01);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4C);  // Select Mag register
  Wire.write(0x00);  // Normal Mode, ODR = 10 Hz
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-Axis enabled
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y Axis = 9
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x52);  // Select Mag register
  Wire.write(16);  // No. of Repetitions for Z-Axis = 15
  Wire.endTransmission();
}
//=====================================================================================//
void BMX055_Accl()
{
  unsigned int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Accl);
    Wire.write((2 + i));// Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
    // Read 6 bytes of data
    // xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (xAccl > 2047)  xAccl -= 4096;
  yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (yAccl > 2047)  yAccl -= 4096;
  zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (zAccl > 2047)  zAccl -= 4096;
  xAccl = xAccl * 0.00098 ; // range = +/-2g更に単位を[m/s^2]へ
  yAccl = yAccl * 0.00098 ; // range = +/-2g
  zAccl = zAccl * 0.00098  ; // range = +/-2g
  

}
//=====================================================================================//
void BMX055_Gyro()
{
  unsigned int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Gyro);
    Wire.write((2 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Gyro, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xGyro = (data[1] * 256) + data[0];
  if (xGyro > 32767)  xGyro -= 65536;
  yGyro = (data[3] * 256) + data[2];
  if (yGyro > 32767)  yGyro -= 65536;
  zGyro = (data[5] * 256) + data[4];
  if (zGyro > 32767)  zGyro -= 65536;

  xGyro = xGyro * 0.0038; //  Full scale = +/- 125 degree/s
  yGyro = yGyro * 0.0038; //  Full scale = +/- 125 degree/s
  zGyro = zGyro * 0.0038; //  Full scale = +/- 125 degree/s

  /*if(num == 1){
    gxrc = gxrc - INITIAL_GYRO_X;
    gyrc = gyrc - INITIAL_GYRO_Y;
    gzrc = gzrc - INITIAL_GYRO_Z;
  }*/
}
//=====================================================================================//
void BMX055_Mag()
{
  unsigned int data[8];
  for (int i = 0; i < 8; i++)
  {
    Wire.beginTransmission(Addr_Mag);
    Wire.write((0x42 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Mag, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
// Convert the data
  xMag = ((data[1] <<5) | (data[0]>>3));
  if (xMag > 4095)  xMag -= 8192;
  yMag = ((data[3] <<5) | (data[2]>>3));
  if (yMag > 4095)  yMag -= 8192;
  zMag = ((data[5] <<7) | (data[4]>>1));
  if (zMag > 16383)  zMag -= 32768;

}

何をしてるのか

  1. まず画面に対して垂直なベクトルを一本用意する
  2. 次にセンサで加速度などの情報を取得し、Madgwickフィルタでロールピッチヨーに対する回転角を求める
  3. 3軸に対する回転角から各軸に対する回転行列を定義、最初に用意しておいたベクトルに対して、ヨー → ピッチ → ロールの順番に乗算
  4. 得られた結果をポインタとして召喚したい画面に対応する形へと変換

みたいなことをやっています。
画面の中心にベクトルの原点を設定して、回転させたベクトルの先端がポインタとして反映されるようにやってる感じです。
座標系についてですが、おそらく左手座標系で開発を進めていたと思います。
ただ、これを書いたのがだいぶ前なので若干あやふやな部分があるかもしれません。もしこの記事を読んで、「これ使えそうやんけ！」と思われた方がいらっしゃいましたら、一旦私の言っていることが正しいか確認してから使うことを強く推奨します。

課題

演算に回転行列を使っているのでかなり動作に癖があります。慣れないと画面の四隅にポインタを持っていくことは難しいかもしれません。
また、同じく操作の癖として、ジンバルロックにも注意する必要があります。すごーく雑に説明すると、ある座標軸が、別の座標軸の初期位置に重なってしまい、回転角を正常に求められなくなる現象のことです。

この辺に気をつけて操作する必要があります。操作中に上記のようなトラブルに遭遇して正常な動作ができなくなった…といった状況を避けるため、マイコンボードにリセット用のスイッチを接続して使うなどの物理的な対策をしたほうがいいと思います。

5
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?