作業備忘録‎ > ‎

MPU6050

InvenSense社の6軸慣性センサ.3軸加速度センサ,3軸角速度センサをデジタルモーションプロセッサー(DMP: Digital Motion Processor)により統合して,姿勢角を出力する.

良いところ:
・姿勢角が出力される (出力フォーマットは四元数,オイラー角など)

微妙なところ:
・デジタルモーションプロセッサー(DMP: Digital Motion Processor)の処理が隠蔽されてること
・サンプリングレートが最大200Hzであること (センサの生データならもっと高速らしい)


サンプルプログラムを動作させた様子:


Arduinoとの接続                                                                                                                                                                              
パーツのコメント欄によると,各端子の接続先は次の通りとなっている. Great thanks, Jeff !!

VDD - Arduino 3.3v
GND - Arduino GND
INT - Arduino digital pin 2
FSYNC - leave unconnected
SCL - Arduino SCL (dedicated pin or Analog 5)
SDA - Arduino SDA (dedicated pin or Analog 4)
VIO - Arduino 3.3v
CLK - leave unconnected
ASCL - leave unconnected
ASDA - leave unconnected


サンプリング周波数の200Hz化                                                                                                                                                          
MPU6050用のArduinoサンプルスケッチでは,サンプリング周期が100Hz となっている.
パーツのコメント欄によると,200Hzにて動作させるためには,
の修正が必要とのこと.

-------------------------------------------------------------------------------------------------------------
    0x07,   0x6C,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_12 inv_send_accel -> inv_construct3_fifo
    0x02,   0x16,   0x02,   0x00, 0x01 → 0x00        // D_0_22 inv_set_fifo_rate

    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

    // It is important to make sure the host processor can keep up with reading and processing
    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
-------------------------------------------------------------------------------------------------------------
261行目の最終バイト(赤文字)が,
0x01 100Hz
0x00 200Hz
という設定を意味している.

この部分を0x00に書き換えることで,
Arduinoは,MPU6050から200Hzでデータを受け取ることができる.

しかし,これだけではうまくいかない.
ArduinoのI2Cの通信速度を確認しておく必要がある.デフォルトでは100kHzとなっている.
これを400kHzにしないと,ティーポットデモはうまく動作しない.
フォーラムによると,libraries\Wire\utilityフォルダの下にあるtwi.hを書き換えるといいらしい.
TWI_FREQを400000Lにすることで高速通信モードとなる.
twi.h ---------------------------------------------------------------------
#ifndef twi_h
#define twi_h

  #include <inttypes.h>

  //#define ATMEGA8

  #ifndef TWI_FREQ
  #define TWI_FREQ 100000L →400000L
  #endif

  #ifndef TWI_BUFFER_LENGTH
  #define TWI_BUFFER_LENGTH 32
  #endif
-----------------------------------------------------------------------------
生データの収集
サンプルプログラムのMPU6050_rawを参考にする.
サンプリング周波数の最大値は,加速度センサ1kHz,ジャイロセンサ8kHzが設定できる.
MPUのデジタルローパスフィルタに通す設定にすると,どちらのセンサも最大1kHzとなる.
測定範囲については,加速度センサが±2g,±4g,±8g,±16gで,ジャイロセンサは250°/s,500°/s,1000°/s,2000°/sが可能である.
アプリケーションとしては,ほぼ静止状態の姿勢を測るので,いずれも最小測定範囲を設定するとした.
センサデータについては,16bitのADCを通って出てくる.
しかし,加速度センサのほうはノイズ密度400 ug√Hzをお持ちなので果たしてどの程度の精度になるのか...

詳細な設定については,MPU6050.cppにも詳細が書かれているので,仕様書と合わせて確認するといい.
サンプルプログラムは文字列でデータを送信するものだったけれど,
より高速にサンプリングしたいのでバイナリ送信にしてみたプログラムを置いておきます.

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"


//===============================================
//  Defines
//===============================================

#define LED_PIN 13

//===============================================
//  Grobal Variables
//===============================================

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

// Packet
//        ;   Ax     Ay     Az     Gx     Gy      Gz          Time        Packet Count
// Size 17: [0][1] [2][3] [4][5] [6][7] [8][9] [10][11] [12][13][14][15]  [16]
//          [0][1] = [upper 8bit][lower 8bit]
//
// Data is transferd binary, not ascii code.
//
uint8_t Packet[17] = { 0,0, 0,0, 0,0, 0,0, 0,0, 0,0,  0,0, 0,0,  0x00};
//                     {ax} {ay} {az} {gx} {gy} {gz} {  time  } {count}


long dT = 1000;	     /* Loop Time [usec]    */
long now_us;        /* current time [usec] */
int state = 65;	     /* 'A': active (data is transfered), other acii code : rest (data is not transfered)*/
int mode  = 65;     /* 'A': 1ms mode,  'B': 2ms mode,  'C': 5ms mode */
bool test =false;
bool blinkState = false;

void setup() 
{
        // join I2C bus (I2Cdev library doesn't do this automatically)
        Wire.begin();
        
        //Serial.begin(115200);
        Serial.begin(230400);
    
        // initialize device
        accelgyro.initialize();
        accelgyro.setRate(0);        // sampling 1kHz
        accelgyro.setDLPFMode(1);    // DLPF 100Hz delay 2ms
    
        // verify connection
        Serial.println(accelgyro.testConnection() ? "success" : "failed");
    
        // configure Arduino LED for
        pinMode(LED_PIN, OUTPUT);
}

void loop() 
{
        // Get now time
        now_us = micros();

        // Get 
        if(readFromPC())
        {
            // Set Loop Time
	    selectMode();
        }
    
        // Do Process (To read sensor data and send it to PC)
        doProcess();

        // Wait for next time
        delayMicroseconds( max(now_us+dT-micros(), 0) );
}

void doProcess()
{
	if(state != 65)
	{
		// read raw accel/gyro measurements from device
                if(test)		
                    getDataTest();
		else
                    getData();
                    
		//Send data
	        sendData2PC();

		// blink LED to indicate activity
		BlinkLED();
	}
	else
	{
		InitPacketCount();
	}
}

void BlinkLED()
{
	blinkState = !blinkState;
	digitalWrite(LED_PIN, blinkState);
}


void selectMode()
{
	switch(mode)
	{
		case 'A':
			setMode1m();
			break;
		case 'B':
			setMode2m();
			break;
		case 'C':
			setMode5m();
			break;
                case 'T':
                        setTestMode();
                        break;
		default:
			break;
	}
}

void setTestMode()
{
        test = !test;
}

void InitPacketCount()
{
	Packet[16] = 0;
}

void setMode1m()
{
        //
        //  Need to set  Serial.begin(230400);
        //
	//Set 1ms
	dT = 996;
}


void setMode2m()
{
        //
        //  Need to set  Serial.begin(115200);
        //
        //Set 2ms
	dT = 1996;
}

void setMode5m()
{
	//Set 5ms
	dT = 4996;
}

bool readFromPC()
{
        if (Serial.available() > 1)
        {
              state = Serial.read();
              mode  = Serial.read();               
              return true;
        }
        return false;
}


void getDataTest()
{
        //set dammy
	uint8_t data[12];
	accelgyro.getMotion6B(data);

        for(int cnt = 0; cnt < 12; cnt++)
          Packet[cnt] = cnt;
        
        //set time
        Packet[12] = byte((now_us>>24) & 0xFF);  
        Packet[13] = byte((now_us>>16) & 0xFF);
        Packet[14] = byte((now_us>> 8) & 0xFF); 
        Packet[15] = byte((now_us    ) & 0xFF);
}

void getData()
{
        //get and set sensor data
	accelgyro.getMotion6B(Packet);
        //set time
        Packet[12] = byte((now_us>>24) & 0xFF);  
        Packet[13] = byte((now_us>>16) & 0xFF);
        Packet[14] = byte((now_us>> 8) & 0xFF); 
        Packet[15] = byte((now_us    ) & 0xFF);
}

void sendData2PC()
{
	Serial.write(Packet,17);
	Packet[16] ++;

	//Loop count
	if(Packet[16]==127)
	{
		Packet[16] = 0;
	}
}

Arduino-serial.writeの時間短縮                                                                                                                                                      

上のサンプルは,最速で1ms間隔で17byteのデータを送ろうとする.
必要なボーレートの見積りは(8+2)*17*1000 = 170kbpsに対して,115200kbpsの通信速度で十分なはずなんだけれど,送信時間がかかるようで遅い.
serial.writeがどうも時間を使っていて,サンプリング周期1msを守ることができない.
ここのフォーラムにも同じ悩みの人が質問している.解決法としては,単純にボーレートをより高くすることで解決する.
Arduinoのおすすめボーレート最大値は,115200kbpsである.これを230400kbpsにすると,serial.writeの時間が短縮され,とりあえず1msを守ることができる.


サンプルコードの実行結果の一例
count
図1:受信パケットのカウントの変化 (0から127の間でループ設定,Sampling freq.: 1kHz)


図2:繰り返し時間 (1009.6±4.2 [μs] , Sampling freq.: 1kHz)


オシロスコープ画像
     図3:PIN13の出力電圧の変化(Sampling freq. 1kHz)

図1から,高いボーレートを指定してもデータの受信漏れはないことが確認できる.
Arduino側でカウントした値が正常に読めている.また,そのカウント値は1カウントアップである.

図2及び図3から,十分な精度で1ms動作していることがわかる.

参考になるサイト:
 日本語

 英語
    ・FreeIMU
Comments