MPU9150Lib.cpp
Go to the documentation of this file.
00001 
00002 //
00003 //  This file is part of MPU9150Lib
00004 //
00005 //  Copyright (c) 2013 Pansenti, LLC
00006 //
00007 //  Permission is hereby granted, free of charge, to any person obtaining a copy of 
00008 //  this software and associated documentation files (the "Software"), to deal in 
00009 //  the Software without restriction, including without limitation the rights to use, 
00010 //  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
00011 //  Software, and to permit persons to whom the Software is furnished to do so, 
00012 //  subject to the following conditions:
00013 //
00014 //  The above copyright notice and this permission notice shall be included in all 
00015 //  copies or substantial portions of the Software.
00016 //
00017 //  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
00018 //  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
00019 //  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
00020 //  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 
00021 //  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 
00022 //  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00023 
00024 #include "MPU9150Lib.h"
00025 #include "inv_mpu.h"
00026 #include "inv_mpu_dmp_motion_driver.h"
00027 #include "MPUQuaternion.h"
00028 
00030 //
00031 //  The functions below are from the InvenSense SDK example code.
00032 //
00033 //  Original copyright notice below:
00034 
00035 /*
00036  $License:
00037     Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
00038     See included License.txt for License information.
00039  $
00040  */
00041 
00042 /* These next two functions converts the orientation matrix (see
00043  * gyro_orientation) to a scalar representation for use by the DMP.
00044  * NOTE: These functions are borrowed from InvenSense's MPL.
00045  */
00046 
00047 static inline unsigned short inv_row_2_scale(const signed char *row)
00048 {
00049     unsigned short b;
00050 
00051     if (row[0] > 0)
00052         b = 0;
00053     else if (row[0] < 0)
00054         b = 4;
00055     else if (row[1] > 0)
00056         b = 1;
00057     else if (row[1] < 0)
00058         b = 5;
00059     else if (row[2] > 0)
00060         b = 2;
00061     else if (row[2] < 0)
00062         b = 6;
00063     else
00064         b = 7;      // error
00065     return b;
00066 }
00067 
00068 /* The sensors can be mounted onto the board in any orientation. The mounting
00069  * matrix seen below tells the MPL how to rotate the raw data from thei
00070  * driver(s).
00071  * TODO: The following matrices refer to the configuration on an internal test
00072  * board at Invensense. If needed, please modify the matrices to match the
00073  * chip-to-body matrix for your particular set up.
00074  */
00075 
00076 static signed char gyro_orientation[9] = { 1, 0, 0,
00077                                            0, 1, 0,
00078                                            0, 0, 1};
00079 
00080 static inline unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
00081 {
00082     unsigned short scalar;
00083     /*
00084 
00085        XYZ  010_001_000 Identity Matrix
00086        XZY  001_010_000
00087        YXZ  010_000_001
00088        YZX  000_010_001
00089        ZXY  001_000_010
00090        ZYX  000_001_010
00091      */
00092     scalar = inv_row_2_scale(mtx);
00093     scalar |= inv_row_2_scale(mtx + 3) << 3;
00094     scalar |= inv_row_2_scale(mtx + 6) << 6;
00095     return scalar;
00096 }
00097 
00098 //
00100 
00101 
00102 MPU9150Lib::MPU9150Lib()
00103 {
00104   //  use calibration if available
00105 
00106   m_useAccelCalibration = true;
00107   m_useMagCalibration = true;
00108   m_device = 0;
00109  }
00110 
00111 void MPU9150Lib::selectDevice(int device)
00112 {
00113   m_device = device;
00114 }
00115 
00116 void MPU9150Lib::useAccelCal(boolean useCal)
00117 {
00118   m_useAccelCalibration = useCal;
00119 }
00120 
00121 void MPU9150Lib::disableAccelCal()
00122 {
00123     if (!m_useAccelCalibration)
00124         return;
00125     m_useAccelCalibration = false;
00126 
00127     m_accelOffset[0] = 0;
00128     m_accelOffset[1] = 0;
00129     m_accelOffset[2] = 0;
00130 
00131     mpu_set_accel_bias(m_accelOffset);
00132 }
00133 
00134 void MPU9150Lib::useMagCal(boolean useCal)
00135 {
00136   m_useMagCalibration = useCal;
00137 }
00138 
00139 boolean MPU9150Lib::init(int mpuRate, int magMix, int magRate, int lpf)
00140 {
00141   struct int_param_s int_param;
00142   int result;
00143 
00144   mpu_select_device(m_device);
00145   dmp_select_device(m_device);
00146   if (magRate > 100)
00147     return false;                                         // rate must be less than or equal to 100Hz
00148   if (magRate < 1)
00149     return false;
00150   m_magInterval = (unsigned long)(1000 / magRate);        // record mag interval
00151   m_lastMagSample = millis();
00152 
00153   if (mpuRate > 1000)
00154     return false;
00155   if (mpuRate < 1)
00156     return false;
00157   m_magMix = magMix;
00158   m_lastDMPYaw = 0;
00159   m_lastYaw = 0;
00160 
00161   // get calibration data if it's there
00162 
00163   if (calLibRead(m_device, &m_calData)) {                 // use calibration data if it's there and wanted
00164     m_useMagCalibration &= m_calData.magValid == 1;
00165     m_useAccelCalibration &= m_calData.accelValid == 1;
00166 
00167     //  Process calibration data for runtime
00168 
00169     if (m_useMagCalibration) {
00170        m_magXOffset = (short)(((long)m_calData.magMaxX + (long)m_calData.magMinX) / 2);
00171        m_magXRange = m_calData.magMaxX - m_magXOffset;
00172        m_magYOffset = (short)(((long)m_calData.magMaxY + (long)m_calData.magMinY) / 2);
00173        m_magYRange = m_calData.magMaxY - m_magYOffset;
00174        m_magZOffset = (short)(((long)m_calData.magMaxZ + (long)m_calData.magMinZ) / 2);
00175        m_magZRange = m_calData.magMaxZ - m_magZOffset;
00176     }
00177 
00178      if (m_useAccelCalibration) {
00179        m_accelOffset[0] = -((long)m_calData.accelMaxX + (long)m_calData.accelMinX) / 2;
00180        m_accelOffset[1] = -((long)m_calData.accelMaxY + (long)m_calData.accelMinY) / 2;
00181        m_accelOffset[2] = -((long)m_calData.accelMaxZ + (long)m_calData.accelMinZ) / 2;
00182 
00183        mpu_set_accel_bias(m_accelOffset);
00184 
00185        m_accelXRange = m_calData.accelMaxX + (short)m_accelOffset[0];
00186        m_accelYRange = m_calData.accelMaxY + (short)m_accelOffset[1];
00187        m_accelZRange = m_calData.accelMaxZ + (short)m_accelOffset[2];
00188      }
00189   } else {
00190       m_useMagCalibration = false;
00191       m_useAccelCalibration = false;
00192   }
00193 
00194 #ifdef MPULIB_DEBUG
00195   if (m_useMagCalibration)
00196           Serial.println("Using mag cal");
00197   if (m_useAccelCalibration)
00198           Serial.println("Using accel cal");
00199 #endif
00200 
00201   mpu_init_structures();
00202     
00203   // Not using interrupts so set up this structure to keep the driver happy
00204         
00205   int_param.cb = NULL;
00206   int_param.pin = 0;
00207   int_param.lp_exit = 0;
00208   int_param.active_low = 1;
00209   result = mpu_init(&int_param);
00210   if (result != 0) {
00211 #ifdef MPULIB_DEBUG
00212      Serial.print("mpu_init failed with code: "); Serial.println(result);
00213 #endif
00214      return false; 
00215   }
00216   mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);   // enable all of the sensors
00217   mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);                  // get accel and gyro data in the FIFO also
00218 
00219 #ifdef MPULIB_DEBUG
00220   Serial.println("Loading firmware");
00221 #endif
00222 
00223   if ((result = dmp_load_motion_driver_firmware()) != 0) {           // try to load the DMP firmware
00224 #ifdef MPULIB_DEBUG
00225     Serial.print("Failed to load dmp firmware: ");
00226     Serial.println(result);
00227 #endif
00228     return false;
00229   }
00230   dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // set up the correct orientation
00231 
00232   dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
00233         DMP_FEATURE_GYRO_CAL);
00234   dmp_set_fifo_rate(mpuRate);
00235   if (mpu_set_dmp_state(1) != 0) {
00236 #ifdef MPULIB_DEBUG
00237     Serial.println("mpu_set_dmp_state failed");
00238 #endif
00239     return false;
00240   }
00241   mpu_set_sample_rate(mpuRate);                                      // set the update rate
00242   mpu_set_compass_sample_rate(magRate);                              // set the compass update rate to match
00243   if (lpf != 0)
00244     mpu_set_lpf(lpf);                                                // set the low pass filter
00245   return true;
00246 }
00247 
00248 boolean MPU9150Lib::read()
00249 {
00250     short intStatus;
00251     int result;
00252     short sensors;
00253     unsigned char more;
00254     unsigned long timestamp;
00255    
00256     mpu_select_device(m_device);
00257     dmp_select_device(m_device);
00258     mpu_get_int_status(&intStatus);                       // get the current MPU state
00259     if ((intStatus & (MPU_INT_STATUS_DMP | MPU_INT_STATUS_DMP_0))
00260             != (MPU_INT_STATUS_DMP | MPU_INT_STATUS_DMP_0))
00261         return false;
00262         
00263     //  get the data from the fifo
00264         
00265     if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, &timestamp, &sensors, &more)) != 0) {
00266       return false;
00267     }      
00268     
00269     //  got the fifo data so now get the mag data if it's time
00270     
00271     if ((millis() - m_lastMagSample) >= m_magInterval) {
00272       if ((result = mpu_get_compass_reg(m_rawMag, &timestamp)) != 0) {
00273 #ifdef MPULIB_DEBUG
00274         Serial.print("Failed to read compass: ");
00275         Serial.println(result);
00276 #endif
00277         return false;
00278       }
00279       //        *** Note mag axes are changed here to align with gyros: Y = -X, X = Y
00280 
00281       m_lastMagSample = millis();
00282 
00283       if (m_useMagCalibration) {
00284         m_calMag[VEC3_Y] = -(short)(((long)(m_rawMag[VEC3_X] - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
00285         m_calMag[VEC3_X] = (short)(((long)(m_rawMag[VEC3_Y] - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
00286         m_calMag[VEC3_Z] = (short)(((long)(m_rawMag[VEC3_Z] - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
00287       } else {
00288         m_calMag[VEC3_Y] = -m_rawMag[VEC3_X];
00289         m_calMag[VEC3_X] = m_rawMag[VEC3_Y];
00290         m_calMag[VEC3_Z] = m_rawMag[VEC3_Z];
00291       }
00292     }
00293     
00294     // got the raw data - now process
00295     
00296     m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W];  // get float version of quaternion
00297     m_dmpQuaternion[QUAT_X] = (float)m_rawQuaternion[QUAT_X];
00298     m_dmpQuaternion[QUAT_Y] = (float)m_rawQuaternion[QUAT_Y];
00299     m_dmpQuaternion[QUAT_Z] = (float)m_rawQuaternion[QUAT_Z];
00300     MPUQuaternionNormalize(m_dmpQuaternion);                 // and normalize
00301     
00302     MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
00303 
00304 
00305     // Scale accel data 
00306 
00307     if (m_useAccelCalibration) {
00308 /*        m_calAccel[VEC3_X] = -(short)((((long)m_rawAccel[VEC3_X] + m_accelOffset[0])
00309                                         * (long)SENSOR_RANGE) / (long)m_accelXRange);
00310         m_calAccel[VEC3_Y] = (short)((((long)m_rawAccel[VEC3_Y] + m_accelOffset[1])
00311                                         * (long)SENSOR_RANGE) / (long)m_accelYRange);
00312         m_calAccel[VEC3_Z] = (short)((((long)m_rawAccel[VEC3_Z] + m_accelOffset[2])
00313                                         * (long)SENSOR_RANGE) / (long)m_accelZRange);
00314 */        if (m_rawAccel[VEC3_X] >= 0)
00315             m_calAccel[VEC3_X] = -(short)((((long)m_rawAccel[VEC3_X])
00316                                         * (long)SENSOR_RANGE) / (long)m_calData.accelMaxX);
00317         else
00318             m_calAccel[VEC3_X] = -(short)((((long)m_rawAccel[VEC3_X])
00319                                         * (long)SENSOR_RANGE) / -(long)m_calData.accelMinX);
00320 
00321         if (m_rawAccel[VEC3_Y] >= 0)
00322             m_calAccel[VEC3_Y] = (short)((((long)m_rawAccel[VEC3_Y])
00323                                         * (long)SENSOR_RANGE) / (long)m_calData.accelMaxY);
00324         else
00325             m_calAccel[VEC3_Y] = (short)((((long)m_rawAccel[VEC3_Y])
00326                                         * (long)SENSOR_RANGE) / -(long)m_calData.accelMinY);
00327 
00328         if (m_rawAccel[VEC3_Z] >= 0)
00329             m_calAccel[VEC3_Z] = (short)((((long)m_rawAccel[VEC3_Z])
00330                                         * (long)SENSOR_RANGE) / (long)m_calData.accelMaxZ);
00331         else
00332             m_calAccel[VEC3_Z] = (short)((((long)m_rawAccel[VEC3_Z])
00333                                         * (long)SENSOR_RANGE) / -(long)m_calData.accelMinZ);
00334 
00335     } else {
00336       m_calAccel[VEC3_X] = -m_rawAccel[VEC3_X];
00337       m_calAccel[VEC3_Y] = m_rawAccel[VEC3_Y];
00338       m_calAccel[VEC3_Z] = m_rawAccel[VEC3_Z];
00339     }
00340     dataFusion();
00341     return true;
00342 }
00343 
00344 void MPU9150Lib::dataFusion()
00345 {
00346   float qMag[4];
00347   float deltaDMPYaw, deltaMagYaw;
00348   float newMagYaw, newYaw;
00349   float temp1[4], unFused[4];
00350   float unFusedConjugate[4];
00351 
00352   // *** NOTE *** pitch direction swapped here
00353 
00354   m_fusedEulerPose[VEC3_X] = m_dmpEulerPose[VEC3_X];
00355   m_fusedEulerPose[VEC3_Y] = -m_dmpEulerPose[VEC3_Y];
00356   m_fusedEulerPose[VEC3_Z] = 0; 
00357   MPUQuaternionEulerToQuaternion(m_fusedEulerPose, unFused);    // create a new quaternion
00358 
00359   deltaDMPYaw = -m_dmpEulerPose[VEC3_Z] + m_lastDMPYaw;         // calculate change in yaw from dmp
00360   m_lastDMPYaw = m_dmpEulerPose[VEC3_Z];                        // update that
00361  
00362   qMag[QUAT_W] = 0;
00363   qMag[QUAT_X] = m_calMag[VEC3_X];
00364   qMag[QUAT_Y] = m_calMag[VEC3_Y];
00365   qMag[QUAT_Z] = m_calMag[VEC3_Z];
00366         
00367   // Tilt compensate mag with the unfused data (i.e. just roll and pitch with yaw 0)
00368         
00369   MPUQuaternionConjugate(unFused, unFusedConjugate);
00370   MPUQuaternionMultiply(qMag, unFusedConjugate, temp1);
00371   MPUQuaternionMultiply(unFused, temp1, qMag);
00372 
00373   // Now fuse this with the dmp yaw gyro information
00374 
00375   newMagYaw = -atan2(qMag[QUAT_Y], qMag[QUAT_X]);
00376 
00377   if (newMagYaw != newMagYaw) {                                 // check for nAn
00378 #ifdef MPULIB_DEBUG
00379     Serial.println("***nAn\n");
00380 #endif
00381     return;                                                     // just ignore in this case
00382   }
00383   if (newMagYaw < 0)
00384     newMagYaw = 2.0f * (float)M_PI + newMagYaw;                 // need 0 <= newMagYaw <= 2*PI
00385 
00386   newYaw = m_lastYaw + deltaDMPYaw;                             // compute new yaw from change
00387 
00388   if (newYaw > (2.0f * (float)M_PI))                            // need 0 <= newYaw <= 2*PI
00389     newYaw -= 2.0f * (float)M_PI;
00390   if (newYaw < 0)
00391     newYaw += 2.0f * (float)M_PI;
00392 
00393   deltaMagYaw = newMagYaw - newYaw;                             // compute difference
00394 
00395   if (deltaMagYaw >= (float)M_PI)
00396     deltaMagYaw = deltaMagYaw - 2.0f * (float)M_PI;
00397   if (deltaMagYaw <= -(float)M_PI)
00398     deltaMagYaw = (2.0f * (float)M_PI + deltaMagYaw);
00399 
00400   if (m_magMix > 0) {
00401     newYaw += deltaMagYaw / m_magMix;                           // apply some of the correction
00402 
00403     if (newYaw > (2.0f * (float)M_PI))                                      // need 0 <= newYaw <= 2*PI
00404       newYaw -= 2.0f * (float)M_PI;
00405     if (newYaw < 0)
00406       newYaw += 2.0f * (float)M_PI;
00407   }
00408 
00409   m_lastYaw = newYaw;
00410 
00411   if (newYaw > (float)M_PI)
00412     newYaw -= 2.0f * (float)M_PI;
00413 
00414   m_fusedEulerPose[VEC3_Z] = newYaw;                            // fill in output yaw value
00415         
00416   MPUQuaternionEulerToQuaternion(m_fusedEulerPose, m_fusedQuaternion);
00417 }
00418 
00419 void MPU9150Lib::printQuaternion(long *quat)
00420 {
00421   Serial.print("w: "); Serial.print(quat[QUAT_W]);  
00422   Serial.print(" x: "); Serial.print(quat[QUAT_X]);  
00423   Serial.print(" y: "); Serial.print(quat[QUAT_Y]);  
00424   Serial.print(" z: "); Serial.print(quat[QUAT_Z]);  
00425 }
00426 
00427 void MPU9150Lib::printQuaternion(float *quat)
00428 {
00429   Serial.print("w: "); Serial.print(quat[QUAT_W]);  
00430   Serial.print(" x: "); Serial.print(quat[QUAT_X]);  
00431   Serial.print(" y: "); Serial.print(quat[QUAT_Y]);  
00432   Serial.print(" z: "); Serial.print(quat[QUAT_Z]);  
00433 }
00434 
00435 void MPU9150Lib::printVector(short *vec)
00436 {
00437   Serial.print("x: "); Serial.print(vec[VEC3_X]);  
00438   Serial.print(" y: "); Serial.print(vec[VEC3_Y]);  
00439   Serial.print(" z: "); Serial.print(vec[VEC3_Z]);    
00440 }
00441 
00442 void MPU9150Lib::printVector(float *vec)
00443 {
00444   Serial.print("x: "); Serial.print(vec[VEC3_X]);  
00445   Serial.print(" y: "); Serial.print(vec[VEC3_Y]);  
00446   Serial.print(" z: "); Serial.print(vec[VEC3_Z]);    
00447 }
00448 
00449 void MPU9150Lib::printAngles(float *vec)
00450 {
00451   Serial.print("x: "); Serial.print(vec[VEC3_X] * RAD_TO_DEGREE);  
00452   Serial.print(" y: "); Serial.print(vec[VEC3_Y] * RAD_TO_DEGREE);  
00453   Serial.print(" z: "); Serial.print(vec[VEC3_Z] * RAD_TO_DEGREE);    
00454 }
00455 


ric_mc
Author(s): RoboTiCan
autogenerated on Thu Aug 27 2015 14:39:49