InvenSense Motion Driver v5.1.2 ported
inv_mpu_dmp_motion_driver.cpp@0:4dd021344a6b, 2014-06-03 (annotated)
- Committer:
- oprospero
- Date:
- Tue Jun 03 07:54:12 2014 +0000
- Revision:
- 0:4dd021344a6b
- Child:
- 3:8ac73f9099ab
Initial MotionDriver v5.1.2 ported from InvenSense
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
oprospero | 0:4dd021344a6b | 1 | /* |
oprospero | 0:4dd021344a6b | 2 | $License: |
oprospero | 0:4dd021344a6b | 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. |
oprospero | 0:4dd021344a6b | 4 | See included License.txt for License information. |
oprospero | 0:4dd021344a6b | 5 | $ |
oprospero | 0:4dd021344a6b | 6 | */ |
oprospero | 0:4dd021344a6b | 7 | /** |
oprospero | 0:4dd021344a6b | 8 | * @addtogroup DRIVERS Sensor Driver Layer |
oprospero | 0:4dd021344a6b | 9 | * @brief Hardware drivers to communicate with sensors via I2C. |
oprospero | 0:4dd021344a6b | 10 | * |
oprospero | 0:4dd021344a6b | 11 | * @{ |
oprospero | 0:4dd021344a6b | 12 | * @file inv_mpu_dmp_motion_driver.c |
oprospero | 0:4dd021344a6b | 13 | * @brief DMP image and interface functions. |
oprospero | 0:4dd021344a6b | 14 | * @details All functions are preceded by the dmp_ prefix to |
oprospero | 0:4dd021344a6b | 15 | * differentiate among MPL and general driver function calls. |
oprospero | 0:4dd021344a6b | 16 | */ |
oprospero | 0:4dd021344a6b | 17 | #include <stdio.h> |
oprospero | 0:4dd021344a6b | 18 | #include <stdint.h> |
oprospero | 0:4dd021344a6b | 19 | #include <stdlib.h> |
oprospero | 0:4dd021344a6b | 20 | #include <string.h> |
oprospero | 0:4dd021344a6b | 21 | #include <math.h> |
oprospero | 0:4dd021344a6b | 22 | #include "inv_mpu.h" |
oprospero | 0:4dd021344a6b | 23 | #include "inv_mpu_dmp_motion_driver.h" |
oprospero | 0:4dd021344a6b | 24 | #include "dmpKey.h" |
oprospero | 0:4dd021344a6b | 25 | #include "dmpmap.h" |
oprospero | 0:4dd021344a6b | 26 | |
oprospero | 0:4dd021344a6b | 27 | #define get_ms(...) |
oprospero | 0:4dd021344a6b | 28 | |
oprospero | 0:4dd021344a6b | 29 | /* The following functions must be defined for this platform: |
oprospero | 0:4dd021344a6b | 30 | * i2c_write(unsigned char slave_addr, unsigned char reg_addr, |
oprospero | 0:4dd021344a6b | 31 | * unsigned char length, unsigned char const *data) |
oprospero | 0:4dd021344a6b | 32 | * i2c_read(unsigned char slave_addr, unsigned char reg_addr, |
oprospero | 0:4dd021344a6b | 33 | * unsigned char length, unsigned char *data) |
oprospero | 0:4dd021344a6b | 34 | * delay_ms(unsigned long num_ms) |
oprospero | 0:4dd021344a6b | 35 | * get_ms(unsigned long *count) |
oprospero | 0:4dd021344a6b | 36 | */ |
oprospero | 0:4dd021344a6b | 37 | //#if defined MOTION_DRIVER_TARGET_MSP430 |
oprospero | 0:4dd021344a6b | 38 | //#include "msp430.h" |
oprospero | 0:4dd021344a6b | 39 | //#include "msp430_clock.h" |
oprospero | 0:4dd021344a6b | 40 | //#define delay_ms msp430_delay_ms |
oprospero | 0:4dd021344a6b | 41 | //#define get_ms msp430_get_clock_ms |
oprospero | 0:4dd021344a6b | 42 | //#define log_i(...) do {} while (0) |
oprospero | 0:4dd021344a6b | 43 | //#define log_e(...) do {} while (0) |
oprospero | 0:4dd021344a6b | 44 | // |
oprospero | 0:4dd021344a6b | 45 | //#elif defined EMPL_TARGET_MSP430 |
oprospero | 0:4dd021344a6b | 46 | //#include "msp430.h" |
oprospero | 0:4dd021344a6b | 47 | //#include "msp430_clock.h" |
oprospero | 0:4dd021344a6b | 48 | //#include "log.h" |
oprospero | 0:4dd021344a6b | 49 | //#define delay_ms msp430_delay_ms |
oprospero | 0:4dd021344a6b | 50 | //#define get_ms msp430_get_clock_ms |
oprospero | 0:4dd021344a6b | 51 | //#define log_i MPL_LOGI |
oprospero | 0:4dd021344a6b | 52 | //#define log_e MPL_LOGE |
oprospero | 0:4dd021344a6b | 53 | // |
oprospero | 0:4dd021344a6b | 54 | //#elif defined EMPL_TARGET_UC3L0 |
oprospero | 0:4dd021344a6b | 55 | ///* Instead of using the standard TWI driver from the ASF library, we're using |
oprospero | 0:4dd021344a6b | 56 | // * a TWI driver that follows the slave address + register address convention. |
oprospero | 0:4dd021344a6b | 57 | // */ |
oprospero | 0:4dd021344a6b | 58 | //#include "delay.h" |
oprospero | 0:4dd021344a6b | 59 | //#include "sysclk.h" |
oprospero | 0:4dd021344a6b | 60 | //#include "log.h" |
oprospero | 0:4dd021344a6b | 61 | //#include "uc3l0_clock.h" |
oprospero | 0:4dd021344a6b | 62 | ///* delay_ms is a function already defined in ASF. */ |
oprospero | 0:4dd021344a6b | 63 | //#define get_ms uc3l0_get_clock_ms |
oprospero | 0:4dd021344a6b | 64 | //#define log_i MPL_LOGI |
oprospero | 0:4dd021344a6b | 65 | //#define log_e MPL_LOGE |
oprospero | 0:4dd021344a6b | 66 | // |
oprospero | 0:4dd021344a6b | 67 | //#else |
oprospero | 0:4dd021344a6b | 68 | //#error Gyro driver is missing the system layer implementations. |
oprospero | 0:4dd021344a6b | 69 | //#endif |
oprospero | 0:4dd021344a6b | 70 | |
oprospero | 0:4dd021344a6b | 71 | /* These defines are copied from dmpDefaultMPU6050.c in the general MPL |
oprospero | 0:4dd021344a6b | 72 | * releases. These defines may change for each DMP image, so be sure to modify |
oprospero | 0:4dd021344a6b | 73 | * these values when switching to a new image. |
oprospero | 0:4dd021344a6b | 74 | */ |
oprospero | 0:4dd021344a6b | 75 | #define CFG_LP_QUAT (2712) |
oprospero | 0:4dd021344a6b | 76 | #define END_ORIENT_TEMP (1866) |
oprospero | 0:4dd021344a6b | 77 | #define CFG_27 (2742) |
oprospero | 0:4dd021344a6b | 78 | #define CFG_20 (2224) |
oprospero | 0:4dd021344a6b | 79 | #define CFG_23 (2745) |
oprospero | 0:4dd021344a6b | 80 | #define CFG_FIFO_ON_EVENT (2690) |
oprospero | 0:4dd021344a6b | 81 | #define END_PREDICTION_UPDATE (1761) |
oprospero | 0:4dd021344a6b | 82 | #define CGNOTICE_INTR (2620) |
oprospero | 0:4dd021344a6b | 83 | #define X_GRT_Y_TMP (1358) |
oprospero | 0:4dd021344a6b | 84 | #define CFG_DR_INT (1029) |
oprospero | 0:4dd021344a6b | 85 | #define CFG_AUTH (1035) |
oprospero | 0:4dd021344a6b | 86 | #define UPDATE_PROP_ROT (1835) |
oprospero | 0:4dd021344a6b | 87 | #define END_COMPARE_Y_X_TMP2 (1455) |
oprospero | 0:4dd021344a6b | 88 | #define SKIP_X_GRT_Y_TMP (1359) |
oprospero | 0:4dd021344a6b | 89 | #define SKIP_END_COMPARE (1435) |
oprospero | 0:4dd021344a6b | 90 | #define FCFG_3 (1088) |
oprospero | 0:4dd021344a6b | 91 | #define FCFG_2 (1066) |
oprospero | 0:4dd021344a6b | 92 | #define FCFG_1 (1062) |
oprospero | 0:4dd021344a6b | 93 | #define END_COMPARE_Y_X_TMP3 (1434) |
oprospero | 0:4dd021344a6b | 94 | #define FCFG_7 (1073) |
oprospero | 0:4dd021344a6b | 95 | #define FCFG_6 (1106) |
oprospero | 0:4dd021344a6b | 96 | #define FLAT_STATE_END (1713) |
oprospero | 0:4dd021344a6b | 97 | #define SWING_END_4 (1616) |
oprospero | 0:4dd021344a6b | 98 | #define SWING_END_2 (1565) |
oprospero | 0:4dd021344a6b | 99 | #define SWING_END_3 (1587) |
oprospero | 0:4dd021344a6b | 100 | #define SWING_END_1 (1550) |
oprospero | 0:4dd021344a6b | 101 | #define CFG_8 (2718) |
oprospero | 0:4dd021344a6b | 102 | #define CFG_15 (2727) |
oprospero | 0:4dd021344a6b | 103 | #define CFG_16 (2746) |
oprospero | 0:4dd021344a6b | 104 | #define CFG_EXT_GYRO_BIAS (1189) |
oprospero | 0:4dd021344a6b | 105 | #define END_COMPARE_Y_X_TMP (1407) |
oprospero | 0:4dd021344a6b | 106 | #define DO_NOT_UPDATE_PROP_ROT (1839) |
oprospero | 0:4dd021344a6b | 107 | #define CFG_7 (1205) |
oprospero | 0:4dd021344a6b | 108 | #define FLAT_STATE_END_TEMP (1683) |
oprospero | 0:4dd021344a6b | 109 | #define END_COMPARE_Y_X (1484) |
oprospero | 0:4dd021344a6b | 110 | #define SKIP_SWING_END_1 (1551) |
oprospero | 0:4dd021344a6b | 111 | #define SKIP_SWING_END_3 (1588) |
oprospero | 0:4dd021344a6b | 112 | #define SKIP_SWING_END_2 (1566) |
oprospero | 0:4dd021344a6b | 113 | #define TILTG75_START (1672) |
oprospero | 0:4dd021344a6b | 114 | #define CFG_6 (2753) |
oprospero | 0:4dd021344a6b | 115 | #define TILTL75_END (1669) |
oprospero | 0:4dd021344a6b | 116 | #define END_ORIENT (1884) |
oprospero | 0:4dd021344a6b | 117 | #define CFG_FLICK_IN (2573) |
oprospero | 0:4dd021344a6b | 118 | #define TILTL75_START (1643) |
oprospero | 0:4dd021344a6b | 119 | #define CFG_MOTION_BIAS (1208) |
oprospero | 0:4dd021344a6b | 120 | #define X_GRT_Y (1408) |
oprospero | 0:4dd021344a6b | 121 | #define TEMPLABEL (2324) |
oprospero | 0:4dd021344a6b | 122 | #define CFG_ANDROID_ORIENT_INT (1853) |
oprospero | 0:4dd021344a6b | 123 | #define CFG_GYRO_RAW_DATA (2722) |
oprospero | 0:4dd021344a6b | 124 | #define X_GRT_Y_TMP2 (1379) |
oprospero | 0:4dd021344a6b | 125 | |
oprospero | 0:4dd021344a6b | 126 | #define D_0_22 (22+512) |
oprospero | 0:4dd021344a6b | 127 | #define D_0_24 (24+512) |
oprospero | 0:4dd021344a6b | 128 | |
oprospero | 0:4dd021344a6b | 129 | #define D_0_36 (36) |
oprospero | 0:4dd021344a6b | 130 | #define D_0_52 (52) |
oprospero | 0:4dd021344a6b | 131 | #define D_0_96 (96) |
oprospero | 0:4dd021344a6b | 132 | #define D_0_104 (104) |
oprospero | 0:4dd021344a6b | 133 | #define D_0_108 (108) |
oprospero | 0:4dd021344a6b | 134 | #define D_0_163 (163) |
oprospero | 0:4dd021344a6b | 135 | #define D_0_188 (188) |
oprospero | 0:4dd021344a6b | 136 | #define D_0_192 (192) |
oprospero | 0:4dd021344a6b | 137 | #define D_0_224 (224) |
oprospero | 0:4dd021344a6b | 138 | #define D_0_228 (228) |
oprospero | 0:4dd021344a6b | 139 | #define D_0_232 (232) |
oprospero | 0:4dd021344a6b | 140 | #define D_0_236 (236) |
oprospero | 0:4dd021344a6b | 141 | |
oprospero | 0:4dd021344a6b | 142 | #define D_1_2 (256 + 2) |
oprospero | 0:4dd021344a6b | 143 | #define D_1_4 (256 + 4) |
oprospero | 0:4dd021344a6b | 144 | #define D_1_8 (256 + 8) |
oprospero | 0:4dd021344a6b | 145 | #define D_1_10 (256 + 10) |
oprospero | 0:4dd021344a6b | 146 | #define D_1_24 (256 + 24) |
oprospero | 0:4dd021344a6b | 147 | #define D_1_28 (256 + 28) |
oprospero | 0:4dd021344a6b | 148 | #define D_1_36 (256 + 36) |
oprospero | 0:4dd021344a6b | 149 | #define D_1_40 (256 + 40) |
oprospero | 0:4dd021344a6b | 150 | #define D_1_44 (256 + 44) |
oprospero | 0:4dd021344a6b | 151 | #define D_1_72 (256 + 72) |
oprospero | 0:4dd021344a6b | 152 | #define D_1_74 (256 + 74) |
oprospero | 0:4dd021344a6b | 153 | #define D_1_79 (256 + 79) |
oprospero | 0:4dd021344a6b | 154 | #define D_1_88 (256 + 88) |
oprospero | 0:4dd021344a6b | 155 | #define D_1_90 (256 + 90) |
oprospero | 0:4dd021344a6b | 156 | #define D_1_92 (256 + 92) |
oprospero | 0:4dd021344a6b | 157 | #define D_1_96 (256 + 96) |
oprospero | 0:4dd021344a6b | 158 | #define D_1_98 (256 + 98) |
oprospero | 0:4dd021344a6b | 159 | #define D_1_106 (256 + 106) |
oprospero | 0:4dd021344a6b | 160 | #define D_1_108 (256 + 108) |
oprospero | 0:4dd021344a6b | 161 | #define D_1_112 (256 + 112) |
oprospero | 0:4dd021344a6b | 162 | #define D_1_128 (256 + 144) |
oprospero | 0:4dd021344a6b | 163 | #define D_1_152 (256 + 12) |
oprospero | 0:4dd021344a6b | 164 | #define D_1_160 (256 + 160) |
oprospero | 0:4dd021344a6b | 165 | #define D_1_176 (256 + 176) |
oprospero | 0:4dd021344a6b | 166 | #define D_1_178 (256 + 178) |
oprospero | 0:4dd021344a6b | 167 | #define D_1_218 (256 + 218) |
oprospero | 0:4dd021344a6b | 168 | #define D_1_232 (256 + 232) |
oprospero | 0:4dd021344a6b | 169 | #define D_1_236 (256 + 236) |
oprospero | 0:4dd021344a6b | 170 | #define D_1_240 (256 + 240) |
oprospero | 0:4dd021344a6b | 171 | #define D_1_244 (256 + 244) |
oprospero | 0:4dd021344a6b | 172 | #define D_1_250 (256 + 250) |
oprospero | 0:4dd021344a6b | 173 | #define D_1_252 (256 + 252) |
oprospero | 0:4dd021344a6b | 174 | #define D_2_12 (512 + 12) |
oprospero | 0:4dd021344a6b | 175 | #define D_2_96 (512 + 96) |
oprospero | 0:4dd021344a6b | 176 | #define D_2_108 (512 + 108) |
oprospero | 0:4dd021344a6b | 177 | #define D_2_208 (512 + 208) |
oprospero | 0:4dd021344a6b | 178 | #define D_2_224 (512 + 224) |
oprospero | 0:4dd021344a6b | 179 | #define D_2_236 (512 + 236) |
oprospero | 0:4dd021344a6b | 180 | #define D_2_244 (512 + 244) |
oprospero | 0:4dd021344a6b | 181 | #define D_2_248 (512 + 248) |
oprospero | 0:4dd021344a6b | 182 | #define D_2_252 (512 + 252) |
oprospero | 0:4dd021344a6b | 183 | |
oprospero | 0:4dd021344a6b | 184 | #define CPASS_BIAS_X (35 * 16 + 4) |
oprospero | 0:4dd021344a6b | 185 | #define CPASS_BIAS_Y (35 * 16 + 8) |
oprospero | 0:4dd021344a6b | 186 | #define CPASS_BIAS_Z (35 * 16 + 12) |
oprospero | 0:4dd021344a6b | 187 | #define CPASS_MTX_00 (36 * 16) |
oprospero | 0:4dd021344a6b | 188 | #define CPASS_MTX_01 (36 * 16 + 4) |
oprospero | 0:4dd021344a6b | 189 | #define CPASS_MTX_02 (36 * 16 + 8) |
oprospero | 0:4dd021344a6b | 190 | #define CPASS_MTX_10 (36 * 16 + 12) |
oprospero | 0:4dd021344a6b | 191 | #define CPASS_MTX_11 (37 * 16) |
oprospero | 0:4dd021344a6b | 192 | #define CPASS_MTX_12 (37 * 16 + 4) |
oprospero | 0:4dd021344a6b | 193 | #define CPASS_MTX_20 (37 * 16 + 8) |
oprospero | 0:4dd021344a6b | 194 | #define CPASS_MTX_21 (37 * 16 + 12) |
oprospero | 0:4dd021344a6b | 195 | #define CPASS_MTX_22 (43 * 16 + 12) |
oprospero | 0:4dd021344a6b | 196 | #define D_EXT_GYRO_BIAS_X (61 * 16) |
oprospero | 0:4dd021344a6b | 197 | #define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 |
oprospero | 0:4dd021344a6b | 198 | #define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 |
oprospero | 0:4dd021344a6b | 199 | #define D_ACT0 (40 * 16) |
oprospero | 0:4dd021344a6b | 200 | #define D_ACSX (40 * 16 + 4) |
oprospero | 0:4dd021344a6b | 201 | #define D_ACSY (40 * 16 + 8) |
oprospero | 0:4dd021344a6b | 202 | #define D_ACSZ (40 * 16 + 12) |
oprospero | 0:4dd021344a6b | 203 | |
oprospero | 0:4dd021344a6b | 204 | #define FLICK_MSG (45 * 16 + 4) |
oprospero | 0:4dd021344a6b | 205 | #define FLICK_COUNTER (45 * 16 + 8) |
oprospero | 0:4dd021344a6b | 206 | #define FLICK_LOWER (45 * 16 + 12) |
oprospero | 0:4dd021344a6b | 207 | #define FLICK_UPPER (46 * 16 + 12) |
oprospero | 0:4dd021344a6b | 208 | |
oprospero | 0:4dd021344a6b | 209 | #define D_AUTH_OUT (992) |
oprospero | 0:4dd021344a6b | 210 | #define D_AUTH_IN (996) |
oprospero | 0:4dd021344a6b | 211 | #define D_AUTH_A (1000) |
oprospero | 0:4dd021344a6b | 212 | #define D_AUTH_B (1004) |
oprospero | 0:4dd021344a6b | 213 | |
oprospero | 0:4dd021344a6b | 214 | #define D_PEDSTD_BP_B (768 + 0x1C) |
oprospero | 0:4dd021344a6b | 215 | #define D_PEDSTD_HP_A (768 + 0x78) |
oprospero | 0:4dd021344a6b | 216 | #define D_PEDSTD_HP_B (768 + 0x7C) |
oprospero | 0:4dd021344a6b | 217 | #define D_PEDSTD_BP_A4 (768 + 0x40) |
oprospero | 0:4dd021344a6b | 218 | #define D_PEDSTD_BP_A3 (768 + 0x44) |
oprospero | 0:4dd021344a6b | 219 | #define D_PEDSTD_BP_A2 (768 + 0x48) |
oprospero | 0:4dd021344a6b | 220 | #define D_PEDSTD_BP_A1 (768 + 0x4C) |
oprospero | 0:4dd021344a6b | 221 | #define D_PEDSTD_INT_THRSH (768 + 0x68) |
oprospero | 0:4dd021344a6b | 222 | #define D_PEDSTD_CLIP (768 + 0x6C) |
oprospero | 0:4dd021344a6b | 223 | #define D_PEDSTD_SB (768 + 0x28) |
oprospero | 0:4dd021344a6b | 224 | #define D_PEDSTD_SB_TIME (768 + 0x2C) |
oprospero | 0:4dd021344a6b | 225 | #define D_PEDSTD_PEAKTHRSH (768 + 0x98) |
oprospero | 0:4dd021344a6b | 226 | #define D_PEDSTD_TIML (768 + 0x2A) |
oprospero | 0:4dd021344a6b | 227 | #define D_PEDSTD_TIMH (768 + 0x2E) |
oprospero | 0:4dd021344a6b | 228 | #define D_PEDSTD_PEAK (768 + 0X94) |
oprospero | 0:4dd021344a6b | 229 | #define D_PEDSTD_STEPCTR (768 + 0x60) |
oprospero | 0:4dd021344a6b | 230 | #define D_PEDSTD_TIMECTR (964) |
oprospero | 0:4dd021344a6b | 231 | #define D_PEDSTD_DECI (768 + 0xA0) |
oprospero | 0:4dd021344a6b | 232 | |
oprospero | 0:4dd021344a6b | 233 | #define D_HOST_NO_MOT (976) |
oprospero | 0:4dd021344a6b | 234 | #define D_ACCEL_BIAS (660) |
oprospero | 0:4dd021344a6b | 235 | |
oprospero | 0:4dd021344a6b | 236 | #define D_ORIENT_GAP (76) |
oprospero | 0:4dd021344a6b | 237 | |
oprospero | 0:4dd021344a6b | 238 | #define D_TILT0_H (48) |
oprospero | 0:4dd021344a6b | 239 | #define D_TILT0_L (50) |
oprospero | 0:4dd021344a6b | 240 | #define D_TILT1_H (52) |
oprospero | 0:4dd021344a6b | 241 | #define D_TILT1_L (54) |
oprospero | 0:4dd021344a6b | 242 | #define D_TILT2_H (56) |
oprospero | 0:4dd021344a6b | 243 | #define D_TILT2_L (58) |
oprospero | 0:4dd021344a6b | 244 | #define D_TILT3_H (60) |
oprospero | 0:4dd021344a6b | 245 | #define D_TILT3_L (62) |
oprospero | 0:4dd021344a6b | 246 | |
oprospero | 0:4dd021344a6b | 247 | #define DMP_CODE_SIZE (3062) |
oprospero | 0:4dd021344a6b | 248 | |
oprospero | 0:4dd021344a6b | 249 | static const unsigned char dmp_memory[DMP_CODE_SIZE] = { |
oprospero | 0:4dd021344a6b | 250 | /* bank # 0 */ |
oprospero | 0:4dd021344a6b | 251 | 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 252 | 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, |
oprospero | 0:4dd021344a6b | 253 | 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, |
oprospero | 0:4dd021344a6b | 254 | 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, |
oprospero | 0:4dd021344a6b | 255 | 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, |
oprospero | 0:4dd021344a6b | 256 | 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, |
oprospero | 0:4dd021344a6b | 257 | 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 258 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 259 | 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, |
oprospero | 0:4dd021344a6b | 260 | 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, |
oprospero | 0:4dd021344a6b | 261 | 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, |
oprospero | 0:4dd021344a6b | 262 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 263 | 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, |
oprospero | 0:4dd021344a6b | 264 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, |
oprospero | 0:4dd021344a6b | 265 | 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 266 | 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 267 | /* bank # 1 */ |
oprospero | 0:4dd021344a6b | 268 | 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, |
oprospero | 0:4dd021344a6b | 269 | 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 270 | 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 271 | 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 272 | 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 273 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 274 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, |
oprospero | 0:4dd021344a6b | 275 | 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 276 | 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 277 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 278 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, |
oprospero | 0:4dd021344a6b | 279 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 280 | 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, |
oprospero | 0:4dd021344a6b | 281 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, |
oprospero | 0:4dd021344a6b | 282 | 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, |
oprospero | 0:4dd021344a6b | 283 | 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, |
oprospero | 0:4dd021344a6b | 284 | /* bank # 2 */ |
oprospero | 0:4dd021344a6b | 285 | 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 286 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, |
oprospero | 0:4dd021344a6b | 287 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
oprospero | 0:4dd021344a6b | 288 | 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, |
oprospero | 0:4dd021344a6b | 289 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 290 | 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 291 | 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 292 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 293 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 294 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 295 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 296 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 297 | 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, |
oprospero | 0:4dd021344a6b | 298 | 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, |
oprospero | 0:4dd021344a6b | 299 | 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, |
oprospero | 0:4dd021344a6b | 300 | 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 301 | /* bank # 3 */ |
oprospero | 0:4dd021344a6b | 302 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 303 | 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, |
oprospero | 0:4dd021344a6b | 304 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, |
oprospero | 0:4dd021344a6b | 305 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 306 | 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, |
oprospero | 0:4dd021344a6b | 307 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 308 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 309 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, |
oprospero | 0:4dd021344a6b | 310 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 311 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, |
oprospero | 0:4dd021344a6b | 312 | 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 313 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 314 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 315 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 316 | 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, |
oprospero | 0:4dd021344a6b | 317 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
oprospero | 0:4dd021344a6b | 318 | |
oprospero | 0:4dd021344a6b | 319 | /* bank # 4 */ |
oprospero | 0:4dd021344a6b | 320 | 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, |
oprospero | 0:4dd021344a6b | 321 | 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, |
oprospero | 0:4dd021344a6b | 322 | 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, |
oprospero | 0:4dd021344a6b | 323 | 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, |
oprospero | 0:4dd021344a6b | 324 | 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, |
oprospero | 0:4dd021344a6b | 325 | 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, |
oprospero | 0:4dd021344a6b | 326 | 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, |
oprospero | 0:4dd021344a6b | 327 | 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, |
oprospero | 0:4dd021344a6b | 328 | 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, |
oprospero | 0:4dd021344a6b | 329 | 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, |
oprospero | 0:4dd021344a6b | 330 | 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, |
oprospero | 0:4dd021344a6b | 331 | 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, |
oprospero | 0:4dd021344a6b | 332 | 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, |
oprospero | 0:4dd021344a6b | 333 | 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, |
oprospero | 0:4dd021344a6b | 334 | 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, |
oprospero | 0:4dd021344a6b | 335 | 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, |
oprospero | 0:4dd021344a6b | 336 | /* bank # 5 */ |
oprospero | 0:4dd021344a6b | 337 | 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, |
oprospero | 0:4dd021344a6b | 338 | 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, |
oprospero | 0:4dd021344a6b | 339 | 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, |
oprospero | 0:4dd021344a6b | 340 | 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, |
oprospero | 0:4dd021344a6b | 341 | 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, |
oprospero | 0:4dd021344a6b | 342 | 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, |
oprospero | 0:4dd021344a6b | 343 | 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, |
oprospero | 0:4dd021344a6b | 344 | 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, |
oprospero | 0:4dd021344a6b | 345 | 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, |
oprospero | 0:4dd021344a6b | 346 | 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, |
oprospero | 0:4dd021344a6b | 347 | 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, |
oprospero | 0:4dd021344a6b | 348 | 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, |
oprospero | 0:4dd021344a6b | 349 | 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, |
oprospero | 0:4dd021344a6b | 350 | 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, |
oprospero | 0:4dd021344a6b | 351 | 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, |
oprospero | 0:4dd021344a6b | 352 | 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, |
oprospero | 0:4dd021344a6b | 353 | /* bank # 6 */ |
oprospero | 0:4dd021344a6b | 354 | 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, |
oprospero | 0:4dd021344a6b | 355 | 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, |
oprospero | 0:4dd021344a6b | 356 | 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, |
oprospero | 0:4dd021344a6b | 357 | 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, |
oprospero | 0:4dd021344a6b | 358 | 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, |
oprospero | 0:4dd021344a6b | 359 | 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, |
oprospero | 0:4dd021344a6b | 360 | 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, |
oprospero | 0:4dd021344a6b | 361 | 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, |
oprospero | 0:4dd021344a6b | 362 | 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, |
oprospero | 0:4dd021344a6b | 363 | 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, |
oprospero | 0:4dd021344a6b | 364 | 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, |
oprospero | 0:4dd021344a6b | 365 | 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, |
oprospero | 0:4dd021344a6b | 366 | 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, |
oprospero | 0:4dd021344a6b | 367 | 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, |
oprospero | 0:4dd021344a6b | 368 | 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, |
oprospero | 0:4dd021344a6b | 369 | 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, |
oprospero | 0:4dd021344a6b | 370 | /* bank # 7 */ |
oprospero | 0:4dd021344a6b | 371 | 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, |
oprospero | 0:4dd021344a6b | 372 | 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, |
oprospero | 0:4dd021344a6b | 373 | 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, |
oprospero | 0:4dd021344a6b | 374 | 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, |
oprospero | 0:4dd021344a6b | 375 | 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, |
oprospero | 0:4dd021344a6b | 376 | 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, |
oprospero | 0:4dd021344a6b | 377 | 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, |
oprospero | 0:4dd021344a6b | 378 | 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, |
oprospero | 0:4dd021344a6b | 379 | 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, |
oprospero | 0:4dd021344a6b | 380 | 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, |
oprospero | 0:4dd021344a6b | 381 | 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, |
oprospero | 0:4dd021344a6b | 382 | 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, |
oprospero | 0:4dd021344a6b | 383 | 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, |
oprospero | 0:4dd021344a6b | 384 | 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, |
oprospero | 0:4dd021344a6b | 385 | 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, |
oprospero | 0:4dd021344a6b | 386 | 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, |
oprospero | 0:4dd021344a6b | 387 | /* bank # 8 */ |
oprospero | 0:4dd021344a6b | 388 | 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, |
oprospero | 0:4dd021344a6b | 389 | 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, |
oprospero | 0:4dd021344a6b | 390 | 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, |
oprospero | 0:4dd021344a6b | 391 | 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, |
oprospero | 0:4dd021344a6b | 392 | 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, |
oprospero | 0:4dd021344a6b | 393 | 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, |
oprospero | 0:4dd021344a6b | 394 | 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, |
oprospero | 0:4dd021344a6b | 395 | 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, |
oprospero | 0:4dd021344a6b | 396 | 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, |
oprospero | 0:4dd021344a6b | 397 | 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, |
oprospero | 0:4dd021344a6b | 398 | 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, |
oprospero | 0:4dd021344a6b | 399 | 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, |
oprospero | 0:4dd021344a6b | 400 | 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, |
oprospero | 0:4dd021344a6b | 401 | 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, |
oprospero | 0:4dd021344a6b | 402 | 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, |
oprospero | 0:4dd021344a6b | 403 | 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, |
oprospero | 0:4dd021344a6b | 404 | /* bank # 9 */ |
oprospero | 0:4dd021344a6b | 405 | 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, |
oprospero | 0:4dd021344a6b | 406 | 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, |
oprospero | 0:4dd021344a6b | 407 | 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, |
oprospero | 0:4dd021344a6b | 408 | 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, |
oprospero | 0:4dd021344a6b | 409 | 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, |
oprospero | 0:4dd021344a6b | 410 | 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, |
oprospero | 0:4dd021344a6b | 411 | 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, |
oprospero | 0:4dd021344a6b | 412 | 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, |
oprospero | 0:4dd021344a6b | 413 | 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, |
oprospero | 0:4dd021344a6b | 414 | 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, |
oprospero | 0:4dd021344a6b | 415 | 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, |
oprospero | 0:4dd021344a6b | 416 | 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, |
oprospero | 0:4dd021344a6b | 417 | 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, |
oprospero | 0:4dd021344a6b | 418 | 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, |
oprospero | 0:4dd021344a6b | 419 | 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, |
oprospero | 0:4dd021344a6b | 420 | 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, |
oprospero | 0:4dd021344a6b | 421 | /* bank # 10 */ |
oprospero | 0:4dd021344a6b | 422 | 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, |
oprospero | 0:4dd021344a6b | 423 | 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, |
oprospero | 0:4dd021344a6b | 424 | 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, |
oprospero | 0:4dd021344a6b | 425 | 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, |
oprospero | 0:4dd021344a6b | 426 | 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, |
oprospero | 0:4dd021344a6b | 427 | 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, |
oprospero | 0:4dd021344a6b | 428 | 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, |
oprospero | 0:4dd021344a6b | 429 | 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, |
oprospero | 0:4dd021344a6b | 430 | 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, |
oprospero | 0:4dd021344a6b | 431 | 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, |
oprospero | 0:4dd021344a6b | 432 | 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, |
oprospero | 0:4dd021344a6b | 433 | 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, |
oprospero | 0:4dd021344a6b | 434 | 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, |
oprospero | 0:4dd021344a6b | 435 | 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, |
oprospero | 0:4dd021344a6b | 436 | 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, |
oprospero | 0:4dd021344a6b | 437 | 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, |
oprospero | 0:4dd021344a6b | 438 | /* bank # 11 */ |
oprospero | 0:4dd021344a6b | 439 | 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, |
oprospero | 0:4dd021344a6b | 440 | 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, |
oprospero | 0:4dd021344a6b | 441 | 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, |
oprospero | 0:4dd021344a6b | 442 | 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, |
oprospero | 0:4dd021344a6b | 443 | 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, |
oprospero | 0:4dd021344a6b | 444 | 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, |
oprospero | 0:4dd021344a6b | 445 | 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, |
oprospero | 0:4dd021344a6b | 446 | 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, |
oprospero | 0:4dd021344a6b | 447 | 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, |
oprospero | 0:4dd021344a6b | 448 | 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, |
oprospero | 0:4dd021344a6b | 449 | 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, |
oprospero | 0:4dd021344a6b | 450 | 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, |
oprospero | 0:4dd021344a6b | 451 | 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, |
oprospero | 0:4dd021344a6b | 452 | 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, |
oprospero | 0:4dd021344a6b | 453 | 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, |
oprospero | 0:4dd021344a6b | 454 | 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff |
oprospero | 0:4dd021344a6b | 455 | }; |
oprospero | 0:4dd021344a6b | 456 | |
oprospero | 0:4dd021344a6b | 457 | static const unsigned short sStartAddress = 0x0400; |
oprospero | 0:4dd021344a6b | 458 | |
oprospero | 0:4dd021344a6b | 459 | /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ |
oprospero | 0:4dd021344a6b | 460 | |
oprospero | 0:4dd021344a6b | 461 | #define INT_SRC_TAP (0x01) |
oprospero | 0:4dd021344a6b | 462 | #define INT_SRC_ANDROID_ORIENT (0x08) |
oprospero | 0:4dd021344a6b | 463 | |
oprospero | 0:4dd021344a6b | 464 | #define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ |
oprospero | 0:4dd021344a6b | 465 | DMP_FEATURE_SEND_CAL_GYRO) |
oprospero | 0:4dd021344a6b | 466 | |
oprospero | 0:4dd021344a6b | 467 | #define MAX_PACKET_LENGTH (32) |
oprospero | 0:4dd021344a6b | 468 | |
oprospero | 0:4dd021344a6b | 469 | #define DMP_SAMPLE_RATE (200) |
oprospero | 0:4dd021344a6b | 470 | #define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) |
oprospero | 0:4dd021344a6b | 471 | |
oprospero | 0:4dd021344a6b | 472 | #define FIFO_CORRUPTION_CHECK |
oprospero | 0:4dd021344a6b | 473 | #ifdef FIFO_CORRUPTION_CHECK |
oprospero | 0:4dd021344a6b | 474 | #define QUAT_ERROR_THRESH (1L<<24) |
oprospero | 0:4dd021344a6b | 475 | #define QUAT_MAG_SQ_NORMALIZED (1L<<28) |
oprospero | 0:4dd021344a6b | 476 | #define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) |
oprospero | 0:4dd021344a6b | 477 | #define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) |
oprospero | 0:4dd021344a6b | 478 | #endif |
oprospero | 0:4dd021344a6b | 479 | |
oprospero | 0:4dd021344a6b | 480 | struct dmp_s { |
oprospero | 0:4dd021344a6b | 481 | void (*tap_cb)(unsigned char count, unsigned char direction); |
oprospero | 0:4dd021344a6b | 482 | void (*android_orient_cb)(unsigned char orientation); |
oprospero | 0:4dd021344a6b | 483 | unsigned short orient; |
oprospero | 0:4dd021344a6b | 484 | unsigned short feature_mask; |
oprospero | 0:4dd021344a6b | 485 | unsigned short fifo_rate; |
oprospero | 0:4dd021344a6b | 486 | unsigned char packet_length; |
oprospero | 0:4dd021344a6b | 487 | }; |
oprospero | 0:4dd021344a6b | 488 | |
oprospero | 0:4dd021344a6b | 489 | static struct dmp_s dmp = { |
oprospero | 0:4dd021344a6b | 490 | .tap_cb = NULL, |
oprospero | 0:4dd021344a6b | 491 | .android_orient_cb = NULL, |
oprospero | 0:4dd021344a6b | 492 | .orient = 0, |
oprospero | 0:4dd021344a6b | 493 | .feature_mask = 0, |
oprospero | 0:4dd021344a6b | 494 | .fifo_rate = 0, |
oprospero | 0:4dd021344a6b | 495 | .packet_length = 0 |
oprospero | 0:4dd021344a6b | 496 | }; |
oprospero | 0:4dd021344a6b | 497 | |
oprospero | 0:4dd021344a6b | 498 | inline unsigned short inv_row_2_scale(const signed char *row) |
oprospero | 0:4dd021344a6b | 499 | { |
oprospero | 0:4dd021344a6b | 500 | unsigned short b; |
oprospero | 0:4dd021344a6b | 501 | |
oprospero | 0:4dd021344a6b | 502 | if (row[0] > 0) |
oprospero | 0:4dd021344a6b | 503 | b = 0; |
oprospero | 0:4dd021344a6b | 504 | else if (row[0] < 0) |
oprospero | 0:4dd021344a6b | 505 | b = 4; |
oprospero | 0:4dd021344a6b | 506 | else if (row[1] > 0) |
oprospero | 0:4dd021344a6b | 507 | b = 1; |
oprospero | 0:4dd021344a6b | 508 | else if (row[1] < 0) |
oprospero | 0:4dd021344a6b | 509 | b = 5; |
oprospero | 0:4dd021344a6b | 510 | else if (row[2] > 0) |
oprospero | 0:4dd021344a6b | 511 | b = 2; |
oprospero | 0:4dd021344a6b | 512 | else if (row[2] < 0) |
oprospero | 0:4dd021344a6b | 513 | b = 6; |
oprospero | 0:4dd021344a6b | 514 | else |
oprospero | 0:4dd021344a6b | 515 | b = 7; // error |
oprospero | 0:4dd021344a6b | 516 | return b; |
oprospero | 0:4dd021344a6b | 517 | } |
oprospero | 0:4dd021344a6b | 518 | |
oprospero | 0:4dd021344a6b | 519 | unsigned short DMP_Motion_Driver::inv_orientation_matrix_to_scalar(const signed char *mtx) |
oprospero | 0:4dd021344a6b | 520 | { |
oprospero | 0:4dd021344a6b | 521 | unsigned short scalar; |
oprospero | 0:4dd021344a6b | 522 | |
oprospero | 0:4dd021344a6b | 523 | /* |
oprospero | 0:4dd021344a6b | 524 | XYZ 010_001_000 Identity Matrix |
oprospero | 0:4dd021344a6b | 525 | XZY 001_010_000 |
oprospero | 0:4dd021344a6b | 526 | YXZ 010_000_001 |
oprospero | 0:4dd021344a6b | 527 | YZX 000_010_001 |
oprospero | 0:4dd021344a6b | 528 | ZXY 001_000_010 |
oprospero | 0:4dd021344a6b | 529 | ZYX 000_001_010 |
oprospero | 0:4dd021344a6b | 530 | */ |
oprospero | 0:4dd021344a6b | 531 | |
oprospero | 0:4dd021344a6b | 532 | scalar = inv_row_2_scale(mtx); |
oprospero | 0:4dd021344a6b | 533 | scalar |= inv_row_2_scale(mtx + 3) << 3; |
oprospero | 0:4dd021344a6b | 534 | scalar |= inv_row_2_scale(mtx + 6) << 6; |
oprospero | 0:4dd021344a6b | 535 | |
oprospero | 0:4dd021344a6b | 536 | |
oprospero | 0:4dd021344a6b | 537 | return scalar; |
oprospero | 0:4dd021344a6b | 538 | } |
oprospero | 0:4dd021344a6b | 539 | |
oprospero | 0:4dd021344a6b | 540 | |
oprospero | 0:4dd021344a6b | 541 | DMP_Motion_Driver::DMP_Motion_Driver() |
oprospero | 0:4dd021344a6b | 542 | { |
oprospero | 0:4dd021344a6b | 543 | InvMpu(); |
oprospero | 0:4dd021344a6b | 544 | } |
oprospero | 0:4dd021344a6b | 545 | |
oprospero | 0:4dd021344a6b | 546 | |
oprospero | 0:4dd021344a6b | 547 | /** |
oprospero | 0:4dd021344a6b | 548 | * @brief Load the DMP with this image. |
oprospero | 0:4dd021344a6b | 549 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 550 | */ |
oprospero | 0:4dd021344a6b | 551 | int DMP_Motion_Driver::dmp_load_motion_driver_firmware(void) |
oprospero | 0:4dd021344a6b | 552 | { |
oprospero | 0:4dd021344a6b | 553 | return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, |
oprospero | 0:4dd021344a6b | 554 | DMP_SAMPLE_RATE); |
oprospero | 0:4dd021344a6b | 555 | } |
oprospero | 0:4dd021344a6b | 556 | |
oprospero | 0:4dd021344a6b | 557 | /** |
oprospero | 0:4dd021344a6b | 558 | * @brief Push gyro and accel orientation to the DMP. |
oprospero | 0:4dd021344a6b | 559 | * The orientation is represented here as the output of |
oprospero | 0:4dd021344a6b | 560 | * @e inv_orientation_matrix_to_scalar. |
oprospero | 0:4dd021344a6b | 561 | * @param[in] orient Gyro and accel orientation in body frame. |
oprospero | 0:4dd021344a6b | 562 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 563 | */ |
oprospero | 0:4dd021344a6b | 564 | int DMP_Motion_Driver::dmp_set_orientation(unsigned short orient) |
oprospero | 0:4dd021344a6b | 565 | { |
oprospero | 0:4dd021344a6b | 566 | unsigned char gyro_regs[3], accel_regs[3]; |
oprospero | 0:4dd021344a6b | 567 | const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; |
oprospero | 0:4dd021344a6b | 568 | const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; |
oprospero | 0:4dd021344a6b | 569 | const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; |
oprospero | 0:4dd021344a6b | 570 | const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; |
oprospero | 0:4dd021344a6b | 571 | |
oprospero | 0:4dd021344a6b | 572 | gyro_regs[0] = gyro_axes[orient & 3]; |
oprospero | 0:4dd021344a6b | 573 | gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; |
oprospero | 0:4dd021344a6b | 574 | gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; |
oprospero | 0:4dd021344a6b | 575 | accel_regs[0] = accel_axes[orient & 3]; |
oprospero | 0:4dd021344a6b | 576 | accel_regs[1] = accel_axes[(orient >> 3) & 3]; |
oprospero | 0:4dd021344a6b | 577 | accel_regs[2] = accel_axes[(orient >> 6) & 3]; |
oprospero | 0:4dd021344a6b | 578 | |
oprospero | 0:4dd021344a6b | 579 | /* Chip-to-body, axes only. */ |
oprospero | 0:4dd021344a6b | 580 | if (mpu_write_mem(FCFG_1, 3, gyro_regs)) |
oprospero | 0:4dd021344a6b | 581 | return -1; |
oprospero | 0:4dd021344a6b | 582 | if (mpu_write_mem(FCFG_2, 3, accel_regs)) |
oprospero | 0:4dd021344a6b | 583 | return -1; |
oprospero | 0:4dd021344a6b | 584 | |
oprospero | 0:4dd021344a6b | 585 | memcpy(gyro_regs, gyro_sign, 3); |
oprospero | 0:4dd021344a6b | 586 | memcpy(accel_regs, accel_sign, 3); |
oprospero | 0:4dd021344a6b | 587 | if (orient & 4) { |
oprospero | 0:4dd021344a6b | 588 | gyro_regs[0] |= 1; |
oprospero | 0:4dd021344a6b | 589 | accel_regs[0] |= 1; |
oprospero | 0:4dd021344a6b | 590 | } |
oprospero | 0:4dd021344a6b | 591 | if (orient & 0x20) { |
oprospero | 0:4dd021344a6b | 592 | gyro_regs[1] |= 1; |
oprospero | 0:4dd021344a6b | 593 | accel_regs[1] |= 1; |
oprospero | 0:4dd021344a6b | 594 | } |
oprospero | 0:4dd021344a6b | 595 | if (orient & 0x100) { |
oprospero | 0:4dd021344a6b | 596 | gyro_regs[2] |= 1; |
oprospero | 0:4dd021344a6b | 597 | accel_regs[2] |= 1; |
oprospero | 0:4dd021344a6b | 598 | } |
oprospero | 0:4dd021344a6b | 599 | |
oprospero | 0:4dd021344a6b | 600 | /* Chip-to-body, sign only. */ |
oprospero | 0:4dd021344a6b | 601 | if (mpu_write_mem(FCFG_3, 3, gyro_regs)) |
oprospero | 0:4dd021344a6b | 602 | return -1; |
oprospero | 0:4dd021344a6b | 603 | if (mpu_write_mem(FCFG_7, 3, accel_regs)) |
oprospero | 0:4dd021344a6b | 604 | return -1; |
oprospero | 0:4dd021344a6b | 605 | dmp.orient = orient; |
oprospero | 0:4dd021344a6b | 606 | return 0; |
oprospero | 0:4dd021344a6b | 607 | } |
oprospero | 0:4dd021344a6b | 608 | |
oprospero | 0:4dd021344a6b | 609 | /** |
oprospero | 0:4dd021344a6b | 610 | * @brief Push gyro biases to the DMP. |
oprospero | 0:4dd021344a6b | 611 | * Because the gyro integration is handled in the DMP, any gyro biases |
oprospero | 0:4dd021344a6b | 612 | * calculated by the MPL should be pushed down to DMP memory to remove |
oprospero | 0:4dd021344a6b | 613 | * 3-axis quaternion drift. |
oprospero | 0:4dd021344a6b | 614 | * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will |
oprospero | 0:4dd021344a6b | 615 | * overwrite the biases written to this location once a new one is computed. |
oprospero | 0:4dd021344a6b | 616 | * @param[in] bias Gyro biases in q16. |
oprospero | 0:4dd021344a6b | 617 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 618 | */ |
oprospero | 0:4dd021344a6b | 619 | int DMP_Motion_Driver::dmp_set_gyro_bias(long *bias) |
oprospero | 0:4dd021344a6b | 620 | { |
oprospero | 0:4dd021344a6b | 621 | long gyro_bias_body[3]; |
oprospero | 0:4dd021344a6b | 622 | unsigned char regs[4]; |
oprospero | 0:4dd021344a6b | 623 | |
oprospero | 0:4dd021344a6b | 624 | gyro_bias_body[0] = bias[dmp.orient & 3]; |
oprospero | 0:4dd021344a6b | 625 | if (dmp.orient & 4) |
oprospero | 0:4dd021344a6b | 626 | gyro_bias_body[0] *= -1; |
oprospero | 0:4dd021344a6b | 627 | gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; |
oprospero | 0:4dd021344a6b | 628 | if (dmp.orient & 0x20) |
oprospero | 0:4dd021344a6b | 629 | gyro_bias_body[1] *= -1; |
oprospero | 0:4dd021344a6b | 630 | gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; |
oprospero | 0:4dd021344a6b | 631 | if (dmp.orient & 0x100) |
oprospero | 0:4dd021344a6b | 632 | gyro_bias_body[2] *= -1; |
oprospero | 0:4dd021344a6b | 633 | |
oprospero | 0:4dd021344a6b | 634 | #ifdef EMPL_NO_64BIT |
oprospero | 0:4dd021344a6b | 635 | gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); |
oprospero | 0:4dd021344a6b | 636 | gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); |
oprospero | 0:4dd021344a6b | 637 | gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); |
oprospero | 0:4dd021344a6b | 638 | #else |
oprospero | 0:4dd021344a6b | 639 | gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); |
oprospero | 0:4dd021344a6b | 640 | gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); |
oprospero | 0:4dd021344a6b | 641 | gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); |
oprospero | 0:4dd021344a6b | 642 | #endif |
oprospero | 0:4dd021344a6b | 643 | |
oprospero | 0:4dd021344a6b | 644 | regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 645 | regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 646 | regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 647 | regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); |
oprospero | 0:4dd021344a6b | 648 | if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) |
oprospero | 0:4dd021344a6b | 649 | return -1; |
oprospero | 0:4dd021344a6b | 650 | |
oprospero | 0:4dd021344a6b | 651 | regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 652 | regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 653 | regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 654 | regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); |
oprospero | 0:4dd021344a6b | 655 | if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) |
oprospero | 0:4dd021344a6b | 656 | return -1; |
oprospero | 0:4dd021344a6b | 657 | |
oprospero | 0:4dd021344a6b | 658 | regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 659 | regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 660 | regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 661 | regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); |
oprospero | 0:4dd021344a6b | 662 | return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); |
oprospero | 0:4dd021344a6b | 663 | } |
oprospero | 0:4dd021344a6b | 664 | |
oprospero | 0:4dd021344a6b | 665 | /** |
oprospero | 0:4dd021344a6b | 666 | * @brief Push accel biases to the DMP. |
oprospero | 0:4dd021344a6b | 667 | * These biases will be removed from the DMP 6-axis quaternion. |
oprospero | 0:4dd021344a6b | 668 | * @param[in] bias Accel biases in q16. |
oprospero | 0:4dd021344a6b | 669 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 670 | */ |
oprospero | 0:4dd021344a6b | 671 | int DMP_Motion_Driver::dmp_set_accel_bias(long *bias) |
oprospero | 0:4dd021344a6b | 672 | { |
oprospero | 0:4dd021344a6b | 673 | long accel_bias_body[3]; |
oprospero | 0:4dd021344a6b | 674 | unsigned char regs[12]; |
oprospero | 0:4dd021344a6b | 675 | long long accel_sf; |
oprospero | 0:4dd021344a6b | 676 | unsigned short accel_sens; |
oprospero | 0:4dd021344a6b | 677 | |
oprospero | 0:4dd021344a6b | 678 | mpu_get_accel_sens(&accel_sens); |
oprospero | 0:4dd021344a6b | 679 | accel_sf = (long long)accel_sens << 15; |
oprospero | 0:4dd021344a6b | 680 | // __no_operation(); |
oprospero | 0:4dd021344a6b | 681 | |
oprospero | 0:4dd021344a6b | 682 | accel_bias_body[0] = bias[dmp.orient & 3]; |
oprospero | 0:4dd021344a6b | 683 | if (dmp.orient & 4) |
oprospero | 0:4dd021344a6b | 684 | accel_bias_body[0] *= -1; |
oprospero | 0:4dd021344a6b | 685 | accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; |
oprospero | 0:4dd021344a6b | 686 | if (dmp.orient & 0x20) |
oprospero | 0:4dd021344a6b | 687 | accel_bias_body[1] *= -1; |
oprospero | 0:4dd021344a6b | 688 | accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; |
oprospero | 0:4dd021344a6b | 689 | if (dmp.orient & 0x100) |
oprospero | 0:4dd021344a6b | 690 | accel_bias_body[2] *= -1; |
oprospero | 0:4dd021344a6b | 691 | |
oprospero | 0:4dd021344a6b | 692 | #ifdef EMPL_NO_64BIT |
oprospero | 0:4dd021344a6b | 693 | accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); |
oprospero | 0:4dd021344a6b | 694 | accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); |
oprospero | 0:4dd021344a6b | 695 | accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); |
oprospero | 0:4dd021344a6b | 696 | #else |
oprospero | 0:4dd021344a6b | 697 | accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); |
oprospero | 0:4dd021344a6b | 698 | accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); |
oprospero | 0:4dd021344a6b | 699 | accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); |
oprospero | 0:4dd021344a6b | 700 | #endif |
oprospero | 0:4dd021344a6b | 701 | |
oprospero | 0:4dd021344a6b | 702 | regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 703 | regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 704 | regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 705 | regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); |
oprospero | 0:4dd021344a6b | 706 | regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 707 | regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 708 | regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 709 | regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); |
oprospero | 0:4dd021344a6b | 710 | regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 711 | regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 712 | regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 713 | regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); |
oprospero | 0:4dd021344a6b | 714 | return mpu_write_mem(D_ACCEL_BIAS, 12, regs); |
oprospero | 0:4dd021344a6b | 715 | } |
oprospero | 0:4dd021344a6b | 716 | |
oprospero | 0:4dd021344a6b | 717 | /** |
oprospero | 0:4dd021344a6b | 718 | * @brief Set DMP output rate. |
oprospero | 0:4dd021344a6b | 719 | * Only used when DMP is on. |
oprospero | 0:4dd021344a6b | 720 | * @param[in] rate Desired fifo rate (Hz). |
oprospero | 0:4dd021344a6b | 721 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 722 | */ |
oprospero | 0:4dd021344a6b | 723 | int DMP_Motion_Driver::dmp_set_fifo_rate(unsigned short rate) |
oprospero | 0:4dd021344a6b | 724 | { |
oprospero | 0:4dd021344a6b | 725 | const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, |
oprospero | 0:4dd021344a6b | 726 | 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; |
oprospero | 0:4dd021344a6b | 727 | unsigned short div; |
oprospero | 0:4dd021344a6b | 728 | unsigned char tmp[8]; |
oprospero | 0:4dd021344a6b | 729 | |
oprospero | 0:4dd021344a6b | 730 | if (rate > DMP_SAMPLE_RATE) |
oprospero | 0:4dd021344a6b | 731 | return -1; |
oprospero | 0:4dd021344a6b | 732 | div = DMP_SAMPLE_RATE / rate - 1; |
oprospero | 0:4dd021344a6b | 733 | tmp[0] = (unsigned char)((div >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 734 | tmp[1] = (unsigned char)(div & 0xFF); |
oprospero | 0:4dd021344a6b | 735 | if (mpu_write_mem(D_0_22, 2, tmp)) |
oprospero | 0:4dd021344a6b | 736 | return -1; |
oprospero | 0:4dd021344a6b | 737 | if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) |
oprospero | 0:4dd021344a6b | 738 | return -1; |
oprospero | 0:4dd021344a6b | 739 | |
oprospero | 0:4dd021344a6b | 740 | dmp.fifo_rate = rate; |
oprospero | 0:4dd021344a6b | 741 | return 0; |
oprospero | 0:4dd021344a6b | 742 | } |
oprospero | 0:4dd021344a6b | 743 | |
oprospero | 0:4dd021344a6b | 744 | /** |
oprospero | 0:4dd021344a6b | 745 | * @brief Get DMP output rate. |
oprospero | 0:4dd021344a6b | 746 | * @param[out] rate Current fifo rate (Hz). |
oprospero | 0:4dd021344a6b | 747 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 748 | */ |
oprospero | 0:4dd021344a6b | 749 | int DMP_Motion_Driver::dmp_get_fifo_rate(unsigned short *rate) |
oprospero | 0:4dd021344a6b | 750 | { |
oprospero | 0:4dd021344a6b | 751 | rate[0] = dmp.fifo_rate; |
oprospero | 0:4dd021344a6b | 752 | return 0; |
oprospero | 0:4dd021344a6b | 753 | } |
oprospero | 0:4dd021344a6b | 754 | |
oprospero | 0:4dd021344a6b | 755 | /** |
oprospero | 0:4dd021344a6b | 756 | * @brief Set tap threshold for a specific axis. |
oprospero | 0:4dd021344a6b | 757 | * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. |
oprospero | 0:4dd021344a6b | 758 | * @param[in] thresh Tap threshold, in mg/ms. |
oprospero | 0:4dd021344a6b | 759 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 760 | */ |
oprospero | 0:4dd021344a6b | 761 | int DMP_Motion_Driver::dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) |
oprospero | 0:4dd021344a6b | 762 | { |
oprospero | 0:4dd021344a6b | 763 | unsigned char tmp[4], accel_fsr; |
oprospero | 0:4dd021344a6b | 764 | float scaled_thresh; |
oprospero | 0:4dd021344a6b | 765 | unsigned short dmp_thresh, dmp_thresh_2; |
oprospero | 0:4dd021344a6b | 766 | if (!(axis & TAP_XYZ) || thresh > 1600) |
oprospero | 0:4dd021344a6b | 767 | return -1; |
oprospero | 0:4dd021344a6b | 768 | |
oprospero | 0:4dd021344a6b | 769 | scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; |
oprospero | 0:4dd021344a6b | 770 | |
oprospero | 0:4dd021344a6b | 771 | mpu_get_accel_fsr(&accel_fsr); |
oprospero | 0:4dd021344a6b | 772 | switch (accel_fsr) { |
oprospero | 0:4dd021344a6b | 773 | case 2: |
oprospero | 0:4dd021344a6b | 774 | dmp_thresh = (unsigned short)(scaled_thresh * 16384); |
oprospero | 0:4dd021344a6b | 775 | /* dmp_thresh * 0.75 */ |
oprospero | 0:4dd021344a6b | 776 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); |
oprospero | 0:4dd021344a6b | 777 | break; |
oprospero | 0:4dd021344a6b | 778 | case 4: |
oprospero | 0:4dd021344a6b | 779 | dmp_thresh = (unsigned short)(scaled_thresh * 8192); |
oprospero | 0:4dd021344a6b | 780 | /* dmp_thresh * 0.75 */ |
oprospero | 0:4dd021344a6b | 781 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); |
oprospero | 0:4dd021344a6b | 782 | break; |
oprospero | 0:4dd021344a6b | 783 | case 8: |
oprospero | 0:4dd021344a6b | 784 | dmp_thresh = (unsigned short)(scaled_thresh * 4096); |
oprospero | 0:4dd021344a6b | 785 | /* dmp_thresh * 0.75 */ |
oprospero | 0:4dd021344a6b | 786 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); |
oprospero | 0:4dd021344a6b | 787 | break; |
oprospero | 0:4dd021344a6b | 788 | case 16: |
oprospero | 0:4dd021344a6b | 789 | dmp_thresh = (unsigned short)(scaled_thresh * 2048); |
oprospero | 0:4dd021344a6b | 790 | /* dmp_thresh * 0.75 */ |
oprospero | 0:4dd021344a6b | 791 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); |
oprospero | 0:4dd021344a6b | 792 | break; |
oprospero | 0:4dd021344a6b | 793 | default: |
oprospero | 0:4dd021344a6b | 794 | return -1; |
oprospero | 0:4dd021344a6b | 795 | } |
oprospero | 0:4dd021344a6b | 796 | tmp[0] = (unsigned char)(dmp_thresh >> 8); |
oprospero | 0:4dd021344a6b | 797 | tmp[1] = (unsigned char)(dmp_thresh & 0xFF); |
oprospero | 0:4dd021344a6b | 798 | tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); |
oprospero | 0:4dd021344a6b | 799 | tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); |
oprospero | 0:4dd021344a6b | 800 | |
oprospero | 0:4dd021344a6b | 801 | if (axis & TAP_X) { |
oprospero | 0:4dd021344a6b | 802 | if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) |
oprospero | 0:4dd021344a6b | 803 | return -1; |
oprospero | 0:4dd021344a6b | 804 | if (mpu_write_mem(D_1_36, 2, tmp+2)) |
oprospero | 0:4dd021344a6b | 805 | return -1; |
oprospero | 0:4dd021344a6b | 806 | } |
oprospero | 0:4dd021344a6b | 807 | if (axis & TAP_Y) { |
oprospero | 0:4dd021344a6b | 808 | if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) |
oprospero | 0:4dd021344a6b | 809 | return -1; |
oprospero | 0:4dd021344a6b | 810 | if (mpu_write_mem(D_1_40, 2, tmp+2)) |
oprospero | 0:4dd021344a6b | 811 | return -1; |
oprospero | 0:4dd021344a6b | 812 | } |
oprospero | 0:4dd021344a6b | 813 | if (axis & TAP_Z) { |
oprospero | 0:4dd021344a6b | 814 | if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) |
oprospero | 0:4dd021344a6b | 815 | return -1; |
oprospero | 0:4dd021344a6b | 816 | if (mpu_write_mem(D_1_44, 2, tmp+2)) |
oprospero | 0:4dd021344a6b | 817 | return -1; |
oprospero | 0:4dd021344a6b | 818 | } |
oprospero | 0:4dd021344a6b | 819 | return 0; |
oprospero | 0:4dd021344a6b | 820 | } |
oprospero | 0:4dd021344a6b | 821 | |
oprospero | 0:4dd021344a6b | 822 | /** |
oprospero | 0:4dd021344a6b | 823 | * @brief Set which axes will register a tap. |
oprospero | 0:4dd021344a6b | 824 | * @param[in] axis 1, 2, and 4 for XYZ, respectively. |
oprospero | 0:4dd021344a6b | 825 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 826 | */ |
oprospero | 0:4dd021344a6b | 827 | int DMP_Motion_Driver::dmp_set_tap_axes(unsigned char axis) |
oprospero | 0:4dd021344a6b | 828 | { |
oprospero | 0:4dd021344a6b | 829 | unsigned char tmp = 0; |
oprospero | 0:4dd021344a6b | 830 | |
oprospero | 0:4dd021344a6b | 831 | if (axis & TAP_X) |
oprospero | 0:4dd021344a6b | 832 | tmp |= 0x30; |
oprospero | 0:4dd021344a6b | 833 | if (axis & TAP_Y) |
oprospero | 0:4dd021344a6b | 834 | tmp |= 0x0C; |
oprospero | 0:4dd021344a6b | 835 | if (axis & TAP_Z) |
oprospero | 0:4dd021344a6b | 836 | tmp |= 0x03; |
oprospero | 0:4dd021344a6b | 837 | return mpu_write_mem(D_1_72, 1, &tmp); |
oprospero | 0:4dd021344a6b | 838 | } |
oprospero | 0:4dd021344a6b | 839 | |
oprospero | 0:4dd021344a6b | 840 | /** |
oprospero | 0:4dd021344a6b | 841 | * @brief Set minimum number of taps needed for an interrupt. |
oprospero | 0:4dd021344a6b | 842 | * @param[in] min_taps Minimum consecutive taps (1-4). |
oprospero | 0:4dd021344a6b | 843 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 844 | */ |
oprospero | 0:4dd021344a6b | 845 | int DMP_Motion_Driver::dmp_set_tap_count(unsigned char min_taps) |
oprospero | 0:4dd021344a6b | 846 | { |
oprospero | 0:4dd021344a6b | 847 | unsigned char tmp; |
oprospero | 0:4dd021344a6b | 848 | |
oprospero | 0:4dd021344a6b | 849 | if (min_taps < 1) |
oprospero | 0:4dd021344a6b | 850 | min_taps = 1; |
oprospero | 0:4dd021344a6b | 851 | else if (min_taps > 4) |
oprospero | 0:4dd021344a6b | 852 | min_taps = 4; |
oprospero | 0:4dd021344a6b | 853 | |
oprospero | 0:4dd021344a6b | 854 | tmp = min_taps - 1; |
oprospero | 0:4dd021344a6b | 855 | return mpu_write_mem(D_1_79, 1, &tmp); |
oprospero | 0:4dd021344a6b | 856 | } |
oprospero | 0:4dd021344a6b | 857 | |
oprospero | 0:4dd021344a6b | 858 | /** |
oprospero | 0:4dd021344a6b | 859 | * @brief Set length between valid taps. |
oprospero | 0:4dd021344a6b | 860 | * @param[in] time Milliseconds between taps. |
oprospero | 0:4dd021344a6b | 861 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 862 | */ |
oprospero | 0:4dd021344a6b | 863 | int DMP_Motion_Driver::dmp_set_tap_time(unsigned short time) |
oprospero | 0:4dd021344a6b | 864 | { |
oprospero | 0:4dd021344a6b | 865 | unsigned short dmp_time; |
oprospero | 0:4dd021344a6b | 866 | unsigned char tmp[2]; |
oprospero | 0:4dd021344a6b | 867 | |
oprospero | 0:4dd021344a6b | 868 | dmp_time = time / (1000 / DMP_SAMPLE_RATE); |
oprospero | 0:4dd021344a6b | 869 | tmp[0] = (unsigned char)(dmp_time >> 8); |
oprospero | 0:4dd021344a6b | 870 | tmp[1] = (unsigned char)(dmp_time & 0xFF); |
oprospero | 0:4dd021344a6b | 871 | return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); |
oprospero | 0:4dd021344a6b | 872 | } |
oprospero | 0:4dd021344a6b | 873 | |
oprospero | 0:4dd021344a6b | 874 | /** |
oprospero | 0:4dd021344a6b | 875 | * @brief Set max time between taps to register as a multi-tap. |
oprospero | 0:4dd021344a6b | 876 | * @param[in] time Max milliseconds between taps. |
oprospero | 0:4dd021344a6b | 877 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 878 | */ |
oprospero | 0:4dd021344a6b | 879 | int DMP_Motion_Driver::dmp_set_tap_time_multi(unsigned short time) |
oprospero | 0:4dd021344a6b | 880 | { |
oprospero | 0:4dd021344a6b | 881 | unsigned short dmp_time; |
oprospero | 0:4dd021344a6b | 882 | unsigned char tmp[2]; |
oprospero | 0:4dd021344a6b | 883 | |
oprospero | 0:4dd021344a6b | 884 | dmp_time = time / (1000 / DMP_SAMPLE_RATE); |
oprospero | 0:4dd021344a6b | 885 | tmp[0] = (unsigned char)(dmp_time >> 8); |
oprospero | 0:4dd021344a6b | 886 | tmp[1] = (unsigned char)(dmp_time & 0xFF); |
oprospero | 0:4dd021344a6b | 887 | return mpu_write_mem(D_1_218, 2, tmp); |
oprospero | 0:4dd021344a6b | 888 | } |
oprospero | 0:4dd021344a6b | 889 | |
oprospero | 0:4dd021344a6b | 890 | /** |
oprospero | 0:4dd021344a6b | 891 | * @brief Set shake rejection threshold. |
oprospero | 0:4dd021344a6b | 892 | * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. |
oprospero | 0:4dd021344a6b | 893 | * @param[in] sf Gyro scale factor. |
oprospero | 0:4dd021344a6b | 894 | * @param[in] thresh Gyro threshold in dps. |
oprospero | 0:4dd021344a6b | 895 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 896 | */ |
oprospero | 0:4dd021344a6b | 897 | int DMP_Motion_Driver::dmp_set_shake_reject_thresh(long sf, unsigned short thresh) |
oprospero | 0:4dd021344a6b | 898 | { |
oprospero | 0:4dd021344a6b | 899 | unsigned char tmp[4]; |
oprospero | 0:4dd021344a6b | 900 | long thresh_scaled = sf / 1000 * thresh; |
oprospero | 0:4dd021344a6b | 901 | tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 902 | tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 903 | tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 904 | tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); |
oprospero | 0:4dd021344a6b | 905 | return mpu_write_mem(D_1_92, 4, tmp); |
oprospero | 0:4dd021344a6b | 906 | } |
oprospero | 0:4dd021344a6b | 907 | |
oprospero | 0:4dd021344a6b | 908 | /** |
oprospero | 0:4dd021344a6b | 909 | * @brief Set shake rejection time. |
oprospero | 0:4dd021344a6b | 910 | * Sets the length of time that the gyro must be outside of the threshold set |
oprospero | 0:4dd021344a6b | 911 | * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory |
oprospero | 0:4dd021344a6b | 912 | * 60 ms is added to this parameter. |
oprospero | 0:4dd021344a6b | 913 | * @param[in] time Time in milliseconds. |
oprospero | 0:4dd021344a6b | 914 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 915 | */ |
oprospero | 0:4dd021344a6b | 916 | int DMP_Motion_Driver::dmp_set_shake_reject_time(unsigned short time) |
oprospero | 0:4dd021344a6b | 917 | { |
oprospero | 0:4dd021344a6b | 918 | unsigned char tmp[2]; |
oprospero | 0:4dd021344a6b | 919 | |
oprospero | 0:4dd021344a6b | 920 | time /= (1000 / DMP_SAMPLE_RATE); |
oprospero | 0:4dd021344a6b | 921 | tmp[0] = time >> 8; |
oprospero | 0:4dd021344a6b | 922 | tmp[1] = time & 0xFF; |
oprospero | 0:4dd021344a6b | 923 | return mpu_write_mem(D_1_90,2,tmp); |
oprospero | 0:4dd021344a6b | 924 | } |
oprospero | 0:4dd021344a6b | 925 | |
oprospero | 0:4dd021344a6b | 926 | /** |
oprospero | 0:4dd021344a6b | 927 | * @brief Set shake rejection timeout. |
oprospero | 0:4dd021344a6b | 928 | * Sets the length of time after a shake rejection that the gyro must stay |
oprospero | 0:4dd021344a6b | 929 | * inside of the threshold before taps can be detected again. A mandatory |
oprospero | 0:4dd021344a6b | 930 | * 60 ms is added to this parameter. |
oprospero | 0:4dd021344a6b | 931 | * @param[in] time Time in milliseconds. |
oprospero | 0:4dd021344a6b | 932 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 933 | */ |
oprospero | 0:4dd021344a6b | 934 | int DMP_Motion_Driver::dmp_set_shake_reject_timeout(unsigned short time) |
oprospero | 0:4dd021344a6b | 935 | { |
oprospero | 0:4dd021344a6b | 936 | unsigned char tmp[2]; |
oprospero | 0:4dd021344a6b | 937 | |
oprospero | 0:4dd021344a6b | 938 | time /= (1000 / DMP_SAMPLE_RATE); |
oprospero | 0:4dd021344a6b | 939 | tmp[0] = time >> 8; |
oprospero | 0:4dd021344a6b | 940 | tmp[1] = time & 0xFF; |
oprospero | 0:4dd021344a6b | 941 | return mpu_write_mem(D_1_88,2,tmp); |
oprospero | 0:4dd021344a6b | 942 | } |
oprospero | 0:4dd021344a6b | 943 | |
oprospero | 0:4dd021344a6b | 944 | /** |
oprospero | 0:4dd021344a6b | 945 | * @brief Get current step count. |
oprospero | 0:4dd021344a6b | 946 | * @param[out] count Number of steps detected. |
oprospero | 0:4dd021344a6b | 947 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 948 | */ |
oprospero | 0:4dd021344a6b | 949 | int DMP_Motion_Driver::dmp_get_pedometer_step_count(unsigned long *count) |
oprospero | 0:4dd021344a6b | 950 | { |
oprospero | 0:4dd021344a6b | 951 | unsigned char tmp[4]; |
oprospero | 0:4dd021344a6b | 952 | if (!count) |
oprospero | 0:4dd021344a6b | 953 | return -1; |
oprospero | 0:4dd021344a6b | 954 | |
oprospero | 0:4dd021344a6b | 955 | if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) |
oprospero | 0:4dd021344a6b | 956 | return -1; |
oprospero | 0:4dd021344a6b | 957 | |
oprospero | 0:4dd021344a6b | 958 | count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | |
oprospero | 0:4dd021344a6b | 959 | ((unsigned long)tmp[2] << 8) | tmp[3]; |
oprospero | 0:4dd021344a6b | 960 | return 0; |
oprospero | 0:4dd021344a6b | 961 | } |
oprospero | 0:4dd021344a6b | 962 | |
oprospero | 0:4dd021344a6b | 963 | /** |
oprospero | 0:4dd021344a6b | 964 | * @brief Overwrite current step count. |
oprospero | 0:4dd021344a6b | 965 | * WARNING: This function writes to DMP memory and could potentially encounter |
oprospero | 0:4dd021344a6b | 966 | * a race condition if called while the pedometer is enabled. |
oprospero | 0:4dd021344a6b | 967 | * @param[in] count New step count. |
oprospero | 0:4dd021344a6b | 968 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 969 | */ |
oprospero | 0:4dd021344a6b | 970 | int DMP_Motion_Driver::dmp_set_pedometer_step_count(unsigned long count) |
oprospero | 0:4dd021344a6b | 971 | { |
oprospero | 0:4dd021344a6b | 972 | unsigned char tmp[4]; |
oprospero | 0:4dd021344a6b | 973 | |
oprospero | 0:4dd021344a6b | 974 | tmp[0] = (unsigned char)((count >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 975 | tmp[1] = (unsigned char)((count >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 976 | tmp[2] = (unsigned char)((count >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 977 | tmp[3] = (unsigned char)(count & 0xFF); |
oprospero | 0:4dd021344a6b | 978 | return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); |
oprospero | 0:4dd021344a6b | 979 | } |
oprospero | 0:4dd021344a6b | 980 | |
oprospero | 0:4dd021344a6b | 981 | /** |
oprospero | 0:4dd021344a6b | 982 | * @brief Get duration of walking time. |
oprospero | 0:4dd021344a6b | 983 | * @param[in] time Walk time in milliseconds. |
oprospero | 0:4dd021344a6b | 984 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 985 | */ |
oprospero | 0:4dd021344a6b | 986 | int DMP_Motion_Driver::dmp_get_pedometer_walk_time(unsigned long *time) |
oprospero | 0:4dd021344a6b | 987 | { |
oprospero | 0:4dd021344a6b | 988 | unsigned char tmp[4]; |
oprospero | 0:4dd021344a6b | 989 | if (!time) |
oprospero | 0:4dd021344a6b | 990 | return -1; |
oprospero | 0:4dd021344a6b | 991 | |
oprospero | 0:4dd021344a6b | 992 | if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) |
oprospero | 0:4dd021344a6b | 993 | return -1; |
oprospero | 0:4dd021344a6b | 994 | |
oprospero | 0:4dd021344a6b | 995 | time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | |
oprospero | 0:4dd021344a6b | 996 | ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; |
oprospero | 0:4dd021344a6b | 997 | return 0; |
oprospero | 0:4dd021344a6b | 998 | } |
oprospero | 0:4dd021344a6b | 999 | |
oprospero | 0:4dd021344a6b | 1000 | /** |
oprospero | 0:4dd021344a6b | 1001 | * @brief Overwrite current walk time. |
oprospero | 0:4dd021344a6b | 1002 | * WARNING: This function writes to DMP memory and could potentially encounter |
oprospero | 0:4dd021344a6b | 1003 | * a race condition if called while the pedometer is enabled. |
oprospero | 0:4dd021344a6b | 1004 | * @param[in] time New walk time in milliseconds. |
oprospero | 0:4dd021344a6b | 1005 | */ |
oprospero | 0:4dd021344a6b | 1006 | int DMP_Motion_Driver::dmp_set_pedometer_walk_time(unsigned long time) |
oprospero | 0:4dd021344a6b | 1007 | { |
oprospero | 0:4dd021344a6b | 1008 | unsigned char tmp[4]; |
oprospero | 0:4dd021344a6b | 1009 | |
oprospero | 0:4dd021344a6b | 1010 | time /= 20; |
oprospero | 0:4dd021344a6b | 1011 | |
oprospero | 0:4dd021344a6b | 1012 | tmp[0] = (unsigned char)((time >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 1013 | tmp[1] = (unsigned char)((time >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 1014 | tmp[2] = (unsigned char)((time >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 1015 | tmp[3] = (unsigned char)(time & 0xFF); |
oprospero | 0:4dd021344a6b | 1016 | return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); |
oprospero | 0:4dd021344a6b | 1017 | } |
oprospero | 0:4dd021344a6b | 1018 | |
oprospero | 0:4dd021344a6b | 1019 | /** |
oprospero | 0:4dd021344a6b | 1020 | * @brief Enable DMP features. |
oprospero | 0:4dd021344a6b | 1021 | * The following \#define's are used in the input mask: |
oprospero | 0:4dd021344a6b | 1022 | * \n DMP_FEATURE_TAP |
oprospero | 0:4dd021344a6b | 1023 | * \n DMP_FEATURE_ANDROID_ORIENT |
oprospero | 0:4dd021344a6b | 1024 | * \n DMP_FEATURE_LP_QUAT |
oprospero | 0:4dd021344a6b | 1025 | * \n DMP_FEATURE_6X_LP_QUAT |
oprospero | 0:4dd021344a6b | 1026 | * \n DMP_FEATURE_GYRO_CAL |
oprospero | 0:4dd021344a6b | 1027 | * \n DMP_FEATURE_SEND_RAW_ACCEL |
oprospero | 0:4dd021344a6b | 1028 | * \n DMP_FEATURE_SEND_RAW_GYRO |
oprospero | 0:4dd021344a6b | 1029 | * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually |
oprospero | 0:4dd021344a6b | 1030 | * exclusive. |
oprospero | 0:4dd021344a6b | 1031 | * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also |
oprospero | 0:4dd021344a6b | 1032 | * mutually exclusive. |
oprospero | 0:4dd021344a6b | 1033 | * @param[in] mask Mask of features to enable. |
oprospero | 0:4dd021344a6b | 1034 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1035 | */ |
oprospero | 0:4dd021344a6b | 1036 | int DMP_Motion_Driver::dmp_enable_feature(unsigned short mask) |
oprospero | 0:4dd021344a6b | 1037 | { |
oprospero | 0:4dd021344a6b | 1038 | unsigned char tmp[10]; |
oprospero | 0:4dd021344a6b | 1039 | |
oprospero | 0:4dd021344a6b | 1040 | /* TODO: All of these settings can probably be integrated into the default |
oprospero | 0:4dd021344a6b | 1041 | * DMP image. |
oprospero | 0:4dd021344a6b | 1042 | */ |
oprospero | 0:4dd021344a6b | 1043 | /* Set integration scale factor. */ |
oprospero | 0:4dd021344a6b | 1044 | tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); |
oprospero | 0:4dd021344a6b | 1045 | tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); |
oprospero | 0:4dd021344a6b | 1046 | tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); |
oprospero | 0:4dd021344a6b | 1047 | tmp[3] = (unsigned char)(GYRO_SF & 0xFF); |
oprospero | 0:4dd021344a6b | 1048 | mpu_write_mem(D_0_104, 4, tmp); |
oprospero | 0:4dd021344a6b | 1049 | |
oprospero | 0:4dd021344a6b | 1050 | /* Send sensor data to the FIFO. */ |
oprospero | 0:4dd021344a6b | 1051 | tmp[0] = 0xA3; |
oprospero | 0:4dd021344a6b | 1052 | if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { |
oprospero | 0:4dd021344a6b | 1053 | tmp[1] = 0xC0; |
oprospero | 0:4dd021344a6b | 1054 | tmp[2] = 0xC8; |
oprospero | 0:4dd021344a6b | 1055 | tmp[3] = 0xC2; |
oprospero | 0:4dd021344a6b | 1056 | } else { |
oprospero | 0:4dd021344a6b | 1057 | tmp[1] = 0xA3; |
oprospero | 0:4dd021344a6b | 1058 | tmp[2] = 0xA3; |
oprospero | 0:4dd021344a6b | 1059 | tmp[3] = 0xA3; |
oprospero | 0:4dd021344a6b | 1060 | } |
oprospero | 0:4dd021344a6b | 1061 | if (mask & DMP_FEATURE_SEND_ANY_GYRO) { |
oprospero | 0:4dd021344a6b | 1062 | tmp[4] = 0xC4; |
oprospero | 0:4dd021344a6b | 1063 | tmp[5] = 0xCC; |
oprospero | 0:4dd021344a6b | 1064 | tmp[6] = 0xC6; |
oprospero | 0:4dd021344a6b | 1065 | } else { |
oprospero | 0:4dd021344a6b | 1066 | tmp[4] = 0xA3; |
oprospero | 0:4dd021344a6b | 1067 | tmp[5] = 0xA3; |
oprospero | 0:4dd021344a6b | 1068 | tmp[6] = 0xA3; |
oprospero | 0:4dd021344a6b | 1069 | } |
oprospero | 0:4dd021344a6b | 1070 | tmp[7] = 0xA3; |
oprospero | 0:4dd021344a6b | 1071 | tmp[8] = 0xA3; |
oprospero | 0:4dd021344a6b | 1072 | tmp[9] = 0xA3; |
oprospero | 0:4dd021344a6b | 1073 | mpu_write_mem(CFG_15,10,tmp); |
oprospero | 0:4dd021344a6b | 1074 | |
oprospero | 0:4dd021344a6b | 1075 | /* Send gesture data to the FIFO. */ |
oprospero | 0:4dd021344a6b | 1076 | if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) |
oprospero | 0:4dd021344a6b | 1077 | tmp[0] = DINA20; |
oprospero | 0:4dd021344a6b | 1078 | else |
oprospero | 0:4dd021344a6b | 1079 | tmp[0] = 0xD8; |
oprospero | 0:4dd021344a6b | 1080 | mpu_write_mem(CFG_27,1,tmp); |
oprospero | 0:4dd021344a6b | 1081 | |
oprospero | 0:4dd021344a6b | 1082 | if (mask & DMP_FEATURE_GYRO_CAL) |
oprospero | 0:4dd021344a6b | 1083 | dmp_enable_gyro_cal(1); |
oprospero | 0:4dd021344a6b | 1084 | else |
oprospero | 0:4dd021344a6b | 1085 | dmp_enable_gyro_cal(0); |
oprospero | 0:4dd021344a6b | 1086 | |
oprospero | 0:4dd021344a6b | 1087 | if (mask & DMP_FEATURE_SEND_ANY_GYRO) { |
oprospero | 0:4dd021344a6b | 1088 | if (mask & DMP_FEATURE_SEND_CAL_GYRO) { |
oprospero | 0:4dd021344a6b | 1089 | tmp[0] = 0xB2; |
oprospero | 0:4dd021344a6b | 1090 | tmp[1] = 0x8B; |
oprospero | 0:4dd021344a6b | 1091 | tmp[2] = 0xB6; |
oprospero | 0:4dd021344a6b | 1092 | tmp[3] = 0x9B; |
oprospero | 0:4dd021344a6b | 1093 | } else { |
oprospero | 0:4dd021344a6b | 1094 | tmp[0] = DINAC0; |
oprospero | 0:4dd021344a6b | 1095 | tmp[1] = DINA80; |
oprospero | 0:4dd021344a6b | 1096 | tmp[2] = DINAC2; |
oprospero | 0:4dd021344a6b | 1097 | tmp[3] = DINA90; |
oprospero | 0:4dd021344a6b | 1098 | } |
oprospero | 0:4dd021344a6b | 1099 | mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); |
oprospero | 0:4dd021344a6b | 1100 | } |
oprospero | 0:4dd021344a6b | 1101 | |
oprospero | 0:4dd021344a6b | 1102 | if (mask & DMP_FEATURE_TAP) { |
oprospero | 0:4dd021344a6b | 1103 | /* Enable tap. */ |
oprospero | 0:4dd021344a6b | 1104 | tmp[0] = 0xF8; |
oprospero | 0:4dd021344a6b | 1105 | mpu_write_mem(CFG_20, 1, tmp); |
oprospero | 0:4dd021344a6b | 1106 | dmp_set_tap_thresh(TAP_XYZ, 250); |
oprospero | 0:4dd021344a6b | 1107 | dmp_set_tap_axes(TAP_XYZ); |
oprospero | 0:4dd021344a6b | 1108 | dmp_set_tap_count(1); |
oprospero | 0:4dd021344a6b | 1109 | dmp_set_tap_time(100); |
oprospero | 0:4dd021344a6b | 1110 | dmp_set_tap_time_multi(500); |
oprospero | 0:4dd021344a6b | 1111 | |
oprospero | 0:4dd021344a6b | 1112 | dmp_set_shake_reject_thresh(GYRO_SF, 200); |
oprospero | 0:4dd021344a6b | 1113 | dmp_set_shake_reject_time(40); |
oprospero | 0:4dd021344a6b | 1114 | dmp_set_shake_reject_timeout(10); |
oprospero | 0:4dd021344a6b | 1115 | } else { |
oprospero | 0:4dd021344a6b | 1116 | tmp[0] = 0xD8; |
oprospero | 0:4dd021344a6b | 1117 | mpu_write_mem(CFG_20, 1, tmp); |
oprospero | 0:4dd021344a6b | 1118 | } |
oprospero | 0:4dd021344a6b | 1119 | |
oprospero | 0:4dd021344a6b | 1120 | if (mask & DMP_FEATURE_ANDROID_ORIENT) { |
oprospero | 0:4dd021344a6b | 1121 | tmp[0] = 0xD9; |
oprospero | 0:4dd021344a6b | 1122 | } else |
oprospero | 0:4dd021344a6b | 1123 | tmp[0] = 0xD8; |
oprospero | 0:4dd021344a6b | 1124 | mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); |
oprospero | 0:4dd021344a6b | 1125 | |
oprospero | 0:4dd021344a6b | 1126 | if (mask & DMP_FEATURE_LP_QUAT) |
oprospero | 0:4dd021344a6b | 1127 | dmp_enable_lp_quat(1); |
oprospero | 0:4dd021344a6b | 1128 | else |
oprospero | 0:4dd021344a6b | 1129 | dmp_enable_lp_quat(0); |
oprospero | 0:4dd021344a6b | 1130 | |
oprospero | 0:4dd021344a6b | 1131 | if (mask & DMP_FEATURE_6X_LP_QUAT) |
oprospero | 0:4dd021344a6b | 1132 | dmp_enable_6x_lp_quat(1); |
oprospero | 0:4dd021344a6b | 1133 | else |
oprospero | 0:4dd021344a6b | 1134 | dmp_enable_6x_lp_quat(0); |
oprospero | 0:4dd021344a6b | 1135 | |
oprospero | 0:4dd021344a6b | 1136 | /* Pedometer is always enabled. */ |
oprospero | 0:4dd021344a6b | 1137 | dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; |
oprospero | 0:4dd021344a6b | 1138 | mpu_reset_fifo(); |
oprospero | 0:4dd021344a6b | 1139 | |
oprospero | 0:4dd021344a6b | 1140 | dmp.packet_length = 0; |
oprospero | 0:4dd021344a6b | 1141 | if (mask & DMP_FEATURE_SEND_RAW_ACCEL) |
oprospero | 0:4dd021344a6b | 1142 | dmp.packet_length += 6; |
oprospero | 0:4dd021344a6b | 1143 | if (mask & DMP_FEATURE_SEND_ANY_GYRO) |
oprospero | 0:4dd021344a6b | 1144 | dmp.packet_length += 6; |
oprospero | 0:4dd021344a6b | 1145 | if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) |
oprospero | 0:4dd021344a6b | 1146 | dmp.packet_length += 16; |
oprospero | 0:4dd021344a6b | 1147 | if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) |
oprospero | 0:4dd021344a6b | 1148 | dmp.packet_length += 4; |
oprospero | 0:4dd021344a6b | 1149 | |
oprospero | 0:4dd021344a6b | 1150 | return 0; |
oprospero | 0:4dd021344a6b | 1151 | } |
oprospero | 0:4dd021344a6b | 1152 | |
oprospero | 0:4dd021344a6b | 1153 | int DMP_Motion_Driver::dmp_get_packet_length() |
oprospero | 0:4dd021344a6b | 1154 | { |
oprospero | 0:4dd021344a6b | 1155 | return dmp.packet_length; |
oprospero | 0:4dd021344a6b | 1156 | } |
oprospero | 0:4dd021344a6b | 1157 | |
oprospero | 0:4dd021344a6b | 1158 | /** |
oprospero | 0:4dd021344a6b | 1159 | * @brief Get list of currently enabled DMP features. |
oprospero | 0:4dd021344a6b | 1160 | * @param[out] Mask of enabled features. |
oprospero | 0:4dd021344a6b | 1161 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1162 | */ |
oprospero | 0:4dd021344a6b | 1163 | int DMP_Motion_Driver::dmp_get_enabled_features(unsigned short *mask) |
oprospero | 0:4dd021344a6b | 1164 | { |
oprospero | 0:4dd021344a6b | 1165 | mask[0] = dmp.feature_mask; |
oprospero | 0:4dd021344a6b | 1166 | return 0; |
oprospero | 0:4dd021344a6b | 1167 | } |
oprospero | 0:4dd021344a6b | 1168 | |
oprospero | 0:4dd021344a6b | 1169 | /** |
oprospero | 0:4dd021344a6b | 1170 | * @brief Calibrate the gyro data in the DMP. |
oprospero | 0:4dd021344a6b | 1171 | * After eight seconds of no motion, the DMP will compute gyro biases and |
oprospero | 0:4dd021344a6b | 1172 | * subtract them from the quaternion output. If @e dmp_enable_feature is |
oprospero | 0:4dd021344a6b | 1173 | * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be |
oprospero | 0:4dd021344a6b | 1174 | * subtracted from the gyro output. |
oprospero | 0:4dd021344a6b | 1175 | * @param[in] enable 1 to enable gyro calibration. |
oprospero | 0:4dd021344a6b | 1176 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1177 | */ |
oprospero | 0:4dd021344a6b | 1178 | int DMP_Motion_Driver::dmp_enable_gyro_cal(unsigned char enable) |
oprospero | 0:4dd021344a6b | 1179 | { |
oprospero | 0:4dd021344a6b | 1180 | if (enable) { |
oprospero | 0:4dd021344a6b | 1181 | unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; |
oprospero | 0:4dd021344a6b | 1182 | return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); |
oprospero | 0:4dd021344a6b | 1183 | } else { |
oprospero | 0:4dd021344a6b | 1184 | unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; |
oprospero | 0:4dd021344a6b | 1185 | return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); |
oprospero | 0:4dd021344a6b | 1186 | } |
oprospero | 0:4dd021344a6b | 1187 | } |
oprospero | 0:4dd021344a6b | 1188 | |
oprospero | 0:4dd021344a6b | 1189 | /** |
oprospero | 0:4dd021344a6b | 1190 | * @brief Generate 3-axis quaternions from the DMP. |
oprospero | 0:4dd021344a6b | 1191 | * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually |
oprospero | 0:4dd021344a6b | 1192 | * exclusive. |
oprospero | 0:4dd021344a6b | 1193 | * @param[in] enable 1 to enable 3-axis quaternion. |
oprospero | 0:4dd021344a6b | 1194 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1195 | */ |
oprospero | 0:4dd021344a6b | 1196 | int DMP_Motion_Driver::dmp_enable_lp_quat(unsigned char enable) |
oprospero | 0:4dd021344a6b | 1197 | { |
oprospero | 0:4dd021344a6b | 1198 | unsigned char regs[4]; |
oprospero | 0:4dd021344a6b | 1199 | if (enable) { |
oprospero | 0:4dd021344a6b | 1200 | regs[0] = DINBC0; |
oprospero | 0:4dd021344a6b | 1201 | regs[1] = DINBC2; |
oprospero | 0:4dd021344a6b | 1202 | regs[2] = DINBC4; |
oprospero | 0:4dd021344a6b | 1203 | regs[3] = DINBC6; |
oprospero | 0:4dd021344a6b | 1204 | } |
oprospero | 0:4dd021344a6b | 1205 | else |
oprospero | 0:4dd021344a6b | 1206 | memset(regs, 0x8B, 4); |
oprospero | 0:4dd021344a6b | 1207 | |
oprospero | 0:4dd021344a6b | 1208 | mpu_write_mem(CFG_LP_QUAT, 4, regs); |
oprospero | 0:4dd021344a6b | 1209 | |
oprospero | 0:4dd021344a6b | 1210 | return mpu_reset_fifo(); |
oprospero | 0:4dd021344a6b | 1211 | } |
oprospero | 0:4dd021344a6b | 1212 | |
oprospero | 0:4dd021344a6b | 1213 | /** |
oprospero | 0:4dd021344a6b | 1214 | * @brief Generate 6-axis quaternions from the DMP. |
oprospero | 0:4dd021344a6b | 1215 | * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually |
oprospero | 0:4dd021344a6b | 1216 | * exclusive. |
oprospero | 0:4dd021344a6b | 1217 | * @param[in] enable 1 to enable 6-axis quaternion. |
oprospero | 0:4dd021344a6b | 1218 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1219 | */ |
oprospero | 0:4dd021344a6b | 1220 | int DMP_Motion_Driver::dmp_enable_6x_lp_quat(unsigned char enable) |
oprospero | 0:4dd021344a6b | 1221 | { |
oprospero | 0:4dd021344a6b | 1222 | unsigned char regs[4]; |
oprospero | 0:4dd021344a6b | 1223 | if (enable) { |
oprospero | 0:4dd021344a6b | 1224 | regs[0] = DINA20; |
oprospero | 0:4dd021344a6b | 1225 | regs[1] = DINA28; |
oprospero | 0:4dd021344a6b | 1226 | regs[2] = DINA30; |
oprospero | 0:4dd021344a6b | 1227 | regs[3] = DINA38; |
oprospero | 0:4dd021344a6b | 1228 | } else |
oprospero | 0:4dd021344a6b | 1229 | memset(regs, 0xA3, 4); |
oprospero | 0:4dd021344a6b | 1230 | |
oprospero | 0:4dd021344a6b | 1231 | mpu_write_mem(CFG_8, 4, regs); |
oprospero | 0:4dd021344a6b | 1232 | |
oprospero | 0:4dd021344a6b | 1233 | return mpu_reset_fifo(); |
oprospero | 0:4dd021344a6b | 1234 | } |
oprospero | 0:4dd021344a6b | 1235 | |
oprospero | 0:4dd021344a6b | 1236 | /** |
oprospero | 0:4dd021344a6b | 1237 | * @brief Decode the four-byte gesture data and execute any callbacks. |
oprospero | 0:4dd021344a6b | 1238 | * @param[in] gesture Gesture data from DMP packet. |
oprospero | 0:4dd021344a6b | 1239 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1240 | */ |
oprospero | 0:4dd021344a6b | 1241 | //static int decode_gesture(unsigned char *gesture) |
oprospero | 0:4dd021344a6b | 1242 | int DMP_Motion_Driver::decode_gesture(unsigned char *gesture) |
oprospero | 0:4dd021344a6b | 1243 | { |
oprospero | 0:4dd021344a6b | 1244 | unsigned char tap, android_orient; |
oprospero | 0:4dd021344a6b | 1245 | |
oprospero | 0:4dd021344a6b | 1246 | android_orient = gesture[3] & 0xC0; |
oprospero | 0:4dd021344a6b | 1247 | tap = 0x3F & gesture[3]; |
oprospero | 0:4dd021344a6b | 1248 | |
oprospero | 0:4dd021344a6b | 1249 | if (gesture[1] & INT_SRC_TAP) { |
oprospero | 0:4dd021344a6b | 1250 | unsigned char direction, count; |
oprospero | 0:4dd021344a6b | 1251 | direction = tap >> 3; |
oprospero | 0:4dd021344a6b | 1252 | count = (tap % 8) + 1; |
oprospero | 0:4dd021344a6b | 1253 | if (dmp.tap_cb) |
oprospero | 0:4dd021344a6b | 1254 | dmp.tap_cb(direction, count); |
oprospero | 0:4dd021344a6b | 1255 | } |
oprospero | 0:4dd021344a6b | 1256 | |
oprospero | 0:4dd021344a6b | 1257 | if (gesture[1] & INT_SRC_ANDROID_ORIENT) { |
oprospero | 0:4dd021344a6b | 1258 | if (dmp.android_orient_cb) |
oprospero | 0:4dd021344a6b | 1259 | dmp.android_orient_cb(android_orient >> 6); |
oprospero | 0:4dd021344a6b | 1260 | } |
oprospero | 0:4dd021344a6b | 1261 | |
oprospero | 0:4dd021344a6b | 1262 | return 0; |
oprospero | 0:4dd021344a6b | 1263 | } |
oprospero | 0:4dd021344a6b | 1264 | |
oprospero | 0:4dd021344a6b | 1265 | /** |
oprospero | 0:4dd021344a6b | 1266 | * @brief Specify when a DMP interrupt should occur. |
oprospero | 0:4dd021344a6b | 1267 | * A DMP interrupt can be configured to trigger on either of the two |
oprospero | 0:4dd021344a6b | 1268 | * conditions below: |
oprospero | 0:4dd021344a6b | 1269 | * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). |
oprospero | 0:4dd021344a6b | 1270 | * \n b. A tap event has been detected. |
oprospero | 0:4dd021344a6b | 1271 | * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. |
oprospero | 0:4dd021344a6b | 1272 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1273 | */ |
oprospero | 0:4dd021344a6b | 1274 | int DMP_Motion_Driver::dmp_set_interrupt_mode(unsigned char mode) |
oprospero | 0:4dd021344a6b | 1275 | { |
oprospero | 0:4dd021344a6b | 1276 | const unsigned char regs_continuous[11] = |
oprospero | 0:4dd021344a6b | 1277 | {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; |
oprospero | 0:4dd021344a6b | 1278 | const unsigned char regs_gesture[11] = |
oprospero | 0:4dd021344a6b | 1279 | {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; |
oprospero | 0:4dd021344a6b | 1280 | |
oprospero | 0:4dd021344a6b | 1281 | switch (mode) { |
oprospero | 0:4dd021344a6b | 1282 | case DMP_INT_CONTINUOUS: |
oprospero | 0:4dd021344a6b | 1283 | return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, |
oprospero | 0:4dd021344a6b | 1284 | (unsigned char*)regs_continuous); |
oprospero | 0:4dd021344a6b | 1285 | case DMP_INT_GESTURE: |
oprospero | 0:4dd021344a6b | 1286 | return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, |
oprospero | 0:4dd021344a6b | 1287 | (unsigned char*)regs_gesture); |
oprospero | 0:4dd021344a6b | 1288 | default: |
oprospero | 0:4dd021344a6b | 1289 | return -1; |
oprospero | 0:4dd021344a6b | 1290 | } |
oprospero | 0:4dd021344a6b | 1291 | } |
oprospero | 0:4dd021344a6b | 1292 | |
oprospero | 0:4dd021344a6b | 1293 | /** |
oprospero | 0:4dd021344a6b | 1294 | * @brief Get one packet from the FIFO. |
oprospero | 0:4dd021344a6b | 1295 | * If @e sensors does not contain a particular sensor, disregard the data |
oprospero | 0:4dd021344a6b | 1296 | * returned to that pointer. |
oprospero | 0:4dd021344a6b | 1297 | * \n @e sensors can contain a combination of the following flags: |
oprospero | 0:4dd021344a6b | 1298 | * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO |
oprospero | 0:4dd021344a6b | 1299 | * \n INV_XYZ_GYRO |
oprospero | 0:4dd021344a6b | 1300 | * \n INV_XYZ_ACCEL |
oprospero | 0:4dd021344a6b | 1301 | * \n INV_WXYZ_QUAT |
oprospero | 0:4dd021344a6b | 1302 | * \n If the FIFO has no new data, @e sensors will be zero. |
oprospero | 0:4dd021344a6b | 1303 | * \n If the FIFO is disabled, @e sensors will be zero and this function will |
oprospero | 0:4dd021344a6b | 1304 | * return a non-zero error code. |
oprospero | 0:4dd021344a6b | 1305 | * @param[out] gyro Gyro data in hardware units. |
oprospero | 0:4dd021344a6b | 1306 | * @param[out] accel Accel data in hardware units. |
oprospero | 0:4dd021344a6b | 1307 | * @param[out] quat 3-axis quaternion data in hardware units. |
oprospero | 0:4dd021344a6b | 1308 | * @param[out] timestamp Timestamp in milliseconds. |
oprospero | 0:4dd021344a6b | 1309 | * @param[out] sensors Mask of sensors read from FIFO. |
oprospero | 0:4dd021344a6b | 1310 | * @param[out] more Number of remaining packets. |
oprospero | 0:4dd021344a6b | 1311 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1312 | */ |
oprospero | 0:4dd021344a6b | 1313 | int DMP_Motion_Driver::dmp_read_fifo(short *gyro, short *accel, long *quat, |
oprospero | 0:4dd021344a6b | 1314 | unsigned long *timestamp, short *sensors, unsigned char *more) |
oprospero | 0:4dd021344a6b | 1315 | { |
oprospero | 0:4dd021344a6b | 1316 | unsigned char fifo_data[MAX_PACKET_LENGTH]; |
oprospero | 0:4dd021344a6b | 1317 | unsigned char ii = 0; |
oprospero | 0:4dd021344a6b | 1318 | |
oprospero | 0:4dd021344a6b | 1319 | /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can |
oprospero | 0:4dd021344a6b | 1320 | * cache this value and save some cycles. |
oprospero | 0:4dd021344a6b | 1321 | */ |
oprospero | 0:4dd021344a6b | 1322 | sensors[0] = 0; |
oprospero | 0:4dd021344a6b | 1323 | |
oprospero | 0:4dd021344a6b | 1324 | /* Get a packet. */ |
oprospero | 0:4dd021344a6b | 1325 | int result = mpu_read_fifo_stream(dmp.packet_length, fifo_data, more); |
oprospero | 0:4dd021344a6b | 1326 | if (result != 0) |
oprospero | 0:4dd021344a6b | 1327 | return result; |
oprospero | 0:4dd021344a6b | 1328 | |
oprospero | 0:4dd021344a6b | 1329 | /* Parse DMP packet. */ |
oprospero | 0:4dd021344a6b | 1330 | if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { |
oprospero | 0:4dd021344a6b | 1331 | #ifdef FIFO_CORRUPTION_CHECK |
oprospero | 0:4dd021344a6b | 1332 | long quat_q14[4], quat_mag_sq; |
oprospero | 0:4dd021344a6b | 1333 | #endif |
oprospero | 0:4dd021344a6b | 1334 | quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | |
oprospero | 0:4dd021344a6b | 1335 | ((long)fifo_data[2] << 8) | fifo_data[3]; |
oprospero | 0:4dd021344a6b | 1336 | quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | |
oprospero | 0:4dd021344a6b | 1337 | ((long)fifo_data[6] << 8) | fifo_data[7]; |
oprospero | 0:4dd021344a6b | 1338 | quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | |
oprospero | 0:4dd021344a6b | 1339 | ((long)fifo_data[10] << 8) | fifo_data[11]; |
oprospero | 0:4dd021344a6b | 1340 | quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | |
oprospero | 0:4dd021344a6b | 1341 | ((long)fifo_data[14] << 8) | fifo_data[15]; |
oprospero | 0:4dd021344a6b | 1342 | ii += 16; |
oprospero | 0:4dd021344a6b | 1343 | #ifdef FIFO_CORRUPTION_CHECK |
oprospero | 0:4dd021344a6b | 1344 | /* We can detect a corrupted FIFO by monitoring the quaternion data and |
oprospero | 0:4dd021344a6b | 1345 | * ensuring that the magnitude is always normalized to one. This |
oprospero | 0:4dd021344a6b | 1346 | * shouldn't happen in normal operation, but if an I2C error occurs, |
oprospero | 0:4dd021344a6b | 1347 | * the FIFO reads might become misaligned. |
oprospero | 0:4dd021344a6b | 1348 | * |
oprospero | 0:4dd021344a6b | 1349 | * Let's start by scaling down the quaternion data to avoid long long |
oprospero | 0:4dd021344a6b | 1350 | * math. |
oprospero | 0:4dd021344a6b | 1351 | */ |
oprospero | 0:4dd021344a6b | 1352 | quat_q14[0] = quat[0] >> 16; |
oprospero | 0:4dd021344a6b | 1353 | quat_q14[1] = quat[1] >> 16; |
oprospero | 0:4dd021344a6b | 1354 | quat_q14[2] = quat[2] >> 16; |
oprospero | 0:4dd021344a6b | 1355 | quat_q14[3] = quat[3] >> 16; |
oprospero | 0:4dd021344a6b | 1356 | quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + |
oprospero | 0:4dd021344a6b | 1357 | quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; |
oprospero | 0:4dd021344a6b | 1358 | if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || |
oprospero | 0:4dd021344a6b | 1359 | (quat_mag_sq > QUAT_MAG_SQ_MAX)) { |
oprospero | 0:4dd021344a6b | 1360 | /* Quaternion is outside of the acceptable threshold. */ |
oprospero | 0:4dd021344a6b | 1361 | mpu_reset_fifo(); |
oprospero | 0:4dd021344a6b | 1362 | sensors[0] = 0; |
oprospero | 0:4dd021344a6b | 1363 | return -4; |
oprospero | 0:4dd021344a6b | 1364 | } |
oprospero | 0:4dd021344a6b | 1365 | sensors[0] |= INV_WXYZ_QUAT; |
oprospero | 0:4dd021344a6b | 1366 | #endif |
oprospero | 0:4dd021344a6b | 1367 | } |
oprospero | 0:4dd021344a6b | 1368 | |
oprospero | 0:4dd021344a6b | 1369 | if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { |
oprospero | 0:4dd021344a6b | 1370 | accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; |
oprospero | 0:4dd021344a6b | 1371 | accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; |
oprospero | 0:4dd021344a6b | 1372 | accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; |
oprospero | 0:4dd021344a6b | 1373 | ii += 6; |
oprospero | 0:4dd021344a6b | 1374 | sensors[0] |= INV_XYZ_ACCEL; |
oprospero | 0:4dd021344a6b | 1375 | } |
oprospero | 0:4dd021344a6b | 1376 | |
oprospero | 0:4dd021344a6b | 1377 | if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { |
oprospero | 0:4dd021344a6b | 1378 | gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; |
oprospero | 0:4dd021344a6b | 1379 | gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; |
oprospero | 0:4dd021344a6b | 1380 | gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; |
oprospero | 0:4dd021344a6b | 1381 | ii += 6; |
oprospero | 0:4dd021344a6b | 1382 | sensors[0] |= INV_XYZ_GYRO; |
oprospero | 0:4dd021344a6b | 1383 | } |
oprospero | 0:4dd021344a6b | 1384 | |
oprospero | 0:4dd021344a6b | 1385 | /* Gesture data is at the end of the DMP packet. Parse it and call |
oprospero | 0:4dd021344a6b | 1386 | * the gesture callbacks (if registered). |
oprospero | 0:4dd021344a6b | 1387 | */ |
oprospero | 0:4dd021344a6b | 1388 | if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) |
oprospero | 0:4dd021344a6b | 1389 | decode_gesture(fifo_data + ii); |
oprospero | 0:4dd021344a6b | 1390 | |
oprospero | 0:4dd021344a6b | 1391 | get_ms(timestamp); |
oprospero | 0:4dd021344a6b | 1392 | return 0; |
oprospero | 0:4dd021344a6b | 1393 | } |
oprospero | 0:4dd021344a6b | 1394 | |
oprospero | 0:4dd021344a6b | 1395 | /** |
oprospero | 0:4dd021344a6b | 1396 | * @brief Register a function to be executed on a tap event. |
oprospero | 0:4dd021344a6b | 1397 | * The tap direction is represented by one of the following: |
oprospero | 0:4dd021344a6b | 1398 | * \n TAP_X_UP |
oprospero | 0:4dd021344a6b | 1399 | * \n TAP_X_DOWN |
oprospero | 0:4dd021344a6b | 1400 | * \n TAP_Y_UP |
oprospero | 0:4dd021344a6b | 1401 | * \n TAP_Y_DOWN |
oprospero | 0:4dd021344a6b | 1402 | * \n TAP_Z_UP |
oprospero | 0:4dd021344a6b | 1403 | * \n TAP_Z_DOWN |
oprospero | 0:4dd021344a6b | 1404 | * @param[in] func Callback function. |
oprospero | 0:4dd021344a6b | 1405 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1406 | */ |
oprospero | 0:4dd021344a6b | 1407 | int DMP_Motion_Driver::dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) |
oprospero | 0:4dd021344a6b | 1408 | { |
oprospero | 0:4dd021344a6b | 1409 | dmp.tap_cb = func; |
oprospero | 0:4dd021344a6b | 1410 | return 0; |
oprospero | 0:4dd021344a6b | 1411 | } |
oprospero | 0:4dd021344a6b | 1412 | |
oprospero | 0:4dd021344a6b | 1413 | /** |
oprospero | 0:4dd021344a6b | 1414 | * @brief Register a function to be executed on a android orientation event. |
oprospero | 0:4dd021344a6b | 1415 | * @param[in] func Callback function. |
oprospero | 0:4dd021344a6b | 1416 | * @return 0 if successful. |
oprospero | 0:4dd021344a6b | 1417 | */ |
oprospero | 0:4dd021344a6b | 1418 | int DMP_Motion_Driver::dmp_register_android_orient_cb(void (*func)(unsigned char)) |
oprospero | 0:4dd021344a6b | 1419 | { |
oprospero | 0:4dd021344a6b | 1420 | dmp.android_orient_cb = func; |
oprospero | 0:4dd021344a6b | 1421 | return 0; |
oprospero | 0:4dd021344a6b | 1422 | } |
oprospero | 0:4dd021344a6b | 1423 | |
oprospero | 0:4dd021344a6b | 1424 | |
oprospero | 0:4dd021344a6b | 1425 | int DMP_Motion_Driver::dmpGetGravity(VectorFloat *v, Quaternion *q) |
oprospero | 0:4dd021344a6b | 1426 | { |
oprospero | 0:4dd021344a6b | 1427 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); |
oprospero | 0:4dd021344a6b | 1428 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); |
oprospero | 0:4dd021344a6b | 1429 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; |
oprospero | 0:4dd021344a6b | 1430 | return 0; |
oprospero | 0:4dd021344a6b | 1431 | } |
oprospero | 0:4dd021344a6b | 1432 | |
oprospero | 0:4dd021344a6b | 1433 | int DMP_Motion_Driver::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) |
oprospero | 0:4dd021344a6b | 1434 | { |
oprospero | 0:4dd021344a6b | 1435 | // yaw: (about Z axis) |
oprospero | 0:4dd021344a6b | 1436 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); |
oprospero | 0:4dd021344a6b | 1437 | // pitch: (nose up/down, about Y axis) |
oprospero | 0:4dd021344a6b | 1438 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); |
oprospero | 0:4dd021344a6b | 1439 | // roll: (tilt left/right, about X axis) |
oprospero | 0:4dd021344a6b | 1440 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); |
oprospero | 0:4dd021344a6b | 1441 | return 0; |
oprospero | 0:4dd021344a6b | 1442 | } |
oprospero | 0:4dd021344a6b | 1443 | |
oprospero | 0:4dd021344a6b | 1444 | /** |
oprospero | 0:4dd021344a6b | 1445 | * @} |
oprospero | 0:4dd021344a6b | 1446 | */ |