File content as of revision 6:eeef849dd7a1:

/*MPU9250_PROGRAM ver4.1 PROGRAMED BY RYOTARO FUNAI 2018/08/14*/
#include <mbed.h>
#include <math.h>

I2C i2c(p9, p10);
Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン

#define MPU9250_ADDRESS 0x68<<1  //I2CでのMPU9250のスレーブアドレス
#define AK8963_ADDRESS 0x0c<<1   //磁気センサのスレーブアドレス
#define Whoami 0x75   //who_am_iレジスタのアドレス、0x71が返ってくる
#define Magadd 0x00   //ak8963のWho_am_i
#define PWR 0x6b      //スリープモードをonにするためのアドレス
#define MAG_OPN 0x37  //mpu9250から磁気センサにアクセスできるようにする
#define ACC_CONFIG 0x1c     //加速度センサ設定用のアドレス
#define ACC_2G 0x00  //加速度センサのレンジ(2G)
#define ACC_4G 0x08  //加速度センサのレンジ(4G)
#define ACC_8G 0x10  //加速度センサのレンジ(8G)
#define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
#define MAG_CONFIG 0x0a  //磁気センサ設定用のアドレス
#define MAG_8HZ 0x12     //磁気センサの出力周期(8Hz)
#define MAG_100HZ 0x16   //磁気センサの出力周期(100Hz)
#define accRange  16.0   //加速度センサの測定レンジ
#define ST2 0x02 //磁気センサのステータスが入っているアドレス
#define Ain 35
#define SDA 21
#define SCL 22
#define led 2   //チェック用のLEDピン

void Ac_Read(int16_t*, int16_t*, int16_t*);   //9軸から加速度の値を取得
void Mag_Read(int16_t*, int16_t*, int16_t*);
//addrにスレーブアドレス、regにアクセスするアドレスを入力する
void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
uint8_t IDcheck();
int EarthMag();

uint8_t accgyrodata[14];
uint8_t magneticdata[7];
uint8_t ST2_Bit;  //磁気センサのステータスを入れておく

int main() {
    EarthMag(); //加速度センサの値は引数として渡したらいいかも
}

int EarthMag(){
    //方位角計測用各種変数群
    int16_t mx, my, mz;
    double magX, magY, magZ, mag;
    int16_t ax, ay, az;
    float accX, accY, accZ, acc;
    double rad;     //単位はラジアン
    double degree;  //単位は度数
    double roll;    //ロール軸の角度
    double pitch;   //ピッチ軸の角度
    double maveX = 0, maveY = 0, maveZ = 0;
    
    //地磁気センサの設定
    i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
    i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
    i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定        
    
    while(1) {
        //z軸方向の誤差を取るためのロールとピッチ
        Ac_Read(&ax,&ay,&az);
        accX = ax * accRange / 32768.0;//[G]に変換
        accY = ay * accRange / 32768.0;//[G]に変換
        accZ = az * accRange / 32768.0;//[G]に変換
        roll = atan2(accY, accZ);
        pitch = atan2(-accX, sin(accY) + cos(accZ));
        for(int i = 0;i < 50;i++){
            Mag_Read(&mx, &my, &mz);
            maveX += mx;
            maveY += my;
            maveZ += mz;
            wait_ms(10);
        }          
        maveX /= 50;
        maveY /= 50;
        maveZ /= 50;
        magX = (maveX + 11.44) / 32768.0f * 4800.0f;//[uT]に変換
        magY = (maveY + 12.1) / 32768.0f * 4800.0f;//[uT]に変換
        magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
        pc.printf("%f,%f\n\r", magX, magY);
        rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
        degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 8.34) % 360 - 360.0);
        pc.printf("%d\n\r",(int)degree);
        return (int)degree;
    }
}

//mpu9250から加速度センサのみ引っ張ってくる
void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
  i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
  i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
  i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
  *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
  *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
  *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
}

//mpu9250から磁気センサのみ引っ張ってくる
void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
  i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
  if(ST2_Bit & 0x01){   //ちゃんと読めたかをST2レジスタの値を読んで確認
    i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
  }
  else pc.printf("ERROR!!\n");
  *mx = ((int16_t)magneticdata[3] << 8) | (int16_t)magneticdata[2];
  *my = ((int16_t)magneticdata[1] << 8) | (int16_t)magneticdata[0];
  *mz = -((int16_t)magneticdata[5] << 8) | (int16_t)magneticdata[4];
}

//mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
  char cmd[1];
  char written_data[14];
  cmd[0] = reg;
  i2c.write(addr, cmd, 1, 1);
  i2c.read(addr, written_data, bytes, 0);
  for(int ii = 0; ii < bytes; ii++) {
    data[ii] = written_data[ii];
  }
}

//mpu9250にデータを送信(dataに送信するデータを入力する)
void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
  char cmd[2];
  cmd[0] = reg;     //レジスタ指定
  cmd[1] = data;    //送信するデータ
  i2c.write(addr, cmd, 2); //レジスタ指定はこうする
}

//Who_am_Iアドレスで接続確認ができる。0x48もしくは10進数で72が返ってくればおk
uint8_t IDcheck(){
  uint8_t address;
  i2cRead(AK8963_ADDRESS, Magadd, 1, &address);
  return address;
}