inv_mpu_dmp_motion_driver.cpp
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  */
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 
00036 #define delay_ms    delay
00037 
00038 #define log_i(...)     do {} while (0)
00039 #define log_e(...)     do {} while (0)
00040 
00041 
00042 /* These defines are copied from dmpDefaultMPU6050.c in the general MPL
00043  * releases. These defines may change for each DMP image, so be sure to modify
00044  * these values when switching to a new image.
00045  */
00046 #define CFG_LP_QUAT             (2712)
00047 #define END_ORIENT_TEMP         (1866)
00048 #define CFG_27                  (2742)
00049 #define CFG_20                  (2224)
00050 #define CFG_23                  (2745)
00051 #define CFG_FIFO_ON_EVENT       (2690)
00052 #define END_PREDICTION_UPDATE   (1761)
00053 #define CGNOTICE_INTR           (2620)
00054 #define X_GRT_Y_TMP             (1358)
00055 #define CFG_DR_INT              (1029)
00056 #define CFG_AUTH                (1035)
00057 #define UPDATE_PROP_ROT         (1835)
00058 #define END_COMPARE_Y_X_TMP2    (1455)
00059 #define SKIP_X_GRT_Y_TMP        (1359)
00060 #define SKIP_END_COMPARE        (1435)
00061 #define FCFG_3                  (1088)
00062 #define FCFG_2                  (1066)
00063 #define FCFG_1                  (1062)
00064 #define END_COMPARE_Y_X_TMP3    (1434)
00065 #define FCFG_7                  (1073)
00066 #define FCFG_6                  (1106)
00067 #define FLAT_STATE_END          (1713)
00068 #define SWING_END_4             (1616)
00069 #define SWING_END_2             (1565)
00070 #define SWING_END_3             (1587)
00071 #define SWING_END_1             (1550)
00072 #define CFG_8                   (2718)
00073 #define CFG_15                  (2727)
00074 #define CFG_16                  (2746)
00075 #define CFG_EXT_GYRO_BIAS       (1189)
00076 #define END_COMPARE_Y_X_TMP     (1407)
00077 #define DO_NOT_UPDATE_PROP_ROT  (1839)
00078 #define CFG_7                   (1205)
00079 #define FLAT_STATE_END_TEMP     (1683)
00080 #define END_COMPARE_Y_X         (1484)
00081 #define SKIP_SWING_END_1        (1551)
00082 #define SKIP_SWING_END_3        (1588)
00083 #define SKIP_SWING_END_2        (1566)
00084 #define TILTG75_START           (1672)
00085 #define CFG_6                   (2753)
00086 #define TILTL75_END             (1669)
00087 #define END_ORIENT              (1884)
00088 #define CFG_FLICK_IN            (2573)
00089 #define TILTL75_START           (1643)
00090 #define CFG_MOTION_BIAS         (1208)
00091 #define X_GRT_Y                 (1408)
00092 #define TEMPLABEL               (2324)
00093 #define CFG_ANDROID_ORIENT_INT  (1853)
00094 #define CFG_GYRO_RAW_DATA       (2722)
00095 #define X_GRT_Y_TMP2            (1379)
00096 
00097 #define D_0_22                  (22+512)
00098 #define D_0_24                  (24+512)
00099 
00100 #define D_0_36                  (36)
00101 #define D_0_52                  (52)
00102 #define D_0_96                  (96)
00103 #define D_0_104                 (104)
00104 #define D_0_108                 (108)
00105 #define D_0_163                 (163)
00106 #define D_0_188                 (188)
00107 #define D_0_192                 (192)
00108 #define D_0_224                 (224)
00109 #define D_0_228                 (228)
00110 #define D_0_232                 (232)
00111 #define D_0_236                 (236)
00112 
00113 #define D_1_2                   (256 + 2)
00114 #define D_1_4                   (256 + 4)
00115 #define D_1_8                   (256 + 8)
00116 #define D_1_10                  (256 + 10)
00117 #define D_1_24                  (256 + 24)
00118 #define D_1_28                  (256 + 28)
00119 #define D_1_36                  (256 + 36)
00120 #define D_1_40                  (256 + 40)
00121 #define D_1_44                  (256 + 44)
00122 #define D_1_72                  (256 + 72)
00123 #define D_1_74                  (256 + 74)
00124 #define D_1_79                  (256 + 79)
00125 #define D_1_88                  (256 + 88)
00126 #define D_1_90                  (256 + 90)
00127 #define D_1_92                  (256 + 92)
00128 #define D_1_96                  (256 + 96)
00129 #define D_1_98                  (256 + 98)
00130 #define D_1_106                 (256 + 106)
00131 #define D_1_108                 (256 + 108)
00132 #define D_1_112                 (256 + 112)
00133 #define D_1_128                 (256 + 144)
00134 #define D_1_152                 (256 + 12)
00135 #define D_1_160                 (256 + 160)
00136 #define D_1_176                 (256 + 176)
00137 #define D_1_178                 (256 + 178)
00138 #define D_1_218                 (256 + 218)
00139 #define D_1_232                 (256 + 232)
00140 #define D_1_236                 (256 + 236)
00141 #define D_1_240                 (256 + 240)
00142 #define D_1_244                 (256 + 244)
00143 #define D_1_250                 (256 + 250)
00144 #define D_1_252                 (256 + 252)
00145 #define D_2_12                  (512 + 12)
00146 #define D_2_96                  (512 + 96)
00147 #define D_2_108                 (512 + 108)
00148 #define D_2_208                 (512 + 208)
00149 #define D_2_224                 (512 + 224)
00150 #define D_2_236                 (512 + 236)
00151 #define D_2_244                 (512 + 244)
00152 #define D_2_248                 (512 + 248)
00153 #define D_2_252                 (512 + 252)
00154 
00155 #define CPASS_BIAS_X            (35 * 16 + 4)
00156 #define CPASS_BIAS_Y            (35 * 16 + 8)
00157 #define CPASS_BIAS_Z            (35 * 16 + 12)
00158 #define CPASS_MTX_00            (36 * 16)
00159 #define CPASS_MTX_01            (36 * 16 + 4)
00160 #define CPASS_MTX_02            (36 * 16 + 8)
00161 #define CPASS_MTX_10            (36 * 16 + 12)
00162 #define CPASS_MTX_11            (37 * 16)
00163 #define CPASS_MTX_12            (37 * 16 + 4)
00164 #define CPASS_MTX_20            (37 * 16 + 8)
00165 #define CPASS_MTX_21            (37 * 16 + 12)
00166 #define CPASS_MTX_22            (43 * 16 + 12)
00167 #define D_EXT_GYRO_BIAS_X       (61 * 16)
00168 #define D_EXT_GYRO_BIAS_Y       (61 * 16) + 4
00169 #define D_EXT_GYRO_BIAS_Z       (61 * 16) + 8
00170 #define D_ACT0                  (40 * 16)
00171 #define D_ACSX                  (40 * 16 + 4)
00172 #define D_ACSY                  (40 * 16 + 8)
00173 #define D_ACSZ                  (40 * 16 + 12)
00174 
00175 #define FLICK_MSG               (45 * 16 + 4)
00176 #define FLICK_COUNTER           (45 * 16 + 8)
00177 #define FLICK_LOWER             (45 * 16 + 12)
00178 #define FLICK_UPPER             (46 * 16 + 12)
00179 
00180 #define D_AUTH_OUT              (992)
00181 #define D_AUTH_IN               (996)
00182 #define D_AUTH_A                (1000)
00183 #define D_AUTH_B                (1004)
00184 
00185 #define D_PEDSTD_BP_B           (768 + 0x1C)
00186 #define D_PEDSTD_HP_A           (768 + 0x78)
00187 #define D_PEDSTD_HP_B           (768 + 0x7C)
00188 #define D_PEDSTD_BP_A4          (768 + 0x40)
00189 #define D_PEDSTD_BP_A3          (768 + 0x44)
00190 #define D_PEDSTD_BP_A2          (768 + 0x48)
00191 #define D_PEDSTD_BP_A1          (768 + 0x4C)
00192 #define D_PEDSTD_INT_THRSH      (768 + 0x68)
00193 #define D_PEDSTD_CLIP           (768 + 0x6C)
00194 #define D_PEDSTD_SB             (768 + 0x28)
00195 #define D_PEDSTD_SB_TIME        (768 + 0x2C)
00196 #define D_PEDSTD_PEAKTHRSH      (768 + 0x98)
00197 #define D_PEDSTD_TIML           (768 + 0x2A)
00198 #define D_PEDSTD_TIMH           (768 + 0x2E)
00199 #define D_PEDSTD_PEAK           (768 + 0X94)
00200 #define D_PEDSTD_STEPCTR        (768 + 0x60)
00201 #define D_PEDSTD_TIMECTR        (964)
00202 #define D_PEDSTD_DECI           (768 + 0xA0)
00203 
00204 #define D_HOST_NO_MOT           (976)
00205 #define D_ACCEL_BIAS            (660)
00206 
00207 #define D_ORIENT_GAP            (76)
00208 
00209 #define D_TILT0_H               (48)
00210 #define D_TILT0_L               (50)
00211 #define D_TILT1_H               (52)
00212 #define D_TILT1_L               (54)
00213 #define D_TILT2_H               (56)
00214 #define D_TILT2_L               (58)
00215 #define D_TILT3_H               (60)
00216 #define D_TILT3_L               (62)
00217 
00218 #define DMP_CODE_SIZE           (3062)
00219 
00220 #ifdef __SAM3X8E__
00221 const uint8_t dmp_memory[DMP_CODE_SIZE] = {
00222 #else
00223 const unsigned char dmp_memory[DMP_CODE_SIZE] PROGMEM = {
00224 #endif
00225     /* bank # 0 */
00226     0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
00227     0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
00228     0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
00229     0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
00230     0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
00231     0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
00232     0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
00233     0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
00234     0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
00235     0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
00236     0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
00237     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
00238     0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
00239     0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
00240     0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
00241     0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00242     /* bank # 1 */
00243     0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
00244     0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
00245     0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
00246     0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00247     0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
00248     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
00249     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
00250     0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
00251     0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00252     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00253     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
00254     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
00255     0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
00256     0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
00257     0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
00258     0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
00259     /* bank # 2 */
00260     0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
00261     0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
00262     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
00263     0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
00264     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00265     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00266     0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00267     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00268     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00269     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00270     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00271     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00272     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
00273     0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
00274     0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
00275     0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00276     /* bank # 3 */
00277     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00278     0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
00279     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
00280     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00281     0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
00282     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00283     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
00284     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
00285     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00286     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
00287     0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00288     0x40, 0x00, 0x00, 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, 0x00, 0x00,
00291     0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
00292     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00293 
00294     /* bank # 4 */
00295     0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
00296     0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
00297     0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
00298     0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
00299     0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
00300     0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
00301     0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
00302     0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
00303     0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
00304     0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
00305     0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
00306     0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
00307     0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
00308     0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
00309     0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
00310     0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
00311     /* bank # 5 */
00312     0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
00313     0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
00314     0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
00315     0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
00316     0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
00317     0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
00318     0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
00319     0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
00320     0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
00321     0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
00322     0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
00323     0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
00324     0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
00325     0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
00326     0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
00327     0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
00328     /* bank # 6 */
00329     0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
00330     0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
00331     0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
00332     0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
00333     0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
00334     0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
00335     0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
00336     0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
00337     0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
00338     0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
00339     0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
00340     0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
00341     0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
00342     0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
00343     0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
00344     0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
00345     /* bank # 7 */
00346     0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
00347     0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
00348     0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
00349     0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
00350     0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
00351     0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
00352     0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
00353     0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
00354     0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
00355     0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
00356     0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
00357     0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
00358     0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
00359     0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
00360     0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
00361     0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
00362     /* bank # 8 */
00363     0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
00364     0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
00365     0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
00366     0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
00367     0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
00368     0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
00369     0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
00370     0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
00371     0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
00372     0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
00373     0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
00374     0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
00375     0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
00376     0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
00377     0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
00378     0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
00379     /* bank # 9 */
00380     0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
00381     0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
00382     0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
00383     0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
00384     0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
00385     0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
00386     0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
00387     0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
00388     0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
00389     0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
00390     0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
00391     0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
00392     0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
00393     0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
00394     0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
00395     0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
00396     /* bank # 10 */
00397     0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
00398     0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
00399     0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
00400     0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
00401     0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
00402     0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
00403     0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
00404     0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
00405     0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
00406     0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
00407     0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
00408     0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
00409     0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
00410     0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
00411     0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
00412     0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
00413     /* bank # 11 */
00414     0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
00415     0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
00416     0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
00417     0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
00418     0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
00419     0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
00420     0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
00421     0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
00422     0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
00423     0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
00424     0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
00425     0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
00426     0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
00427     0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
00428     0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
00429     0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
00430 };
00431 
00432 static const unsigned short sStartAddress = 0x0400;
00433 
00434 /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
00435 
00436 #define INT_SRC_TAP             (0x01)
00437 #define INT_SRC_ANDROID_ORIENT  (0x08)
00438 
00439 #define DMP_FEATURE_SEND_ANY_GYRO   (DMP_FEATURE_SEND_RAW_GYRO | \
00440                                      DMP_FEATURE_SEND_CAL_GYRO)
00441 
00442 #define MAX_PACKET_LENGTH   (32)
00443 
00444 #define DMP_SAMPLE_RATE     (200)
00445 #define GYRO_SF             (46850825LL * 200 / DMP_SAMPLE_RATE)
00446 
00447 #define FIFO_CORRUPTION_CHECK
00448 #ifdef FIFO_CORRUPTION_CHECK
00449 #define QUAT_ERROR_THRESH       (1L<<24)
00450 #define QUAT_MAG_SQ_NORMALIZED  (1L<<28)
00451 #define QUAT_MAG_SQ_MIN         (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
00452 #define QUAT_MAG_SQ_MAX         (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
00453 #endif
00454 
00455 struct dmp_s {
00456     void (*tap_cb)(unsigned char count, unsigned char direction);
00457     void (*android_orient_cb)(unsigned char orientation);
00458     unsigned short orient;
00459     unsigned short feature_mask;
00460     unsigned short fifo_rate;
00461     unsigned char packet_length;
00462 };
00463 
00464 struct dmp_s dmpArray[MPU_MAX_DEVICES];
00465 
00466 struct dmp_s *dmp;
00467 static int deviceIndex = 0;
00468 
00469 int dmp_select_device(int device)
00470 {
00471   if ((device < 0) || (device >= MPU_MAX_DEVICES))
00472     return -1;
00473 
00474   deviceIndex = device;
00475   dmp = dmpArray + device;
00476   return 0;
00477 }
00478 
00479 void dmp_init_structures()
00480 {
00481   dmp->tap_cb = NULL;
00482   dmp->android_orient_cb = NULL;
00483   dmp->orient = 0;
00484   dmp->feature_mask = 0;
00485   dmp->fifo_rate = 0;
00486   dmp->packet_length = 0;
00487 }
00488 
00493 int dmp_load_motion_driver_firmware(void)
00494 {
00495     return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
00496         DMP_SAMPLE_RATE);
00497 }
00498 
00506 int dmp_set_orientation(unsigned short orient)
00507 {
00508     unsigned char gyro_regs[3], accel_regs[3];
00509     const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
00510     const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
00511     const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
00512     const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
00513 
00514     gyro_regs[0] = gyro_axes[orient & 3];
00515     gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
00516     gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
00517     accel_regs[0] = accel_axes[orient & 3];
00518     accel_regs[1] = accel_axes[(orient >> 3) & 3];
00519     accel_regs[2] = accel_axes[(orient >> 6) & 3];
00520 
00521     /* Chip-to-body, axes only. */
00522     if (mpu_write_mem(FCFG_1, 3, gyro_regs))
00523         return -1;
00524     if (mpu_write_mem(FCFG_2, 3, accel_regs))
00525         return -1;
00526 
00527     memcpy(gyro_regs, gyro_sign, 3);
00528     memcpy(accel_regs, accel_sign, 3);
00529     if (orient & 4) {
00530         gyro_regs[0] |= 1;
00531         accel_regs[0] |= 1;
00532     }
00533     if (orient & 0x20) {
00534         gyro_regs[1] |= 1;
00535         accel_regs[1] |= 1;
00536     }
00537     if (orient & 0x100) {
00538         gyro_regs[2] |= 1;
00539         accel_regs[2] |= 1;
00540     }
00541 
00542     /* Chip-to-body, sign only. */
00543     if (mpu_write_mem(FCFG_3, 3, gyro_regs))
00544         return -1;
00545     if (mpu_write_mem(FCFG_7, 3, accel_regs))
00546         return -1;
00547     dmp->orient = orient;
00548     return 0;
00549 }
00550 
00561 int dmp_set_gyro_bias(long *bias)
00562 {
00563     long gyro_bias_body[3];
00564     unsigned char regs[4];
00565 
00566     gyro_bias_body[0] = bias[dmp->orient & 3];
00567     if (dmp->orient & 4)
00568         gyro_bias_body[0] *= -1;
00569     gyro_bias_body[1] = bias[(dmp->orient >> 3) & 3];
00570     if (dmp->orient & 0x20)
00571         gyro_bias_body[1] *= -1;
00572     gyro_bias_body[2] = bias[(dmp->orient >> 6) & 3];
00573     if (dmp->orient & 0x100)
00574         gyro_bias_body[2] *= -1;
00575 
00576 #ifdef EMPL_NO_64BIT
00577     gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
00578     gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
00579     gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
00580 #else
00581     gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
00582     gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
00583     gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
00584 #endif
00585 
00586     regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
00587     regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
00588     regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
00589     regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
00590     if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
00591         return -1;
00592 
00593     regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
00594     regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
00595     regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
00596     regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
00597     if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
00598         return -1;
00599 
00600     regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
00601     regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
00602     regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
00603     regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
00604     return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
00605 }
00606 
00613 int dmp_set_accel_bias(long *bias)
00614 {
00615     long accel_bias_body[3];
00616     unsigned char regs[12];
00617     long long accel_sf;
00618     unsigned short accel_sens;
00619 
00620     mpu_get_accel_sens(&accel_sens);
00621     accel_sf = (long long)accel_sens << 15;
00622     delay(1);
00623 
00624     accel_bias_body[0] = bias[dmp->orient & 3];
00625     if (dmp->orient & 4)
00626         accel_bias_body[0] *= -1;
00627     accel_bias_body[1] = bias[(dmp->orient >> 3) & 3];
00628     if (dmp->orient & 0x20)
00629         accel_bias_body[1] *= -1;
00630     accel_bias_body[2] = bias[(dmp->orient >> 6) & 3];
00631     if (dmp->orient & 0x100)
00632         accel_bias_body[2] *= -1;
00633 
00634 #ifdef EMPL_NO_64BIT
00635     accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
00636     accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
00637     accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
00638 #else
00639     accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
00640     accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
00641     accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
00642 #endif
00643 
00644     regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
00645     regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
00646     regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
00647     regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
00648     regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
00649     regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
00650     regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
00651     regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
00652     regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
00653     regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
00654     regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
00655     regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
00656     return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
00657 }
00658 
00665 int dmp_set_fifo_rate(unsigned short rate)
00666 {
00667     const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
00668         0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
00669     unsigned short div;
00670     unsigned char tmp[8];
00671 
00672     if (rate > DMP_SAMPLE_RATE)
00673         return -1;
00674     div = DMP_SAMPLE_RATE / rate - 1;
00675     tmp[0] = (unsigned char)((div >> 8) & 0xFF);
00676     tmp[1] = (unsigned char)(div & 0xFF);
00677     if (mpu_write_mem(D_0_22, 2, tmp))
00678         return -1;
00679     if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
00680         return -1;
00681 
00682     dmp->fifo_rate = rate;
00683     return 0;
00684 }
00685 
00691 int dmp_get_fifo_rate(unsigned short *rate)
00692 {
00693     rate[0] = dmp->fifo_rate;
00694     return 0;
00695 }
00696 
00697 #ifdef MPU_MAXIMAL
00698 
00705 int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
00706 {
00707     unsigned char tmp[4], accel_fsr;
00708     float scaled_thresh;
00709     unsigned short dmp_thresh, dmp_thresh_2;
00710     if (!(axis & TAP_XYZ) || thresh > 1600)
00711         return -1;
00712 
00713     scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
00714 
00715     mpu_get_accel_fsr(&accel_fsr);
00716     switch (accel_fsr) {
00717     case 2:
00718         dmp_thresh = (unsigned short)(scaled_thresh * 16384);
00719         /* dmp_thresh * 0.75 */
00720         dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
00721         break;
00722     case 4:
00723         dmp_thresh = (unsigned short)(scaled_thresh * 8192);
00724         /* dmp_thresh * 0.75 */
00725         dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
00726         break;
00727     case 8:
00728         dmp_thresh = (unsigned short)(scaled_thresh * 4096);
00729         /* dmp_thresh * 0.75 */
00730         dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
00731         break;
00732     case 16:
00733         dmp_thresh = (unsigned short)(scaled_thresh * 2048);
00734         /* dmp_thresh * 0.75 */
00735         dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
00736         break;
00737     default:
00738         return -1;
00739     }
00740     tmp[0] = (unsigned char)(dmp_thresh >> 8);
00741     tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
00742     tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
00743     tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
00744 
00745     if (axis & TAP_X) {
00746         if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
00747             return -1;
00748         if (mpu_write_mem(D_1_36, 2, tmp+2))
00749             return -1;
00750     }
00751     if (axis & TAP_Y) {
00752         if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
00753             return -1;
00754         if (mpu_write_mem(D_1_40, 2, tmp+2))
00755             return -1;
00756     }
00757     if (axis & TAP_Z) {
00758         if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
00759             return -1;
00760         if (mpu_write_mem(D_1_44, 2, tmp+2))
00761             return -1;
00762     }
00763     return 0;
00764 }
00765 
00771 int dmp_set_tap_axes(unsigned char axis)
00772 {
00773     unsigned char tmp = 0;
00774 
00775     if (axis & TAP_X)
00776         tmp |= 0x30;
00777     if (axis & TAP_Y)
00778         tmp |= 0x0C;
00779     if (axis & TAP_Z)
00780         tmp |= 0x03;
00781     return mpu_write_mem(D_1_72, 1, &tmp);
00782 }
00783 
00789 int dmp_set_tap_count(unsigned char min_taps)
00790 {
00791     unsigned char tmp;
00792 
00793     if (min_taps < 1)
00794         min_taps = 1;
00795     else if (min_taps > 4)
00796         min_taps = 4;
00797 
00798     tmp = min_taps - 1;
00799     return mpu_write_mem(D_1_79, 1, &tmp);
00800 }
00801 
00807 int dmp_set_tap_time(unsigned short time)
00808 {
00809     unsigned short dmp_time;
00810     unsigned char tmp[2];
00811 
00812     dmp_time = time / (1000 / DMP_SAMPLE_RATE);
00813     tmp[0] = (unsigned char)(dmp_time >> 8);
00814     tmp[1] = (unsigned char)(dmp_time & 0xFF);
00815     return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
00816 }
00817 
00823 int dmp_set_tap_time_multi(unsigned short time)
00824 {
00825     unsigned short dmp_time;
00826     unsigned char tmp[2];
00827 
00828     dmp_time = time / (1000 / DMP_SAMPLE_RATE);
00829     tmp[0] = (unsigned char)(dmp_time >> 8);
00830     tmp[1] = (unsigned char)(dmp_time & 0xFF);
00831     return mpu_write_mem(D_1_218, 2, tmp);
00832 }
00833 
00841 int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
00842 {
00843     unsigned char tmp[4];
00844     long thresh_scaled = sf / 1000 * thresh;
00845     tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
00846     tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
00847     tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
00848     tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
00849     return mpu_write_mem(D_1_92, 4, tmp);
00850 }
00851 
00860 int dmp_set_shake_reject_time(unsigned short time)
00861 {
00862     unsigned char tmp[2];
00863 
00864     time /= (1000 / DMP_SAMPLE_RATE);
00865     tmp[0] = time >> 8;
00866     tmp[1] = time & 0xFF;
00867     return mpu_write_mem(D_1_90,2,tmp);
00868 }
00869 
00878 int dmp_set_shake_reject_timeout(unsigned short time)
00879 {
00880     unsigned char tmp[2];
00881 
00882     time /= (1000 / DMP_SAMPLE_RATE);
00883     tmp[0] = time >> 8;
00884     tmp[1] = time & 0xFF;
00885     return mpu_write_mem(D_1_88,2,tmp);
00886 }
00887 
00893 int dmp_get_pedometer_step_count(unsigned long *count)
00894 {
00895     unsigned char tmp[4];
00896     if (!count)
00897         return -1;
00898 
00899     if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
00900         return -1;
00901 
00902     count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
00903         ((unsigned long)tmp[2] << 8) | tmp[3];
00904     return 0;
00905 }
00906 
00914 int dmp_set_pedometer_step_count(unsigned long count)
00915 {
00916     unsigned char tmp[4];
00917 
00918     tmp[0] = (unsigned char)((count >> 24) & 0xFF);
00919     tmp[1] = (unsigned char)((count >> 16) & 0xFF);
00920     tmp[2] = (unsigned char)((count >> 8) & 0xFF);
00921     tmp[3] = (unsigned char)(count & 0xFF);
00922     return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);
00923 }
00924 
00930 int dmp_get_pedometer_walk_time(unsigned long *time)
00931 {
00932     unsigned char tmp[4];
00933     if (!time)
00934         return -1;
00935 
00936     if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
00937         return -1;
00938 
00939     time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
00940         ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
00941     return 0;
00942 }
00943 
00950 int dmp_set_pedometer_walk_time(unsigned long time)
00951 {
00952     unsigned char tmp[4];
00953 
00954     time /= 20;
00955 
00956     tmp[0] = (unsigned char)((time >> 24) & 0xFF);
00957     tmp[1] = (unsigned char)((time >> 16) & 0xFF);
00958     tmp[2] = (unsigned char)((time >> 8) & 0xFF);
00959     tmp[3] = (unsigned char)(time & 0xFF);
00960     return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);
00961 }
00962 #endif // MPU_MAXIMAL
00963 
00981 int dmp_enable_feature(unsigned short mask)
00982 {
00983     unsigned char tmp[10];
00984 
00985     /* TODO: All of these settings can probably be integrated into the default
00986      * DMP image.
00987      */
00988     /* Set integration scale factor. */
00989     tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
00990     tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
00991     tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
00992     tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
00993     mpu_write_mem(D_0_104, 4, tmp);
00994 
00995     /* Send sensor data to the FIFO. */
00996     tmp[0] = 0xA3;
00997     if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
00998         tmp[1] = 0xC0;
00999         tmp[2] = 0xC8;
01000         tmp[3] = 0xC2;
01001     } else {
01002         tmp[1] = 0xA3;
01003         tmp[2] = 0xA3;
01004         tmp[3] = 0xA3;
01005     }
01006     if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
01007         tmp[4] = 0xC4;
01008         tmp[5] = 0xCC;
01009         tmp[6] = 0xC6;
01010     } else {
01011         tmp[4] = 0xA3;
01012         tmp[5] = 0xA3;
01013         tmp[6] = 0xA3;
01014     }
01015     tmp[7] = 0xA3;
01016     tmp[8] = 0xA3;
01017     tmp[9] = 0xA3;
01018     mpu_write_mem(CFG_15,10,tmp);
01019 
01020     /* Send gesture data to the FIFO. */
01021     if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
01022         tmp[0] = DINA20;
01023     else
01024         tmp[0] = 0xD8;
01025     mpu_write_mem(CFG_27,1,tmp);
01026 
01027     if (mask & DMP_FEATURE_GYRO_CAL)
01028         dmp_enable_gyro_cal(1);
01029     else
01030         dmp_enable_gyro_cal(0);
01031 
01032     if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
01033         if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
01034             tmp[0] = 0xB2;
01035             tmp[1] = 0x8B;
01036             tmp[2] = 0xB6;
01037             tmp[3] = 0x9B;
01038         } else {
01039             tmp[0] = DINAC0;
01040             tmp[1] = DINA80;
01041             tmp[2] = DINAC2;
01042             tmp[3] = DINA90;
01043         }
01044         mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
01045     }
01046 
01047 #ifdef MPU_MAXIMAL
01048     if (mask & DMP_FEATURE_TAP) {
01049         /* Enable tap. */
01050         tmp[0] = 0xF8;
01051         mpu_write_mem(CFG_20, 1, tmp);
01052         dmp_set_tap_thresh(TAP_XYZ, 250);
01053         dmp_set_tap_axes(TAP_XYZ);
01054         dmp_set_tap_count(1);
01055         dmp_set_tap_time(100);
01056         dmp_set_tap_time_multi(500);
01057 
01058         dmp_set_shake_reject_thresh(GYRO_SF, 200);
01059         dmp_set_shake_reject_time(40);
01060         dmp_set_shake_reject_timeout(10);
01061     } else {
01062         tmp[0] = 0xD8;
01063         mpu_write_mem(CFG_20, 1, tmp);
01064     }
01065 
01066     if (mask & DMP_FEATURE_ANDROID_ORIENT) {
01067         tmp[0] = 0xD9;
01068     } else
01069         tmp[0] = 0xD8;
01070     mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
01071 #endif // MPU_MAXIMAL
01072 
01073     if (mask & DMP_FEATURE_LP_QUAT)
01074         dmp_enable_lp_quat(1);
01075     else
01076         dmp_enable_lp_quat(0);
01077 
01078     if (mask & DMP_FEATURE_6X_LP_QUAT)
01079         dmp_enable_6x_lp_quat(1);
01080     else
01081         dmp_enable_6x_lp_quat(0);
01082 
01083     /* Pedometer is always enabled. */
01084     dmp->feature_mask = mask | DMP_FEATURE_PEDOMETER;
01085     mpu_reset_fifo();
01086 
01087     dmp->packet_length = 0;
01088     if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
01089         dmp->packet_length += 6;
01090     if (mask & DMP_FEATURE_SEND_ANY_GYRO)
01091         dmp->packet_length += 6;
01092     if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
01093         dmp->packet_length += 16;
01094     if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
01095         dmp->packet_length += 4;
01096 
01097     return 0;
01098 }
01099 
01105 int dmp_get_enabled_features(unsigned short *mask)
01106 {
01107     mask[0] = dmp->feature_mask;
01108     return 0;
01109 }
01110 
01120 int dmp_enable_gyro_cal(unsigned char enable)
01121 {
01122     if (enable) {
01123         unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
01124         return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
01125     } else {
01126         unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
01127         return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
01128     }
01129 }
01130 
01138 int dmp_enable_lp_quat(unsigned char enable)
01139 {
01140     unsigned char regs[4];
01141     if (enable) {
01142         regs[0] = DINBC0;
01143         regs[1] = DINBC2;
01144         regs[2] = DINBC4;
01145         regs[3] = DINBC6;
01146     }
01147     else
01148         memset(regs, 0x8B, 4);
01149 
01150     mpu_write_mem(CFG_LP_QUAT, 4, regs);
01151 
01152     return mpu_reset_fifo();
01153 }
01154 
01162 int dmp_enable_6x_lp_quat(unsigned char enable)
01163 {
01164     unsigned char regs[4];
01165     if (enable) {
01166         regs[0] = DINA20;
01167         regs[1] = DINA28;
01168         regs[2] = DINA30;
01169         regs[3] = DINA38;
01170     } else
01171         memset(regs, 0xA3, 4);
01172 
01173     mpu_write_mem(CFG_8, 4, regs);
01174 
01175     return mpu_reset_fifo();
01176 }
01177 
01178 #ifdef MPU_MAXIMAL
01179 
01184 static int decode_gesture(unsigned char *gesture)
01185 {
01186     unsigned char tap, android_orient;
01187 
01188     android_orient = gesture[3] & 0xC0;
01189     tap = 0x3F & gesture[3];
01190 
01191     if (gesture[1] & INT_SRC_TAP) {
01192         unsigned char direction, count;
01193         direction = tap >> 3;
01194         count = (tap % 8) + 1;
01195         if (dmp->tap_cb)
01196             dmp->tap_cb(direction, count);
01197     }
01198 
01199     if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
01200         if (dmp->android_orient_cb)
01201             dmp->android_orient_cb(android_orient >> 6);
01202     }
01203 
01204     return 0;
01205 }
01206 #endif // MPU_MAXIMAL
01207 
01217 int dmp_set_interrupt_mode(unsigned char mode)
01218 {
01219     const unsigned char regs_continuous[11] =
01220         {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
01221     const unsigned char regs_gesture[11] =
01222         {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
01223 
01224     switch (mode) {
01225     case DMP_INT_CONTINUOUS:
01226         return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
01227             (unsigned char*)regs_continuous);
01228     case DMP_INT_GESTURE:
01229         return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
01230             (unsigned char*)regs_gesture);
01231     default:
01232         return -1;
01233     }
01234 }
01235 
01256 int dmp_read_fifo(short *gyro, short *accel, long *quat,
01257     unsigned long *timestamp, short *sensors, unsigned char *more)
01258 {
01259     unsigned char fifo_data[MAX_PACKET_LENGTH];
01260     unsigned char ii = 0;
01261     int errCode;
01262 
01263     /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
01264      * cache this value and save some cycles.
01265      */
01266     sensors[0] = 0;
01267 
01268     /* Get a packet. */
01269     if ((errCode = mpu_read_fifo_stream(dmp->packet_length, fifo_data, more)))
01270         return -1;
01271 
01272     /* Parse DMP packet. */
01273     if (dmp->feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
01274 #ifdef FIFO_CORRUPTION_CHECK
01275         long quat_q14[4], quat_mag_sq;
01276 #endif
01277         quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
01278             ((long)fifo_data[2] << 8) | fifo_data[3];
01279         quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
01280             ((long)fifo_data[6] << 8) | fifo_data[7];
01281         quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
01282             ((long)fifo_data[10] << 8) | fifo_data[11];
01283         quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
01284             ((long)fifo_data[14] << 8) | fifo_data[15];
01285         ii += 16;
01286 #ifdef FIFO_CORRUPTION_CHECK
01287         /* We can detect a corrupted FIFO by monitoring the quaternion data and
01288          * ensuring that the magnitude is always normalized to one. This
01289          * shouldn't happen in normal operation, but if an I2C error occurs,
01290          * the FIFO reads might become misaligned.
01291          *
01292          * Let's start by scaling down the quaternion data to avoid long long
01293          * math.
01294          */
01295         quat_q14[0] = quat[0] >> 16;
01296         quat_q14[1] = quat[1] >> 16;
01297         quat_q14[2] = quat[2] >> 16;
01298         quat_q14[3] = quat[3] >> 16;
01299         quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
01300             quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
01301         if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
01302             (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
01303             /* Quaternion is outside of the acceptable threshold. */
01304             mpu_reset_fifo();
01305             sensors[0] = 0;
01306             return -2;
01307         }
01308         sensors[0] |= INV_WXYZ_QUAT;
01309 #endif
01310     }
01311 
01312     if (dmp->feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
01313         accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
01314         accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
01315         accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
01316         ii += 6;
01317         sensors[0] |= INV_XYZ_ACCEL;
01318     }
01319 
01320     if (dmp->feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
01321         gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
01322         gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
01323         gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
01324         ii += 6;
01325         sensors[0] |= INV_XYZ_GYRO;
01326     }
01327 #ifdef MPU_MAXIMAL
01328     /* Gesture data is at the end of the DMP packet. Parse it and call
01329      * the gesture callbacks (if registered).
01330      */
01331     if (dmp->feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
01332         decode_gesture(fifo_data + ii);
01333 #endif // MPU_MAXIMAL
01334     get_ms(timestamp);
01335     return 0;
01336 }
01337 
01338 #ifdef MPU_MAXIMAL
01339 
01351 int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
01352 {
01353     dmp->tap_cb = func;
01354     return 0;
01355 }
01356 
01362 int dmp_register_android_orient_cb(void (*func)(unsigned char))
01363 {
01364     dmp->android_orient_cb = func;
01365     return 0;
01366 }
01367 #endif // MPU_MAXIMAL
01368 


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:55