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

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

https://mangoooou.blogspot.com/2018/04/ak8963mpu9250.html

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

https://mangoooou.blogspot.com/2017/08/mpu6050.html

#配線
|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が読み込めたら成功

```C

#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等での確認をしてほしい

https://playground.arduino.cc/Main/I2cScanner/

#設定データについて
![1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/62f73919-70c8-d665-26a1-32b45f255ce6.png)
オレンジ色の着色がデフォルト

![2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/3798c763-e64e-2323-b89f-f30c9dba2784.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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/bab1abdb-f3e7-c9a1-1d38-5308a98ef0e3.png)

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

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

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

#動作プログラム
これで、加速度・角速度の計測ができます

```C

#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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/dee12098-5119-0f73-e11e-170cccb5688d.png)

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

![5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/d8c14e15-d744-a4ab-822e-f8906cde57ab.png)

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

https://strawberry-linux.com/pub/AK8963.pdf


![6.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/38ef18fa-5a15-9f45-5dc4-91b14812db9c.png)
アドレスは0x0Cになります。

###動作確認
Who_I_am(アドレス0x00)を読んでみましょう

```C

#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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/79516230-1e78-a551-7f1c-234a661d0334.png)

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

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

![7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/c1aa4ef4-4847-b37c-c7ce-5dd059131898.png)


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

```C

#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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/a3d26bc0-fec3-a582-3288-d16a1545700c.png)

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

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

https://garchiving.com/how-to-madgwick-library-of-arduino/

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/8a475679-dcde-a9d3-77b2-4abc633e2d8f.png)

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/d8182c54-244f-15d3-8211-6184aecc5c09.png)

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

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

```C
#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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/fa753b61-b4b7-e495-d47e-65393319bfad.png)

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

###完成

```C
#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"内の

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



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