CIA / Mbed 2 deprecated MPU9250_Quaternion_Binary_Serial

Dependencies:   Eigen mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU9250.cpp Source File

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 }