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@7:56e591a74939, 2015-02-13 (annotated)

Committer:
moklumbys
Date:
Fri Feb 13 01:04:17 2015 +0000
Revision:
7:56e591a74939
Parent:
6:502448484f91
Child:
8:b1570b99df9e
So, added the final function to the library, which would allow to compute angle, knowing the time difference between each calculation.

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 6:502448484f91 153 float 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 6:502448484f91 173 this->getGyro(gyro); //take real life measurements
moklumbys 6:502448484f91 174 this->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 }
moklumbys 7:56e591a74939 184
moklumbys 7:56e591a74939 185 void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime)
moklumbys 7:56e591a74939 186 {
moklumbys 7:56e591a74939 187 float gyro[3];
moklumbys 7:56e591a74939 188 float accAngle[3];
moklumbys 7:56e591a74939 189
moklumbys 7:56e591a74939 190 this->getGyro(gyro);
moklumbys 7:56e591a74939 191 this->getAcceleroAngle(accAngle);
moklumbys 7:56e591a74939 192
moklumbys 7:56e591a74939 193 for (int i = 0; i < 3; i++)
moklumbys 7:56e591a74939 194 {
moklumbys 7:56e591a74939 195 gyro[i] -= gyroOffset[i];
moklumbys 7:56e591a74939 196 accAngle[i] -= accOffset[i];
moklumbys 7:56e591a74939 197 }
moklumbys 7:56e591a74939 198
moklumbys 7:56e591a74939 199 angle[X_AXIS] = ALPHA * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[X_AXIS]; //apply filter on both reading to get all angles
moklumbys 7:56e591a74939 200 angle[Y_AXIS] = ALPHA * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Y_AXIS];
moklumbys 7:56e591a74939 201 //angle[Z_AXIS] = ALPHA * (angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Z_AXIS];
moklumbys 7:56e591a74939 202 angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime); //this is Yaw hopefully :D
moklumbys 7:56e591a74939 203 //Y = atan2(rawY,rawZ) * 180 / PI; //This spits out values between -180 to 180 (360 degrees)
moklumbys 7:56e591a74939 204 }
Sissors 0:6757f7363a9f 205 //--------------------------------------------------
Sissors 0:6757f7363a9f 206 //------------------Gyroscope-----------------------
Sissors 0:6757f7363a9f 207 //--------------------------------------------------
Sissors 0:6757f7363a9f 208 void MPU6050::setGyroRange( char range ) {
Sissors 0:6757f7363a9f 209 char temp;
Sissors 0:6757f7363a9f 210 currentGyroRange = range;
Sissors 0:6757f7363a9f 211 range = range & 0x03;
Sissors 0:6757f7363a9f 212 temp = this->read(MPU6050_GYRO_CONFIG_REG);
Sissors 0:6757f7363a9f 213 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 214 temp = temp + range<<3;
Sissors 0:6757f7363a9f 215 this->write(MPU6050_GYRO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 216 }
Sissors 0:6757f7363a9f 217
Sissors 0:6757f7363a9f 218 int MPU6050::getGyroRawX( void ) {
Sissors 0:6757f7363a9f 219 short retval;
Sissors 0:6757f7363a9f 220 char data[2];
Sissors 0:6757f7363a9f 221 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 222 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 223 return (int)retval;
Sissors 0:6757f7363a9f 224 }
Sissors 0:6757f7363a9f 225
Sissors 0:6757f7363a9f 226 int MPU6050::getGyroRawY( void ) {
Sissors 0:6757f7363a9f 227 short retval;
Sissors 0:6757f7363a9f 228 char data[2];
Sissors 0:6757f7363a9f 229 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 230 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 231 return (int)retval;
Sissors 0:6757f7363a9f 232 }
Sissors 0:6757f7363a9f 233
Sissors 0:6757f7363a9f 234 int MPU6050::getGyroRawZ( void ) {
Sissors 0:6757f7363a9f 235 short retval;
Sissors 0:6757f7363a9f 236 char data[2];
Sissors 0:6757f7363a9f 237 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 238 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 239 return (int)retval;
Sissors 0:6757f7363a9f 240 }
Sissors 0:6757f7363a9f 241
Sissors 0:6757f7363a9f 242 void MPU6050::getGyroRaw( int *data ) {
Sissors 0:6757f7363a9f 243 char temp[6];
Sissors 0:6757f7363a9f 244 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 245 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 246 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 247 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 248 }
Sissors 0:6757f7363a9f 249
Sissors 0:6757f7363a9f 250 void MPU6050::getGyro( float *data ) {
Sissors 0:6757f7363a9f 251 int temp[3];
Sissors 0:6757f7363a9f 252 this->getGyroRaw(temp);
Sissors 1:a3366f09e95c 253 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
moklumbys 3:187152513f8d 254 data[0]=(float)temp[0] / 301.0;
moklumbys 3:187152513f8d 255 data[1]=(float)temp[1] / 301.0;
moklumbys 3:187152513f8d 256 data[2]=(float)temp[2] / 301.0;
moklumbys 3:187152513f8d 257 } //7505.5
Sissors 1:a3366f09e95c 258 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
Sissors 0:6757f7363a9f 259 data[0]=(float)temp[0] / 3752.9;
Sissors 0:6757f7363a9f 260 data[1]=(float)temp[1] / 3752.9;
Sissors 0:6757f7363a9f 261 data[2]=(float)temp[2] / 3752.9;
Sissors 0:6757f7363a9f 262 }
Sissors 1:a3366f09e95c 263 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
Sissors 0:6757f7363a9f 264 data[0]=(float)temp[0] / 1879.3;;
Sissors 0:6757f7363a9f 265 data[1]=(float)temp[1] / 1879.3;
Sissors 0:6757f7363a9f 266 data[2]=(float)temp[2] / 1879.3;
Sissors 0:6757f7363a9f 267 }
Sissors 1:a3366f09e95c 268 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
Sissors 0:6757f7363a9f 269 data[0]=(float)temp[0] / 939.7;
Sissors 0:6757f7363a9f 270 data[1]=(float)temp[1] / 939.7;
Sissors 0:6757f7363a9f 271 data[2]=(float)temp[2] / 939.7;
Sissors 0:6757f7363a9f 272 }
Sissors 0:6757f7363a9f 273 }
Sissors 0:6757f7363a9f 274 //--------------------------------------------------
Sissors 0:6757f7363a9f 275 //-------------------Temperature--------------------
Sissors 0:6757f7363a9f 276 //--------------------------------------------------
Sissors 0:6757f7363a9f 277 int MPU6050::getTempRaw( void ) {
Sissors 0:6757f7363a9f 278 short retval;
Sissors 0:6757f7363a9f 279 char data[2];
Sissors 0:6757f7363a9f 280 this->read(MPU6050_TEMP_H_REG, data, 2);
Sissors 0:6757f7363a9f 281 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 282 return (int)retval;
Sissors 0:6757f7363a9f 283 }
Sissors 0:6757f7363a9f 284
Sissors 0:6757f7363a9f 285 float MPU6050::getTemp( void ) {
Sissors 0:6757f7363a9f 286 float retval;
Sissors 0:6757f7363a9f 287 retval=(float)this->getTempRaw();
Sissors 0:6757f7363a9f 288 retval=(retval+521.0)/340.0+35.0;
Sissors 0:6757f7363a9f 289 return retval;
Sissors 0:6757f7363a9f 290 }
Sissors 0:6757f7363a9f 291