Sakai Ritaro / MPU6050

MPU6050 simple library

Dependents:   MPU6050_test Drone MPU6050_test acelerometro ... more

MPU6050.cpp@16:002d3ac85242, 2019-08-19 (annotated)

Committer:
ritarosakai
Date:
Mon Aug 19 08:57:14 2019 +0000
Revision:
16:002d3ac85242
Parent:
10:31cec9253c7c 
Remove __disable_irq

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 3:023399fc7ca5 14 write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);//disable sleep mode
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 2:2844a4ac1154 17 write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 470Hz
ritarosakai 3:023399fc7ca5 18 //to set more, see MPU6000 Register Map
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);
ritarosakai 1:cf3a9ec7205e 34 *gx = float(short(data[0] << 8 | data[1]))*0.0305f;
ritarosakai 1:cf3a9ec7205e 35 *gy = float(short(data[2] << 8 | data[3]))*0.0305f;
ritarosakai 1:cf3a9ec7205e 36 *gz = float(short(data[4] << 8 | data[5]))*0.0305f;
ritarosakai 1:cf3a9ec7205e 37 *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f;
ritarosakai 1:cf3a9ec7205e 38 *ay = float(short(data2[2] << 8 | data2[3]))*0.000122f;
ritarosakai 1:cf3a9ec7205e 39 *az = float(short(data2[4] << 8 | data2[5]))*0.000122f;
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;
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 return result;
ritarosakai 0:6d1b9bcd64ca 77 }
ritarosakai 0:6d1b9bcd64ca 78
ritarosakai 0:6d1b9bcd64ca 79
ritarosakai 0:6d1b9bcd64ca 80 bool MPU6050::read_data(char sad, char sub, char *buf, int length)
ritarosakai 0:6d1b9bcd64ca 81 {
ritarosakai 0:6d1b9bcd64ca 82 if (length > 1) sub |= 0x80;
ritarosakai 0:6d1b9bcd64ca 83
ritarosakai 0:6d1b9bcd64ca 84 return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0;
ritarosakai 0:6d1b9bcd64ca 85 }