LAURUS / Mbed 2 deprecated Laurus_acc_gyro

read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle

Dependencies:   mbed

MPU6050.cpp

Committer:
ojan
Date:
2015-05-13
Revision:
3:40559ebef0f1

File content as of revision 3:40559ebef0f1:

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

MPU6050::MPU6050(I2C* i2c) {
    this->i2c = 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;
    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;
    
}