Dima Dudin / MPU6050

1

Fork of MPU6050 by Stanislav Dudin

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?

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