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     //12月19日18:45時点：上のどっかで0が返されてしまっているらしい
00027     
00028     return 1;
00029 }
00030 
00031 int MPU6050::read() {
00032     char cmd[1] = {0x3b};
00033     int val;
00034     int ret = i2c->write(mpu_addr, cmd, 1, true);
00035     if(ret != 0) return 0;
00036     val = i2c->read(mpu_addr | 0x01, (char*)data.reg, 14, false);
00037     checker_set(val);
00038     // データのHとLが逆に読み込まれているのでスワップする
00039     for(int i=0; i<7; i++) {
00040         uint8_t temp = 0;
00041         temp = data.reg[i*2];
00042         data.reg[i*2] = data.reg[i*2+1];
00043         data.reg[i*2+1] = temp;
00044     }
00045     
00046     return 1;
00047     
00048 }