AAAAAAAAA

Dependents:   MPU6050_test_AAA

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,0x08);//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 
00065 bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v)
00066 {
00067     char data[2] = {addr_reg, v};
00068     return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0;
00069 }
00070 
00071 bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v)
00072 {
00073     char data = addr_reg;
00074     bool result = false;
00075     __disable_irq();
00076     if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) {
00077         *v = data;
00078         result = true;
00079     }
00080     __enable_irq();
00081     return result;
00082 }
00083 
00084 
00085 bool MPU6050::read_data(char sad, char sub, char *buf, int length)
00086 {
00087     if (length > 1) sub |= 0x80;
00088 
00089     return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0;
00090 }