quadcopter cufe
mpu6050 nadafa
MPU6050_6Axis_MotionApps20.h@0:8fe8d6dd7cd6, 2015-06-23 (annotated)
- Committer:
- khaledelmadawi
- Date:
- Tue Jun 23 14:47:52 2015 +0000
- Revision:
- 0:8fe8d6dd7cd6
mpu nadafa
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| khaledelmadawi | 0:8fe8d6dd7cd6 | 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 3 | // 5/20/2013 by Jeff Rowberg <jeff@rowberg.net> |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 5 | // |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 6 | // Changelog: |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 7 | // ... - ongoing debug release |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 8 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 9 | /* ============================================ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 10 | I2Cdev device library code is placed under the MIT license |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 11 | Copyright (c) 2012 Jeff Rowberg |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 12 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 14 | of this software and associated documentation files (the "Software"), to deal |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 15 | in the Software without restriction, including without limitation the rights |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 17 | copies of the Software, and to permit persons to whom the Software is |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 18 | furnished to do so, subject to the following conditions: |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 19 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 20 | The above copyright notice and this permission notice shall be included in |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 21 | all copies or substantial portions of the Software. |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 22 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 29 | THE SOFTWARE. |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 30 | =============================================== |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 31 | */ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 32 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 33 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 34 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 35 | #include <iostream> |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 36 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 37 | #include "I2Cdev.h" |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 38 | #include "helper_3dmath.h" |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 39 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 40 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 41 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 42 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 43 | #include "MPU6050.h" |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 44 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 45 | // Tom Carpenter's conditional PROGMEM code |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 46 | // http://forum.arduino.cc/index.php?topic=129407.0 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 47 | #ifndef __arm__ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 48 | #include <avr/pgmspace.h> |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 49 | #else |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 50 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 51 | #ifndef __PGMSPACE_H_ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 52 | #define __PGMSPACE_H_ 1 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 53 | #include <inttypes.h> |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 54 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 55 | #define PROGMEM |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 56 | #define PGM_P const char * |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 57 | #define PSTR(str) (str) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 58 | #define F(x) x |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 59 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 60 | typedef void prog_void; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 61 | typedef char prog_char; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 62 | typedef unsigned char prog_uchar; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 63 | typedef int8_t prog_int8_t; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 64 | typedef uint8_t prog_uint8_t; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 65 | typedef int16_t prog_int16_t; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 66 | typedef uint16_t prog_uint16_t; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 67 | typedef int32_t prog_int32_t; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 68 | typedef uint32_t prog_uint32_t; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 69 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 70 | #define strcpy_P(dest, src) strcpy((dest), (src)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 71 | #define strcat_P(dest, src) strcat((dest), (src)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 72 | #define strcmp_P(a, b) strcmp((a), (b)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 73 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 74 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 75 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 76 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 77 | #define pgm_read_float(addr) (*(const float *)(addr)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 78 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 79 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 80 | #define pgm_read_word_near(addr) pgm_read_word(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 81 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 82 | #define pgm_read_float_near(addr) pgm_read_float(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 83 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 84 | #define pgm_read_word_far(addr) pgm_read_word(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 85 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 86 | #define pgm_read_float_far(addr) pgm_read_float(addr) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 87 | #endif |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 88 | #endif |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 89 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 90 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 91 | * unavailable, unless you happen to be amazing as decompiling binary by |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 92 | * hand (in which case, please contact me, and I'm totally serious). |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 93 | * |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 94 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 95 | * DMP reverse-engineering he did to help make this bit of wizardry |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 96 | * possible. |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 97 | */ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 98 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 99 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 100 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 101 | // after moving string constants to flash memory storage using the F() |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 102 | // compiler macro (Arduino IDE 1.0+ required). |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 103 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 104 | //#define DEBUG |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 105 | #ifdef DEBUG |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 106 | #define DEBUG_PRINT(x) pc.printf(x) //Serial.print(x) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 107 | #define DEBUG_PRINTF(x, y) pc.printf(x,y) //Serial.print(x, y) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 108 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 109 | #define F(x) x |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 110 | #else |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 111 | #define DEBUG_PRINT(x) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 112 | #define DEBUG_PRINTF(x, y) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 113 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 114 | #endif |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 115 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 116 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 117 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 118 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 119 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 120 | /* ================================================================================================ * |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 121 | | Default MotionApps v2.0 42-byte FIFO packet structure: | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 122 | | | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 123 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 124 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 125 | | | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 126 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 127 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 128 | * ================================================================================================ */ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 129 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 130 | // this block of memory gets written to the MPU on start-up, and it seems |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 132 | // second though) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 134 | // bank 0, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 151 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 152 | // bank 1, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 169 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 170 | // bank 2, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 187 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 188 | // bank 3, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 205 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 206 | // bank 4, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 223 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 224 | // bank 5, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 241 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 242 | // bank 6, 256 bytes |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 259 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 260 | // bank 7, 138 bytes (remainder) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 270 | }; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 271 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 272 | // thanks to Noah Zerkin for piecing this stuff together! |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 274 | // BANK OFFSET LENGTH [DATA] |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 283 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 284 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 285 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 286 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 287 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 288 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 289 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 290 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 291 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 295 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 296 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 297 | // SPECIAL 0x01 = enable interrupts |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 298 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 299 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 300 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 301 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 302 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 303 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 304 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 305 | 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 306 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 310 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 311 | // It is important to make sure the host processor can keep up with reading and processing |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 313 | }; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 314 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 315 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 316 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 317 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 318 | 0x01, 0x6A, 0x02, 0x06, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 319 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 320 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 321 | 0x01, 0x62, 0x02, 0x00, 0x00, |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 322 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 323 | }; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 324 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 325 | uint8_t MPU6050::dmpInitialize() { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 326 | // reset device |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 327 | Serial pc(USBTX, USBRX); // tx, rx |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 328 | pc.baud(115200); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 329 | wait_ms(50); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 330 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 331 | pc.printf("\n\nSet USB Baudrate to 115200...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 332 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 333 | pc.printf("\n\nResetting MPU6050...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 334 | reset(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 335 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 336 | wait_ms(30); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 337 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 338 | // enable sleep mode and wake cycle |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 339 | /*Serial.println(F("Enabling sleep mode...")); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 340 | setSleepEnabled(true); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 341 | Serial.println(F("Enabling wake cycle...")); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 342 | setWakeCycleEnabled(true);*/ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 343 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 344 | // disable sleep mode |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 345 | DEBUG_PRINT("Disabling sleep mode...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 346 | setSleepEnabled(false); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 347 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 348 | // get MPU hardware revision |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 349 | DEBUG_PRINT("Selecting user bank 16...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 350 | setMemoryBank(0x10, true, true); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 351 | DEBUG_PRINT("Selecting memory byte 6...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 352 | setMemoryStartAddress(0x06); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 353 | DEBUG_PRINT("Checking hardware revision...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 354 | uint8_t hwRevision = readMemoryByte(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 355 | DEBUG_PRINT("Revision @ user[16][6] = "); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 356 | DEBUG_PRINTF("%x\n",hwRevision); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 357 | DEBUG_PRINT("Resetting memory bank selection to 0...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 358 | setMemoryBank(0, false, false); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 359 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 360 | // check OTP bank valid |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 361 | DEBUG_PRINT("Reading OTP bank valid flag...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 362 | uint8_t otpValid = getOTPBankValid(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 363 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 364 | DEBUG_PRINT("OTP bank is "); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 365 | if(otpValid) DEBUG_PRINT("valid!\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 366 | else DEBUG_PRINT("invalid!\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 367 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 368 | // get X/Y/Z gyro offsets |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 369 | /* |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 370 | DEBUG_PRINT("\nReading gyro offset TC values...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 371 | int8_t xgOffsetTC = mpu.getXGyroOffsetTC(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 372 | int8_t ygOffsetTC = getYGyroOffsetTC(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 373 | int8_t zgOffsetTC = getZGyroOffsetTC(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 374 | DEBUG_PRINTF("X gyro offset = %u\n",xgOffset); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 375 | DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 376 | DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 377 | */ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 378 | // setup weird slave stuff (?) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 379 | DEBUG_PRINT("Setting slave 0 address to 0x7F...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 380 | setSlaveAddress(0, 0x7F); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 381 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 382 | DEBUG_PRINT("Disabling I2C Master mode..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 383 | setI2CMasterModeEnabled(false); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 384 | DEBUG_PRINT("Setting slave 0 address to 0x68 (self)..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 385 | setSlaveAddress(0, 0x68); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 386 | DEBUG_PRINT("Resetting I2C Master control...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 387 | resetI2CMaster(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 388 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 389 | wait_ms(20); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 390 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 391 | // load DMP code into memory banks |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 392 | DEBUG_PRINT("Writing DMP code to MPU memory banks ("); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 393 | DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 394 | DEBUG_PRINT(" bytes)\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 395 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 396 | DEBUG_PRINT("Success! DMP code written and verified.\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 397 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 398 | // write DMP configuration |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 399 | DEBUG_PRINT("Writing DMP configuration to MPU memory banks ("); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 400 | DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 401 | DEBUG_PRINT(" bytes in config def)\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 402 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 403 | DEBUG_PRINT("Success! DMP configuration written and verified.\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 404 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 405 | DEBUG_PRINT("Setting clock source to Z Gyro...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 406 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 407 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 408 | DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 409 | setIntEnabled(0x12); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 410 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 411 | DEBUG_PRINT("Setting sample rate to 200Hz..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 412 | setRate(4); // 1khz / (1 + 4) = 200 Hz |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 413 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 414 | DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 415 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 416 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 417 | DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 418 | setDLPFMode(MPU6050_DLPF_BW_42); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 419 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 420 | DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 421 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 422 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 423 | DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 424 | setDMPConfig1(0x03); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 425 | setDMPConfig2(0x00); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 426 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 427 | DEBUG_PRINT("Clearing OTP Bank flag..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 428 | setOTPBankValid(false); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 429 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 430 | DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 431 | //setXGyroOffsetTC(xgOffsetTC); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 432 | //setYGyroOffsetTC(ygOffsetTC); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 433 | //setZGyroOffsetTC(zgOffsetTC); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 434 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 435 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 436 | //setXGyroOffset(0); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 437 | //setYGyroOffset(0); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 438 | //setZGyroOffset(0); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 439 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 440 | DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 441 | uint8_t dmpUpdate[16], j; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 442 | uint16_t pos = 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 443 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 444 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 445 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 446 | DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 447 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 448 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 449 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 450 | DEBUG_PRINT("Resetting FIFO...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 451 | resetFIFO(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 452 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 453 | DEBUG_PRINT("Reading FIFO count...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 454 | uint16_t fifoCount = getFIFOCount(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 455 | uint8_t fifoBuffer[128]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 456 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 457 | DEBUG_PRINT("Current FIFO count="); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 458 | DEBUG_PRINTF("%u\n",fifoCount); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 459 | getFIFOBytes(fifoBuffer, fifoCount); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 460 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 461 | DEBUG_PRINT("Setting motion detection threshold to 2...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 462 | setMotionDetectionThreshold(2); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 463 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 464 | DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 465 | setZeroMotionDetectionThreshold(156); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 466 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 467 | DEBUG_PRINT("Setting motion detection duration to 80..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 468 | setMotionDetectionDuration(80); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 469 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 470 | DEBUG_PRINT("Setting zero-motion detection duration to 0..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 471 | setZeroMotionDetectionDuration(0); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 472 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 473 | DEBUG_PRINT("Resetting FIFO...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 474 | resetFIFO(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 475 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 476 | DEBUG_PRINT("Enabling FIFO...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 477 | setFIFOEnabled(true); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 478 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 479 | DEBUG_PRINT("Enabling DMP...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 480 | setDMPEnabled(true); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 481 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 482 | DEBUG_PRINT("Resetting DMP...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 483 | resetDMP(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 484 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 485 | DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 486 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 487 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 488 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 489 | DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 490 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 491 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 492 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 493 | DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 494 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 495 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 496 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 497 | DEBUG_PRINT("Waiting for FIFO count > 2...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 498 | while ((fifoCount = getFIFOCount()) < 3); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 499 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 500 | DEBUG_PRINT("Current FIFO count="); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 501 | DEBUG_PRINTF("%u\n",fifoCount); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 502 | DEBUG_PRINT("Reading FIFO data...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 503 | getFIFOBytes(fifoBuffer, fifoCount); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 504 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 505 | DEBUG_PRINT("Reading interrupt status...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 506 | uint8_t mpuIntStatus = getIntStatus(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 507 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 508 | DEBUG_PRINT("Current interrupt status="); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 509 | DEBUG_PRINTF("%x\n",mpuIntStatus); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 510 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 511 | DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 512 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 513 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 514 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 515 | DEBUG_PRINT("Waiting for FIFO count > 2...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 516 | while ((fifoCount = getFIFOCount()) < 3); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 517 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 518 | DEBUG_PRINT("Current FIFO count="); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 519 | DEBUG_PRINTF("%u\n",fifoCount); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 520 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 521 | DEBUG_PRINT("Reading FIFO data...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 522 | getFIFOBytes(fifoBuffer, fifoCount); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 523 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 524 | DEBUG_PRINT("Reading interrupt status...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 525 | mpuIntStatus = getIntStatus(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 526 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 527 | DEBUG_PRINT("Current interrupt status="); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 528 | DEBUG_PRINTF("%x\n",mpuIntStatus); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 529 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 530 | DEBUG_PRINT("Writing final memory update 7/7 (function unknown)..."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 531 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 532 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 533 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 534 | DEBUG_PRINT("DMP is good to go! Finally.\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 535 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 536 | DEBUG_PRINT("Disabling DMP (you turn it on later)...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 537 | setDMPEnabled(false); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 538 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 539 | DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 540 | dmpPacketSize = 42; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 541 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 542 | return 3; // TODO: proper error code for no memory |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 543 | }*/ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 544 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 545 | DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 546 | resetFIFO(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 547 | getIntStatus(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 548 | } else { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 549 | DEBUG_PRINT("ERROR! DMP configuration verification failed.\n"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 550 | return 2; // configuration block loading failed |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 551 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 552 | } else { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 553 | DEBUG_PRINT("ERROR! DMP code verification failed."); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 554 | return 1; // main binary block loading failed |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 555 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 556 | return 0; // success |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 557 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 558 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 559 | bool MPU6050::dmpPacketAvailable() { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 560 | return getFIFOCount() >= dmpGetFIFOPacketSize(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 561 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 562 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 563 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 564 | // uint8_t MPU6050::dmpGetFIFORate(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 565 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 566 | // uint8_t MPU6050::dmpGetSampleFrequency(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 567 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 568 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 569 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 570 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 571 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 572 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 573 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 574 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 575 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 576 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 577 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 578 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 579 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 580 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 581 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 582 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 583 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 584 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 585 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 586 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 587 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 588 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 589 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 590 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 591 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 592 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 593 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 594 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 595 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 596 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 597 | data[0] = (packet[28] << 8) + packet[29]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 598 | data[1] = (packet[32] << 8) + packet[33]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 599 | data[2] = (packet[36] << 8) + packet[37]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 600 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 601 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 602 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 603 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 604 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 605 | v -> x = (packet[28] << 8) + packet[29]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 606 | v -> y = (packet[32] << 8) + packet[33]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 607 | v -> z = (packet[36] << 8) + packet[37]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 608 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 609 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 610 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 611 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 612 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 613 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 614 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 615 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 616 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 617 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 618 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 619 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 620 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 621 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 622 | data[0] = ((packet[0] << 8) + packet[1]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 623 | data[1] = ((packet[4] << 8) + packet[5]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 624 | data[2] = ((packet[8] << 8) + packet[9]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 625 | data[3] = ((packet[12] << 8) + packet[13]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 626 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 627 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 628 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 629 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 630 | int16_t qI[4]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 631 | uint8_t status = dmpGetQuaternion(qI, packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 632 | if (status == 0) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 633 | q -> w = (float)qI[0] / 16384.0f; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 634 | q -> x = (float)qI[1] / 16384.0f; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 635 | q -> y = (float)qI[2] / 16384.0f; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 636 | q -> z = (float)qI[3] / 16384.0f; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 637 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 638 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 639 | return status; // int16 return value, indicates error if this line is reached |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 640 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 641 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 642 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 643 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 644 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 645 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 646 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 647 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 648 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 649 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 650 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 651 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 652 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 653 | if (packet == 0) packet = dmpPacketBuffer; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 654 | data[0] = (packet[16] << 8) + packet[17]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 655 | data[1] = (packet[20] << 8) + packet[21]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 656 | data[2] = (packet[24] << 8) + packet[25]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 657 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 658 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 659 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 660 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 661 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 662 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 663 | v -> x = vRaw -> x - gravity -> x*8192; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 664 | v -> y = vRaw -> y - gravity -> y*8192; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 665 | v -> z = vRaw -> z - gravity -> z*8192; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 666 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 667 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 668 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 669 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 670 | // rotate measured 3D acceleration vector into original state |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 671 | // frame of reference based on orientation quaternion |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 672 | memcpy(v, vReal, sizeof(VectorInt16)); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 673 | v -> rotate(q); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 674 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 675 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 676 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 677 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 678 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 679 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 680 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 681 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 682 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 683 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 684 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 685 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 686 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 687 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 688 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 689 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 690 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 691 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 692 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 693 | 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 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 694 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 695 | 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 |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 696 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 697 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 698 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 699 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 700 | // yaw: (about Z axis) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 701 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 702 | // pitch: (nose up/down, about Y axis) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 703 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 704 | // roll: (tilt left/right, about X axis) |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 705 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 706 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 707 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 708 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 709 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 710 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 711 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 712 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 713 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 714 | if (dmpData[k] < 0x10) Serial.print("0"); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 715 | Serial.print(dmpData[k], HEX); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 716 | Serial.print(" "); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 717 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 718 | Serial.print("\n");*/ |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 719 | //Serial.println((uint16_t)dmpPacketBuffer); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 720 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 721 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 722 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 723 | uint8_t status; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 724 | uint8_t buf[dmpPacketSize]; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 725 | for (uint8_t i = 0; i < numPackets; i++) { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 726 | // read packet from FIFO |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 727 | getFIFOBytes(buf, dmpPacketSize); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 728 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 729 | // process packet |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 730 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 731 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 732 | // increment external process count variable, if supplied |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 733 | if (processed != 0) *processed++; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 734 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 735 | return 0; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 736 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 737 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 738 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 739 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 740 | // uint8_t MPU6050::dmpInitFIFOParam(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 741 | // uint8_t MPU6050::dmpCloseFIFO(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 742 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 743 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 744 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 745 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 746 | // void MPU6050::dmpOverrideQuaternion(long *q); |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 747 | uint16_t MPU6050::dmpGetFIFOPacketSize() { |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 748 | return dmpPacketSize; |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 749 | } |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 750 | |
| khaledelmadawi | 0:8fe8d6dd7cd6 | 751 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ |