MPU6050 simple library
Dependents: MPU6050_test Drone MPU6050_test acelerometro ... more
MPU6050.cpp@8:566b467537c8, 2019-08-15 (annotated)
- Committer:
- ritarosakai
- Date:
- Thu Aug 15 03:19:48 2019 +0000
- Revision:
- 8:566b467537c8
- Parent:
- 3:023399fc7ca5
- Child:
- 9:2dd52ac40c96
Fix doxgen
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ritarosakai | 8:566b467537c8 | 1 | /** |
ritarosakai | 8:566b467537c8 | 2 | *@file MPU6050.cpp |
ritarosakai | 8:566b467537c8 | 3 | * |
ritarosakai | 8:566b467537c8 | 4 | */ |
ritarosakai | 8:566b467537c8 | 5 | |
ritarosakai | 0:6d1b9bcd64ca | 6 | #include "mbed.h" |
ritarosakai | 0:6d1b9bcd64ca | 7 | #include "MPU6050.h" |
ritarosakai | 0:6d1b9bcd64ca | 8 | |
ritarosakai | 0:6d1b9bcd64ca | 9 | #define ADDRESS 0xD1 |
ritarosakai | 0:6d1b9bcd64ca | 10 | |
ritarosakai | 0:6d1b9bcd64ca | 11 | MPU6050::MPU6050(PinName sda, PinName scl): |
ritarosakai | 0:6d1b9bcd64ca | 12 | _MPU6050(sda, scl) |
ritarosakai | 0:6d1b9bcd64ca | 13 | { |
ritarosakai | 0:6d1b9bcd64ca | 14 | _MPU6050.frequency(400000); |
ritarosakai | 0:6d1b9bcd64ca | 15 | } |
ritarosakai | 0:6d1b9bcd64ca | 16 | |
ritarosakai | 0:6d1b9bcd64ca | 17 | void MPU6050::start(void) |
ritarosakai | 0:6d1b9bcd64ca | 18 | { |
ritarosakai | 3:023399fc7ca5 | 19 | write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);//disable sleep mode |
ritarosakai | 0:6d1b9bcd64ca | 20 | write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s |
ritarosakai | 0:6d1b9bcd64ca | 21 | write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G |
ritarosakai | 2:2844a4ac1154 | 22 | write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 470Hz |
ritarosakai | 3:023399fc7ca5 | 23 | //to set more, see MPU6000 Register Map |
ritarosakai | 0:6d1b9bcd64ca | 24 | } |
ritarosakai | 0:6d1b9bcd64ca | 25 | |
ritarosakai | 0:6d1b9bcd64ca | 26 | char MPU6050::getID(void) |
ritarosakai | 0:6d1b9bcd64ca | 27 | { |
ritarosakai | 0:6d1b9bcd64ca | 28 | char devID; |
ritarosakai | 0:6d1b9bcd64ca | 29 | read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID); |
ritarosakai | 0:6d1b9bcd64ca | 30 | return devID; |
ritarosakai | 0:6d1b9bcd64ca | 31 | } |
ritarosakai | 0:6d1b9bcd64ca | 32 | |
ritarosakai | 0:6d1b9bcd64ca | 33 | bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az) |
ritarosakai | 0:6d1b9bcd64ca | 34 | { |
ritarosakai | 0:6d1b9bcd64ca | 35 | char data[6]; |
ritarosakai | 0:6d1b9bcd64ca | 36 | char data2[6]; |
ritarosakai | 0:6d1b9bcd64ca | 37 | if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { |
ritarosakai | 0:6d1b9bcd64ca | 38 | read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); |
ritarosakai | 1:cf3a9ec7205e | 39 | *gx = float(short(data[0] << 8 | data[1]))*0.0305f; |
ritarosakai | 1:cf3a9ec7205e | 40 | *gy = float(short(data[2] << 8 | data[3]))*0.0305f; |
ritarosakai | 1:cf3a9ec7205e | 41 | *gz = float(short(data[4] << 8 | data[5]))*0.0305f; |
ritarosakai | 1:cf3a9ec7205e | 42 | *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f; |
ritarosakai | 1:cf3a9ec7205e | 43 | *ay = float(short(data2[2] << 8 | data2[3]))*0.000122f; |
ritarosakai | 1:cf3a9ec7205e | 44 | *az = float(short(data2[4] << 8 | data2[5]))*0.000122f; |
ritarosakai | 1:cf3a9ec7205e | 45 | return true; |
ritarosakai | 1:cf3a9ec7205e | 46 | } |
ritarosakai | 1:cf3a9ec7205e | 47 | return false; |
ritarosakai | 1:cf3a9ec7205e | 48 | } |
ritarosakai | 1:cf3a9ec7205e | 49 | |
ritarosakai | 1:cf3a9ec7205e | 50 | bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az) |
ritarosakai | 1:cf3a9ec7205e | 51 | { |
ritarosakai | 1:cf3a9ec7205e | 52 | char data[6]; |
ritarosakai | 1:cf3a9ec7205e | 53 | char data2[6]; |
ritarosakai | 1:cf3a9ec7205e | 54 | if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { |
ritarosakai | 1:cf3a9ec7205e | 55 | read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); |
ritarosakai | 1:cf3a9ec7205e | 56 | *gx = int(short(data[0] << 8 | data[1])); |
ritarosakai | 1:cf3a9ec7205e | 57 | *gy = int(short(data[2] << 8 | data[3])); |
ritarosakai | 1:cf3a9ec7205e | 58 | *gz = int(short(data[4] << 8 | data[5])); |
ritarosakai | 1:cf3a9ec7205e | 59 | *ax = int(short(data2[0] << 8 | data2[1])); |
ritarosakai | 1:cf3a9ec7205e | 60 | *ay = int(short(data2[2] << 8 | data2[3])); |
ritarosakai | 1:cf3a9ec7205e | 61 | *az = int(short(data2[4] << 8 | data2[5])); |
ritarosakai | 0:6d1b9bcd64ca | 62 | return true; |
ritarosakai | 0:6d1b9bcd64ca | 63 | } |
ritarosakai | 0:6d1b9bcd64ca | 64 | return false; |
ritarosakai | 0:6d1b9bcd64ca | 65 | } |
ritarosakai | 0:6d1b9bcd64ca | 66 | |
ritarosakai | 0:6d1b9bcd64ca | 67 | bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v) |
ritarosakai | 0:6d1b9bcd64ca | 68 | { |
ritarosakai | 0:6d1b9bcd64ca | 69 | char data[2] = {addr_reg, v}; |
ritarosakai | 0:6d1b9bcd64ca | 70 | return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 71 | } |
ritarosakai | 0:6d1b9bcd64ca | 72 | |
ritarosakai | 0:6d1b9bcd64ca | 73 | bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v) |
ritarosakai | 0:6d1b9bcd64ca | 74 | { |
ritarosakai | 0:6d1b9bcd64ca | 75 | char data = addr_reg; |
ritarosakai | 0:6d1b9bcd64ca | 76 | bool result = false; |
ritarosakai | 0:6d1b9bcd64ca | 77 | __disable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 78 | if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) { |
ritarosakai | 0:6d1b9bcd64ca | 79 | *v = data; |
ritarosakai | 0:6d1b9bcd64ca | 80 | result = true; |
ritarosakai | 0:6d1b9bcd64ca | 81 | } |
ritarosakai | 0:6d1b9bcd64ca | 82 | __enable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 83 | return result; |
ritarosakai | 0:6d1b9bcd64ca | 84 | } |
ritarosakai | 0:6d1b9bcd64ca | 85 | |
ritarosakai | 0:6d1b9bcd64ca | 86 | |
ritarosakai | 0:6d1b9bcd64ca | 87 | bool MPU6050::read_data(char sad, char sub, char *buf, int length) |
ritarosakai | 0:6d1b9bcd64ca | 88 | { |
ritarosakai | 0:6d1b9bcd64ca | 89 | if (length > 1) sub |= 0x80; |
ritarosakai | 0:6d1b9bcd64ca | 90 | |
ritarosakai | 0:6d1b9bcd64ca | 91 | return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 92 | } |
ritarosakai | 0:6d1b9bcd64ca | 93 | |
ritarosakai | 0:6d1b9bcd64ca | 94 | /* sample code for Nucleo-F042K6 |
ritarosakai | 0:6d1b9bcd64ca | 95 | #include "mbed.h" |
ritarosakai | 0:6d1b9bcd64ca | 96 | #include "MPU6050.h" |
ritarosakai | 0:6d1b9bcd64ca | 97 | |
ritarosakai | 0:6d1b9bcd64ca | 98 | MPU6050 mpu(D7,D8); |
ritarosakai | 0:6d1b9bcd64ca | 99 | Serial pc(USBTX,USBRX); |
ritarosakai | 0:6d1b9bcd64ca | 100 | |
ritarosakai | 0:6d1b9bcd64ca | 101 | float gx,gy,gz,ax,ay,az; |
ritarosakai | 0:6d1b9bcd64ca | 102 | |
ritarosakai | 0:6d1b9bcd64ca | 103 | int main() |
ritarosakai | 0:6d1b9bcd64ca | 104 | { |
ritarosakai | 0:6d1b9bcd64ca | 105 | pc.baud(115200); |
ritarosakai | 0:6d1b9bcd64ca | 106 | if(mpu.getID()==0x68) { |
ritarosakai | 0:6d1b9bcd64ca | 107 | pc.printf("MPU6050 OK"); |
ritarosakai | 0:6d1b9bcd64ca | 108 | wait(1); |
ritarosakai | 0:6d1b9bcd64ca | 109 | } else { |
ritarosakai | 0:6d1b9bcd64ca | 110 | pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID()); |
ritarosakai | 0:6d1b9bcd64ca | 111 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 112 | } |
ritarosakai | 0:6d1b9bcd64ca | 113 | } |
ritarosakai | 0:6d1b9bcd64ca | 114 | mpu.start(); |
ritarosakai | 0:6d1b9bcd64ca | 115 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 116 | mpu.read(&gx,&gy,&gz,&ax,&ay,&az); |
ritarosakai | 0:6d1b9bcd64ca | 117 | 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 | 118 | wait(0.1); |
ritarosakai | 0:6d1b9bcd64ca | 119 | } |
ritarosakai | 0:6d1b9bcd64ca | 120 | } |
ritarosakai | 3:023399fc7ca5 | 121 | */ |