地磁気センサーのキャリブレーション方法

電子工作
2020.05.16

皆さんこんにちは
おかしょです．

今回は地磁気センサーから方位角を求める前に必ずやらなければならないキャリブレーション方法についてご説明します．
キャリブレーションの必要性の確認から実際にキャリブレーションをするところまで行っていきたいと思います．

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

  • キャリブレーションとは
  • 地磁気センサーのキャリブレーション方法

　

この記事を読む前に

この記事では地磁気センサーを使用します．

地磁気センサーはLSM9DS1というものを使用しています．
このセンサーからデータを取得する方法については以下の記事で解説しているので，そちらを先に読んでおくことをおすすめします．

[電子工作] 加速度・ジャイロ・地磁気センサのデータを取得する
みなさん，こんにちは．おかしょです． この記事ではセンサからデータを取得する方法を解説します． このブログでは倒立振子ロボットを自立させるために，必要な知識を紹介しています．その中の必要な工程として，倒立振子の傾きを知る必要があります． ロ...
okasho-engineer.com
2020.03.11

　

今回使用するもの

  • Arduino Uno
  • USBケーブル（書き込み用）
  • ブレッドボード
  • ジャンパー線
  • XBee×2（シリアル通信用）
  • XBeeピッチ変換基盤
  • XBee USBインターフェースボード
  • USBケーブル（通信用）
  • 電池（9V）
  • LSM9DS1（9軸センサー）

シリアル通信はセンサーを動かしやすいようにXBeeで無線化しています．

特に問題がない方はUSBケーブルでシリアル通信を行っても大丈夫なので，XBee関連の部品と電池は必要ありません．

　

なぜキャリブレーションが必要なのか

まずは，キャリブレーションがなぜ必要なのかを説明します．

そもそもキャリブレーションとは，現在の環境をセンサーに教え込む作業を言います．地磁気センサーの場合は，キャリブレーションを行うことによって現在の地磁気の流れを教えることができます．

それでは，皆さんは地磁気とは何かご存知でしょうか．

この説明をすると本題に入れなくなるので割愛しますが，地球は北極側がS極，南極側がN極となっています．そのため，磁力線は南極から北極に入っていきます．
この流れを観測するのが地磁気センサーです．

この磁気の流れは場所や周りの環境によって異なります．周りに鉄製品などが多い場所では，それぞれの物質が磁力を持っているため，南極から北極へと流れる磁力を乱してしまいます．そのため，地磁気センサーにもその影響が現れます．鉄製品が多すぎる環境での使用は避けた方が良いのですが，影響が小さい環境ではキャリブレーションを行うことによって，地磁気センサーは周囲の磁力の流れを把握し，補正することができます．

　

キャリブレーションの方法

ここからはどのようにして地磁気のキャリブレーションを行うのかについて説明します．

先程はキャリブレーションとはセンサーに周囲の環境を教え込むことだと言いましが，正確に言うと，センサーのゼロ点と感度を調べる作業のことを言います．

地磁気センサーの出力は磁場に比例します．磁場の値が0であれば，出力される値も0となるはずです．しかし，実際にはそうはなりません．磁場の値が0の時に出力される値をゼロ点と呼びます．このゼロ点を0に合わせることによってキャリブレーションを行います．

　

2次元の場合

とりあえず，キャリブレーションを行う前のデータを見てみます．この時に用いたプログラムも示しておきます．

#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        500    // 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 MagX, MagY, MagZ;

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 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;
  GetMag();
  if((millis()-PrintTime)>1/PRINT_RATE*1000)
  {
    Serial.print(MagX);
    Serial.print(',');
    Serial.print(MagY);
    Serial.print(',');
    Serial.println(MagZ);
    PrintTime = millis();
  }
}

このデータは以下のようにセンサーをブレッドボードに載せてx-y平面内で回転させた結果から得られました．

このように出力されるデータは円を描くことがわかります．この円の中心を原点に合わせる作業をキャリブレーションと言います．それでは，まずはこの状態（2次元）でのキャリブレーションを行っていきます．

キャリブレーションの方法は様々ありますが，最も一般的に用いられ，簡単な方法を以下に示していきます．

ゼロ点を求めるには，各軸方向で出力される値の最大値と最小値を求め，それらの中間点を算出することによって求められます．数式で表すと以下のようになります．

$$「ゼロ点」=\frac{「最大値」+「最小値」}{2}$$

ゼロ点のことをオフセット値などと呼ぶこともあります．このような2次元のキャリブレーションを行うプログラムを以下に示します．

#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 MagX, MagY, MagZ;
int16_t MagZero[2];

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 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 CalibMag()
{
  int16_t MagMax[2];
  int16_t MagMin[2];
  GetMag();
  MagMax[0] = MagX;
  MagMin[0] = MagX;
  MagMax[1] = MagY;
  MagMin[1] = MagY;
  for(uint16_t i=0; i<1500;)
  {
    GetMag();
    if(MagX>MagMax[0])
    {
      MagMax[0] = MagX;
    }
    else if(MagX<MagMin[0])
    {
      MagMin[0] = MagX;
    }
    if(MagY>MagMax[1])
    {
      MagMax[1] = MagY;
    }
    else if(MagY<MagMin[1])
    {
      MagMin[1] = MagY;
    }
    
    if((millis()-PrintTime)>1/PRINT_RATE*1000)
    {
      i++;
      PrintTime = millis();
    }
  }

  for(uint8_t i=0; i<2; i++)
  {
    MagZero[i] = (MagMax[i]+MagMin[i])/2;
  }
}

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

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

  CalibMag();

  InitTime = millis();
}

void loop() {
  RunTime = millis()-InitTime;
  GetMag();
  if((millis()-PrintTime)>1/PRINT_RATE*1000)
  {
    Serial.print("MagX,\t");
    Serial.print(MagX-MagZero[0]);
    Serial.print("\t,MagY,\t");
    Serial.println(MagY-MagZero[1]);
    PrintTime = millis();
  }
}

この結果，以下のようなグラフができます．

先程の図と比べて，円の中心位置が原点に来ていることが確認できます．このようにして，キャリブレーションが行えたら，方位角の算出を行う準備ができたと言えます．

以上のプログラムでは2次元のキャリブレーションを行いましたが，マルチコプターのように3次元で運動するロボットに搭載する場合は，3次元でキャリブレーションを行う必要があります．なので，3次元でもキャリブレーションを行ってみましょう．

　

3次元の場合

まずは，キャリブレーションを行う前のデータを見てみます．

赤い点がこの球の中心を表していて，わかりづらいかもしれませんが原点からずれています．座標で言うと\((x, y, z)=(484, 269, 507)\)となっています．

3次元でキャリブレーションを行う場合も，方法は先ほどと同じように，最大値と最小値を求めて，その中間点を求めます．

プログラムは以下のようになります．

#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 OFFSET_X_REG      0x05
#define OFFSET_Y_REG      0x07
#define OFFSET_Z_REG      0x09
#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        500    // 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 MagX, MagY, MagZ;
int16_t MagZero[3];

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 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 CalibMag()
{
  int16_t MagMax[3];
  int16_t MagMin[3];
  GetMag();
  MagMax[0] = MagX;
  MagMin[0] = MagX;
  MagMax[1] = MagY;
  MagMin[1] = MagY;
  MagMax[2] = MagZ;
  MagMin[2] = MagZ;
  for(uint16_t i=0; i<30000;)
  {
    GetMag();
    if(MagX>MagMax[0])
    {
      MagMax[0] = MagX;
    }
    else if(MagX<MagMin[0])
    {
      MagMin[0] = MagX;
    }
    if(MagY>MagMax[1])
    {
      MagMax[1] = MagY;
    }
    else if(MagY<MagMin[1])
    {
      MagMin[1] = MagY;
    }
    if(MagZ>MagMax[2])
    {
      MagMax[2] = MagZ;
    }
    else if(MagZ<MagMin[2])
    {
      MagMin[2] = MagZ;
    }
    
    if((millis()-PrintTime)>1/PRINT_RATE*1000)
    {
      i++;
      PrintTime = millis();
    }
  }

  for(uint8_t i=0; i<3; i++)
  {
    MagZero[i] = (MagMax[i]+MagMin[i])/2;
  }
  
  uint8_t RegisterData = 0;

  RegisterData=(uint8_t)(MagZero[0]&0x00FF);
  I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_X_REG, RegisterData);
  RegisterData=(uint8_t)((MagZero[0]&0xFF00)>>8);
  I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_X_REG+1, RegisterData);
  RegisterData=(uint8_t)(MagZero[1]&0x00FF);
  I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Y_REG, RegisterData);
  RegisterData=(uint8_t)((MagZero[1]&0xFF00)>>8);
  I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Y_REG+1, RegisterData);
  RegisterData=(uint8_t)(MagZero[2]&0x00FF);
  I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Z_REG, RegisterData);
  RegisterData=(uint8_t)((MagZero[2]&0xFF00)>>8);
  I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Z_REG+1, RegisterData);
}

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

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

  CalibMag();
  Serial.print("int16_t MagZero[3] = {");
  Serial.print(MagZero[0]);
  Serial.print(',');
  Serial.print(MagZero[1]);
  Serial.print(',');
  Serial.print(MagZero[2]);
  Serial.print("};");

  InitTime = millis();
}

void loop() {
  RunTime = millis()-InitTime;
  GetMag();
  if((millis()-PrintTime)>1/PRINT_RATE*1000)
  {
//    Serial.print(MagX);
//    Serial.print(',');
//    Serial.print(MagY);
//    Serial.print(',');
//    Serial.println(MagZ);
    PrintTime = millis();
  }
}

このプログラムを実行した結果，以下のようなデータが得られます．

中心点を求めると，\((x, y, z)=(48, -86, 41)\)となり，中心が原点に近づいたことがわかります．

　

まとめ

この記事では地磁気センサーのキャリブレーション方法を解説しました．

以上のようにすることで，地磁気のキャリブレーションを行うことができます．

今回は原点の座標を求めて，出力されるデータを補正しましたが，センサーにはオフセットを設定するレジスタが用意されているので，実際に使うときはオフセット値をレジスタに書き込んで使用すると良いでしょう．

私はオフセットを取るために，椅子に座り，センサーを傾けながら椅子を回転させたのですが，目が回って気持ち悪くなりました．皆さんも気を付けてください．

　

続けて読む

地磁気センサーのキャリブレーションはセンサーの向きをあらゆる方法に向ける必要があります．

その向きにむらがあったりすると，キャリブレーションが十分に行えないことがあります．

そこで，キャリブレーションを自動で行うロボットを作ってみます．

以下の記事で，そのロボットの構想を紹介しているので興味のある方は参考にしてください．

センサーキャリブレーション用ロボット（構想編）
みなさん，こんにちはおかしょです． 地磁気センサーに限らず，センサーのキャリブレーションを行うにはセンサーをさまざまな方向に向ける必要があります．以前まではセンサーを持ってぐるぐる回したり，椅子の上で自分が回りながら手に持っているセンサーを...
okasho-engineer.com
2020.05.17

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．

それでは最後まで読んでいただきありがとうございました．

コメント

  1. センサーキャリブレーション用ロボット（構想編） – 理系大学院生の知識の森 より:
    2020/05/17 2:01 PM

    […] 以前，地磁気センサーのキャリブレーション方法を解説した記事を公開しました．地磁気センサーに限らず，センサーのキャリブレーションを行うにはセンサーをさまざまな方向に向け […]

    返信
タイトルとURLをコピーしました