タグ: ロボットカーコンテスト

これまで、方位についてはGPSから入力される信号を用いていたのだが、

利点として

・移動時の測定においては移動方向の方位が取得できる。

・GPSの精度が良ければ方位も十分な精度が期待できる。

　ここでいう精度とは再現性である。

欠点として

・移動していないと方位の取得が取れない

これまでは、基本的に移動している状態で方位を算出していたので、GPSでの方位取得を行ってきた。

しかし、今回は必要性に迫られた部分もあり磁気センサから方位算出を行うこととした。

使用したのは、 MPU9250 と呼ばれる9軸センサ。
ジャイロ、加速度、磁気センサが入って9軸というわけだが、今後の遊びも含めてということでこれを選定。

値段もまぁ許される程度です。

しかし、中身はちょっと素直ではなさそう。

いくつかのサンプルを試してみたが、高速で読もうとすると時々0が戻ってきたりする。　サンプルが悪い気もしなくはないのだが、データシートを見ると、磁気センサはSDA/SCLというよりも AUX_CL,AUX_DAに接続されているので、そちらをI2Cと想定して試してみた。 というわけであくまで保証はできなく参考程度にしてください。　また、この磁気センサは旭化成の AK8963 であり、使い方などはAK8963 のデータシートが参考になる。　下記はMPU-925のブロック図です。

ESP32との配線は、以下の感じ。

MPU-9250のSCL/SDAには接続せずEDA/ECLに配線することによって磁気センサに直接アクセスが可能となる。

参考のためにソースコードを記載する。 転用などは特に連絡不要ですが無保証です。

また、勝手な想像によるキャリブレーション 機能も搭載しています。

// This software is released under the MIT License.
// See license.txt at http://moon.robots.jp/ehp/ 
// The software is provided "AS IS", without warranty of any kind, 
// express or implied, including but not limited to the warranties
// of merchantability. (c) 2021 Team Katy
// 
// 2021/5/23 ver 1.0 first released by S.K
//
//

#include <Wire.h>

const int   I2C_AK8963_addr=0x0c;   // i2c: ak8963 slave address

////// AK8963 register 
const int AK_ID= 0x00;  //  00H READ  Device ID; ID is 0x48
const int AK_ST1= 0x02; //  02H READ  Status 1  DataStatus; D0:True; data ready D1:True data overrun
const int AK_HXL= 0x03; //  03H READ  X-axis data 8190:4912uT -8190:-4912uT
const int AK_CNTL1= 0x0A; //  0AH READ/WRITE  Control1  Function Control
                          // see data sheet D0:3 mode; D4:0 is 14 bit, 1 is 16bit
const int AK_ASAX= 0x10;  //  10H READ  X-axis sensitivity adjustment value Fuse ROM
const int   AK_WAIT=100;     // Wait for mode change [us]
const int   AK_MOD_PWRDWN=0x0;   // Power down mode 
const int   AK_MOD_READ_METHOD=0x1;   
                // Read method:0x1:Single
                //             0x2:Continus 8Hz
                //             0x3:Continus 100Hz
const int   AK_MOD_FUSE=0x0f;    // mode change to read FUSE
const int   AK_MOD_BIT=0x10;   // # of read bits: 0x10:16 bit, 0x00:14 bit
const int   AK_I2C_ADD=0x48;   // I2C address for AK8963
static float x_bias = 0; // calibration value
static float x_gain = 0; // calibration value
static float y_bias = 0; // calibration value
static float y_gain = 1; // calibration value
static float z_bias = 1; // calibration value
static float z_gain = 1; // calibration value

static int  i2c_buf[8];    // read buffer for I2C
const int   max_char=8;    // max read characters from serial monitor

//********** i2c_read **********//
int i2c_read(int addr, int cnt= 1, int *buf=NULL)
{
  if (cnt<1 or 128<=cnt) return(-1);
  int   rtn_code, i, dat;

  Wire.beginTransmission(I2C_AK8963_addr);
  Wire.write(addr);
  rtn_code= Wire.endTransmission(false);
  if (rtn_code!=0) return(-1);

  Wire.requestFrom(I2C_AK8963_addr, cnt);
  for (i=0; i<cnt; i++)
  {
    dat=Wire.read();
    if (buf==NULL)
      break;
    else
      buf[i]=dat;
  }

  while (Wire.available()) Wire.read();

  if (buf == NULL)
    return(dat);
  else 
    return(buf[0]);
}

//********** i2c_write **********//
int i2c_write(int addr, int dat)
{
  int rtn_code;

  Wire.beginTransmission(I2C_AK8963_addr);
  Wire.write(addr);
  Wire.write(dat);
  rtn_code=Wire.endTransmission();
  if (rtn_code!=0) 
    return (-1);
  else 
    return(0);
}

//********** mag_read **********//
int mag_read(float &x, float &y, float &z)
{
  int   rtn_code;
  delayMicroseconds(AK_WAIT);
  rtn_code=i2c_write(AK_CNTL1, AK_MOD_READ_METHOD | AK_MOD_BIT);
  if (rtn_code!=0) {
    Serial.println("Error in configuration setting for AK8963");
    return(-1);
  }

  while ((i2c_read(AK_ST1) and 0x01)==0); // wait until ready.

  i2c_read(AK_HXL, 7,i2c_buf); // read measure data and status2 
  if (i2c_buf[6] & 0x08)        // check sensor overflow in status2
  {
    Serial.println(" Sensor overflow ");
    return (-1);
  }

  x=int ((i2c_buf[1]<<8) + i2c_buf[0]);
  y=int ((i2c_buf[3]<<8) + i2c_buf[2]);
  z=int ((i2c_buf[5]<<8) + i2c_buf[4]);

  if (x>32767) x= x-65536;
  if (y>32767) y= y-65536;
  if (z>32767) z= z-65536;
  
  x+=x_bias;
  y+=y_bias;
  z+=z_bias;

  return (0);
}

//********** calib_mag **********//
void calib_mag()
{
  int   i, pre_cnt = 10;
  float xmin=9999,xmax=0,ymin=9999,ymax=0,zmin=9999,zmax=0;
  float x,y,z;
  int   calib_count = 150;
  float cx[calib_count];
  float cy[calib_count];
  float cz[calib_count];
  x_bias =0;
  y_bias =0;
  z_bias =0;
 
  Serial.println("Entering Mag");

  for (i=0; i<=pre_cnt; i++){
    delay(1000);
    Serial.print(pre_cnt-i);
    Serial.println(" sec reamin for calibration.");
  }
  
  Serial.println(" Start calibration, please rotate the sensor.");
  for (i=0; i<calib_count-1; i++){
    mag_read(x,y,z);
    Serial.print(calib_count-i);
    Serial.print("/");
    Serial.print(calib_count);
    Serial.print(": x="); Serial.print(x);
    Serial.print(": y="); Serial.print(y);
    Serial.print(": z="); Serial.println(z);
    cx[i]=x;
    cy[i]=y;
    cz[i]=z;
    if(xmin>x) xmin = x;
    if(ymin>y) ymin = y;
    if(zmin>z) zmin = z;
    if(xmax<x) xmax = x;
    if(ymax<y) ymax = y;
    if(zmax<z) zmax = z;
    delay(100);
  }
  x_bias = 0 - (xmax + xmin) / 2;
  y_bias = 0 - (ymax + ymin) / 2;
  z_bias = 0 - (zmax + zmin) / 2;
  x_gain = 1 / ((xmax - xmin) / 2);
  y_gain = 1 / ((ymax - ymin) / 2);
  z_gain = 1 / ((zmax - zmin) / 2);
  Serial.print("Calibrated Values are X Bias ="); Serial.print(x_bias);
  Serial.print(", Y Bias "); Serial.print(y_bias,3);
  Serial.print(", Z Bias "); Serial.print(x_bias,3);
  Serial.print(", X Gain "); Serial.print(x_gain,5);
  Serial.print(", Y Gain "); Serial.print(y_gain,5);
  Serial.print(", Z Gain "); Serial.println(z_gain,5);
  
  Serial.println("Wait a 15 seconds");
  delay(15000);
}

//********** read_serial **********//
int read_serial()
{
  if (Serial.available()){
    char ch = Serial.read();
    return((int) ch );
  }
  else return(0);
}

//********** setup **********//
void  setup (void)
{
  Serial.begin(115200);
  Wire.begin();
  if (i2c_read(AK_ID) != AK_I2C_ADD) Serial.println("NOT FOUND AK8963");
  delayMicroseconds(AK_WAIT);
  
}

//********** loop **********//
void  loop(void)
{
  int ch_i = read_serial();
  Serial.print("ch = ");   Serial.print(ch_i); 
  
  if ( ch_i >= 49){ // ch_i = '1' 
    Serial.println("Calib Start");
    calib_mag();
  }
  float x,y,z;
  int rad;
  mag_read(x,y,z);
  rad=atan(y/x)*(180/3.1416) + ((x>=0)? 0: (y<0)? -180: 180);
  Serial.print(rad); 
  Serial.print(": "); 
  Serial.print(x,5); 
  Serial.print(","); 
  Serial.print(y,5); 
  Serial.print(","); 
  Serial.println(z,5); 
}