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.
MPU9250.cpp
00001 /** Daqn change log 00002 * 2018/02/16 : Replace words "MPU6050" with "MPU9250". 00003 * 2018/02/16 : L57, flip T/F by adding "!". 00004 * 2018/02/21 : L 00005 */ 00006 00007 /** 00008 * Includes 00009 */ 00010 #include "MPU9250.h" 00011 00012 MPU9250::MPU9250(PinName sda, PinName scl) : connection(sda, scl) { 00013 this->setSleepMode(false); 00014 00015 //Initializations: 00016 currentGyroRange = 0; 00017 currentAcceleroRange=0; 00018 alpha = ALPHA; 00019 } 00020 00021 //-------------------------------------------------- 00022 //-------------------General------------------------ 00023 //-------------------------------------------------- 00024 00025 void MPU9250::write(char address, char data) { 00026 char temp[2]; 00027 temp[0]=address; 00028 temp[1]=data; 00029 00030 connection.write(MPU9250_ADDRESS * 2,temp,2); 00031 } 00032 00033 char MPU9250::read(char address) { 00034 char retval; 00035 connection.write(MPU9250_ADDRESS * 2, &address, 1, true); 00036 connection.read(MPU9250_ADDRESS * 2, &retval, 1); 00037 return retval; 00038 } 00039 00040 void MPU9250::read(char address, char *data, int length) { 00041 connection.write(MPU9250_ADDRESS * 2, &address, 1, true); 00042 connection.read(MPU9250_ADDRESS * 2, data, length); 00043 } 00044 00045 void MPU9250::setSleepMode(bool state) { 00046 char temp; 00047 temp = this->read(MPU9250_PWR_MGMT_1_REG); 00048 if (state == true) 00049 temp |= 1<<MPU9250_SLP_BIT; 00050 if (state == false) 00051 temp &= ~(1<<MPU9250_SLP_BIT); 00052 this->write(MPU9250_PWR_MGMT_1_REG, temp); 00053 } 00054 00055 bool MPU9250::testConnection( void ) { 00056 char temp; 00057 temp = this->read(MPU9250_WHO_AM_I_REG); 00058 return !(temp == (MPU9250_ADDRESS & 0xFE)); // Daqn 2018/02/16 00059 } 00060 00061 void MPU9250::setBW(char BW) { 00062 char temp; 00063 BW=BW & 0x07; 00064 temp = this->read(MPU9250_CONFIG_REG); 00065 temp &= 0xF8; 00066 temp = temp + BW; 00067 this->write(MPU9250_CONFIG_REG, temp); 00068 } 00069 00070 void MPU9250::setI2CBypass(bool state) { 00071 char temp; 00072 temp = this->read(MPU9250_INT_PIN_CFG); 00073 if (state == true) 00074 temp |= 1<<MPU9250_BYPASS_BIT; 00075 if (state == false) 00076 temp &= ~(1<<MPU9250_BYPASS_BIT); 00077 this->write(MPU9250_INT_PIN_CFG, temp); 00078 } 00079 00080 //-------------------------------------------------- 00081 //----------------Accelerometer--------------------- 00082 //-------------------------------------------------- 00083 00084 void MPU9250::setAcceleroRange( char range ) { 00085 char temp; 00086 range = range & 0x03; 00087 currentAcceleroRange = range; 00088 00089 temp = this->read(MPU9250_ACCELERO_CONFIG_REG); 00090 temp &= ~(3<<3); 00091 temp = temp + (range<<3); 00092 this->write(MPU9250_ACCELERO_CONFIG_REG, temp); 00093 } 00094 00095 int MPU9250::getAcceleroRawX( void ) { 00096 short retval; 00097 char data[2]; 00098 this->read(MPU9250_ACCEL_XOUT_H_REG, data, 2); 00099 retval = (data[0]<<8) + data[1]; 00100 return (int)retval; 00101 } 00102 00103 int MPU9250::getAcceleroRawY( void ) { 00104 short retval; 00105 char data[2]; 00106 this->read(MPU9250_ACCEL_YOUT_H_REG, data, 2); 00107 retval = (data[0]<<8) + data[1]; 00108 return (int)retval; 00109 } 00110 00111 int MPU9250::getAcceleroRawZ( void ) { 00112 short retval; 00113 char data[2]; 00114 this->read(MPU9250_ACCEL_ZOUT_H_REG, data, 2); 00115 retval = (data[0]<<8) + data[1]; 00116 return (int)retval; 00117 } 00118 00119 void MPU9250::getAcceleroRaw( int *data ) { 00120 char temp[6]; 00121 this->read(MPU9250_ACCEL_XOUT_H_REG, temp, 6); 00122 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00123 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00124 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00125 } 00126 00127 void MPU9250::getAccelero( float *data ) { 00128 int temp[3]; 00129 this->getAcceleroRaw(temp); 00130 if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_2G) { 00131 data[0]=(float)temp[0] / 16384.0 * 9.81; 00132 data[1]=(float)temp[1] / 16384.0 * 9.81; 00133 data[2]=(float)temp[2] / 16384.0 * 9.81; 00134 } 00135 if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_4G){ 00136 data[0]=(float)temp[0] / 8192.0 * 9.81; 00137 data[1]=(float)temp[1] / 8192.0 * 9.81; 00138 data[2]=(float)temp[2] / 8192.0 * 9.81; 00139 } 00140 if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_8G){ 00141 data[0]=(float)temp[0] / 4096.0 * 9.81; 00142 data[1]=(float)temp[1] / 4096.0 * 9.81; 00143 data[2]=(float)temp[2] / 4096.0 * 9.81; 00144 } 00145 if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_16G){ 00146 data[0]=(float)temp[0] / 2048.0 * 9.81; 00147 data[1]=(float)temp[1] / 2048.0 * 9.81; 00148 data[2]=(float)temp[2] / 2048.0 * 9.81; 00149 } 00150 00151 #ifdef DOUBLE_ACCELERO 00152 data[0]*=2; 00153 data[1]*=2; 00154 data[2]*=2; 00155 #endif 00156 } 00157 //-------------------------------------------------- 00158 //------------------Gyroscope----------------------- 00159 //-------------------------------------------------- 00160 void MPU9250::setGyroRange( char range ) { 00161 char temp; 00162 currentGyroRange = range; 00163 range = range & 0x03; 00164 temp = this->read(MPU9250_GYRO_CONFIG_REG); 00165 temp &= ~(3<<3); 00166 temp = temp + range<<3; 00167 this->write(MPU9250_GYRO_CONFIG_REG, temp); 00168 } 00169 00170 int MPU9250::getGyroRawX( void ) { 00171 short retval; 00172 char data[2]; 00173 this->read(MPU9250_GYRO_XOUT_H_REG, data, 2); 00174 retval = (data[0]<<8) + data[1]; 00175 return (int)retval; 00176 } 00177 00178 int MPU9250::getGyroRawY( void ) { 00179 short retval; 00180 char data[2]; 00181 this->read(MPU9250_GYRO_YOUT_H_REG, data, 2); 00182 retval = (data[0]<<8) + data[1]; 00183 return (int)retval; 00184 } 00185 00186 int MPU9250::getGyroRawZ( void ) { 00187 short retval; 00188 char data[2]; 00189 this->read(MPU9250_GYRO_ZOUT_H_REG, data, 2); 00190 retval = (data[0]<<8) + data[1]; 00191 return (int)retval; 00192 } 00193 00194 void MPU9250::getGyroRaw( int *data ) { 00195 char temp[6]; 00196 this->read(MPU9250_GYRO_XOUT_H_REG, temp, 6); 00197 data[0] = (int)(short)((temp[0]<<8) + temp[1]); 00198 data[1] = (int)(short)((temp[2]<<8) + temp[3]); 00199 data[2] = (int)(short)((temp[4]<<8) + temp[5]); 00200 } 00201 00202 void MPU9250::getGyro( float *data ) { 00203 int temp[3]; 00204 this->getGyroRaw(temp); 00205 if (currentGyroRange == MPU9250_GYRO_RANGE_250) { 00206 data[0]=(float)temp[0] / 301.0; 00207 data[1]=(float)temp[1] / 301.0; 00208 data[2]=(float)temp[2] / 301.0; 00209 } //7505.5 00210 if (currentGyroRange == MPU9250_GYRO_RANGE_500){ 00211 data[0]=(float)temp[0] / 3752.9; 00212 data[1]=(float)temp[1] / 3752.9; 00213 data[2]=(float)temp[2] / 3752.9; 00214 } 00215 if (currentGyroRange == MPU9250_GYRO_RANGE_1000){ 00216 data[0]=(float)temp[0] / 1879.3;; 00217 data[1]=(float)temp[1] / 1879.3; 00218 data[2]=(float)temp[2] / 1879.3; 00219 } 00220 if (currentGyroRange == MPU9250_GYRO_RANGE_2000){ 00221 data[0]=(float)temp[0] / 939.7; 00222 data[1]=(float)temp[1] / 939.7; 00223 data[2]=(float)temp[2] / 939.7; 00224 } 00225 } 00226 //-------------------------------------------------- 00227 //-------------------Temperature-------------------- 00228 //-------------------------------------------------- 00229 int MPU9250::getTempRaw( void ) { 00230 short retval; 00231 char data[2]; 00232 this->read(MPU9250_TEMP_H_REG, data, 2); 00233 retval = (data[0]<<8) + data[1]; 00234 return (int)retval; 00235 } 00236 00237 float MPU9250::getTemp( void ) { 00238 float retval; 00239 retval=(float)this->getTempRaw(); 00240 retval=(retval+521.0)/340.0+35.0; 00241 return retval; 00242 } 00243 00244 00245 //-------------------------------------------------- 00246 //------------------Magnetometer-------------------- 00247 //-------------------------------------------------- 00248 void MPU9250::read(char address, char subaddress, char *data, int length) 00249 { 00250 // connection.write(MPU9250_ADDRESS * 2, &address, 1, true); 00251 // connection.read(MPU9250_ADDRESS * 2, data, length); 00252 connection.write(address, &subaddress, 1, true); 00253 connection.read(address, data, length); 00254 } 00255 00256 //void MPU9250::setMagnetoRange( char range ) { 00257 // char temp; 00258 // currentMagnetoRange = range; 00259 // range = range & 0x03; 00260 // temp = this->read(MPU9250_MAGNETO_CONFIG_REG); 00261 // temp &= ~(3<<3); 00262 // temp = temp + range<<3; 00263 // this->write(MPU9250_MAGNETO_CONFIG_REG, temp); 00264 //} 00265 00266 00267 00268 int MPU9250::getMagnetoRawX( void ) 00269 { 00270 short retval; 00271 char data[2]; 00272 this->read(AK8963_ADDRESS, AK8963_XOUT_L, data, 2); 00273 retval = (data[0]<<8) + data[1]; 00274 return (int)retval; 00275 } 00276 00277 int MPU9250::getMagnetoRawY( void ) 00278 { 00279 short retval; 00280 char data[2]; 00281 this->read(AK8963_ADDRESS, AK8963_YOUT_L, data, 2); 00282 retval = (data[0]<<8) + data[1]; 00283 return (int)retval; 00284 } 00285 // 00286 int MPU9250::getMagnetoRawZ( void ) 00287 { 00288 short retval; 00289 char data[2]; 00290 this->read(AK8963_ADDRESS, AK8963_ZOUT_L, data, 2); 00291 retval = (data[0]<<8) + data[1]; 00292 return (int)retval; 00293 } 00294 00295 void MPU9250::getMagnetoRaw( int *data ) { 00296 char temp[6]; 00297 this->read(AK8963_ADDRESS, AK8963_XOUT_L, temp, 6); 00298 data[0] = (int)(short)((temp[1]<<8) + temp[0]); 00299 data[1] = (int)(short)((temp[3]<<8) + temp[2]); 00300 data[2] = (int)(short)((temp[5]<<8) + temp[4]); 00301 } 00302 00303 void MPU9250::getMagneto( float *data ) { 00304 int temp[3]; 00305 this->getMagnetoRaw(temp); 00306 data[0]=(float)temp[0] * .15f; 00307 data[1]=(float)temp[1] * .15f; 00308 data[2]=(float)temp[2] * .15f; 00309 } 00310 00311 /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more. 00312 function for getting angles in degrees from accelerometer 00313 */ 00314 void MPU9250::getAcceleroAngle( float *data ) { 00315 float temp[3]; 00316 this->getAccelero(temp); 00317 00318 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 00319 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 00320 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 00321 00322 // data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees) 00323 // 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 00324 } 00325 00326 ///function for getting offset values for the gyro & accelerometer 00327 void MPU9250::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ 00328 float gyro[3]; 00329 float accAngle[3]; 00330 00331 for (int i = 0; i < 3; i++) { 00332 accOffset[i] = 0.0; //initialise offsets to 0.0 00333 gyroOffset[i] = 0.0; 00334 } 00335 00336 for (int i = 0; i < sampleSize; i++){ 00337 this->getGyro(gyro); //take real life measurements 00338 this->getAcceleroAngle (accAngle); 00339 00340 for (int j = 0; j < 3; j++){ 00341 *(accOffset+j) += accAngle[j]/sampleSize; //average measurements 00342 *(gyroOffset+j) += gyro[j]/sampleSize; 00343 } 00344 wait (0.01); //wait between each reading for accuracy 00345 } 00346 } 00347 00348 ///function for computing angles for roll, pitch anf yaw 00349 void MPU9250::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){ 00350 float gyro[3]; 00351 float accAngle[3]; 00352 00353 this->getGyro(gyro); //get gyro value in rad/s 00354 this->getAcceleroAngle(accAngle); //get angle from accelerometer 00355 00356 for (int i = 0; i < 3; i++){ 00357 gyro[i] -= gyroOffset[i]; //substract offset values 00358 accAngle[i] -= accOffset[i]; 00359 } 00360 00361 //apply filters on pitch and roll to get accurate angle values 00362 angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 00363 angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS]; 00364 00365 //calculate Yaw using just the gyroscope values - inaccurate 00366 angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval; 00367 } 00368 00369 ///function for setting a different Alpha value, which is used in complemetary filter calculations 00370 void MPU9250::setAlpha(float val){ 00371 alpha = val; 00372 } 00373 00374 ///function for enabling interrupts on MPU9250 INT pin, when the data is ready to take 00375 void MPU9250::enableInt( void ){ 00376 char temp; 00377 temp = this->read(MPU9250_RA_INT_ENABLE); 00378 temp |= 0x01; 00379 this->write(MPU9250_RA_INT_ENABLE, temp); 00380 } 00381 00382 ///function for disbling interrupts on MPU9250 INT pin, when the data is ready to take 00383 void MPU9250::disableInt ( void ){ 00384 char temp; 00385 temp = this->read(MPU9250_RA_INT_ENABLE); 00386 temp &= 0xFE; 00387 this->write(MPU9250_RA_INT_ENABLE, temp); 00388 }
Generated on Tue Jul 12 2022 21:52:16 by
