satoshi shimada / Mbed 2 deprecated mbed_mpu6050_i2c_raw_register

MPU-6050 sample code

Dependencies:   mbed

Dependents:   i2c_gyro_disp

mpu6050.cpp@3:ebd656a9c89f, 2018-12-13 (annotated)

Committer:
satoshi1204
Date:
Thu Dec 13 15:35:41 2018 +0000
Revision:
3:ebd656a9c89f
Parent:
1:6f743fe9a027 
fix register address.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
satoshi1204 0:abd3a0fd55a9 1 #include "mpu6050.h"
satoshi1204 0:abd3a0fd55a9 2
satoshi1204 1:6f743fe9a027 3 static const double RATE_ACC_LIST[4] = {
satoshi1204 0:abd3a0fd55a9 4 16384.0,
satoshi1204 0:abd3a0fd55a9 5 8192.0,
satoshi1204 0:abd3a0fd55a9 6 4096.0,
satoshi1204 0:abd3a0fd55a9 7 2048.0,
satoshi1204 0:abd3a0fd55a9 8 };
satoshi1204 0:abd3a0fd55a9 9
satoshi1204 1:6f743fe9a027 10 static const double RATE_GYRO_LIST[4] = {
satoshi1204 0:abd3a0fd55a9 11 131.0,
satoshi1204 0:abd3a0fd55a9 12 65.5,
satoshi1204 0:abd3a0fd55a9 13 32.8,
satoshi1204 0:abd3a0fd55a9 14 16.4,
satoshi1204 0:abd3a0fd55a9 15 };
satoshi1204 0:abd3a0fd55a9 16
satoshi1204 3:ebd656a9c89f 17 MPU6050::MPU6050(PinName i2c_sda, PinName i2c_scl) : i2c(i2c_sda, i2c_scl), address(0x68 << 1), rate_accelemeter(RATE_ACC_LIST[0]), rate_gyroscope(RATE_GYRO_LIST[0])
satoshi1204 0:abd3a0fd55a9 18 {
satoshi1204 1:6f743fe9a027 19 setSleep(false);
satoshi1204 0:abd3a0fd55a9 20 setMaxScale(MAX_ACC_2G, MAX_GYRO_250degpersec);
satoshi1204 0:abd3a0fd55a9 21 }
satoshi1204 0:abd3a0fd55a9 22
satoshi1204 0:abd3a0fd55a9 23 MPU6050::~MPU6050()
satoshi1204 0:abd3a0fd55a9 24 {
satoshi1204 0:abd3a0fd55a9 25 }
satoshi1204 0:abd3a0fd55a9 26
satoshi1204 0:abd3a0fd55a9 27 char MPU6050::readByte(char reg)
satoshi1204 0:abd3a0fd55a9 28 {
satoshi1204 0:abd3a0fd55a9 29 int result_write = i2c.write(address, &reg, 1);
satoshi1204 0:abd3a0fd55a9 30 if(result_write) {
satoshi1204 0:abd3a0fd55a9 31 // failed
satoshi1204 0:abd3a0fd55a9 32 }
satoshi1204 0:abd3a0fd55a9 33 char read_data;
satoshi1204 0:abd3a0fd55a9 34 int result_read = i2c.read(address, &read_data, 1);
satoshi1204 0:abd3a0fd55a9 35 if(result_read) {
satoshi1204 0:abd3a0fd55a9 36 // failed
satoshi1204 0:abd3a0fd55a9 37 }
satoshi1204 0:abd3a0fd55a9 38 return read_data;
satoshi1204 0:abd3a0fd55a9 39 }
satoshi1204 0:abd3a0fd55a9 40
satoshi1204 0:abd3a0fd55a9 41 void MPU6050::writeByte(char reg, char data)
satoshi1204 0:abd3a0fd55a9 42 {
satoshi1204 0:abd3a0fd55a9 43 char buf[2];
satoshi1204 0:abd3a0fd55a9 44 buf[0] = reg;
satoshi1204 0:abd3a0fd55a9 45 buf[1] = data;
satoshi1204 0:abd3a0fd55a9 46 int result = i2c.write(address, buf, 2);
satoshi1204 0:abd3a0fd55a9 47 if(result) {
satoshi1204 0:abd3a0fd55a9 48 // failed
satoshi1204 0:abd3a0fd55a9 49 }
satoshi1204 0:abd3a0fd55a9 50 }
satoshi1204 0:abd3a0fd55a9 51
satoshi1204 0:abd3a0fd55a9 52 void MPU6050::readAccelemeter(double &acc_x, double &acc_y, double &acc_z)
satoshi1204 0:abd3a0fd55a9 53 {
satoshi1204 0:abd3a0fd55a9 54 char h, l;
satoshi1204 0:abd3a0fd55a9 55 short x, y, z;
satoshi1204 0:abd3a0fd55a9 56 // read X
satoshi1204 0:abd3a0fd55a9 57 h = readByte(0x3B);
satoshi1204 3:ebd656a9c89f 58 l = readByte(0x3C);
satoshi1204 0:abd3a0fd55a9 59 x = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 60 // read Y
satoshi1204 0:abd3a0fd55a9 61 h = readByte(0x3D);
satoshi1204 0:abd3a0fd55a9 62 l = readByte(0x3E);
satoshi1204 0:abd3a0fd55a9 63 y = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 64 // read Z
satoshi1204 0:abd3a0fd55a9 65 h = readByte(0x3F);
satoshi1204 0:abd3a0fd55a9 66 l = readByte(0x40);
satoshi1204 0:abd3a0fd55a9 67 z = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 68 acc_x = (double)x / rate_accelemeter;
satoshi1204 0:abd3a0fd55a9 69 acc_y = (double)y / rate_accelemeter;
satoshi1204 0:abd3a0fd55a9 70 acc_z = (double)z / rate_accelemeter;
satoshi1204 0:abd3a0fd55a9 71 }
satoshi1204 0:abd3a0fd55a9 72
satoshi1204 0:abd3a0fd55a9 73 void MPU6050::readGyroscope(double &gyro_x, double &gyro_y, double &gyro_z)
satoshi1204 0:abd3a0fd55a9 74 {
satoshi1204 0:abd3a0fd55a9 75 char h, l;
satoshi1204 0:abd3a0fd55a9 76 short x, y, z;
satoshi1204 0:abd3a0fd55a9 77 // read X
satoshi1204 0:abd3a0fd55a9 78 h = readByte(0x43);
satoshi1204 0:abd3a0fd55a9 79 l = readByte(0x44);
satoshi1204 0:abd3a0fd55a9 80 x = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 81 // read Y
satoshi1204 0:abd3a0fd55a9 82 h = readByte(0x45);
satoshi1204 0:abd3a0fd55a9 83 l = readByte(0x46);
satoshi1204 0:abd3a0fd55a9 84 y = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 85 // read Z
satoshi1204 0:abd3a0fd55a9 86 h = readByte(0x47);
satoshi1204 0:abd3a0fd55a9 87 l = readByte(0x48);
satoshi1204 0:abd3a0fd55a9 88 z = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 89 gyro_x = (double)x / rate_gyroscope;
satoshi1204 0:abd3a0fd55a9 90 gyro_y = (double)y / rate_gyroscope;
satoshi1204 0:abd3a0fd55a9 91 gyro_z = (double)z / rate_gyroscope;
satoshi1204 0:abd3a0fd55a9 92 }
satoshi1204 0:abd3a0fd55a9 93
satoshi1204 0:abd3a0fd55a9 94 void MPU6050::getMaxScale(int &acc, int &gyro)
satoshi1204 0:abd3a0fd55a9 95 {
satoshi1204 0:abd3a0fd55a9 96 char reg;
satoshi1204 0:abd3a0fd55a9 97 reg = readByte(0x1C);
satoshi1204 0:abd3a0fd55a9 98 acc = reg;
satoshi1204 0:abd3a0fd55a9 99 reg = readByte(0x1B);
satoshi1204 0:abd3a0fd55a9 100 gyro = reg;
satoshi1204 0:abd3a0fd55a9 101 }
satoshi1204 0:abd3a0fd55a9 102
satoshi1204 0:abd3a0fd55a9 103 void MPU6050::setMaxScale(int max_acc, int max_gyro)
satoshi1204 0:abd3a0fd55a9 104 {
satoshi1204 0:abd3a0fd55a9 105 switch(max_acc) {
satoshi1204 0:abd3a0fd55a9 106 case MAX_ACC_2G:
satoshi1204 0:abd3a0fd55a9 107 writeByte(0x1C, 0x00);
satoshi1204 0:abd3a0fd55a9 108 break;
satoshi1204 0:abd3a0fd55a9 109 case MAX_ACC_4G:
satoshi1204 0:abd3a0fd55a9 110 writeByte(0x1C, 0x08);
satoshi1204 0:abd3a0fd55a9 111 break;
satoshi1204 0:abd3a0fd55a9 112 case MAX_ACC_8G:
satoshi1204 0:abd3a0fd55a9 113 writeByte(0x1C, 0x10);
satoshi1204 0:abd3a0fd55a9 114 break;
satoshi1204 0:abd3a0fd55a9 115 case MAX_ACC_16G:
satoshi1204 0:abd3a0fd55a9 116 writeByte(0x1C, 0x18);
satoshi1204 0:abd3a0fd55a9 117 break;
satoshi1204 0:abd3a0fd55a9 118 }
satoshi1204 0:abd3a0fd55a9 119 rate_accelemeter = RATE_ACC_LIST[max_acc];
satoshi1204 0:abd3a0fd55a9 120 switch(max_gyro) {
satoshi1204 0:abd3a0fd55a9 121 case MAX_GYRO_250degpersec:
satoshi1204 0:abd3a0fd55a9 122 writeByte(0x1B, 0x00);
satoshi1204 0:abd3a0fd55a9 123 break;
satoshi1204 0:abd3a0fd55a9 124 case MAX_GYRO_500degpersec:
satoshi1204 0:abd3a0fd55a9 125 writeByte(0x1B, 0x08);
satoshi1204 0:abd3a0fd55a9 126 break;
satoshi1204 0:abd3a0fd55a9 127 case MAX_GYRO_1000degpersec:
satoshi1204 0:abd3a0fd55a9 128 writeByte(0x1B, 0x10);
satoshi1204 0:abd3a0fd55a9 129 break;
satoshi1204 0:abd3a0fd55a9 130 case MAX_GYRO_2000degpersec:
satoshi1204 0:abd3a0fd55a9 131 writeByte(0x1B, 0x18);
satoshi1204 0:abd3a0fd55a9 132 break;
satoshi1204 0:abd3a0fd55a9 133 }
satoshi1204 0:abd3a0fd55a9 134 rate_gyroscope = RATE_GYRO_LIST[max_gyro];
satoshi1204 0:abd3a0fd55a9 135 }
satoshi1204 1:6f743fe9a027 136
satoshi1204 1:6f743fe9a027 137 void MPU6050::setSleep(bool sleep)
satoshi1204 1:6f743fe9a027 138 {
satoshi1204 1:6f743fe9a027 139 const char reg_sleep = 0x6B;
satoshi1204 1:6f743fe9a027 140 const char current_state = readByte(reg_sleep);
satoshi1204 1:6f743fe9a027 141 char write_data;
satoshi1204 1:6f743fe9a027 142 if(sleep) {
satoshi1204 1:6f743fe9a027 143 write_data = current_state | 0x40;
satoshi1204 1:6f743fe9a027 144 } else {
satoshi1204 1:6f743fe9a027 145 write_data = current_state & (~0x40);
satoshi1204 1:6f743fe9a027 146 }
satoshi1204 1:6f743fe9a027 147 writeByte(reg_sleep, write_data);
satoshi1204 1:6f743fe9a027 148 }