Dima Dudin / MPU6050

1

Fork of MPU6050 by Stanislav Dudin

MPU6050.cpp@6:b4f5edf825be, 2018-11-18 (annotated)

Committer:
dima285
Date:
Sun Nov 18 10:24:23 2018 +0000
Revision:
6:b4f5edf825be
Parent:
5:f806657f0009 
plohoy ugol

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
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);
dima285 5:f806657f0009 15 write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x00);//gyro +-500deg/s //0x10);//gyro +-1000deg/s
Stas285 2:c68be621f2f1 16 write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x00);//accel +-2G //0x08);//accel +-4G
dima285 5:f806657f0009 17 write_reg(ADDRESS,MPU6050_CONFIG,0x06);//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);
dima285 6:b4f5edf825be 34 *gx = float(short(data[0] << 8 | data[1]))*2.663e-4/2; //rad/s
dima285 6:b4f5edf825be 35 *gy = float(short(data[2] << 8 | data[3]))*2.663e-4/2;
dima285 6:b4f5edf825be 36 *gz = float(short(data[4] << 8 | data[5]))*2.663e-4/2;
dima285 6:b4f5edf825be 37 *ax = float(short(data2[0] << 8 | data2[1]))*5.9875e-6; //m/s^2
dima285 6:b4f5edf825be 38 *ay = float(short(data2[2] << 8 | data2[3]))*5.9875e-6;
dima285 6:b4f5edf825be 39 *az = float(short(data2[4] << 8 | data2[5]))*5.9875e-6;
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 */