Dima Dudin / MPU6050

1

Fork of MPU6050 by Stanislav Dudin

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?

UserRevisionLine numberNew 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 */