Rev 2194 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2194 | - | 1 | /* |
| 2 | MPU6050 lib 0x02 |
||
| 3 | |||
| 4 | copyright (c) Davide Gironi, 2012 |
||
| 5 | |||
| 6 | Released under GPLv3. |
||
| 7 | Please refer to LICENSE file for licensing information. |
||
| 8 | */ |
||
| 9 | |||
| 2198 | - | 10 | //############################################################################ |
| 11 | //# HISTORY mpu6050dmp6.c |
||
| 12 | //# |
||
| 13 | //# 19.09.2015 cebra |
||
| 14 | //# - add: Library für MPU6050 Gyro, ACC, DMP-Routinen für IMU, GY-87 Sensorboard |
||
| 15 | //# |
||
| 16 | //############################################################################ |
||
| 17 | |||
| 18 | #ifdef USE_KOMPASS |
||
| 2194 | - | 19 | /* |
| 20 | * This file contains all the functions needed to process 6-axis orientation by the internal chip processor |
||
| 21 | */ |
||
| 22 | |||
| 23 | //to enable get roll, pitch and yaw function we include the math function |
||
| 24 | //if some error appears we must add -lm and -lc to C linker, the linker line should become somethink like this |
||
| 25 | //${COMMAND} -lm ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS} -lc |
||
| 26 | |||
| 27 | |||
| 28 | #include "mpu6050.h" |
||
| 29 | |||
| 30 | #if MPU6050_GETATTITUDE == 2 |
||
| 31 | |||
| 32 | #include <avr/io.h> |
||
| 33 | #include <avr/pgmspace.h> |
||
| 34 | #include <avr/interrupt.h> |
||
| 35 | #include <util/delay.h> |
||
| 36 | #include <math.h> //include libm |
||
| 37 | |||
| 38 | #define MPU6050_DMP_CODE_SIZE 1929 |
||
| 39 | #define MPU6050_DMP_CONFIG_SIZE 192 |
||
| 40 | #define MPU6050_DMP_UPDATES_SIZE 47 |
||
| 41 | |||
| 42 | volatile uint8_t mpu6050_mpuInterrupt = 0; |
||
| 43 | uint8_t *dmpPacketBuffer; |
||
| 44 | uint16_t mpu6050_fifoCount = 0; |
||
| 45 | uint8_t mpu6050_mpuIntStatus = 0; |
||
| 46 | uint8_t mpu6050_fifoBuffer[64]; |
||
| 47 | |||
| 48 | |||
| 49 | /* ================================================================================================ * |
||
| 50 | | Default MotionApps v2.0 42-byte FIFO packet structure: | |
||
| 51 | | | |
||
| 52 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
||
| 53 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
||
| 54 | | | |
||
| 55 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
||
| 56 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
||
| 57 | * ================================================================================================ */ |
||
| 58 | |||
| 59 | // this block of memory gets written to the MPU on start-up, and it seems |
||
| 60 | // to be volatile memory, so it has to be done each time (it only takes ~1 |
||
| 61 | // second though) |
||
| 62 | const unsigned char mpu6050_dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
||
| 63 | // bank 0, 256 bytes |
||
| 64 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
||
| 65 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
||
| 66 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 67 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
||
| 68 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
||
| 69 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 70 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
||
| 71 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
||
| 72 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
||
| 73 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
||
| 74 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
||
| 75 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
||
| 76 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 77 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
||
| 78 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
||
| 79 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
||
| 80 | |||
| 81 | // bank 1, 256 bytes |
||
| 82 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 83 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
||
| 84 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
||
| 85 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
||
| 86 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
||
| 87 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
||
| 88 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
||
| 89 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 90 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 91 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 92 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
||
| 93 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
||
| 94 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
||
| 95 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 96 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 97 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
||
| 98 | |||
| 99 | // bank 2, 256 bytes |
||
| 100 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 101 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
||
| 102 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
||
| 103 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 104 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 105 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 106 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 107 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 108 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 109 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 110 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 111 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
||
| 112 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 113 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 114 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 115 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 116 | |||
| 117 | // bank 3, 256 bytes |
||
| 118 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, |
||
| 119 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, |
||
| 120 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, |
||
| 121 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, |
||
| 122 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, |
||
| 123 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, |
||
| 124 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, |
||
| 125 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, |
||
| 126 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, |
||
| 127 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, |
||
| 128 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, |
||
| 129 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, |
||
| 130 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, |
||
| 131 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, |
||
| 132 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
||
| 133 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
||
| 134 | |||
| 135 | // bank 4, 256 bytes |
||
| 136 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
||
| 137 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
||
| 138 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
||
| 139 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
||
| 140 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
||
| 141 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
||
| 142 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
||
| 143 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
||
| 144 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
||
| 145 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
||
| 146 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
||
| 147 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
||
| 148 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
||
| 149 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
||
| 150 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
||
| 151 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
||
| 152 | |||
| 153 | // bank 5, 256 bytes |
||
| 154 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
||
| 155 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
||
| 156 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
||
| 157 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
||
| 158 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
||
| 159 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
||
| 160 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
||
| 161 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
||
| 162 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, |
||
| 163 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, |
||
| 164 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, |
||
| 165 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, |
||
| 166 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, |
||
| 167 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, |
||
| 168 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, |
||
| 169 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, |
||
| 170 | |||
| 171 | // bank 6, 256 bytes |
||
| 172 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, |
||
| 173 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, |
||
| 174 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, |
||
| 175 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, |
||
| 176 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, |
||
| 177 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, |
||
| 178 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, |
||
| 179 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, |
||
| 180 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, |
||
| 181 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, |
||
| 182 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, |
||
| 183 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, |
||
| 184 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, |
||
| 185 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, |
||
| 186 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, |
||
| 187 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, |
||
| 188 | |||
| 189 | // bank 7, 138 bytes (remainder) |
||
| 190 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, |
||
| 191 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, |
||
| 192 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, |
||
| 193 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, |
||
| 194 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, |
||
| 195 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
||
| 196 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, |
||
| 197 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, |
||
| 198 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
||
| 199 | }; |
||
| 200 | |||
| 201 | const unsigned char mpu6050_dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
||
| 202 | // BANK OFFSET LENGTH [DATA] |
||
| 203 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
||
| 204 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
||
| 205 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration |
||
| 206 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration |
||
| 207 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
||
| 208 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration |
||
| 209 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration |
||
| 210 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration |
||
| 211 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
||
| 212 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
||
| 213 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
||
| 214 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
||
| 215 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
||
| 216 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
||
| 217 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
||
| 218 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
||
| 219 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
||
| 220 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
||
| 221 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
||
| 222 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
||
| 223 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
||
| 224 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone |
||
| 225 | // SPECIAL 0x01 = enable interrupts |
||
| 226 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
||
| 227 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt |
||
| 228 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
||
| 229 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer |
||
| 230 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
||
| 231 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
||
| 232 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
||
| 233 | 0x02, 0x16, 0x02, 0x00, 0x09 // D_0_22 inv_set_fifo_rate |
||
| 234 | |||
| 235 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
||
| 236 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
||
| 237 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
||
| 238 | |||
| 239 | // It is important to make sure the host processor can keep up with reading and processing |
||
| 240 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
||
| 241 | }; |
||
| 242 | |||
| 243 | const unsigned char mpu6050_dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
||
| 244 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, |
||
| 245 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
||
| 246 | 0x01, 0x6A, 0x02, 0x06, 0x00, |
||
| 247 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
| 248 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
||
| 249 | 0x01, 0x62, 0x02, 0x00, 0x00, |
||
| 250 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
||
| 251 | }; |
||
| 252 | |||
| 253 | /* |
||
| 254 | * initialize mpu6050 dmp |
||
| 255 | */ |
||
| 256 | uint8_t mpu6050_dmpInitialize() { |
||
| 257 | //setup interrupt |
||
| 258 | MPU6050_DMP_INT0SETUP; |
||
| 259 | |||
| 260 | //reset |
||
| 261 | mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); |
||
| 262 | _delay_ms(30);//wait after reset |
||
| 263 | |||
| 264 | //disable sleep mode |
||
| 265 | mpu6050_setSleepDisabled(); |
||
| 266 | |||
| 267 | //set memorybank to 0 |
||
| 268 | mpu6050_setMemoryBank(0, 0, 0); |
||
| 269 | |||
| 270 | //get X/Y/Z gyro offsets |
||
| 271 | int8_t xgOffset = mpu6050_getXGyroOffset(); |
||
| 272 | int8_t ygOffset = mpu6050_getYGyroOffset(); |
||
| 273 | int8_t zgOffset = mpu6050_getZGyroOffset(); |
||
| 274 | |||
| 275 | //setting slave 0 address to 0x7F |
||
| 276 | mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); |
||
| 277 | //disabling I2C Master mode |
||
| 278 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); |
||
| 279 | //setting slave 0 address to 0x68 (self) |
||
| 280 | mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); |
||
| 281 | //resetting I2C Master control |
||
| 282 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); |
||
| 283 | _delay_ms(20); |
||
| 284 | |||
| 285 | //load DMP code into memory banks |
||
| 286 | if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { |
||
| 287 | if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { |
||
| 288 | |||
| 289 | //set clock source |
||
| 290 | mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); |
||
| 291 | |||
| 292 | //set DMP and FIFO_OFLOW interrupts enabled |
||
| 293 | mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); |
||
| 294 | |||
| 295 | //set sample rate |
||
| 296 | mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz |
||
| 297 | |||
| 298 | //set external frame sync to TEMP_OUT_L[0] |
||
| 299 | mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); |
||
| 300 | |||
| 301 | //set DLPF bandwidth to 42Hz |
||
| 302 | mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
||
| 303 | |||
| 304 | //set gyro sensitivity to +/- 2000 deg/sec |
||
| 305 | mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); |
||
| 306 | |||
| 307 | //set DMP configuration bytes (function unknown) |
||
| 308 | mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); |
||
| 309 | mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); |
||
| 310 | |||
| 311 | //clear OTP Bank flag |
||
| 312 | mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); |
||
| 313 | |||
| 314 | //set X/Y/Z gyro offsets to previous values |
||
| 315 | //xgOffset = 0; |
||
| 316 | //ygOffset = 0; |
||
| 317 | zgOffset = 90; |
||
| 318 | |||
| 319 | mpu6050_setXGyroOffset(xgOffset); |
||
| 320 | mpu6050_setYGyroOffset(ygOffset); |
||
| 321 | mpu6050_setZGyroOffset(zgOffset); |
||
| 322 | |||
| 323 | //set X/Y/Z gyro user offsets to zero |
||
| 324 | mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); |
||
| 325 | mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); |
||
| 326 | mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0); |
||
| 327 | |||
| 328 | //writing final memory update 1/7 (function unknown) |
||
| 329 | uint8_t dmpUpdate[16], j; |
||
| 330 | uint16_t pos = 0; |
||
| 331 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 332 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
| 333 | |||
| 334 | //writing final memory update 2/7 (function unknown) |
||
| 335 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 336 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
| 337 | |||
| 338 | //reset FIFO |
||
| 339 | mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); |
||
| 340 | |||
| 341 | //reading FIFO count |
||
| 342 | uint8_t fifoCount = mpu6050_getFIFOCount(); |
||
| 343 | uint8_t fifoBuffer[128]; |
||
| 344 | |||
| 345 | //current FIFO count |
||
| 346 | mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); |
||
| 347 | |||
| 348 | //setting motion detection threshold to 2 |
||
| 349 | mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); |
||
| 350 | |||
| 351 | //setting zero-motion detection threshold to 156 |
||
| 352 | mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); |
||
| 353 | |||
| 354 | //setting motion detection duration to 80 |
||
| 355 | mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); |
||
| 356 | |||
| 357 | //setting zero-motion detection duration to 0 |
||
| 358 | mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); |
||
| 359 | |||
| 360 | //reset FIFO |
||
| 361 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
||
| 362 | |||
| 363 | //enabling FIFO |
||
| 364 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); |
||
| 365 | |||
| 366 | //enabling DMP |
||
| 367 | mpu6050_dmpEnable(); |
||
| 368 | |||
| 369 | //resetting DMP |
||
| 370 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); |
||
| 371 | |||
| 372 | //waiting for FIFO count > 2 |
||
| 373 | while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
||
| 374 | |||
| 375 | //writing final memory update 3/7 (function unknown) |
||
| 376 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 377 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
| 378 | |||
| 379 | //writing final memory update 4/7 (function unknown) |
||
| 380 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 381 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
| 382 | |||
| 383 | //writing final memory update 5/7 (function unknown) |
||
| 384 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 385 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
| 386 | |||
| 387 | //reading FIFO data...")); |
||
| 388 | mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
||
| 389 | |||
| 390 | //reading final memory update 6/7 (function unknown) |
||
| 391 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 392 | mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
||
| 393 | |||
| 394 | //waiting for FIFO count > 2 |
||
| 395 | while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
||
| 396 | |||
| 397 | //reading FIFO data |
||
| 398 | mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
||
| 399 | |||
| 400 | //writing final memory update 7/7 (function unknown) |
||
| 401 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
| 402 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
| 403 | |||
| 404 | //disabling DMP (you turn it on later) |
||
| 405 | mpu6050_dmpDisable(); |
||
| 406 | |||
| 407 | //resetting FIFO and clearing INT status one last time |
||
| 408 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
||
| 409 | } else { |
||
| 410 | return 2; // configuration block loading failed |
||
| 411 | } |
||
| 412 | } else { |
||
| 413 | return 1; // main binary block loading failed |
||
| 414 | } |
||
| 415 | return 0; // success |
||
| 416 | } |
||
| 417 | |||
| 418 | /* |
||
| 419 | * enable dmp |
||
| 420 | */ |
||
| 421 | void mpu6050_dmpEnable() { |
||
| 422 | MPU6050_DMP_INT0ENABLE; |
||
| 423 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1); |
||
| 424 | } |
||
| 425 | |||
| 426 | /* |
||
| 427 | * disable dmp |
||
| 428 | */ |
||
| 429 | void mpu6050_dmpDisable() { |
||
| 430 | MPU6050_DMP_INT0DISABLE; |
||
| 431 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 0); |
||
| 432 | } |
||
| 433 | |||
| 434 | /* |
||
| 435 | * get quaternion from packet |
||
| 436 | */ |
||
| 437 | void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz) { |
||
| 438 | if (packet == 0) packet = dmpPacketBuffer; |
||
| 439 | *qw = (double)((packet[0] << 8) + packet[1]) / 16384.0f; |
||
| 440 | *qx = (double)((packet[4] << 8) + packet[5]) / 16384.0f; |
||
| 441 | *qy = (double)((packet[8] << 8) + packet[9]) / 16384.0f; |
||
| 442 | *qz = (double)((packet[12] << 8) + packet[13]) / 16384.0f; |
||
| 443 | } |
||
| 444 | |||
| 445 | /* |
||
| 446 | * get euler angles |
||
| 447 | * aerospace sequence, to obtain sensor attitude: |
||
| 448 | * 1. rotate around sensor Z plane by yaw |
||
| 449 | * 2. rotate around sensor Y plane by pitch |
||
| 450 | * 3. rotate around sensor X plane by roll |
||
| 451 | */ |
||
| 452 | void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw) { |
||
| 453 | *yaw = atan2(2*qx*qy - 2*qw*qz, 2*qw*qw + 2*qx*qx - 1); |
||
| 454 | *pitch = -asin(2*qx*qz + 2*qw*qy); |
||
| 455 | *roll = atan2(2*qy*qz - 2*qw*qx, 2*qw*qw + 2*qz*qz - 1); |
||
| 456 | } |
||
| 457 | |||
| 458 | /* |
||
| 459 | * get quaternion and wait |
||
| 460 | */ |
||
| 461 | uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) { |
||
| 462 | while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize); |
||
| 463 | //reset interrupt |
||
| 464 | mpu6050_mpuInterrupt = 0; |
||
| 465 | |||
| 466 | //check for overflow |
||
| 467 | mpu6050_mpuIntStatus = mpu6050_getIntStatus(); |
||
| 468 | mpu6050_fifoCount = mpu6050_getFIFOCount(); |
||
| 469 | if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) { |
||
| 470 | //reset |
||
| 471 | mpu6050_resetFIFO(); |
||
| 472 | } else if (mpu6050_mpuIntStatus & 0x02) { |
||
| 473 | //wait for correct available data length, should be a VERY short wait |
||
| 474 | while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize) |
||
| 475 | mpu6050_fifoCount = mpu6050_getFIFOCount(); |
||
| 476 | //read a packet from FIFO |
||
| 477 | mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize); |
||
| 478 | mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize; |
||
| 479 | //get quaternion |
||
| 480 | mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz); |
||
| 481 | return 1; |
||
| 482 | } |
||
| 483 | |||
| 484 | return 0; |
||
| 485 | } |
||
| 486 | |||
| 487 | /* |
||
| 488 | * on interrupt set data availabe |
||
| 489 | */ |
||
| 490 | ISR (INT0_vect) { |
||
| 491 | mpu6050_mpuInterrupt = 1; |
||
| 492 | } |
||
| 493 | |||
| 494 | #endif |
||
| 2198 | - | 495 | #endif |