SparkFunMPU9250-DMP.cpp

00001 /******************************************************************************
00002 SparkFunMPU9250-DMP.cpp - MPU-9250 Digital Motion Processor Arduino Library 
00003 Jim Lindblom @ SparkFun Electronics
00004 original creation date: November 23, 2016
00005 https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library
00006 
00007 This library implements motion processing functions of Invensense's MPU-9250.
00008 It is based on their Emedded MotionDriver 6.12 library.
00009     https://www.invensense.com/developers/software-downloads/
00010 
00011 Supported Platforms:
00012 any mbed enabled system.
00013 Corrections:
00014 computeEulerAngles function under Sparkfun-DMP-lib is corrected according to issue below. 
00015 Now angles are correct too. https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5 
00016 ******************************************************************************/
00017 #include "SparkFunMPU9250-DMP.h"
00018 #include "MPU9250_RegisterMap.h"
00019 #include "mdcompat.h"
00020 extern "C" {
00021 #include "inv_mpu.h"
00022 }
00023 static unsigned char mpu9250_orientation;
00024 static unsigned char tap_count;
00025 static unsigned char tap_direction;
00026 static bool _tap_available;
00027 static void orient_cb(unsigned char orient);
00028 static void tap_cb(unsigned char direction, unsigned char count);
00029 
00030 MPU9250_DMP::MPU9250_DMP()
00031 {
00032     _mSense = 6.665f; // Constant - 4915 / 32760
00033     _aSense = 0.0f;   // Updated after accel FSR is set
00034     _gSense = 0.0f;   // Updated after gyro FSR is set
00035 }
00036 
00037 inv_error_t MPU9250_DMP::begin(void)
00038 {
00039     inv_error_t result;
00040     struct int_param_s int_param;
00041     result = mpu_init(&int_param);
00042     if (result)
00043         return result;
00044     
00045     mpu_set_bypass(1); // Place all slaves (including compass) on primary bus
00046     
00047     setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
00048     
00049     _gSense = getGyroSens();
00050     _aSense = getAccelSens();
00051     
00052     return result;
00053 }
00054 
00055 inv_error_t MPU9250_DMP::enableInterrupt(unsigned char enable)
00056 {
00057     return set_int_enable(enable);
00058 }
00059 
00060 inv_error_t MPU9250_DMP::setIntLevel(unsigned char active_low)
00061 {
00062     return mpu_set_int_level(active_low);
00063 }
00064 
00065 inv_error_t MPU9250_DMP::setIntLatched(unsigned char enable)
00066 {
00067     return mpu_set_int_latched(enable);
00068 }
00069 
00070 short MPU9250_DMP::getIntStatus(void)
00071 {
00072     short status;
00073     if (mpu_get_int_status(&status) == INV_SUCCESS)
00074     {
00075         return status;
00076     }
00077     return 0;
00078 }
00079 
00080 // Accelerometer Low-Power Mode. Rate options:
00081 // 1.25 (1), 2.5 (2), 5, 10, 20, 40, 
00082 // 80, 160, 320, or 640 Hz
00083 // Disables compass and gyro
00084 inv_error_t MPU9250_DMP::lowPowerAccel(unsigned short rate)
00085 {
00086     return mpu_lp_accel_mode(rate);
00087 }
00088 
00089 inv_error_t MPU9250_DMP::setGyroFSR(unsigned short fsr)
00090 {
00091     inv_error_t err;
00092     err = mpu_set_gyro_fsr(fsr);
00093     if (err == INV_SUCCESS)
00094     {
00095         _gSense = getGyroSens();
00096     }
00097     return err;
00098 }
00099 
00100 inv_error_t MPU9250_DMP::setAccelFSR(unsigned char fsr)
00101 {
00102     inv_error_t err;
00103     err = mpu_set_accel_fsr(fsr);
00104     if (err == INV_SUCCESS)
00105     {
00106         _aSense = getAccelSens();
00107     }
00108     return err;
00109 }
00110 
00111 unsigned short MPU9250_DMP::getGyroFSR(void)
00112 {
00113     unsigned short tmp;
00114     if (mpu_get_gyro_fsr(&tmp) == INV_SUCCESS)
00115     {
00116         return tmp;
00117     }
00118     return 0;
00119 }
00120 
00121 unsigned char MPU9250_DMP::getAccelFSR(void)
00122 {
00123     unsigned char tmp;
00124     if (mpu_get_accel_fsr(&tmp) == INV_SUCCESS)
00125     {
00126         return tmp;
00127     }
00128     return 0;   
00129 }
00130 
00131 unsigned short MPU9250_DMP::getMagFSR(void)
00132 {
00133     unsigned short tmp;
00134     if (mpu_get_compass_fsr(&tmp) == INV_SUCCESS)
00135     {
00136         return tmp;
00137     }
00138     return 0;
00139 }
00140 
00141 inv_error_t MPU9250_DMP::setLPF(unsigned short lpf)
00142 {
00143     return mpu_set_lpf(lpf);
00144 }
00145 
00146 unsigned short MPU9250_DMP::getLPF(void)
00147 {
00148     unsigned short tmp;
00149     if (mpu_get_lpf(&tmp) == INV_SUCCESS)
00150     {
00151         return tmp;
00152     }
00153     return 0;
00154 }
00155 
00156 inv_error_t MPU9250_DMP::setSampleRate(unsigned short rate)
00157 {
00158     return mpu_set_sample_rate(rate);
00159 }
00160 
00161 unsigned short MPU9250_DMP::getSampleRate(void)
00162 {
00163     unsigned short tmp;
00164     if (mpu_get_sample_rate(&tmp) == INV_SUCCESS)
00165     {
00166         return tmp;
00167     }
00168     return 0;
00169 }
00170 
00171 inv_error_t MPU9250_DMP::setCompassSampleRate(unsigned short rate)
00172 {
00173     return mpu_set_compass_sample_rate(rate);
00174 }
00175 
00176 unsigned short MPU9250_DMP::getCompassSampleRate(void)
00177 {
00178     unsigned short tmp;
00179     if (mpu_get_compass_sample_rate(&tmp) == INV_SUCCESS)
00180     {
00181         return tmp;
00182     }
00183     
00184     return 0;
00185 }
00186 
00187 float MPU9250_DMP::getGyroSens(void)
00188 {
00189     float sens;
00190     if (mpu_get_gyro_sens(&sens) == INV_SUCCESS)
00191     {
00192         return sens;
00193     }
00194     return 0;
00195 }
00196     
00197 unsigned short MPU9250_DMP::getAccelSens(void)
00198 {
00199     unsigned short sens;
00200     if (mpu_get_accel_sens(&sens) == INV_SUCCESS)
00201     {
00202         return sens;
00203     }
00204     return 0;
00205 }
00206 
00207 float MPU9250_DMP::getMagSens(void)
00208 {
00209     return 0.15; // Static, 4915/32760
00210 }
00211 
00212 unsigned char MPU9250_DMP::getFifoConfig(void)
00213 {
00214     unsigned char sensors;
00215     if (mpu_get_fifo_config(&sensors) == INV_SUCCESS)
00216     {
00217         return sensors;
00218     }
00219     return 0;
00220 }
00221 
00222 inv_error_t MPU9250_DMP::configureFifo(unsigned char sensors)
00223 {
00224     return mpu_configure_fifo(sensors);
00225 }
00226 
00227 inv_error_t MPU9250_DMP::resetFifo(void)
00228 {
00229     return mpu_reset_fifo();
00230 }
00231 
00232 unsigned short MPU9250_DMP::fifoAvailable(void)
00233 {
00234     unsigned char fifoH, fifoL;
00235     if (mpu_read_reg(MPU9250_FIFO_COUNTH, &fifoH) != INV_SUCCESS)
00236         return 0;
00237     if (mpu_read_reg(MPU9250_FIFO_COUNTL, &fifoL) != INV_SUCCESS)
00238         return 0;
00239     
00240     return (fifoH << 8 ) | fifoL;
00241 }
00242 
00243 inv_error_t MPU9250_DMP::updateFifo(void)
00244 {
00245     short gyro[3], accel[3];
00246     unsigned long timestamp;
00247     unsigned char sensors, more;
00248     
00249     if (mpu_read_fifo(gyro, accel, &timestamp, &sensors, &more) != INV_SUCCESS)
00250         return INV_ERROR;
00251     
00252     if (sensors & INV_XYZ_ACCEL)
00253     {
00254         ax = accel[X_AXIS];
00255         ay = accel[Y_AXIS];
00256         az = accel[Z_AXIS];
00257     }
00258     if (sensors & INV_X_GYRO)
00259         gx = gyro[X_AXIS];
00260     if (sensors & INV_Y_GYRO)
00261         gy = gyro[Y_AXIS];
00262     if (sensors & INV_Z_GYRO)
00263         gz = gyro[Z_AXIS];
00264     
00265     time = timestamp;
00266     
00267     return INV_SUCCESS;
00268 }
00269 
00270 inv_error_t MPU9250_DMP::setSensors(unsigned char sensors)
00271 {
00272     return mpu_set_sensors(sensors);
00273 }
00274 
00275 bool MPU9250_DMP::dataReady()
00276 {
00277     unsigned char intStatusReg;
00278     
00279     if (mpu_read_reg(MPU9250_INT_STATUS, &intStatusReg) == INV_SUCCESS)
00280     {
00281         return (intStatusReg & (1<<INT_STATUS_RAW_DATA_RDY_INT));
00282     }
00283     return false;
00284 }
00285 
00286 inv_error_t MPU9250_DMP::update(unsigned char sensors)
00287 {
00288     inv_error_t aErr = INV_SUCCESS;
00289     inv_error_t gErr = INV_SUCCESS;
00290     inv_error_t mErr = INV_SUCCESS;
00291     inv_error_t tErr = INV_SUCCESS;
00292     
00293     if (sensors & UPDATE_ACCEL)
00294         aErr = updateAccel();
00295     if (sensors & UPDATE_GYRO)
00296         gErr = updateGyro();
00297     if (sensors & UPDATE_COMPASS)
00298         mErr = updateCompass();
00299     if (sensors & UPDATE_TEMP)
00300         tErr = updateTemperature();
00301     
00302     return aErr | gErr | mErr | tErr;
00303 }
00304 
00305 int MPU9250_DMP::updateAccel(void)
00306 {
00307     short data[3];
00308     
00309     if (mpu_get_accel_reg(data, &time))
00310     {
00311         return INV_ERROR;       
00312     }
00313     ax = data[X_AXIS];
00314     ay = data[Y_AXIS];
00315     az = data[Z_AXIS];
00316     return INV_SUCCESS;
00317 }
00318 
00319 int MPU9250_DMP::updateGyro(void)
00320 {
00321     short data[3];
00322     
00323     if (mpu_get_gyro_reg(data, &time))
00324     {
00325         return INV_ERROR;       
00326     }
00327     gx = data[X_AXIS];
00328     gy = data[Y_AXIS];
00329     gz = data[Z_AXIS];
00330     return INV_SUCCESS;
00331 }
00332 
00333 int MPU9250_DMP::updateCompass(void)
00334 {
00335     short data[3];
00336     
00337     if (mpu_get_compass_reg(data, &time))
00338     {
00339         return INV_ERROR;       
00340     }
00341     mx = data[X_AXIS];
00342     my = data[Y_AXIS];
00343     mz = data[Z_AXIS];
00344     return INV_SUCCESS;
00345 }
00346 
00347 inv_error_t MPU9250_DMP::updateTemperature(void)
00348 {
00349     return mpu_get_temperature(&temperature, &time);
00350 }
00351 
00352 int MPU9250_DMP::selfTest(unsigned char debug)
00353 {
00354     long gyro[3], accel[3];
00355     return mpu_run_self_test(gyro, accel);
00356 }
00357 
00358 inv_error_t MPU9250_DMP::dmpBegin(unsigned short features, unsigned short fifoRate)
00359 {
00360     unsigned short feat = features;
00361     unsigned short rate = fifoRate;
00362 
00363     if (dmpLoad() != INV_SUCCESS)
00364         return INV_ERROR;
00365     
00366     // 3-axis and 6-axis LP quat are mutually exclusive.
00367     // If both are selected, default to 3-axis
00368     if (feat & DMP_FEATURE_LP_QUAT)
00369     {
00370         feat &= ~(DMP_FEATURE_6X_LP_QUAT);
00371         dmp_enable_lp_quat(1);
00372     }
00373     else if (feat & DMP_FEATURE_6X_LP_QUAT)
00374         dmp_enable_6x_lp_quat(1);
00375     
00376     if (feat & DMP_FEATURE_GYRO_CAL)
00377         dmp_enable_gyro_cal(1);
00378     
00379     if (dmpEnableFeatures(feat) != INV_SUCCESS)
00380         return INV_ERROR;
00381     
00382     rate = constrain(rate, 1, 200);
00383     if (dmpSetFifoRate(rate) != INV_SUCCESS)
00384         return INV_ERROR;
00385     
00386     return mpu_set_dmp_state(1);
00387 }
00388 
00389 inv_error_t MPU9250_DMP::dmpLoad(void)
00390 {
00391     return dmp_load_motion_driver_firmware();
00392 }
00393 
00394 unsigned short MPU9250_DMP::dmpGetFifoRate(void)
00395 {
00396     unsigned short rate;
00397     if (dmp_get_fifo_rate(&rate) == INV_SUCCESS)
00398         return rate;
00399     
00400     return 0;
00401 }
00402 
00403 inv_error_t MPU9250_DMP::dmpSetFifoRate(unsigned short rate)
00404 {
00405     if (rate > MAX_DMP_SAMPLE_RATE) rate = MAX_DMP_SAMPLE_RATE;
00406     return dmp_set_fifo_rate(rate);
00407 }
00408 
00409 inv_error_t MPU9250_DMP::dmpUpdateFifo(void)
00410 {
00411     short gyro[3];
00412     short accel[3];
00413     long quat[4];
00414     unsigned long timestamp;
00415     short sensors;
00416     unsigned char more;
00417     
00418     if (dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more)
00419            != INV_SUCCESS)
00420     {
00421        return INV_ERROR;
00422     }
00423     
00424     if (sensors & INV_XYZ_ACCEL)
00425     {
00426         ax = accel[X_AXIS];
00427         ay = accel[Y_AXIS];
00428         az = accel[Z_AXIS];
00429     }
00430     if (sensors & INV_X_GYRO)
00431         gx = gyro[X_AXIS];
00432     if (sensors & INV_Y_GYRO)
00433         gy = gyro[Y_AXIS];
00434     if (sensors & INV_Z_GYRO)
00435         gz = gyro[Z_AXIS];
00436     if (sensors & INV_WXYZ_QUAT)
00437     {
00438         qw = quat[0];
00439         qx = quat[1];
00440         qy = quat[2];
00441         qz = quat[3];
00442     }
00443     
00444     time = timestamp;
00445     
00446     return INV_SUCCESS;
00447 }
00448 
00449 inv_error_t MPU9250_DMP::dmpEnableFeatures(unsigned short mask)
00450 {
00451     unsigned short enMask = 0;
00452     enMask |= mask;
00453     // Combat known issue where fifo sample rate is incorrect
00454     // unless tap is enabled in the DMP.
00455     enMask |= DMP_FEATURE_TAP; 
00456     return dmp_enable_feature(enMask);
00457 }
00458 
00459 unsigned short MPU9250_DMP::dmpGetEnabledFeatures(void)
00460 {
00461     unsigned short mask;
00462     if (dmp_get_enabled_features(&mask) == INV_SUCCESS)
00463         return mask;
00464     return 0;
00465 }
00466 
00467 inv_error_t MPU9250_DMP::dmpSetTap(
00468         unsigned short xThresh, unsigned short yThresh, unsigned short zThresh,
00469         unsigned char taps, unsigned short tapTime, unsigned short tapMulti)
00470 {
00471     unsigned char axes = 0;
00472     if (xThresh > 0)
00473     {
00474         axes |= TAP_X;
00475         xThresh = constrain(xThresh, 1, 1600);
00476         if (dmp_set_tap_thresh(1<<X_AXIS, xThresh) != INV_SUCCESS)
00477             return INV_ERROR;
00478     }
00479     if (yThresh > 0)
00480     {
00481         axes |= TAP_Y;
00482         yThresh = constrain(yThresh, 1, 1600);
00483         if (dmp_set_tap_thresh(1<<Y_AXIS, yThresh) != INV_SUCCESS)
00484             return INV_ERROR;
00485     }
00486     if (zThresh > 0)
00487     {
00488         axes |= TAP_Z;
00489         zThresh = constrain(zThresh, 1, 1600);
00490         if (dmp_set_tap_thresh(1<<Z_AXIS, zThresh) != INV_SUCCESS)
00491             return INV_ERROR;
00492     }
00493     if (dmp_set_tap_axes(axes) != INV_SUCCESS)
00494         return INV_ERROR;
00495     if (dmp_set_tap_count(taps) != INV_SUCCESS)
00496         return INV_ERROR;
00497     if (dmp_set_tap_time(tapTime) != INV_SUCCESS)
00498         return INV_ERROR;
00499     if (dmp_set_tap_time_multi(tapMulti) != INV_SUCCESS)
00500         return INV_ERROR;
00501     
00502     dmp_register_tap_cb(tap_cb);
00503     
00504     return INV_SUCCESS;
00505 }
00506 
00507 unsigned char MPU9250_DMP::getTapDir(void)
00508 {
00509     _tap_available = false;
00510     return tap_direction;
00511 }
00512 
00513 unsigned char MPU9250_DMP::getTapCount(void)
00514 {
00515     _tap_available = false;
00516     return tap_count;
00517 }
00518 
00519 bool MPU9250_DMP::tapAvailable(void)
00520 {
00521     return _tap_available;
00522 }
00523 
00524 inv_error_t MPU9250_DMP::dmpSetOrientation(const signed char * orientationMatrix)
00525 {
00526     unsigned short scalar;
00527     scalar = orientation_row_2_scale(orientationMatrix);
00528     scalar |= orientation_row_2_scale(orientationMatrix + 3) << 3;
00529     scalar |= orientation_row_2_scale(orientationMatrix + 6) << 6;
00530     
00531     dmp_register_android_orient_cb(orient_cb);
00532     
00533     return dmp_set_orientation(scalar);
00534 }
00535 
00536 unsigned char MPU9250_DMP::dmpGetOrientation(void)
00537 {
00538     return mpu9250_orientation;
00539 }
00540 
00541 inv_error_t MPU9250_DMP::dmpEnable3Quat(void)
00542 {
00543     unsigned short dmpFeatures;
00544     
00545     // 3-axis and 6-axis quat are mutually exclusive
00546     dmpFeatures = dmpGetEnabledFeatures();
00547     dmpFeatures &= ~(DMP_FEATURE_6X_LP_QUAT);
00548     dmpFeatures |= DMP_FEATURE_LP_QUAT;
00549     
00550     if (dmpEnableFeatures(dmpFeatures) != INV_SUCCESS)
00551         return INV_ERROR;
00552     
00553     return dmp_enable_lp_quat(1);
00554 }
00555     
00556 unsigned long MPU9250_DMP::dmpGetPedometerSteps(void)
00557 {
00558     unsigned long steps;
00559     if (dmp_get_pedometer_step_count(&steps) == INV_SUCCESS)
00560     {
00561         return steps;
00562     }
00563     return 0;
00564 }
00565 
00566 inv_error_t MPU9250_DMP::dmpSetPedometerSteps(unsigned long steps)
00567 {
00568     return dmp_set_pedometer_step_count(steps);
00569 }
00570 
00571 unsigned long MPU9250_DMP::dmpGetPedometerTime(void)
00572 {
00573     unsigned long walkTime;
00574     if (dmp_get_pedometer_walk_time(&walkTime) == INV_SUCCESS)
00575     {
00576         return walkTime;
00577     }
00578     return 0;
00579 }
00580 
00581 inv_error_t MPU9250_DMP::dmpSetPedometerTime(unsigned long time)
00582 {
00583     return dmp_set_pedometer_walk_time(time);
00584 }
00585 
00586 float MPU9250_DMP::calcAccel(int axis)
00587 {
00588     return (float) axis / (float) _aSense;
00589 }
00590 
00591 float MPU9250_DMP::calcGyro(int axis)
00592 {
00593     return (float) axis / (float) _gSense;
00594 }
00595 
00596 float MPU9250_DMP::calcMag(int axis)
00597 {
00598     return (float) axis / (float) _mSense;
00599 }
00600 
00601 float MPU9250_DMP::calcQuat(long axis)
00602 {
00603     return qToFloat(axis, 30);
00604 }
00605     
00606 float MPU9250_DMP::qToFloat(long number, unsigned char q)
00607 {
00608     unsigned long mask;
00609     for (int i=0; i<q; i++)
00610     {
00611         mask |= (1<<i);
00612     }
00613     return (number >> q) + ((number & mask) / (float) (2<<(q-1)));
00614 }
00615 
00616 void MPU9250_DMP::computeEulerAngles(bool degrees)
00617 {
00618     float dqw = qToFloat(qw, 30);
00619     float dqx = qToFloat(qx, 30);
00620     float dqy = qToFloat(qy, 30);
00621     float dqz = qToFloat(qz, 30);
00622 
00623     float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz);
00624     dqw = dqw/norm;
00625     dqx = dqx/norm;
00626     dqy = dqy/norm;
00627     dqz = dqz/norm;
00628 
00629     float ysqr = dqy * dqy;
00630 
00631     // roll (x-axis rotation)
00632     float t0 = +2.0 * (dqw * dqx + dqy * dqz);
00633     float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr);
00634     roll = atan2(t0, t1);
00635 
00636     // pitch (y-axis rotation)
00637     float t2 = +2.0 * (dqw * dqy - dqz * dqx);
00638     t2 = t2 > 1.0 ? 1.0 : t2;
00639     t2 = t2 < -1.0 ? -1.0 : t2;
00640     pitch = asin(t2);
00641 
00642     // yaw (z-axis rotation)
00643     float t3 = +2.0 * (dqw * dqz + dqx * dqy);
00644     float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz);  
00645     yaw = atan2(t3, t4);
00646     
00647     if (degrees)
00648     {
00649         pitch *= (180.0 / PI);
00650         roll *= (180.0 / PI);
00651         yaw *= (180.0 / PI);
00652         if (pitch < 0) pitch = 360.0 + pitch;
00653         if (roll < 0) roll = 360.0 + roll;
00654         if (yaw < 0) yaw = 360.0 + yaw; 
00655     }
00656 }
00657 
00658 float MPU9250_DMP::computeCompassHeading(void)
00659 {
00660     if (my == 0)
00661         heading = (mx < 0) ? 180.0 : 0;
00662     else
00663         heading = atan2((double)mx, (double)my);
00664     
00665     if (heading > PI) heading -= (2 * PI);
00666     else if (heading < -PI) heading += (2 * PI);
00667     else if (heading < 0) heading += 2 * PI;
00668     
00669     heading*= 180.0 / PI;
00670     
00671     return heading;
00672 }
00673 
00674 unsigned short MPU9250_DMP::orientation_row_2_scale(const signed char *row)
00675 {
00676     unsigned short b;
00677 
00678     if (row[0] > 0)
00679         b = 0;
00680     else if (row[0] < 0)
00681         b = 4;
00682     else if (row[1] > 0)
00683         b = 1;
00684     else if (row[1] < 0)
00685         b = 5;
00686     else if (row[2] > 0)
00687         b = 2;
00688     else if (row[2] < 0)
00689         b = 6;
00690     else
00691         b = 7;      // error
00692     return b;
00693 }
00694         
00695 static void tap_cb(unsigned char direction, unsigned char count)
00696 {
00697     _tap_available = true;
00698     tap_count = count;
00699     tap_direction = direction;
00700 }
00701 
00702 static void orient_cb(unsigned char orient)
00703 {
00704     mpu9250_orientation = orient;
00705 }