LoginSignup
TrendQuestionOfficial EventOfficial ColumnOpportunitiesOrganization
1
1

More than 1 year has passed since last update.

@Ninagawa_Izumi

posted at

updated at

BMX055とESP32でMadgwickフィルタを使う

ESP32,Magdwick,BMX055

SS 192.png でもBMX055はもうディスコンらしい

前回記事

の続きです。

やること

9軸センサのBMX055とESP32devkitCの組み合わせでMadgwickフィルタを使います。
そのメモです。
BMX055は絶版らしく、代わりにBNO055が店頭に出ています。
BNO055はセンサーフュージョンもついていて性能も良いようですが、まずはBMX055を動かします。

準備物・実施環境

  • ESP32devkitC
  • 秋月の9軸センサ AE-BMX055
  • Arduino IDE

参考

下記のサイトを参考にさせていただきました。

接続

AE-BMX055 ESP32devkitC
VCC 5V
GND GND
SDA SDA(Pin21)
SCL SCL(Pin22)

ESP32DevkitCのSDA,SCLのデフォルトピンが上記になります。
ピンを変更したい場合はWire.begin(,);で変更ができます。Wire.begin(18,19);で動くことが確認できました。

ジャンパピン

AE-BMX055 ジャンパ内容
JP1,JP2,JP3 アドレス設定 オープン
JP4,JP5 プルアップ ショート
JP6 電源設定 オープン
JP7 電源設定 ショート
JP8 電源設定 オープン

ショートすべき箇所をハンダブリッジで接続します。
上記の設定では、I2Cアドレスはデフォルト値、プルアップ有効、
電源設定は入力3.3V or 5V、信号レベルが3.3Vとなるようにしています。

プログラム

秋月のサンプルコードを少し改造し、割り込み処理にて
ロール軸、ピッチ軸、ヨー軸の値をシリアルに出力します。

ESP32系

#include<Wire.h>
#include<MadgwickAHRS.h>
Madgwick MadgwickFilter;
#include <Ticker.h>
Ticker magdwickticker;
#define MagdwickHz 100 //周波数。１秒間に何回データを読み込むかの値
const float magdwickinterval = 1000 / MagdwickHz;

// BMX055
#define Addr_Accl 0x19  // 加速度センサのI2Cアドレス
#define Addr_Gyro 0x69  // ジャイロセンサのI2Cアドレス
#define Addr_Mag 0x13   // 磁気センサのI2Cアドレス

// センサーの値を保存するグローバル変数
float ax = 0.00;
float ay = 0.00;
float az = 0.00;
float gx = 0.00;
float gy = 0.00;
float gz = 0.00;
int mx = 0.00;
int my = 0.00;
int mz = 0.00;
float pitch = 0.00;
float roll = 0.00;
float yaw = 0.00;

void BMX055_Init() //BMX055の初期化
{
  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 0x00 //100Hz 0x07
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-is enabled
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y is = 9
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x52);  // Select Mag register
  Wire.write(0x16);  // No. of Repetitions for Z-Axis = 15
  Wire.endTransmission();
}

void BMX055_All() //BMX055から全データとMadgwickフィルタの結果を取得する
{
  unsigned int data[8];

  //加速度データを取得する
  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
    // ax lsb, ax msb, ay lsb, ay msb, az lsb, az msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  ax = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (ax > 2047)  ax -= 4096;
  ay = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (ay > 2047)  ay -= 4096;
  az = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (az > 2047)  az -= 4096;
  ax = ax * 0.0098; // range = +/-2g
  ay = ay * 0.0098; // range = +/-2g
  az = az * 0.0098; // range = +/-2g

  //ジャイロデータを取得する
  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
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  gx = (data[1] * 256) + data[0];
  if (gx > 32767)  gx -= 65536;
  gy = (data[3] * 256) + data[2];
  if (gy > 32767)  gy -= 65536;
  gz = (data[5] * 256) + data[4];
  if (gz > 32767)  gz -= 65536;

  gx = gx * 0.0038; //  Full scale = +/- 125 degree/s
  gy = gy * 0.0038; //  Full scale = +/- 125 degree/s
  gz = gz * 0.0038; //  Full scale = +/- 125 degree/s

  //コンパスデータを取得する
  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
    // mx lsb, mx msb, my lsb, my msb, mz lsb, mz msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  mx = ((data[1] << 5) | (data[0] >> 3));
  if (mx > 4095)  mx -= 8192;
  my = ((data[3] << 5) | (data[2] >> 3));
  if (my > 4095)  my -= 8192;
  mz = ((data[5] << 7) | (data[4] >> 1));
  if (mz > 16383)  mz -= 32768;

  MadgwickFilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  float pitch = MadgwickFilter.getPitch();
  float roll  = MadgwickFilter.getRoll();
  float yaw   = MadgwickFilter.getYaw();
  Serial.print(pitch);
  Serial.print(",");
  Serial.print(roll);
  Serial.print(",");
  Serial.print(yaw);
  Serial.println("");
}

void setup()
{
  MadgwickFilter.begin(MagdwickHz);
  Wire.begin();//I2Cの初期化
  Serial.begin(2000000); //デバッグ用シリアル通信
  BMX055_Init(); //BMX055の初期化
  //#define MagdwickHz で設定した周波数にてセンサー読み取りを割り込み実行
  magdwickticker.attach_ms(magdwickinterval, BMX055_All);
  delay(300);
}

void loop()
{
}

実行

実行してArduino IDEのツールメニューより「シリアルプロッタ」を選択します。
開始から20秒ほどで値が安定します。
センサの傾きにグラフが反応します。

おわりに

秋月のサンプルに若干手を加えていますが、もっとコンパクトにする方法があるような気はしています。
また、とりあえず動作するというレベルですので、調整は必要かと思います。

Processingを使った画面描画で動作を確認したい場合には、下記の記事が参考になるかもしれません。

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
What you can do with signing up
Sign upLogin
1
1