モーションセンサ比較　- MPU6050 vs MPU9250 vs BNO055

モーションセンサが３種ほどそろったので基本動作確認で簡単に比較してみました。

MPU6050

６軸モーションセンサ (３軸加速度＋３軸ジャイロ)です。安定のHiLetgoで購入。

以下でArduinoライブラリとプログラム例が提供されています。
　https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

以下のライブラリも必要です。
　https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev

ここでは以下のプログラム例を用いてProcessingの3Dモデルを動かしてみます。
　https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050/examples/MPU6050_DMP6

構成

Arduinoプログラム

以下のプログラム例を修正します。本デバイスのDMP(Digital Motion Processor)という姿勢角を算出する機能を使用します。
　https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino

100行をコメントアウトして117行のコメントアウトを解除してセンサの姿勢角(クォータニオン)を出力するようにして書き込みます。

Processingプログラム

以下のプログラム例を使用します　
　https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde

71行で接続しているマイコンのポートを指定し実行ます。

動作

#MPS6050 オフセット未調整 #arduino #processing

Home Made Garbageさん(@homemadegarbage)がシェアした投稿 – 2017 5月 12 3:09午前 PDT

オフセット調整をしていないため姿勢角、特にYaw角が安定してません。。。

オフセット調整

MPU6050センサを平行に置いた状態でZ軸加速度が1ｇ(16384)、残りが0になるように調整します。

適切なオフセット値は以下のプログラムで調べることが出来ます。便利！
　https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050/examples/IMU_Zero

こちらを書き込んで実行すると数分でオフセット値が導出されます。

Initializing I2C devices...
Testing device connections...
MPU6050 connection successful
averaging 1000 readings each time
expanding:
....	XAccel			YAccel				ZAccel			XGyro			YGyro			ZGyro
 [0,0] --> [32203,32250]	[0,0] --> [14594,14613]	[0,0] --> [2606,2584]	[0,0] --> [-836,-837]	[0,0] --> [261,262]	[0,0] --> [-59,-60]
.... [-1000,0] --> [22757,32238]	[-1000,0] --> [6247,14588]	[0,1000] --> [2583,11059]	[0,1000] --> [-837,3146]	[-1000,0] --> [-3729,249]	[0,1000] --> [-60,3923]
.... [-2000,0] --> [13275,32233]	[-2000,0] --> [-2113,14593]	[0,2000] --> [2597,19563]	[0,1000] --> [-825,3153]	[-1000,0] --> [-3725,250]	[0,1000] --> [-48,3926]
.... [-3000,0] --> [3783,32229]	[-2000,0] --> [-2115,14599]	[0,2000] --> [2605,19569]	[0,1000] --> [-826,3153]	[-1000,0] --> [-3725,254]	[0,1000] --> [-49,3931]
.... [-4000,0] --> [-5688,32196]	[-2000,0] --> [-2102,14581]	[0,2000] --> [2618,19540]	[0,1000] --> [-825,3146]	[-1000,0] --> [-3729,249]	[0,1000] --> [-48,3923]

closing in:
..	XAccel			YAccel				ZAccel			XGyro			YGyro			ZGyro
 [-4000,-2000] --> [-5688,13276]	[-2000,-1000] --> [-2102,6252]	[1000,2000] --> [11084,19540]	[0,500] --> [-825,1166]	[-500,0] --> [-1731,249]	[0,500] --> [-48,1942]
.. [-4000,-3000] --> [-5688,3761]	[-2000,-1500] --> [-2102,2059]	[1500,2000] --> [15328,19540]	[0,250] --> [-825,165]	[-250,0] --> [-741,249]	[0,250] --> [-48,942]
.. [-3500,-3000] --> [-995,3761]	[-1750,-1500] --> [-32,2059]	[1500,1750] --> [15328,17458]	[125,250] --> [-336,165]	[-125,0] --> [-239,249]	[0,125] --> [-48,439]
.. [-3500,-3250] --> [-995,1373]	[-1750,-1625] --> [-32,999]	[1500,1625] --> [15328,16389]	[187,250] --> [-90,165]	[-125,-62] --> [-239,13]	[0,62] --> [-48,188]
.. [-3500,-3375] --> [-995,178]	[-1750,-1687] --> [-32,482]	[1562,1625] --> [15862,16389]	[187,218] --> [-90,33]	[-93,-62] --> [-109,13]	[0,31] --> [-48,63]
..	XAccel			YAccel				ZAccel			XGyro			YGyro			ZGyro
 [-3437,-3375] --> [-411,178]	[-1750,-1718] --> [-32,230]	[1593,1625] --> [16112,16389]	[202,218] --> [-29,33]	[-77,-62] --> [-45,13]	[15,31] --> [-1,63]
.. [-3406,-3375] --> [-106,178]	[-1750,-1734] --> [-32,96]	[1609,1625] --> [16252,16389]	[202,210] --> [-29,2]	[-69,-62] --> [-14,13]	[15,23] --> [-1,31]
.. [-3406,-3390] --> [-106,43]	[-1750,-1742] --> [-32,31]	[1617,1625] --> [16319,16389]	[206,210] --> [-13,2]	[-69,-65] --> [-14,1]	[15,19] --> [-1,15]
.. [-3398,-3390] --> [-29,43]	[-1746,-1742] --> [0,31]	[1621,1625] --> [16355,16389]	[208,210] --> [-6,2]	[-67,-65] --> [-6,1]	[15,17] --> [-1,7]
.. [-3398,-3394] --> [-29,7]	[-1746,-1744] --> [0,5]	[1623,1625] --> [16372,16389]	[209,210] --> [-1,2]	[-66,-65] --> [-2,1]	[15,16] --> [-1,3]
averaging 10000 readings each time
....................	XAccel			YAccel				ZAccel			XGyro			YGyro			ZGyro
 [-3396,-3394] --> [-10,7]	[-1745,-1744] --> [-14,5]	[1624,1625] --> [16380,16389]	[209,210] --> [-2,2]	[-66,-65] --> [-2,1]	[15,16] --> [0,3]
.................... [-3395,-3394] --> [-9,7]	[-1745,-1744] --> [-15,5]	[1624,1624] --> [16380,16386]	[209,210] --> [-2,2]	[-66,-65] --> [-2,1]	[15,16] --> [0,3]
.................... [-3395,-3394] --> [-9,7]	[-1745,-1744] --> [-15,5]	[1624,1624] --> [16380,16385]	[209,210] --> [-2,2]	[-66,-65] --> [-3,1]	[15,16] --> [-1,3]
-------------- done --------------

Initializing I2C devices...

Testing device connections...

MPU6050 connection successful

averaging 1000 readings each time

expanding:

.... XAccel YAccel ZAccel XGyro YGyro ZGyro

[0,0] --> [32203,32250] [0,0] --> [14594,14613] [0,0] --> [2606,2584] [0,0] --> [-836,-837] [0,0] --> [261,262] [0,0] --> [-59,-60]

.... [-1000,0] --> [22757,32238] [-1000,0] --> [6247,14588] [0,1000] --> [2583,11059] [0,1000] --> [-837,3146] [-1000,0] --> [-3729,249] [0,1000] --> [-60,3923]

.... [-2000,0] --> [13275,32233] [-2000,0] --> [-2113,14593] [0,2000] --> [2597,19563] [0,1000] --> [-825,3153] [-1000,0] --> [-3725,250] [0,1000] --> [-48,3926]

.... [-3000,0] --> [3783,32229] [-2000,0] --> [-2115,14599] [0,2000] --> [2605,19569] [0,1000] --> [-826,3153] [-1000,0] --> [-3725,254] [0,1000] --> [-49,3931]

.... [-4000,0] --> [-5688,32196] [-2000,0] --> [-2102,14581] [0,2000] --> [2618,19540] [0,1000] --> [-825,3146] [-1000,0] --> [-3729,249] [0,1000] --> [-48,3923]

closing in:

.. XAccel YAccel ZAccel XGyro YGyro ZGyro

[-4000,-2000] --> [-5688,13276] [-2000,-1000] --> [-2102,6252] [1000,2000] --> [11084,19540] [0,500] --> [-825,1166] [-500,0] --> [-1731,249] [0,500] --> [-48,1942]

.. [-4000,-3000] --> [-5688,3761] [-2000,-1500] --> [-2102,2059] [1500,2000] --> [15328,19540] [0,250] --> [-825,165] [-250,0] --> [-741,249] [0,250] --> [-48,942]

.. [-3500,-3000] --> [-995,3761] [-1750,-1500] --> [-32,2059] [1500,1750] --> [15328,17458] [125,250] --> [-336,165] [-125,0] --> [-239,249] [0,125] --> [-48,439]

.. [-3500,-3250] --> [-995,1373] [-1750,-1625] --> [-32,999] [1500,1625] --> [15328,16389] [187,250] --> [-90,165] [-125,-62] --> [-239,13] [0,62] --> [-48,188]

.. [-3500,-3375] --> [-995,178] [-1750,-1687] --> [-32,482] [1562,1625] --> [15862,16389] [187,218] --> [-90,33] [-93,-62] --> [-109,13] [0,31] --> [-48,63]

.. XAccel YAccel ZAccel XGyro YGyro ZGyro

[-3437,-3375] --> [-411,178] [-1750,-1718] --> [-32,230] [1593,1625] --> [16112,16389] [202,218] --> [-29,33] [-77,-62] --> [-45,13] [15,31] --> [-1,63]

.. [-3406,-3375] --> [-106,178] [-1750,-1734] --> [-32,96] [1609,1625] --> [16252,16389] [202,210] --> [-29,2] [-69,-62] --> [-14,13] [15,23] --> [-1,31]

.. [-3406,-3390] --> [-106,43] [-1750,-1742] --> [-32,31] [1617,1625] --> [16319,16389] [206,210] --> [-13,2] [-69,-65] --> [-14,1] [15,19] --> [-1,15]

.. [-3398,-3390] --> [-29,43] [-1746,-1742] --> [0,31] [1621,1625] --> [16355,16389] [208,210] --> [-6,2] [-67,-65] --> [-6,1] [15,17] --> [-1,7]

.. [-3398,-3394] --> [-29,7] [-1746,-1744] --> [0,5] [1623,1625] --> [16372,16389] [209,210] --> [-1,2] [-66,-65] --> [-2,1] [15,16] --> [-1,3]

averaging 10000 readings each time

.................... XAccel YAccel ZAccel XGyro YGyro ZGyro

[-3396,-3394] --> [-10,7] [-1745,-1744] --> [-14,5] [1624,1625] --> [16380,16389] [209,210] --> [-2,2] [-66,-65] --> [-2,1] [15,16] --> [0,3]

.................... [-3395,-3394] --> [-9,7] [-1745,-1744] --> [-15,5] [1624,1624] --> [16380,16386] [209,210] --> [-2,2] [-66,-65] --> [-2,1] [15,16] --> [0,3]

.................... [-3395,-3394] --> [-9,7] [-1745,-1744] --> [-15,5] [1624,1624] --> [16380,16385] [209,210] --> [-2,2] [-66,-65] --> [-3,1] [15,16] --> [-1,3]

-------------- done --------------

得られたオフセットをもとに以下の
　https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino

203〜206行を得られたオフセット値である以下に修正して書き込み、Processingを実行します。

mpu.setXAccelOffset(-3394);
mpu.setYAccelOffset(-1744);
mpu.setZAccelOffset(1624);
mpu.setXGyroOffset(210);
mpu.setYGyroOffset(-65);
mpu.setZGyroOffset(15);

mpu.setXAccelOffset(-3394);

mpu.setYAccelOffset(-1744);

mpu.setZAccelOffset(1624);

mpu.setXGyroOffset(210);

mpu.setYGyroOffset(-65);

mpu.setZGyroOffset(15);

動作(オフセット調整版)

#MPS6050 オフセット調整 #arduino #processing

Home Made Garbageさん(@homemadegarbage)がシェアした投稿 – 2017 5月 12 3:12午前 PDT

調整により安定してセンサの姿勢が取得出来ているのがわかります。但しYaw角の初期値は絶対的なものではありません。

MPU9250

6軸センサMPU6500(３軸加速度＋３軸ジャイロ)と３軸コンパスAK8963を搭載した9軸モーションセンサです。こちらもHiLetgoで購入 🙂

結論から言うと。。。うまく行きませんでした。。。MPU6050のようにDMPを使用したいい感じのプログラムがなかったので。。。

しかたないのでSparkfunによるMPU9150モジュール用の以下のプログラムを元に9軸センサから姿勢角(クォータニオン)を取得しMPU6050同様Processingのモデルを動かして見ました。(MPU9250はMPU9150を低消費電力化した上位機種とのこと)
　https://github.com/sparkfun/MPU-9150_Breakout/blob/master/firmware/MPU6050/Examples/MPU9150_AHRS.ino

AHRS(エーハース)[ Attitude Heading Reference System]という電子コンパスで絶対Yaw角(方位)も出力する方式のプログラムのようです。またクォータニオン生成アルゴリズムを2種から選択できます。

以下のライブラリを使用します。
　https://github.com/sparkfun/MPU-9150_Breakout/tree/master/firmware

構成

Arduinoプログラム

改造プログラム

#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_9Axis_MotionApps41.h"

// Declare device MPU6050 class
MPU6050 mpu;

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
#define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f)      // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
#define beta sqrt(3.0f / 4.0f) * GyroMeasError   // compute beta
#define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift   // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f

int16_t a1, a2, a3, g1, g2, g3, m1, m2, m3;     // raw data arrays reading
uint16_t mcount = 0; // used to control display output rate
uint8_t MagRate;     // read rate for magnetometer data

float pitch, yaw, roll;
float deltat = 0.0f;        // integration interval for both filter schemes
uint16_t lastUpdate = 0; // used to calculate integration interval
uint16_t now = 0;        // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

unsigned int qw, qx, qy, qz;

void setup() {
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
  #endif

  Serial.begin(115200); // Start serial at 38400 bps

  // initialize MPU6050 device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  
  mpu.setXAccelOffset(-5393);
  mpu.setYAccelOffset(-6001);
  mpu.setZAccelOffset(9927);
  mpu.setXGyroOffset(-12);
  mpu.setYGyroOffset(-51);
  mpu.setZGyroOffset(-30);
  
  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU9150 connection successful") : F("MPU9150 connection failed"));

  // Set up the accelerometer, gyro, and magnetometer for data output
  mpu.setRate(7); // set gyro rate to 8 kHz/(1 * rate) shows 1 kHz, accelerometer ODR is fixed at 1 KHz
  
  MagRate = 100; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
  
  mpu.setDLPFMode(4); // set bandwidth of both gyro and accelerometer to ~20 Hz
  mpu.setFullScaleGyroRange(0); // set gyro range to 250 degrees/sec
  mpu.setFullScaleAccelRange(0); // set accelerometer to 2 g range

  mpu.setIntDataReadyEnabled(true); // enable data ready interrupt
}

void loop(){
  if(mpu.getIntDataReadyStatus() == 1) { // wait for data ready status register to update all data registers
    mcount++;
    // read the raw sensor data
    mpu.getAcceleration  ( &a1, &a2, &a3  );
    ax = a1*2.0f/32768.0f; // 2 g full range for accelerometer
    ay = a2*2.0f/32768.0f;
    az = a3*2.0f/32768.0f;

    mpu.getRotation  ( &g1, &g2, &g3  );
    gx = g1*250.0f/32768.0f; // 250 deg/s full range for gyroscope
    gy = g2*250.0f/32768.0f;
    gz = g3*250.0f/32768.0f;

    if (mcount > 1000/MagRate) {  // this is a poor man's way of setting the magnetometer read rate (see below) 
      mpu.getMag  ( &m1, &m2, &m3 );
      mx = m1*10.0f*1229.0f/4096.0f + 18.0f; // milliGauss (1229 microTesla per 2^12 bits, 10 mG per microTesla)
      my = m2*10.0f*1229.0f/4096.0f + 70.0f; // apply calibration offsets in mG that correspond to your environment and magnetometer
      mz = m3*10.0f*1229.0f/4096.0f + 270.0f;
      mcount = 0;
    }           
  }
   
  now = micros();
  deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
  lastUpdate = now;

  //クォータニオン生成アルゴリズム2種
  //MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

  /*
  Serial.print("q0 = "); Serial.print(q[0]);
  Serial.print(" qx = "); Serial.print(q[1]); 
  Serial.print(" qy = "); Serial.print(q[2]); 
  Serial.print(" qz = "); Serial.println(q[3]);
  */

  qw =int(q[0] * 16384.0f);
  qx =int(q[1] * 16384.0f);
  qy =int(q[2] * 16384.0f);
  qz =int(q[3] * 16384.0f);

  // display quaternion values in InvenSense Teapot demo format:
  teapotPacket[2] = highByte(qw);
  teapotPacket[3] = lowByte(qw);
  teapotPacket[4] = highByte(qx);
  teapotPacket[5] = lowByte(qx);
  teapotPacket[6] = highByte(qy);
  teapotPacket[7] = lowByte(qy);
  teapotPacket[8] = highByte(qz);
  teapotPacket[9] = lowByte(qz);
  Serial.write(teapotPacket, 14);
  teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
}

以下 クォータニオンアルゴリズム 略

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

#include <Wire.h>

#include "I2Cdev.h"

#include "MPU6050_9Axis_MotionApps41.h"

// Declare device MPU6050 class

MPU6050 mpu;

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)

#define GyroMeasError PI * (40.0f / 180.0f) // gyroscope measurement error in rads/s (shown as 3 deg/s)

#define GyroMeasDrift PI * (0.0f / 180.0f) // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)

#define beta sqrt(3.0f / 4.0f) * GyroMeasError // compute beta

#define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value

#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral

#define Ki 0.0f

int16_t a1, a2, a3, g1, g2, g3, m1, m2, m3; // raw data arrays reading

uint16_t mcount = 0; // used to control display output rate

uint8_t MagRate; // read rate for magnetometer data

float pitch, yaw, roll;

float deltat = 0.0f; // integration interval for both filter schemes

uint16_t lastUpdate = 0; // used to calculate integration interval

uint16_t now = 0; // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values

float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion

float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method

// packet structure for InvenSense teapot demo

uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

unsigned int qw, qx, qy, qz;

void setup() {

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin();

Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400, true);

#endif

Serial.begin(115200); // Start serial at 38400 bps

// initialize MPU6050 device

Serial.println(F("Initializing I2C devices..."));

mpu.initialize();

mpu.setXAccelOffset(-5393);

mpu.setYAccelOffset(-6001);

mpu.setZAccelOffset(9927);

mpu.setXGyroOffset(-12);

mpu.setYGyroOffset(-51);

mpu.setZGyroOffset(-30);

// verify connection

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU9150 connection successful") : F("MPU9150 connection failed"));

// Set up the accelerometer, gyro, and magnetometer for data output

mpu.setRate(7); // set gyro rate to 8 kHz/(1 * rate) shows 1 kHz, accelerometer ODR is fixed at 1 KHz

MagRate = 100; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values

mpu.setDLPFMode(4); // set bandwidth of both gyro and accelerometer to ~20 Hz

mpu.setFullScaleGyroRange(0); // set gyro range to 250 degrees/sec

mpu.setFullScaleAccelRange(0); // set accelerometer to 2 g range

mpu.setIntDataReadyEnabled(true); // enable data ready interrupt

}

void loop(){

if(mpu.getIntDataReadyStatus() == 1) { // wait for data ready status register to update all data registers

mcount++;

// read the raw sensor data

mpu.getAcceleration ( &a1, &a2, &a3 );

ax = a1*2.0f/32768.0f; // 2 g full range for accelerometer

ay = a2*2.0f/32768.0f;

az = a3*2.0f/32768.0f;

mpu.getRotation ( &g1, &g2, &g3 );

gx = g1*250.0f/32768.0f; // 250 deg/s full range for gyroscope

gy = g2*250.0f/32768.0f;

gz = g3*250.0f/32768.0f;

if (mcount > 1000/MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below)

mpu.getMag ( &m1, &m2, &m3 );

mx = m1*10.0f*1229.0f/4096.0f + 18.0f; // milliGauss (1229 microTesla per 2^12 bits, 10 mG per microTesla)

my = m2*10.0f*1229.0f/4096.0f + 70.0f; // apply calibration offsets in mG that correspond to your environment and magnetometer

mz = m3*10.0f*1229.0f/4096.0f + 270.0f;

mcount = 0;

}

now = micros();

deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update

lastUpdate = now;

//クォータニオン生成アルゴリズム2種

//MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

Serial.print("q0 = "); Serial.print(q[0]);

Serial.print(" qx = "); Serial.print(q[1]);

Serial.print(" qy = "); Serial.print(q[2]);

Serial.print(" qz = "); Serial.println(q[3]);

qw =int(q[0] * 16384.0f);

qx =int(q[1] * 16384.0f);

qy =int(q[2] * 16384.0f);

qz =int(q[3] * 16384.0f);

// display quaternion values in InvenSense Teapot demo format:

teapotPacket[2] = highByte(qw);

teapotPacket[3] = lowByte(qw);

teapotPacket[4] = highByte(qx);

teapotPacket[5] = lowByte(qx);

teapotPacket[6] = highByte(qy);

teapotPacket[7] = lowByte(qy);

teapotPacket[8] = highByte(qz);

teapotPacket[9] = lowByte(qz);

Serial.write(teapotPacket, 14);

teapotPacket[11]++; // packetCount, loops at 0xFF on purpose

}

以下クォータニオンアルゴリズム略

Processingプログラム

MPU6050と同様です。

動作

#MPU9250 #arduino #processing

Home Made Garbageさん(@homemadegarbage)がシェアした投稿 – 2017 5月 16 6:46午前 PDT

う〜んやっぱりだめだー、電子コンパスでYaw角補正してるような動きはしてるけどなんかオカシイ。。。

参考

DMP関連と思われるプログラム。。力尽きたけど今度勉強してみます。。

あとオフセットの調整ができなかったのだけど、MPU6050とはレジスタの値が違うようです。以下を参考に修正しオフセット補正実現できましたがいい感じの姿勢角は得れませんでした。。。。

http://qiita.com/7of9/items/d3c765912b435815cfdf

追記（2018/11/15）

Adafruit Itsy Bitsy M0 ExpressというマイコンでMPU9250 のDMP動作確認できました。よろしければご参照ください。

モーションセンサ比較　- MPU6050 vs MPU9250 vs BNO055 –

BNO055

9軸モーションセンサ (３軸加速度＋３軸ジャイロ＋３軸コンパス)。以前Adafruitで購入したものです。

Adafruit 9-DOF Absolute Orientation IMU Fusion Breakout - BNO055

www.adafruit.com

国内なら以下で手に入ります。

[商品価格に関しましては、リンクが作成された時点と現時点で情報が変更されている場合がございます。]

BNO055 9DOF絶対方位IMUモジュール
価格：3526円（税込、送料別) (2018/10/11時点)

楽天で購入

　
詳細は以下の通り

“キング・オブ・ダークネス” EVIL の鎌を自作

動作

#adafruit #bno055 https://homemadegarbage.com/bno055-01

Home Made Garbageさん(@homemadegarbage)がシェアした投稿 – 2017 2月 25 11:59午後 PST

オフセット調整必要なく安定した姿勢角を出力してくれる。

中身よくわからないけど、Yaw角もうまいこと３軸コンパスとか用いて姿勢角補正してくれてるんだと思う。。。

総括

正確な姿勢角を得たいのであれば高いけどBNO055がよさそう。。。でも簡単なセンサとしてはMPU6050かなぁ安いし。。。MPU9250くやしいなぁ(T_T)

モーションセンサで作ったモノ

測距センサ VL53L0X を用いた身長測定器

RedBear Duo セットアップ方法 for Arduino IDE

スマート靴占い『 IしたoTんきになぁ〜れ』

“キング・オブ・ダークネス”EVIL ハンドをハック