Embedded MotionDriver 6.1 Sourced from InvenSense
Diff: inv_mpu_dmp_motion_driver.c
- Revision:
- 0:5fa30cf392c3
diff -r 000000000000 -r 5fa30cf392c3 inv_mpu_dmp_motion_driver.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inv_mpu_dmp_motion_driver.c Sun Aug 24 00:52:07 2014 +0000 @@ -0,0 +1,1379 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @addtogroup DRIVERS Sensor Driver Layer + * @brief Hardware drivers to communicate with sensors via I2C. + * + * @{ + * @file inv_mpu_dmp_motion_driver.c + * @brief DMP image and interface functions. + * @details All functions are preceded by the dmp_ prefix to + * differentiate among MPL and general driver function calls. + */ +#include <stdio.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include "inv_mpu.h" +#include "inv_mpu_dmp_motion_driver.h" +#include "dmpKey.h" +#include "dmpmap.h" + +/* The following functions must be defined for this platform: + * i2c_write(unsigned char slave_addr, unsigned char reg_addr, + * unsigned char length, unsigned char const *data) + * i2c_read(unsigned char slave_addr, unsigned char reg_addr, + * unsigned char length, unsigned char *data) + * delay_ms(unsigned long num_ms) + * get_ms(unsigned long *count) + */ +#if defined EMPL_TARGET_STM32F4 +#include "i2c.h" +#include "main.h" +#include "board-st_discovery.h" + +#define i2c_write Sensors_I2C_WriteRegister +#define i2c_read Sensors_I2C_ReadRegister +#define get_ms get_tick_count + +#elif defined MOTION_DRIVER_TARGET_MSP430 +#include "msp430.h" +#include "msp430_clock.h" +#define delay_ms msp430_delay_ms +#define get_ms msp430_get_clock_ms +#define log_i(...) do {} while (0) +#define log_e(...) do {} while (0) + +#elif defined EMPL_TARGET_MSP430 +#include "msp430.h" +#include "msp430_clock.h" +#include "log.h" +#define delay_ms msp430_delay_ms +#define get_ms msp430_get_clock_ms +#define log_i MPL_LOGI +#define log_e MPL_LOGE + +#elif defined EMPL_TARGET_UC3L0 +/* Instead of using the standard TWI driver from the ASF library, we're using + * a TWI driver that follows the slave address + register address convention. + */ +#include "delay.h" +#include "sysclk.h" +#include "log.h" +#include "uc3l0_clock.h" +/* delay_ms is a function already defined in ASF. */ +#define get_ms uc3l0_get_clock_ms +#define log_i MPL_LOGI +#define log_e MPL_LOGE + +#else +#error Gyro driver is missing the system layer implementations. +#endif + +/* These defines are copied from dmpDefaultMPU6050.c in the general MPL + * releases. These defines may change for each DMP image, so be sure to modify + * these values when switching to a new image. + */ +#define CFG_LP_QUAT (2712) +#define END_ORIENT_TEMP (1866) +#define CFG_27 (2742) +#define CFG_20 (2224) +#define CFG_23 (2745) +#define CFG_FIFO_ON_EVENT (2690) +#define END_PREDICTION_UPDATE (1761) +#define CGNOTICE_INTR (2620) +#define X_GRT_Y_TMP (1358) +#define CFG_DR_INT (1029) +#define CFG_AUTH (1035) +#define UPDATE_PROP_ROT (1835) +#define END_COMPARE_Y_X_TMP2 (1455) +#define SKIP_X_GRT_Y_TMP (1359) +#define SKIP_END_COMPARE (1435) +#define FCFG_3 (1088) +#define FCFG_2 (1066) +#define FCFG_1 (1062) +#define END_COMPARE_Y_X_TMP3 (1434) +#define FCFG_7 (1073) +#define FCFG_6 (1106) +#define FLAT_STATE_END (1713) +#define SWING_END_4 (1616) +#define SWING_END_2 (1565) +#define SWING_END_3 (1587) +#define SWING_END_1 (1550) +#define CFG_8 (2718) +#define CFG_15 (2727) +#define CFG_16 (2746) +#define CFG_EXT_GYRO_BIAS (1189) +#define END_COMPARE_Y_X_TMP (1407) +#define DO_NOT_UPDATE_PROP_ROT (1839) +#define CFG_7 (1205) +#define FLAT_STATE_END_TEMP (1683) +#define END_COMPARE_Y_X (1484) +#define SKIP_SWING_END_1 (1551) +#define SKIP_SWING_END_3 (1588) +#define SKIP_SWING_END_2 (1566) +#define TILTG75_START (1672) +#define CFG_6 (2753) +#define TILTL75_END (1669) +#define END_ORIENT (1884) +#define CFG_FLICK_IN (2573) +#define TILTL75_START (1643) +#define CFG_MOTION_BIAS (1208) +#define X_GRT_Y (1408) +#define TEMPLABEL (2324) +#define CFG_ANDROID_ORIENT_INT (1853) +#define CFG_GYRO_RAW_DATA (2722) +#define X_GRT_Y_TMP2 (1379) + +#define D_0_22 (22+512) +#define D_0_24 (24+512) + +#define D_0_36 (36) +#define D_0_52 (52) +#define D_0_96 (96) +#define D_0_104 (104) +#define D_0_108 (108) +#define D_0_163 (163) +#define D_0_188 (188) +#define D_0_192 (192) +#define D_0_224 (224) +#define D_0_228 (228) +#define D_0_232 (232) +#define D_0_236 (236) + +#define D_1_2 (256 + 2) +#define D_1_4 (256 + 4) +#define D_1_8 (256 + 8) +#define D_1_10 (256 + 10) +#define D_1_24 (256 + 24) +#define D_1_28 (256 + 28) +#define D_1_36 (256 + 36) +#define D_1_40 (256 + 40) +#define D_1_44 (256 + 44) +#define D_1_72 (256 + 72) +#define D_1_74 (256 + 74) +#define D_1_79 (256 + 79) +#define D_1_88 (256 + 88) +#define D_1_90 (256 + 90) +#define D_1_92 (256 + 92) +#define D_1_96 (256 + 96) +#define D_1_98 (256 + 98) +#define D_1_106 (256 + 106) +#define D_1_108 (256 + 108) +#define D_1_112 (256 + 112) +#define D_1_128 (256 + 144) +#define D_1_152 (256 + 12) +#define D_1_160 (256 + 160) +#define D_1_176 (256 + 176) +#define D_1_178 (256 + 178) +#define D_1_218 (256 + 218) +#define D_1_232 (256 + 232) +#define D_1_236 (256 + 236) +#define D_1_240 (256 + 240) +#define D_1_244 (256 + 244) +#define D_1_250 (256 + 250) +#define D_1_252 (256 + 252) +#define D_2_12 (512 + 12) +#define D_2_96 (512 + 96) +#define D_2_108 (512 + 108) +#define D_2_208 (512 + 208) +#define D_2_224 (512 + 224) +#define D_2_236 (512 + 236) +#define D_2_244 (512 + 244) +#define D_2_248 (512 + 248) +#define D_2_252 (512 + 252) + +#define CPASS_BIAS_X (35 * 16 + 4) +#define CPASS_BIAS_Y (35 * 16 + 8) +#define CPASS_BIAS_Z (35 * 16 + 12) +#define CPASS_MTX_00 (36 * 16) +#define CPASS_MTX_01 (36 * 16 + 4) +#define CPASS_MTX_02 (36 * 16 + 8) +#define CPASS_MTX_10 (36 * 16 + 12) +#define CPASS_MTX_11 (37 * 16) +#define CPASS_MTX_12 (37 * 16 + 4) +#define CPASS_MTX_20 (37 * 16 + 8) +#define CPASS_MTX_21 (37 * 16 + 12) +#define CPASS_MTX_22 (43 * 16 + 12) +#define D_EXT_GYRO_BIAS_X (61 * 16) +#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 +#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 +#define D_ACT0 (40 * 16) +#define D_ACSX (40 * 16 + 4) +#define D_ACSY (40 * 16 + 8) +#define D_ACSZ (40 * 16 + 12) + +#define FLICK_MSG (45 * 16 + 4) +#define FLICK_COUNTER (45 * 16 + 8) +#define FLICK_LOWER (45 * 16 + 12) +#define FLICK_UPPER (46 * 16 + 12) + +#define D_AUTH_OUT (992) +#define D_AUTH_IN (996) +#define D_AUTH_A (1000) +#define D_AUTH_B (1004) + +#define D_PEDSTD_BP_B (768 + 0x1C) +#define D_PEDSTD_HP_A (768 + 0x78) +#define D_PEDSTD_HP_B (768 + 0x7C) +#define D_PEDSTD_BP_A4 (768 + 0x40) +#define D_PEDSTD_BP_A3 (768 + 0x44) +#define D_PEDSTD_BP_A2 (768 + 0x48) +#define D_PEDSTD_BP_A1 (768 + 0x4C) +#define D_PEDSTD_INT_THRSH (768 + 0x68) +#define D_PEDSTD_CLIP (768 + 0x6C) +#define D_PEDSTD_SB (768 + 0x28) +#define D_PEDSTD_SB_TIME (768 + 0x2C) +#define D_PEDSTD_PEAKTHRSH (768 + 0x98) +#define D_PEDSTD_TIML (768 + 0x2A) +#define D_PEDSTD_TIMH (768 + 0x2E) +#define D_PEDSTD_PEAK (768 + 0X94) +#define D_PEDSTD_STEPCTR (768 + 0x60) +#define D_PEDSTD_TIMECTR (964) +#define D_PEDSTD_DECI (768 + 0xA0) + +#define D_HOST_NO_MOT (976) +#define D_ACCEL_BIAS (660) + +#define D_ORIENT_GAP (76) + +#define D_TILT0_H (48) +#define D_TILT0_L (50) +#define D_TILT1_H (52) +#define D_TILT1_L (54) +#define D_TILT2_H (56) +#define D_TILT2_L (58) +#define D_TILT3_H (60) +#define D_TILT3_L (62) + +#define DMP_CODE_SIZE (3062) + +static const unsigned char dmp_memory[DMP_CODE_SIZE] = { + /* bank # 0 */ + 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, + 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, + 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, + 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, + 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, + 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, + 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, + 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, + 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, + 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, + 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 1 */ + 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, + 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, + 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, + 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, + 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, + 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, + 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, + /* bank # 2 */ + 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, + 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, + 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, + 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + /* bank # 4 */ + 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, + 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, + 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, + 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, + 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, + 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, + 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, + 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, + 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, + 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, + 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, + 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, + 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, + 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, + 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, + 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, + /* bank # 5 */ + 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, + 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, + 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, + 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, + 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, + 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, + 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, + 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, + 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, + 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, + 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, + 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, + 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, + 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, + 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, + 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, + /* bank # 6 */ + 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, + 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, + 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, + 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, + 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, + 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, + 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, + 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, + 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, + 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, + 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, + 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, + 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, + 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, + 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, + 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, + /* bank # 7 */ + 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, + 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, + 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, + 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, + 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, + 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, + 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, + 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, + 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, + 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, + 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, + 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, + 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, + 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, + 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, + 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, + /* bank # 8 */ + 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, + 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, + 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, + 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, + 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, + 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, + 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, + 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, + 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, + 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, + 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, + 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, + 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, + 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, + 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, + 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, + /* bank # 9 */ + 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, + 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, + 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, + 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, + 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, + 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, + 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, + 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, + 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, + 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, + 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, + 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, + 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, + 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, + 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, + 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, + /* bank # 10 */ + 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, + 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, + 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, + 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, + 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, + 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, + 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, + 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, + 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, + 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, + 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, + 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, + 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, + 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, + /* bank # 11 */ + 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, + 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, + 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, + 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, + 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, + 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, + 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, + 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, + 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, + 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, + 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, + 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, + 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, + 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, + 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, + 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff +}; + +static const unsigned short sStartAddress = 0x0400; + +/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ + +#define INT_SRC_TAP (0x01) +#define INT_SRC_ANDROID_ORIENT (0x08) + +#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ + DMP_FEATURE_SEND_CAL_GYRO) + +#define MAX_PACKET_LENGTH (32) + +#define DMP_SAMPLE_RATE (200) +#define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) + +#define FIFO_CORRUPTION_CHECK +#ifdef FIFO_CORRUPTION_CHECK +#define QUAT_ERROR_THRESH (1L<<24) +#define QUAT_MAG_SQ_NORMALIZED (1L<<28) +#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) +#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) +#endif + +struct dmp_s { + void (*tap_cb)(unsigned char count, unsigned char direction); + void (*android_orient_cb)(unsigned char orientation); + unsigned short orient; + unsigned short feature_mask; + unsigned short fifo_rate; + unsigned char packet_length; +}; + +static struct dmp_s dmp = { + .tap_cb = NULL, + .android_orient_cb = NULL, + .orient = 0, + .feature_mask = 0, + .fifo_rate = 0, + .packet_length = 0 +}; + +/** + * @brief Load the DMP with this image. + * @return 0 if successful. + */ +int dmp_load_motion_driver_firmware(void) +{ + return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, + DMP_SAMPLE_RATE); +} + +/** + * @brief Push gyro and accel orientation to the DMP. + * The orientation is represented here as the output of + * @e inv_orientation_matrix_to_scalar. + * @param[in] orient Gyro and accel orientation in body frame. + * @return 0 if successful. + */ +int dmp_set_orientation(unsigned short orient) +{ + unsigned char gyro_regs[3], accel_regs[3]; + const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; + const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; + const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; + const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; + + gyro_regs[0] = gyro_axes[orient & 3]; + gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; + gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; + accel_regs[0] = accel_axes[orient & 3]; + accel_regs[1] = accel_axes[(orient >> 3) & 3]; + accel_regs[2] = accel_axes[(orient >> 6) & 3]; + + /* Chip-to-body, axes only. */ + if (mpu_write_mem(FCFG_1, 3, gyro_regs)) + return -1; + if (mpu_write_mem(FCFG_2, 3, accel_regs)) + return -1; + + memcpy(gyro_regs, gyro_sign, 3); + memcpy(accel_regs, accel_sign, 3); + if (orient & 4) { + gyro_regs[0] |= 1; + accel_regs[0] |= 1; + } + if (orient & 0x20) { + gyro_regs[1] |= 1; + accel_regs[1] |= 1; + } + if (orient & 0x100) { + gyro_regs[2] |= 1; + accel_regs[2] |= 1; + } + + /* Chip-to-body, sign only. */ + if (mpu_write_mem(FCFG_3, 3, gyro_regs)) + return -1; + if (mpu_write_mem(FCFG_7, 3, accel_regs)) + return -1; + dmp.orient = orient; + return 0; +} + +/** + * @brief Push gyro biases to the DMP. + * Because the gyro integration is handled in the DMP, any gyro biases + * calculated by the MPL should be pushed down to DMP memory to remove + * 3-axis quaternion drift. + * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will + * overwrite the biases written to this location once a new one is computed. + * @param[in] bias Gyro biases in q16. + * @return 0 if successful. + */ +int dmp_set_gyro_bias(long *bias) +{ + long gyro_bias_body[3]; + unsigned char regs[4]; + + gyro_bias_body[0] = bias[dmp.orient & 3]; + if (dmp.orient & 4) + gyro_bias_body[0] *= -1; + gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; + if (dmp.orient & 0x20) + gyro_bias_body[1] *= -1; + gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; + if (dmp.orient & 0x100) + gyro_bias_body[2] *= -1; + +#ifdef EMPL_NO_64BIT + gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); + gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); + gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); +#else + gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); + gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); + gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); +#endif + + regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); + regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); + regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); + regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); + if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) + return -1; + + regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); + regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); + regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); + regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); + if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) + return -1; + + regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); + regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); + regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); + regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); + return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); +} + +/** + * @brief Push accel biases to the DMP. + * These biases will be removed from the DMP 6-axis quaternion. + * @param[in] bias Accel biases in q16. + * @return 0 if successful. + */ +int dmp_set_accel_bias(long *bias) +{ + long accel_bias_body[3]; + unsigned char regs[12]; + long long accel_sf; + unsigned short accel_sens; + + mpu_get_accel_sens(&accel_sens); + accel_sf = (long long)accel_sens << 15; + __no_operation(); + + accel_bias_body[0] = bias[dmp.orient & 3]; + if (dmp.orient & 4) + accel_bias_body[0] *= -1; + accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; + if (dmp.orient & 0x20) + accel_bias_body[1] *= -1; + accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; + if (dmp.orient & 0x100) + accel_bias_body[2] *= -1; + +#ifdef EMPL_NO_64BIT + accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); + accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); + accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); +#else + accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); + accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); + accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); +#endif + + regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); + regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); + regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); + regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); + regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); + regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); + regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); + regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); + regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); + regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); + regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); + regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); + return mpu_write_mem(D_ACCEL_BIAS, 12, regs); +} + +/** + * @brief Set DMP output rate. + * Only used when DMP is on. + * @param[in] rate Desired fifo rate (Hz). + * @return 0 if successful. + */ +int dmp_set_fifo_rate(unsigned short rate) +{ + const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, + 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; + unsigned short div; + unsigned char tmp[8]; + + if (rate > DMP_SAMPLE_RATE) + return -1; + div = DMP_SAMPLE_RATE / rate - 1; + tmp[0] = (unsigned char)((div >> 8) & 0xFF); + tmp[1] = (unsigned char)(div & 0xFF); + if (mpu_write_mem(D_0_22, 2, tmp)) + return -1; + if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) + return -1; + + dmp.fifo_rate = rate; + return 0; +} + +/** + * @brief Get DMP output rate. + * @param[out] rate Current fifo rate (Hz). + * @return 0 if successful. + */ +int dmp_get_fifo_rate(unsigned short *rate) +{ + rate[0] = dmp.fifo_rate; + return 0; +} + +/** + * @brief Set tap threshold for a specific axis. + * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. + * @param[in] thresh Tap threshold, in mg/ms. + * @return 0 if successful. + */ +int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) +{ + unsigned char tmp[4], accel_fsr; + float scaled_thresh; + unsigned short dmp_thresh, dmp_thresh_2; + if (!(axis & TAP_XYZ) || thresh > 1600) + return -1; + + scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; + + mpu_get_accel_fsr(&accel_fsr); + switch (accel_fsr) { + case 2: + dmp_thresh = (unsigned short)(scaled_thresh * 16384); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); + break; + case 4: + dmp_thresh = (unsigned short)(scaled_thresh * 8192); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); + break; + case 8: + dmp_thresh = (unsigned short)(scaled_thresh * 4096); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); + break; + case 16: + dmp_thresh = (unsigned short)(scaled_thresh * 2048); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); + break; + default: + return -1; + } + tmp[0] = (unsigned char)(dmp_thresh >> 8); + tmp[1] = (unsigned char)(dmp_thresh & 0xFF); + tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); + tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); + + if (axis & TAP_X) { + if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) + return -1; + if (mpu_write_mem(D_1_36, 2, tmp+2)) + return -1; + } + if (axis & TAP_Y) { + if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) + return -1; + if (mpu_write_mem(D_1_40, 2, tmp+2)) + return -1; + } + if (axis & TAP_Z) { + if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) + return -1; + if (mpu_write_mem(D_1_44, 2, tmp+2)) + return -1; + } + return 0; +} + +/** + * @brief Set which axes will register a tap. + * @param[in] axis 1, 2, and 4 for XYZ, respectively. + * @return 0 if successful. + */ +int dmp_set_tap_axes(unsigned char axis) +{ + unsigned char tmp = 0; + + if (axis & TAP_X) + tmp |= 0x30; + if (axis & TAP_Y) + tmp |= 0x0C; + if (axis & TAP_Z) + tmp |= 0x03; + return mpu_write_mem(D_1_72, 1, &tmp); +} + +/** + * @brief Set minimum number of taps needed for an interrupt. + * @param[in] min_taps Minimum consecutive taps (1-4). + * @return 0 if successful. + */ +int dmp_set_tap_count(unsigned char min_taps) +{ + unsigned char tmp; + + if (min_taps < 1) + min_taps = 1; + else if (min_taps > 4) + min_taps = 4; + + tmp = min_taps - 1; + return mpu_write_mem(D_1_79, 1, &tmp); +} + +/** + * @brief Set length between valid taps. + * @param[in] time Milliseconds between taps. + * @return 0 if successful. + */ +int dmp_set_tap_time(unsigned short time) +{ + unsigned short dmp_time; + unsigned char tmp[2]; + + dmp_time = time / (1000 / DMP_SAMPLE_RATE); + tmp[0] = (unsigned char)(dmp_time >> 8); + tmp[1] = (unsigned char)(dmp_time & 0xFF); + return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); +} + +/** + * @brief Set max time between taps to register as a multi-tap. + * @param[in] time Max milliseconds between taps. + * @return 0 if successful. + */ +int dmp_set_tap_time_multi(unsigned short time) +{ + unsigned short dmp_time; + unsigned char tmp[2]; + + dmp_time = time / (1000 / DMP_SAMPLE_RATE); + tmp[0] = (unsigned char)(dmp_time >> 8); + tmp[1] = (unsigned char)(dmp_time & 0xFF); + return mpu_write_mem(D_1_218, 2, tmp); +} + +/** + * @brief Set shake rejection threshold. + * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. + * @param[in] sf Gyro scale factor. + * @param[in] thresh Gyro threshold in dps. + * @return 0 if successful. + */ +int dmp_set_shake_reject_thresh(long sf, unsigned short thresh) +{ + unsigned char tmp[4]; + long thresh_scaled = sf / 1000 * thresh; + tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); + tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); + tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); + tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); + return mpu_write_mem(D_1_92, 4, tmp); +} + +/** + * @brief Set shake rejection time. + * Sets the length of time that the gyro must be outside of the threshold set + * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory + * 60 ms is added to this parameter. + * @param[in] time Time in milliseconds. + * @return 0 if successful. + */ +int dmp_set_shake_reject_time(unsigned short time) +{ + unsigned char tmp[2]; + + time /= (1000 / DMP_SAMPLE_RATE); + tmp[0] = time >> 8; + tmp[1] = time & 0xFF; + return mpu_write_mem(D_1_90,2,tmp); +} + +/** + * @brief Set shake rejection timeout. + * Sets the length of time after a shake rejection that the gyro must stay + * inside of the threshold before taps can be detected again. A mandatory + * 60 ms is added to this parameter. + * @param[in] time Time in milliseconds. + * @return 0 if successful. + */ +int dmp_set_shake_reject_timeout(unsigned short time) +{ + unsigned char tmp[2]; + + time /= (1000 / DMP_SAMPLE_RATE); + tmp[0] = time >> 8; + tmp[1] = time & 0xFF; + return mpu_write_mem(D_1_88,2,tmp); +} + +/** + * @brief Get current step count. + * @param[out] count Number of steps detected. + * @return 0 if successful. + */ +int dmp_get_pedometer_step_count(unsigned long *count) +{ + unsigned char tmp[4]; + if (!count) + return -1; + + if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) + return -1; + + count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | + ((unsigned long)tmp[2] << 8) | tmp[3]; + return 0; +} + +/** + * @brief Overwrite current step count. + * WARNING: This function writes to DMP memory and could potentially encounter + * a race condition if called while the pedometer is enabled. + * @param[in] count New step count. + * @return 0 if successful. + */ +int dmp_set_pedometer_step_count(unsigned long count) +{ + unsigned char tmp[4]; + + tmp[0] = (unsigned char)((count >> 24) & 0xFF); + tmp[1] = (unsigned char)((count >> 16) & 0xFF); + tmp[2] = (unsigned char)((count >> 8) & 0xFF); + tmp[3] = (unsigned char)(count & 0xFF); + return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); +} + +/** + * @brief Get duration of walking time. + * @param[in] time Walk time in milliseconds. + * @return 0 if successful. + */ +int dmp_get_pedometer_walk_time(unsigned long *time) +{ + unsigned char tmp[4]; + if (!time) + return -1; + + if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) + return -1; + + time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | + ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; + return 0; +} + +/** + * @brief Overwrite current walk time. + * WARNING: This function writes to DMP memory and could potentially encounter + * a race condition if called while the pedometer is enabled. + * @param[in] time New walk time in milliseconds. + */ +int dmp_set_pedometer_walk_time(unsigned long time) +{ + unsigned char tmp[4]; + + time /= 20; + + tmp[0] = (unsigned char)((time >> 24) & 0xFF); + tmp[1] = (unsigned char)((time >> 16) & 0xFF); + tmp[2] = (unsigned char)((time >> 8) & 0xFF); + tmp[3] = (unsigned char)(time & 0xFF); + return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); +} + +/** + * @brief Enable DMP features. + * The following \#define's are used in the input mask: + * \n DMP_FEATURE_TAP + * \n DMP_FEATURE_ANDROID_ORIENT + * \n DMP_FEATURE_LP_QUAT + * \n DMP_FEATURE_6X_LP_QUAT + * \n DMP_FEATURE_GYRO_CAL + * \n DMP_FEATURE_SEND_RAW_ACCEL + * \n DMP_FEATURE_SEND_RAW_GYRO + * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually + * exclusive. + * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also + * mutually exclusive. + * @param[in] mask Mask of features to enable. + * @return 0 if successful. + */ +int dmp_enable_feature(unsigned short mask) +{ + unsigned char tmp[10]; + + /* TODO: All of these settings can probably be integrated into the default + * DMP image. + */ + /* Set integration scale factor. */ + tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); + tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); + tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); + tmp[3] = (unsigned char)(GYRO_SF & 0xFF); + mpu_write_mem(D_0_104, 4, tmp); + + /* Send sensor data to the FIFO. */ + tmp[0] = 0xA3; + if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { + tmp[1] = 0xC0; + tmp[2] = 0xC8; + tmp[3] = 0xC2; + } else { + tmp[1] = 0xA3; + tmp[2] = 0xA3; + tmp[3] = 0xA3; + } + if (mask & DMP_FEATURE_SEND_ANY_GYRO) { + tmp[4] = 0xC4; + tmp[5] = 0xCC; + tmp[6] = 0xC6; + } else { + tmp[4] = 0xA3; + tmp[5] = 0xA3; + tmp[6] = 0xA3; + } + tmp[7] = 0xA3; + tmp[8] = 0xA3; + tmp[9] = 0xA3; + mpu_write_mem(CFG_15,10,tmp); + + /* Send gesture data to the FIFO. */ + if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) + tmp[0] = DINA20; + else + tmp[0] = 0xD8; + mpu_write_mem(CFG_27,1,tmp); + + if (mask & DMP_FEATURE_GYRO_CAL) + dmp_enable_gyro_cal(1); + else + dmp_enable_gyro_cal(0); + + if (mask & DMP_FEATURE_SEND_ANY_GYRO) { + if (mask & DMP_FEATURE_SEND_CAL_GYRO) { + tmp[0] = 0xB2; + tmp[1] = 0x8B; + tmp[2] = 0xB6; + tmp[3] = 0x9B; + } else { + tmp[0] = DINAC0; + tmp[1] = DINA80; + tmp[2] = DINAC2; + tmp[3] = DINA90; + } + mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); + } + + if (mask & DMP_FEATURE_TAP) { + /* Enable tap. */ + tmp[0] = 0xF8; + mpu_write_mem(CFG_20, 1, tmp); + dmp_set_tap_thresh(TAP_XYZ, 250); + dmp_set_tap_axes(TAP_XYZ); + dmp_set_tap_count(1); + dmp_set_tap_time(100); + dmp_set_tap_time_multi(500); + + dmp_set_shake_reject_thresh(GYRO_SF, 200); + dmp_set_shake_reject_time(40); + dmp_set_shake_reject_timeout(10); + } else { + tmp[0] = 0xD8; + mpu_write_mem(CFG_20, 1, tmp); + } + + if (mask & DMP_FEATURE_ANDROID_ORIENT) { + tmp[0] = 0xD9; + } else + tmp[0] = 0xD8; + mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); + + if (mask & DMP_FEATURE_LP_QUAT) + dmp_enable_lp_quat(1); + else + dmp_enable_lp_quat(0); + + if (mask & DMP_FEATURE_6X_LP_QUAT) + dmp_enable_6x_lp_quat(1); + else + dmp_enable_6x_lp_quat(0); + + /* Pedometer is always enabled. */ + dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; + mpu_reset_fifo(); + + dmp.packet_length = 0; + if (mask & DMP_FEATURE_SEND_RAW_ACCEL) + dmp.packet_length += 6; + if (mask & DMP_FEATURE_SEND_ANY_GYRO) + dmp.packet_length += 6; + if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) + dmp.packet_length += 16; + if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) + dmp.packet_length += 4; + + return 0; +} + +/** + * @brief Get list of currently enabled DMP features. + * @param[out] Mask of enabled features. + * @return 0 if successful. + */ +int dmp_get_enabled_features(unsigned short *mask) +{ + mask[0] = dmp.feature_mask; + return 0; +} + +/** + * @brief Calibrate the gyro data in the DMP. + * After eight seconds of no motion, the DMP will compute gyro biases and + * subtract them from the quaternion output. If @e dmp_enable_feature is + * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be + * subtracted from the gyro output. + * @param[in] enable 1 to enable gyro calibration. + * @return 0 if successful. + */ +int dmp_enable_gyro_cal(unsigned char enable) +{ + if (enable) { + unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; + return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); + } else { + unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; + return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); + } +} + +/** + * @brief Generate 3-axis quaternions from the DMP. + * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually + * exclusive. + * @param[in] enable 1 to enable 3-axis quaternion. + * @return 0 if successful. + */ +int dmp_enable_lp_quat(unsigned char enable) +{ + unsigned char regs[4]; + if (enable) { + regs[0] = DINBC0; + regs[1] = DINBC2; + regs[2] = DINBC4; + regs[3] = DINBC6; + } + else + memset(regs, 0x8B, 4); + + mpu_write_mem(CFG_LP_QUAT, 4, regs); + + return mpu_reset_fifo(); +} + +/** + * @brief Generate 6-axis quaternions from the DMP. + * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually + * exclusive. + * @param[in] enable 1 to enable 6-axis quaternion. + * @return 0 if successful. + */ +int dmp_enable_6x_lp_quat(unsigned char enable) +{ + unsigned char regs[4]; + if (enable) { + regs[0] = DINA20; + regs[1] = DINA28; + regs[2] = DINA30; + regs[3] = DINA38; + } else + memset(regs, 0xA3, 4); + + mpu_write_mem(CFG_8, 4, regs); + + return mpu_reset_fifo(); +} + +/** + * @brief Decode the four-byte gesture data and execute any callbacks. + * @param[in] gesture Gesture data from DMP packet. + * @return 0 if successful. + */ +static int decode_gesture(unsigned char *gesture) +{ + unsigned char tap, android_orient; + + android_orient = gesture[3] & 0xC0; + tap = 0x3F & gesture[3]; + + if (gesture[1] & INT_SRC_TAP) { + unsigned char direction, count; + direction = tap >> 3; + count = (tap % 8) + 1; + if (dmp.tap_cb) + dmp.tap_cb(direction, count); + } + + if (gesture[1] & INT_SRC_ANDROID_ORIENT) { + if (dmp.android_orient_cb) + dmp.android_orient_cb(android_orient >> 6); + } + + return 0; +} + +/** + * @brief Specify when a DMP interrupt should occur. + * A DMP interrupt can be configured to trigger on either of the two + * conditions below: + * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). + * \n b. A tap event has been detected. + * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. + * @return 0 if successful. + */ +int dmp_set_interrupt_mode(unsigned char mode) +{ + const unsigned char regs_continuous[11] = + {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; + const unsigned char regs_gesture[11] = + {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; + + switch (mode) { + case DMP_INT_CONTINUOUS: + return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, + (unsigned char*)regs_continuous); + case DMP_INT_GESTURE: + return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, + (unsigned char*)regs_gesture); + default: + return -1; + } +} + +/** + * @brief Get one packet from the FIFO. + * If @e sensors does not contain a particular sensor, disregard the data + * returned to that pointer. + * \n @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * \n INV_WXYZ_QUAT + * \n If the FIFO has no new data, @e sensors will be zero. + * \n If the FIFO is disabled, @e sensors will be zero and this function will + * return a non-zero error code. + * @param[out] gyro Gyro data in hardware units. + * @param[out] accel Accel data in hardware units. + * @param[out] quat 3-axis quaternion data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. + * @param[out] sensors Mask of sensors read from FIFO. + * @param[out] more Number of remaining packets. + * @return 0 if successful. + */ +int dmp_read_fifo(short *gyro, short *accel, long *quat, + unsigned long *timestamp, short *sensors, unsigned char *more) +{ + unsigned char fifo_data[MAX_PACKET_LENGTH]; + unsigned char ii = 0; + + /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can + * cache this value and save some cycles. + */ + sensors[0] = 0; + + /* Get a packet. */ + if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) + return -1; + + /* Parse DMP packet. */ + if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { +#ifdef FIFO_CORRUPTION_CHECK + long quat_q14[4], quat_mag_sq; +#endif + quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | + ((long)fifo_data[2] << 8) | fifo_data[3]; + quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | + ((long)fifo_data[6] << 8) | fifo_data[7]; + quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | + ((long)fifo_data[10] << 8) | fifo_data[11]; + quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | + ((long)fifo_data[14] << 8) | fifo_data[15]; + ii += 16; +#ifdef FIFO_CORRUPTION_CHECK + /* We can detect a corrupted FIFO by monitoring the quaternion data and + * ensuring that the magnitude is always normalized to one. This + * shouldn't happen in normal operation, but if an I2C error occurs, + * the FIFO reads might become misaligned. + * + * Let's start by scaling down the quaternion data to avoid long long + * math. + */ + quat_q14[0] = quat[0] >> 16; + quat_q14[1] = quat[1] >> 16; + quat_q14[2] = quat[2] >> 16; + quat_q14[3] = quat[3] >> 16; + quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + + quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; + if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || + (quat_mag_sq > QUAT_MAG_SQ_MAX)) { + /* Quaternion is outside of the acceptable threshold. */ + mpu_reset_fifo(); + sensors[0] = 0; + return -1; + } + sensors[0] |= INV_WXYZ_QUAT; +#endif + } + + if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { + accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; + accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; + accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; + ii += 6; + sensors[0] |= INV_XYZ_ACCEL; + } + + if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { + gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; + gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; + gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; + ii += 6; + sensors[0] |= INV_XYZ_GYRO; + } + + /* Gesture data is at the end of the DMP packet. Parse it and call + * the gesture callbacks (if registered). + */ + if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) + decode_gesture(fifo_data + ii); + + get_ms(timestamp); + return 0; +} + +/** + * @brief Register a function to be executed on a tap event. + * The tap direction is represented by one of the following: + * \n TAP_X_UP + * \n TAP_X_DOWN + * \n TAP_Y_UP + * \n TAP_Y_DOWN + * \n TAP_Z_UP + * \n TAP_Z_DOWN + * @param[in] func Callback function. + * @return 0 if successful. + */ +int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) +{ + dmp.tap_cb = func; + return 0; +} + +/** + * @brief Register a function to be executed on a android orientation event. + * @param[in] func Callback function. + * @return 0 if successful. + */ +int dmp_register_android_orient_cb(void (*func)(unsigned char)) +{ + dmp.android_orient_cb = func; + return 0; +} + +/** + * @} + */ + +