MPU6050.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 
00004 MPU6050::MPU6050(I2C* p_i2c): i2c(p_i2c){
00005 }
00006 
00007 MPU6050::~MPU6050() {
00008     i2c = NULL;
00009 }
00010 
00011 int MPU6050::init() {
00012     // スリープモード解除
00013     char cmd[2] = {0x6b, 0x00};
00014     int ret = i2c->write(mpu_addr, cmd, 2);
00015     if(ret != 0) return 0;
00016     
00017     // ジャイロのレンジを500deg/sに設定
00018     char data = 0;
00019     cmd[0] = 0x1b;
00020     ret = i2c->write(mpu_addr, cmd, 1, true);
00021     if(ret != 0) return 0;
00022     i2c->read(mpu_addr | 0x01, &data, 1, false);
00023     cmd[1] = data | 0x08;
00024     ret = i2c->write(mpu_addr, cmd, 2);
00025     if(ret != 0) return 0;
00026     
00027     return 1;
00028 }
00029 
00030 int MPU6050::read() {
00031     char cmd[1] = {0x3b};
00032     int ret = i2c->write(mpu_addr, cmd, 1, true);
00033     if(ret != 0) return 0;
00034     
00035     i2c->read(mpu_addr | 0x01, (char*)data.reg, 14, false);
00036     
00037     // データのHとLが逆に読み込まれているのでスワップする
00038     for(int i=0; i<7; i++) {
00039         uint8_t temp = 0;
00040         temp = data.reg[i*2];
00041         data.reg[i*2] = data.reg[i*2+1];
00042         data.reg[i*2+1] = temp;
00043     }
00044     
00045     return 1;
00046     
00047 }