LoginSignup
5
6

More than 5 years have passed since last update.

@7of9

傾斜計 > MPU-9250の加速度の校正方法 (おそらく)

Last updated at Posted at 2016-07-04
動作環境
ESP-WROOM-02
MPU-9250 https://strawberry-linux.com/catalog/items?code=12250
ライブラリ https://github.com/jrowberg/i2cdevlib/

MPU-9250で加速度の校正ができたようだ。方法に関する資料はなく、結果を見ながらの推測。

手順は以下の通り。

`1. Arduino\libraries\MPU9150\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_YA_OFFS_H        0x08 //[15:0] YA_OFFS
#define MPU9150_RA_YA_OFFS_L_TC     0x09
#define MPU9150_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS
#define MPU9150_RA_ZA_OFFS_L_TC     0x0B
*/
#define MPU9150_RA_XA_OFFS_H        0x77 //[15:0] XA_OFFS
#define MPU9150_RA_XA_OFFS_L_TC     0x78
#define MPU9150_RA_YA_OFFS_H        0x7A //[15:0] YA_OFFS
#define MPU9150_RA_YA_OFFS_L_TC     0x7B
#define MPU9150_RA_ZA_OFFS_H        0x7D //[15:0] ZA_OFFS
#define MPU9150_RA_ZA_OFFS_L_TC     0x7E

`2. getXAccelOffset()などで元の値を取得する
(例: XOFFSET:-5170, YOFFSET:5390. ZOFFSET:8424)

  Serial.println(accelgyro.getXAccelOffset());
  Serial.println(accelgyro.getYAccelOffset());
  Serial.println(accelgyro.getZAccelOffset());

`3. 元の値に2の倍数を足し引きする
測定値と正負反対の値を8で割った値に近い2の倍数とする。
(奇数を足し引きすると全く異なる値になる)

  accelgyro.setXAccelOffset(-5170 - 8); // divide by 8, but should be even not odd number
  accelgyro.setYAccelOffset(5390 - 38); // divide by 8, but should be even not odd number 
  accelgyro.setZAccelOffset(8424 + 16); // divide by 8, but should be even not odd number

code v0.2

v0.2 @ github

esp8266_160703_calibMPU9250.ino
/*
 * v0.2 2016 Jul. 05
 *   - add calibration example
 * 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()
{
  Serial.println(accelgyro.getXAccelOffset());
  Serial.println(accelgyro.getYAccelOffset());
  Serial.println(accelgyro.getZAccelOffset());

//  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(-5170 - 8); // divide by 8, but should be even not odd number
  accelgyro.setYAccelOffset(5390 - 38); // divide by 8, but should be even not odd number 
  accelgyro.setZAccelOffset(8424 + 16); // divide by 8, but should be even not odd number
  accelgyro.setXGyroOffset( 80 / 4); // divide by 4 
  accelgyro.setYGyroOffset( 8 / 4); // divide by 4
  accelgyro.setZGyroOffset( 286 / 4); // divide by 4

  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();
  }

}
結果
Send any character to start sketch.

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...
-5170
5390
8424
 ,mean_ax:63 ,mean_ay:304 ,mean_az:16262 ,mean_gx:-78 ,mean_gy:-10 ,mean_gz:-287
 ,mean_ax:63 ,mean_ay:306 ,mean_az:16261 ,mean_gx:-78 ,mean_gy:-10 ,mean_gz:-285
 ,mean_ax:60 ,mean_ay:307 ,mean_az:16264 ,mean_gx:-77 ,mean_gy:-8 ,mean_gz:-285
 ,mean_ax:60 ,mean_ay:308 ,mean_az:16264 ,mean_gx:-78 ,mean_gy:-8 ,mean_gz:-286
 ,mean_ax:62 ,mean_ay:309 ,mean_az:16267 ,mean_gx:-78 ,mean_gy:-9 ,mean_gz:-285
 ,mean_ax:65 ,mean_ay:309 ,mean_az:16263 ,mean_gx:-77 ,mean_gy:-8 ,mean_gz:-286
 ,mean_ax:66 ,mean_ay:309 ,mean_az:16255 ,mean_gx:-77 ,mean_gy:-7 ,mean_gz:-286
 ,mean_ax:64 ,mean_ay:309 ,mean_az:16258 ,mean_gx:-79 ,mean_gy:-8 ,mean_gz:-285
 ,mean_ax:65 ,mean_ay:304 ,mean_az:16260 ,mean_gx:-79 ,mean_gy:-9 ,mean_gz:-285
 ,mean_ax:55 ,mean_ay:305 ,mean_az:16256 ,mean_gx:-78 ,mean_gy:-8 ,mean_gz:-285
 ,mean_ax:-3 ,mean_ay:7 ,mean_az:16388 ,mean_gx:2 ,mean_gy:0 ,mean_gz:-2
 ,mean_ax:-4 ,mean_ay:1 ,mean_az:16388 ,mean_gx:1 ,mean_gy:0 ,mean_gz:-1
 ,mean_ax:0 ,mean_ay:3 ,mean_az:16388 ,mean_gx:1 ,mean_gy:0 ,mean_gz:-2
 ,mean_ax:0 ,mean_ay:7 ,mean_az:16390 ,mean_gx:1 ,mean_gy:0 ,mean_gz:-2
 ,mean_ax:0 ,mean_ay:4 ,mean_az:16387 ,mean_gx:1 ,mean_gy:-2 ,mean_gz:-1
 ,mean_ax:0 ,mean_ay:6 ,mean_az:16389 ,mean_gx:2 ,mean_gy:-1 ,mean_gz:-1
 ,mean_ax:-1 ,mean_ay:5 ,mean_az:16388 ,mean_gx:0 ,mean_gy:0 ,mean_gz:-1
 ,mean_ax:0 ,mean_ay:1 ,mean_az:16390 ,mean_gx:1 ,mean_gy:0 ,mean_gz:-1
 ,mean_ax:0 ,mean_ay:5 ,mean_az:16383 ,mean_gx:0 ,mean_gy:0 ,mean_gz:0
 ,mean_ax:-3 ,mean_ay:1 ,mean_az:16391 ,mean_gx:0 ,mean_gy:-2 ,mean_gz:-2

Sensor readings with zero offsets:  0   9   16387   2   -2  -2

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

補足

  • 加速度の補正値を8で割ったり、ジャイロの補正値を4で割る理由は不明だが、MPU-6050の補正でも8や4で割っていた。

  • 補正値を奇数にすると値がおかしくなる

    • レジスタマップのp.44, XA_OFFS_Lの説明にBIT[7:1]とあり、LSB([0]のこと)については設定しないようになっている。奇数の場合(LSB=1)おかしな測定値が出力されるのと関係があるのかもしれない

  • azは16384に近ければ補正できている。他の値は0に近いこと。

  • 一番最初に加速度のオフセットを0にしてしまうと、元の値とまったく異なる値になり、32768や-32768の測定値になってしまう。元の値からの補正にすること。

5
6
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
6