MPU6050 の Digital Motion Processor の性能を試してみた（４）：息子と一緒に Makers：So-netブログ

	ブログをはじめるログイン

MPU6050 の Digital Mo..｜MPU6050 の Digital Mo.. ブログトップ

MPU6050 の Digital Motion Processor の性能を試してみた（４）　[Arduino] [編集]

前回の検討で、MPU6050をCalibrationをしてもオフセットが残ってしまう問題が残りました。何か解決策を考えなければなりません。

一般的なやり方は、センサーの出力値からローパスフィルターを使ってＤＣ成分を得て除去するというもの。Android のページでも紹介されています。

SensorEvent
https://developer.android.com/reference/android/hardware/SensorEvent.html

具体的には下記のようなアルゴリズムです。

 // alpha は 0.5 ～ 0.8 で調整
 lpf_signal = alpha * x[i-1] + (1 - alpha) * x[i]; // low pass filter
 hpf_signal = x[i] - lpf_signal; // distill high pass value using low pass value

しかし、これを実際のセンサーの出力（10msec間隔）に適用すると、ローパスフィルタの値がセンサーの出力値とほぼ重なってしまい、正しい加速度が算出できません。（赤がローパスフィルターの波形で、その下に青のセンサーからの出力波形が隠れています）

理想的には数十サンプルの平均値をとるようなローパスフィルターを実現したいところです。でも配列はメモリを食うので使いたくないし・・・。

ということで、下記のようなアルゴリズムを考えてみました。

#include 
#include 
#include "I2Cdev.h"
#include "MPU6050DMP.h"
extern "C" {
#include "user_interface.h"
}

#define INT_PIN 4

MPU6050DMP mpu6050;
bool dmpReady = false;
uint16_t  packetSize = 0;

volatile bool mpuInterrupt = false;
void dmpDataReady() {
    mpuInterrupt = true;
}

unsigned long getCurrentTimeMS() {
  return ESP.getCycleCount() / 80000L;
}

void setup() {

  Wire.begin(12, 14);  
  Wire.setClock(400000);
  Serial.begin(115200);
  pinMode(INT_PIN, INPUT);
  WiFi.disconnect();
  WiFi.mode(WIFI_OFF);
  WiFi.forceSleepBegin();

  if (mpu6050.testConnection()) {
    Serial.println("MPU6050 OK");
    attachInterrupt(INT_PIN, dmpDataReady, RISING);
  } else {
    Serial.println("MPU6050 NG");
    return;
  }
  
  mpu6050.initialize();
  // offset values for my sensor
  mpu6050.setXAccelOffset(565);
  mpu6050.setYAccelOffset(-5);
  mpu6050.setZAccelOffset(1432);
  mpu6050.setXGyroOffset(17);
  mpu6050.setYGyroOffset(550);
  mpu6050.setZGyroOffset(464);

  Serial.println("");
  Serial.println("");

  if (mpu6050.dmpInitialize() == 0) {
    Serial.println("MPU6050DMP OK");
    mpu6050.setDMPEnabled(true); 
    packetSize = mpu6050.dmpGetFIFOPacketSize();
    dmpReady = true;    
  } else {
    Serial.println("MPU6050DMP NG");
  }
}

uint16_t fifoCount  = 0;
uint8_t  fifoBuffer[64];
unsigned long preTime = 0;

unsigned int loop_counter = 0;

#define WSIZE 16 // change this value for your purpose

long sum_x = 0;
long sum_y = 0;
long sum_z = 0;
int adj_x = 0;
int adj_y = 0;
int adj_z = 0;


void loop() {
  
  if (!dmpReady) return;
  
  while (!mpuInterrupt);
  mpuInterrupt = false;

  while (fifoCount < packetSize) {
    fifoCount = mpu6050.getFIFOCount();
  }
  
  uint8_t mpuIntStatus = mpu6050.getIntStatus();
  
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu6050.resetFIFO();
    return;
  } else if ((mpuIntStatus & 0x02) == false) {
    return;
  }
  
  mpu6050.getFIFOBytes(fifoBuffer, packetSize);
  fifoCount -= packetSize;

  unsigned long curTime = getCurrentTimeMS();
  unsigned long duration = curTime - preTime;

  Quaternion q;
  VectorInt16 aa;
  VectorInt16 aaReal;
  VectorFloat gravity;
  
  mpu6050.dmpGetQuaternion(&q, fifoBuffer);
  mpu6050.dmpGetAccel(&aa, fifoBuffer);
  mpu6050.dmpGetGravity(&gravity, &q);
  mpu6050.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  
  if (loop_counter < 2000) {
    if (loop_counter % 80 == 0)
      Serial.println("");
    ++loop_counter;
    Serial.print(".");
    return;
  } else if (loop_counter < 2000 + WSIZE) {
    sum_x += aaReal.x;
    sum_y += aaReal.y;
    sum_z += aaReal.z;
    if (loop_counter % 80 == 0)
      Serial.println("");
    ++loop_counter;
    Serial.print("*");
    return;
  } else {
    adj_x = sum_x / WSIZE;
    adj_y = sum_y / WSIZE;
    adj_z = sum_z / WSIZE;
    
    sum_x = sum_x - adj_x + aaReal.x;
    sum_y = sum_y - adj_y + aaReal.y;
    sum_z = sum_z - adj_z + aaReal.z;  
    
    /* 
    // High-pass values
    aaReal.x -= adj_x;
    aaReal.y -= adj_y;
    aaReal.z -= adj_z; 
    */   
  }
  
  Serial.print("areal\t");
  Serial.print(aaReal.x);
  Serial.print("\t");
  Serial.print(aaReal.y);
  Serial.print("\t");
  Serial.print(aaReal.z);
  Serial.print("\t");
  Serial.print(adj_x);
  Serial.print("\t");
  Serial.print(adj_y);
  Serial.print("\t");
  Serial.print(adj_z);
  Serial.print("\t");
  Serial.print(duration);
  Serial.println("");
  preTime = curTime;
}

アイディアは単純です。指定サンプル分の積算値を保持しておき、そこから平均値を出します。計算が終わったら、積算値から平均値を一つ分を減算して、直近の得られた値を積算値に加えるというものです。

このアルゴリズム適用した結果を以下に示します。赤がサンプル数分の平均値で、青がセンサーからの出力です。なかなか良い感じのデータが得られました。

16サンプル平均