Sheila Pham
/
IMU6050Ver1
Added for gyro and testing
Embed:
(wiki syntax)
Show/hide line numbers
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 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ 00215 data[0]=(float)temp[0] / 939.7; 00216 data[1]=(float)temp[1] / 939.7; 00217 data[2]=(float)temp[2] / 939.7; 00218 } 00219 } 00220 //-------------------------------------------------- 00221 //-------------------Temperature-------------------- 00222 //-------------------------------------------------- 00223 int MPU6050::getTempRaw( void ) { 00224 short retval; 00225 char data[2]; 00226 this->read(MPU6050_TEMP_H_REG, data, 2); 00227 retval = (data[0]<<8) + data[1]; 00228 return (int)retval; 00229 } 00230 00231 float MPU6050::getTemp( void ) { 00232 float retval; 00233 retval=(float)this->getTempRaw(); 00234 retval=(retval+521.0)/340.0+35.0; 00235 return retval; 00236 } 00237 00238 /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more. 00239 function for getting angles in degrees from accelerometer 00240 */ 00241 void MPU6050::getAcceleroAngle( float *data ) { 00242 float temp[3]; 00243 this->getAccelero(temp); 00244 00245 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 00246 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 00247 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 00248 00249 // data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees) 00250 // 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 00251 } 00252 00253 ///function for getting offset values for the gyro & accelerometer 00254 void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ 00255 float gyro[3]; 00256 float accAngle[3]; 00257 00258 for (int i = 0; i < 3; i++) { 00259 accOffset[i] = 0.0; //initialise offsets to 0.0 00260 gyroOffset[i] = 0.0; 00261 } 00262 00263 for (int i = 0; i < sampleSize; i++){ 00264 this->getGyro(gyro); //take real life measurements 00265 this->getAcceleroAngle (accAngle); 00266 00267 for (int j = 0; j < 3; j++){ 00268 *(accOffset+j) += accAngle[j]/sampleSize; //average measurements 00269 *(gyroOffset+j) += gyro[j]/sampleSize; 00270 } 00271 wait (0.01); //wait between each reading for accuracy 00272 } 00273 } 00274 00275 ///function for computing angles for roll, pitch anf yaw 00276 void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){ 00277 float gyro[3]; 00278 float accAngle[3]; 00279 00280 this->getGyro(gyro); //get gyro value in rad/s 00281 this->getAcceleroAngle(accAngle); //get angle from accelerometer 00282 00283 for (int i = 0; i < 3; i++){ 00284 gyro[i] -= gyroOffset[i]; //substract offset values 00285 accAngle[i] -= accOffset[i]; 00286 } 00287 00288 //apply filters on pitch and roll to get accurate angle values 00289 angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 00290 angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS]; 00291 00292 //calculate Yaw using just the gyroscope values - inaccurate 00293 angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval; 00294 } 00295 00296 ///function for setting a different Alpha value, which is used in complemetary filter calculations 00297 void MPU6050::setAlpha(float val){ 00298 alpha = val; 00299 } 00300 00301 ///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take 00302 void MPU6050::enableInt( void ){ 00303 char temp; 00304 temp = this->read(MPU6050_RA_INT_ENABLE); 00305 temp |= 0x01; 00306 this->write(MPU6050_RA_INT_ENABLE, temp); 00307 } 00308 00309 ///function for disabling interrupts on MPU6050 INT pin, when the data is ready to take 00310 void MPU6050::disableInt ( void ){ 00311 char temp; 00312 temp = this->read(MPU6050_RA_INT_ENABLE); 00313 temp &= 0xFE; 00314 this->write(MPU6050_RA_INT_ENABLE, temp); 00315 }
Generated on Sat Jul 16 2022 12:46:53 by 1.7.2