秋月のLSM9DS1でMadgwickを使う

LSM9DS1しか手に入らない時のMadgwickがわからぬ

やること

過去の２つの記事の続きです。

LSM9DS1を使用した秋月の9軸センサモジュール(9DoF IMU)の値を読みとり、Madgwickフィルタを適用してみます。

準備

・秋月の9軸センサモジュール(9DoF IMU)
・Arduino系のマイコン
・ライブラリ SparkFunLSM9DS1 の導入
　　→ライブラリはArduinoIDEのライブラリマネージャから導入できます

配線例

Arduino系	9DoF IMU
3.3v	1. +V
GND	2. GND
SDA	3. SDA
SCL	4. SCL

Arduino系用スケッチ

Arduino

# include <SparkFunLSM9DS1.h>
LSM9DS1 ahrs;

# include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

# define LSM9DS1_M 0x1C // コンパスのI2C初期アドレス
# define LSM9DS1_AG 0x6A // 加速度とジャイロのI2C初期アドレス

float ROLL, PITCH, YAW;

void setup()
{
  Wire.begin();
  Serial.begin(115200);
  ahrs.settings.accel.scale = 2; // 加速度のレンジを+/-2g
  ahrs.settings.gyro.scale = 245; // ジャイロのレンジを+/-500dps
  ahrs.settings.mag.scale = 4; // 磁力のレンジを +/-8Gs

  delay(300);
  if (ahrs.begin(LSM9DS1_AG, LSM9DS1_M) == false)
  {
    Serial.println("Failed to communicate with LSM9DS1.");
    while (1);
  }

  MadgwickFilter.begin(100);//100Hz
}

void loop()
{
  getAHRS();
  MadgwickFilter.updateIMU(ahrs.calcGyro(ahrs.gx), ahrs.calcGyro(ahrs.gy), ahrs.calcGyro(ahrs.gz), ahrs.calcAccel(ahrs.ax), ahrs.calcAccel(ahrs.ay), ahrs.calcAccel(ahrs.az));
  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW  = MadgwickFilter.getYaw();
  Serial.print(ROLL); Serial.print(",");
  Serial.print(PITCH); Serial.print(",");
  Serial.print(YAW);
  Serial.print("\n");
  delay(10);
}

void getAHRS() {//IMUの値を取得する関数
  if ( ahrs.gyroAvailable() )
  {
    ahrs.readGyro();
  }
  if ( ahrs.accelAvailable() )
  {
    ahrs.readAccel();
  }
  if ( ahrs.magAvailable() )
  {
    ahrs.readMag();
  }

Processing用スケッチ

Processing

import processing.serial.*; // シリアルライブラリをインポート

Serial myPort;  //シリアルポートのインスタンス
int available_serialport = 2; // シリアル検索プログラムで調べたシリアルポートの番号に設定数値を変更しておく
String arduinoPort = Serial.list()[available_serialport ]; //シリアルポートの設定
float []data = new float [3]; //シリアルのデータを格納する配列を宣言

void setup() {
  lights();
  size(300, 300, P3D); // キャンバスサイズ
  myPort = new Serial(this, arduinoPort, 115200); // シリアルポートの設定
}

void draw() {
  background(230); //背景グレー
  translate(width / 2, height / 2, 0); // 原点を図形の中心に
  rotateX(-radians(data[1])); //ここではProcessingのX軸としてPITCH軸の値を渡している.センサの初期方向にあわせお好みで調整
  rotateZ(radians(data[0])); //ここではProcessingのZ軸としてROLL軸の値を渡している.センサの初期方向にあわせお好みで調整

  int size = 10;//図形のサイズ倍率。キャンバスを大きくする時に変更可
  box(20 * size, 1 * size, 15 * size); //GY-521基盤のような直方体を描く
  translate(0, -4 * size, 0);
}

void serialEvent(Serial p) { //シリアルを監視
  String inString = myPort.readStringUntil('\n'); //データがあったら改行のところまで読み込む
  if (inString != null) { //シリアルの文字列データが何か入っていれば
    inString = trim(inString); //シリアル文字列の前後の空白を削除
    data = float(split(inString, ',')); //ピリオドで分割して配列に格納
    println(data);//受信した配列データをprocessingのコンソールに出力
  }
}

ポイント

レンジや単位を合わせるとよさそうです。

この方法で動くことは動くのですが、ちょっと収束が遅かったり、角度が十分に周りきらなかったりするので、使い方のどこかが間違っている可能性が高いです。

お願い

LSM9DS1を使う機会は今後増えるかもしれませんので、上手な使い方をご存知の方はぜひ情報をシェアいただけるとありがたいです。
9軸全部をつかったMadgwickや6軸でのヨー軸推定ライブラリなどもこのあと試してみようと思います。