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