Code for our FYDP -only one IMU works right now -RTOS is working
Diff: drivers/MPU6050-DMP-Ian/MPU/src/MPU6051_6Axis_MotionApps20.cpp
- Revision:
- 0:964eb6a2ef00
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/drivers/MPU6050-DMP-Ian/MPU/src/MPU6051_6Axis_MotionApps20.cpp Wed Mar 18 22:23:48 2015 +0000 @@ -0,0 +1,470 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg <jeff@rowberg.net> +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "MPU6051_6Axis_MotionApps20.h" + +uint8_t MPU6051::dmpInitialize() +{ + // reset device + wait_ms(50); + reset(); + wait_ms(30); + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + //DEBUG_PRINT("Disabling sleep mode...\n"); + setSleepEnabled(false); + + // get MPU hardware revision + //DEBUG_PRINT("Selecting user bank 16...\n"); + setMemoryBank(0x10, true, true); + //DEBUG_PRINT("Selecting memory byte 6...\n"); + setMemoryStartAddress(0x06); + //DEBUG_PRINT("Checking hardware revision...\n"); + uint8_t hwRevision = readMemoryByte(); + //DEBUG_PRINT("Revision @ user[16][6] = "); + //DEBUG_PRINTF("%x\n",hwRevision); + //DEBUG_PRINT("Resetting memory bank selection to 0...\n"); + setMemoryBank(0, false, false); + + // check OTP bank valid + //DEBUG_PRINT("Reading OTP bank valid flag...\n"); + uint8_t otpValid = getOTPBankValid(); + + //DEBUG_PRINT("OTP bank is "); + if(otpValid); //DEBUG_PRINT("valid!\n"); + else; //DEBUG_PRINT("invalid!\n"); + + // get X/Y/Z gyro offsets + /* + DEBUG_PRINT("\nReading gyro offset TC values...\n"); + int8_t xgOffsetTC = mpu.getXGyroOffsetTC(); + int8_t ygOffsetTC = getYGyroOffsetTC(); + int8_t zgOffsetTC = getZGyroOffsetTC(); + DEBUG_PRINTF("X gyro offset = %u\n",xgOffset); + DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset); + DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset); + */ + // setup weird slave stuff (?) + //DEBUG_PRINT("Setting slave 0 address to 0x7F...\n"); + setSlaveAddress(0, 0x7F); + + //DEBUG_PRINT("Disabling I2C Master mode..."); + setI2CMasterModeEnabled(false); + //DEBUG_PRINT("Setting slave 0 address to 0x68 (self)..."); + setSlaveAddress(0, 0x68); + //DEBUG_PRINT("Resetting I2C Master control...\n"); + resetI2CMaster(); + + wait_ms(20); + + // load DMP code into memory banks + //DEBUG_PRINT("Writing DMP code to MPU memory banks ("); + //DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE); + //DEBUG_PRINT(" bytes)\n"); + if (writeProgMemoryBlock(dmpMemory2, MPU6050_DMP_CODE_SIZE)) { + //DEBUG_PRINT("Success! DMP code written and verified.\n"); + + // write DMP configuration + //DEBUG_PRINT("Writing DMP configuration to MPU memory banks ("); + //DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE); + //DEBUG_PRINT(" bytes in config def)\n"); + if (writeProgDMPConfigurationSet(dmpConfig2, MPU6050_DMP_CONFIG_SIZE)) { + //DEBUG_PRINT("Success! DMP configuration written and verified.\n"); + + //DEBUG_PRINT("Setting clock source to Z Gyro...\n"); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + //DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n"); + setIntEnabled(0x12); + + //DEBUG_PRINT("Setting sample rate to 200Hz..."); + setRate(4); // 1khz / (1 + 4) = 200 Hz + + //DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n"); + setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); + + //DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n"); + setDLPFMode(MPU6050_DLPF_BW_42); + + //DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n"); + setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + + //DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n"); + setDMPConfig1(0x03); + setDMPConfig2(0x00); + + //DEBUG_PRINT("Clearing OTP Bank flag..."); + setOTPBankValid(false); + + //DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n"); + //setXGyroOffsetTC(xgOffsetTC); + //setYGyroOffsetTC(ygOffsetTC); + //setZGyroOffsetTC(zgOffsetTC); + + //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); + //setXGyroOffset(0); + //setYGyroOffset(0); + //setZGyroOffset(0); + + //DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n"); + uint8_t dmpUpdate[16], j; + uint16_t pos = 0; + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n"); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("Resetting FIFO...\n"); + resetFIFO(); + + //DEBUG_PRINT("Reading FIFO count...\n"); + uint16_t fifoCount = getFIFOCount(); + uint8_t fifoBuffer[128]; + + //DEBUG_PRINT("Current FIFO count="); + //DEBUG_PRINTF("%u\n",fifoCount); + getFIFOBytes(fifoBuffer, fifoCount); + + //DEBUG_PRINT("Setting motion detection threshold to 2...\n"); + setMotionDetectionThreshold(2); + + //DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n"); + setZeroMotionDetectionThreshold(156); + + //DEBUG_PRINT("Setting motion detection duration to 80..."); + setMotionDetectionDuration(80); + + //DEBUG_PRINT("Setting zero-motion detection duration to 0..."); + setZeroMotionDetectionDuration(0); + + //DEBUG_PRINT("Resetting FIFO...\n"); + resetFIFO(); + + //DEBUG_PRINT("Enabling FIFO...\n"); + setFIFOEnabled(true); + + //DEBUG_PRINT("Enabling DMP...\n"); + setDMPEnabled(true); + + //DEBUG_PRINT("Resetting DMP...\n"); + resetDMP(); + + //DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n"); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n"); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n"); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("Waiting for FIFO count > 2...\n"); + while ((fifoCount = getFIFOCount()) < 3); + + //DEBUG_PRINT("Current FIFO count="); + //DEBUG_PRINTF("%u\n",fifoCount); + //DEBUG_PRINT("Reading FIFO data...\n"); + getFIFOBytes(fifoBuffer, fifoCount); + + //DEBUG_PRINT("Reading interrupt status...\n"); + uint8_t mpuIntStatus = getIntStatus(); + + //DEBUG_PRINT("Current interrupt status="); + //DEBUG_PRINTF("%x\n",mpuIntStatus); + + //DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n"); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("Waiting for FIFO count > 2...\n"); + while ((fifoCount = getFIFOCount()) < 3); + + //DEBUG_PRINT("Current FIFO count="); + //DEBUG_PRINTF("%u\n",fifoCount); + + //DEBUG_PRINT("Reading FIFO data...\n"); + getFIFOBytes(fifoBuffer, fifoCount); + + //DEBUG_PRINT("Reading interrupt status...\n"); + mpuIntStatus = getIntStatus(); + + //DEBUG_PRINT("Current interrupt status="); + //DEBUG_PRINTF("%x\n",mpuIntStatus); + + //DEBUG_PRINT("Writing final memory update 7/7 (function unknown)..."); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates2[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + //DEBUG_PRINT("DMP is good to go! Finally.\n"); + + //DEBUG_PRINT("Disabling DMP (you turn it on later)...\n"); + setDMPEnabled(false); + + //DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n"); + dmpPacketSize = 42; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + //DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n"); + resetFIFO(); + getIntStatus(); + } else { + //DEBUG_PRINT("ERROR! DMP configuration verification failed.\n"); + return 2; // configuration block loading failed + } + } else { + //DEBUG_PRINT("ERROR! DMP code verification failed."); + return 1; // main binary block loading failed + } + return 0; // success +} + +bool MPU6051::dmpPacketAvailable() +{ + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6051::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6051::dmpGetFIFORate(); +// uint8_t MPU6051::dmpGetSampleStepSizeMS(); +// uint8_t MPU6051::dmpGetSampleFrequency(); +// int32_t MPU6051::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6051::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6051::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6051::dmpRunFIFORateProcesses(); + +// uint8_t MPU6051::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6051::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6051::dmpGetAccel(int32_t *data, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); + data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); + data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); + return 0; +} +uint8_t MPU6051::dmpGetAccel(int16_t *data, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) + packet[29]; + data[1] = (packet[32] << 8) + packet[33]; + data[2] = (packet[36] << 8) + packet[37]; + return 0; +} +uint8_t MPU6051::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[28] << 8) + packet[29]; + v -> y = (packet[32] << 8) + packet[33]; + v -> z = (packet[36] << 8) + packet[37]; + return 0; +} +uint8_t MPU6051::dmpGetQuaternion(int32_t *data, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); + data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); + data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); + data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); + return 0; +} +uint8_t MPU6051::dmpGetQuaternion(int16_t *data, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) + packet[1]); + data[1] = ((packet[4] << 8) + packet[5]); + data[2] = ((packet[8] << 8) + packet[9]); + data[3] = ((packet[12] << 8) + packet[13]); + return 0; +} +uint8_t MPU6051::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6051::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6051::dmpGetGyro(int32_t *data, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); + data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); + data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); + return 0; +} +uint8_t MPU6051::dmpGetGyro(int16_t *data, const uint8_t* packet) +{ + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) + packet[17]; + data[1] = (packet[20] << 8) + packet[21]; + data[2] = (packet[24] << 8) + packet[25]; + return 0; +} +// uint8_t MPU6051::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6051::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6051::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) +{ + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6051::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6051::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) +{ + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6051::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6051::dmpGetGravity(VectorFloat *v, Quaternion *q) +{ + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6051::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6051::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6051::dmpGetEuler(float *data, Quaternion *q) +{ + 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 + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + 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 + return 0; +} + +uint8_t MPU6051::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) +{ + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} + +// uint8_t MPU6051::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6051::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6051::dmpProcessFIFOPacket(const unsigned char *dmpData) +{ + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6051::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) +{ + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) *processed++; + } + return 0; +} + +// uint8_t MPU6051::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6051::dmpInitFIFOParam(); +// uint8_t MPU6051::dmpCloseFIFO(); +// uint8_t MPU6051::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6051::dmpDecodeQuantizedAccel(); +// uint32_t MPU6051::dmpGetGyroSumOfSquare(); +// uint32_t MPU6051::dmpGetAccelSumOfSquare(); +// void MPU6051::dmpOverrideQuaternion(long *q); +uint16_t MPU6051::dmpGetFIFOPacketSize() +{ + return dmpPacketSize; +} \ No newline at end of file