動作環境

ESP-WROOM-02
MPU-9250 https://strawberry-linux.com/catalog/items?code=12250
ライブラリ https://github.com/jrowberg/i2cdevlib/

http://qiita.com/7of9/items/906f92cbb9a4beb97914
に記載のコードで自動校正しようとしたがうまくいかない。

単純にゼロオフセットの時の測定値を出力するコードにした。
割込みを使うように変更もした。

code v0.1

v0.1 @ github

esp8266_160703_calibMPU9250.ino

/*
 * v0.1 2016 Jul. 03
 *   - remove Calibration()
 *   - use Interrupt
 *   - import for MPU-9250
 *   - based on the code at
 *   http://www.i2cdevlib.com/forums/topic/96-arduino-sketch-to-automatically-calculate-mpu6050-offsets/
 */

# include "I2Cdev.h"
# include "MPU9150_9Axis_MotionApps41.h"
# include "Wire.h"
# include <ESP8266WiFi.h>
MPU9150 accelgyro;

# define INTERRUPT_PIN 14  // ESP8266

int buffersize = 1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int16_t ax, ay, az,gx, gy, gz;

int mean_ax, mean_ay, mean_az;
int mean_gx, mean_gy, mean_gz;
int state=0;
int ax_offset, ay_offset, az_offset;
int gx_offset, gy_offset, gz_offset;

uint16_t packetSize;
uint16_t fifoCount;

void meansensors();
void showData();

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

void setup() {
  WiFi.disconnect();
  Wire.begin();
  Wire.setClock(400000L); // 400kHz
  Serial.begin(115200);

  accelgyro.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available()){
    Serial.println(F("Send any character to start sketch.\n"));
    delay(1500);
  }                
  while (Serial.available() && Serial.read()); // empty buffer again

  packetSize = accelgyro.dmpGetFIFOPacketSize();

  Serial.println("\nYour MPU-9250 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);
//  // verify connection
//  Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
//  delay(1000);
}

void loop() {
  if (state==0){
    Serial.println("\nReading sensors for first time...");
    meansensors();
    state++;
    delay(1000);
  }

  if (state==1) {
    Serial.println("\nMeasuring...");
    showData(); 
    state++;
    delay(1000);
  }

  if (state==2) {
    meansensors();
    Serial.print("\nSensor readings with zero offsets:\t");
    Serial.print(mean_ax); 
    Serial.print("\t");
    Serial.print(mean_ay); 
    Serial.print("\t");
    Serial.print(mean_az); 
    Serial.print("\t");
    Serial.print(mean_gx); 
    Serial.print("\t");
    Serial.print(mean_gy); 
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    state++;
  }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors(){
  long i=0;
  int buff_ax=0;
  int buff_ay=0;
  int buff_az=0;
  int buff_gx=0;
  int buff_gy=0;
  int buff_gz=0;

  while (i < (buffersize + 101) ){
    fifoCount = accelgyro.getFIFOCount();
    if (!mpuInterrupt && fifoCount < packetSize) {
      continue;
    }
    mpuInterrupt = false;
    
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        
    if ( (i > 100) && i <= (buffersize + 100) ){ // First 100 discarded
      buff_ax = buff_ax + ax;
      buff_ay = buff_ay + ay;
      buff_az = buff_az + az;
      buff_gx = buff_gx + gx;
      buff_gy = buff_gy + gy;
      buff_gz = buff_gz + gz;
    }
    if ( i == (buffersize + 100) ) {
      mean_ax = buff_ax / buffersize;
      mean_ay = buff_ay / buffersize;
      mean_az = buff_az / buffersize;
      mean_gx = buff_gx / buffersize;
      mean_gy = buff_gy / buffersize;
      mean_gz = buff_gz / buffersize;
    }
    i++;
  }
}

void showData()
{
  accelgyro.setXAccelOffset(0);
  accelgyro.setYAccelOffset(0);
  accelgyro.setZAccelOffset(0);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);

  for(int loop = 0; loop < 10; loop++) {
    meansensors();
    Serial.print(" ,mean_ax:");
    Serial.print(mean_ax);
    Serial.print(" ,mean_ay:");
    Serial.print(mean_ay);
    Serial.print(" ,mean_az:");
    Serial.print(mean_az);
    //
    Serial.print(" ,mean_gx:");
    Serial.print(mean_gx);
    Serial.print(" ,mean_gy:");
    Serial.print(mean_gy);
    Serial.print(" ,mean_gz:");
    Serial.print(mean_gz);
    //
    Serial.println();
  }

  accelgyro.setXAccelOffset(-81/8);
  accelgyro.setYAccelOffset(-264/8);
  accelgyro.setZAccelOffset((16384-16308)/8);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);

  for(int loop = 0; loop < 10; loop++) {
    meansensors();
    Serial.print(" ,mean_ax:");
    Serial.print(mean_ax);
    Serial.print(" ,mean_ay:");
    Serial.print(mean_ay);
    Serial.print(" ,mean_az:");
    Serial.print(mean_az);
    //
    Serial.print(" ,mean_gx:");
    Serial.print(mean_gx);
    Serial.print(" ,mean_gy:");
    Serial.print(mean_gy);
    Serial.print(" ,mean_gz:");
    Serial.print(mean_gz);
    //
    Serial.println();
  } 
}

showData()において、最初の10回はゼロオフセットの値を表示、次の10回は(加速度のみ)オフセット値を適用した値とした。

結果

Send any character to start sketch.


Your MPU-9250 should be placed in horizontal position, with package letters facing up. 
Don't touch it until you see a finish message.


Reading sensors for first time...

Measuring...
 ,mean_ax:98 ,mean_ay:267 ,mean_az:16311 ,mean_gx:-67 ,mean_gy:56 ,mean_gz:-286
 ,mean_ax:105 ,mean_ay:270 ,mean_az:16318 ,mean_gx:-67 ,mean_gy:57 ,mean_gz:-286
 ,mean_ax:105 ,mean_ay:257 ,mean_az:16319 ,mean_gx:-66 ,mean_gy:56 ,mean_gz:-287
 ,mean_ax:100 ,mean_ay:260 ,mean_az:16321 ,mean_gx:-66 ,mean_gy:58 ,mean_gz:-286
 ,mean_ax:99 ,mean_ay:257 ,mean_az:16314 ,mean_gx:-64 ,mean_gy:57 ,mean_gz:-286
 ,mean_ax:104 ,mean_ay:258 ,mean_az:16318 ,mean_gx:-66 ,mean_gy:56 ,mean_gz:-287
 ,mean_ax:102 ,mean_ay:263 ,mean_az:16317 ,mean_gx:-65 ,mean_gy:58 ,mean_gz:-285
 ,mean_ax:103 ,mean_ay:262 ,mean_az:16324 ,mean_gx:-65 ,mean_gy:56 ,mean_gz:-284
 ,mean_ax:103 ,mean_ay:260 ,mean_az:16315 ,mean_gx:-69 ,mean_gy:58 ,mean_gz:-284
 ,mean_ax:104 ,mean_ay:262 ,mean_az:16318 ,mean_gx:-66 ,mean_gy:56 ,mean_gz:-285
 ,mean_ax:103 ,mean_ay:260 ,mean_az:16307 ,mean_gx:-65 ,mean_gy:-1108 ,mean_gz:-219
 ,mean_ax:101 ,mean_ay:261 ,mean_az:16311 ,mean_gx:-66 ,mean_gy:-1107 ,mean_gz:-222
 ,mean_ax:100 ,mean_ay:262 ,mean_az:16316 ,mean_gx:-66 ,mean_gy:-1106 ,mean_gz:-219
 ,mean_ax:101 ,mean_ay:261 ,mean_az:16313 ,mean_gx:-67 ,mean_gy:-1104 ,mean_gz:-222
 ,mean_ax:100 ,mean_ay:261 ,mean_az:16312 ,mean_gx:-68 ,mean_gy:-1103 ,mean_gz:-220
 ,mean_ax:104 ,mean_ay:260 ,mean_az:16312 ,mean_gx:-66 ,mean_gy:-1103 ,mean_gz:-221
 ,mean_ax:102 ,mean_ay:259 ,mean_az:16312 ,mean_gx:-68 ,mean_gy:-1103 ,mean_gz:-221
 ,mean_ax:101 ,mean_ay:264 ,mean_az:16309 ,mean_gx:-66 ,mean_gy:-1102 ,mean_gz:-220
 ,mean_ax:101 ,mean_ay:266 ,mean_az:16314 ,mean_gx:-66 ,mean_gy:-1101 ,mean_gz:-220
 ,mean_ax:102 ,mean_ay:260 ,mean_az:16318 ,mean_gx:-67 ,mean_gy:-1101 ,mean_gz:-221

Sensor readings with zero offsets:	106	260	16310	-66	-1101	-220

Data is printed as: acelX acelY acelZ giroX giroY giroZ
Check that your sensor readings are close to 0 0 16384 0 0 0

最初の10回のmean_axの値と、オフセットを適用した次の10回のmean_axｎ値がほぼ同じ。
代わりにmean_gy, mean_gz(ジャイロのY,Z)の値が変更されている。

setXAccelOffset(),setYAccelOffset(),setZAccelOffset()にバグがあるのだろうか。
MPU-6050でなくMPU-9150で使う場合の使い方が間違っているのかもしれない。

setXGyroOffset(),setYGyroOffset(),setZGyroOffset()の方はきちんと校正できる。

ライブラリの中身

setXAccelOffset()とsetXGyroOffset()

setXAccelOffset()やsetXGyroOffset()はic2devライブラリのMPU9150.cppにて宣言されている。

MPU9150.cpp

int16_t MPU9150::getXAccelOffset() {
    I2Cdev::readBytes(devAddr, MPU9150_RA_XA_OFFS_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU9150::setXAccelOffset(int16_t offset) {
    I2Cdev::writeWord(devAddr, MPU9150_RA_XA_OFFS_H, offset);
}

MPU9150.cpp

int16_t MPU9150::getXGyroOffset() {
    I2Cdev::readBytes(devAddr, MPU9150_RA_XG_OFFS_USRH, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}
void MPU9150::setXGyroOffset(int16_t offset) {
    I2Cdev::writeWord(devAddr, MPU9150_RA_XG_OFFS_USRH, offset);
}

成功するsetXGyroOffset()と失敗しているgetXAccelOffset()ともにI2Cdev::writeWord()にて書込みしている点に違いはない。

レジスタの定義

MPU9150.hの定義

MPU9150.hにおいて以下のようにレジスタ定義がされている。

# define MPU9150_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS
# define MPU9150_RA_XA_OFFS_L_TC     0x07

# define MPU9150_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR
# define MPU9150_RA_XG_OFFS_USRL     0x14

そして、某処で見たMPU-9250用のRegister Mapではアドレスは以下となっている。

0x77: XA_OFFSET_H [14:7]
0x78: XA_OFFSET_L [6:0]
0x13: XG_OFFS_USR_H [15:8]
0x14: XG_OFFS_USR_L [7:0]

MPU9150.hの定義しているレジスタアドレス(XA_OFFSET_H: 0x06で定義)が実際のRegister Mapの値(0x77など)に一致していないということだろうか。

試しに上記の0x77などの値をMPU9150_RA_XA_OFFS_Hなどに定義してみたが、その場合は加速度の値が32768や-32768になってしまった。

以下では0x77を使っているようではある。
https://developer.mbed.org/questions/6482/Configuring-MPU9250-magnometer/

kriswinerさんの定義

https://github.com/kriswiner/MPU-9250/blob/master/STM32F401/MPU9250.h
において以下のように定義はしている。

# define XA_OFFSET_H      0x77
# define XA_OFFSET_L      0x78
# define YA_OFFSET_H      0x7A
# define YA_OFFSET_L      0x7B
# define ZA_OFFSET_H      0x7D
# define ZA_OFFSET_L      0x7E

その定義にもとづき読込みをしている。

readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];

しかしながら、書込みはしていない。以下のコメントがついている。

// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
/*  writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
  writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
  writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
  writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
  writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
  writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
*/

レジスタ書込み失敗することに対して、プログラム自身で引き算をしているようだ (manual subtraction)。

// Output scaled accelerometer biases for manual subtraction in the main program
   dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 
   dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
   dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;

補正方法

(追記 2016/07/05)

以下に記載した。
http://qiita.com/7of9/items/d3c765912b435815cfdf