Montvydas Klumbys / MPU6050

Thanks Erik Olieman for his beta library, that saved me a huge amount of time when getting Raw data from MPU6050 module! I was able to update this library by adding additional functions, which would allow a fast angle calculation by using complementary filter. I will probably be updating this library more soon to add additional functionality or make some changes that would look sensible.

Dependents:   QuadcopterProgram 3DTracking ControlYokutan2017 Gyro ... more

Fork of MPU6050 by Erik -

MPU6050.cpp@5:5873df1e58be, 2015-02-12 (annotated)

Committer:
moklumbys
Date:
Thu Feb 12 20:56:04 2015 +0000
Revision:
5:5873df1e58be
Parent:
4:268d3fcb92ba
Child:
6:502448484f91
Added changes to the original library to allow getting offset values.

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
Sissors 0:6757f7363a9f 6 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
Sissors 0:6757f7363a9f 7 this->setSleepMode(false);
Sissors 0:6757f7363a9f 8
Sissors 0:6757f7363a9f 9 //Initializations:
Sissors 0:6757f7363a9f 10 currentGyroRange = 0;
Sissors 0:6757f7363a9f 11 currentAcceleroRange=0;
Sissors 0:6757f7363a9f 12 }
Sissors 0:6757f7363a9f 13
Sissors 0:6757f7363a9f 14 //--------------------------------------------------
Sissors 0:6757f7363a9f 15 //-------------------General------------------------
Sissors 0:6757f7363a9f 16 //--------------------------------------------------
Sissors 0:6757f7363a9f 17
Sissors 0:6757f7363a9f 18 void MPU6050::write(char address, char data) {
Sissors 0:6757f7363a9f 19 char temp[2];
Sissors 0:6757f7363a9f 20 temp[0]=address;
Sissors 0:6757f7363a9f 21 temp[1]=data;
Sissors 0:6757f7363a9f 22
Sissors 0:6757f7363a9f 23 connection.write(MPU6050_ADDRESS * 2,temp,2);
Sissors 0:6757f7363a9f 24 }
Sissors 0:6757f7363a9f 25
Sissors 0:6757f7363a9f 26 char MPU6050::read(char address) {
Sissors 0:6757f7363a9f 27 char retval;
Sissors 0:6757f7363a9f 28 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Sissors 0:6757f7363a9f 29 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
Sissors 0:6757f7363a9f 30 return retval;
Sissors 0:6757f7363a9f 31 }
Sissors 0:6757f7363a9f 32
Sissors 0:6757f7363a9f 33 void MPU6050::read(char address, char *data, int length) {
Sissors 0:6757f7363a9f 34 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Sissors 0:6757f7363a9f 35 connection.read(MPU6050_ADDRESS * 2, data, length);
Sissors 0:6757f7363a9f 36 }
Sissors 0:6757f7363a9f 37
Sissors 0:6757f7363a9f 38 void MPU6050::setSleepMode(bool state) {
Sissors 0:6757f7363a9f 39 char temp;
Sissors 0:6757f7363a9f 40 temp = this->read(MPU6050_PWR_MGMT_1_REG);
Sissors 0:6757f7363a9f 41 if (state == true)
Sissors 0:6757f7363a9f 42 temp |= 1<<MPU6050_SLP_BIT;
Sissors 0:6757f7363a9f 43 if (state == false)
Sissors 0:6757f7363a9f 44 temp &= ~(1<<MPU6050_SLP_BIT);
Sissors 0:6757f7363a9f 45 this->write(MPU6050_PWR_MGMT_1_REG, temp);
Sissors 0:6757f7363a9f 46 }
Sissors 0:6757f7363a9f 47
Sissors 0:6757f7363a9f 48 bool MPU6050::testConnection( void ) {
Sissors 0:6757f7363a9f 49 char temp;
Sissors 0:6757f7363a9f 50 temp = this->read(MPU6050_WHO_AM_I_REG);
Sissors 0:6757f7363a9f 51 return (temp == (MPU6050_ADDRESS & 0xFE));
Sissors 0:6757f7363a9f 52 }
Sissors 0:6757f7363a9f 53
Sissors 0:6757f7363a9f 54 void MPU6050::setBW(char BW) {
Sissors 0:6757f7363a9f 55 char temp;
Sissors 0:6757f7363a9f 56 BW=BW & 0x07;
Sissors 0:6757f7363a9f 57 temp = this->read(MPU6050_CONFIG_REG);
Sissors 0:6757f7363a9f 58 temp &= 0xF8;
Sissors 0:6757f7363a9f 59 temp = temp + BW;
Sissors 0:6757f7363a9f 60 this->write(MPU6050_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 61 }
Sissors 0:6757f7363a9f 62
Sissors 0:6757f7363a9f 63 void MPU6050::setI2CBypass(bool state) {
Sissors 0:6757f7363a9f 64 char temp;
Sissors 0:6757f7363a9f 65 temp = this->read(MPU6050_INT_PIN_CFG);
Sissors 0:6757f7363a9f 66 if (state == true)
Sissors 0:6757f7363a9f 67 temp |= 1<<MPU6050_BYPASS_BIT;
Sissors 0:6757f7363a9f 68 if (state == false)
Sissors 0:6757f7363a9f 69 temp &= ~(1<<MPU6050_BYPASS_BIT);
Sissors 0:6757f7363a9f 70 this->write(MPU6050_INT_PIN_CFG, temp);
Sissors 0:6757f7363a9f 71 }
Sissors 0:6757f7363a9f 72
Sissors 0:6757f7363a9f 73 //--------------------------------------------------
Sissors 0:6757f7363a9f 74 //----------------Accelerometer---------------------
Sissors 0:6757f7363a9f 75 //--------------------------------------------------
Sissors 0:6757f7363a9f 76
Sissors 0:6757f7363a9f 77 void MPU6050::setAcceleroRange( char range ) {
Sissors 0:6757f7363a9f 78 char temp;
Sissors 0:6757f7363a9f 79 range = range & 0x03;
Sissors 0:6757f7363a9f 80 currentAcceleroRange = range;
Sissors 0:6757f7363a9f 81
Sissors 0:6757f7363a9f 82 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
Sissors 0:6757f7363a9f 83 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 84 temp = temp + (range<<3);
Sissors 0:6757f7363a9f 85 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 86 }
Sissors 0:6757f7363a9f 87
Sissors 0:6757f7363a9f 88 int MPU6050::getAcceleroRawX( void ) {
Sissors 0:6757f7363a9f 89 short retval;
Sissors 0:6757f7363a9f 90 char data[2];
Sissors 0:6757f7363a9f 91 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 92 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 93 return (int)retval;
Sissors 0:6757f7363a9f 94 }
Sissors 0:6757f7363a9f 95
Sissors 0:6757f7363a9f 96 int MPU6050::getAcceleroRawY( void ) {
Sissors 0:6757f7363a9f 97 short retval;
Sissors 0:6757f7363a9f 98 char data[2];
Sissors 0:6757f7363a9f 99 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 100 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 101 return (int)retval;
Sissors 0:6757f7363a9f 102 }
Sissors 0:6757f7363a9f 103
Sissors 0:6757f7363a9f 104 int MPU6050::getAcceleroRawZ( void ) {
Sissors 0:6757f7363a9f 105 short retval;
Sissors 0:6757f7363a9f 106 char data[2];
Sissors 0:6757f7363a9f 107 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 108 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 109 return (int)retval;
Sissors 0:6757f7363a9f 110 }
Sissors 0:6757f7363a9f 111
Sissors 0:6757f7363a9f 112 void MPU6050::getAcceleroRaw( int *data ) {
Sissors 0:6757f7363a9f 113 char temp[6];
Sissors 0:6757f7363a9f 114 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 115 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 116 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 117 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 118 }
Sissors 0:6757f7363a9f 119
Sissors 0:6757f7363a9f 120 void MPU6050::getAccelero( float *data ) {
Sissors 0:6757f7363a9f 121 int temp[3];
Sissors 0:6757f7363a9f 122 this->getAcceleroRaw(temp);
Sissors 0:6757f7363a9f 123 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
Sissors 0:6757f7363a9f 124 data[0]=(float)temp[0] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 125 data[1]=(float)temp[1] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 126 data[2]=(float)temp[2] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 127 }
Sissors 0:6757f7363a9f 128 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
Sissors 0:6757f7363a9f 129 data[0]=(float)temp[0] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 130 data[1]=(float)temp[1] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 131 data[2]=(float)temp[2] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 132 }
Sissors 0:6757f7363a9f 133 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
Sissors 0:6757f7363a9f 134 data[0]=(float)temp[0] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 135 data[1]=(float)temp[1] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 136 data[2]=(float)temp[2] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 137 }
Sissors 0:6757f7363a9f 138 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
Sissors 0:6757f7363a9f 139 data[0]=(float)temp[0] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 140 data[1]=(float)temp[1] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 141 data[2]=(float)temp[2] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 142 }
Sissors 0:6757f7363a9f 143
Sissors 0:6757f7363a9f 144 #ifdef DOUBLE_ACCELERO
Sissors 0:6757f7363a9f 145 data[0]*=2;
Sissors 0:6757f7363a9f 146 data[1]*=2;
Sissors 0:6757f7363a9f 147 data[2]*=2;
Sissors 0:6757f7363a9f 148 #endif
Sissors 0:6757f7363a9f 149 }
Sissors 0:6757f7363a9f 150
moklumbys 4:268d3fcb92ba 151 //function for getting angles from accelerometer
moklumbys 4:268d3fcb92ba 152 void MPU6050::getAcceleroAngle( float *data ) {
moklumbys 4:268d3fcb92ba 153 int temp[3];
moklumbys 4:268d3fcb92ba 154 this->getAccelero(temp);
moklumbys 4:268d3fcb92ba 155
moklumbys 4:268d3fcb92ba 156 data[X_AXIS] = -1*atan (-1*temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
moklumbys 4:268d3fcb92ba 157 data[Y_AXIS] = -1*atan (temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
moklumbys 4:268d3fcb92ba 158 data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //not sure what is this one :D
moklumbys 4:268d3fcb92ba 159
moklumbys 4:268d3fcb92ba 160 }
moklumbys 5:5873df1e58be 161 void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
moklumbys 5:5873df1e58be 162 float gyro[3];
moklumbys 5:5873df1e58be 163 float accAngle[3];
moklumbys 5:5873df1e58be 164
moklumbys 5:5873df1e58be 165 for (int i = 0; i < 3; i++) //initialise to 0.0 offsets
moklumbys 5:5873df1e58be 166 {
moklumbys 5:5873df1e58be 167 accOffset[i] = 0.0;
moklumbys 5:5873df1e58be 168 gyroOffset[i] = 0.0;
moklumbys 5:5873df1e58be 169 }
moklumbys 5:5873df1e58be 170
moklumbys 5:5873df1e58be 171 for (int i = 0; i < sampleSize; i++)
moklumbys 5:5873df1e58be 172 {
moklumbys 5:5873df1e58be 173 mpu.getGyro(gyro); //take real life measurements
moklumbys 5:5873df1e58be 174 mpu.getAcceleroAngle (accAngle);
moklumbys 5:5873df1e58be 175
moklumbys 5:5873df1e58be 176 for (int j = 0; j < 3; j++)
moklumbys 5:5873df1e58be 177 {
moklumbys 5:5873df1e58be 178 *(accOffset+j) += accAngle[j]/sampleSize; //average measurements
moklumbys 5:5873df1e58be 179 *(gyroOffset+j) += gyro[j]/sampleSize;
moklumbys 5:5873df1e58be 180 }
moklumbys 5:5873df1e58be 181 wait (0.01); //wait between each reading for accuracy (maybe will not need later on) ****))#$(#@)$@#)(#@(%)@#(%@)(#%#@$--------------
moklumbys 5:5873df1e58be 182 }
moklumbys 5:5873df1e58be 183 }
Sissors 0:6757f7363a9f 184 //--------------------------------------------------
Sissors 0:6757f7363a9f 185 //------------------Gyroscope-----------------------
Sissors 0:6757f7363a9f 186 //--------------------------------------------------
Sissors 0:6757f7363a9f 187 void MPU6050::setGyroRange( char range ) {
Sissors 0:6757f7363a9f 188 char temp;
Sissors 0:6757f7363a9f 189 currentGyroRange = range;
Sissors 0:6757f7363a9f 190 range = range & 0x03;
Sissors 0:6757f7363a9f 191 temp = this->read(MPU6050_GYRO_CONFIG_REG);
Sissors 0:6757f7363a9f 192 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 193 temp = temp + range<<3;
Sissors 0:6757f7363a9f 194 this->write(MPU6050_GYRO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 195 }
Sissors 0:6757f7363a9f 196
Sissors 0:6757f7363a9f 197 int MPU6050::getGyroRawX( void ) {
Sissors 0:6757f7363a9f 198 short retval;
Sissors 0:6757f7363a9f 199 char data[2];
Sissors 0:6757f7363a9f 200 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 201 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 202 return (int)retval;
Sissors 0:6757f7363a9f 203 }
Sissors 0:6757f7363a9f 204
Sissors 0:6757f7363a9f 205 int MPU6050::getGyroRawY( void ) {
Sissors 0:6757f7363a9f 206 short retval;
Sissors 0:6757f7363a9f 207 char data[2];
Sissors 0:6757f7363a9f 208 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 209 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 210 return (int)retval;
Sissors 0:6757f7363a9f 211 }
Sissors 0:6757f7363a9f 212
Sissors 0:6757f7363a9f 213 int MPU6050::getGyroRawZ( void ) {
Sissors 0:6757f7363a9f 214 short retval;
Sissors 0:6757f7363a9f 215 char data[2];
Sissors 0:6757f7363a9f 216 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 217 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 218 return (int)retval;
Sissors 0:6757f7363a9f 219 }
Sissors 0:6757f7363a9f 220
Sissors 0:6757f7363a9f 221 void MPU6050::getGyroRaw( int *data ) {
Sissors 0:6757f7363a9f 222 char temp[6];
Sissors 0:6757f7363a9f 223 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 224 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 225 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 226 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 227 }
Sissors 0:6757f7363a9f 228
Sissors 0:6757f7363a9f 229 void MPU6050::getGyro( float *data ) {
Sissors 0:6757f7363a9f 230 int temp[3];
Sissors 0:6757f7363a9f 231 this->getGyroRaw(temp);
Sissors 1:a3366f09e95c 232 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
moklumbys 3:187152513f8d 233 data[0]=(float)temp[0] / 301.0;
moklumbys 3:187152513f8d 234 data[1]=(float)temp[1] / 301.0;
moklumbys 3:187152513f8d 235 data[2]=(float)temp[2] / 301.0;
moklumbys 3:187152513f8d 236 } //7505.5
Sissors 1:a3366f09e95c 237 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
Sissors 0:6757f7363a9f 238 data[0]=(float)temp[0] / 3752.9;
Sissors 0:6757f7363a9f 239 data[1]=(float)temp[1] / 3752.9;
Sissors 0:6757f7363a9f 240 data[2]=(float)temp[2] / 3752.9;
Sissors 0:6757f7363a9f 241 }
Sissors 1:a3366f09e95c 242 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
Sissors 0:6757f7363a9f 243 data[0]=(float)temp[0] / 1879.3;;
Sissors 0:6757f7363a9f 244 data[1]=(float)temp[1] / 1879.3;
Sissors 0:6757f7363a9f 245 data[2]=(float)temp[2] / 1879.3;
Sissors 0:6757f7363a9f 246 }
Sissors 1:a3366f09e95c 247 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
Sissors 0:6757f7363a9f 248 data[0]=(float)temp[0] / 939.7;
Sissors 0:6757f7363a9f 249 data[1]=(float)temp[1] / 939.7;
Sissors 0:6757f7363a9f 250 data[2]=(float)temp[2] / 939.7;
Sissors 0:6757f7363a9f 251 }
Sissors 0:6757f7363a9f 252 }
Sissors 0:6757f7363a9f 253 //--------------------------------------------------
Sissors 0:6757f7363a9f 254 //-------------------Temperature--------------------
Sissors 0:6757f7363a9f 255 //--------------------------------------------------
Sissors 0:6757f7363a9f 256 int MPU6050::getTempRaw( void ) {
Sissors 0:6757f7363a9f 257 short retval;
Sissors 0:6757f7363a9f 258 char data[2];
Sissors 0:6757f7363a9f 259 this->read(MPU6050_TEMP_H_REG, data, 2);
Sissors 0:6757f7363a9f 260 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 261 return (int)retval;
Sissors 0:6757f7363a9f 262 }
Sissors 0:6757f7363a9f 263
Sissors 0:6757f7363a9f 264 float MPU6050::getTemp( void ) {
Sissors 0:6757f7363a9f 265 float retval;
Sissors 0:6757f7363a9f 266 retval=(float)this->getTempRaw();
Sissors 0:6757f7363a9f 267 retval=(retval+521.0)/340.0+35.0;
Sissors 0:6757f7363a9f 268 return retval;
Sissors 0:6757f7363a9f 269 }
Sissors 0:6757f7363a9f 270