LoginSignup
2
2

More than 1 year has passed since last update.

@GANTZ(GANTZ G)

MPU9250の磁気センサー（AK8963）をI2Cで使う・Magwickフィルタと連携

Last updated at Posted at 2021-08-24

概要

タイトル通りです。
SPIで使う方法は今後
参考

2年以上前に自分が書いたものも参考にしました

配線

MPU9250 expression Arduino
Vcc 5.0V 5.0V
GND GND GND
SCL clock A5
SDA Data Input A4

AD0をHIGH　－＞　アドレスが0x69
AD0をLOW 　－＞　アドレスが0x68 (何も接続しない場合)

動作test

最初に　レジスタアドレス0x6B　の　Bit6 の　SLEEP　を　LOW　にしないと動作しない。（デフォルトで0x6B-> 0x40 のため、最初はSLEEPがついている。

接続確認のために、Who_I_amレジスタ(アドレス0x75)を読む、0x71が読み込めたら成功


#include <Wire.h>
#define mpu_address 0x68


void setup()
{
  Wire.begin();

  Serial.begin(9600);
  Serial.println("Test start");

  write_mpu(0x6B,0x00);

}
void loop() {
  Serial.println(read_mpu(0x75),HEX);
  Serial.println("-----");
  delay(1000);
}

void write_mpu(byte add, byte data) {
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mpu(byte add) {
  byte k;
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}

ここでうまくいかない場合、接続が悪い可能性がある、I2C_scanner等での確認をしてほしい

設定データについて

1.png
オレンジ色の着色がデフォルト

2.png

address expression デフォルト
0x19 sampling rate 0x00
0x1A sampling rate 0x00
0x1B ジャイロ感度・ローパスフィルタ・sampling rate 0x00
0x1C 加速度感度 0x00
0x1D ローパスフィルタ 0x00
  • デフォルトでFCHOICE_B は00　です。Bは反転を表すので、デフォルトで　FCHOICE = 11です。
  • 0x19のSMPLRT_DIVはFCHOICE = 00の時のみ効果があり、SAMPLE_RATE= Internal_Sample_Rate / (1 + SMPLRT_DIV)となるようです。

感度設定

3.png

dps = degree per second
360[dps] = 2π [rad/s]

デフォルトは250dpsである。

250dpsの時
得られたデータが262(0x106)ならば、250/(2^15) * 262 = 2°/s の角加速度ということ

動作プログラム

これで、加速度・角速度の計測ができます


#include <Wire.h>
#define mpu_address 0x68


void setup()
{
  Wire.begin();

  Serial.begin(9600);
  Serial.println("Test start");

  write_mpu(0x6B, 0x00); //sleep 解除

  //write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz
}
void loop() {
  /*
    Serial.println(read_mpu(0x19));
    Serial.println(read_mpu(0x1B));
    Serial.println(read_mpu(0x1D));
    Serial.println(read_mpu(0x1A));
    Serial.println("-----");
  */

  byte gx_H = read_mpu(0x43);
  byte gx_L = read_mpu(0x44);
  byte gy_H = read_mpu(0x45);
  byte gy_L = read_mpu(0x46);
  byte gz_H = read_mpu(0x47);
  byte gz_L = read_mpu(0x48);

  int gx = gx_H << 8 | gx_L;
  int gy = gy_H << 8 | gy_L;
  int gz = gz_H << 8 | gz_L;

  float gx_dps = gx / 131.0;     //degree / s　へ変換
  float gy_dps = gy / 131.0;
  float gz_dps = gz / 131.0;

  byte ax_H = read_mpu(0x3B);
  byte ax_L = read_mpu(0x3C);
  byte ay_H = read_mpu(0x3D);
  byte ay_L = read_mpu(0x3E);
  byte az_H = read_mpu(0x3F);
  byte az_L = read_mpu(0x40);

  int ax = ax_H << 8 | ax_L;
  int ay = ay_H << 8 | ay_L;
  int az = az_H << 8 | az_L;


/*
  Serial.print(gx);
  Serial.print("  ");
  Serial.print(gy);
  Serial.print("  ");
  Serial.print(gz);
  Serial.println("  ");
*/
  Serial.print(gx_dps);
  Serial.print("  ");
  Serial.print(gy_dps);
  Serial.print("  ");
  Serial.print(gz_dps);
  Serial.println("  ");

}

void write_mpu(byte add, byte data) {
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mpu(byte add) {
  byte k;
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}
byte read_mpu_multiByte(byte add, int num) {
  byte k[10];
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  return k;
}

磁気センサーを使う

ようやく本題に入ります。

4.png

レジスタ55(0x37)のBYPASS_ENを1にすることでバイパスモードに入ります。そうすることで、

5.png

Interface Bypass Mux によって、AK8963と接続されます。データシート再度載せます

6.png
アドレスは0x0Cになります。

動作確認

Who_I_am(アドレス0x00)を読んでみましょう


#include <Wire.h>
#define mpu_address 0x68
#define mag_address 0x0C

void setup()
{
  Wire.begin();

  Serial.begin(9600);
  Serial.println("Test start");

  write_mpu(0x6B, 0x00); //sleep 解除

  //write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz

  write_mpu(0x37,0x02);  //AK8963接続
}
void loop() {

  byte gx_H = read_mpu(0x43);
  byte gx_L = read_mpu(0x44);
  byte gy_H = read_mpu(0x45);
  byte gy_L = read_mpu(0x46);
  byte gz_H = read_mpu(0x47);
  byte gz_L = read_mpu(0x48);

  int gx = gx_H << 8 | gx_L;
  int gy = gy_H << 8 | gy_L;
  int gz = gz_H << 8 | gz_L;

  float gx_dps = gx / 131.0;     //degree / s　へ変換
  float gy_dps = gy / 131.0;
  float gz_dps = gz / 131.0;

  byte ax_H = read_mpu(0x3B);
  byte ax_L = read_mpu(0x3C);
  byte ay_H = read_mpu(0x3D);
  byte ay_L = read_mpu(0x3E);
  byte az_H = read_mpu(0x3F);
  byte az_L = read_mpu(0x40);

  int ax = ax_H << 8 | ax_L;
  int ay = ay_H << 8 | ay_L;
  int az = az_H << 8 | az_L;


/*
  Serial.print(gx);
  Serial.print("  ");
  Serial.print(gy);
  Serial.print("  ");
  Serial.print(gz);
  Serial.println("  ");
*/
/*
  Serial.print(gx_dps);
  Serial.print("  ");
  Serial.print(gy_dps);
  Serial.print("  ");
  Serial.print(gz_dps);
  Serial.println("  ");
*/
  Serial.println(read_mag(0x00),HEX);
  Serial.println("-----");
}

void write_mpu(byte add, byte data) {
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mpu(byte add) {
  byte k;
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}
void write_mag(byte add, byte data) {
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mag(byte add) {
  byte k;
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}
byte read_mpu_multiByte(byte add, int num) {
  byte k[10];
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  return k;
}

0x48が読めれば成功です！

磁気データ読み込み

image.png

起動にはCNTL1(0x0A)のモードを変更する必要があります（デフォルトでパワーダウンモード）

重要な注意点は ST2レジスタ(0x09)の読み込みを行わないと、データが更新されないことです。
バースト読み込みで0x03から、0x09まで読み込むことを想定して作られているということです。

7.png

ポインタとか使おうかと思いましたが、わかりにくいので、全部グローバル変数にしちゃいました・・・


#include <Wire.h>
#define mpu_address 0x68
#define mag_address 0x0C

volatile int mag_x;
volatile int mag_y;
volatile int mag_z;

void setup()
{
  Wire.begin();

  Serial.begin(9600);
  Serial.println("Test start");

  write_mpu(0x6B, 0x00); //sleep 解除

  //write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz

  write_mpu(0x37,0x02);  //AK8963接続
  write_mag(0x0A,0x06);  //連続測定mode2

}
void loop() {

  byte gx_H = read_mpu(0x43);
  byte gx_L = read_mpu(0x44);
  byte gy_H = read_mpu(0x45);
  byte gy_L = read_mpu(0x46);
  byte gz_H = read_mpu(0x47);
  byte gz_L = read_mpu(0x48);

  int gx = gx_H << 8 | gx_L;
  int gy = gy_H << 8 | gy_L;
  int gz = gz_H << 8 | gz_L;

  float gx_dps = gx / 131.0;     //degree / s　へ変換
  float gy_dps = gy / 131.0;
  float gz_dps = gz / 131.0;

  byte ax_H = read_mpu(0x3B);
  byte ax_L = read_mpu(0x3C);
  byte ay_H = read_mpu(0x3D);
  byte ay_L = read_mpu(0x3E);
  byte az_H = read_mpu(0x3F);
  byte az_L = read_mpu(0x40);

  int ax = ax_H << 8 | ax_L;
  int ay = ay_H << 8 | ay_L;
  int az = az_H << 8 | az_L;

  byte magx_L = read_mag(0x03);
  byte magx_H = read_mag(0x04);

  //磁気データ計測
  read_mag_multiByte(0x03,7);   //7じゃないと動かない

/*
  Serial.print(gx);
  Serial.print("  ");
  Serial.print(gy);
  Serial.print("  ");
  Serial.print(gz);
  Serial.println("  ");
*/
/*
  Serial.print(gx_dps);
  Serial.print("  ");
  Serial.print(gy_dps);
  Serial.print("  ");
  Serial.print(gz_dps);
  Serial.println("  ");
*/

  Serial.print(mag_x);
  Serial.print("  ");
  Serial.print(mag_y);
  Serial.print("  ");
  Serial.print(mag_z);
  Serial.println("  ");

}

void write_mpu(byte add, byte data) {
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mpu(byte add) {
  byte k;
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}
void write_mag(byte add, byte data) {
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mag(byte add) {
  byte k;
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}


void read_mag_multiByte(byte add, int num) {
  byte k[10];
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  mag_x = k[1]<<8 | k[0];
  mag_y = k[3]<<8 | k[2];
  mag_z = k[5]<<8 | k[4];  
}

image.png

ひとまず、こんな感じでデータが取れます。このグラフ表示は、Arduino IDEのシリアルプロッタです（Ctrl+Shift+Lで起動）

Magwick Filterを使った、9軸姿勢検出

こちらを参考にしました。
加速度・角速度の読み込みをバーストに変更しました。

image.png

ひとまず、やった感じ。ジャイロのオフセットかな。deg/sへの変換でした。

image.png

ちょっと動かすと、すごく振動する・・・

感度テスト

250dpsらしいですが、ちょっと感度に不安を感じたので、ジャイロgzを積分し、一回転したとき360度になることを確認するプログラムを作りました。

#include <Wire.h>
#include <FlexiTimer2.h>
#define mpu_address 0x68
#define mag_address 0x0C

#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

volatile int mag_x;
volatile int mag_y;
volatile int mag_z;
volatile  int gx ;
volatile  int gy ;
volatile  int gz ;
volatile  int ax ;
volatile  int ay ;
volatile  int az ;
volatile  float gx_dps ;
volatile  float gy_dps ;
volatile  float gz_dps ;

volatile unsigned long time_now = 0;
volatile boolean control_flag = false;

volatile float degree=0;

void control() {

  if (time_now % 100 == 0) {
    control_flag = true;
  }
  time_now++;
}

void setup()
{
  Wire.begin();

  Serial.begin(250000);
  //Serial.println("Test start");
  Serial.println("ROLL PITCH YAW");

  write_mpu(0x6B, 0x00); //sleep 解除

  write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz

  write_mpu(0x37, 0x02); //AK8963接続
  write_mag(0x0A, 0x06); //連続測定mode2

  FlexiTimer2::set(1, 1.0 / 10000, control); // 0.1msごとに
  FlexiTimer2::start();
  MadgwickFilter.begin(100); //100Hz

}
void loop() {
  if (control_flag == true ) {
    control_flag = false;


    //6軸データ計測
    read_mpu_multiByte(0x3B, 14);  //7じゃないと動かない

    //磁気データ計測
    read_mag_multiByte(0x03, 7);  //7じゃないと動かない

//        Serial.print(gx);
//        Serial.print("  ");
//        Serial.print(gy);
//        Serial.print("  ");
//        Serial.print(gz);
//        Serial.println("  ");
//    

    /*
      Serial.print(gx_dps);
      Serial.print("  ");
      Serial.print(gy_dps);
      Serial.print("  ");
      Serial.print(gz_dps);
      Serial.println("  ");
    */
    /*
      Serial.print(mag_x);
      Serial.print("  ");
      Serial.print(mag_y);
      Serial.print("  ");
      Serial.print(mag_z);
      Serial.println("  ");
    */
    MadgwickFilter.update(gx, gy, gz, ax, ay, az, mag_x, mag_y, mag_z);

    float ROLL = MadgwickFilter.getRoll();
    float PITCH = MadgwickFilter.getPitch();
    float YAW   = MadgwickFilter.getYaw();

//    Serial.print(ROLL);
//    Serial.print("  ");
//    Serial.print(PITCH);
//    Serial.print("  ");
//    Serial.print(YAW);
//    Serial.println("  ");

    degree+= gz_dps*0.01;
    Serial.println(degree);
  }
}

void write_mpu(byte add, byte data) {
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mpu(byte add) {
  byte k;
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}
void read_mpu_multiByte(byte add, int num) {
  byte k[14];
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  ax = k[0] << 8 | k[1];
  ay = k[2] << 8 | k[3];
  az = k[4] << 8 | k[5];

  gx = (k[8] << 8 | k[9])-149;
  gy = (k[10] << 8 | k[11]) -96;
  gz = (k[12] << 8 | k[13])-115;

  gx_dps = gx/131.0;
  gy_dps = gy/131.0;
  gz_dps = gz/131.0;

}

void write_mag(byte add, byte data) {
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mag(byte add) {
  byte k;
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}


void read_mag_multiByte(byte add, int num) {
  byte k[10];
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  mag_x = k[1] << 8 | k[0];
  mag_y = k[3] << 8 | k[2];
  mag_z = k[5] << 8 | k[4];
}

image.png

ちゃんと半回転で180になりました

完成

#include <Wire.h>
#include <FlexiTimer2.h>
#define mpu_address 0x68
#define mag_address 0x0C

#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

volatile int mag_x;
volatile int mag_y;
volatile int mag_z;
volatile  int gx ;
volatile  int gy ;
volatile  int gz ;
volatile  int ax ;
volatile  int ay ;
volatile  int az ;
volatile  float gx_dps ;
volatile  float gy_dps ;
volatile  float gz_dps ;

volatile unsigned long time_now = 0;
volatile boolean control_flag = false;

volatile float degree=0;

void control() {

  if (time_now % 100 == 0) {
    control_flag = true;
  }
  time_now++;
}

void setup()
{
  Wire.begin();

  Serial.begin(250000);
  //Serial.println("Test start");
  Serial.println("ROLL PITCH YAW");

  write_mpu(0x6B, 0x00); //sleep 解除

  write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz

  write_mpu(0x37, 0x02); //AK8963接続
  write_mag(0x0A, 0x06); //連続測定mode2

  FlexiTimer2::set(1, 1.0 / 10000, control); // 0.1msごとに
  FlexiTimer2::start();
  MadgwickFilter.begin(100); //100Hz

}
void loop() {
  if (control_flag == true ) {
    control_flag = false;


    //6軸データ計測
    read_mpu_multiByte(0x3B, 14);  //7じゃないと動かない

    //磁気データ計測
    read_mag_multiByte(0x03, 7);  //7じゃないと動かない

//        Serial.print(gx);
//        Serial.print("  ");
//        Serial.print(gy);
//        Serial.print("  ");
//        Serial.print(gz);
//        Serial.println("  ");
//    

    /*
      Serial.print(gx_dps);
      Serial.print("  ");
      Serial.print(gy_dps);
      Serial.print("  ");
      Serial.print(gz_dps);
      Serial.println("  ");
    */
    /*
      Serial.print(mag_x);
      Serial.print("  ");
      Serial.print(mag_y);
      Serial.print("  ");
      Serial.print(mag_z);
      Serial.println("  ");
    */
    MadgwickFilter.update(gx_dps, gy_dps, gz_dps, ax, ay, az, mag_x, mag_y, mag_z);

    float ROLL = MadgwickFilter.getRoll();
    float PITCH = MadgwickFilter.getPitch();
    float YAW   = MadgwickFilter.getYaw();

    Serial.print(ROLL);
    Serial.print("  ");
    Serial.print(PITCH);
    Serial.print("  ");
    Serial.print(YAW);
    Serial.println("  ");

  }
}

void write_mpu(byte add, byte data) {
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mpu(byte add) {
  byte k;
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}
void read_mpu_multiByte(byte add, int num) {
  byte k[14];
  Wire.beginTransmission(mpu_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mpu_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  ax = k[0] << 8 | k[1];
  ay = k[2] << 8 | k[3];
  az = k[4] << 8 | k[5];

  gx = (k[8] << 8 | k[9])-149;
  gy = (k[10] << 8 | k[11]) -96;
  gz = (k[12] << 8 | k[13])-115;

  gx_dps = gx/131.0;
  gy_dps = gy/131.0;
  gz_dps = gz/131.0;

}

void write_mag(byte add, byte data) {
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.write(data);
  Wire.endTransmission();

}
byte read_mag(byte add) {
  byte k;
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, 1);
  while (Wire.available()) {
    k = Wire.read();
  }
  return k;
}


void read_mag_multiByte(byte add, int num) {
  byte k[10];
  Wire.beginTransmission(mag_address);
  Wire.write(add);
  Wire.endTransmission();
  Wire.requestFrom(mag_address, num);
  int i = 0;
  while (Wire.available()) {
    k[i] = Wire.read();
    i++;
  }
  mag_x = k[1] << 8 | k[0];
  mag_y = k[3] << 8 | k[2];
  mag_z = k[5] << 8 | k[4];
}

ちゃんと動くものができました。

ちなみに角度の初期値は
"MagwickAHRS.cpp"内の

Madgwick::Madgwick() {
    beta = betaDef;
    q0 = 0.0f;
    q1 = 0.0f;
    q2 = 1.0f;
    q3 = 0.0f;
    invSampleFreq = 1.0f / sampleFreqDef;
    anglesComputed = 0;
}

の"q0~q3"を変更すればできます。ただ、private変数のため外部で直接変更することはできません。

誰かの参考になることを願います。ではまた。

2
2
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
2
2