Víctor Najarro / MPU6050

library of measure the sensor MPU6050 by I2C

Dependents:   1-K64F_with_5_acel

Fork of MPU6050 by Erik -

MPU6050.cpp@4:52b05c0e09a6, 2018-01-18 (annotated)

Committer:
vinajarr
Date:
Thu Jan 18 07:52:45 2018 +0000
Revision:
4:52b05c0e09a6
Parent:
3:6d0ea7c8c5c4 
implement interrupt

Who changed what in which revision?

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