fork
MPU6050.cpp@1:cf3a9ec7205e, 2017-01-05 (annotated)
- Committer:
- ritarosakai
- Date:
- Thu Jan 05 05:04:56 2017 +0000
- Revision:
- 1:cf3a9ec7205e
- Parent:
- 0:6d1b9bcd64ca
- Child:
- 2:c68be621f2f1
add readraw; change to use FPU
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 | 1:cf3a9ec7205e | 33 | *gx = float(short(data[0] << 8 | data[1]))*0.0305f; |
ritarosakai | 1:cf3a9ec7205e | 34 | *gy = float(short(data[2] << 8 | data[3]))*0.0305f; |
ritarosakai | 1:cf3a9ec7205e | 35 | *gz = float(short(data[4] << 8 | data[5]))*0.0305f; |
ritarosakai | 1:cf3a9ec7205e | 36 | *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f; |
ritarosakai | 1:cf3a9ec7205e | 37 | *ay = float(short(data2[2] << 8 | data2[3]))*0.000122f; |
ritarosakai | 1:cf3a9ec7205e | 38 | *az = float(short(data2[4] << 8 | data2[5]))*0.000122f; |
ritarosakai | 1:cf3a9ec7205e | 39 | return true; |
ritarosakai | 1:cf3a9ec7205e | 40 | } |
ritarosakai | 1:cf3a9ec7205e | 41 | return false; |
ritarosakai | 1:cf3a9ec7205e | 42 | } |
ritarosakai | 1:cf3a9ec7205e | 43 | |
ritarosakai | 1:cf3a9ec7205e | 44 | bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az) |
ritarosakai | 1:cf3a9ec7205e | 45 | { |
ritarosakai | 1:cf3a9ec7205e | 46 | char data[6]; |
ritarosakai | 1:cf3a9ec7205e | 47 | char data2[6]; |
ritarosakai | 1:cf3a9ec7205e | 48 | if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { |
ritarosakai | 1:cf3a9ec7205e | 49 | read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); |
ritarosakai | 1:cf3a9ec7205e | 50 | *gx = int(short(data[0] << 8 | data[1])); |
ritarosakai | 1:cf3a9ec7205e | 51 | *gy = int(short(data[2] << 8 | data[3])); |
ritarosakai | 1:cf3a9ec7205e | 52 | *gz = int(short(data[4] << 8 | data[5])); |
ritarosakai | 1:cf3a9ec7205e | 53 | *ax = int(short(data2[0] << 8 | data2[1])); |
ritarosakai | 1:cf3a9ec7205e | 54 | *ay = int(short(data2[2] << 8 | data2[3])); |
ritarosakai | 1:cf3a9ec7205e | 55 | *az = int(short(data2[4] << 8 | data2[5])); |
ritarosakai | 0:6d1b9bcd64ca | 56 | return true; |
ritarosakai | 0:6d1b9bcd64ca | 57 | } |
ritarosakai | 0:6d1b9bcd64ca | 58 | return false; |
ritarosakai | 0:6d1b9bcd64ca | 59 | } |
ritarosakai | 0:6d1b9bcd64ca | 60 | |
ritarosakai | 0:6d1b9bcd64ca | 61 | bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v) |
ritarosakai | 0:6d1b9bcd64ca | 62 | { |
ritarosakai | 0:6d1b9bcd64ca | 63 | char data[2] = {addr_reg, v}; |
ritarosakai | 0:6d1b9bcd64ca | 64 | return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 65 | } |
ritarosakai | 0:6d1b9bcd64ca | 66 | |
ritarosakai | 0:6d1b9bcd64ca | 67 | bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v) |
ritarosakai | 0:6d1b9bcd64ca | 68 | { |
ritarosakai | 0:6d1b9bcd64ca | 69 | char data = addr_reg; |
ritarosakai | 0:6d1b9bcd64ca | 70 | bool result = false; |
ritarosakai | 0:6d1b9bcd64ca | 71 | __disable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 72 | if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) { |
ritarosakai | 0:6d1b9bcd64ca | 73 | *v = data; |
ritarosakai | 0:6d1b9bcd64ca | 74 | result = true; |
ritarosakai | 0:6d1b9bcd64ca | 75 | } |
ritarosakai | 0:6d1b9bcd64ca | 76 | __enable_irq(); |
ritarosakai | 0:6d1b9bcd64ca | 77 | return result; |
ritarosakai | 0:6d1b9bcd64ca | 78 | } |
ritarosakai | 0:6d1b9bcd64ca | 79 | |
ritarosakai | 0:6d1b9bcd64ca | 80 | |
ritarosakai | 0:6d1b9bcd64ca | 81 | bool MPU6050::read_data(char sad, char sub, char *buf, int length) |
ritarosakai | 0:6d1b9bcd64ca | 82 | { |
ritarosakai | 0:6d1b9bcd64ca | 83 | if (length > 1) sub |= 0x80; |
ritarosakai | 0:6d1b9bcd64ca | 84 | |
ritarosakai | 0:6d1b9bcd64ca | 85 | return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0; |
ritarosakai | 0:6d1b9bcd64ca | 86 | } |
ritarosakai | 0:6d1b9bcd64ca | 87 | |
ritarosakai | 0:6d1b9bcd64ca | 88 | /* sample code for Nucleo-F042K6 |
ritarosakai | 0:6d1b9bcd64ca | 89 | #include "mbed.h" |
ritarosakai | 0:6d1b9bcd64ca | 90 | #include "MPU6050.h" |
ritarosakai | 0:6d1b9bcd64ca | 91 | |
ritarosakai | 0:6d1b9bcd64ca | 92 | MPU6050 mpu(D7,D8); |
ritarosakai | 0:6d1b9bcd64ca | 93 | Serial pc(USBTX,USBRX); |
ritarosakai | 0:6d1b9bcd64ca | 94 | |
ritarosakai | 0:6d1b9bcd64ca | 95 | float gx,gy,gz,ax,ay,az; |
ritarosakai | 0:6d1b9bcd64ca | 96 | |
ritarosakai | 0:6d1b9bcd64ca | 97 | int main() |
ritarosakai | 0:6d1b9bcd64ca | 98 | { |
ritarosakai | 0:6d1b9bcd64ca | 99 | pc.baud(115200); |
ritarosakai | 0:6d1b9bcd64ca | 100 | if(mpu.getID()==0x68) { |
ritarosakai | 0:6d1b9bcd64ca | 101 | pc.printf("MPU6050 OK"); |
ritarosakai | 0:6d1b9bcd64ca | 102 | wait(1); |
ritarosakai | 0:6d1b9bcd64ca | 103 | } else { |
ritarosakai | 0:6d1b9bcd64ca | 104 | pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID()); |
ritarosakai | 0:6d1b9bcd64ca | 105 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 106 | } |
ritarosakai | 0:6d1b9bcd64ca | 107 | } |
ritarosakai | 0:6d1b9bcd64ca | 108 | mpu.start(); |
ritarosakai | 0:6d1b9bcd64ca | 109 | while(1) { |
ritarosakai | 0:6d1b9bcd64ca | 110 | mpu.read(&gx,&gy,&gz,&ax,&ay,&az); |
ritarosakai | 0:6d1b9bcd64ca | 111 | 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 | 112 | wait(0.1); |
ritarosakai | 0:6d1b9bcd64ca | 113 | } |
ritarosakai | 0:6d1b9bcd64ca | 114 | } |
ritarosakai | 0:6d1b9bcd64ca | 115 | */ |