Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
MPU6050.cpp
00001 /** 00002 * Includes 00003 */ 00004 #include "MPU6050.h" 00005 00006 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { 00007 this->setSleepMode(false); 00008 00009 //Initializations: 00010 currentGyroRange = 0; 00011 currentAcceleroRange=0; 00012 alpha = ALPHA; 00013 } 00014 00015 //-------------------------------------------------- 00016 //-------------------General------------------------ 00017 //-------------------------------------------------- 00018 00019 void MPU6050::write(char address, char data) { 00020 char temp[2]; 00021 temp[0]=address; 00022 temp[1]=data; 00023 00024 connection.write(MPU6050_ADDRESS * 2,temp,2); 00025 } 00026 00027 char MPU6050::read(char address) { 00028 char retval; 00029 connection.write(MPU6050_ADDRESS * 2, &address, 1, true); 00030 connection.read(MPU6050_ADDRESS * 2, &retval, 1); 00031 return retval; 00032 } 00033 00034 void MPU6050::read(char address, char *data, int length) { 00035 connection.write(MPU6050_ADDRESS * 2, &address, 1, true); 00036 connection.read(MPU6050_ADDRESS * 2, data, length); 00037 } 00038 00039 void MPU6050::setSleepMode(bool state) { 00040 char temp; 00041 temp = this->read(MPU6050_PWR_MGMT_1_REG); 00042 if (state == true) 00043 temp |= 1<<MPU6050_SLP_BIT; 00044 if (state == false) 00045 temp &= ~(1<<MPU6050_SLP_BIT); 00046 this->write(MPU6050_PWR_MGMT_1_REG, temp); 00047 } 00048 00049 bool MPU6050::testConnection( void ) { 00050 char temp; 00051 temp = this->read(MPU6050_WHO_AM_I_REG); 00052 return (temp == (MPU6050_ADDRESS & 0xFE)); 00053 } 00054 00055 void MPU6050::setBW(char BW) { 00056 char temp; 00057 BW=BW & 0x07; 00058 temp = this->read(MPU6050_CONFIG_REG); 00059 temp &= 0xF8; 00060 temp = temp + BW; 00061 this->write(MPU6050_CONFIG_REG, temp); 00062 } 00063 00064 void MPU6050::setI2CBypass(bool state) { 00065 char temp; 00066 temp = this->read(MPU6050_INT_PIN_CFG); 00067 if (state == true) 00068 temp |= 1<<MPU6050_BYPASS_BIT; 00069 if (state == false) 00070 temp &= ~(1<<MPU6050_BYPASS_BIT); 00071 this->write(MPU6050_INT_PIN_CFG, temp); 00072 } 00073 00074 //-------------------------------------------------- 00075 //----------------Accelerometer--------------------- 00076 //-------------------------------------------------- 00077 00078 void MPU6050::setAcceleroRange( char range ) { 00079 char temp; 00080 range = range & 0x03; 00081 currentAcceleroRange = range; 00082 00083 temp = this->read(MPU6050_ACCELERO_CONFIG_REG); 00084 temp &= ~(3<<3); 00085 temp = temp + (range<<3); 00086 this->write(MPU6050_ACCELERO_CONFIG_REG, temp); 00087 } 00088 00089 int MPU6050::getAcceleroRawX( void ) { 00090 short retval; 00091 char data[2]; 00092 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); 00093 retval = (data[0]<<8) + data[1]; 00094 return (int)retval; 00095 } 00096 00097 int MPU6050::getAcceleroRawY( void ) { 00098 short retval; 00099 char data[2]; 00100 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); 00101 retval = (data[0]<<8) + data[1]; 00102 return (int)retval; 00103 } 00104 00105 int MPU6050::getAcceleroRawZ( void ) { 00106 short retval; 00107 char data[2]; 00108 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); 00109 retval = (data[0]<<8) + data[1]; 00110 return (int)retval; 00111 } 00112 00113 void MPU6050::getAcceleroRaw( int *data ) { 00114 char temp[6]; 00115 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); 00116 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00117 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00118 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00119 } 00120 00121 void MPU6050::getAccelero( float *data ) { 00122 int temp[3]; 00123 this->getAcceleroRaw(temp); 00124 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { 00125 data[0]=(float)temp[0] / 16384.0 * 9.81; 00126 data[1]=(float)temp[1] / 16384.0 * 9.81; 00127 data[2]=(float)temp[2] / 16384.0 * 9.81; 00128 } 00129 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ 00130 data[0]=(float)temp[0] / 8192.0 * 9.81; 00131 data[1]=(float)temp[1] / 8192.0 * 9.81; 00132 data[2]=(float)temp[2] / 8192.0 * 9.81; 00133 } 00134 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ 00135 data[0]=(float)temp[0] / 4096.0 * 9.81; 00136 data[1]=(float)temp[1] / 4096.0 * 9.81; 00137 data[2]=(float)temp[2] / 4096.0 * 9.81; 00138 } 00139 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ 00140 data[0]=(float)temp[0] / 2048.0 * 9.81; 00141 data[1]=(float)temp[1] / 2048.0 * 9.81; 00142 data[2]=(float)temp[2] / 2048.0 * 9.81; 00143 } 00144 00145 #ifdef DOUBLE_ACCELERO 00146 data[0]*=2; 00147 data[1]*=2; 00148 data[2]*=2; 00149 #endif 00150 } 00151 //-------------------------------------------------- 00152 //------------------Gyroscope----------------------- 00153 //-------------------------------------------------- 00154 void MPU6050::setGyroRange( char range ) { 00155 char temp; 00156 currentGyroRange = range; 00157 range = range & 0x03; 00158 temp = this->read(MPU6050_GYRO_CONFIG_REG); 00159 temp &= ~(3<<3); 00160 temp = temp + range<<3; 00161 this->write(MPU6050_GYRO_CONFIG_REG, temp); 00162 } 00163 00164 int MPU6050::getGyroRawX( void ) { 00165 short retval; 00166 char data[2]; 00167 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); 00168 retval = (data[0]<<8) + data[1]; 00169 return (int)retval; 00170 } 00171 00172 int MPU6050::getGyroRawY( void ) { 00173 short retval; 00174 char data[2]; 00175 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); 00176 retval = (data[0]<<8) + data[1]; 00177 return (int)retval; 00178 } 00179 00180 int MPU6050::getGyroRawZ( void ) { 00181 short retval; 00182 char data[2]; 00183 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); 00184 retval = (data[0]<<8) + data[1]; 00185 return (int)retval; 00186 } 00187 00188 void MPU6050::getGyroRaw( int *data ) { 00189 char temp[6]; 00190 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); 00191 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00192 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00193 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00194 } 00195 00196 void MPU6050::getGyro( float *data ) { 00197 int temp[3]; 00198 this->getGyroRaw(temp); 00199 if (currentGyroRange == MPU6050_GYRO_RANGE_250) { 00200 data[0]=(float)temp[0] / 301.0; 00201 data[1]=(float)temp[1] / 301.0; 00202 data[2]=(float)temp[2] / 301.0; 00203 } //7505.5 00204 if (currentGyroRange == MPU6050_GYRO_RANGE_500){ 00205 data[0]=(float)temp[0] / 3752.9; 00206 data[1]=(float)temp[1] / 3752.9; 00207 data[2]=(float)temp[2] / 3752.9; 00208 } 00209 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ 00210 data[0]=(float)temp[0]/ 1879.3; 00211 data[1]=(float)temp[1] / 1879.3; 00212 data[2]=(float)temp[2] / 1879.3; 00213 00214 } 00215 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ 00216 data[0]=(float)temp[0] / 939.7; 00217 data[1]=(float)temp[1] / 939.7; 00218 data[2]=(float)temp[2] / 939.7; 00219 } 00220 00221 } 00222 //-------------------------------------------------- 00223 //-------------------Temperature-------------------- 00224 //-------------------------------------------------- 00225 int MPU6050::getTempRaw( void ) { 00226 short retval; 00227 char data[2]; 00228 this->read(MPU6050_TEMP_H_REG, data, 2); 00229 retval = (data[0]<<8) + data[1]; 00230 return (int)retval; 00231 } 00232 00233 float MPU6050::getTemp( void ) { 00234 float retval; 00235 retval=(float)this->getTempRaw(); 00236 retval=(retval+521.0)/340.0+35.0; 00237 return retval; 00238 } 00239 00240 /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more. 00241 function for getting angles in degrees from accelerometer 00242 */ 00243 void MPU6050::getAcceleroAngle( float *data ) { 00244 float temp[3]; 00245 this->getAccelero(temp); 00246 00247 data[X_AXIS] = atan (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 00248 data[Y_AXIS] = atan (-1*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 00249 data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on 00250 00251 // data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees) 00252 // data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES; //but it takes longer and system gets unstable when angles ~90 degrees 00253 } 00254 00255 ///function for getting offset values for the gyro & accelerometer 00256 void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ 00257 float gyro[3]; 00258 float accAngle[3]; 00259 00260 for (int i = 0; i < 3; i++) { 00261 accOffset[i] = 0.0; //initialise offsets to 0.0 00262 gyroOffset[i] = 0.0; 00263 } 00264 00265 for (int i = 0; i < sampleSize; i++){ 00266 this->getGyro(gyro); //take real life measurements 00267 this->getAcceleroAngle (accAngle); 00268 00269 for (int j = 0; j < 3; j++){ 00270 *(accOffset+j) += accAngle[j]/sampleSize; //average measurements 00271 *(gyroOffset+j) += gyro[j]/sampleSize; 00272 } 00273 wait (0.01); //wait between each reading for accuracy 00274 } 00275 } 00276 00277 ///function for computing angles for roll, pitch anf yaw 00278 void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){ 00279 float gyro[3]; 00280 float accAngle[3]; 00281 00282 this->getGyro(gyro); //get gyro value in rad/s 00283 this->getAcceleroAngle(accAngle); //get angle from accelerometer 00284 00285 for (int i = 0; i < 3; i++){ 00286 gyro[i] -= gyroOffset[i]; //substract offset values 00287 accAngle[i] -= accOffset[i]; 00288 } 00289 00290 //apply filters on pitch and roll to get accurate angle values 00291 angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 00292 angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS]; 00293 00294 //calculate Yaw using just the gyroscope values - inaccurate 00295 angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval; 00296 } 00297 00298 ///function for setting a different Alpha value, which is used in complemetary filter calculations 00299 void MPU6050::setAlpha(float val){ 00300 alpha = val; 00301 } 00302 00303 ///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take 00304 void MPU6050::enableInt( void ){ 00305 char temp; 00306 temp = this->read(MPU6050_RA_INT_ENABLE); 00307 temp |= 0x01; 00308 this->write(MPU6050_RA_INT_ENABLE, temp); 00309 } 00310 00311 ///function for disabling interrupts on MPU6050 INT pin, when the data is ready to take 00312 void MPU6050::disableInt ( void ){ 00313 char temp; 00314 temp = this->read(MPU6050_RA_INT_ENABLE); 00315 temp &= 0xFE; 00316 this->write(MPU6050_RA_INT_ENABLE, temp); 00317 }
Generated on Thu Jul 21 2022 09:50:43 by
