BBBBB

Dependents:   measure_1_BBB measure_1_CCC measure_1_DDD

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU6050.cpp Source File

MPU6050.cpp

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