Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

drivers/MPU6050-DMP-Ian/MPU/src/MPU6050_6Axis_MotionApps20.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

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