Mark Vandermeulen / MPU6050-DMP-Ian

dsafeadf

Dependents:   DMP_tidy_working

Fork of MPU6050-DMP-Ian by Ian Hua

MPU/src/MPU6050_6Axis_MotionApps20.cpp@14:3f0bc381e1dd, 2014-05-27 (annotated)

Committer:
majik
Date:
Tue May 27 09:21:09 2014 +0000
Revision:
14:3f0bc381e1dd
Parent:
13:b1b98e5c61df 
tempory code

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...");
majik 14:3f0bc381e1dd 116 setRate(4); // 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 }