[電子工作] 加速度・ジャイロ・地磁気センサのデータを取得する

みなさん，こんにちは．
おかしょです．

この記事ではセンサからデータを取得する方法を解説します．

このブログでは倒立振子ロボットを自立させるために，必要な知識を紹介しています．その中の必要な工程として，倒立振子の傾きを知る必要があります．

ロボットの姿勢を知りたいときは，よく加速度センサやジャイロセンサを利用して姿勢を算出します．加速度センサだけ，もしくはジャイロセンサだけでも理論的には姿勢を算出できます．しかし，実際にはセンサーが一つだけでは姿勢を算出することができません．そのため，加速度センサとジャイロセンサ両方を利用します．

地磁気センサはロボットの向きを知りたいときに使用します．倒立振子ロボットを立たせるためだけであれば必要ありません．ロボットの位置などを誘導したい場合は，方位角の情報が必要になるので，地磁気センサが必要になります．

この記事ではすべてのセンサの設定を行い，データの取得まで行います．

この記事を読むと以下のようなことがわかる・できるようになります．

センサの設定の仕方
センサからデータを取得する方法・プログラム

この記事を読む前に
必要なもの
使用したセンサーについて
センサーの設定方法
センサーのデータを表示する
まとめ
続けて読む
1. 共有:
2. 関連

この記事を読む前に

この記事ではArduinoを使ってセンサーとI2C通信を行います．

そのため，I2C通信とは何なのかわかっている必要があります．

I2C通信についてよくわからない場合は，以下の記事でI2C通信とはどのような通信方式でどのようなプログラムで通信ができるのかを解説しているので，まだ読んでいない方は読んでおくことをおすすめします．

必要なもの

今回使用したものはこれだけです．

Arduino
USBケーブル（書き込み・電源用）
ブレッドボード
ジャンパー線
9軸センサ（LSM9DS1）

使用したセンサーについて

今回はセンサーにLSM9DS1を使用しました．

他のブログなどではMPU6050などを使ったものをよく見かけますが，MPU6050では加速度とジャイロしか取得できません．そのため，方位角も求めたい場合は，地磁気センサーを他に用意する必要があります．

LSM9DS1を使用すれば加速度とジャイロ，地磁気が一つにまとまっているため，私はこのセンサーを使用しています．

簡単にこのセンサーの特徴をまとめると以下のようになります．

3軸加速度，3軸ジャイロ，3軸地磁気が取得可能
加速度は+-2, +-4, +-8, +-16gの範囲で測定可能
地磁気は+-4, +-8, +-12, +-16gaussの範囲で測定可能
ジャイロは+-245, +-500, +-2000dpsの範囲で測定可能（dps: degree per second）
それぞれのデータは16bitでデータを出力
SPI通信とI2C通信が可能（ボードによってはどちらかに限定される）
供給電圧は1.9vから3.6v
温度センサーが実装されている

また，このセンサーの利用例は以下のものがあります．

室内誘導
スマートデバイス
ジェスチャー認識
ゲーミングデバイス
地図ブラウジング

センサーの設定方法

センサーの設定は，使用する用途によって異なります．

ここでは倒立振子ロボットに実装することを想定した設定を行っていきたいと思います．

センサーの設定をするにあたって，最も重要となるのがdatasheetを読むことです．

使用するセンサーがこの記事で使用しているものと同じものの場合は，同じように設定すればいいので問題ありませんが，別のものを使用する場合は，設定が異なります．その場合は，自分でdatasheetを読んで，どの設定がどのレジスターに割り当てられているかなどを確認しなければなりません．この，datasheetを読むという作業が初心者の方にとっては非常に大変で，挫折する原因となります．datasheetの内容自体が難解なのも原因の一つですが，ほとんどのdatasheetが英語で表記されていることも大きな要因と言えます．

はじめは大変だと思いますが，頑張って読んでみましょう．一つのセンサーのdatasheetが読めるようになれば，他のセンサーに挑戦するときに，ハードルが少し下がると思います．

とは言っても，ただの趣味で電子工作を始めた方は，datasheetの難解さに嫌気がさしてしまうかもしれません．そのような方はArduinoのライブラリを活用しましょう．ほとんどのセンサーはArduinoのライブラリが用意されていて，初心者の方でもそのライブラリを用いることによってすぐにセンサーを使用することができます．

しかし，このブログではそのようなライブラリは使用しません（大いに参考にはしますが）．ライブラリを使ってしまうと，ある程度型が決まってしまっているため，思った通りの動作をさせるのが難しい，またはできない可能性があるからです．なので，自分で設定したいところは自由に設定して，使用しない部分はライブラリと同様な設定にしていきます．

それでは実際に設定をしていきましょう．

実際にセンサーに設定を書き込むプログラムは以下になります．

  RegisterData=ODR_G<<5;
  RegisterData|=FS_G<<3;
  RegisterData|=BW_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG1_G, RegisterData);
  RegisterData=0;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG2_G, RegisterData);
  RegisterData=LP_mode<<7;
  RegisterData|=HP_EN<<6;
  RegisterData|=HPCF_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG3_G, RegisterData);
  RegisterData=0;
  RegisterData=Zen_G<<5;
  RegisterData|=Yen_G<<4;
  RegisterData|=Xen_G<<3;
  RegisterData|=LIR_XL1<<2;
  RegisterData|=FourD_XL1;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG4, RegisterData);
  RegisterData=0;
  RegisterData=SignX_G<<5;
  RegisterData|=SignY_G<<4;
  RegisterData|=SignZ_G<<3;
  RegisterData|=Orient;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, ORIENT_CFG_G, RegisterData);
  RegisterData=0;
  RegisterData=DEC_<<6;
  RegisterData|=Zen_XL<<5;
  RegisterData|=Yen_XL<<4;
  RegisterData|=Xen_XL<<3;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG5_XL, RegisterData);
  RegisterData=0;
  RegisterData=ODR_XL<<5;
  RegisterData|=FS_XL<<3;
  RegisterData|=BW_SCAL_ODR<<2;
  RegisterData|=BW_XL;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG6_XL, RegisterData);
  RegisterData=0;
  RegisterData=HR<<7;
  RegisterData|=DCF<<5;
  RegisterData|=FDS<<2;
  RegisterData|=HPIS;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG7_XL, RegisterData);
  RegisterData=0;
  RegisterData=TEMP_COMP<<7;
  RegisterData|=OM<<5;
  RegisterData|=DO<<2;
  RegisterData|=FAST_ODR<<1;
  RegisterData|=ST;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG1_M, RegisterData);
  RegisterData=0;
  RegisterData=FS<<5;
  RegisterData|=REBOOT<<3;
  RegisterData|=SOFT_RST<<2;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG2_M, RegisterData);
  RegisterData=0;
  RegisterData=I2C_DISABLE<<7;
  RegisterData|=LP<<5;
  RegisterData|=SIM<<2;
  RegisterData|=MD;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG3_M, RegisterData);
  RegisterData=0;
  RegisterData=OMZ<<2;
  RegisterData|=BLE<<1;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG4_M, RegisterData);
  RegisterData=0;
  RegisterData=FAST_READ<<7;
  RegisterData|=BLE<<6;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG5_M, RegisterData);

上記のプログラムによって，LSM9DS1の設定は終了になります．例として一番上に書かれている設定を用いて説明します．

  RegisterData=ODR_G<<5;
  RegisterData|=FS_G<<3;
  RegisterData|=BW_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG1_G, RegisterData);

ここでは，まずRegisterDataに書き込むデータを格納していきます．そして，I2Cwrite1byteという関数（この関数についてはこちらを参照）を用いてAG_DEVICE_ADDRESSのCTRL_REG1_GにRegisterDataを書き込んでいます．

ここで，CTRL_REG1_Gというレジスターがどのようなbit配置になっているのかを見てみましょう．

CTRL_REG1_G

上図のように左3bitでODRの設定，次の2bitでFSの設定，ひとつ飛ばして残り2bitでBWの設定をしています．全てのレジスターがこのように各bitに数値を書き込むことによってさまざまな設定ができるようになっています．

上記のプログラムでODR_Gを左に5bitシフトしているのはこのためです．このような操作をすべての設定用レジスターに対して行うことで，使用環境に適した性能を有するセンサーに設定することができます．

倒立振子ロボットに搭載することを考えると，上記の設定変数は以下のように定義すると良いと思います．

  uint8_t ODR_G = 3;  // Output data rate: 1=14.9, 2=59.5, 3=119, 4=238, 5=476, 6=952　Hz
  uint8_t FS_G = 0;   // Full scale: 0=245, 1=500, 2=no, 3=2000 dps
  uint8_t BW_G = 0;   // cutoff frequency: table 47
  uint8_t LP_mode = 0;  // low power mode: 0=disable, 1=enable
  uint8_t HP_EN = 1;    // high pass filter: 0=disable, 1=enable
  uint8_t HPCF_G = 1;   // high pass cutoff frequency: table 52
  uint8_t Zen_G = 1;    // enable gyro Z:  0=disable, 1=enable
  uint8_t Yen_G = 1;    // enable gyro Y:  0=disable, 1=enable
  uint8_t Xen_G = 1;    // enable gyro X:  0=disable, 1=enable
  uint8_t LIR_XL1 = 1;  // latch interrupt: 0=no request, 1=request
  uint8_t FourD_XL1 = 0;
  uint8_t SignX_G = 0;  // x axis sign: 0=positive, 1=negative
  uint8_t SignY_G = 0;  // y axis sign: 0=positive, 1=negative
  uint8_t SignZ_G = 0;  // z axis sign: 0=positive, 1=negative
  uint8_t Orient = 0;
  uint8_t DEC_ = 0;
  uint8_t Zen_XL = 1;   // enable accel Z:  0=disable, 1=enable
  uint8_t Yen_XL = 1;   // enable accel Y:  0=disable, 1=enable
  uint8_t Xen_XL = 1;   // enable accel X:  0=disable, 1=enable
  uint8_t ODR_XL = 1;   // Output date rate: 1=10, 4=238, 2=50, 5=476, 3=119, 6=952 Hz
  uint8_t FS_XL = 3;   // Full scale: 0=2, 1=16, 2=4, 3=8 g
  uint8_t BW_SCAL_ODR = 1;  // 0=band width determined by ODR, 1=determined by BW_XL
  uint8_t BW_XL = 0;    // band width: 0=408, 1=211, 2=105, 3=50 Hz
  uint8_t HR = 0;       // high resolution mode: 0=disable, 1=enable
  uint8_t DCF = 0;      // low pass cutoff frequency: 0=ODR/50, 2=ODR/9,　1=ODR/100, 3=ODR/400
  uint8_t FDS = 0;
  uint8_t HPIS = 0;  
  uint8_t TEMP_COMP = 0;  // temperature compensation: 0=disable, 1=enable
  uint8_t OM = 0;         // operation mode: 0=continuous-conversion, 1=single-conversion,　2=power-down
  uint8_t DO = 5;         // Output data rate: 0=0.625, 1=1.25, 2=2.5, 3=5, 4=10, 5=20, 6=40, 7=80 Hz
  uint8_t FAST_ODR = 0;
  uint8_t ST = 0;
  uint8_t FS = 2;         // full scale: 0=4, 1=8, 2=12, 3=16 gauss
  uint8_t REBOOT = 0;
  uint8_t SOFT_RST = 0;
  uint8_t I2C_DISABLE = 0;  // I2c interface: 0=enable, 1=disable
  uint8_t LP = 0;           // low power mode: 0=disable, 1=enable
  uint8_t SIM = 0;          // SPI interface
  uint8_t MD = 0;           // operation mode: 0=continuous-conversion,　1=single-conversion,　2=power-down
  uint8_t OMZ = 3;    // z axis operation mode: 0=low power, 1=medium, 2=high, 3=ultra-high performance
  uint8_t BLE = 0;
  uint8_t FAST_READ = 0;
  uint8_t BDU = 0;

もっとうまく書くこともできるのですが，ここではとりあえずこのような書き方をしました．

センサーのデータを表示する

センサーの設定ができたら，データを表示します．

その前に，書き込みが終わり，センサーとの通信が行えているのかを念のために確認しておくといいでしょう．センサーのArduino ライブラリでもそのようにしています．

この確認は[電子工作] センサとI2C通信を行うでも行ったレジスターにアクセスして確認します．実際に確認するプログラムは以下になります．

  who_ag = I2Cread1byte(AG_DEVICE_ADDRESS, AG_WHO_AM_I);
  who_m = I2Cread1byte(M_DEVICE_ADDRESS, M_WHO_AM_I);
  if (who_ag==0x68&&who_m==0x3D)
  {
    return true;
  }
  else
  {
    return false;
  }

詳細はこちらを参照してください．

回路図と実際に配線した結果は以下のようになります．

通信の確認ができたら，センサからデータを取得します．

データ取得に用いたプログラムは以下のようになります．

#include <Wire.h>

#define AG_DEVICE_ADDRESS 0x6B
#define M_DEVICE_ADDRESS  0x1E
#define AG_WHO_AM_I       0x0F
#define CTRL_REG1_G       0x10
#define CTRL_REG2_G       0x11
#define CTRL_REG3_G       0x12
#define ORIENT_CFG_G      0x13
#define OUT_T             0x15
#define STATUS_REG_T      0x17
#define OUT_G             0x18
#define CTRL_REG4         0x1E
#define CTRL_REG5_XL      0x1F
#define CTRL_REG6_XL      0x20
#define CTRL_REG7_XL      0x21
#define STATUS_REG_AG     0x27
#define OUT_A             0x28
#define M_WHO_AM_I        0x0F
#define CTRL_REG1_M       0x20
#define CTRL_REG2_M       0x21
#define CTRL_REG3_M       0x22
#define CTRL_REG4_M       0x23
#define CTRL_REG5_M       0x25
#define STATUS_REG_M      0x27
#define OUT_M             0x28

#define PRINT_RATE        50    // PRINT_RATE Hzで出力する

  uint8_t ODR_G = 3;  // Output data rate: 1=14.9, 2=59.5, 3=119, 4=238, 5=476, 6=952　Hz
  uint8_t FS_G = 0;   // Full scale: 0=245, 1=500, 2=no, 3=2000 dps
  uint8_t BW_G = 0;   // cutoff frequency: table 47
  uint8_t LP_mode = 0;  // low power mode: 0=disable, 1=enable
  uint8_t HP_EN = 1;    // high pass filter: 0=disable, 1=enable
  uint8_t HPCF_G = 1;   // high pass cutoff frequency: table 52
  uint8_t Zen_G = 1;    // enable gyro Z:  0=disable, 1=enable
  uint8_t Yen_G = 1;    // enable gyro Y:  0=disable, 1=enable
  uint8_t Xen_G = 1;    // enable gyro X:  0=disable, 1=enable
  uint8_t LIR_XL1 = 1;  // latch interrupt: 0=no request, 1=request
  uint8_t FourD_XL1 = 0;
  uint8_t SignX_G = 0;  // x axis sign: 0=positive, 1=negative
  uint8_t SignY_G = 0;  // y axis sign: 0=positive, 1=negative
  uint8_t SignZ_G = 0;  // z axis sign: 0=positive, 1=negative
  uint8_t Orient = 0;
  uint8_t DEC_ = 0;
  uint8_t Zen_XL = 1;   // enable accel Z:  0=disable, 1=enable
  uint8_t Yen_XL = 1;   // enable accel Y:  0=disable, 1=enable
  uint8_t Xen_XL = 1;   // enable accel X:  0=disable, 1=enable
  uint8_t ODR_XL = 1;   // Output date rate: 1=10, 4=238, 2=50, 5=476, 3=119, 6=952 Hz
  uint8_t FS_XL = 3;   // Full scale: 0=2, 1=16, 2=4, 3=8 g
  uint8_t BW_SCAL_ODR = 1;  // 0=band width determined by ODR, 1=determined by BW_XL
  uint8_t BW_XL = 0;    // band width: 0=408, 1=211, 2=105, 3=50 Hz
  uint8_t HR = 0;       // high resolution mode: 0=disable, 1=enable
  uint8_t DCF = 0;      // low pass cutoff frequency: 0=ODR/50, 2=ODR/9,　1=ODR/100, 3=ODR/400
  uint8_t FDS = 0;
  uint8_t HPIS = 0;  
  uint8_t TEMP_COMP = 0;  // temperature compensation: 0=disable, 1=enable
  uint8_t OM = 0;         // operation mode: 0=continuous-conversion, 1=single-conversion,　2=power-down
  uint8_t DO = 5;         // Output data rate: 0=0.625, 1=1.25, 2=2.5, 3=5, 4=10, 5=20, 6=40, 7=80 Hz
  uint8_t FAST_ODR = 0;
  uint8_t ST = 0;
  uint8_t FS = 2;         // full scale: 0=4, 1=8, 2=12, 3=16 gauss
  uint8_t REBOOT = 0;
  uint8_t SOFT_RST = 0;
  uint8_t I2C_DISABLE = 0;  // I2c interface: 0=enable, 1=disable
  uint8_t LP = 0;           // low power mode: 0=disable, 1=enable
  uint8_t SIM = 0;          // SPI interface
  uint8_t MD = 0;           // operation mode: 0=continuous-conversion,　1=single-conversion,　2=power-down
  uint8_t OMZ = 3;    // z axis operation mode: 0=low power, 1=medium, 2=high, 3=ultra-high performance
  uint8_t BLE = 0;
  uint8_t FAST_READ = 0;
  uint8_t BDU = 0;

// 時間変数
unsigned long InitTime, RunTime, PrintTime;

int16_t AccX, AccY, AccZ;
int16_t GyrX, GyrY, GyrZ;
int16_t MagX, MagY, MagZ;
int16_t Temp;

uint8_t I2Cread1byte(uint8_t DeviceAddress, uint8_t SlaveAddress)
{
  Wire.beginTransmission(DeviceAddress); // 通信を開始する
  Wire.write(SlaveAddress);             // レジスターアドレスを送信
  Wire.endTransmission(false);  // リスタートメッセージをリクエストの後に送信し，接続を続ける
  Wire.requestFrom(DeviceAddress, (uint8_t) 1);  // 1 byte分のデータを要求する
  return Wire.read();         // データを返す
}

uint8_t I2CreadMULTIbyte(uint8_t DeviceAddress, uint8_t SlaveAddress, uint8_t num, uint8_t * data)
{
  Wire.beginTransmission(DeviceAddress);
  Wire.write(SlaveAddress|0x80);
  Wire.endTransmission(false);
  Wire.requestFrom(DeviceAddress, num);
  for (uint8_t i=0; i<num; i++)
  {
    data[i] = Wire.read();
  }
  return num;
}

void I2Cwrite1byte(uint8_t DeviceAddress, uint8_t SlaveAddress, uint8_t data)
{
  Wire.beginTransmission(DeviceAddress); // 通信を開始する
  Wire.write(SlaveAddress);             // レジスターアドレスを送信
  Wire.write(data);             // レジスターアドレスを送信
  Wire.endTransmission();       // 通信を終了する
}

bool LSM9DS1setting()
{
  uint8_t who_ag, who_m;

  uint8_t RegisterData = 0;

  RegisterData=ODR_G<<5;
  RegisterData|=FS_G<<3;
  RegisterData|=BW_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG1_G, RegisterData);
  RegisterData=0;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG2_G, RegisterData);
  RegisterData=LP_mode<<7;
  RegisterData|=HP_EN<<6;
  RegisterData|=HPCF_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG3_G, RegisterData);
  RegisterData=0;
  RegisterData=Zen_G<<5;
  RegisterData|=Yen_G<<4;
  RegisterData|=Xen_G<<3;
  RegisterData|=LIR_XL1<<2;
  RegisterData|=FourD_XL1;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG4, RegisterData);
  RegisterData=0;
  RegisterData=SignX_G<<5;
  RegisterData|=SignY_G<<4;
  RegisterData|=SignZ_G<<3;
  RegisterData|=Orient;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, ORIENT_CFG_G, RegisterData);
  RegisterData=0;
  RegisterData=DEC_<<6;
  RegisterData|=Zen_XL<<5;
  RegisterData|=Yen_XL<<4;
  RegisterData|=Xen_XL<<3;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG5_XL, RegisterData);
  RegisterData=0;
  RegisterData=ODR_XL<<5;
  RegisterData|=FS_XL<<3;
  RegisterData|=BW_SCAL_ODR<<2;
  RegisterData|=BW_XL;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG6_XL, RegisterData);
  RegisterData=0;
  RegisterData=HR<<7;
  RegisterData|=DCF<<5;
  RegisterData|=FDS<<2;
  RegisterData|=HPIS;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG7_XL, RegisterData);
  RegisterData=0;
  RegisterData=TEMP_COMP<<7;
  RegisterData|=OM<<5;
  RegisterData|=DO<<2;
  RegisterData|=FAST_ODR<<1;
  RegisterData|=ST;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG1_M, RegisterData);
  RegisterData=0;
  RegisterData=FS<<5;
  RegisterData|=REBOOT<<3;
  RegisterData|=SOFT_RST<<2;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG2_M, RegisterData);
  RegisterData=0;
  RegisterData=I2C_DISABLE<<7;
  RegisterData|=LP<<5;
  RegisterData|=SIM<<2;
  RegisterData|=MD;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG3_M, RegisterData);
  RegisterData=0;
  RegisterData=OMZ<<2;
  RegisterData|=BLE<<1;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG4_M, RegisterData);
  RegisterData=0;
  RegisterData=FAST_READ<<7;
  RegisterData|=BLE<<6;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG5_M, RegisterData);


  who_ag = I2Cread1byte(AG_DEVICE_ADDRESS, AG_WHO_AM_I);
  who_m = I2Cread1byte(M_DEVICE_ADDRESS, M_WHO_AM_I);
  if (who_ag==0x68&&who_m==0x3D)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void GetAccel()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&1)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_A, 6, data)==6)
    {
      AccX = (data[1]<<8)|data[0];
      AccY = (data[3]<<8)|data[2];
      AccZ = (data[5]<<8)|data[4];
    }
  }
}

void GetGyro()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&2)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_G, 6, data)==6)
    {
      GyrX = (data[1]<<8)|data[0];
      GyrY = (data[3]<<8)|data[2];
      GyrZ = (data[5]<<8)|data[4];
    }
  }
}

void GetTemp()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&4)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_T, 2, data)==2)
    {
      Temp = (data[1]<<8)|data[0];
    }
  }
}

void GetMag()
{
  uint8_t data[6];
  if(I2Cread1byte(M_DEVICE_ADDRESS, STATUS_REG_M)&8)  // データの更新確認
  {
    if(I2CreadMULTIbyte(M_DEVICE_ADDRESS, OUT_M, 6, data)==6)
    {
      MagX = (data[1]<<8)|data[0];
      MagY = (data[3]<<8)|data[2];
      MagZ = (data[5]<<8)|data[4];
    }
  }
}

void setup() {
  Serial.begin(9600); // シリアル通信のレートを設定
  Wire.begin();         // I2C通信の初期化

  if(LSM9DS1setting())
  {
    Serial.println("LSM9DS1 setting success");
  }
  else
  {
    Serial.println("LSM9DS1 setting failure");
    while(1);
  }

  InitTime = millis();
}

void loop() {
  RunTime = millis()-InitTime;
  GetAccel();
  GetGyro();
  GetTemp();
  GetMag();
  if((millis()-PrintTime)>1/PRINT_RATE*1000)
  {
    Serial.print("AccelX:\t");
    Serial.print(AccX);
    Serial.print("\tAccelY:\t");
    Serial.print(AccY);
    Serial.print("\tAccelZ:\t");
    Serial.println(AccZ);
    Serial.print("GyroX:\t");
    Serial.print(GyrX);
    Serial.print("\tGyroY:\t");
    Serial.print(GyrY);
    Serial.print("\tGyroZ:\t");
    Serial.println(GyrZ);
    Serial.print("Temp:\t");
    Serial.println(Temp);
    Serial.print("MagX:\t");
    Serial.print(MagX);
    Serial.print("\tMagY:\t");
    Serial.print(MagY);
    Serial.print("\tMagZ:\t");
    Serial.println(MagZ);
    PrintTime = millis();
  }
}

プログラム内で使用している関数の解説は以下の記事で行っているので，そちらを参照してください．
[Arduino] I2C通信で1byteだけ通信する関数
 [Arduino] I2C通信で2byte以上の通信をする関数