MPU6050 simple library
Dependents: MPU6050_test Drone MPU6050_test acelerometro ... more
MPU6050.cpp@0:6d1b9bcd64ca, 2016-09-19 (annotated)
- Committer:
- ritarosakai
- Date:
- Mon Sep 19 05:09:07 2016 +0000
- Revision:
- 0:6d1b9bcd64ca
- Child:
- 1:cf3a9ec7205e
MPU6050
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 | |
ritarosakai | 0:6d1b9bcd64ca | 4 | #define ADDRESS 0xD1 |
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); |
ritarosakai | 0:6d1b9bcd64ca | 15 | write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s |
ritarosakai | 0:6d1b9bcd64ca | 16 | write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G |
ritarosakai | 0:6d1b9bcd64ca | 17 | write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 500Hz |
ritarosakai | 0:6d1b9bcd64ca | 18 | } |
ritarosakai | 0:6d1b9bcd64ca | 19 | |
ritarosakai | 0:6d1b9bcd64ca | 20 | char MPU6050::getID(void) |
ritarosakai | 0:6d1b9bcd64ca | 21 | { |
ritarosakai | 0:6d1b9bcd64ca | 22 | char devID; |
ritarosakai | 0:6d1b9bcd64ca | 23 | read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID); |
ritarosakai | 0:6d1b9bcd64ca | 24 | return devID; |
ritarosakai | 0:6d1b9bcd64ca | 25 | } |
ritarosakai | 0:6d1b9bcd64ca | 26 | |
ritarosakai | 0:6d1b9bcd64ca | 27 | bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az) |
ritarosakai | 0:6d1b9bcd64ca | 28 | { |
ritarosakai | 0:6d1b9bcd64ca | 29 | char data[6]; |
ritarosakai | 0:6d1b9bcd64ca | 30 | char data2[6]; |
ritarosakai | 0:6d1b9bcd64ca | 31 | if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { |
ritarosakai | 0:6d1b9bcd64ca | 32 | read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); |
ritarosakai | 0:6d1b9bcd64ca | 33 | *gx = float(short(data[0] << 8 | data[1]))*0.0305; |
ritarosakai | 0:6d1b9bcd64ca | 34 | *gy = float(short(data[2] << 8 | data[3]))*0.0305; |
ritarosakai | 0:6d1b9bcd64ca | 35 | *gz = float(short(data[4] << 8 | data[5]))*0.0305; |
ritarosakai | 0:6d1b9bcd64ca | 36 | *ax = float(short(data2[0] << 8 | data2[1]))*0.000122; |
ritarosakai | 0:6d1b9bcd64ca | 37 | *ay = float(short(data2[2] << 8 | data2[3]))*0.000122; |
ritarosakai | 0:6d1b9bcd64ca | 38 | *az = float(short(data2[4] << 8 | data2[5]))*0.000122; |
ritarosakai | 0:6d1b9bcd64ca | 39 | return true; |
ritarosakai | 0:6d1b9bcd64ca | 40 | } |
ritarosakai | 0:6d1b9bcd64ca | 41 | return false; |
ritarosakai | 0:6d1b9bcd64ca | 42 | } |
ritarosakai | 0:6d1b9bcd64ca | 43 | |
ritarosakai | 0:6d1b9bcd64ca | 44 | bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v) |
ritarosakai | 0:6d1b9bcd64ca | 45 | { |
ritarosakai | 0:6d1b9bcd64ca | 46 | char data[2] = {addr_reg, v}; |
ritarosakai | 0:6d1b9bcd64ca | 47 | return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 48 | } |
ritarosakai | 0:6d1b9bcd64ca | 49 | |
ritarosakai | 0:6d1b9bcd64ca | 50 | bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v) |
ritarosakai | 0:6d1b9bcd64ca | 51 | { |
ritarosakai | 0:6d1b9bcd64ca | 52 | char data = addr_reg; |
ritarosakai | 0:6d1b9bcd64ca | 53 | bool result = false; |
ritarosakai | 0:6d1b9bcd64ca | 54 | __disable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 55 | if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) { |
ritarosakai | 0:6d1b9bcd64ca | 56 | *v = data; |
ritarosakai | 0:6d1b9bcd64ca | 57 | result = true; |
ritarosakai | 0:6d1b9bcd64ca | 58 | } |
ritarosakai | 0:6d1b9bcd64ca | 59 | __enable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 60 | return result; |
ritarosakai | 0:6d1b9bcd64ca | 61 | } |
ritarosakai | 0:6d1b9bcd64ca | 62 | |
ritarosakai | 0:6d1b9bcd64ca | 63 | |
ritarosakai | 0:6d1b9bcd64ca | 64 | bool MPU6050::read_data(char sad, char sub, char *buf, int length) |
ritarosakai | 0:6d1b9bcd64ca | 65 | { |
ritarosakai | 0:6d1b9bcd64ca | 66 | if (length > 1) sub |= 0x80; |
ritarosakai | 0:6d1b9bcd64ca | 67 | |
ritarosakai | 0:6d1b9bcd64ca | 68 | return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 69 | } |
ritarosakai | 0:6d1b9bcd64ca | 70 | |
ritarosakai | 0:6d1b9bcd64ca | 71 | /* sample code for Nucleo-F042K6 |
ritarosakai | 0:6d1b9bcd64ca | 72 | #include "mbed.h" |
ritarosakai | 0:6d1b9bcd64ca | 73 | #include "MPU6050.h" |
ritarosakai | 0:6d1b9bcd64ca | 74 | |
ritarosakai | 0:6d1b9bcd64ca | 75 | MPU6050 mpu(D7,D8); |
ritarosakai | 0:6d1b9bcd64ca | 76 | Serial pc(USBTX,USBRX); |
ritarosakai | 0:6d1b9bcd64ca | 77 | |
ritarosakai | 0:6d1b9bcd64ca | 78 | float gx,gy,gz,ax,ay,az; |
ritarosakai | 0:6d1b9bcd64ca | 79 | |
ritarosakai | 0:6d1b9bcd64ca | 80 | int main() |
ritarosakai | 0:6d1b9bcd64ca | 81 | { |
ritarosakai | 0:6d1b9bcd64ca | 82 | pc.baud(115200); |
ritarosakai | 0:6d1b9bcd64ca | 83 | if(mpu.getID()==0x68) { |
ritarosakai | 0:6d1b9bcd64ca | 84 | pc.printf("MPU6050 OK"); |
ritarosakai | 0:6d1b9bcd64ca | 85 | wait(1); |
ritarosakai | 0:6d1b9bcd64ca | 86 | } else { |
ritarosakai | 0:6d1b9bcd64ca | 87 | pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID()); |
ritarosakai | 0:6d1b9bcd64ca | 88 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 89 | } |
ritarosakai | 0:6d1b9bcd64ca | 90 | } |
ritarosakai | 0:6d1b9bcd64ca | 91 | mpu.start(); |
ritarosakai | 0:6d1b9bcd64ca | 92 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 93 | mpu.read(&gx,&gy,&gz,&ax,&ay,&az); |
ritarosakai | 0:6d1b9bcd64ca | 94 | 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 | 95 | wait(0.1); |
ritarosakai | 0:6d1b9bcd64ca | 96 | } |
ritarosakai | 0:6d1b9bcd64ca | 97 | } |
ritarosakai | 0:6d1b9bcd64ca | 98 | */ |