Alex S / MPU6050

Added support for different sensor addresses

Fork of MPU6050 by wisnu pamungkas

Only the MPU6050 constructor was changed. MPU6050 was added as a class member instead of being #defined

MPU6050.cpp@1:527a0db5f64b, 2015-05-13 (annotated)

Committer:
nirvana77
Date:
Wed May 13 21:19:07 2015 +0000
Revision:
1:527a0db5f64b
Parent:
0:0e23b7f6dccd 
added support for two sensors

Who changed what in which revision?

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