Akash Vibhute / MPU6050_DMP_Nucleo-I2Cdev

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   MPU9150_nucleo_i2cdev Orion_newPCB_test_LV Orion_PCB_test_Faulhaber_gr41_wptcmd_V1 MPU9150_nucleo_i2cdev ... more

Fork of MPU6050-DMP-Ian by Ian Hua

MPU/MPU6050_6Axis_MotionApps20.cpp@6:2dc23167c8d8, 2014-04-29 (annotated)

Committer:
pHysiX
Date:
Tue Apr 29 10:36:21 2014 +0000
Revision:
6:2dc23167c8d8
Working in RTOS

Who changed what in which revision?

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