File content as of revision 1:6cd6d2760856:

#include "mbed.h"
#include "MPU6050.h"

MPU6050::MPU6050(I2C* p_i2c): i2c(p_i2c){
}

MPU6050::~MPU6050() {
    i2c = NULL;
}

int MPU6050::init() {
    // スリープモード解除
    char cmd[2] = {0x6b, 0x00};
    int ret = i2c->write(mpu_addr, cmd, 2);
    if(ret != 0) return 0;
    
    // ジャイロのレンジを500deg/sに設定
    char data = 0;
    cmd[0] = 0x1b;
    ret = i2c->write(mpu_addr, cmd, 1, true);
    if(ret != 0) return 0;
    i2c->read(mpu_addr | 0x01, &data, 1, false);
    cmd[1] = data | 0x08;
    ret = i2c->write(mpu_addr, cmd, 2);
    if(ret != 0) return 0;
    
    return 1;
}

int MPU6050::read() {
    char cmd[1] = {0x3b};
    int ret = i2c->write(mpu_addr, cmd, 1, true);
    if(ret != 0) return 0;
    
    i2c->read(mpu_addr | 0x01, (char*)data.reg, 14, false);
    
    // データのHとLが逆に読み込まれているのでスワップする
    for(int i=0; i<7; i++) {
        uint8_t temp = 0;
        temp = data.reg[i*2];
        data.reg[i*2] = data.reg[i*2+1];
        data.reg[i*2+1] = temp;
    }
    
    return 1;
    
}