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