Sakai Ritaro / MPU6050

MPU6050 simple library

Dependents:   MPU6050_test Drone MPU6050_test acelerometro ... more

MPU6050.cpp@8:566b467537c8, 2019-08-15 (annotated)

Committer:
ritarosakai
Date:
Thu Aug 15 03:19:48 2019 +0000
Revision:
8:566b467537c8
Parent:
3:023399fc7ca5
Child:
9:2dd52ac40c96
Fix doxgen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ritarosakai 8:566b467537c8 1 /**
ritarosakai 8:566b467537c8 2 *@file MPU6050.cpp
ritarosakai 8:566b467537c8 3 *
ritarosakai 8:566b467537c8 4 */
ritarosakai 8:566b467537c8 5
ritarosakai 0:6d1b9bcd64ca 6 #include "mbed.h"
ritarosakai 0:6d1b9bcd64ca 7 #include "MPU6050.h"
ritarosakai 0:6d1b9bcd64ca 8
ritarosakai 0:6d1b9bcd64ca 9 #define ADDRESS 0xD1
ritarosakai 0:6d1b9bcd64ca 10
ritarosakai 0:6d1b9bcd64ca 11 MPU6050::MPU6050(PinName sda, PinName scl):
ritarosakai 0:6d1b9bcd64ca 12 _MPU6050(sda, scl)
ritarosakai 0:6d1b9bcd64ca 13 {
ritarosakai 0:6d1b9bcd64ca 14 _MPU6050.frequency(400000);
ritarosakai 0:6d1b9bcd64ca 15 }
ritarosakai 0:6d1b9bcd64ca 16
ritarosakai 0:6d1b9bcd64ca 17 void MPU6050::start(void)
ritarosakai 0:6d1b9bcd64ca 18 {
ritarosakai 3:023399fc7ca5 19 write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);//disable sleep mode
ritarosakai 0:6d1b9bcd64ca 20 write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s
ritarosakai 0:6d1b9bcd64ca 21 write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G
ritarosakai 2:2844a4ac1154 22 write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 470Hz
ritarosakai 3:023399fc7ca5 23 //to set more, see MPU6000 Register Map
ritarosakai 0:6d1b9bcd64ca 24 }
ritarosakai 0:6d1b9bcd64ca 25
ritarosakai 0:6d1b9bcd64ca 26 char MPU6050::getID(void)
ritarosakai 0:6d1b9bcd64ca 27 {
ritarosakai 0:6d1b9bcd64ca 28 char devID;
ritarosakai 0:6d1b9bcd64ca 29 read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID);
ritarosakai 0:6d1b9bcd64ca 30 return devID;
ritarosakai 0:6d1b9bcd64ca 31 }
ritarosakai 0:6d1b9bcd64ca 32
ritarosakai 0:6d1b9bcd64ca 33 bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az)
ritarosakai 0:6d1b9bcd64ca 34 {
ritarosakai 0:6d1b9bcd64ca 35 char data[6];
ritarosakai 0:6d1b9bcd64ca 36 char data2[6];
ritarosakai 0:6d1b9bcd64ca 37 if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
ritarosakai 0:6d1b9bcd64ca 38 read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
ritarosakai 1:cf3a9ec7205e 39 *gx = float(short(data[0] << 8 | data[1]))*0.0305f;
ritarosakai 1:cf3a9ec7205e 40 *gy = float(short(data[2] << 8 | data[3]))*0.0305f;
ritarosakai 1:cf3a9ec7205e 41 *gz = float(short(data[4] << 8 | data[5]))*0.0305f;
ritarosakai 1:cf3a9ec7205e 42 *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f;
ritarosakai 1:cf3a9ec7205e 43 *ay = float(short(data2[2] << 8 | data2[3]))*0.000122f;
ritarosakai 1:cf3a9ec7205e 44 *az = float(short(data2[4] << 8 | data2[5]))*0.000122f;
ritarosakai 1:cf3a9ec7205e 45 return true;
ritarosakai 1:cf3a9ec7205e 46 }
ritarosakai 1:cf3a9ec7205e 47 return false;
ritarosakai 1:cf3a9ec7205e 48 }
ritarosakai 1:cf3a9ec7205e 49
ritarosakai 1:cf3a9ec7205e 50 bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az)
ritarosakai 1:cf3a9ec7205e 51 {
ritarosakai 1:cf3a9ec7205e 52 char data[6];
ritarosakai 1:cf3a9ec7205e 53 char data2[6];
ritarosakai 1:cf3a9ec7205e 54 if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
ritarosakai 1:cf3a9ec7205e 55 read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
ritarosakai 1:cf3a9ec7205e 56 *gx = int(short(data[0] << 8 | data[1]));
ritarosakai 1:cf3a9ec7205e 57 *gy = int(short(data[2] << 8 | data[3]));
ritarosakai 1:cf3a9ec7205e 58 *gz = int(short(data[4] << 8 | data[5]));
ritarosakai 1:cf3a9ec7205e 59 *ax = int(short(data2[0] << 8 | data2[1]));
ritarosakai 1:cf3a9ec7205e 60 *ay = int(short(data2[2] << 8 | data2[3]));
ritarosakai 1:cf3a9ec7205e 61 *az = int(short(data2[4] << 8 | data2[5]));
ritarosakai 0:6d1b9bcd64ca 62 return true;
ritarosakai 0:6d1b9bcd64ca 63 }
ritarosakai 0:6d1b9bcd64ca 64 return false;
ritarosakai 0:6d1b9bcd64ca 65 }
ritarosakai 0:6d1b9bcd64ca 66
ritarosakai 0:6d1b9bcd64ca 67 bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v)
ritarosakai 0:6d1b9bcd64ca 68 {
ritarosakai 0:6d1b9bcd64ca 69 char data[2] = {addr_reg, v};
ritarosakai 0:6d1b9bcd64ca 70 return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0;
ritarosakai 0:6d1b9bcd64ca 71 }
ritarosakai 0:6d1b9bcd64ca 72
ritarosakai 0:6d1b9bcd64ca 73 bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v)
ritarosakai 0:6d1b9bcd64ca 74 {
ritarosakai 0:6d1b9bcd64ca 75 char data = addr_reg;
ritarosakai 0:6d1b9bcd64ca 76 bool result = false;
ritarosakai 0:6d1b9bcd64ca 77 __disable_irq();
ritarosakai 0:6d1b9bcd64ca 78 if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) {
ritarosakai 0:6d1b9bcd64ca 79 *v = data;
ritarosakai 0:6d1b9bcd64ca 80 result = true;
ritarosakai 0:6d1b9bcd64ca 81 }
ritarosakai 0:6d1b9bcd64ca 82 __enable_irq();
ritarosakai 0:6d1b9bcd64ca 83 return result;
ritarosakai 0:6d1b9bcd64ca 84 }
ritarosakai 0:6d1b9bcd64ca 85
ritarosakai 0:6d1b9bcd64ca 86
ritarosakai 0:6d1b9bcd64ca 87 bool MPU6050::read_data(char sad, char sub, char *buf, int length)
ritarosakai 0:6d1b9bcd64ca 88 {
ritarosakai 0:6d1b9bcd64ca 89 if (length > 1) sub |= 0x80;
ritarosakai 0:6d1b9bcd64ca 90
ritarosakai 0:6d1b9bcd64ca 91 return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0;
ritarosakai 0:6d1b9bcd64ca 92 }
ritarosakai 0:6d1b9bcd64ca 93
ritarosakai 0:6d1b9bcd64ca 94 /* sample code for Nucleo-F042K6
ritarosakai 0:6d1b9bcd64ca 95 #include "mbed.h"
ritarosakai 0:6d1b9bcd64ca 96 #include "MPU6050.h"
ritarosakai 0:6d1b9bcd64ca 97
ritarosakai 0:6d1b9bcd64ca 98 MPU6050 mpu(D7,D8);
ritarosakai 0:6d1b9bcd64ca 99 Serial pc(USBTX,USBRX);
ritarosakai 0:6d1b9bcd64ca 100
ritarosakai 0:6d1b9bcd64ca 101 float gx,gy,gz,ax,ay,az;
ritarosakai 0:6d1b9bcd64ca 102
ritarosakai 0:6d1b9bcd64ca 103 int main()
ritarosakai 0:6d1b9bcd64ca 104 {
ritarosakai 0:6d1b9bcd64ca 105 pc.baud(115200);
ritarosakai 0:6d1b9bcd64ca 106 if(mpu.getID()==0x68) {
ritarosakai 0:6d1b9bcd64ca 107 pc.printf("MPU6050 OK");
ritarosakai 0:6d1b9bcd64ca 108 wait(1);
ritarosakai 0:6d1b9bcd64ca 109 } else {
ritarosakai 0:6d1b9bcd64ca 110 pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
ritarosakai 0:6d1b9bcd64ca 111 while(1) {
ritarosakai 0:6d1b9bcd64ca 112 }
ritarosakai 0:6d1b9bcd64ca 113 }
ritarosakai 0:6d1b9bcd64ca 114 mpu.start();
ritarosakai 0:6d1b9bcd64ca 115 while(1) {
ritarosakai 0:6d1b9bcd64ca 116 mpu.read(&gx,&gy,&gz,&ax,&ay,&az);
ritarosakai 0:6d1b9bcd64ca 117 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 118 wait(0.1);
ritarosakai 0:6d1b9bcd64ca 119 }
ritarosakai 0:6d1b9bcd64ca 120 }
ritarosakai 3:023399fc7ca5 121 */