Momo-Medical / MPU6050

test

Dependents:   Nucleo_printf Nucleo-transfer

Fork of MPU6050 by Momo Medical

MPU6050.cpp@5:dc92d3573d50, 2017-11-30 (annotated)

Committer:
Ishy
Date:
Thu Nov 30 08:59:55 2017 +0000
Revision:
5:dc92d3573d50
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ishy 5:dc92d3573d50 1 /**
Ishy 5:dc92d3573d50 2 * Includes
Ishy 5:dc92d3573d50 3 */
Ishy 5:dc92d3573d50 4 #include "MPU6050.h"
Ishy 5:dc92d3573d50 5
Ishy 5:dc92d3573d50 6 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
Ishy 5:dc92d3573d50 7 this->setSleepMode(false);
Ishy 5:dc92d3573d50 8
Ishy 5:dc92d3573d50 9 //Initializations:
Ishy 5:dc92d3573d50 10 currentGyroRange = 0;
Ishy 5:dc92d3573d50 11 currentAcceleroRange=0;
Ishy 5:dc92d3573d50 12 }
Ishy 5:dc92d3573d50 13
Ishy 5:dc92d3573d50 14 //--------------------------------------------------
Ishy 5:dc92d3573d50 15 //-------------------General------------------------
Ishy 5:dc92d3573d50 16 //--------------------------------------------------
Ishy 5:dc92d3573d50 17
Ishy 5:dc92d3573d50 18 void MPU6050::write(char address, char data) {
Ishy 5:dc92d3573d50 19 char temp[2];
Ishy 5:dc92d3573d50 20 temp[0]=address;
Ishy 5:dc92d3573d50 21 temp[1]=data;
Ishy 5:dc92d3573d50 22
Ishy 5:dc92d3573d50 23 connection.write(MPU6050_ADDRESS * 2,temp,2);
Ishy 5:dc92d3573d50 24 }
Ishy 5:dc92d3573d50 25
Ishy 5:dc92d3573d50 26 char MPU6050::read(char address) {
Ishy 5:dc92d3573d50 27 char retval;
Ishy 5:dc92d3573d50 28 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Ishy 5:dc92d3573d50 29 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
Ishy 5:dc92d3573d50 30 return retval;
Ishy 5:dc92d3573d50 31 }
Ishy 5:dc92d3573d50 32
Ishy 5:dc92d3573d50 33 void MPU6050::read(char address, char *data, int length) {
Ishy 5:dc92d3573d50 34 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Ishy 5:dc92d3573d50 35 connection.read(MPU6050_ADDRESS * 2, data, length);
Ishy 5:dc92d3573d50 36 }
Ishy 5:dc92d3573d50 37
Ishy 5:dc92d3573d50 38 void MPU6050::setSleepMode(bool state) {
Ishy 5:dc92d3573d50 39 char temp;
Ishy 5:dc92d3573d50 40 temp = this->read(MPU6050_PWR_MGMT_1_REG);
Ishy 5:dc92d3573d50 41 if (state == true)
Ishy 5:dc92d3573d50 42 temp |= 1<<MPU6050_SLP_BIT;
Ishy 5:dc92d3573d50 43 if (state == false)
Ishy 5:dc92d3573d50 44 temp &= ~(1<<MPU6050_SLP_BIT);
Ishy 5:dc92d3573d50 45 this->write(MPU6050_PWR_MGMT_1_REG, temp);
Ishy 5:dc92d3573d50 46 }
Ishy 5:dc92d3573d50 47
Ishy 5:dc92d3573d50 48 bool MPU6050::testConnection( void ) {
Ishy 5:dc92d3573d50 49 char temp;
Ishy 5:dc92d3573d50 50 temp = this->read(MPU6050_WHO_AM_I_REG);
Ishy 5:dc92d3573d50 51 return (temp == (MPU6050_ADDRESS & 0xFE));
Ishy 5:dc92d3573d50 52 }
Ishy 5:dc92d3573d50 53
Ishy 5:dc92d3573d50 54 void MPU6050::setBW(char BW) {
Ishy 5:dc92d3573d50 55 char temp;
Ishy 5:dc92d3573d50 56 BW=BW & 0x07;
Ishy 5:dc92d3573d50 57 temp = this->read(MPU6050_CONFIG_REG);
Ishy 5:dc92d3573d50 58 temp &= 0xF8;
Ishy 5:dc92d3573d50 59 temp = temp + BW;
Ishy 5:dc92d3573d50 60 this->write(MPU6050_CONFIG_REG, temp);
Ishy 5:dc92d3573d50 61 }
Ishy 5:dc92d3573d50 62
Ishy 5:dc92d3573d50 63 void MPU6050::setI2CBypass(bool state) {
Ishy 5:dc92d3573d50 64 char temp;
Ishy 5:dc92d3573d50 65 temp = this->read(MPU6050_INT_PIN_CFG);
Ishy 5:dc92d3573d50 66 if (state == true)
Ishy 5:dc92d3573d50 67 temp |= 1<<MPU6050_BYPASS_BIT;
Ishy 5:dc92d3573d50 68 if (state == false)
Ishy 5:dc92d3573d50 69 temp &= ~(1<<MPU6050_BYPASS_BIT);
Ishy 5:dc92d3573d50 70 this->write(MPU6050_INT_PIN_CFG, temp);
Ishy 5:dc92d3573d50 71 }
Ishy 5:dc92d3573d50 72
Ishy 5:dc92d3573d50 73 //--------------------------------------------------
Ishy 5:dc92d3573d50 74 //----------------Accelerometer---------------------
Ishy 5:dc92d3573d50 75 //--------------------------------------------------
Ishy 5:dc92d3573d50 76
Ishy 5:dc92d3573d50 77 void MPU6050::setAcceleroRange( char range ) {
Ishy 5:dc92d3573d50 78 char temp;
Ishy 5:dc92d3573d50 79 range = range & 0x03;
Ishy 5:dc92d3573d50 80 currentAcceleroRange = range;
Ishy 5:dc92d3573d50 81
Ishy 5:dc92d3573d50 82 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
Ishy 5:dc92d3573d50 83 temp &= ~(3<<3);
Ishy 5:dc92d3573d50 84 temp = temp + (range<<3);
Ishy 5:dc92d3573d50 85 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
Ishy 5:dc92d3573d50 86 }
Ishy 5:dc92d3573d50 87
Ishy 5:dc92d3573d50 88 int MPU6050::getAcceleroRawX( void ) {
Ishy 5:dc92d3573d50 89 short retval;
Ishy 5:dc92d3573d50 90 char data[2];
Ishy 5:dc92d3573d50 91 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
Ishy 5:dc92d3573d50 92 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 93 return (int)retval;
Ishy 5:dc92d3573d50 94 }
Ishy 5:dc92d3573d50 95
Ishy 5:dc92d3573d50 96 int MPU6050::getAcceleroRawY( void ) {
Ishy 5:dc92d3573d50 97 short retval;
Ishy 5:dc92d3573d50 98 char data[2];
Ishy 5:dc92d3573d50 99 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
Ishy 5:dc92d3573d50 100 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 101 return (int)retval;
Ishy 5:dc92d3573d50 102 }
Ishy 5:dc92d3573d50 103
Ishy 5:dc92d3573d50 104 int MPU6050::getAcceleroRawZ( void ) {
Ishy 5:dc92d3573d50 105 short retval;
Ishy 5:dc92d3573d50 106 char data[2];
Ishy 5:dc92d3573d50 107 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
Ishy 5:dc92d3573d50 108 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 109 return (int)retval;
Ishy 5:dc92d3573d50 110 }
Ishy 5:dc92d3573d50 111
Ishy 5:dc92d3573d50 112 void MPU6050::getAcceleroRaw( int *data ) {
Ishy 5:dc92d3573d50 113 char temp[6];
Ishy 5:dc92d3573d50 114 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
Ishy 5:dc92d3573d50 115 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Ishy 5:dc92d3573d50 116 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Ishy 5:dc92d3573d50 117 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Ishy 5:dc92d3573d50 118 }
Ishy 5:dc92d3573d50 119
Ishy 5:dc92d3573d50 120 void MPU6050::getAccelero( float *data ) {
Ishy 5:dc92d3573d50 121 int temp[3];
Ishy 5:dc92d3573d50 122 this->getAcceleroRaw(temp);
Ishy 5:dc92d3573d50 123 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
Ishy 5:dc92d3573d50 124 data[0]=(float)temp[0] / 16384.0 * 9.81;
Ishy 5:dc92d3573d50 125 data[1]=(float)temp[1] / 16384.0 * 9.81;
Ishy 5:dc92d3573d50 126 data[2]=(float)temp[2] / 16384.0 * 9.81;
Ishy 5:dc92d3573d50 127 }
Ishy 5:dc92d3573d50 128 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
Ishy 5:dc92d3573d50 129 data[0]=(float)temp[0] / 8192.0 * 9.81;
Ishy 5:dc92d3573d50 130 data[1]=(float)temp[1] / 8192.0 * 9.81;
Ishy 5:dc92d3573d50 131 data[2]=(float)temp[2] / 8192.0 * 9.81;
Ishy 5:dc92d3573d50 132 }
Ishy 5:dc92d3573d50 133 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
Ishy 5:dc92d3573d50 134 data[0]=(float)temp[0] / 4096.0 * 9.81;
Ishy 5:dc92d3573d50 135 data[1]=(float)temp[1] / 4096.0 * 9.81;
Ishy 5:dc92d3573d50 136 data[2]=(float)temp[2] / 4096.0 * 9.81;
Ishy 5:dc92d3573d50 137 }
Ishy 5:dc92d3573d50 138 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
Ishy 5:dc92d3573d50 139 data[0]=(float)temp[0] / 2048.0 * 9.81;
Ishy 5:dc92d3573d50 140 data[1]=(float)temp[1] / 2048.0 * 9.81;
Ishy 5:dc92d3573d50 141 data[2]=(float)temp[2] / 2048.0 * 9.81;
Ishy 5:dc92d3573d50 142 }
Ishy 5:dc92d3573d50 143
Ishy 5:dc92d3573d50 144 #ifdef DOUBLE_ACCELERO
Ishy 5:dc92d3573d50 145 data[0]*=2;
Ishy 5:dc92d3573d50 146 data[1]*=2;
Ishy 5:dc92d3573d50 147 data[2]*=2;
Ishy 5:dc92d3573d50 148 #endif
Ishy 5:dc92d3573d50 149 }
Ishy 5:dc92d3573d50 150
Ishy 5:dc92d3573d50 151 //--------------------------------------------------
Ishy 5:dc92d3573d50 152 //------------------Gyroscope-----------------------
Ishy 5:dc92d3573d50 153 //--------------------------------------------------
Ishy 5:dc92d3573d50 154 void MPU6050::setGyroRange( char range ) {
Ishy 5:dc92d3573d50 155 char temp;
Ishy 5:dc92d3573d50 156 currentGyroRange = range;
Ishy 5:dc92d3573d50 157 range = range & 0x03;
Ishy 5:dc92d3573d50 158 temp = this->read(MPU6050_GYRO_CONFIG_REG);
Ishy 5:dc92d3573d50 159 temp &= ~(3<<3);
Ishy 5:dc92d3573d50 160 temp = temp + range<<3;
Ishy 5:dc92d3573d50 161 this->write(MPU6050_GYRO_CONFIG_REG, temp);
Ishy 5:dc92d3573d50 162 }
Ishy 5:dc92d3573d50 163
Ishy 5:dc92d3573d50 164 int MPU6050::getGyroRawX( void ) {
Ishy 5:dc92d3573d50 165 short retval;
Ishy 5:dc92d3573d50 166 char data[2];
Ishy 5:dc92d3573d50 167 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
Ishy 5:dc92d3573d50 168 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 169 return (int)retval;
Ishy 5:dc92d3573d50 170 }
Ishy 5:dc92d3573d50 171
Ishy 5:dc92d3573d50 172 int MPU6050::getGyroRawY( void ) {
Ishy 5:dc92d3573d50 173 short retval;
Ishy 5:dc92d3573d50 174 char data[2];
Ishy 5:dc92d3573d50 175 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
Ishy 5:dc92d3573d50 176 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 177 return (int)retval;
Ishy 5:dc92d3573d50 178 }
Ishy 5:dc92d3573d50 179
Ishy 5:dc92d3573d50 180 int MPU6050::getGyroRawZ( void ) {
Ishy 5:dc92d3573d50 181 short retval;
Ishy 5:dc92d3573d50 182 char data[2];
Ishy 5:dc92d3573d50 183 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
Ishy 5:dc92d3573d50 184 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 185 return (int)retval;
Ishy 5:dc92d3573d50 186 }
Ishy 5:dc92d3573d50 187
Ishy 5:dc92d3573d50 188 void MPU6050::getGyroRaw( int *data ) {
Ishy 5:dc92d3573d50 189 char temp[6];
Ishy 5:dc92d3573d50 190 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
Ishy 5:dc92d3573d50 191 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Ishy 5:dc92d3573d50 192 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Ishy 5:dc92d3573d50 193 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Ishy 5:dc92d3573d50 194 }
Ishy 5:dc92d3573d50 195
Ishy 5:dc92d3573d50 196 void MPU6050::getGyro( float *data ) {
Ishy 5:dc92d3573d50 197 int temp[3];
Ishy 5:dc92d3573d50 198 this->getGyroRaw(temp);
Ishy 5:dc92d3573d50 199 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
Ishy 5:dc92d3573d50 200 data[0]=(float)temp[0] / 7505.7;
Ishy 5:dc92d3573d50 201 data[1]=(float)temp[1] / 7505.7;
Ishy 5:dc92d3573d50 202 data[2]=(float)temp[2] / 7505.7;
Ishy 5:dc92d3573d50 203 }
Ishy 5:dc92d3573d50 204 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
Ishy 5:dc92d3573d50 205 data[0]=(float)temp[0] / 3752.9;
Ishy 5:dc92d3573d50 206 data[1]=(float)temp[1] / 3752.9;
Ishy 5:dc92d3573d50 207 data[2]=(float)temp[2] / 3752.9;
Ishy 5:dc92d3573d50 208 }
Ishy 5:dc92d3573d50 209 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
Ishy 5:dc92d3573d50 210 data[0]=(float)temp[0] / 1879.3;;
Ishy 5:dc92d3573d50 211 data[1]=(float)temp[1] / 1879.3;
Ishy 5:dc92d3573d50 212 data[2]=(float)temp[2] / 1879.3;
Ishy 5:dc92d3573d50 213 }
Ishy 5:dc92d3573d50 214 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
Ishy 5:dc92d3573d50 215 data[0]=(float)temp[0] / 939.7;
Ishy 5:dc92d3573d50 216 data[1]=(float)temp[1] / 939.7;
Ishy 5:dc92d3573d50 217 data[2]=(float)temp[2] / 939.7;
Ishy 5:dc92d3573d50 218 }
Ishy 5:dc92d3573d50 219 }
Ishy 5:dc92d3573d50 220 //--------------------------------------------------
Ishy 5:dc92d3573d50 221 //-------------------Temperature--------------------
Ishy 5:dc92d3573d50 222 //--------------------------------------------------
Ishy 5:dc92d3573d50 223 int MPU6050::getTempRaw( void ) {
Ishy 5:dc92d3573d50 224 short retval;
Ishy 5:dc92d3573d50 225 char data[2];
Ishy 5:dc92d3573d50 226 this->read(MPU6050_TEMP_H_REG, data, 2);
Ishy 5:dc92d3573d50 227 retval = (data[0]<<8) + data[1];
Ishy 5:dc92d3573d50 228 return (int)retval;
Ishy 5:dc92d3573d50 229 }
Ishy 5:dc92d3573d50 230
Ishy 5:dc92d3573d50 231 float MPU6050::getTemp( void ) {
Ishy 5:dc92d3573d50 232 float retval;
Ishy 5:dc92d3573d50 233 retval=(float)this->getTempRaw();
Ishy 5:dc92d3573d50 234 retval=(retval+521.0)/340.0+35.0;
Ishy 5:dc92d3573d50 235 return retval;
Ishy 5:dc92d3573d50 236 }
Ishy 5:dc92d3573d50 237