LAURUS / Mbed 2 deprecated Laurus_acc_gyro

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

Dependencies:   mbed

MPU6050.cpp@3:40559ebef0f1, 2015-05-13 (annotated)

Committer:
ojan
Date:
Wed May 13 04:02:27 2015 +0000
Revision:
3:40559ebef0f1
estimate gravity vector & geomagnetic vector

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 3:40559ebef0f1 1 #include "mbed.h"
ojan 3:40559ebef0f1 2 #include "myConstants.h"
ojan 3:40559ebef0f1 3 #include "MPU6050.h"
ojan 3:40559ebef0f1 4
ojan 3:40559ebef0f1 5 MPU6050::MPU6050(I2C* i2c) {
ojan 3:40559ebef0f1 6 this->i2c = i2c;
ojan 3:40559ebef0f1 7 }
ojan 3:40559ebef0f1 8
ojan 3:40559ebef0f1 9 MPU6050::~MPU6050() {
ojan 3:40559ebef0f1 10 i2c = NULL;
ojan 3:40559ebef0f1 11 }
ojan 3:40559ebef0f1 12
ojan 3:40559ebef0f1 13 int MPU6050::init() {
ojan 3:40559ebef0f1 14 char cmd[2] = {0x6b, 0x00};
ojan 3:40559ebef0f1 15 int ret = i2c->write(mpu_addr, cmd, 2);
ojan 3:40559ebef0f1 16
ojan 3:40559ebef0f1 17 if(ret != 0) return 0;
ojan 3:40559ebef0f1 18 return 1;
ojan 3:40559ebef0f1 19 }
ojan 3:40559ebef0f1 20
ojan 3:40559ebef0f1 21 int MPU6050::read() {
ojan 3:40559ebef0f1 22 char cmd[1] = {0x3b};
ojan 3:40559ebef0f1 23 int ret = i2c->write(mpu_addr, cmd, 1, true);
ojan 3:40559ebef0f1 24 if(ret != 0) return 0;
ojan 3:40559ebef0f1 25
ojan 3:40559ebef0f1 26 i2c->read(mpu_addr | 0x01, (char*)data.reg, 14, false);
ojan 3:40559ebef0f1 27
ojan 3:40559ebef0f1 28 // データのHとLが逆に読み込まれているのでスワップする
ojan 3:40559ebef0f1 29 for(int i=0; i<7; i++) {
ojan 3:40559ebef0f1 30 uint8_t temp = 0;
ojan 3:40559ebef0f1 31 temp = data.reg[i*2];
ojan 3:40559ebef0f1 32 data.reg[i*2] = data.reg[i*2+1];
ojan 3:40559ebef0f1 33 data.reg[i*2+1] = temp;
ojan 3:40559ebef0f1 34 }
ojan 3:40559ebef0f1 35
ojan 3:40559ebef0f1 36 return 1;
ojan 3:40559ebef0f1 37
ojan 3:40559ebef0f1 38 }