概要

タイトル通りです。
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等での確認をしてほしい

設定データについて

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

|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)となるようです。

感度設定

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;
}

磁気センサーを使う

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

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

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

アドレスは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が読めれば成功です！

磁気データ読み込み

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

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

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


# 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];  
}

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

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

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

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

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

感度テスト

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];
}

ちゃんと半回転で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変数のため外部で直接変更することはできません。

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