NM500 / NeuroShield

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:   NeuroShield_SimpleScript NeuroShield_andIMU NeuroShield_Gesture_Recognition

mpu6050.cpp@3:6163399b611e, 2020-02-11 (annotated)

Committer:
nepes_ai
Date:
Tue Feb 11 00:48:18 2020 +0000
Revision:
3:6163399b611e
Parent:
0:529602524696 
Release version 1.1.5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nepes_ai 0:529602524696 1 #include "mbed.h"
nepes_ai 0:529602524696 2 #include "mpu6050.h"
nepes_ai 0:529602524696 3
nepes_ai 0:529602524696 4 MPU6050::MPU6050() : connection(PB_9, PB_8) { // SDA(D14), SCL(D15) // pinnames.h
nepes_ai 0:529602524696 5 dev_addr = MPU6050_DEFAULT_ADDRESS;
nepes_ai 0:529602524696 6 }
nepes_ai 0:529602524696 7
nepes_ai 0:529602524696 8 MPU6050::MPU6050(uint8_t address) : connection(PB_9, PB_8) {
nepes_ai 0:529602524696 9 dev_addr = address;
nepes_ai 0:529602524696 10 }
nepes_ai 0:529602524696 11
nepes_ai 0:529602524696 12 MPU6050::MPU6050(uint8_t address, PinName sda, PinName scl) : connection(sda, scl) {
nepes_ai 0:529602524696 13 dev_addr = address;
nepes_ai 0:529602524696 14 }
nepes_ai 0:529602524696 15
nepes_ai 0:529602524696 16 void MPU6050::initialize() {
nepes_ai 0:529602524696 17 setClockSource(MPU6050_CLOCK_PLL_XGYRO);
nepes_ai 0:529602524696 18 setFullScaleGyroRange(MPU6050_GYRO_FS_250);
nepes_ai 0:529602524696 19 setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
nepes_ai 0:529602524696 20 setSleepEnable(false);
nepes_ai 0:529602524696 21 }
nepes_ai 0:529602524696 22
nepes_ai 0:529602524696 23 bool MPU6050::testConnection() {
nepes_ai 0:529602524696 24 return (getDeviceID() == 0x68);
nepes_ai 0:529602524696 25 }
nepes_ai 0:529602524696 26
nepes_ai 0:529602524696 27 void MPU6050::write(char address, char data) {
nepes_ai 0:529602524696 28 char temp[2];
nepes_ai 0:529602524696 29 temp[0] = address;
nepes_ai 0:529602524696 30 temp[1] = data;
nepes_ai 0:529602524696 31 connection.write((dev_addr << 1), temp, 2);
nepes_ai 0:529602524696 32 }
nepes_ai 0:529602524696 33
nepes_ai 0:529602524696 34 void MPU6050::writeWord(char address, int16_t data) {
nepes_ai 0:529602524696 35 char temp[3];
nepes_ai 0:529602524696 36 temp[0] = address;
nepes_ai 0:529602524696 37 temp[1] = (char)((data >> 8) & 0x00ff);
nepes_ai 0:529602524696 38 temp[2] = (char)(data & 0x00ff);
nepes_ai 0:529602524696 39 connection.write((dev_addr << 1), temp, 3);
nepes_ai 0:529602524696 40 }
nepes_ai 0:529602524696 41
nepes_ai 0:529602524696 42 char MPU6050::read(char address) {
nepes_ai 0:529602524696 43 char retval;
nepes_ai 0:529602524696 44 connection.write((dev_addr << 1), &address, true);
nepes_ai 0:529602524696 45 connection.read((dev_addr << 1), &retval, 1);
nepes_ai 0:529602524696 46 return retval;
nepes_ai 0:529602524696 47 }
nepes_ai 0:529602524696 48
nepes_ai 0:529602524696 49 void MPU6050::read(char address, char* data, int length) {
nepes_ai 0:529602524696 50 connection.write((dev_addr << 1), &address, true);
nepes_ai 0:529602524696 51 connection.read((dev_addr << 1), data, length);
nepes_ai 0:529602524696 52 }
nepes_ai 0:529602524696 53
nepes_ai 0:529602524696 54 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 55 // CLKSEL[2:0] = (0x6B)PWR_MGMT_1[2:0]
nepes_ai 0:529602524696 56 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 57 void MPU6050::setClockSource(uint8_t source) {
nepes_ai 0:529602524696 58 char read_val, write_val;
nepes_ai 0:529602524696 59
nepes_ai 0:529602524696 60 read_val = this->read(MPU6050_PWR_MGMT_1_REG);
nepes_ai 0:529602524696 61 write_val = (read_val & 0xF8) | (source & 0x07);
nepes_ai 0:529602524696 62
nepes_ai 0:529602524696 63 this->write(MPU6050_PWR_MGMT_1_REG, write_val);
nepes_ai 0:529602524696 64 }
nepes_ai 0:529602524696 65
nepes_ai 0:529602524696 66 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 67 // SLEEP = (0x6B)PWR_MGMT_1[6]
nepes_ai 0:529602524696 68 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 69 void MPU6050::setSleepEnable(bool enabled) {
nepes_ai 0:529602524696 70 char read_val, write_val;
nepes_ai 0:529602524696 71
nepes_ai 0:529602524696 72 read_val = this->read(MPU6050_PWR_MGMT_1_REG);
nepes_ai 0:529602524696 73 if (enabled)
nepes_ai 0:529602524696 74 write_val = read_val | 0x40;
nepes_ai 0:529602524696 75 else
nepes_ai 0:529602524696 76 write_val = read_val & 0xBF;
nepes_ai 0:529602524696 77
nepes_ai 0:529602524696 78 this->write(MPU6050_PWR_MGMT_1_REG, write_val);
nepes_ai 0:529602524696 79 }
nepes_ai 0:529602524696 80
nepes_ai 0:529602524696 81 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 82 // WHO_AM_I[5:0] = (0x75)WHO_AM_I[6:1]
nepes_ai 0:529602524696 83 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 84 uint8_t MPU6050::getDeviceID() {
nepes_ai 0:529602524696 85 return (this->read(MPU6050_WHO_AM_I_REG));
nepes_ai 0:529602524696 86 }
nepes_ai 0:529602524696 87
nepes_ai 0:529602524696 88 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 89 // FS_SEL[1:0] = (0x1B)GYRO_CONFIG[4:3]
nepes_ai 0:529602524696 90 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 91 void MPU6050::setFullScaleGyroRange(int8_t range) {
nepes_ai 0:529602524696 92 char read_val, write_val;
nepes_ai 0:529602524696 93
nepes_ai 0:529602524696 94 read_val = this->read(MPU6050_GYRO_CONFIG_REG);
nepes_ai 0:529602524696 95 write_val = (read_val & 0xE7) | ((range << 3) & 0x18);
nepes_ai 0:529602524696 96
nepes_ai 0:529602524696 97 this->write(MPU6050_GYRO_CONFIG_REG, write_val);
nepes_ai 0:529602524696 98 }
nepes_ai 0:529602524696 99
nepes_ai 0:529602524696 100 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 101 // AFS_SEL[1:0] = (0x1C)ACCEL_CONFIG[4:3]
nepes_ai 0:529602524696 102 ///////////////////////////////////////////////////////////
nepes_ai 0:529602524696 103 void MPU6050::setFullScaleAccelRange(int8_t range) {
nepes_ai 0:529602524696 104 char read_val, write_val;
nepes_ai 0:529602524696 105
nepes_ai 0:529602524696 106 read_val = this->read(MPU6050_ACCEL_CONFIG_REG);
nepes_ai 0:529602524696 107 write_val = (read_val & 0xE7) | ((range << 3) & 0x18);
nepes_ai 0:529602524696 108
nepes_ai 0:529602524696 109 this->write(MPU6050_ACCEL_CONFIG_REG, write_val);
nepes_ai 0:529602524696 110 }
nepes_ai 0:529602524696 111
nepes_ai 0:529602524696 112 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 113 // XA_OFFSET[14:0] = (0x06)XA_OFFSET_H[7:0] + (0x07)XA_OFFSET_L[7:1]
nepes_ai 0:529602524696 114 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 115 void MPU6050::setXAccelOffset(int16_t offset) {
nepes_ai 0:529602524696 116 this->writeWord(MPU6050_XA_OFFS_H_REG, offset);
nepes_ai 0:529602524696 117 }
nepes_ai 0:529602524696 118
nepes_ai 0:529602524696 119 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 120 // YA_OFFSET[14:0] = (0x08)YA_OFFSET_H[7:0] + (0x09)YA_OFFSET_L[7:1]
nepes_ai 0:529602524696 121 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 122 void MPU6050::setYAccelOffset(int16_t offset) {
nepes_ai 0:529602524696 123 this->writeWord(MPU6050_YA_OFFS_H_REG, offset);
nepes_ai 0:529602524696 124 }
nepes_ai 0:529602524696 125
nepes_ai 0:529602524696 126 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 127 // ZA_OFFSET[14:0] = (0x0A)ZA_OFFSET_H[7:0] + (0x0B)ZA_OFFSET_L[7:1]
nepes_ai 0:529602524696 128 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 129 void MPU6050::setZAccelOffset(int16_t offset) {
nepes_ai 0:529602524696 130 this->writeWord(MPU6050_ZA_OFFS_H_REG, offset);
nepes_ai 0:529602524696 131 }
nepes_ai 0:529602524696 132
nepes_ai 0:529602524696 133 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 134 // XG_OFFSET[15:0] = (0x13)XG_OFFSET_H[7:0] + (0x14)XG_OFFSET_L[7:0]
nepes_ai 0:529602524696 135 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 136 void MPU6050::setXGyroOffset(int16_t offset) {
nepes_ai 0:529602524696 137 this->writeWord(MPU6050_XG_OFFS_USRH_REG, offset);
nepes_ai 0:529602524696 138 }
nepes_ai 0:529602524696 139
nepes_ai 0:529602524696 140 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 141 // YG_OFFSET[15:0] = (0x15)YG_OFFSET_H[7:0] + (0x16)YG_OFFSET_L[7:0]
nepes_ai 0:529602524696 142 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 143 void MPU6050::setYGyroOffset(int16_t offset) {
nepes_ai 0:529602524696 144 this->writeWord(MPU6050_YG_OFFS_USRH_REG, offset);
nepes_ai 0:529602524696 145 }
nepes_ai 0:529602524696 146
nepes_ai 0:529602524696 147 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 148 // ZG_OFFSET[15:0] = (0x17)ZG_OFFSET_H[7:0] + (0x18)ZG_OFFSET_L[7:0]
nepes_ai 0:529602524696 149 //////////////////////////////////////////////////////////////////////
nepes_ai 0:529602524696 150 void MPU6050::setZGyroOffset(int16_t offset) {
nepes_ai 0:529602524696 151 this->writeWord(MPU6050_ZG_OFFS_USRH_REG, offset);
nepes_ai 0:529602524696 152 }
nepes_ai 0:529602524696 153
nepes_ai 0:529602524696 154 void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
nepes_ai 0:529602524696 155 char buffer[14];
nepes_ai 0:529602524696 156 this->read(MPU6050_ACCEL_XOUT_H_REG, buffer, 14);
nepes_ai 0:529602524696 157 *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
nepes_ai 0:529602524696 158 *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
nepes_ai 0:529602524696 159 *az = (((int16_t)buffer[4]) << 8) | buffer[5];
nepes_ai 0:529602524696 160 *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
nepes_ai 0:529602524696 161 *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
nepes_ai 0:529602524696 162 *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
nepes_ai 0:529602524696 163 }