LoginSignup
0
1

More than 5 years have passed since last update.

@7of9

傾斜計の校正 (v0.1) > setXAccelOffset()で校正できない

Last updated at Posted at 2016-07-03
動作環境
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

0
1
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
0
1