BBBBB
Dependents: measure_1_BBB measure_1_CCC measure_1_DDD
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 }
Generated on Wed Jul 13 2022 03:07:56 by
1.7.2