MPU6050_6Axis_MotionApps20.cpp

00001 // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
00002 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
00003 // 5/20/2013 by Jeff Rowberg <jeff@rowberg.net>
00004 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
00005 //
00006 // Changelog:
00007 //     ... - ongoing debug release
00008  
00009 /* ============================================
00010 I2Cdev device library code is placed under the MIT license
00011 Copyright (c) 2012 Jeff Rowberg
00012  
00013 Permission is hereby granted, free of charge, to any person obtaining a copy
00014 of this software and associated documentation files (the "Software"), to deal
00015 in the Software without restriction, including without limitation the rights
00016 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00017 copies of the Software, and to permit persons to whom the Software is
00018 furnished to do so, subject to the following conditions:
00019  
00020 The above copyright notice and this permission notice shall be included in
00021 all copies or substantial portions of the Software.
00022  
00023 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00024 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00025 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00026 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00027 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00028 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00029 THE SOFTWARE.
00030 ===============================================
00031 */
00032 
00033 #include "MPU6050_6Axis_MotionApps20.h"
00034 
00035 uint8_t MPU6050::dmpInitialize()
00036 {
00037     // reset device
00038     wait_ms(50);
00039     reset();
00040     wait_ms(30);
00041 
00042     // enable sleep mode and wake cycle
00043     /*Serial.println(F("Enabling sleep mode..."));
00044     setSleepEnabled(true);
00045     Serial.println(F("Enabling wake cycle..."));
00046     setWakeCycleEnabled(true);*/
00047 
00048     // disable sleep mode
00049     //DEBUG_PRINT("Disabling sleep mode...\n");
00050     setSleepEnabled(false);
00051 
00052     // get MPU hardware revision
00053     //DEBUG_PRINT("Selecting user bank 16...\n");
00054     setMemoryBank(0x10, true, true);
00055     //DEBUG_PRINT("Selecting memory byte 6...\n");
00056     setMemoryStartAddress(0x06);
00057     //DEBUG_PRINT("Checking hardware revision...\n");
00058     uint8_t hwRevision = readMemoryByte();
00059     //DEBUG_PRINT("Revision @ user[16][6] = ");
00060     //DEBUG_PRINTF("%x\n",hwRevision);
00061     //DEBUG_PRINT("Resetting memory bank selection to 0...\n");
00062     setMemoryBank(0, false, false);
00063 
00064     // check OTP bank valid
00065     //DEBUG_PRINT("Reading OTP bank valid flag...\n");
00066     uint8_t otpValid = getOTPBankValid();
00067 
00068     //DEBUG_PRINT("OTP bank is ");
00069     if(otpValid);  //DEBUG_PRINT("valid!\n");
00070     else;  //DEBUG_PRINT("invalid!\n");
00071 
00072     // get X/Y/Z gyro offsets
00073     /*
00074     DEBUG_PRINT("\nReading gyro offset TC values...\n");
00075     int8_t xgOffsetTC = mpu.getXGyroOffsetTC();
00076     int8_t ygOffsetTC = getYGyroOffsetTC();
00077     int8_t zgOffsetTC = getZGyroOffsetTC();
00078     DEBUG_PRINTF("X gyro offset = %u\n",xgOffset);
00079     DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset);
00080     DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset);
00081     */
00082     // setup weird slave stuff (?)
00083     //DEBUG_PRINT("Setting slave 0 address to 0x7F...\n");
00084     setSlaveAddress(0, 0x7F);
00085 
00086     //DEBUG_PRINT("Disabling I2C Master mode...");
00087     setI2CMasterModeEnabled(false);
00088     //DEBUG_PRINT("Setting slave 0 address to 0x68 (self)...");
00089     setSlaveAddress(0, 0x68);
00090     //DEBUG_PRINT("Resetting I2C Master control...\n");
00091     resetI2CMaster();
00092 
00093     wait_ms(20);
00094 
00095     // load DMP code into memory banks
00096     //DEBUG_PRINT("Writing DMP code to MPU memory banks (");
00097     //DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
00098     //DEBUG_PRINT(" bytes)\n");
00099     if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
00100         //DEBUG_PRINT("Success! DMP code written and verified.\n");
00101 
00102         // write DMP configuration
00103         //DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
00104         //DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE);
00105         //DEBUG_PRINT(" bytes in config def)\n");
00106         if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
00107             //DEBUG_PRINT("Success! DMP configuration written and verified.\n");
00108 
00109             //DEBUG_PRINT("Setting clock source to Z Gyro...\n");
00110             setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
00111 
00112             //DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
00113             setIntEnabled(0x12);
00114 
00115             //DEBUG_PRINT("Setting sample rate to 200Hz...");
00116             setRate(IMU_SAMPLE_RATE_DIVIDER); // 1khz / (1 + 4) = 200 Hz
00117 
00118             //DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
00119             setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
00120 
00121             //DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
00122             setDLPFMode(MPU6050_DLPF_BW_42);
00123 
00124             //DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
00125             setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
00126 
00127             //DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
00128             setDMPConfig1(0x03);
00129             setDMPConfig2(0x00);
00130 
00131             //DEBUG_PRINT("Clearing OTP Bank flag...");
00132             setOTPBankValid(false);
00133 
00134             //DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n");
00135             //setXGyroOffsetTC(xgOffsetTC);
00136             //setYGyroOffsetTC(ygOffsetTC);
00137             //setZGyroOffsetTC(zgOffsetTC);
00138 
00139             //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
00140             //setXGyroOffset(0);
00141             //setYGyroOffset(0);
00142             //setZGyroOffset(0);
00143 
00144             //DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n");
00145             uint8_t dmpUpdate[16], j;
00146             uint16_t pos = 0;
00147             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00148             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00149 
00150             //DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n");
00151             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00152             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00153 
00154             //DEBUG_PRINT("Resetting FIFO...\n");
00155             resetFIFO();
00156 
00157             //DEBUG_PRINT("Reading FIFO count...\n");
00158             uint16_t fifoCount = getFIFOCount();
00159             uint8_t fifoBuffer[128];
00160 
00161             //DEBUG_PRINT("Current FIFO count=");
00162             //DEBUG_PRINTF("%u\n",fifoCount);
00163             getFIFOBytes(fifoBuffer, fifoCount);
00164 
00165             //DEBUG_PRINT("Setting motion detection threshold to 2...\n");
00166             setMotionDetectionThreshold(2);
00167 
00168             //DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
00169             setZeroMotionDetectionThreshold(156);
00170 
00171             //DEBUG_PRINT("Setting motion detection duration to 80...");
00172             setMotionDetectionDuration(80);
00173 
00174             //DEBUG_PRINT("Setting zero-motion detection duration to 0...");
00175             setZeroMotionDetectionDuration(0);
00176 
00177             //DEBUG_PRINT("Resetting FIFO...\n");
00178             resetFIFO();
00179 
00180             //DEBUG_PRINT("Enabling FIFO...\n");
00181             setFIFOEnabled(true);
00182 
00183             //DEBUG_PRINT("Enabling DMP...\n");
00184             setDMPEnabled(true);
00185 
00186             //DEBUG_PRINT("Resetting DMP...\n");
00187             resetDMP();
00188 
00189             //DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n");
00190             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00191             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00192 
00193             //DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n");
00194             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00195             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00196 
00197             //DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n");
00198             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00199             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00200 
00201             //DEBUG_PRINT("Waiting for FIFO count > 2...\n");
00202             while ((fifoCount = getFIFOCount()) < 3);
00203 
00204             //DEBUG_PRINT("Current FIFO count=");
00205             //DEBUG_PRINTF("%u\n",fifoCount);
00206             //DEBUG_PRINT("Reading FIFO data...\n");
00207             getFIFOBytes(fifoBuffer, fifoCount);
00208 
00209             //DEBUG_PRINT("Reading interrupt status...\n");
00210             uint8_t mpuIntStatus = getIntStatus();
00211 
00212             //DEBUG_PRINT("Current interrupt status=");
00213             //DEBUG_PRINTF("%x\n",mpuIntStatus);
00214 
00215             //DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n");
00216             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00217             readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00218 
00219             //DEBUG_PRINT("Waiting for FIFO count > 2...\n");
00220             while ((fifoCount = getFIFOCount()) < 3);
00221 
00222             //DEBUG_PRINT("Current FIFO count=");
00223             //DEBUG_PRINTF("%u\n",fifoCount);
00224 
00225             //DEBUG_PRINT("Reading FIFO data...\n");
00226             getFIFOBytes(fifoBuffer, fifoCount);
00227 
00228             //DEBUG_PRINT("Reading interrupt status...\n");
00229             mpuIntStatus = getIntStatus();
00230 
00231             //DEBUG_PRINT("Current interrupt status=");
00232             //DEBUG_PRINTF("%x\n",mpuIntStatus);
00233 
00234             //DEBUG_PRINT("Writing final memory update 7/7 (function unknown)...");
00235             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00236             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00237 
00238             //DEBUG_PRINT("DMP is good to go! Finally.\n");
00239 
00240             //DEBUG_PRINT("Disabling DMP (you turn it on later)...\n");
00241             setDMPEnabled(false);
00242 
00243             //DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n");
00244             dmpPacketSize = 42;
00245             /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
00246                 return 3; // TODO: proper error code for no memory
00247             }*/
00248 
00249             //DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
00250             resetFIFO();
00251             getIntStatus();
00252         } else {
00253             //DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
00254             return 2; // configuration block loading failed
00255         }
00256     } else {
00257         //DEBUG_PRINT("ERROR! DMP code verification failed.");
00258         return 1; // main binary block loading failed
00259     }
00260     return 0; // success
00261 }
00262 
00263 bool MPU6050::dmpPacketAvailable()
00264 {
00265     return getFIFOCount() >= dmpGetFIFOPacketSize();
00266 }
00267 
00268 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
00269 // uint8_t MPU6050::dmpGetFIFORate();
00270 // uint8_t MPU6050::dmpGetSampleStepSizeMS();
00271 // uint8_t MPU6050::dmpGetSampleFrequency();
00272 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
00273 
00274 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
00275 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
00276 //uint8_t MPU6050::dmpRunFIFORateProcesses();
00277 
00278 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
00279 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
00280 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00281 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00282 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
00283 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
00284 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
00285 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
00286 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
00287 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
00288 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00289 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
00290 
00291 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet)
00292 {
00293     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00294     if (packet == 0) packet = dmpPacketBuffer;
00295     data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]);
00296     data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]);
00297     data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]);
00298     return 0;
00299 }
00300 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet)
00301 {
00302     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00303     if (packet == 0) packet = dmpPacketBuffer;
00304     data[0] = (packet[28] << 8) + packet[29];
00305     data[1] = (packet[32] << 8) + packet[33];
00306     data[2] = (packet[36] << 8) + packet[37];
00307     return 0;
00308 }
00309 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet)
00310 {
00311     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00312     if (packet == 0) packet = dmpPacketBuffer;
00313     v -> x = (packet[28] << 8) + packet[29];
00314     v -> y = (packet[32] << 8) + packet[33];
00315     v -> z = (packet[36] << 8) + packet[37];
00316     return 0;
00317 }
00318 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet)
00319 {
00320     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00321     if (packet == 0) packet = dmpPacketBuffer;
00322     data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
00323     data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
00324     data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
00325     data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
00326     return 0;
00327 }
00328 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet)
00329 {
00330     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00331     if (packet == 0) packet = dmpPacketBuffer;
00332     data[0] = ((packet[0] << 8) + packet[1]);
00333     data[1] = ((packet[4] << 8) + packet[5]);
00334     data[2] = ((packet[8] << 8) + packet[9]);
00335     data[3] = ((packet[12] << 8) + packet[13]);
00336     return 0;
00337 }
00338 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet)
00339 {
00340     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00341     int16_t qI[4];
00342     uint8_t status = dmpGetQuaternion(qI, packet);
00343     if (status == 0) {
00344         q -> w = (float)qI[0] / 16384.0f;
00345         q -> x = (float)qI[1] / 16384.0f;
00346         q -> y = (float)qI[2] / 16384.0f;
00347         q -> z = (float)qI[3] / 16384.0f;
00348         return 0;
00349     }
00350     return status; // int16 return value, indicates error if this line is reached
00351 }
00352 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
00353 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
00354 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet)
00355 {
00356     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00357     if (packet == 0) packet = dmpPacketBuffer;
00358     data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
00359     data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
00360     data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
00361     return 0;
00362 }
00363 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet)
00364 {
00365     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00366     if (packet == 0) packet = dmpPacketBuffer;
00367     data[0] = (packet[16] << 8) + packet[17];
00368     data[1] = (packet[20] << 8) + packet[21];
00369     data[2] = (packet[24] << 8) + packet[25];
00370     return 0;
00371 }
00372 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
00373 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
00374 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity)
00375 {
00376     // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
00377     v -> x = vRaw -> x - gravity -> x*8192;
00378     v -> y = vRaw -> y - gravity -> y*8192;
00379     v -> z = vRaw -> z - gravity -> z*8192;
00380     return 0;
00381 }
00382 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
00383 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q)
00384 {
00385     // rotate measured 3D acceleration vector into original state
00386     // frame of reference based on orientation quaternion
00387     memcpy(v, vReal, sizeof(VectorInt16));
00388     v -> rotate(q);
00389     return 0;
00390 }
00391 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
00392 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
00393 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
00394 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
00395 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
00396 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q)
00397 {
00398     v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
00399     v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
00400     v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
00401     return 0;
00402 }
00403 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
00404 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
00405 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
00406 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
00407 
00408 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q)
00409 {
00410     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
00411     data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
00412     data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
00413     return 0;
00414 }
00415 
00416 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity)
00417 {
00418     // yaw: (about Z axis)
00419     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
00420     // pitch: (nose up/down, about Y axis)
00421     data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
00422     // roll: (tilt left/right, about X axis)
00423     data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
00424     return 0;
00425 }
00426 
00427 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
00428 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
00429 
00430 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData)
00431 {
00432     /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
00433         if (dmpData[k] < 0x10) Serial.print("0");
00434         Serial.print(dmpData[k], HEX);
00435         Serial.print(" ");
00436     }
00437     Serial.print("\n");*/
00438     //Serial.println((uint16_t)dmpPacketBuffer);
00439     return 0;
00440 }
00441 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed)
00442 {
00443     uint8_t status;
00444     uint8_t buf[dmpPacketSize];
00445     for (uint8_t i = 0; i < numPackets; i++) {
00446         // read packet from FIFO
00447         getFIFOBytes(buf, dmpPacketSize);
00448 
00449         // process packet
00450         if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
00451 
00452         // increment external process count variable, if supplied
00453         if (processed != 0) *processed++;
00454     }
00455     return 0;
00456 }
00457 
00458 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
00459 
00460 // uint8_t MPU6050::dmpInitFIFOParam();
00461 // uint8_t MPU6050::dmpCloseFIFO();
00462 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
00463 // uint8_t MPU6050::dmpDecodeQuantizedAccel();
00464 // uint32_t MPU6050::dmpGetGyroSumOfSquare();
00465 // uint32_t MPU6050::dmpGetAccelSumOfSquare();
00466 // void MPU6050::dmpOverrideQuaternion(long *q);
00467 uint16_t MPU6050::dmpGetFIFOPacketSize()
00468 {
00469     return dmpPacketSize;
00470 }