fork
MPU6050.cpp@4:2d0953219845, 2017-08-18 (annotated)
- Committer:
- dima285
- Date:
- Fri Aug 18 08:53:05 2017 +0000
- Revision:
- 4:2d0953219845
- Parent:
- 3:48773a5c8d48
- Child:
- 5:f806657f0009
hfdfjlkljjhgfgrtftgmed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ritarosakai | 0:6d1b9bcd64ca | 1 | #include "mbed.h" |
ritarosakai | 0:6d1b9bcd64ca | 2 | #include "MPU6050.h" |
ritarosakai | 0:6d1b9bcd64ca | 3 | |
Stas285 | 2:c68be621f2f1 | 4 | #define ADDRESS 0xD1 //68 shifted left: 7-bit to 8-bit address conversion |
ritarosakai | 0:6d1b9bcd64ca | 5 | |
ritarosakai | 0:6d1b9bcd64ca | 6 | MPU6050::MPU6050(PinName sda, PinName scl): |
ritarosakai | 0:6d1b9bcd64ca | 7 | _MPU6050(sda, scl) |
ritarosakai | 0:6d1b9bcd64ca | 8 | { |
ritarosakai | 0:6d1b9bcd64ca | 9 | _MPU6050.frequency(400000); |
ritarosakai | 0:6d1b9bcd64ca | 10 | } |
ritarosakai | 0:6d1b9bcd64ca | 11 | |
ritarosakai | 0:6d1b9bcd64ca | 12 | void MPU6050::start(void) |
ritarosakai | 0:6d1b9bcd64ca | 13 | { |
ritarosakai | 0:6d1b9bcd64ca | 14 | write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00); |
Stas285 | 2:c68be621f2f1 | 15 | write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x08);//gyro +-500deg/s //0x10);//gyro +-1000deg/s |
Stas285 | 2:c68be621f2f1 | 16 | write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x00);//accel +-2G //0x08);//accel +-4G |
dima285 | 4:2d0953219845 | 17 | write_reg(ADDRESS,MPU6050_CONFIG,0x02);//2 - bandwidth 94Hz, delay 3 mS; 3 - bandwidth 44Hz, delay 5 mS; 4 - bandwidth 21Hz, delay 8.5 mS; 5 - bandwidth 10Hz, delay 14 mS |
dima285 | 4:2d0953219845 | 18 | write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x0);//sample rate 1000Hz (1000/5) |
ritarosakai | 0:6d1b9bcd64ca | 19 | } |
ritarosakai | 0:6d1b9bcd64ca | 20 | |
ritarosakai | 0:6d1b9bcd64ca | 21 | char MPU6050::getID(void) |
ritarosakai | 0:6d1b9bcd64ca | 22 | { |
ritarosakai | 0:6d1b9bcd64ca | 23 | char devID; |
ritarosakai | 0:6d1b9bcd64ca | 24 | read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID); |
ritarosakai | 0:6d1b9bcd64ca | 25 | return devID; |
ritarosakai | 0:6d1b9bcd64ca | 26 | } |
ritarosakai | 0:6d1b9bcd64ca | 27 | |
ritarosakai | 0:6d1b9bcd64ca | 28 | bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az) |
ritarosakai | 0:6d1b9bcd64ca | 29 | { |
ritarosakai | 0:6d1b9bcd64ca | 30 | char data[6]; |
ritarosakai | 0:6d1b9bcd64ca | 31 | char data2[6]; |
ritarosakai | 0:6d1b9bcd64ca | 32 | if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { |
ritarosakai | 0:6d1b9bcd64ca | 33 | read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); |
Stas285 | 2:c68be621f2f1 | 34 | *gx = float(short(data[0] << 8 | data[1]))*1.526e-2; //deg/s |
Stas285 | 2:c68be621f2f1 | 35 | *gy = float(short(data[2] << 8 | data[3]))*1.526e-2; |
Stas285 | 2:c68be621f2f1 | 36 | *gz = float(short(data[4] << 8 | data[5]))*1.526e-2; |
dima285 | 4:2d0953219845 | 37 | *ax = float(short(data2[0] << 8 | data2[1]))*5.9875e-4; //cm/s^2 |
Stas285 | 2:c68be621f2f1 | 38 | *ay = float(short(data2[2] << 8 | data2[3]))*5.9875e-4; |
Stas285 | 2:c68be621f2f1 | 39 | *az = float(short(data2[4] << 8 | data2[5]))*5.9875e-4; |
ritarosakai | 1:cf3a9ec7205e | 40 | return true; |
ritarosakai | 1:cf3a9ec7205e | 41 | } |
ritarosakai | 1:cf3a9ec7205e | 42 | return false; |
ritarosakai | 1:cf3a9ec7205e | 43 | } |
ritarosakai | 1:cf3a9ec7205e | 44 | |
ritarosakai | 1:cf3a9ec7205e | 45 | bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az) |
ritarosakai | 1:cf3a9ec7205e | 46 | { |
ritarosakai | 1:cf3a9ec7205e | 47 | char data[6]; |
ritarosakai | 1:cf3a9ec7205e | 48 | char data2[6]; |
ritarosakai | 1:cf3a9ec7205e | 49 | if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { |
ritarosakai | 1:cf3a9ec7205e | 50 | read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); |
ritarosakai | 1:cf3a9ec7205e | 51 | *gx = int(short(data[0] << 8 | data[1])); |
ritarosakai | 1:cf3a9ec7205e | 52 | *gy = int(short(data[2] << 8 | data[3])); |
ritarosakai | 1:cf3a9ec7205e | 53 | *gz = int(short(data[4] << 8 | data[5])); |
ritarosakai | 1:cf3a9ec7205e | 54 | *ax = int(short(data2[0] << 8 | data2[1])); |
ritarosakai | 1:cf3a9ec7205e | 55 | *ay = int(short(data2[2] << 8 | data2[3])); |
ritarosakai | 1:cf3a9ec7205e | 56 | *az = int(short(data2[4] << 8 | data2[5])); |
ritarosakai | 0:6d1b9bcd64ca | 57 | return true; |
ritarosakai | 0:6d1b9bcd64ca | 58 | } |
ritarosakai | 0:6d1b9bcd64ca | 59 | return false; |
ritarosakai | 0:6d1b9bcd64ca | 60 | } |
ritarosakai | 0:6d1b9bcd64ca | 61 | |
ritarosakai | 0:6d1b9bcd64ca | 62 | bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v) |
ritarosakai | 0:6d1b9bcd64ca | 63 | { |
ritarosakai | 0:6d1b9bcd64ca | 64 | char data[2] = {addr_reg, v}; |
ritarosakai | 0:6d1b9bcd64ca | 65 | return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 66 | } |
ritarosakai | 0:6d1b9bcd64ca | 67 | |
ritarosakai | 0:6d1b9bcd64ca | 68 | bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v) |
ritarosakai | 0:6d1b9bcd64ca | 69 | { |
ritarosakai | 0:6d1b9bcd64ca | 70 | char data = addr_reg; |
ritarosakai | 0:6d1b9bcd64ca | 71 | bool result = false; |
Stas285 | 2:c68be621f2f1 | 72 | // __disable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 73 | if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) { |
ritarosakai | 0:6d1b9bcd64ca | 74 | *v = data; |
ritarosakai | 0:6d1b9bcd64ca | 75 | result = true; |
ritarosakai | 0:6d1b9bcd64ca | 76 | } |
Stas285 | 2:c68be621f2f1 | 77 | // __enable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 78 | return result; |
ritarosakai | 0:6d1b9bcd64ca | 79 | } |
ritarosakai | 0:6d1b9bcd64ca | 80 | |
ritarosakai | 0:6d1b9bcd64ca | 81 | |
ritarosakai | 0:6d1b9bcd64ca | 82 | bool MPU6050::read_data(char sad, char sub, char *buf, int length) |
ritarosakai | 0:6d1b9bcd64ca | 83 | { |
ritarosakai | 0:6d1b9bcd64ca | 84 | if (length > 1) sub |= 0x80; |
ritarosakai | 0:6d1b9bcd64ca | 85 | |
ritarosakai | 0:6d1b9bcd64ca | 86 | return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 87 | } |
ritarosakai | 0:6d1b9bcd64ca | 88 | |
ritarosakai | 0:6d1b9bcd64ca | 89 | /* sample code for Nucleo-F042K6 |
ritarosakai | 0:6d1b9bcd64ca | 90 | #include "mbed.h" |
ritarosakai | 0:6d1b9bcd64ca | 91 | #include "MPU6050.h" |
ritarosakai | 0:6d1b9bcd64ca | 92 | |
ritarosakai | 0:6d1b9bcd64ca | 93 | MPU6050 mpu(D7,D8); |
ritarosakai | 0:6d1b9bcd64ca | 94 | Serial pc(USBTX,USBRX); |
ritarosakai | 0:6d1b9bcd64ca | 95 | |
ritarosakai | 0:6d1b9bcd64ca | 96 | float gx,gy,gz,ax,ay,az; |
ritarosakai | 0:6d1b9bcd64ca | 97 | |
ritarosakai | 0:6d1b9bcd64ca | 98 | int main() |
ritarosakai | 0:6d1b9bcd64ca | 99 | { |
ritarosakai | 0:6d1b9bcd64ca | 100 | pc.baud(115200); |
ritarosakai | 0:6d1b9bcd64ca | 101 | if(mpu.getID()==0x68) { |
ritarosakai | 0:6d1b9bcd64ca | 102 | pc.printf("MPU6050 OK"); |
ritarosakai | 0:6d1b9bcd64ca | 103 | wait(1); |
ritarosakai | 0:6d1b9bcd64ca | 104 | } else { |
ritarosakai | 0:6d1b9bcd64ca | 105 | pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID()); |
ritarosakai | 0:6d1b9bcd64ca | 106 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 107 | } |
ritarosakai | 0:6d1b9bcd64ca | 108 | } |
ritarosakai | 0:6d1b9bcd64ca | 109 | mpu.start(); |
ritarosakai | 0:6d1b9bcd64ca | 110 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 111 | mpu.read(&gx,&gy,&gz,&ax,&ay,&az); |
ritarosakai | 0:6d1b9bcd64ca | 112 | pc.printf("gx,gy,gz,ax,ay,az %.1f,%.1f,%.1f,%.2f,%.2f,%.2f\r\n",gx,gy,gz,ax,ay,az); |
ritarosakai | 0:6d1b9bcd64ca | 113 | wait(0.1); |
ritarosakai | 0:6d1b9bcd64ca | 114 | } |
ritarosakai | 0:6d1b9bcd64ca | 115 | } |
ritarosakai | 0:6d1b9bcd64ca | 116 | */ |