MPU6050.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 
00004 #define ADDRESS 0xD1
00005 
00006 MPU6050::MPU6050(PinName sda, PinName scl):
00007     _MPU6050(sda, scl)
00008 {
00009     _MPU6050.frequency(400000);
00010 }
00011 
00012 void MPU6050::start(void)
00013 {
00014     write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);//disable sleep mode
00015     write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s
00016     write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G
00017     write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 470Hz
00018     //to set more, see MPU6000 Register Map
00019 }
00020 
00021 char MPU6050::getID(void)
00022 {
00023     char devID;
00024     read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID);
00025     return devID;
00026 }
00027 
00028 bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az)
00029 {
00030     char data[6];
00031     char data2[6];
00032     if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
00033         read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
00034         *gx = float(short(data[0] << 8 | data[1]))*0.0305f;
00035         *gy =  float(short(data[2] << 8 | data[3]))*0.0305f;
00036         *gz =  float(short(data[4] << 8 | data[5]))*0.0305f;
00037         *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f;
00038         *ay =  float(short(data2[2] << 8 | data2[3]))*0.000122f;
00039         *az =  float(short(data2[4] << 8 | data2[5]))*0.000122f;
00040         return true;
00041     }
00042     return false;
00043 }
00044 
00045 bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az)
00046 {
00047     char data[6];
00048     char data2[6];
00049     if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
00050         read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
00051         *gx = int(short(data[0] << 8 | data[1]));
00052         *gy =  int(short(data[2] << 8 | data[3]));
00053         *gz =  int(short(data[4] << 8 | data[5]));
00054         *ax = int(short(data2[0] << 8 | data2[1]));
00055         *ay =  int(short(data2[2] << 8 | data2[3]));
00056         *az =  int(short(data2[4] << 8 | data2[5]));
00057         return true;
00058     }
00059     return false;
00060 }
00061 
00062 bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v)
00063 {
00064     char data[2] = {addr_reg, v};
00065     return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0;
00066 }
00067 
00068 bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v)
00069 {
00070     char data = addr_reg;
00071     bool result = false;
00072     if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) {
00073         *v = data;
00074         result = true;
00075     }
00076     return result;
00077 }
00078 
00079 
00080 bool MPU6050::read_data(char sad, char sub, char *buf, int length)
00081 {
00082     if (length > 1) sub |= 0x80;
00083 
00084     return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0;
00085 }