Mark Vandermeulen
Code for our FYDP -only one IMU works right now -RTOS is working
Embed:
(wiki syntax)
Show/hide line numbers
MPU6050_6Axis_MotionApps20.cpp
00001 // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 00002 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 00003 // 5/20/2013 by Jeff Rowberg <jeff@rowberg.net> 00004 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 00005 // 00006 // Changelog: 00007 // ... - ongoing debug release 00008 00009 /* ============================================ 00010 I2Cdev device library code is placed under the MIT license 00011 Copyright (c) 2012 Jeff Rowberg 00012 00013 Permission is hereby granted, free of charge, to any person obtaining a copy 00014 of this software and associated documentation files (the "Software"), to deal 00015 in the Software without restriction, including without limitation the rights 00016 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00017 copies of the Software, and to permit persons to whom the Software is 00018 furnished to do so, subject to the following conditions: 00019 00020 The above copyright notice and this permission notice shall be included in 00021 all copies or substantial portions of the Software. 00022 00023 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00024 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00025 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00026 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00027 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00028 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00029 THE SOFTWARE. 00030 =============================================== 00031 */ 00032 00033 #include "MPU6050_6Axis_MotionApps20.h" 00034 00035 uint8_t MPU6050::dmpInitialize() 00036 { 00037 // reset device 00038 wait_ms(50); 00039 reset(); 00040 wait_ms(30); 00041 00042 // enable sleep mode and wake cycle 00043 /*Serial.println(F("Enabling sleep mode...")); 00044 setSleepEnabled(true); 00045 Serial.println(F("Enabling wake cycle...")); 00046 setWakeCycleEnabled(true);*/ 00047 00048 // disable sleep mode 00049 //DEBUG_PRINT("Disabling sleep mode...\n"); 00050 setSleepEnabled(false); 00051 00052 // get MPU hardware revision 00053 //DEBUG_PRINT("Selecting user bank 16...\n"); 00054 setMemoryBank(0x10, true, true); 00055 //DEBUG_PRINT("Selecting memory byte 6...\n"); 00056 setMemoryStartAddress(0x06); 00057 //DEBUG_PRINT("Checking hardware revision...\n"); 00058 uint8_t hwRevision = readMemoryByte(); 00059 //DEBUG_PRINT("Revision @ user[16][6] = "); 00060 //DEBUG_PRINTF("%x\n",hwRevision); 00061 //DEBUG_PRINT("Resetting memory bank selection to 0...\n"); 00062 setMemoryBank(0, false, false); 00063 00064 // check OTP bank valid 00065 //DEBUG_PRINT("Reading OTP bank valid flag...\n"); 00066 uint8_t otpValid = getOTPBankValid(); 00067 00068 //DEBUG_PRINT("OTP bank is "); 00069 if(otpValid); //DEBUG_PRINT("valid!\n"); 00070 else; //DEBUG_PRINT("invalid!\n"); 00071 00072 // get X/Y/Z gyro offsets 00073 /* 00074 DEBUG_PRINT("\nReading gyro offset TC values...\n"); 00075 int8_t xgOffsetTC = mpu.getXGyroOffsetTC(); 00076 int8_t ygOffsetTC = getYGyroOffsetTC(); 00077 int8_t zgOffsetTC = getZGyroOffsetTC(); 00078 DEBUG_PRINTF("X gyro offset = %u\n",xgOffset); 00079 DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset); 00080 DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset); 00081 */ 00082 // setup weird slave stuff (?) 00083 //DEBUG_PRINT("Setting slave 0 address to 0x7F...\n"); 00084 setSlaveAddress(0, 0x7F); 00085 00086 //DEBUG_PRINT("Disabling I2C Master mode..."); 00087 setI2CMasterModeEnabled(false); 00088 //DEBUG_PRINT("Setting slave 0 address to 0x68 (self)..."); 00089 setSlaveAddress(0, 0x68); 00090 //DEBUG_PRINT("Resetting I2C Master control...\n"); 00091 resetI2CMaster(); 00092 00093 wait_ms(20); 00094 00095 // load DMP code into memory banks 00096 //DEBUG_PRINT("Writing DMP code to MPU memory banks ("); 00097 //DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE); 00098 //DEBUG_PRINT(" bytes)\n"); 00099 if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 00100 //DEBUG_PRINT("Success! DMP code written and verified.\n"); 00101 00102 // write DMP configuration 00103 //DEBUG_PRINT("Writing DMP configuration to MPU memory banks ("); 00104 //DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE); 00105 //DEBUG_PRINT(" bytes in config def)\n"); 00106 if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 00107 //DEBUG_PRINT("Success! DMP configuration written and verified.\n"); 00108 00109 //DEBUG_PRINT("Setting clock source to Z Gyro...\n"); 00110 setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 00111 00112 //DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n"); 00113 setIntEnabled(0x12); 00114 00115 //DEBUG_PRINT("Setting sample rate to 200Hz..."); 00116 setRate(4); // 1khz / (1 + 4) = 200 Hz 00117 00118 //DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n"); 00119 setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 00120 00121 //DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n"); 00122 setDLPFMode(MPU6050_DLPF_BW_42); 00123 00124 //DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n"); 00125 setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 00126 00127 //DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n"); 00128 setDMPConfig1(0x03); 00129 setDMPConfig2(0x00); 00130 00131 //DEBUG_PRINT("Clearing OTP Bank flag..."); 00132 setOTPBankValid(false); 00133 00134 //DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n"); 00135 //setXGyroOffsetTC(xgOffsetTC); 00136 //setYGyroOffsetTC(ygOffsetTC); 00137 //setZGyroOffsetTC(zgOffsetTC); 00138 00139 //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 00140 //setXGyroOffset(0); 00141 //setYGyroOffset(0); 00142 //setZGyroOffset(0); 00143 00144 //DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n"); 00145 uint8_t dmpUpdate[16], j; 00146 uint16_t pos = 0; 00147 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00148 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00149 00150 //DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n"); 00151 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00152 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00153 00154 //DEBUG_PRINT("Resetting FIFO...\n"); 00155 resetFIFO(); 00156 00157 //DEBUG_PRINT("Reading FIFO count...\n"); 00158 uint16_t fifoCount = getFIFOCount(); 00159 uint8_t fifoBuffer[128]; 00160 00161 //DEBUG_PRINT("Current FIFO count="); 00162 //DEBUG_PRINTF("%u\n",fifoCount); 00163 getFIFOBytes(fifoBuffer, fifoCount); 00164 00165 //DEBUG_PRINT("Setting motion detection threshold to 2...\n"); 00166 setMotionDetectionThreshold(2); 00167 00168 //DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n"); 00169 setZeroMotionDetectionThreshold(156); 00170 00171 //DEBUG_PRINT("Setting motion detection duration to 80..."); 00172 setMotionDetectionDuration(80); 00173 00174 //DEBUG_PRINT("Setting zero-motion detection duration to 0..."); 00175 setZeroMotionDetectionDuration(0); 00176 00177 //DEBUG_PRINT("Resetting FIFO...\n"); 00178 resetFIFO(); 00179 00180 //DEBUG_PRINT("Enabling FIFO...\n"); 00181 setFIFOEnabled(true); 00182 00183 //DEBUG_PRINT("Enabling DMP...\n"); 00184 setDMPEnabled(true); 00185 00186 //DEBUG_PRINT("Resetting DMP...\n"); 00187 resetDMP(); 00188 00189 //DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n"); 00190 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00191 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00192 00193 //DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n"); 00194 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00195 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00196 00197 //DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n"); 00198 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00199 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00200 00201 //DEBUG_PRINT("Waiting for FIFO count > 2...\n"); 00202 while ((fifoCount = getFIFOCount()) < 3); 00203 00204 //DEBUG_PRINT("Current FIFO count="); 00205 //DEBUG_PRINTF("%u\n",fifoCount); 00206 //DEBUG_PRINT("Reading FIFO data...\n"); 00207 getFIFOBytes(fifoBuffer, fifoCount); 00208 00209 //DEBUG_PRINT("Reading interrupt status...\n"); 00210 uint8_t mpuIntStatus = getIntStatus(); 00211 00212 //DEBUG_PRINT("Current interrupt status="); 00213 //DEBUG_PRINTF("%x\n",mpuIntStatus); 00214 00215 //DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n"); 00216 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00217 readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00218 00219 //DEBUG_PRINT("Waiting for FIFO count > 2...\n"); 00220 while ((fifoCount = getFIFOCount()) < 3); 00221 00222 //DEBUG_PRINT("Current FIFO count="); 00223 //DEBUG_PRINTF("%u\n",fifoCount); 00224 00225 //DEBUG_PRINT("Reading FIFO data...\n"); 00226 getFIFOBytes(fifoBuffer, fifoCount); 00227 00228 //DEBUG_PRINT("Reading interrupt status...\n"); 00229 mpuIntStatus = getIntStatus(); 00230 00231 //DEBUG_PRINT("Current interrupt status="); 00232 //DEBUG_PRINTF("%x\n",mpuIntStatus); 00233 00234 //DEBUG_PRINT("Writing final memory update 7/7 (function unknown)..."); 00235 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00236 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00237 00238 //DEBUG_PRINT("DMP is good to go! Finally.\n"); 00239 00240 //DEBUG_PRINT("Disabling DMP (you turn it on later)...\n"); 00241 setDMPEnabled(false); 00242 00243 //DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n"); 00244 dmpPacketSize = 42; 00245 /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 00246 return 3; // TODO: proper error code for no memory 00247 }*/ 00248 00249 //DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n"); 00250 resetFIFO(); 00251 getIntStatus(); 00252 } else { 00253 //DEBUG_PRINT("ERROR! DMP configuration verification failed.\n"); 00254 return 2; // configuration block loading failed 00255 } 00256 } else { 00257 //DEBUG_PRINT("ERROR! DMP code verification failed."); 00258 return 1; // main binary block loading failed 00259 } 00260 return 0; // success 00261 } 00262 00263 bool MPU6050::dmpPacketAvailable() 00264 { 00265 return getFIFOCount() >= dmpGetFIFOPacketSize(); 00266 } 00267 00268 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 00269 // uint8_t MPU6050::dmpGetFIFORate(); 00270 // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 00271 // uint8_t MPU6050::dmpGetSampleFrequency(); 00272 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 00273 00274 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 00275 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 00276 //uint8_t MPU6050::dmpRunFIFORateProcesses(); 00277 00278 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 00279 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 00280 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00281 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00282 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 00283 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 00284 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 00285 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 00286 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 00287 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 00288 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00289 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 00290 00291 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) 00292 { 00293 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00294 if (packet == 0) packet = dmpPacketBuffer; 00295 data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 00296 data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 00297 data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 00298 return 0; 00299 } 00300 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) 00301 { 00302 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00303 if (packet == 0) packet = dmpPacketBuffer; 00304 data[0] = (packet[28] << 8) + packet[29]; 00305 data[1] = (packet[32] << 8) + packet[33]; 00306 data[2] = (packet[36] << 8) + packet[37]; 00307 return 0; 00308 } 00309 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) 00310 { 00311 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00312 if (packet == 0) packet = dmpPacketBuffer; 00313 v -> x = (packet[28] << 8) + packet[29]; 00314 v -> y = (packet[32] << 8) + packet[33]; 00315 v -> z = (packet[36] << 8) + packet[37]; 00316 return 0; 00317 } 00318 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) 00319 { 00320 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00321 if (packet == 0) packet = dmpPacketBuffer; 00322 data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 00323 data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 00324 data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 00325 data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 00326 return 0; 00327 } 00328 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) 00329 { 00330 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00331 if (packet == 0) packet = dmpPacketBuffer; 00332 data[0] = ((packet[0] << 8) + packet[1]); 00333 data[1] = ((packet[4] << 8) + packet[5]); 00334 data[2] = ((packet[8] << 8) + packet[9]); 00335 data[3] = ((packet[12] << 8) + packet[13]); 00336 return 0; 00337 } 00338 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) 00339 { 00340 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00341 int16_t qI[4]; 00342 uint8_t status = dmpGetQuaternion(qI, packet); 00343 if (status == 0) { 00344 q -> w = (float)qI[0] / 16384.0f; 00345 q -> x = (float)qI[1] / 16384.0f; 00346 q -> y = (float)qI[2] / 16384.0f; 00347 q -> z = (float)qI[3] / 16384.0f; 00348 return 0; 00349 } 00350 return status; // int16 return value, indicates error if this line is reached 00351 } 00352 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 00353 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 00354 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) 00355 { 00356 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00357 if (packet == 0) packet = dmpPacketBuffer; 00358 data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 00359 data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 00360 data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 00361 return 0; 00362 } 00363 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) 00364 { 00365 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00366 if (packet == 0) packet = dmpPacketBuffer; 00367 data[0] = (packet[16] << 8) + packet[17]; 00368 data[1] = (packet[20] << 8) + packet[21]; 00369 data[2] = (packet[24] << 8) + packet[25]; 00370 return 0; 00371 } 00372 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 00373 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 00374 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) 00375 { 00376 // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 00377 v -> x = vRaw -> x - gravity -> x*8192; 00378 v -> y = vRaw -> y - gravity -> y*8192; 00379 v -> z = vRaw -> z - gravity -> z*8192; 00380 return 0; 00381 } 00382 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 00383 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) 00384 { 00385 // rotate measured 3D acceleration vector into original state 00386 // frame of reference based on orientation quaternion 00387 memcpy(v, vReal, sizeof(VectorInt16)); 00388 v -> rotate(q); 00389 return 0; 00390 } 00391 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 00392 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 00393 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 00394 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 00395 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 00396 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) 00397 { 00398 v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 00399 v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 00400 v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 00401 return 0; 00402 } 00403 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 00404 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 00405 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 00406 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 00407 00408 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) 00409 { 00410 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 00411 data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 00412 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 00413 return 0; 00414 } 00415 00416 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) 00417 { 00418 // yaw: (about Z axis) 00419 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 00420 // pitch: (nose up/down, about Y axis) 00421 data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 00422 // roll: (tilt left/right, about X axis) 00423 data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 00424 return 0; 00425 } 00426 00427 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 00428 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 00429 00430 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) 00431 { 00432 /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 00433 if (dmpData[k] < 0x10) Serial.print("0"); 00434 Serial.print(dmpData[k], HEX); 00435 Serial.print(" "); 00436 } 00437 Serial.print("\n");*/ 00438 //Serial.println((uint16_t)dmpPacketBuffer); 00439 return 0; 00440 } 00441 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) 00442 { 00443 uint8_t status; 00444 uint8_t buf[dmpPacketSize]; 00445 for (uint8_t i = 0; i < numPackets; i++) { 00446 // read packet from FIFO 00447 getFIFOBytes(buf, dmpPacketSize); 00448 00449 // process packet 00450 if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 00451 00452 // increment external process count variable, if supplied 00453 if (processed != 0) *processed++; 00454 } 00455 return 0; 00456 } 00457 00458 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 00459 00460 // uint8_t MPU6050::dmpInitFIFOParam(); 00461 // uint8_t MPU6050::dmpCloseFIFO(); 00462 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 00463 // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 00464 // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 00465 // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 00466 // void MPU6050::dmpOverrideQuaternion(long *q); 00467 uint16_t MPU6050::dmpGetFIFOPacketSize() 00468 { 00469 return dmpPacketSize; 00470 }
Generated on Wed Jul 13 2022 10:32:23 by
1.7.2