AAAAAAAAA
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Mon Jul 25 2022 19:09:30 by
1.7.2