Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
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;
00065 return b;
00066 }
00067
00068
00069
00070
00071
00072
00073
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
00086
00087
00088
00089
00090
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
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;
00148 if (magRate < 1)
00149 return false;
00150 m_magInterval = (unsigned long)(1000 / magRate);
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
00162
00163 if (calLibRead(m_device, &m_calData)) {
00164 m_useMagCalibration &= m_calData.magValid == 1;
00165 m_useAccelCalibration &= m_calData.accelValid == 1;
00166
00167
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
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);
00217 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00218
00219 #ifdef MPULIB_DEBUG
00220 Serial.println("Loading firmware");
00221 #endif
00222
00223 if ((result = dmp_load_motion_driver_firmware()) != 0) {
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));
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);
00242 mpu_set_compass_sample_rate(magRate);
00243 if (lpf != 0)
00244 mpu_set_lpf(lpf);
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);
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
00264
00265 if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, ×tamp, &sensors, &more)) != 0) {
00266 return false;
00267 }
00268
00269
00270
00271 if ((millis() - m_lastMagSample) >= m_magInterval) {
00272 if ((result = mpu_get_compass_reg(m_rawMag, ×tamp)) != 0) {
00273 #ifdef MPULIB_DEBUG
00274 Serial.print("Failed to read compass: ");
00275 Serial.println(result);
00276 #endif
00277 return false;
00278 }
00279
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
00295
00296 m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W];
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);
00301
00302 MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
00303
00304
00305
00306
00307 if (m_useAccelCalibration) {
00308
00309
00310
00311
00312
00313
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
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);
00358
00359 deltaDMPYaw = -m_dmpEulerPose[VEC3_Z] + m_lastDMPYaw;
00360 m_lastDMPYaw = m_dmpEulerPose[VEC3_Z];
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
00368
00369 MPUQuaternionConjugate(unFused, unFusedConjugate);
00370 MPUQuaternionMultiply(qMag, unFusedConjugate, temp1);
00371 MPUQuaternionMultiply(unFused, temp1, qMag);
00372
00373
00374
00375 newMagYaw = -atan2(qMag[QUAT_Y], qMag[QUAT_X]);
00376
00377 if (newMagYaw != newMagYaw) {
00378 #ifdef MPULIB_DEBUG
00379 Serial.println("***nAn\n");
00380 #endif
00381 return;
00382 }
00383 if (newMagYaw < 0)
00384 newMagYaw = 2.0f * (float)M_PI + newMagYaw;
00385
00386 newYaw = m_lastYaw + deltaDMPYaw;
00387
00388 if (newYaw > (2.0f * (float)M_PI))
00389 newYaw -= 2.0f * (float)M_PI;
00390 if (newYaw < 0)
00391 newYaw += 2.0f * (float)M_PI;
00392
00393 deltaMagYaw = newMagYaw - newYaw;
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;
00402
00403 if (newYaw > (2.0f * (float)M_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;
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