Ian Hua / MPU6050-DMP-Ian

Rearranged original code port/fork to: * Make library compatible with TiltyQuad IMU; * Prevent multiple definition, and added inclusion guard; * Cleaner access to library functions and file structure; and * "Broke out" code to control Sampling Rate and FIFO buffer update rate. By Trung Tin Ian HUA 2014. Credit to Jeff Rowberg for his original code, the best DMP implementation thus far; and szymon gaertig for porting the arduino library to mbed.

Dependents:   MPU6050-DMP_test

Fork of MPU6050 by Shundo Kishi

MPU/src/MPU6050_6Axis_MotionApps20.cpp@14:86afc8447df9, 2014-05-14 (annotated)

Committer:
pHysiX
Date:
Wed May 14 12:41:19 2014 +0000
Revision:
14:86afc8447df9
Parent:
13:b1b98e5c61df 
Moved configuration to file named "config.h"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 13:b1b98e5c61df 1 // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
pHysiX 13:b1b98e5c61df 2 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
pHysiX 13:b1b98e5c61df 3 // 5/20/2013 by Jeff Rowberg <jeff@rowberg.net>
pHysiX 13:b1b98e5c61df 4 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
pHysiX 13:b1b98e5c61df 5 //
pHysiX 13:b1b98e5c61df 6 // Changelog:
pHysiX 13:b1b98e5c61df 7 // ... - ongoing debug release
pHysiX 13:b1b98e5c61df 8
pHysiX 13:b1b98e5c61df 9 /* ============================================
pHysiX 13:b1b98e5c61df 10 I2Cdev device library code is placed under the MIT license
pHysiX 13:b1b98e5c61df 11 Copyright (c) 2012 Jeff Rowberg
pHysiX 13:b1b98e5c61df 12
pHysiX 13:b1b98e5c61df 13 Permission is hereby granted, free of charge, to any person obtaining a copy
pHysiX 13:b1b98e5c61df 14 of this software and associated documentation files (the "Software"), to deal
pHysiX 13:b1b98e5c61df 15 in the Software without restriction, including without limitation the rights
pHysiX 13:b1b98e5c61df 16 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
pHysiX 13:b1b98e5c61df 17 copies of the Software, and to permit persons to whom the Software is
pHysiX 13:b1b98e5c61df 18 furnished to do so, subject to the following conditions:
pHysiX 13:b1b98e5c61df 19
pHysiX 13:b1b98e5c61df 20 The above copyright notice and this permission notice shall be included in
pHysiX 13:b1b98e5c61df 21 all copies or substantial portions of the Software.
pHysiX 13:b1b98e5c61df 22
pHysiX 13:b1b98e5c61df 23 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
pHysiX 13:b1b98e5c61df 24 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
pHysiX 13:b1b98e5c61df 25 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
pHysiX 13:b1b98e5c61df 26 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
pHysiX 13:b1b98e5c61df 27 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
pHysiX 13:b1b98e5c61df 28 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
pHysiX 13:b1b98e5c61df 29 THE SOFTWARE.
pHysiX 13:b1b98e5c61df 30 ===============================================
pHysiX 13:b1b98e5c61df 31 */
pHysiX 13:b1b98e5c61df 32
pHysiX 6:2dc23167c8d8 33 #include "MPU6050_6Axis_MotionApps20.h"
pHysiX 6:2dc23167c8d8 34
pHysiX 6:2dc23167c8d8 35 uint8_t MPU6050::dmpInitialize()
pHysiX 6:2dc23167c8d8 36 {
pHysiX 6:2dc23167c8d8 37 // reset device
pHysiX 6:2dc23167c8d8 38 wait_ms(50);
pHysiX 6:2dc23167c8d8 39 reset();
pHysiX 6:2dc23167c8d8 40 wait_ms(30);
pHysiX 6:2dc23167c8d8 41
pHysiX 6:2dc23167c8d8 42 // enable sleep mode and wake cycle
pHysiX 6:2dc23167c8d8 43 /*Serial.println(F("Enabling sleep mode..."));
pHysiX 6:2dc23167c8d8 44 setSleepEnabled(true);
pHysiX 6:2dc23167c8d8 45 Serial.println(F("Enabling wake cycle..."));
pHysiX 6:2dc23167c8d8 46 setWakeCycleEnabled(true);*/
pHysiX 6:2dc23167c8d8 47
pHysiX 6:2dc23167c8d8 48 // disable sleep mode
pHysiX 11:c248bdcece21 49 //DEBUG_PRINT("Disabling sleep mode...\n");
pHysiX 6:2dc23167c8d8 50 setSleepEnabled(false);
pHysiX 6:2dc23167c8d8 51
pHysiX 6:2dc23167c8d8 52 // get MPU hardware revision
pHysiX 11:c248bdcece21 53 //DEBUG_PRINT("Selecting user bank 16...\n");
pHysiX 6:2dc23167c8d8 54 setMemoryBank(0x10, true, true);
pHysiX 11:c248bdcece21 55 //DEBUG_PRINT("Selecting memory byte 6...\n");
pHysiX 6:2dc23167c8d8 56 setMemoryStartAddress(0x06);
pHysiX 11:c248bdcece21 57 //DEBUG_PRINT("Checking hardware revision...\n");
pHysiX 6:2dc23167c8d8 58 uint8_t hwRevision = readMemoryByte();
pHysiX 11:c248bdcece21 59 //DEBUG_PRINT("Revision @ user[16][6] = ");
pHysiX 11:c248bdcece21 60 //DEBUG_PRINTF("%x\n",hwRevision);
pHysiX 11:c248bdcece21 61 //DEBUG_PRINT("Resetting memory bank selection to 0...\n");
pHysiX 6:2dc23167c8d8 62 setMemoryBank(0, false, false);
pHysiX 6:2dc23167c8d8 63
pHysiX 6:2dc23167c8d8 64 // check OTP bank valid
pHysiX 11:c248bdcece21 65 //DEBUG_PRINT("Reading OTP bank valid flag...\n");
pHysiX 6:2dc23167c8d8 66 uint8_t otpValid = getOTPBankValid();
pHysiX 6:2dc23167c8d8 67
pHysiX 11:c248bdcece21 68 //DEBUG_PRINT("OTP bank is ");
pHysiX 11:c248bdcece21 69 if(otpValid); //DEBUG_PRINT("valid!\n");
pHysiX 11:c248bdcece21 70 else; //DEBUG_PRINT("invalid!\n");
pHysiX 6:2dc23167c8d8 71
pHysiX 6:2dc23167c8d8 72 // get X/Y/Z gyro offsets
pHysiX 6:2dc23167c8d8 73 /*
pHysiX 6:2dc23167c8d8 74 DEBUG_PRINT("\nReading gyro offset TC values...\n");
pHysiX 6:2dc23167c8d8 75 int8_t xgOffsetTC = mpu.getXGyroOffsetTC();
pHysiX 6:2dc23167c8d8 76 int8_t ygOffsetTC = getYGyroOffsetTC();
pHysiX 6:2dc23167c8d8 77 int8_t zgOffsetTC = getZGyroOffsetTC();
pHysiX 6:2dc23167c8d8 78 DEBUG_PRINTF("X gyro offset = %u\n",xgOffset);
pHysiX 6:2dc23167c8d8 79 DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset);
pHysiX 6:2dc23167c8d8 80 DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset);
pHysiX 6:2dc23167c8d8 81 */
pHysiX 6:2dc23167c8d8 82 // setup weird slave stuff (?)
pHysiX 11:c248bdcece21 83 //DEBUG_PRINT("Setting slave 0 address to 0x7F...\n");
pHysiX 6:2dc23167c8d8 84 setSlaveAddress(0, 0x7F);
pHysiX 6:2dc23167c8d8 85
pHysiX 11:c248bdcece21 86 //DEBUG_PRINT("Disabling I2C Master mode...");
pHysiX 6:2dc23167c8d8 87 setI2CMasterModeEnabled(false);
pHysiX 11:c248bdcece21 88 //DEBUG_PRINT("Setting slave 0 address to 0x68 (self)...");
pHysiX 6:2dc23167c8d8 89 setSlaveAddress(0, 0x68);
pHysiX 11:c248bdcece21 90 //DEBUG_PRINT("Resetting I2C Master control...\n");
pHysiX 6:2dc23167c8d8 91 resetI2CMaster();
pHysiX 6:2dc23167c8d8 92
pHysiX 6:2dc23167c8d8 93 wait_ms(20);
pHysiX 6:2dc23167c8d8 94
pHysiX 6:2dc23167c8d8 95 // load DMP code into memory banks
pHysiX 11:c248bdcece21 96 //DEBUG_PRINT("Writing DMP code to MPU memory banks (");
pHysiX 11:c248bdcece21 97 //DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
pHysiX 11:c248bdcece21 98 //DEBUG_PRINT(" bytes)\n");
pHysiX 6:2dc23167c8d8 99 if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
pHysiX 11:c248bdcece21 100 //DEBUG_PRINT("Success! DMP code written and verified.\n");
pHysiX 6:2dc23167c8d8 101
pHysiX 6:2dc23167c8d8 102 // write DMP configuration
pHysiX 11:c248bdcece21 103 //DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
pHysiX 11:c248bdcece21 104 //DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE);
pHysiX 11:c248bdcece21 105 //DEBUG_PRINT(" bytes in config def)\n");
pHysiX 6:2dc23167c8d8 106 if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
pHysiX 11:c248bdcece21 107 //DEBUG_PRINT("Success! DMP configuration written and verified.\n");
pHysiX 6:2dc23167c8d8 108
pHysiX 11:c248bdcece21 109 //DEBUG_PRINT("Setting clock source to Z Gyro...\n");
pHysiX 6:2dc23167c8d8 110 setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
pHysiX 6:2dc23167c8d8 111
pHysiX 11:c248bdcece21 112 //DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
pHysiX 6:2dc23167c8d8 113 setIntEnabled(0x12);
pHysiX 6:2dc23167c8d8 114
pHysiX 11:c248bdcece21 115 //DEBUG_PRINT("Setting sample rate to 200Hz...");
pHysiX 8:5c62d55f3f23 116 setRate(IMU_SAMPLE_RATE_DIVIDER); // 1khz / (1 + 4) = 200 Hz
pHysiX 6:2dc23167c8d8 117
pHysiX 11:c248bdcece21 118 //DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
pHysiX 6:2dc23167c8d8 119 setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
pHysiX 6:2dc23167c8d8 120
pHysiX 11:c248bdcece21 121 //DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
pHysiX 6:2dc23167c8d8 122 setDLPFMode(MPU6050_DLPF_BW_42);
pHysiX 6:2dc23167c8d8 123
pHysiX 11:c248bdcece21 124 //DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
pHysiX 8:5c62d55f3f23 125 setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
pHysiX 6:2dc23167c8d8 126
pHysiX 11:c248bdcece21 127 //DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
pHysiX 6:2dc23167c8d8 128 setDMPConfig1(0x03);
pHysiX 6:2dc23167c8d8 129 setDMPConfig2(0x00);
pHysiX 6:2dc23167c8d8 130
pHysiX 11:c248bdcece21 131 //DEBUG_PRINT("Clearing OTP Bank flag...");
pHysiX 6:2dc23167c8d8 132 setOTPBankValid(false);
pHysiX 6:2dc23167c8d8 133
pHysiX 11:c248bdcece21 134 //DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n");
pHysiX 6:2dc23167c8d8 135 //setXGyroOffsetTC(xgOffsetTC);
pHysiX 6:2dc23167c8d8 136 //setYGyroOffsetTC(ygOffsetTC);
pHysiX 6:2dc23167c8d8 137 //setZGyroOffsetTC(zgOffsetTC);
pHysiX 6:2dc23167c8d8 138
pHysiX 6:2dc23167c8d8 139 //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
pHysiX 6:2dc23167c8d8 140 //setXGyroOffset(0);
pHysiX 6:2dc23167c8d8 141 //setYGyroOffset(0);
pHysiX 6:2dc23167c8d8 142 //setZGyroOffset(0);
pHysiX 6:2dc23167c8d8 143
pHysiX 11:c248bdcece21 144 //DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n");
pHysiX 6:2dc23167c8d8 145 uint8_t dmpUpdate[16], j;
pHysiX 6:2dc23167c8d8 146 uint16_t pos = 0;
pHysiX 6:2dc23167c8d8 147 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 148 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 149
pHysiX 11:c248bdcece21 150 //DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n");
pHysiX 6:2dc23167c8d8 151 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 152 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 153
pHysiX 11:c248bdcece21 154 //DEBUG_PRINT("Resetting FIFO...\n");
pHysiX 6:2dc23167c8d8 155 resetFIFO();
pHysiX 6:2dc23167c8d8 156
pHysiX 11:c248bdcece21 157 //DEBUG_PRINT("Reading FIFO count...\n");
pHysiX 6:2dc23167c8d8 158 uint16_t fifoCount = getFIFOCount();
pHysiX 6:2dc23167c8d8 159 uint8_t fifoBuffer[128];
pHysiX 6:2dc23167c8d8 160
pHysiX 11:c248bdcece21 161 //DEBUG_PRINT("Current FIFO count=");
pHysiX 11:c248bdcece21 162 //DEBUG_PRINTF("%u\n",fifoCount);
pHysiX 6:2dc23167c8d8 163 getFIFOBytes(fifoBuffer, fifoCount);
pHysiX 6:2dc23167c8d8 164
pHysiX 11:c248bdcece21 165 //DEBUG_PRINT("Setting motion detection threshold to 2...\n");
pHysiX 6:2dc23167c8d8 166 setMotionDetectionThreshold(2);
pHysiX 6:2dc23167c8d8 167
pHysiX 11:c248bdcece21 168 //DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
pHysiX 6:2dc23167c8d8 169 setZeroMotionDetectionThreshold(156);
pHysiX 6:2dc23167c8d8 170
pHysiX 11:c248bdcece21 171 //DEBUG_PRINT("Setting motion detection duration to 80...");
pHysiX 6:2dc23167c8d8 172 setMotionDetectionDuration(80);
pHysiX 6:2dc23167c8d8 173
pHysiX 11:c248bdcece21 174 //DEBUG_PRINT("Setting zero-motion detection duration to 0...");
pHysiX 6:2dc23167c8d8 175 setZeroMotionDetectionDuration(0);
pHysiX 6:2dc23167c8d8 176
pHysiX 11:c248bdcece21 177 //DEBUG_PRINT("Resetting FIFO...\n");
pHysiX 6:2dc23167c8d8 178 resetFIFO();
pHysiX 6:2dc23167c8d8 179
pHysiX 11:c248bdcece21 180 //DEBUG_PRINT("Enabling FIFO...\n");
pHysiX 6:2dc23167c8d8 181 setFIFOEnabled(true);
pHysiX 6:2dc23167c8d8 182
pHysiX 11:c248bdcece21 183 //DEBUG_PRINT("Enabling DMP...\n");
pHysiX 6:2dc23167c8d8 184 setDMPEnabled(true);
pHysiX 6:2dc23167c8d8 185
pHysiX 11:c248bdcece21 186 //DEBUG_PRINT("Resetting DMP...\n");
pHysiX 6:2dc23167c8d8 187 resetDMP();
pHysiX 6:2dc23167c8d8 188
pHysiX 11:c248bdcece21 189 //DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n");
pHysiX 6:2dc23167c8d8 190 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 191 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 192
pHysiX 11:c248bdcece21 193 //DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n");
pHysiX 6:2dc23167c8d8 194 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 195 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 196
pHysiX 11:c248bdcece21 197 //DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n");
pHysiX 6:2dc23167c8d8 198 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 199 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 200
pHysiX 11:c248bdcece21 201 //DEBUG_PRINT("Waiting for FIFO count > 2...\n");
pHysiX 6:2dc23167c8d8 202 while ((fifoCount = getFIFOCount()) < 3);
pHysiX 6:2dc23167c8d8 203
pHysiX 11:c248bdcece21 204 //DEBUG_PRINT("Current FIFO count=");
pHysiX 11:c248bdcece21 205 //DEBUG_PRINTF("%u\n",fifoCount);
pHysiX 11:c248bdcece21 206 //DEBUG_PRINT("Reading FIFO data...\n");
pHysiX 6:2dc23167c8d8 207 getFIFOBytes(fifoBuffer, fifoCount);
pHysiX 6:2dc23167c8d8 208
pHysiX 11:c248bdcece21 209 //DEBUG_PRINT("Reading interrupt status...\n");
pHysiX 6:2dc23167c8d8 210 uint8_t mpuIntStatus = getIntStatus();
pHysiX 6:2dc23167c8d8 211
pHysiX 11:c248bdcece21 212 //DEBUG_PRINT("Current interrupt status=");
pHysiX 11:c248bdcece21 213 //DEBUG_PRINTF("%x\n",mpuIntStatus);
pHysiX 6:2dc23167c8d8 214
pHysiX 11:c248bdcece21 215 //DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n");
pHysiX 6:2dc23167c8d8 216 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 217 readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 218
pHysiX 11:c248bdcece21 219 //DEBUG_PRINT("Waiting for FIFO count > 2...\n");
pHysiX 6:2dc23167c8d8 220 while ((fifoCount = getFIFOCount()) < 3);
pHysiX 6:2dc23167c8d8 221
pHysiX 11:c248bdcece21 222 //DEBUG_PRINT("Current FIFO count=");
pHysiX 11:c248bdcece21 223 //DEBUG_PRINTF("%u\n",fifoCount);
pHysiX 6:2dc23167c8d8 224
pHysiX 11:c248bdcece21 225 //DEBUG_PRINT("Reading FIFO data...\n");
pHysiX 6:2dc23167c8d8 226 getFIFOBytes(fifoBuffer, fifoCount);
pHysiX 6:2dc23167c8d8 227
pHysiX 11:c248bdcece21 228 //DEBUG_PRINT("Reading interrupt status...\n");
pHysiX 6:2dc23167c8d8 229 mpuIntStatus = getIntStatus();
pHysiX 6:2dc23167c8d8 230
pHysiX 11:c248bdcece21 231 //DEBUG_PRINT("Current interrupt status=");
pHysiX 11:c248bdcece21 232 //DEBUG_PRINTF("%x\n",mpuIntStatus);
pHysiX 6:2dc23167c8d8 233
pHysiX 11:c248bdcece21 234 //DEBUG_PRINT("Writing final memory update 7/7 (function unknown)...");
pHysiX 6:2dc23167c8d8 235 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
pHysiX 6:2dc23167c8d8 236 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
pHysiX 6:2dc23167c8d8 237
pHysiX 11:c248bdcece21 238 //DEBUG_PRINT("DMP is good to go! Finally.\n");
pHysiX 6:2dc23167c8d8 239
pHysiX 11:c248bdcece21 240 //DEBUG_PRINT("Disabling DMP (you turn it on later)...\n");
pHysiX 6:2dc23167c8d8 241 setDMPEnabled(false);
pHysiX 6:2dc23167c8d8 242
pHysiX 11:c248bdcece21 243 //DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n");
pHysiX 6:2dc23167c8d8 244 dmpPacketSize = 42;
pHysiX 6:2dc23167c8d8 245 /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
pHysiX 6:2dc23167c8d8 246 return 3; // TODO: proper error code for no memory
pHysiX 6:2dc23167c8d8 247 }*/
pHysiX 6:2dc23167c8d8 248
pHysiX 11:c248bdcece21 249 //DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
pHysiX 6:2dc23167c8d8 250 resetFIFO();
pHysiX 6:2dc23167c8d8 251 getIntStatus();
pHysiX 6:2dc23167c8d8 252 } else {
pHysiX 11:c248bdcece21 253 //DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
pHysiX 6:2dc23167c8d8 254 return 2; // configuration block loading failed
pHysiX 6:2dc23167c8d8 255 }
pHysiX 6:2dc23167c8d8 256 } else {
pHysiX 11:c248bdcece21 257 //DEBUG_PRINT("ERROR! DMP code verification failed.");
pHysiX 6:2dc23167c8d8 258 return 1; // main binary block loading failed
pHysiX 6:2dc23167c8d8 259 }
pHysiX 6:2dc23167c8d8 260 return 0; // success
pHysiX 6:2dc23167c8d8 261 }
pHysiX 6:2dc23167c8d8 262
pHysiX 6:2dc23167c8d8 263 bool MPU6050::dmpPacketAvailable()
pHysiX 6:2dc23167c8d8 264 {
pHysiX 6:2dc23167c8d8 265 return getFIFOCount() >= dmpGetFIFOPacketSize();
pHysiX 6:2dc23167c8d8 266 }
pHysiX 6:2dc23167c8d8 267
pHysiX 6:2dc23167c8d8 268 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
pHysiX 6:2dc23167c8d8 269 // uint8_t MPU6050::dmpGetFIFORate();
pHysiX 6:2dc23167c8d8 270 // uint8_t MPU6050::dmpGetSampleStepSizeMS();
pHysiX 6:2dc23167c8d8 271 // uint8_t MPU6050::dmpGetSampleFrequency();
pHysiX 6:2dc23167c8d8 272 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
pHysiX 6:2dc23167c8d8 273
pHysiX 6:2dc23167c8d8 274 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
pHysiX 6:2dc23167c8d8 275 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
pHysiX 6:2dc23167c8d8 276 //uint8_t MPU6050::dmpRunFIFORateProcesses();
pHysiX 6:2dc23167c8d8 277
pHysiX 6:2dc23167c8d8 278 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 279 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 280 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 281 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 282 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 283 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 284 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 285 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 286 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 287 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 288 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 289 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
pHysiX 6:2dc23167c8d8 290
pHysiX 6:2dc23167c8d8 291 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 292 {
pHysiX 6:2dc23167c8d8 293 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 294 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 295 data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]);
pHysiX 6:2dc23167c8d8 296 data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]);
pHysiX 6:2dc23167c8d8 297 data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]);
pHysiX 6:2dc23167c8d8 298 return 0;
pHysiX 6:2dc23167c8d8 299 }
pHysiX 6:2dc23167c8d8 300 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 301 {
pHysiX 6:2dc23167c8d8 302 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 303 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 304 data[0] = (packet[28] << 8) + packet[29];
pHysiX 6:2dc23167c8d8 305 data[1] = (packet[32] << 8) + packet[33];
pHysiX 6:2dc23167c8d8 306 data[2] = (packet[36] << 8) + packet[37];
pHysiX 6:2dc23167c8d8 307 return 0;
pHysiX 6:2dc23167c8d8 308 }
pHysiX 6:2dc23167c8d8 309 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 310 {
pHysiX 6:2dc23167c8d8 311 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 312 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 313 v -> x = (packet[28] << 8) + packet[29];
pHysiX 6:2dc23167c8d8 314 v -> y = (packet[32] << 8) + packet[33];
pHysiX 6:2dc23167c8d8 315 v -> z = (packet[36] << 8) + packet[37];
pHysiX 6:2dc23167c8d8 316 return 0;
pHysiX 6:2dc23167c8d8 317 }
pHysiX 6:2dc23167c8d8 318 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 319 {
pHysiX 6:2dc23167c8d8 320 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 321 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 322 data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
pHysiX 6:2dc23167c8d8 323 data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
pHysiX 6:2dc23167c8d8 324 data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
pHysiX 6:2dc23167c8d8 325 data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
pHysiX 6:2dc23167c8d8 326 return 0;
pHysiX 6:2dc23167c8d8 327 }
pHysiX 6:2dc23167c8d8 328 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 329 {
pHysiX 6:2dc23167c8d8 330 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 331 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 332 data[0] = ((packet[0] << 8) + packet[1]);
pHysiX 6:2dc23167c8d8 333 data[1] = ((packet[4] << 8) + packet[5]);
pHysiX 6:2dc23167c8d8 334 data[2] = ((packet[8] << 8) + packet[9]);
pHysiX 6:2dc23167c8d8 335 data[3] = ((packet[12] << 8) + packet[13]);
pHysiX 6:2dc23167c8d8 336 return 0;
pHysiX 6:2dc23167c8d8 337 }
pHysiX 6:2dc23167c8d8 338 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 339 {
pHysiX 6:2dc23167c8d8 340 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 341 int16_t qI[4];
pHysiX 6:2dc23167c8d8 342 uint8_t status = dmpGetQuaternion(qI, packet);
pHysiX 6:2dc23167c8d8 343 if (status == 0) {
pHysiX 6:2dc23167c8d8 344 q -> w = (float)qI[0] / 16384.0f;
pHysiX 6:2dc23167c8d8 345 q -> x = (float)qI[1] / 16384.0f;
pHysiX 6:2dc23167c8d8 346 q -> y = (float)qI[2] / 16384.0f;
pHysiX 6:2dc23167c8d8 347 q -> z = (float)qI[3] / 16384.0f;
pHysiX 6:2dc23167c8d8 348 return 0;
pHysiX 6:2dc23167c8d8 349 }
pHysiX 6:2dc23167c8d8 350 return status; // int16 return value, indicates error if this line is reached
pHysiX 6:2dc23167c8d8 351 }
pHysiX 6:2dc23167c8d8 352 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 353 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 354 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 355 {
pHysiX 6:2dc23167c8d8 356 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 357 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 358 data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
pHysiX 6:2dc23167c8d8 359 data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
pHysiX 6:2dc23167c8d8 360 data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
pHysiX 6:2dc23167c8d8 361 return 0;
pHysiX 6:2dc23167c8d8 362 }
pHysiX 6:2dc23167c8d8 363 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet)
pHysiX 6:2dc23167c8d8 364 {
pHysiX 6:2dc23167c8d8 365 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
pHysiX 6:2dc23167c8d8 366 if (packet == 0) packet = dmpPacketBuffer;
pHysiX 6:2dc23167c8d8 367 data[0] = (packet[16] << 8) + packet[17];
pHysiX 6:2dc23167c8d8 368 data[1] = (packet[20] << 8) + packet[21];
pHysiX 6:2dc23167c8d8 369 data[2] = (packet[24] << 8) + packet[25];
pHysiX 6:2dc23167c8d8 370 return 0;
pHysiX 6:2dc23167c8d8 371 }
pHysiX 6:2dc23167c8d8 372 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
pHysiX 6:2dc23167c8d8 373 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 374 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity)
pHysiX 6:2dc23167c8d8 375 {
pHysiX 6:2dc23167c8d8 376 // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
pHysiX 6:2dc23167c8d8 377 v -> x = vRaw -> x - gravity -> x*8192;
pHysiX 6:2dc23167c8d8 378 v -> y = vRaw -> y - gravity -> y*8192;
pHysiX 6:2dc23167c8d8 379 v -> z = vRaw -> z - gravity -> z*8192;
pHysiX 6:2dc23167c8d8 380 return 0;
pHysiX 6:2dc23167c8d8 381 }
pHysiX 6:2dc23167c8d8 382 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 383 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q)
pHysiX 6:2dc23167c8d8 384 {
pHysiX 6:2dc23167c8d8 385 // rotate measured 3D acceleration vector into original state
pHysiX 6:2dc23167c8d8 386 // frame of reference based on orientation quaternion
pHysiX 6:2dc23167c8d8 387 memcpy(v, vReal, sizeof(VectorInt16));
pHysiX 6:2dc23167c8d8 388 v -> rotate(q);
pHysiX 6:2dc23167c8d8 389 return 0;
pHysiX 6:2dc23167c8d8 390 }
pHysiX 6:2dc23167c8d8 391 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 392 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 393 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 394 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 395 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 396 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q)
pHysiX 6:2dc23167c8d8 397 {
pHysiX 6:2dc23167c8d8 398 v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
pHysiX 6:2dc23167c8d8 399 v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
pHysiX 6:2dc23167c8d8 400 v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
pHysiX 6:2dc23167c8d8 401 return 0;
pHysiX 6:2dc23167c8d8 402 }
pHysiX 6:2dc23167c8d8 403 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 404 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 405 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 406 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 407
pHysiX 6:2dc23167c8d8 408 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q)
pHysiX 6:2dc23167c8d8 409 {
pHysiX 6:2dc23167c8d8 410 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
pHysiX 6:2dc23167c8d8 411 data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
pHysiX 6:2dc23167c8d8 412 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
pHysiX 6:2dc23167c8d8 413 return 0;
pHysiX 6:2dc23167c8d8 414 }
pHysiX 6:2dc23167c8d8 415
pHysiX 6:2dc23167c8d8 416 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity)
pHysiX 6:2dc23167c8d8 417 {
pHysiX 6:2dc23167c8d8 418 // yaw: (about Z axis)
pHysiX 6:2dc23167c8d8 419 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
pHysiX 6:2dc23167c8d8 420 // pitch: (nose up/down, about Y axis)
pHysiX 6:2dc23167c8d8 421 data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
pHysiX 6:2dc23167c8d8 422 // roll: (tilt left/right, about X axis)
pHysiX 6:2dc23167c8d8 423 data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
pHysiX 6:2dc23167c8d8 424 return 0;
pHysiX 6:2dc23167c8d8 425 }
pHysiX 6:2dc23167c8d8 426
pHysiX 6:2dc23167c8d8 427 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 428 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
pHysiX 6:2dc23167c8d8 429
pHysiX 6:2dc23167c8d8 430 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData)
pHysiX 6:2dc23167c8d8 431 {
pHysiX 6:2dc23167c8d8 432 /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
pHysiX 6:2dc23167c8d8 433 if (dmpData[k] < 0x10) Serial.print("0");
pHysiX 6:2dc23167c8d8 434 Serial.print(dmpData[k], HEX);
pHysiX 6:2dc23167c8d8 435 Serial.print(" ");
pHysiX 6:2dc23167c8d8 436 }
pHysiX 6:2dc23167c8d8 437 Serial.print("\n");*/
pHysiX 6:2dc23167c8d8 438 //Serial.println((uint16_t)dmpPacketBuffer);
pHysiX 6:2dc23167c8d8 439 return 0;
pHysiX 6:2dc23167c8d8 440 }
pHysiX 6:2dc23167c8d8 441 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed)
pHysiX 6:2dc23167c8d8 442 {
pHysiX 6:2dc23167c8d8 443 uint8_t status;
pHysiX 6:2dc23167c8d8 444 uint8_t buf[dmpPacketSize];
pHysiX 6:2dc23167c8d8 445 for (uint8_t i = 0; i < numPackets; i++) {
pHysiX 6:2dc23167c8d8 446 // read packet from FIFO
pHysiX 6:2dc23167c8d8 447 getFIFOBytes(buf, dmpPacketSize);
pHysiX 6:2dc23167c8d8 448
pHysiX 6:2dc23167c8d8 449 // process packet
pHysiX 6:2dc23167c8d8 450 if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
pHysiX 6:2dc23167c8d8 451
pHysiX 6:2dc23167c8d8 452 // increment external process count variable, if supplied
pHysiX 6:2dc23167c8d8 453 if (processed != 0) *processed++;
pHysiX 6:2dc23167c8d8 454 }
pHysiX 6:2dc23167c8d8 455 return 0;
pHysiX 6:2dc23167c8d8 456 }
pHysiX 6:2dc23167c8d8 457
pHysiX 6:2dc23167c8d8 458 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
pHysiX 6:2dc23167c8d8 459
pHysiX 6:2dc23167c8d8 460 // uint8_t MPU6050::dmpInitFIFOParam();
pHysiX 6:2dc23167c8d8 461 // uint8_t MPU6050::dmpCloseFIFO();
pHysiX 6:2dc23167c8d8 462 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
pHysiX 6:2dc23167c8d8 463 // uint8_t MPU6050::dmpDecodeQuantizedAccel();
pHysiX 6:2dc23167c8d8 464 // uint32_t MPU6050::dmpGetGyroSumOfSquare();
pHysiX 6:2dc23167c8d8 465 // uint32_t MPU6050::dmpGetAccelSumOfSquare();
pHysiX 6:2dc23167c8d8 466 // void MPU6050::dmpOverrideQuaternion(long *q);
pHysiX 6:2dc23167c8d8 467 uint16_t MPU6050::dmpGetFIFOPacketSize()
pHysiX 6:2dc23167c8d8 468 {
pHysiX 6:2dc23167c8d8 469 return dmpPacketSize;
pHysiX 6:2dc23167c8d8 470 }