Satoshi Iyobe / MPU6050

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   measure_1_BBB measure_1_CCC measure_1_DDD

MPU6050.cpp@1:c9dc906f8b9f, 2021-05-13 (annotated)

Committer:
sato_shi
Date:
Thu May 13 05:11:28 2021 +0000
Revision:
1:c9dc906f8b9f
Parent:
0:3045b1e4107b
Child:
2:8622f361c1a4
BBBBBBBB;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Satoshisatoshi 0:3045b1e4107b 1 #include "mbed.h"
Satoshisatoshi 0:3045b1e4107b 2 #include "MPU6050.h"
Satoshisatoshi 0:3045b1e4107b 3
Satoshisatoshi 0:3045b1e4107b 4 //#define ADDRESS 0xD1
Satoshisatoshi 0:3045b1e4107b 5 //#define ADDRESS 0x68
Satoshisatoshi 0:3045b1e4107b 6
Satoshisatoshi 0:3045b1e4107b 7
Satoshisatoshi 0:3045b1e4107b 8 MPU6050::MPU6050(PinName sda, PinName scl, int address):
Satoshisatoshi 0:3045b1e4107b 9 _MPU6050(sda, scl)
Satoshisatoshi 0:3045b1e4107b 10 {
Satoshisatoshi 0:3045b1e4107b 11 _MPU6050.frequency(400000);
Satoshisatoshi 0:3045b1e4107b 12 ADDRESS = (address << 1) + 1;//convert 7bit to 8bit
Satoshisatoshi 0:3045b1e4107b 13 }
Satoshisatoshi 0:3045b1e4107b 14
Satoshisatoshi 0:3045b1e4107b 15 void MPU6050::start(void)
Satoshisatoshi 0:3045b1e4107b 16 {
Satoshisatoshi 0:3045b1e4107b 17 write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);//disable sleep mode
Satoshisatoshi 0:3045b1e4107b 18 write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s
sato_shi 1:c9dc906f8b9f 19 write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x18);//accel +-4G
Satoshisatoshi 0:3045b1e4107b 20 write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 470Hz
Satoshisatoshi 0:3045b1e4107b 21 //to set more, see MPU6000 Register Map
Satoshisatoshi 0:3045b1e4107b 22 }
Satoshisatoshi 0:3045b1e4107b 23
Satoshisatoshi 0:3045b1e4107b 24 char MPU6050::getID(void)
Satoshisatoshi 0:3045b1e4107b 25 {
Satoshisatoshi 0:3045b1e4107b 26 char devID;
Satoshisatoshi 0:3045b1e4107b 27 read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID);
Satoshisatoshi 0:3045b1e4107b 28 return devID;
Satoshisatoshi 0:3045b1e4107b 29 }
Satoshisatoshi 0:3045b1e4107b 30
Satoshisatoshi 0:3045b1e4107b 31 bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az)
Satoshisatoshi 0:3045b1e4107b 32 {
Satoshisatoshi 0:3045b1e4107b 33 char data[6];
Satoshisatoshi 0:3045b1e4107b 34 char data2[6];
Satoshisatoshi 0:3045b1e4107b 35 if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
Satoshisatoshi 0:3045b1e4107b 36 read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
Satoshisatoshi 0:3045b1e4107b 37 *gx = float(short(data[0] << 8 | data[1]))*0.0305f;
Satoshisatoshi 0:3045b1e4107b 38 *gy = float(short(data[2] << 8 | data[3]))*0.0305f;
Satoshisatoshi 0:3045b1e4107b 39 *gz = float(short(data[4] << 8 | data[5]))*0.0305f;
Satoshisatoshi 0:3045b1e4107b 40 *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f;
Satoshisatoshi 0:3045b1e4107b 41 *ay = float(short(data2[2] << 8 | data2[3]))*0.000122f;
Satoshisatoshi 0:3045b1e4107b 42 *az = float(short(data2[4] << 8 | data2[5]))*0.000122f;
Satoshisatoshi 0:3045b1e4107b 43 return true;
Satoshisatoshi 0:3045b1e4107b 44 }
Satoshisatoshi 0:3045b1e4107b 45 return false;
Satoshisatoshi 0:3045b1e4107b 46 }
Satoshisatoshi 0:3045b1e4107b 47
Satoshisatoshi 0:3045b1e4107b 48 bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *az)
Satoshisatoshi 0:3045b1e4107b 49 {
Satoshisatoshi 0:3045b1e4107b 50 char data[6];
Satoshisatoshi 0:3045b1e4107b 51 char data2[6];
Satoshisatoshi 0:3045b1e4107b 52 if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
Satoshisatoshi 0:3045b1e4107b 53 read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
Satoshisatoshi 0:3045b1e4107b 54 *gx = int(short(data[0] << 8 | data[1]));
Satoshisatoshi 0:3045b1e4107b 55 *gy = int(short(data[2] << 8 | data[3]));
Satoshisatoshi 0:3045b1e4107b 56 *gz = int(short(data[4] << 8 | data[5]));
Satoshisatoshi 0:3045b1e4107b 57 *ax = int(short(data2[0] << 8 | data2[1]));
Satoshisatoshi 0:3045b1e4107b 58 *ay = int(short(data2[2] << 8 | data2[3]));
Satoshisatoshi 0:3045b1e4107b 59 *az = int(short(data2[4] << 8 | data2[5]));
Satoshisatoshi 0:3045b1e4107b 60 return true;
Satoshisatoshi 0:3045b1e4107b 61 }
Satoshisatoshi 0:3045b1e4107b 62 return false;
Satoshisatoshi 0:3045b1e4107b 63 }
sato_shi 1:c9dc906f8b9f 64 bool MPU6050::readraw2(int *ax, int *ay, int *az)
sato_shi 1:c9dc906f8b9f 65 {
sato_shi 1:c9dc906f8b9f 66 char data2[6];
sato_shi 1:c9dc906f8b9f 67 if (read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6)) {
sato_shi 1:c9dc906f8b9f 68 *ax = int(short(data2[0] << 8 | data2[1]));
sato_shi 1:c9dc906f8b9f 69 *ay = int(short(data2[2] << 8 | data2[3]));
sato_shi 1:c9dc906f8b9f 70 *az = int(short(data2[4] << 8 | data2[5]));
sato_shi 1:c9dc906f8b9f 71 return true;
sato_shi 1:c9dc906f8b9f 72 }
sato_shi 1:c9dc906f8b9f 73 return false;
sato_shi 1:c9dc906f8b9f 74 }
Satoshisatoshi 0:3045b1e4107b 75 bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v)
Satoshisatoshi 0:3045b1e4107b 76 {
Satoshisatoshi 0:3045b1e4107b 77 char data[2] = {addr_reg, v};
Satoshisatoshi 0:3045b1e4107b 78 return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0;
Satoshisatoshi 0:3045b1e4107b 79 }
Satoshisatoshi 0:3045b1e4107b 80
Satoshisatoshi 0:3045b1e4107b 81 bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v)
Satoshisatoshi 0:3045b1e4107b 82 {
Satoshisatoshi 0:3045b1e4107b 83 char data = addr_reg;
Satoshisatoshi 0:3045b1e4107b 84 bool result = false;
Satoshisatoshi 0:3045b1e4107b 85 __disable_irq();
Satoshisatoshi 0:3045b1e4107b 86 if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) {
Satoshisatoshi 0:3045b1e4107b 87 *v = data;
Satoshisatoshi 0:3045b1e4107b 88 result = true;
Satoshisatoshi 0:3045b1e4107b 89 }
Satoshisatoshi 0:3045b1e4107b 90 __enable_irq();
Satoshisatoshi 0:3045b1e4107b 91 return result;
Satoshisatoshi 0:3045b1e4107b 92 }
Satoshisatoshi 0:3045b1e4107b 93
Satoshisatoshi 0:3045b1e4107b 94
Satoshisatoshi 0:3045b1e4107b 95 bool MPU6050::read_data(char sad, char sub, char *buf, int length)
Satoshisatoshi 0:3045b1e4107b 96 {
Satoshisatoshi 0:3045b1e4107b 97 if (length > 1) sub |= 0x80;
Satoshisatoshi 0:3045b1e4107b 98
Satoshisatoshi 0:3045b1e4107b 99 return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0;
Satoshisatoshi 0:3045b1e4107b 100 }