Yihui Xiong
initial
Embed:
(wiki syntax)
Show/hide line numbers
inv_mpu_dmp_motion_driver.c
Go to the documentation of this file.
00001 /* 00002 $License: 00003 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 00004 See included License.txt for License information. 00005 $ 00006 */ 00007 /** 00008 * @addtogroup DRIVERS Sensor Driver Layer 00009 * @brief Hardware drivers to communicate with sensors via I2C. 00010 * 00011 * @{ 00012 * @file inv_mpu_dmp_motion_driver.c 00013 * @brief DMP image and interface functions. 00014 * @details All functions are preceded by the dmp_ prefix to 00015 * differentiate among MPL and general driver function calls. 00016 */ 00017 #include <stdio.h> 00018 #include <stdint.h> 00019 #include <stdlib.h> 00020 #include <string.h> 00021 #include <math.h> 00022 #include "inv_mpu.h" 00023 #include "inv_mpu_dmp_motion_driver.h" 00024 #include "dmpKey.h" 00025 #include "dmpmap.h" 00026 00027 /* The following functions must be defined for this platform: 00028 * i2c_write(unsigned char slave_addr, unsigned char reg_addr, 00029 * unsigned char length, unsigned char const *data) 00030 * i2c_read(unsigned char slave_addr, unsigned char reg_addr, 00031 * unsigned char length, unsigned char *data) 00032 * delay_ms(unsigned long num_ms) 00033 * get_ms(unsigned long *count) 00034 */ 00035 #if defined MOTION_DRIVER_TARGET_MSP430 00036 #include "msp430.h" 00037 #include "msp430_clock.h" 00038 #define delay_ms msp430_delay_ms 00039 #define get_ms msp430_get_clock_ms 00040 #define log_i(...) do {} while (0) 00041 #define log_e(...) do {} while (0) 00042 00043 #elif defined EMPL_TARGET_MSP430 00044 #include "msp430.h" 00045 #include "msp430_clock.h" 00046 #include "log.h" 00047 #define delay_ms msp430_delay_ms 00048 #define get_ms msp430_get_clock_ms 00049 #define log_i MPL_LOGI 00050 #define log_e MPL_LOGE 00051 00052 #elif defined EMPL_TARGET_UC3L0 00053 /* Instead of using the standard TWI driver from the ASF library, we're using 00054 * a TWI driver that follows the slave address + register address convention. 00055 */ 00056 #include "delay.h" 00057 #include "sysclk.h" 00058 #include "log.h" 00059 #include "uc3l0_clock.h" 00060 /* delay_ms is a function already defined in ASF. */ 00061 #define get_ms uc3l0_get_clock_ms 00062 #define log_i MPL_LOGI 00063 #define log_e MPL_LOGE 00064 00065 #elif defined __MBED__ 00066 #include "mpu_mbed_config.h" 00067 #else 00068 #error Gyro driver is missing the system layer implementations. 00069 #endif 00070 00071 /* These defines are copied from dmpDefaultMPU6050.c in the general MPL 00072 * releases. These defines may change for each DMP image, so be sure to modify 00073 * these values when switching to a new image. 00074 */ 00075 #define CFG_LP_QUAT (2712) 00076 #define END_ORIENT_TEMP (1866) 00077 #define CFG_27 (2742) 00078 #define CFG_20 (2224) 00079 #define CFG_23 (2745) 00080 #define CFG_FIFO_ON_EVENT (2690) 00081 #define END_PREDICTION_UPDATE (1761) 00082 #define CGNOTICE_INTR (2620) 00083 #define X_GRT_Y_TMP (1358) 00084 #define CFG_DR_INT (1029) 00085 #define CFG_AUTH (1035) 00086 #define UPDATE_PROP_ROT (1835) 00087 #define END_COMPARE_Y_X_TMP2 (1455) 00088 #define SKIP_X_GRT_Y_TMP (1359) 00089 #define SKIP_END_COMPARE (1435) 00090 #define FCFG_3 (1088) 00091 #define FCFG_2 (1066) 00092 #define FCFG_1 (1062) 00093 #define END_COMPARE_Y_X_TMP3 (1434) 00094 #define FCFG_7 (1073) 00095 #define FCFG_6 (1106) 00096 #define FLAT_STATE_END (1713) 00097 #define SWING_END_4 (1616) 00098 #define SWING_END_2 (1565) 00099 #define SWING_END_3 (1587) 00100 #define SWING_END_1 (1550) 00101 #define CFG_8 (2718) 00102 #define CFG_15 (2727) 00103 #define CFG_16 (2746) 00104 #define CFG_EXT_GYRO_BIAS (1189) 00105 #define END_COMPARE_Y_X_TMP (1407) 00106 #define DO_NOT_UPDATE_PROP_ROT (1839) 00107 #define CFG_7 (1205) 00108 #define FLAT_STATE_END_TEMP (1683) 00109 #define END_COMPARE_Y_X (1484) 00110 #define SKIP_SWING_END_1 (1551) 00111 #define SKIP_SWING_END_3 (1588) 00112 #define SKIP_SWING_END_2 (1566) 00113 #define TILTG75_START (1672) 00114 #define CFG_6 (2753) 00115 #define TILTL75_END (1669) 00116 #define END_ORIENT (1884) 00117 #define CFG_FLICK_IN (2573) 00118 #define TILTL75_START (1643) 00119 #define CFG_MOTION_BIAS (1208) 00120 #define X_GRT_Y (1408) 00121 #define TEMPLABEL (2324) 00122 #define CFG_ANDROID_ORIENT_INT (1853) 00123 #define CFG_GYRO_RAW_DATA (2722) 00124 #define X_GRT_Y_TMP2 (1379) 00125 00126 #define D_0_22 (22+512) 00127 #define D_0_24 (24+512) 00128 00129 #define D_0_36 (36) 00130 #define D_0_52 (52) 00131 #define D_0_96 (96) 00132 #define D_0_104 (104) 00133 #define D_0_108 (108) 00134 #define D_0_163 (163) 00135 #define D_0_188 (188) 00136 #define D_0_192 (192) 00137 #define D_0_224 (224) 00138 #define D_0_228 (228) 00139 #define D_0_232 (232) 00140 #define D_0_236 (236) 00141 00142 #define D_1_2 (256 + 2) 00143 #define D_1_4 (256 + 4) 00144 #define D_1_8 (256 + 8) 00145 #define D_1_10 (256 + 10) 00146 #define D_1_24 (256 + 24) 00147 #define D_1_28 (256 + 28) 00148 #define D_1_36 (256 + 36) 00149 #define D_1_40 (256 + 40) 00150 #define D_1_44 (256 + 44) 00151 #define D_1_72 (256 + 72) 00152 #define D_1_74 (256 + 74) 00153 #define D_1_79 (256 + 79) 00154 #define D_1_88 (256 + 88) 00155 #define D_1_90 (256 + 90) 00156 #define D_1_92 (256 + 92) 00157 #define D_1_96 (256 + 96) 00158 #define D_1_98 (256 + 98) 00159 #define D_1_106 (256 + 106) 00160 #define D_1_108 (256 + 108) 00161 #define D_1_112 (256 + 112) 00162 #define D_1_128 (256 + 144) 00163 #define D_1_152 (256 + 12) 00164 #define D_1_160 (256 + 160) 00165 #define D_1_176 (256 + 176) 00166 #define D_1_178 (256 + 178) 00167 #define D_1_218 (256 + 218) 00168 #define D_1_232 (256 + 232) 00169 #define D_1_236 (256 + 236) 00170 #define D_1_240 (256 + 240) 00171 #define D_1_244 (256 + 244) 00172 #define D_1_250 (256 + 250) 00173 #define D_1_252 (256 + 252) 00174 #define D_2_12 (512 + 12) 00175 #define D_2_96 (512 + 96) 00176 #define D_2_108 (512 + 108) 00177 #define D_2_208 (512 + 208) 00178 #define D_2_224 (512 + 224) 00179 #define D_2_236 (512 + 236) 00180 #define D_2_244 (512 + 244) 00181 #define D_2_248 (512 + 248) 00182 #define D_2_252 (512 + 252) 00183 00184 #define CPASS_BIAS_X (35 * 16 + 4) 00185 #define CPASS_BIAS_Y (35 * 16 + 8) 00186 #define CPASS_BIAS_Z (35 * 16 + 12) 00187 #define CPASS_MTX_00 (36 * 16) 00188 #define CPASS_MTX_01 (36 * 16 + 4) 00189 #define CPASS_MTX_02 (36 * 16 + 8) 00190 #define CPASS_MTX_10 (36 * 16 + 12) 00191 #define CPASS_MTX_11 (37 * 16) 00192 #define CPASS_MTX_12 (37 * 16 + 4) 00193 #define CPASS_MTX_20 (37 * 16 + 8) 00194 #define CPASS_MTX_21 (37 * 16 + 12) 00195 #define CPASS_MTX_22 (43 * 16 + 12) 00196 #define D_EXT_GYRO_BIAS_X (61 * 16) 00197 #define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 00198 #define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 00199 #define D_ACT0 (40 * 16) 00200 #define D_ACSX (40 * 16 + 4) 00201 #define D_ACSY (40 * 16 + 8) 00202 #define D_ACSZ (40 * 16 + 12) 00203 00204 #define FLICK_MSG (45 * 16 + 4) 00205 #define FLICK_COUNTER (45 * 16 + 8) 00206 #define FLICK_LOWER (45 * 16 + 12) 00207 #define FLICK_UPPER (46 * 16 + 12) 00208 00209 #define D_AUTH_OUT (992) 00210 #define D_AUTH_IN (996) 00211 #define D_AUTH_A (1000) 00212 #define D_AUTH_B (1004) 00213 00214 #define D_PEDSTD_BP_B (768 + 0x1C) 00215 #define D_PEDSTD_HP_A (768 + 0x78) 00216 #define D_PEDSTD_HP_B (768 + 0x7C) 00217 #define D_PEDSTD_BP_A4 (768 + 0x40) 00218 #define D_PEDSTD_BP_A3 (768 + 0x44) 00219 #define D_PEDSTD_BP_A2 (768 + 0x48) 00220 #define D_PEDSTD_BP_A1 (768 + 0x4C) 00221 #define D_PEDSTD_INT_THRSH (768 + 0x68) 00222 #define D_PEDSTD_CLIP (768 + 0x6C) 00223 #define D_PEDSTD_SB (768 + 0x28) 00224 #define D_PEDSTD_SB_TIME (768 + 0x2C) 00225 #define D_PEDSTD_PEAKTHRSH (768 + 0x98) 00226 #define D_PEDSTD_TIML (768 + 0x2A) 00227 #define D_PEDSTD_TIMH (768 + 0x2E) 00228 #define D_PEDSTD_PEAK (768 + 0X94) 00229 #define D_PEDSTD_STEPCTR (768 + 0x60) 00230 #define D_PEDSTD_TIMECTR (964) 00231 #define D_PEDSTD_DECI (768 + 0xA0) 00232 00233 #define D_HOST_NO_MOT (976) 00234 #define D_ACCEL_BIAS (660) 00235 00236 #define D_ORIENT_GAP (76) 00237 00238 #define D_TILT0_H (48) 00239 #define D_TILT0_L (50) 00240 #define D_TILT1_H (52) 00241 #define D_TILT1_L (54) 00242 #define D_TILT2_H (56) 00243 #define D_TILT2_L (58) 00244 #define D_TILT3_H (60) 00245 #define D_TILT3_L (62) 00246 00247 #define DMP_CODE_SIZE (3062) 00248 00249 static const unsigned char dmp_memory[DMP_CODE_SIZE] = { 00250 /* bank # 0 */ 00251 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 00252 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 00253 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 00254 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 00255 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 00256 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, 00257 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 00258 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 00259 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 00260 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 00261 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 00262 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 00263 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 00264 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 00265 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, 00266 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00267 /* bank # 1 */ 00268 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 00269 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, 00270 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 00271 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00272 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 00273 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 00274 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 00275 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 00276 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00277 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00278 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 00279 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 00280 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 00281 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 00282 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 00283 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 00284 /* bank # 2 */ 00285 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 00286 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, 00287 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 00288 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 00289 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00290 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00291 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00292 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00293 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00294 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00295 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00296 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00297 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 00298 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, 00299 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 00300 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00301 /* bank # 3 */ 00302 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00303 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 00304 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, 00305 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00306 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, 00307 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00308 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, 00309 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, 00310 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00311 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, 00312 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00313 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00314 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00315 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00316 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, 00317 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00318 00319 /* bank # 4 */ 00320 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 00321 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 00322 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, 00323 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, 00324 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 00325 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 00326 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 00327 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 00328 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 00329 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, 00330 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 00331 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 00332 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 00333 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, 00334 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 00335 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, 00336 /* bank # 5 */ 00337 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, 00338 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, 00339 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 00340 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 00341 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 00342 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 00343 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 00344 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 00345 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 00346 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 00347 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 00348 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 00349 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 00350 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 00351 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, 00352 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, 00353 /* bank # 6 */ 00354 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, 00355 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, 00356 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, 00357 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 00358 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, 00359 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, 00360 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, 00361 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, 00362 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, 00363 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, 00364 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, 00365 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, 00366 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, 00367 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, 00368 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, 00369 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, 00370 /* bank # 7 */ 00371 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, 00372 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, 00373 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, 00374 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, 00375 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, 00376 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, 00377 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 00378 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 00379 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 00380 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 00381 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 00382 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 00383 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 00384 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 00385 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 00386 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 00387 /* bank # 8 */ 00388 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 00389 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 00390 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 00391 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 00392 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 00393 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 00394 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 00395 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 00396 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 00397 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 00398 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 00399 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 00400 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 00401 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 00402 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 00403 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 00404 /* bank # 9 */ 00405 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 00406 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 00407 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 00408 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 00409 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 00410 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 00411 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 00412 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 00413 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 00414 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 00415 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 00416 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 00417 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, 00418 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, 00419 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, 00420 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 00421 /* bank # 10 */ 00422 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, 00423 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 00424 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, 00425 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 00426 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 00427 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, 00428 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, 00429 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, 00430 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, 00431 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 00432 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 00433 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 00434 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 00435 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 00436 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 00437 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 00438 /* bank # 11 */ 00439 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 00440 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 00441 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 00442 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 00443 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 00444 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 00445 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 00446 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 00447 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 00448 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 00449 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 00450 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 00451 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 00452 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 00453 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 00454 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff 00455 }; 00456 00457 static const unsigned short sStartAddress = 0x0400; 00458 00459 /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ 00460 00461 #define INT_SRC_TAP (0x01) 00462 #define INT_SRC_ANDROID_ORIENT (0x08) 00463 00464 #define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ 00465 DMP_FEATURE_SEND_CAL_GYRO) 00466 00467 #define MAX_PACKET_LENGTH (32) 00468 00469 #define DMP_SAMPLE_RATE (200) 00470 #define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) 00471 00472 #define FIFO_CORRUPTION_CHECK 00473 #ifdef FIFO_CORRUPTION_CHECK 00474 #define QUAT_ERROR_THRESH (1L<<24) 00475 #define QUAT_MAG_SQ_NORMALIZED (1L<<28) 00476 #define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) 00477 #define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) 00478 #endif 00479 00480 struct dmp_s { 00481 void (*tap_cb)(unsigned char count, unsigned char direction); 00482 void (*android_orient_cb)(unsigned char orientation); 00483 unsigned short orient; 00484 unsigned short feature_mask; 00485 unsigned short fifo_rate; 00486 unsigned char packet_length; 00487 }; 00488 00489 static struct dmp_s dmp = { 00490 .tap_cb = NULL, 00491 .android_orient_cb = NULL, 00492 .orient = 0, 00493 .feature_mask = 0, 00494 .fifo_rate = 0, 00495 .packet_length = 0 00496 }; 00497 00498 /** 00499 * @brief Load the DMP with this image. 00500 * @return 0 if successful. 00501 */ 00502 int dmp_load_motion_driver_firmware(void) 00503 { 00504 return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, 00505 DMP_SAMPLE_RATE); 00506 } 00507 00508 /** 00509 * @brief Push gyro and accel orientation to the DMP. 00510 * The orientation is represented here as the output of 00511 * @e inv_orientation_matrix_to_scalar. 00512 * @param[in] orient Gyro and accel orientation in body frame. 00513 * @return 0 if successful. 00514 */ 00515 int dmp_set_orientation(unsigned short orient) 00516 { 00517 unsigned char gyro_regs[3], accel_regs[3]; 00518 const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; 00519 const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; 00520 const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; 00521 const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; 00522 00523 gyro_regs[0] = gyro_axes[orient & 3]; 00524 gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; 00525 gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; 00526 accel_regs[0] = accel_axes[orient & 3]; 00527 accel_regs[1] = accel_axes[(orient >> 3) & 3]; 00528 accel_regs[2] = accel_axes[(orient >> 6) & 3]; 00529 00530 /* Chip-to-body, axes only. */ 00531 if (mpu_write_mem(FCFG_1, 3, gyro_regs)) 00532 return -1; 00533 if (mpu_write_mem(FCFG_2, 3, accel_regs)) 00534 return -1; 00535 00536 memcpy(gyro_regs, gyro_sign, 3); 00537 memcpy(accel_regs, accel_sign, 3); 00538 if (orient & 4) { 00539 gyro_regs[0] |= 1; 00540 accel_regs[0] |= 1; 00541 } 00542 if (orient & 0x20) { 00543 gyro_regs[1] |= 1; 00544 accel_regs[1] |= 1; 00545 } 00546 if (orient & 0x100) { 00547 gyro_regs[2] |= 1; 00548 accel_regs[2] |= 1; 00549 } 00550 00551 /* Chip-to-body, sign only. */ 00552 if (mpu_write_mem(FCFG_3, 3, gyro_regs)) 00553 return -1; 00554 if (mpu_write_mem(FCFG_7, 3, accel_regs)) 00555 return -1; 00556 dmp.orient = orient; 00557 return 0; 00558 } 00559 00560 /** 00561 * @brief Push gyro biases to the DMP. 00562 * Because the gyro integration is handled in the DMP, any gyro biases 00563 * calculated by the MPL should be pushed down to DMP memory to remove 00564 * 3-axis quaternion drift. 00565 * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will 00566 * overwrite the biases written to this location once a new one is computed. 00567 * @param[in] bias Gyro biases in q16. 00568 * @return 0 if successful. 00569 */ 00570 int dmp_set_gyro_bias(long *bias) 00571 { 00572 long gyro_bias_body[3]; 00573 unsigned char regs[4]; 00574 00575 gyro_bias_body[0] = bias[dmp.orient & 3]; 00576 if (dmp.orient & 4) 00577 gyro_bias_body[0] *= -1; 00578 gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; 00579 if (dmp.orient & 0x20) 00580 gyro_bias_body[1] *= -1; 00581 gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; 00582 if (dmp.orient & 0x100) 00583 gyro_bias_body[2] *= -1; 00584 00585 #ifdef EMPL_NO_64BIT 00586 gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); 00587 gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); 00588 gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); 00589 #else 00590 gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); 00591 gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); 00592 gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); 00593 #endif 00594 00595 regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); 00596 regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); 00597 regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); 00598 regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); 00599 if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) 00600 return -1; 00601 00602 regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); 00603 regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); 00604 regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); 00605 regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); 00606 if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) 00607 return -1; 00608 00609 regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); 00610 regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); 00611 regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); 00612 regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); 00613 return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); 00614 } 00615 00616 /** 00617 * @brief Push accel biases to the DMP. 00618 * These biases will be removed from the DMP 6-axis quaternion. 00619 * @param[in] bias Accel biases in q16. 00620 * @return 0 if successful. 00621 */ 00622 int dmp_set_accel_bias(long *bias) 00623 { 00624 long accel_bias_body[3]; 00625 unsigned char regs[12]; 00626 long long accel_sf; 00627 unsigned short accel_sens; 00628 00629 mpu_get_accel_sens(&accel_sens); 00630 accel_sf = (long long)accel_sens << 15; 00631 __no_operation(); 00632 00633 accel_bias_body[0] = bias[dmp.orient & 3]; 00634 if (dmp.orient & 4) 00635 accel_bias_body[0] *= -1; 00636 accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; 00637 if (dmp.orient & 0x20) 00638 accel_bias_body[1] *= -1; 00639 accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; 00640 if (dmp.orient & 0x100) 00641 accel_bias_body[2] *= -1; 00642 00643 #ifdef EMPL_NO_64BIT 00644 accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); 00645 accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); 00646 accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); 00647 #else 00648 accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); 00649 accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); 00650 accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); 00651 #endif 00652 00653 regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); 00654 regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); 00655 regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); 00656 regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); 00657 regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); 00658 regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); 00659 regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); 00660 regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); 00661 regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); 00662 regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); 00663 regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); 00664 regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); 00665 return mpu_write_mem(D_ACCEL_BIAS, 12, regs); 00666 } 00667 00668 /** 00669 * @brief Set DMP output rate. 00670 * Only used when DMP is on. 00671 * @param[in] rate Desired fifo rate (Hz). 00672 * @return 0 if successful. 00673 */ 00674 int dmp_set_fifo_rate(unsigned short rate) 00675 { 00676 const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, 00677 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; 00678 unsigned short div; 00679 unsigned char tmp[8]; 00680 00681 if (rate > DMP_SAMPLE_RATE) 00682 return -1; 00683 div = DMP_SAMPLE_RATE / rate - 1; 00684 tmp[0] = (unsigned char)((div >> 8) & 0xFF); 00685 tmp[1] = (unsigned char)(div & 0xFF); 00686 if (mpu_write_mem(D_0_22, 2, tmp)) 00687 return -1; 00688 if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) 00689 return -1; 00690 00691 dmp.fifo_rate = rate; 00692 return 0; 00693 } 00694 00695 /** 00696 * @brief Get DMP output rate. 00697 * @param[out] rate Current fifo rate (Hz). 00698 * @return 0 if successful. 00699 */ 00700 int dmp_get_fifo_rate(unsigned short *rate) 00701 { 00702 rate[0] = dmp.fifo_rate; 00703 return 0; 00704 } 00705 00706 /** 00707 * @brief Set tap threshold for a specific axis. 00708 * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. 00709 * @param[in] thresh Tap threshold, in mg/ms. 00710 * @return 0 if successful. 00711 */ 00712 int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) 00713 { 00714 unsigned char tmp[4], accel_fsr; 00715 float scaled_thresh; 00716 unsigned short dmp_thresh, dmp_thresh_2; 00717 if (!(axis & TAP_XYZ) || thresh > 1600) 00718 return -1; 00719 00720 scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; 00721 00722 mpu_get_accel_fsr(&accel_fsr); 00723 switch (accel_fsr) { 00724 case 2: 00725 dmp_thresh = (unsigned short)(scaled_thresh * 16384); 00726 /* dmp_thresh * 0.75 */ 00727 dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); 00728 break; 00729 case 4: 00730 dmp_thresh = (unsigned short)(scaled_thresh * 8192); 00731 /* dmp_thresh * 0.75 */ 00732 dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); 00733 break; 00734 case 8: 00735 dmp_thresh = (unsigned short)(scaled_thresh * 4096); 00736 /* dmp_thresh * 0.75 */ 00737 dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); 00738 break; 00739 case 16: 00740 dmp_thresh = (unsigned short)(scaled_thresh * 2048); 00741 /* dmp_thresh * 0.75 */ 00742 dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); 00743 break; 00744 default: 00745 return -1; 00746 } 00747 tmp[0] = (unsigned char)(dmp_thresh >> 8); 00748 tmp[1] = (unsigned char)(dmp_thresh & 0xFF); 00749 tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); 00750 tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); 00751 00752 if (axis & TAP_X) { 00753 if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) 00754 return -1; 00755 if (mpu_write_mem(D_1_36, 2, tmp+2)) 00756 return -1; 00757 } 00758 if (axis & TAP_Y) { 00759 if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) 00760 return -1; 00761 if (mpu_write_mem(D_1_40, 2, tmp+2)) 00762 return -1; 00763 } 00764 if (axis & TAP_Z) { 00765 if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) 00766 return -1; 00767 if (mpu_write_mem(D_1_44, 2, tmp+2)) 00768 return -1; 00769 } 00770 return 0; 00771 } 00772 00773 /** 00774 * @brief Set which axes will register a tap. 00775 * @param[in] axis 1, 2, and 4 for XYZ, respectively. 00776 * @return 0 if successful. 00777 */ 00778 int dmp_set_tap_axes(unsigned char axis) 00779 { 00780 unsigned char tmp = 0; 00781 00782 if (axis & TAP_X) 00783 tmp |= 0x30; 00784 if (axis & TAP_Y) 00785 tmp |= 0x0C; 00786 if (axis & TAP_Z) 00787 tmp |= 0x03; 00788 return mpu_write_mem(D_1_72, 1, &tmp); 00789 } 00790 00791 /** 00792 * @brief Set minimum number of taps needed for an interrupt. 00793 * @param[in] min_taps Minimum consecutive taps (1-4). 00794 * @return 0 if successful. 00795 */ 00796 int dmp_set_tap_count(unsigned char min_taps) 00797 { 00798 unsigned char tmp; 00799 00800 if (min_taps < 1) 00801 min_taps = 1; 00802 else if (min_taps > 4) 00803 min_taps = 4; 00804 00805 tmp = min_taps - 1; 00806 return mpu_write_mem(D_1_79, 1, &tmp); 00807 } 00808 00809 /** 00810 * @brief Set length between valid taps. 00811 * @param[in] time Milliseconds between taps. 00812 * @return 0 if successful. 00813 */ 00814 int dmp_set_tap_time(unsigned short time) 00815 { 00816 unsigned short dmp_time; 00817 unsigned char tmp[2]; 00818 00819 dmp_time = time / (1000 / DMP_SAMPLE_RATE); 00820 tmp[0] = (unsigned char)(dmp_time >> 8); 00821 tmp[1] = (unsigned char)(dmp_time & 0xFF); 00822 return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); 00823 } 00824 00825 /** 00826 * @brief Set max time between taps to register as a multi-tap. 00827 * @param[in] time Max milliseconds between taps. 00828 * @return 0 if successful. 00829 */ 00830 int dmp_set_tap_time_multi(unsigned short time) 00831 { 00832 unsigned short dmp_time; 00833 unsigned char tmp[2]; 00834 00835 dmp_time = time / (1000 / DMP_SAMPLE_RATE); 00836 tmp[0] = (unsigned char)(dmp_time >> 8); 00837 tmp[1] = (unsigned char)(dmp_time & 0xFF); 00838 return mpu_write_mem(D_1_218, 2, tmp); 00839 } 00840 00841 /** 00842 * @brief Set shake rejection threshold. 00843 * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. 00844 * @param[in] sf Gyro scale factor. 00845 * @param[in] thresh Gyro threshold in dps. 00846 * @return 0 if successful. 00847 */ 00848 int dmp_set_shake_reject_thresh(long sf, unsigned short thresh) 00849 { 00850 unsigned char tmp[4]; 00851 long thresh_scaled = sf / 1000 * thresh; 00852 tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); 00853 tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); 00854 tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); 00855 tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); 00856 return mpu_write_mem(D_1_92, 4, tmp); 00857 } 00858 00859 /** 00860 * @brief Set shake rejection time. 00861 * Sets the length of time that the gyro must be outside of the threshold set 00862 * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory 00863 * 60 ms is added to this parameter. 00864 * @param[in] time Time in milliseconds. 00865 * @return 0 if successful. 00866 */ 00867 int dmp_set_shake_reject_time(unsigned short time) 00868 { 00869 unsigned char tmp[2]; 00870 00871 time /= (1000 / DMP_SAMPLE_RATE); 00872 tmp[0] = time >> 8; 00873 tmp[1] = time & 0xFF; 00874 return mpu_write_mem(D_1_90,2,tmp); 00875 } 00876 00877 /** 00878 * @brief Set shake rejection timeout. 00879 * Sets the length of time after a shake rejection that the gyro must stay 00880 * inside of the threshold before taps can be detected again. A mandatory 00881 * 60 ms is added to this parameter. 00882 * @param[in] time Time in milliseconds. 00883 * @return 0 if successful. 00884 */ 00885 int dmp_set_shake_reject_timeout(unsigned short time) 00886 { 00887 unsigned char tmp[2]; 00888 00889 time /= (1000 / DMP_SAMPLE_RATE); 00890 tmp[0] = time >> 8; 00891 tmp[1] = time & 0xFF; 00892 return mpu_write_mem(D_1_88,2,tmp); 00893 } 00894 00895 /** 00896 * @brief Get current step count. 00897 * @param[out] count Number of steps detected. 00898 * @return 0 if successful. 00899 */ 00900 int dmp_get_pedometer_step_count(unsigned long *count) 00901 { 00902 unsigned char tmp[4]; 00903 if (!count) 00904 return -1; 00905 00906 if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) 00907 return -1; 00908 00909 count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | 00910 ((unsigned long)tmp[2] << 8) | tmp[3]; 00911 return 0; 00912 } 00913 00914 /** 00915 * @brief Overwrite current step count. 00916 * WARNING: This function writes to DMP memory and could potentially encounter 00917 * a race condition if called while the pedometer is enabled. 00918 * @param[in] count New step count. 00919 * @return 0 if successful. 00920 */ 00921 int dmp_set_pedometer_step_count(unsigned long count) 00922 { 00923 unsigned char tmp[4]; 00924 00925 tmp[0] = (unsigned char)((count >> 24) & 0xFF); 00926 tmp[1] = (unsigned char)((count >> 16) & 0xFF); 00927 tmp[2] = (unsigned char)((count >> 8) & 0xFF); 00928 tmp[3] = (unsigned char)(count & 0xFF); 00929 return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); 00930 } 00931 00932 /** 00933 * @brief Get duration of walking time. 00934 * @param[in] time Walk time in milliseconds. 00935 * @return 0 if successful. 00936 */ 00937 int dmp_get_pedometer_walk_time(unsigned long *time) 00938 { 00939 unsigned char tmp[4]; 00940 if (!time) 00941 return -1; 00942 00943 if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) 00944 return -1; 00945 00946 time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | 00947 ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; 00948 return 0; 00949 } 00950 00951 /** 00952 * @brief Overwrite current walk time. 00953 * WARNING: This function writes to DMP memory and could potentially encounter 00954 * a race condition if called while the pedometer is enabled. 00955 * @param[in] time New walk time in milliseconds. 00956 */ 00957 int dmp_set_pedometer_walk_time(unsigned long time) 00958 { 00959 unsigned char tmp[4]; 00960 00961 time /= 20; 00962 00963 tmp[0] = (unsigned char)((time >> 24) & 0xFF); 00964 tmp[1] = (unsigned char)((time >> 16) & 0xFF); 00965 tmp[2] = (unsigned char)((time >> 8) & 0xFF); 00966 tmp[3] = (unsigned char)(time & 0xFF); 00967 return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); 00968 } 00969 00970 /** 00971 * @brief Enable DMP features. 00972 * The following \#define's are used in the input mask: 00973 * \n DMP_FEATURE_TAP 00974 * \n DMP_FEATURE_ANDROID_ORIENT 00975 * \n DMP_FEATURE_LP_QUAT 00976 * \n DMP_FEATURE_6X_LP_QUAT 00977 * \n DMP_FEATURE_GYRO_CAL 00978 * \n DMP_FEATURE_SEND_RAW_ACCEL 00979 * \n DMP_FEATURE_SEND_RAW_GYRO 00980 * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually 00981 * exclusive. 00982 * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also 00983 * mutually exclusive. 00984 * @param[in] mask Mask of features to enable. 00985 * @return 0 if successful. 00986 */ 00987 int dmp_enable_feature(unsigned short mask) 00988 { 00989 unsigned char tmp[10]; 00990 00991 /* TODO: All of these settings can probably be integrated into the default 00992 * DMP image. 00993 */ 00994 /* Set integration scale factor. */ 00995 tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); 00996 tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); 00997 tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); 00998 tmp[3] = (unsigned char)(GYRO_SF & 0xFF); 00999 mpu_write_mem(D_0_104, 4, tmp); 01000 01001 /* Send sensor data to the FIFO. */ 01002 tmp[0] = 0xA3; 01003 if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { 01004 tmp[1] = 0xC0; 01005 tmp[2] = 0xC8; 01006 tmp[3] = 0xC2; 01007 } else { 01008 tmp[1] = 0xA3; 01009 tmp[2] = 0xA3; 01010 tmp[3] = 0xA3; 01011 } 01012 if (mask & DMP_FEATURE_SEND_ANY_GYRO) { 01013 tmp[4] = 0xC4; 01014 tmp[5] = 0xCC; 01015 tmp[6] = 0xC6; 01016 } else { 01017 tmp[4] = 0xA3; 01018 tmp[5] = 0xA3; 01019 tmp[6] = 0xA3; 01020 } 01021 tmp[7] = 0xA3; 01022 tmp[8] = 0xA3; 01023 tmp[9] = 0xA3; 01024 mpu_write_mem(CFG_15,10,tmp); 01025 01026 /* Send gesture data to the FIFO. */ 01027 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 01028 tmp[0] = DINA20; 01029 else 01030 tmp[0] = 0xD8; 01031 mpu_write_mem(CFG_27,1,tmp); 01032 01033 if (mask & DMP_FEATURE_GYRO_CAL) 01034 dmp_enable_gyro_cal(1); 01035 else 01036 dmp_enable_gyro_cal(0); 01037 01038 if (mask & DMP_FEATURE_SEND_ANY_GYRO) { 01039 if (mask & DMP_FEATURE_SEND_CAL_GYRO) { 01040 tmp[0] = 0xB2; 01041 tmp[1] = 0x8B; 01042 tmp[2] = 0xB6; 01043 tmp[3] = 0x9B; 01044 } else { 01045 tmp[0] = DINAC0; 01046 tmp[1] = DINA80; 01047 tmp[2] = DINAC2; 01048 tmp[3] = DINA90; 01049 } 01050 mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); 01051 } 01052 01053 if (mask & DMP_FEATURE_TAP) { 01054 /* Enable tap. */ 01055 tmp[0] = 0xF8; 01056 mpu_write_mem(CFG_20, 1, tmp); 01057 dmp_set_tap_thresh(TAP_XYZ, 250); 01058 dmp_set_tap_axes(TAP_XYZ); 01059 dmp_set_tap_count(1); 01060 dmp_set_tap_time(100); 01061 dmp_set_tap_time_multi(500); 01062 01063 dmp_set_shake_reject_thresh(GYRO_SF, 200); 01064 dmp_set_shake_reject_time(40); 01065 dmp_set_shake_reject_timeout(10); 01066 } else { 01067 tmp[0] = 0xD8; 01068 mpu_write_mem(CFG_20, 1, tmp); 01069 } 01070 01071 if (mask & DMP_FEATURE_ANDROID_ORIENT) { 01072 tmp[0] = 0xD9; 01073 } else 01074 tmp[0] = 0xD8; 01075 mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); 01076 01077 if (mask & DMP_FEATURE_LP_QUAT) 01078 dmp_enable_lp_quat(1); 01079 else 01080 dmp_enable_lp_quat(0); 01081 01082 if (mask & DMP_FEATURE_6X_LP_QUAT) 01083 dmp_enable_6x_lp_quat(1); 01084 else 01085 dmp_enable_6x_lp_quat(0); 01086 01087 /* Pedometer is always enabled. */ 01088 dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; 01089 mpu_reset_fifo(); 01090 01091 dmp.packet_length = 0; 01092 if (mask & DMP_FEATURE_SEND_RAW_ACCEL) 01093 dmp.packet_length += 6; 01094 if (mask & DMP_FEATURE_SEND_ANY_GYRO) 01095 dmp.packet_length += 6; 01096 if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) 01097 dmp.packet_length += 16; 01098 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 01099 dmp.packet_length += 4; 01100 01101 return 0; 01102 } 01103 01104 /** 01105 * @brief Get list of currently enabled DMP features. 01106 * @param[out] Mask of enabled features. 01107 * @return 0 if successful. 01108 */ 01109 int dmp_get_enabled_features(unsigned short *mask) 01110 { 01111 mask[0] = dmp.feature_mask; 01112 return 0; 01113 } 01114 01115 /** 01116 * @brief Calibrate the gyro data in the DMP. 01117 * After eight seconds of no motion, the DMP will compute gyro biases and 01118 * subtract them from the quaternion output. If @e dmp_enable_feature is 01119 * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be 01120 * subtracted from the gyro output. 01121 * @param[in] enable 1 to enable gyro calibration. 01122 * @return 0 if successful. 01123 */ 01124 int dmp_enable_gyro_cal(unsigned char enable) 01125 { 01126 if (enable) { 01127 unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; 01128 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); 01129 } else { 01130 unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; 01131 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); 01132 } 01133 } 01134 01135 /** 01136 * @brief Generate 3-axis quaternions from the DMP. 01137 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually 01138 * exclusive. 01139 * @param[in] enable 1 to enable 3-axis quaternion. 01140 * @return 0 if successful. 01141 */ 01142 int dmp_enable_lp_quat(unsigned char enable) 01143 { 01144 unsigned char regs[4]; 01145 if (enable) { 01146 regs[0] = DINBC0; 01147 regs[1] = DINBC2; 01148 regs[2] = DINBC4; 01149 regs[3] = DINBC6; 01150 } 01151 else 01152 memset(regs, 0x8B, 4); 01153 01154 mpu_write_mem(CFG_LP_QUAT, 4, regs); 01155 01156 return mpu_reset_fifo(); 01157 } 01158 01159 /** 01160 * @brief Generate 6-axis quaternions from the DMP. 01161 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually 01162 * exclusive. 01163 * @param[in] enable 1 to enable 6-axis quaternion. 01164 * @return 0 if successful. 01165 */ 01166 int dmp_enable_6x_lp_quat(unsigned char enable) 01167 { 01168 unsigned char regs[4]; 01169 if (enable) { 01170 regs[0] = DINA20; 01171 regs[1] = DINA28; 01172 regs[2] = DINA30; 01173 regs[3] = DINA38; 01174 } else 01175 memset(regs, 0xA3, 4); 01176 01177 mpu_write_mem(CFG_8, 4, regs); 01178 01179 return mpu_reset_fifo(); 01180 } 01181 01182 /** 01183 * @brief Decode the four-byte gesture data and execute any callbacks. 01184 * @param[in] gesture Gesture data from DMP packet. 01185 * @return 0 if successful. 01186 */ 01187 static int decode_gesture(unsigned char *gesture) 01188 { 01189 unsigned char tap, android_orient; 01190 01191 android_orient = gesture[3] & 0xC0; 01192 tap = 0x3F & gesture[3]; 01193 01194 if (gesture[1] & INT_SRC_TAP) { 01195 unsigned char direction, count; 01196 direction = tap >> 3; 01197 count = (tap % 8) + 1; 01198 if (dmp.tap_cb) 01199 dmp.tap_cb(direction, count); 01200 } 01201 01202 if (gesture[1] & INT_SRC_ANDROID_ORIENT) { 01203 if (dmp.android_orient_cb) 01204 dmp.android_orient_cb(android_orient >> 6); 01205 } 01206 01207 return 0; 01208 } 01209 01210 /** 01211 * @brief Specify when a DMP interrupt should occur. 01212 * A DMP interrupt can be configured to trigger on either of the two 01213 * conditions below: 01214 * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). 01215 * \n b. A tap event has been detected. 01216 * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. 01217 * @return 0 if successful. 01218 */ 01219 int dmp_set_interrupt_mode(unsigned char mode) 01220 { 01221 const unsigned char regs_continuous[11] = 01222 {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; 01223 const unsigned char regs_gesture[11] = 01224 {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; 01225 01226 switch (mode) { 01227 case DMP_INT_CONTINUOUS: 01228 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, 01229 (unsigned char*)regs_continuous); 01230 case DMP_INT_GESTURE: 01231 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, 01232 (unsigned char*)regs_gesture); 01233 default: 01234 return -1; 01235 } 01236 } 01237 01238 /** 01239 * @brief Get one packet from the FIFO. 01240 * If @e sensors does not contain a particular sensor, disregard the data 01241 * returned to that pointer. 01242 * \n @e sensors can contain a combination of the following flags: 01243 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 01244 * \n INV_XYZ_GYRO 01245 * \n INV_XYZ_ACCEL 01246 * \n INV_WXYZ_QUAT 01247 * \n If the FIFO has no new data, @e sensors will be zero. 01248 * \n If the FIFO is disabled, @e sensors will be zero and this function will 01249 * return a non-zero error code. 01250 * @param[out] gyro Gyro data in hardware units. 01251 * @param[out] accel Accel data in hardware units. 01252 * @param[out] quat 3-axis quaternion data in hardware units. 01253 * @param[out] timestamp Timestamp in milliseconds. 01254 * @param[out] sensors Mask of sensors read from FIFO. 01255 * @param[out] more Number of remaining packets. 01256 * @return 0 if successful. 01257 */ 01258 int dmp_read_fifo(short *gyro, short *accel, long *quat, 01259 unsigned long *timestamp, short *sensors, unsigned char *more) 01260 { 01261 unsigned char fifo_data[MAX_PACKET_LENGTH]; 01262 unsigned char ii = 0; 01263 01264 /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can 01265 * cache this value and save some cycles. 01266 */ 01267 sensors[0] = 0; 01268 01269 /* Get a packet. */ 01270 if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) 01271 return -1; 01272 01273 /* Parse DMP packet. */ 01274 if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { 01275 #ifdef FIFO_CORRUPTION_CHECK 01276 long quat_q14[4], quat_mag_sq; 01277 #endif 01278 quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | 01279 ((long)fifo_data[2] << 8) | fifo_data[3]; 01280 quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | 01281 ((long)fifo_data[6] << 8) | fifo_data[7]; 01282 quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | 01283 ((long)fifo_data[10] << 8) | fifo_data[11]; 01284 quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | 01285 ((long)fifo_data[14] << 8) | fifo_data[15]; 01286 ii += 16; 01287 #ifdef FIFO_CORRUPTION_CHECK 01288 /* We can detect a corrupted FIFO by monitoring the quaternion data and 01289 * ensuring that the magnitude is always normalized to one. This 01290 * shouldn't happen in normal operation, but if an I2C error occurs, 01291 * the FIFO reads might become misaligned. 01292 * 01293 * Let's start by scaling down the quaternion data to avoid long long 01294 * math. 01295 */ 01296 quat_q14[0] = quat[0] >> 16; 01297 quat_q14[1] = quat[1] >> 16; 01298 quat_q14[2] = quat[2] >> 16; 01299 quat_q14[3] = quat[3] >> 16; 01300 quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + 01301 quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; 01302 if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || 01303 (quat_mag_sq > QUAT_MAG_SQ_MAX)) { 01304 /* Quaternion is outside of the acceptable threshold. */ 01305 mpu_reset_fifo(); 01306 sensors[0] = 0; 01307 return -1; 01308 } 01309 sensors[0] |= INV_WXYZ_QUAT; 01310 #endif 01311 } 01312 01313 if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { 01314 accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; 01315 accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; 01316 accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; 01317 ii += 6; 01318 sensors[0] |= INV_XYZ_ACCEL; 01319 } 01320 01321 if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { 01322 gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; 01323 gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; 01324 gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; 01325 ii += 6; 01326 sensors[0] |= INV_XYZ_GYRO; 01327 } 01328 01329 /* Gesture data is at the end of the DMP packet. Parse it and call 01330 * the gesture callbacks (if registered). 01331 */ 01332 if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 01333 decode_gesture(fifo_data + ii); 01334 01335 get_ms(timestamp); 01336 return 0; 01337 } 01338 01339 /** 01340 * @brief Register a function to be executed on a tap event. 01341 * The tap direction is represented by one of the following: 01342 * \n TAP_X_UP 01343 * \n TAP_X_DOWN 01344 * \n TAP_Y_UP 01345 * \n TAP_Y_DOWN 01346 * \n TAP_Z_UP 01347 * \n TAP_Z_DOWN 01348 * @param[in] func Callback function. 01349 * @return 0 if successful. 01350 */ 01351 int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) 01352 { 01353 dmp.tap_cb = func; 01354 return 0; 01355 } 01356 01357 /** 01358 * @brief Register a function to be executed on a android orientation event. 01359 * @param[in] func Callback function. 01360 * @return 0 if successful. 01361 */ 01362 int dmp_register_android_orient_cb(void (*func)(unsigned char)) 01363 { 01364 dmp.android_orient_cb = func; 01365 return 0; 01366 } 01367 01368 /** 01369 * @} 01370 */ 01371
Generated on Wed Jul 13 2022 20:01:33 by
1.7.2