S Classen / eMPL_MPU6050

fork of the Seeed Studio port of the InvenSense MPU6050 driver

Fork of eMPL_MPU6050 by Seeed

inv_mpu_dmp_motion_driver.c@2:9f98336fa3cc, 2016-03-10 (annotated)

Committer:
sclassen
Date:
Thu Mar 10 05:34:59 2016 +0000
Revision:
2:9f98336fa3cc
Parent:
0:1b6dab73c06b 
avoid prints to stdio

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:1b6dab73c06b 1 /*
yihui 0:1b6dab73c06b 2 $License:
yihui 0:1b6dab73c06b 3 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
yihui 0:1b6dab73c06b 4 See included License.txt for License information.
yihui 0:1b6dab73c06b 5 $
yihui 0:1b6dab73c06b 6 */
yihui 0:1b6dab73c06b 7 /**
yihui 0:1b6dab73c06b 8 * @addtogroup DRIVERS Sensor Driver Layer
yihui 0:1b6dab73c06b 9 * @brief Hardware drivers to communicate with sensors via I2C.
yihui 0:1b6dab73c06b 10 *
yihui 0:1b6dab73c06b 11 * @{
yihui 0:1b6dab73c06b 12 * @file inv_mpu_dmp_motion_driver.c
yihui 0:1b6dab73c06b 13 * @brief DMP image and interface functions.
yihui 0:1b6dab73c06b 14 * @details All functions are preceded by the dmp_ prefix to
yihui 0:1b6dab73c06b 15 * differentiate among MPL and general driver function calls.
yihui 0:1b6dab73c06b 16 */
yihui 0:1b6dab73c06b 17 #include <stdio.h>
yihui 0:1b6dab73c06b 18 #include <stdint.h>
yihui 0:1b6dab73c06b 19 #include <stdlib.h>
yihui 0:1b6dab73c06b 20 #include <string.h>
yihui 0:1b6dab73c06b 21 #include <math.h>
yihui 0:1b6dab73c06b 22 #include "inv_mpu.h"
yihui 0:1b6dab73c06b 23 #include "inv_mpu_dmp_motion_driver.h"
yihui 0:1b6dab73c06b 24 #include "dmpKey.h"
yihui 0:1b6dab73c06b 25 #include "dmpmap.h"
yihui 0:1b6dab73c06b 26
yihui 0:1b6dab73c06b 27 /* The following functions must be defined for this platform:
yihui 0:1b6dab73c06b 28 * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
yihui 0:1b6dab73c06b 29 * unsigned char length, unsigned char const *data)
yihui 0:1b6dab73c06b 30 * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
yihui 0:1b6dab73c06b 31 * unsigned char length, unsigned char *data)
yihui 0:1b6dab73c06b 32 * delay_ms(unsigned long num_ms)
yihui 0:1b6dab73c06b 33 * get_ms(unsigned long *count)
yihui 0:1b6dab73c06b 34 */
yihui 0:1b6dab73c06b 35 #if defined MOTION_DRIVER_TARGET_MSP430
yihui 0:1b6dab73c06b 36 #include "msp430.h"
yihui 0:1b6dab73c06b 37 #include "msp430_clock.h"
yihui 0:1b6dab73c06b 38 #define delay_ms msp430_delay_ms
yihui 0:1b6dab73c06b 39 #define get_ms msp430_get_clock_ms
yihui 0:1b6dab73c06b 40 #define log_i(...) do {} while (0)
yihui 0:1b6dab73c06b 41 #define log_e(...) do {} while (0)
yihui 0:1b6dab73c06b 42
yihui 0:1b6dab73c06b 43 #elif defined EMPL_TARGET_MSP430
yihui 0:1b6dab73c06b 44 #include "msp430.h"
yihui 0:1b6dab73c06b 45 #include "msp430_clock.h"
yihui 0:1b6dab73c06b 46 #include "log.h"
yihui 0:1b6dab73c06b 47 #define delay_ms msp430_delay_ms
yihui 0:1b6dab73c06b 48 #define get_ms msp430_get_clock_ms
yihui 0:1b6dab73c06b 49 #define log_i MPL_LOGI
yihui 0:1b6dab73c06b 50 #define log_e MPL_LOGE
yihui 0:1b6dab73c06b 51
yihui 0:1b6dab73c06b 52 #elif defined EMPL_TARGET_UC3L0
yihui 0:1b6dab73c06b 53 /* Instead of using the standard TWI driver from the ASF library, we're using
yihui 0:1b6dab73c06b 54 * a TWI driver that follows the slave address + register address convention.
yihui 0:1b6dab73c06b 55 */
yihui 0:1b6dab73c06b 56 #include "delay.h"
yihui 0:1b6dab73c06b 57 #include "sysclk.h"
yihui 0:1b6dab73c06b 58 #include "log.h"
yihui 0:1b6dab73c06b 59 #include "uc3l0_clock.h"
yihui 0:1b6dab73c06b 60 /* delay_ms is a function already defined in ASF. */
yihui 0:1b6dab73c06b 61 #define get_ms uc3l0_get_clock_ms
yihui 0:1b6dab73c06b 62 #define log_i MPL_LOGI
yihui 0:1b6dab73c06b 63 #define log_e MPL_LOGE
yihui 0:1b6dab73c06b 64
yihui 0:1b6dab73c06b 65 #elif defined __MBED__
yihui 0:1b6dab73c06b 66 #include "wait_api.h"
yihui 0:1b6dab73c06b 67 #include "us_ticker_api.h"
yihui 0:1b6dab73c06b 68
yihui 0:1b6dab73c06b 69 #define __no_operation __nop
yihui 0:1b6dab73c06b 70 #define delay_ms wait_ms
yihui 0:1b6dab73c06b 71 static inline unsigned long get_ms(unsigned long *t)
yihui 0:1b6dab73c06b 72 {
yihui 0:1b6dab73c06b 73 unsigned long ms = us_ticker_read() / 1000;
yihui 0:1b6dab73c06b 74 *t = ms;
yihui 0:1b6dab73c06b 75 return ms;
yihui 0:1b6dab73c06b 76 }
yihui 0:1b6dab73c06b 77
yihui 0:1b6dab73c06b 78 #define log_i printf
yihui 0:1b6dab73c06b 79 #define log_e printf
yihui 0:1b6dab73c06b 80
yihui 0:1b6dab73c06b 81 #else
yihui 0:1b6dab73c06b 82 #error Gyro driver is missing the system layer implementations.
yihui 0:1b6dab73c06b 83 #endif
yihui 0:1b6dab73c06b 84
yihui 0:1b6dab73c06b 85 /* These defines are copied from dmpDefaultMPU6050.c in the general MPL
yihui 0:1b6dab73c06b 86 * releases. These defines may change for each DMP image, so be sure to modify
yihui 0:1b6dab73c06b 87 * these values when switching to a new image.
yihui 0:1b6dab73c06b 88 */
yihui 0:1b6dab73c06b 89 #define CFG_LP_QUAT (2712)
yihui 0:1b6dab73c06b 90 #define END_ORIENT_TEMP (1866)
yihui 0:1b6dab73c06b 91 #define CFG_27 (2742)
yihui 0:1b6dab73c06b 92 #define CFG_20 (2224)
yihui 0:1b6dab73c06b 93 #define CFG_23 (2745)
yihui 0:1b6dab73c06b 94 #define CFG_FIFO_ON_EVENT (2690)
yihui 0:1b6dab73c06b 95 #define END_PREDICTION_UPDATE (1761)
yihui 0:1b6dab73c06b 96 #define CGNOTICE_INTR (2620)
yihui 0:1b6dab73c06b 97 #define X_GRT_Y_TMP (1358)
yihui 0:1b6dab73c06b 98 #define CFG_DR_INT (1029)
yihui 0:1b6dab73c06b 99 #define CFG_AUTH (1035)
yihui 0:1b6dab73c06b 100 #define UPDATE_PROP_ROT (1835)
yihui 0:1b6dab73c06b 101 #define END_COMPARE_Y_X_TMP2 (1455)
yihui 0:1b6dab73c06b 102 #define SKIP_X_GRT_Y_TMP (1359)
yihui 0:1b6dab73c06b 103 #define SKIP_END_COMPARE (1435)
yihui 0:1b6dab73c06b 104 #define FCFG_3 (1088)
yihui 0:1b6dab73c06b 105 #define FCFG_2 (1066)
yihui 0:1b6dab73c06b 106 #define FCFG_1 (1062)
yihui 0:1b6dab73c06b 107 #define END_COMPARE_Y_X_TMP3 (1434)
yihui 0:1b6dab73c06b 108 #define FCFG_7 (1073)
yihui 0:1b6dab73c06b 109 #define FCFG_6 (1106)
yihui 0:1b6dab73c06b 110 #define FLAT_STATE_END (1713)
yihui 0:1b6dab73c06b 111 #define SWING_END_4 (1616)
yihui 0:1b6dab73c06b 112 #define SWING_END_2 (1565)
yihui 0:1b6dab73c06b 113 #define SWING_END_3 (1587)
yihui 0:1b6dab73c06b 114 #define SWING_END_1 (1550)
yihui 0:1b6dab73c06b 115 #define CFG_8 (2718)
yihui 0:1b6dab73c06b 116 #define CFG_15 (2727)
yihui 0:1b6dab73c06b 117 #define CFG_16 (2746)
yihui 0:1b6dab73c06b 118 #define CFG_EXT_GYRO_BIAS (1189)
yihui 0:1b6dab73c06b 119 #define END_COMPARE_Y_X_TMP (1407)
yihui 0:1b6dab73c06b 120 #define DO_NOT_UPDATE_PROP_ROT (1839)
yihui 0:1b6dab73c06b 121 #define CFG_7 (1205)
yihui 0:1b6dab73c06b 122 #define FLAT_STATE_END_TEMP (1683)
yihui 0:1b6dab73c06b 123 #define END_COMPARE_Y_X (1484)
yihui 0:1b6dab73c06b 124 #define SKIP_SWING_END_1 (1551)
yihui 0:1b6dab73c06b 125 #define SKIP_SWING_END_3 (1588)
yihui 0:1b6dab73c06b 126 #define SKIP_SWING_END_2 (1566)
yihui 0:1b6dab73c06b 127 #define TILTG75_START (1672)
yihui 0:1b6dab73c06b 128 #define CFG_6 (2753)
yihui 0:1b6dab73c06b 129 #define TILTL75_END (1669)
yihui 0:1b6dab73c06b 130 #define END_ORIENT (1884)
yihui 0:1b6dab73c06b 131 #define CFG_FLICK_IN (2573)
yihui 0:1b6dab73c06b 132 #define TILTL75_START (1643)
yihui 0:1b6dab73c06b 133 #define CFG_MOTION_BIAS (1208)
yihui 0:1b6dab73c06b 134 #define X_GRT_Y (1408)
yihui 0:1b6dab73c06b 135 #define TEMPLABEL (2324)
yihui 0:1b6dab73c06b 136 #define CFG_ANDROID_ORIENT_INT (1853)
yihui 0:1b6dab73c06b 137 #define CFG_GYRO_RAW_DATA (2722)
yihui 0:1b6dab73c06b 138 #define X_GRT_Y_TMP2 (1379)
yihui 0:1b6dab73c06b 139
yihui 0:1b6dab73c06b 140 #define D_0_22 (22+512)
yihui 0:1b6dab73c06b 141 #define D_0_24 (24+512)
yihui 0:1b6dab73c06b 142
yihui 0:1b6dab73c06b 143 #define D_0_36 (36)
yihui 0:1b6dab73c06b 144 #define D_0_52 (52)
yihui 0:1b6dab73c06b 145 #define D_0_96 (96)
yihui 0:1b6dab73c06b 146 #define D_0_104 (104)
yihui 0:1b6dab73c06b 147 #define D_0_108 (108)
yihui 0:1b6dab73c06b 148 #define D_0_163 (163)
yihui 0:1b6dab73c06b 149 #define D_0_188 (188)
yihui 0:1b6dab73c06b 150 #define D_0_192 (192)
yihui 0:1b6dab73c06b 151 #define D_0_224 (224)
yihui 0:1b6dab73c06b 152 #define D_0_228 (228)
yihui 0:1b6dab73c06b 153 #define D_0_232 (232)
yihui 0:1b6dab73c06b 154 #define D_0_236 (236)
yihui 0:1b6dab73c06b 155
yihui 0:1b6dab73c06b 156 #define D_1_2 (256 + 2)
yihui 0:1b6dab73c06b 157 #define D_1_4 (256 + 4)
yihui 0:1b6dab73c06b 158 #define D_1_8 (256 + 8)
yihui 0:1b6dab73c06b 159 #define D_1_10 (256 + 10)
yihui 0:1b6dab73c06b 160 #define D_1_24 (256 + 24)
yihui 0:1b6dab73c06b 161 #define D_1_28 (256 + 28)
yihui 0:1b6dab73c06b 162 #define D_1_36 (256 + 36)
yihui 0:1b6dab73c06b 163 #define D_1_40 (256 + 40)
yihui 0:1b6dab73c06b 164 #define D_1_44 (256 + 44)
yihui 0:1b6dab73c06b 165 #define D_1_72 (256 + 72)
yihui 0:1b6dab73c06b 166 #define D_1_74 (256 + 74)
yihui 0:1b6dab73c06b 167 #define D_1_79 (256 + 79)
yihui 0:1b6dab73c06b 168 #define D_1_88 (256 + 88)
yihui 0:1b6dab73c06b 169 #define D_1_90 (256 + 90)
yihui 0:1b6dab73c06b 170 #define D_1_92 (256 + 92)
yihui 0:1b6dab73c06b 171 #define D_1_96 (256 + 96)
yihui 0:1b6dab73c06b 172 #define D_1_98 (256 + 98)
yihui 0:1b6dab73c06b 173 #define D_1_106 (256 + 106)
yihui 0:1b6dab73c06b 174 #define D_1_108 (256 + 108)
yihui 0:1b6dab73c06b 175 #define D_1_112 (256 + 112)
yihui 0:1b6dab73c06b 176 #define D_1_128 (256 + 144)
yihui 0:1b6dab73c06b 177 #define D_1_152 (256 + 12)
yihui 0:1b6dab73c06b 178 #define D_1_160 (256 + 160)
yihui 0:1b6dab73c06b 179 #define D_1_176 (256 + 176)
yihui 0:1b6dab73c06b 180 #define D_1_178 (256 + 178)
yihui 0:1b6dab73c06b 181 #define D_1_218 (256 + 218)
yihui 0:1b6dab73c06b 182 #define D_1_232 (256 + 232)
yihui 0:1b6dab73c06b 183 #define D_1_236 (256 + 236)
yihui 0:1b6dab73c06b 184 #define D_1_240 (256 + 240)
yihui 0:1b6dab73c06b 185 #define D_1_244 (256 + 244)
yihui 0:1b6dab73c06b 186 #define D_1_250 (256 + 250)
yihui 0:1b6dab73c06b 187 #define D_1_252 (256 + 252)
yihui 0:1b6dab73c06b 188 #define D_2_12 (512 + 12)
yihui 0:1b6dab73c06b 189 #define D_2_96 (512 + 96)
yihui 0:1b6dab73c06b 190 #define D_2_108 (512 + 108)
yihui 0:1b6dab73c06b 191 #define D_2_208 (512 + 208)
yihui 0:1b6dab73c06b 192 #define D_2_224 (512 + 224)
yihui 0:1b6dab73c06b 193 #define D_2_236 (512 + 236)
yihui 0:1b6dab73c06b 194 #define D_2_244 (512 + 244)
yihui 0:1b6dab73c06b 195 #define D_2_248 (512 + 248)
yihui 0:1b6dab73c06b 196 #define D_2_252 (512 + 252)
yihui 0:1b6dab73c06b 197
yihui 0:1b6dab73c06b 198 #define CPASS_BIAS_X (35 * 16 + 4)
yihui 0:1b6dab73c06b 199 #define CPASS_BIAS_Y (35 * 16 + 8)
yihui 0:1b6dab73c06b 200 #define CPASS_BIAS_Z (35 * 16 + 12)
yihui 0:1b6dab73c06b 201 #define CPASS_MTX_00 (36 * 16)
yihui 0:1b6dab73c06b 202 #define CPASS_MTX_01 (36 * 16 + 4)
yihui 0:1b6dab73c06b 203 #define CPASS_MTX_02 (36 * 16 + 8)
yihui 0:1b6dab73c06b 204 #define CPASS_MTX_10 (36 * 16 + 12)
yihui 0:1b6dab73c06b 205 #define CPASS_MTX_11 (37 * 16)
yihui 0:1b6dab73c06b 206 #define CPASS_MTX_12 (37 * 16 + 4)
yihui 0:1b6dab73c06b 207 #define CPASS_MTX_20 (37 * 16 + 8)
yihui 0:1b6dab73c06b 208 #define CPASS_MTX_21 (37 * 16 + 12)
yihui 0:1b6dab73c06b 209 #define CPASS_MTX_22 (43 * 16 + 12)
yihui 0:1b6dab73c06b 210 #define D_EXT_GYRO_BIAS_X (61 * 16)
yihui 0:1b6dab73c06b 211 #define D_EXT_GYRO_BIAS_Y (61 * 16) + 4
yihui 0:1b6dab73c06b 212 #define D_EXT_GYRO_BIAS_Z (61 * 16) + 8
yihui 0:1b6dab73c06b 213 #define D_ACT0 (40 * 16)
yihui 0:1b6dab73c06b 214 #define D_ACSX (40 * 16 + 4)
yihui 0:1b6dab73c06b 215 #define D_ACSY (40 * 16 + 8)
yihui 0:1b6dab73c06b 216 #define D_ACSZ (40 * 16 + 12)
yihui 0:1b6dab73c06b 217
yihui 0:1b6dab73c06b 218 #define FLICK_MSG (45 * 16 + 4)
yihui 0:1b6dab73c06b 219 #define FLICK_COUNTER (45 * 16 + 8)
yihui 0:1b6dab73c06b 220 #define FLICK_LOWER (45 * 16 + 12)
yihui 0:1b6dab73c06b 221 #define FLICK_UPPER (46 * 16 + 12)
yihui 0:1b6dab73c06b 222
yihui 0:1b6dab73c06b 223 #define D_AUTH_OUT (992)
yihui 0:1b6dab73c06b 224 #define D_AUTH_IN (996)
yihui 0:1b6dab73c06b 225 #define D_AUTH_A (1000)
yihui 0:1b6dab73c06b 226 #define D_AUTH_B (1004)
yihui 0:1b6dab73c06b 227
yihui 0:1b6dab73c06b 228 #define D_PEDSTD_BP_B (768 + 0x1C)
yihui 0:1b6dab73c06b 229 #define D_PEDSTD_HP_A (768 + 0x78)
yihui 0:1b6dab73c06b 230 #define D_PEDSTD_HP_B (768 + 0x7C)
yihui 0:1b6dab73c06b 231 #define D_PEDSTD_BP_A4 (768 + 0x40)
yihui 0:1b6dab73c06b 232 #define D_PEDSTD_BP_A3 (768 + 0x44)
yihui 0:1b6dab73c06b 233 #define D_PEDSTD_BP_A2 (768 + 0x48)
yihui 0:1b6dab73c06b 234 #define D_PEDSTD_BP_A1 (768 + 0x4C)
yihui 0:1b6dab73c06b 235 #define D_PEDSTD_INT_THRSH (768 + 0x68)
yihui 0:1b6dab73c06b 236 #define D_PEDSTD_CLIP (768 + 0x6C)
yihui 0:1b6dab73c06b 237 #define D_PEDSTD_SB (768 + 0x28)
yihui 0:1b6dab73c06b 238 #define D_PEDSTD_SB_TIME (768 + 0x2C)
yihui 0:1b6dab73c06b 239 #define D_PEDSTD_PEAKTHRSH (768 + 0x98)
yihui 0:1b6dab73c06b 240 #define D_PEDSTD_TIML (768 + 0x2A)
yihui 0:1b6dab73c06b 241 #define D_PEDSTD_TIMH (768 + 0x2E)
yihui 0:1b6dab73c06b 242 #define D_PEDSTD_PEAK (768 + 0X94)
yihui 0:1b6dab73c06b 243 #define D_PEDSTD_STEPCTR (768 + 0x60)
yihui 0:1b6dab73c06b 244 #define D_PEDSTD_TIMECTR (964)
yihui 0:1b6dab73c06b 245 #define D_PEDSTD_DECI (768 + 0xA0)
yihui 0:1b6dab73c06b 246
yihui 0:1b6dab73c06b 247 #define D_HOST_NO_MOT (976)
yihui 0:1b6dab73c06b 248 #define D_ACCEL_BIAS (660)
yihui 0:1b6dab73c06b 249
yihui 0:1b6dab73c06b 250 #define D_ORIENT_GAP (76)
yihui 0:1b6dab73c06b 251
yihui 0:1b6dab73c06b 252 #define D_TILT0_H (48)
yihui 0:1b6dab73c06b 253 #define D_TILT0_L (50)
yihui 0:1b6dab73c06b 254 #define D_TILT1_H (52)
yihui 0:1b6dab73c06b 255 #define D_TILT1_L (54)
yihui 0:1b6dab73c06b 256 #define D_TILT2_H (56)
yihui 0:1b6dab73c06b 257 #define D_TILT2_L (58)
yihui 0:1b6dab73c06b 258 #define D_TILT3_H (60)
yihui 0:1b6dab73c06b 259 #define D_TILT3_L (62)
yihui 0:1b6dab73c06b 260
yihui 0:1b6dab73c06b 261 #define DMP_CODE_SIZE (3062)
yihui 0:1b6dab73c06b 262
yihui 0:1b6dab73c06b 263 static const unsigned char dmp_memory[DMP_CODE_SIZE] = {
yihui 0:1b6dab73c06b 264 /* bank # 0 */
yihui 0:1b6dab73c06b 265 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
yihui 0:1b6dab73c06b 266 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
yihui 0:1b6dab73c06b 267 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
yihui 0:1b6dab73c06b 268 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
yihui 0:1b6dab73c06b 269 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
yihui 0:1b6dab73c06b 270 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
yihui 0:1b6dab73c06b 271 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
yihui 0:1b6dab73c06b 272 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 273 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
yihui 0:1b6dab73c06b 274 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
yihui 0:1b6dab73c06b 275 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
yihui 0:1b6dab73c06b 276 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 277 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
yihui 0:1b6dab73c06b 278 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
yihui 0:1b6dab73c06b 279 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 280 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 281 /* bank # 1 */
yihui 0:1b6dab73c06b 282 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
yihui 0:1b6dab73c06b 283 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
yihui 0:1b6dab73c06b 284 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
yihui 0:1b6dab73c06b 285 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 286 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
yihui 0:1b6dab73c06b 287 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
yihui 0:1b6dab73c06b 288 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
yihui 0:1b6dab73c06b 289 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
yihui 0:1b6dab73c06b 290 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 291 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 292 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
yihui 0:1b6dab73c06b 293 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
yihui 0:1b6dab73c06b 294 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
yihui 0:1b6dab73c06b 295 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
yihui 0:1b6dab73c06b 296 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
yihui 0:1b6dab73c06b 297 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
yihui 0:1b6dab73c06b 298 /* bank # 2 */
yihui 0:1b6dab73c06b 299 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 300 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
yihui 0:1b6dab73c06b 301 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
yihui 0:1b6dab73c06b 302 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
yihui 0:1b6dab73c06b 303 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 304 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 305 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 306 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 307 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 308 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 309 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 310 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 311 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
yihui 0:1b6dab73c06b 312 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
yihui 0:1b6dab73c06b 313 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
yihui 0:1b6dab73c06b 314 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 315 /* bank # 3 */
yihui 0:1b6dab73c06b 316 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 317 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
yihui 0:1b6dab73c06b 318 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
yihui 0:1b6dab73c06b 319 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 320 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
yihui 0:1b6dab73c06b 321 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 322 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 323 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
yihui 0:1b6dab73c06b 324 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 325 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
yihui 0:1b6dab73c06b 326 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 327 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 328 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 329 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 330 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
yihui 0:1b6dab73c06b 331 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
yihui 0:1b6dab73c06b 332
yihui 0:1b6dab73c06b 333 /* bank # 4 */
yihui 0:1b6dab73c06b 334 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
yihui 0:1b6dab73c06b 335 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
yihui 0:1b6dab73c06b 336 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
yihui 0:1b6dab73c06b 337 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
yihui 0:1b6dab73c06b 338 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
yihui 0:1b6dab73c06b 339 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
yihui 0:1b6dab73c06b 340 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
yihui 0:1b6dab73c06b 341 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
yihui 0:1b6dab73c06b 342 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
yihui 0:1b6dab73c06b 343 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
yihui 0:1b6dab73c06b 344 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
yihui 0:1b6dab73c06b 345 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
yihui 0:1b6dab73c06b 346 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
yihui 0:1b6dab73c06b 347 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
yihui 0:1b6dab73c06b 348 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
yihui 0:1b6dab73c06b 349 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
yihui 0:1b6dab73c06b 350 /* bank # 5 */
yihui 0:1b6dab73c06b 351 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
yihui 0:1b6dab73c06b 352 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
yihui 0:1b6dab73c06b 353 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
yihui 0:1b6dab73c06b 354 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
yihui 0:1b6dab73c06b 355 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
yihui 0:1b6dab73c06b 356 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
yihui 0:1b6dab73c06b 357 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
yihui 0:1b6dab73c06b 358 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
yihui 0:1b6dab73c06b 359 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
yihui 0:1b6dab73c06b 360 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
yihui 0:1b6dab73c06b 361 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
yihui 0:1b6dab73c06b 362 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
yihui 0:1b6dab73c06b 363 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
yihui 0:1b6dab73c06b 364 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
yihui 0:1b6dab73c06b 365 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
yihui 0:1b6dab73c06b 366 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
yihui 0:1b6dab73c06b 367 /* bank # 6 */
yihui 0:1b6dab73c06b 368 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
yihui 0:1b6dab73c06b 369 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
yihui 0:1b6dab73c06b 370 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
yihui 0:1b6dab73c06b 371 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
yihui 0:1b6dab73c06b 372 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
yihui 0:1b6dab73c06b 373 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
yihui 0:1b6dab73c06b 374 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
yihui 0:1b6dab73c06b 375 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
yihui 0:1b6dab73c06b 376 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
yihui 0:1b6dab73c06b 377 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
yihui 0:1b6dab73c06b 378 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
yihui 0:1b6dab73c06b 379 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
yihui 0:1b6dab73c06b 380 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
yihui 0:1b6dab73c06b 381 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
yihui 0:1b6dab73c06b 382 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
yihui 0:1b6dab73c06b 383 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
yihui 0:1b6dab73c06b 384 /* bank # 7 */
yihui 0:1b6dab73c06b 385 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
yihui 0:1b6dab73c06b 386 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
yihui 0:1b6dab73c06b 387 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
yihui 0:1b6dab73c06b 388 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
yihui 0:1b6dab73c06b 389 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
yihui 0:1b6dab73c06b 390 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
yihui 0:1b6dab73c06b 391 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
yihui 0:1b6dab73c06b 392 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
yihui 0:1b6dab73c06b 393 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
yihui 0:1b6dab73c06b 394 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
yihui 0:1b6dab73c06b 395 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
yihui 0:1b6dab73c06b 396 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
yihui 0:1b6dab73c06b 397 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
yihui 0:1b6dab73c06b 398 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
yihui 0:1b6dab73c06b 399 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
yihui 0:1b6dab73c06b 400 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
yihui 0:1b6dab73c06b 401 /* bank # 8 */
yihui 0:1b6dab73c06b 402 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
yihui 0:1b6dab73c06b 403 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
yihui 0:1b6dab73c06b 404 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
yihui 0:1b6dab73c06b 405 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
yihui 0:1b6dab73c06b 406 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
yihui 0:1b6dab73c06b 407 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
yihui 0:1b6dab73c06b 408 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
yihui 0:1b6dab73c06b 409 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
yihui 0:1b6dab73c06b 410 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
yihui 0:1b6dab73c06b 411 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
yihui 0:1b6dab73c06b 412 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
yihui 0:1b6dab73c06b 413 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
yihui 0:1b6dab73c06b 414 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
yihui 0:1b6dab73c06b 415 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
yihui 0:1b6dab73c06b 416 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
yihui 0:1b6dab73c06b 417 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
yihui 0:1b6dab73c06b 418 /* bank # 9 */
yihui 0:1b6dab73c06b 419 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
yihui 0:1b6dab73c06b 420 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
yihui 0:1b6dab73c06b 421 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
yihui 0:1b6dab73c06b 422 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
yihui 0:1b6dab73c06b 423 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
yihui 0:1b6dab73c06b 424 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
yihui 0:1b6dab73c06b 425 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
yihui 0:1b6dab73c06b 426 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
yihui 0:1b6dab73c06b 427 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
yihui 0:1b6dab73c06b 428 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
yihui 0:1b6dab73c06b 429 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
yihui 0:1b6dab73c06b 430 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
yihui 0:1b6dab73c06b 431 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
yihui 0:1b6dab73c06b 432 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
yihui 0:1b6dab73c06b 433 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
yihui 0:1b6dab73c06b 434 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
yihui 0:1b6dab73c06b 435 /* bank # 10 */
yihui 0:1b6dab73c06b 436 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
yihui 0:1b6dab73c06b 437 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
yihui 0:1b6dab73c06b 438 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
yihui 0:1b6dab73c06b 439 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
yihui 0:1b6dab73c06b 440 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
yihui 0:1b6dab73c06b 441 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
yihui 0:1b6dab73c06b 442 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
yihui 0:1b6dab73c06b 443 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
yihui 0:1b6dab73c06b 444 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
yihui 0:1b6dab73c06b 445 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
yihui 0:1b6dab73c06b 446 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
yihui 0:1b6dab73c06b 447 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
yihui 0:1b6dab73c06b 448 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
yihui 0:1b6dab73c06b 449 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
yihui 0:1b6dab73c06b 450 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
yihui 0:1b6dab73c06b 451 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
yihui 0:1b6dab73c06b 452 /* bank # 11 */
yihui 0:1b6dab73c06b 453 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
yihui 0:1b6dab73c06b 454 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
yihui 0:1b6dab73c06b 455 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
yihui 0:1b6dab73c06b 456 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
yihui 0:1b6dab73c06b 457 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
yihui 0:1b6dab73c06b 458 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
yihui 0:1b6dab73c06b 459 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
yihui 0:1b6dab73c06b 460 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
yihui 0:1b6dab73c06b 461 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
yihui 0:1b6dab73c06b 462 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
yihui 0:1b6dab73c06b 463 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
yihui 0:1b6dab73c06b 464 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
yihui 0:1b6dab73c06b 465 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
yihui 0:1b6dab73c06b 466 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
yihui 0:1b6dab73c06b 467 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
yihui 0:1b6dab73c06b 468 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
yihui 0:1b6dab73c06b 469 };
yihui 0:1b6dab73c06b 470
yihui 0:1b6dab73c06b 471 static const unsigned short sStartAddress = 0x0400;
yihui 0:1b6dab73c06b 472
yihui 0:1b6dab73c06b 473 /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
yihui 0:1b6dab73c06b 474
yihui 0:1b6dab73c06b 475 #define INT_SRC_TAP (0x01)
yihui 0:1b6dab73c06b 476 #define INT_SRC_ANDROID_ORIENT (0x08)
yihui 0:1b6dab73c06b 477
yihui 0:1b6dab73c06b 478 #define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \
yihui 0:1b6dab73c06b 479 DMP_FEATURE_SEND_CAL_GYRO)
yihui 0:1b6dab73c06b 480
yihui 0:1b6dab73c06b 481 #define MAX_PACKET_LENGTH (32)
yihui 0:1b6dab73c06b 482
yihui 0:1b6dab73c06b 483 #define DMP_SAMPLE_RATE (200)
yihui 0:1b6dab73c06b 484 #define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE)
yihui 0:1b6dab73c06b 485
yihui 0:1b6dab73c06b 486 #define FIFO_CORRUPTION_CHECK
yihui 0:1b6dab73c06b 487 #ifdef FIFO_CORRUPTION_CHECK
yihui 0:1b6dab73c06b 488 #define QUAT_ERROR_THRESH (1L<<24)
yihui 0:1b6dab73c06b 489 #define QUAT_MAG_SQ_NORMALIZED (1L<<28)
yihui 0:1b6dab73c06b 490 #define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
yihui 0:1b6dab73c06b 491 #define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
yihui 0:1b6dab73c06b 492 #endif
yihui 0:1b6dab73c06b 493
yihui 0:1b6dab73c06b 494 struct dmp_s {
yihui 0:1b6dab73c06b 495 void (*tap_cb)(unsigned char count, unsigned char direction);
yihui 0:1b6dab73c06b 496 void (*android_orient_cb)(unsigned char orientation);
yihui 0:1b6dab73c06b 497 unsigned short orient;
yihui 0:1b6dab73c06b 498 unsigned short feature_mask;
yihui 0:1b6dab73c06b 499 unsigned short fifo_rate;
yihui 0:1b6dab73c06b 500 unsigned char packet_length;
yihui 0:1b6dab73c06b 501 };
yihui 0:1b6dab73c06b 502
yihui 0:1b6dab73c06b 503 static struct dmp_s dmp = {
yihui 0:1b6dab73c06b 504 .tap_cb = NULL,
yihui 0:1b6dab73c06b 505 .android_orient_cb = NULL,
yihui 0:1b6dab73c06b 506 .orient = 0,
yihui 0:1b6dab73c06b 507 .feature_mask = 0,
yihui 0:1b6dab73c06b 508 .fifo_rate = 0,
yihui 0:1b6dab73c06b 509 .packet_length = 0
yihui 0:1b6dab73c06b 510 };
yihui 0:1b6dab73c06b 511
yihui 0:1b6dab73c06b 512 /**
yihui 0:1b6dab73c06b 513 * @brief Load the DMP with this image.
yihui 0:1b6dab73c06b 514 * @return 0 if successful.
yihui 0:1b6dab73c06b 515 */
yihui 0:1b6dab73c06b 516 int dmp_load_motion_driver_firmware(void)
yihui 0:1b6dab73c06b 517 {
yihui 0:1b6dab73c06b 518 return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
yihui 0:1b6dab73c06b 519 DMP_SAMPLE_RATE);
yihui 0:1b6dab73c06b 520 }
yihui 0:1b6dab73c06b 521
yihui 0:1b6dab73c06b 522 /**
yihui 0:1b6dab73c06b 523 * @brief Push gyro and accel orientation to the DMP.
yihui 0:1b6dab73c06b 524 * The orientation is represented here as the output of
yihui 0:1b6dab73c06b 525 * @e inv_orientation_matrix_to_scalar.
yihui 0:1b6dab73c06b 526 * @param[in] orient Gyro and accel orientation in body frame.
yihui 0:1b6dab73c06b 527 * @return 0 if successful.
yihui 0:1b6dab73c06b 528 */
yihui 0:1b6dab73c06b 529 int dmp_set_orientation(unsigned short orient)
yihui 0:1b6dab73c06b 530 {
yihui 0:1b6dab73c06b 531 unsigned char gyro_regs[3], accel_regs[3];
yihui 0:1b6dab73c06b 532 const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
yihui 0:1b6dab73c06b 533 const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
yihui 0:1b6dab73c06b 534 const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
yihui 0:1b6dab73c06b 535 const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
yihui 0:1b6dab73c06b 536
yihui 0:1b6dab73c06b 537 gyro_regs[0] = gyro_axes[orient & 3];
yihui 0:1b6dab73c06b 538 gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
yihui 0:1b6dab73c06b 539 gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
yihui 0:1b6dab73c06b 540 accel_regs[0] = accel_axes[orient & 3];
yihui 0:1b6dab73c06b 541 accel_regs[1] = accel_axes[(orient >> 3) & 3];
yihui 0:1b6dab73c06b 542 accel_regs[2] = accel_axes[(orient >> 6) & 3];
yihui 0:1b6dab73c06b 543
yihui 0:1b6dab73c06b 544 /* Chip-to-body, axes only. */
yihui 0:1b6dab73c06b 545 if (mpu_write_mem(FCFG_1, 3, gyro_regs))
yihui 0:1b6dab73c06b 546 return -1;
yihui 0:1b6dab73c06b 547 if (mpu_write_mem(FCFG_2, 3, accel_regs))
yihui 0:1b6dab73c06b 548 return -1;
yihui 0:1b6dab73c06b 549
yihui 0:1b6dab73c06b 550 memcpy(gyro_regs, gyro_sign, 3);
yihui 0:1b6dab73c06b 551 memcpy(accel_regs, accel_sign, 3);
yihui 0:1b6dab73c06b 552 if (orient & 4) {
yihui 0:1b6dab73c06b 553 gyro_regs[0] |= 1;
yihui 0:1b6dab73c06b 554 accel_regs[0] |= 1;
yihui 0:1b6dab73c06b 555 }
yihui 0:1b6dab73c06b 556 if (orient & 0x20) {
yihui 0:1b6dab73c06b 557 gyro_regs[1] |= 1;
yihui 0:1b6dab73c06b 558 accel_regs[1] |= 1;
yihui 0:1b6dab73c06b 559 }
yihui 0:1b6dab73c06b 560 if (orient & 0x100) {
yihui 0:1b6dab73c06b 561 gyro_regs[2] |= 1;
yihui 0:1b6dab73c06b 562 accel_regs[2] |= 1;
yihui 0:1b6dab73c06b 563 }
yihui 0:1b6dab73c06b 564
yihui 0:1b6dab73c06b 565 /* Chip-to-body, sign only. */
yihui 0:1b6dab73c06b 566 if (mpu_write_mem(FCFG_3, 3, gyro_regs))
yihui 0:1b6dab73c06b 567 return -1;
yihui 0:1b6dab73c06b 568 if (mpu_write_mem(FCFG_7, 3, accel_regs))
yihui 0:1b6dab73c06b 569 return -1;
yihui 0:1b6dab73c06b 570 dmp.orient = orient;
yihui 0:1b6dab73c06b 571 return 0;
yihui 0:1b6dab73c06b 572 }
yihui 0:1b6dab73c06b 573
yihui 0:1b6dab73c06b 574 /**
yihui 0:1b6dab73c06b 575 * @brief Push gyro biases to the DMP.
yihui 0:1b6dab73c06b 576 * Because the gyro integration is handled in the DMP, any gyro biases
yihui 0:1b6dab73c06b 577 * calculated by the MPL should be pushed down to DMP memory to remove
yihui 0:1b6dab73c06b 578 * 3-axis quaternion drift.
yihui 0:1b6dab73c06b 579 * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
yihui 0:1b6dab73c06b 580 * overwrite the biases written to this location once a new one is computed.
yihui 0:1b6dab73c06b 581 * @param[in] bias Gyro biases in q16.
yihui 0:1b6dab73c06b 582 * @return 0 if successful.
yihui 0:1b6dab73c06b 583 */
yihui 0:1b6dab73c06b 584 int dmp_set_gyro_bias(long *bias)
yihui 0:1b6dab73c06b 585 {
yihui 0:1b6dab73c06b 586 long gyro_bias_body[3];
yihui 0:1b6dab73c06b 587 unsigned char regs[4];
yihui 0:1b6dab73c06b 588
yihui 0:1b6dab73c06b 589 gyro_bias_body[0] = bias[dmp.orient & 3];
yihui 0:1b6dab73c06b 590 if (dmp.orient & 4)
yihui 0:1b6dab73c06b 591 gyro_bias_body[0] *= -1;
yihui 0:1b6dab73c06b 592 gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];
yihui 0:1b6dab73c06b 593 if (dmp.orient & 0x20)
yihui 0:1b6dab73c06b 594 gyro_bias_body[1] *= -1;
yihui 0:1b6dab73c06b 595 gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];
yihui 0:1b6dab73c06b 596 if (dmp.orient & 0x100)
yihui 0:1b6dab73c06b 597 gyro_bias_body[2] *= -1;
yihui 0:1b6dab73c06b 598
yihui 0:1b6dab73c06b 599 #ifdef EMPL_NO_64BIT
yihui 0:1b6dab73c06b 600 gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
yihui 0:1b6dab73c06b 601 gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
yihui 0:1b6dab73c06b 602 gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
yihui 0:1b6dab73c06b 603 #else
yihui 0:1b6dab73c06b 604 gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
yihui 0:1b6dab73c06b 605 gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
yihui 0:1b6dab73c06b 606 gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
yihui 0:1b6dab73c06b 607 #endif
yihui 0:1b6dab73c06b 608
yihui 0:1b6dab73c06b 609 regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
yihui 0:1b6dab73c06b 610 regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
yihui 0:1b6dab73c06b 611 regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
yihui 0:1b6dab73c06b 612 regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
yihui 0:1b6dab73c06b 613 if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
yihui 0:1b6dab73c06b 614 return -1;
yihui 0:1b6dab73c06b 615
yihui 0:1b6dab73c06b 616 regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
yihui 0:1b6dab73c06b 617 regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
yihui 0:1b6dab73c06b 618 regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
yihui 0:1b6dab73c06b 619 regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
yihui 0:1b6dab73c06b 620 if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
yihui 0:1b6dab73c06b 621 return -1;
yihui 0:1b6dab73c06b 622
yihui 0:1b6dab73c06b 623 regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
yihui 0:1b6dab73c06b 624 regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
yihui 0:1b6dab73c06b 625 regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
yihui 0:1b6dab73c06b 626 regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
yihui 0:1b6dab73c06b 627 return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
yihui 0:1b6dab73c06b 628 }
yihui 0:1b6dab73c06b 629
yihui 0:1b6dab73c06b 630 /**
yihui 0:1b6dab73c06b 631 * @brief Push accel biases to the DMP.
yihui 0:1b6dab73c06b 632 * These biases will be removed from the DMP 6-axis quaternion.
yihui 0:1b6dab73c06b 633 * @param[in] bias Accel biases in q16.
yihui 0:1b6dab73c06b 634 * @return 0 if successful.
yihui 0:1b6dab73c06b 635 */
yihui 0:1b6dab73c06b 636 int dmp_set_accel_bias(long *bias)
yihui 0:1b6dab73c06b 637 {
yihui 0:1b6dab73c06b 638 long accel_bias_body[3];
yihui 0:1b6dab73c06b 639 unsigned char regs[12];
yihui 0:1b6dab73c06b 640 long long accel_sf;
yihui 0:1b6dab73c06b 641 unsigned short accel_sens;
yihui 0:1b6dab73c06b 642
yihui 0:1b6dab73c06b 643 mpu_get_accel_sens(&accel_sens);
yihui 0:1b6dab73c06b 644 accel_sf = (long long)accel_sens << 15;
yihui 0:1b6dab73c06b 645 __no_operation();
yihui 0:1b6dab73c06b 646
yihui 0:1b6dab73c06b 647 accel_bias_body[0] = bias[dmp.orient & 3];
yihui 0:1b6dab73c06b 648 if (dmp.orient & 4)
yihui 0:1b6dab73c06b 649 accel_bias_body[0] *= -1;
yihui 0:1b6dab73c06b 650 accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];
yihui 0:1b6dab73c06b 651 if (dmp.orient & 0x20)
yihui 0:1b6dab73c06b 652 accel_bias_body[1] *= -1;
yihui 0:1b6dab73c06b 653 accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];
yihui 0:1b6dab73c06b 654 if (dmp.orient & 0x100)
yihui 0:1b6dab73c06b 655 accel_bias_body[2] *= -1;
yihui 0:1b6dab73c06b 656
yihui 0:1b6dab73c06b 657 #ifdef EMPL_NO_64BIT
yihui 0:1b6dab73c06b 658 accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
yihui 0:1b6dab73c06b 659 accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
yihui 0:1b6dab73c06b 660 accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
yihui 0:1b6dab73c06b 661 #else
yihui 0:1b6dab73c06b 662 accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
yihui 0:1b6dab73c06b 663 accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
yihui 0:1b6dab73c06b 664 accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
yihui 0:1b6dab73c06b 665 #endif
yihui 0:1b6dab73c06b 666
yihui 0:1b6dab73c06b 667 regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
yihui 0:1b6dab73c06b 668 regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
yihui 0:1b6dab73c06b 669 regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
yihui 0:1b6dab73c06b 670 regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
yihui 0:1b6dab73c06b 671 regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
yihui 0:1b6dab73c06b 672 regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
yihui 0:1b6dab73c06b 673 regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
yihui 0:1b6dab73c06b 674 regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
yihui 0:1b6dab73c06b 675 regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
yihui 0:1b6dab73c06b 676 regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
yihui 0:1b6dab73c06b 677 regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
yihui 0:1b6dab73c06b 678 regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
yihui 0:1b6dab73c06b 679 return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
yihui 0:1b6dab73c06b 680 }
yihui 0:1b6dab73c06b 681
yihui 0:1b6dab73c06b 682 /**
yihui 0:1b6dab73c06b 683 * @brief Set DMP output rate.
yihui 0:1b6dab73c06b 684 * Only used when DMP is on.
yihui 0:1b6dab73c06b 685 * @param[in] rate Desired fifo rate (Hz).
yihui 0:1b6dab73c06b 686 * @return 0 if successful.
yihui 0:1b6dab73c06b 687 */
yihui 0:1b6dab73c06b 688 int dmp_set_fifo_rate(unsigned short rate)
yihui 0:1b6dab73c06b 689 {
yihui 0:1b6dab73c06b 690 const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
yihui 0:1b6dab73c06b 691 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
yihui 0:1b6dab73c06b 692 unsigned short div;
yihui 0:1b6dab73c06b 693 unsigned char tmp[8];
yihui 0:1b6dab73c06b 694
yihui 0:1b6dab73c06b 695 if (rate > DMP_SAMPLE_RATE)
yihui 0:1b6dab73c06b 696 return -1;
yihui 0:1b6dab73c06b 697 div = DMP_SAMPLE_RATE / rate - 1;
yihui 0:1b6dab73c06b 698 tmp[0] = (unsigned char)((div >> 8) & 0xFF);
yihui 0:1b6dab73c06b 699 tmp[1] = (unsigned char)(div & 0xFF);
yihui 0:1b6dab73c06b 700 if (mpu_write_mem(D_0_22, 2, tmp))
yihui 0:1b6dab73c06b 701 return -1;
yihui 0:1b6dab73c06b 702 if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
yihui 0:1b6dab73c06b 703 return -1;
yihui 0:1b6dab73c06b 704
yihui 0:1b6dab73c06b 705 dmp.fifo_rate = rate;
yihui 0:1b6dab73c06b 706 return 0;
yihui 0:1b6dab73c06b 707 }
yihui 0:1b6dab73c06b 708
yihui 0:1b6dab73c06b 709 /**
yihui 0:1b6dab73c06b 710 * @brief Get DMP output rate.
yihui 0:1b6dab73c06b 711 * @param[out] rate Current fifo rate (Hz).
yihui 0:1b6dab73c06b 712 * @return 0 if successful.
yihui 0:1b6dab73c06b 713 */
yihui 0:1b6dab73c06b 714 int dmp_get_fifo_rate(unsigned short *rate)
yihui 0:1b6dab73c06b 715 {
yihui 0:1b6dab73c06b 716 rate[0] = dmp.fifo_rate;
yihui 0:1b6dab73c06b 717 return 0;
yihui 0:1b6dab73c06b 718 }
yihui 0:1b6dab73c06b 719
yihui 0:1b6dab73c06b 720 /**
yihui 0:1b6dab73c06b 721 * @brief Set tap threshold for a specific axis.
yihui 0:1b6dab73c06b 722 * @param[in] axis 1, 2, and 4 for XYZ accel, respectively.
yihui 0:1b6dab73c06b 723 * @param[in] thresh Tap threshold, in mg/ms.
yihui 0:1b6dab73c06b 724 * @return 0 if successful.
yihui 0:1b6dab73c06b 725 */
yihui 0:1b6dab73c06b 726 int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
yihui 0:1b6dab73c06b 727 {
yihui 0:1b6dab73c06b 728 unsigned char tmp[4], accel_fsr;
yihui 0:1b6dab73c06b 729 float scaled_thresh;
yihui 0:1b6dab73c06b 730 unsigned short dmp_thresh, dmp_thresh_2;
yihui 0:1b6dab73c06b 731 if (!(axis & TAP_XYZ) || thresh > 1600)
yihui 0:1b6dab73c06b 732 return -1;
yihui 0:1b6dab73c06b 733
yihui 0:1b6dab73c06b 734 scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
yihui 0:1b6dab73c06b 735
yihui 0:1b6dab73c06b 736 mpu_get_accel_fsr(&accel_fsr);
yihui 0:1b6dab73c06b 737 switch (accel_fsr) {
yihui 0:1b6dab73c06b 738 case 2:
yihui 0:1b6dab73c06b 739 dmp_thresh = (unsigned short)(scaled_thresh * 16384);
yihui 0:1b6dab73c06b 740 /* dmp_thresh * 0.75 */
yihui 0:1b6dab73c06b 741 dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
yihui 0:1b6dab73c06b 742 break;
yihui 0:1b6dab73c06b 743 case 4:
yihui 0:1b6dab73c06b 744 dmp_thresh = (unsigned short)(scaled_thresh * 8192);
yihui 0:1b6dab73c06b 745 /* dmp_thresh * 0.75 */
yihui 0:1b6dab73c06b 746 dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
yihui 0:1b6dab73c06b 747 break;
yihui 0:1b6dab73c06b 748 case 8:
yihui 0:1b6dab73c06b 749 dmp_thresh = (unsigned short)(scaled_thresh * 4096);
yihui 0:1b6dab73c06b 750 /* dmp_thresh * 0.75 */
yihui 0:1b6dab73c06b 751 dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
yihui 0:1b6dab73c06b 752 break;
yihui 0:1b6dab73c06b 753 case 16:
yihui 0:1b6dab73c06b 754 dmp_thresh = (unsigned short)(scaled_thresh * 2048);
yihui 0:1b6dab73c06b 755 /* dmp_thresh * 0.75 */
yihui 0:1b6dab73c06b 756 dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
yihui 0:1b6dab73c06b 757 break;
yihui 0:1b6dab73c06b 758 default:
yihui 0:1b6dab73c06b 759 return -1;
yihui 0:1b6dab73c06b 760 }
yihui 0:1b6dab73c06b 761 tmp[0] = (unsigned char)(dmp_thresh >> 8);
yihui 0:1b6dab73c06b 762 tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
yihui 0:1b6dab73c06b 763 tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
yihui 0:1b6dab73c06b 764 tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
yihui 0:1b6dab73c06b 765
yihui 0:1b6dab73c06b 766 if (axis & TAP_X) {
yihui 0:1b6dab73c06b 767 if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
yihui 0:1b6dab73c06b 768 return -1;
yihui 0:1b6dab73c06b 769 if (mpu_write_mem(D_1_36, 2, tmp+2))
yihui 0:1b6dab73c06b 770 return -1;
yihui 0:1b6dab73c06b 771 }
yihui 0:1b6dab73c06b 772 if (axis & TAP_Y) {
yihui 0:1b6dab73c06b 773 if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
yihui 0:1b6dab73c06b 774 return -1;
yihui 0:1b6dab73c06b 775 if (mpu_write_mem(D_1_40, 2, tmp+2))
yihui 0:1b6dab73c06b 776 return -1;
yihui 0:1b6dab73c06b 777 }
yihui 0:1b6dab73c06b 778 if (axis & TAP_Z) {
yihui 0:1b6dab73c06b 779 if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
yihui 0:1b6dab73c06b 780 return -1;
yihui 0:1b6dab73c06b 781 if (mpu_write_mem(D_1_44, 2, tmp+2))
yihui 0:1b6dab73c06b 782 return -1;
yihui 0:1b6dab73c06b 783 }
yihui 0:1b6dab73c06b 784 return 0;
yihui 0:1b6dab73c06b 785 }
yihui 0:1b6dab73c06b 786
yihui 0:1b6dab73c06b 787 /**
yihui 0:1b6dab73c06b 788 * @brief Set which axes will register a tap.
yihui 0:1b6dab73c06b 789 * @param[in] axis 1, 2, and 4 for XYZ, respectively.
yihui 0:1b6dab73c06b 790 * @return 0 if successful.
yihui 0:1b6dab73c06b 791 */
yihui 0:1b6dab73c06b 792 int dmp_set_tap_axes(unsigned char axis)
yihui 0:1b6dab73c06b 793 {
yihui 0:1b6dab73c06b 794 unsigned char tmp = 0;
yihui 0:1b6dab73c06b 795
yihui 0:1b6dab73c06b 796 if (axis & TAP_X)
yihui 0:1b6dab73c06b 797 tmp |= 0x30;
yihui 0:1b6dab73c06b 798 if (axis & TAP_Y)
yihui 0:1b6dab73c06b 799 tmp |= 0x0C;
yihui 0:1b6dab73c06b 800 if (axis & TAP_Z)
yihui 0:1b6dab73c06b 801 tmp |= 0x03;
yihui 0:1b6dab73c06b 802 return mpu_write_mem(D_1_72, 1, &tmp);
yihui 0:1b6dab73c06b 803 }
yihui 0:1b6dab73c06b 804
yihui 0:1b6dab73c06b 805 /**
yihui 0:1b6dab73c06b 806 * @brief Set minimum number of taps needed for an interrupt.
yihui 0:1b6dab73c06b 807 * @param[in] min_taps Minimum consecutive taps (1-4).
yihui 0:1b6dab73c06b 808 * @return 0 if successful.
yihui 0:1b6dab73c06b 809 */
yihui 0:1b6dab73c06b 810 int dmp_set_tap_count(unsigned char min_taps)
yihui 0:1b6dab73c06b 811 {
yihui 0:1b6dab73c06b 812 unsigned char tmp;
yihui 0:1b6dab73c06b 813
yihui 0:1b6dab73c06b 814 if (min_taps < 1)
yihui 0:1b6dab73c06b 815 min_taps = 1;
yihui 0:1b6dab73c06b 816 else if (min_taps > 4)
yihui 0:1b6dab73c06b 817 min_taps = 4;
yihui 0:1b6dab73c06b 818
yihui 0:1b6dab73c06b 819 tmp = min_taps - 1;
yihui 0:1b6dab73c06b 820 return mpu_write_mem(D_1_79, 1, &tmp);
yihui 0:1b6dab73c06b 821 }
yihui 0:1b6dab73c06b 822
yihui 0:1b6dab73c06b 823 /**
yihui 0:1b6dab73c06b 824 * @brief Set length between valid taps.
yihui 0:1b6dab73c06b 825 * @param[in] time Milliseconds between taps.
yihui 0:1b6dab73c06b 826 * @return 0 if successful.
yihui 0:1b6dab73c06b 827 */
yihui 0:1b6dab73c06b 828 int dmp_set_tap_time(unsigned short time)
yihui 0:1b6dab73c06b 829 {
yihui 0:1b6dab73c06b 830 unsigned short dmp_time;
yihui 0:1b6dab73c06b 831 unsigned char tmp[2];
yihui 0:1b6dab73c06b 832
yihui 0:1b6dab73c06b 833 dmp_time = time / (1000 / DMP_SAMPLE_RATE);
yihui 0:1b6dab73c06b 834 tmp[0] = (unsigned char)(dmp_time >> 8);
yihui 0:1b6dab73c06b 835 tmp[1] = (unsigned char)(dmp_time & 0xFF);
yihui 0:1b6dab73c06b 836 return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
yihui 0:1b6dab73c06b 837 }
yihui 0:1b6dab73c06b 838
yihui 0:1b6dab73c06b 839 /**
yihui 0:1b6dab73c06b 840 * @brief Set max time between taps to register as a multi-tap.
yihui 0:1b6dab73c06b 841 * @param[in] time Max milliseconds between taps.
yihui 0:1b6dab73c06b 842 * @return 0 if successful.
yihui 0:1b6dab73c06b 843 */
yihui 0:1b6dab73c06b 844 int dmp_set_tap_time_multi(unsigned short time)
yihui 0:1b6dab73c06b 845 {
yihui 0:1b6dab73c06b 846 unsigned short dmp_time;
yihui 0:1b6dab73c06b 847 unsigned char tmp[2];
yihui 0:1b6dab73c06b 848
yihui 0:1b6dab73c06b 849 dmp_time = time / (1000 / DMP_SAMPLE_RATE);
yihui 0:1b6dab73c06b 850 tmp[0] = (unsigned char)(dmp_time >> 8);
yihui 0:1b6dab73c06b 851 tmp[1] = (unsigned char)(dmp_time & 0xFF);
yihui 0:1b6dab73c06b 852 return mpu_write_mem(D_1_218, 2, tmp);
yihui 0:1b6dab73c06b 853 }
yihui 0:1b6dab73c06b 854
yihui 0:1b6dab73c06b 855 /**
yihui 0:1b6dab73c06b 856 * @brief Set shake rejection threshold.
yihui 0:1b6dab73c06b 857 * If the DMP detects a gyro sample larger than @e thresh, taps are rejected.
yihui 0:1b6dab73c06b 858 * @param[in] sf Gyro scale factor.
yihui 0:1b6dab73c06b 859 * @param[in] thresh Gyro threshold in dps.
yihui 0:1b6dab73c06b 860 * @return 0 if successful.
yihui 0:1b6dab73c06b 861 */
yihui 0:1b6dab73c06b 862 int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
yihui 0:1b6dab73c06b 863 {
yihui 0:1b6dab73c06b 864 unsigned char tmp[4];
yihui 0:1b6dab73c06b 865 long thresh_scaled = sf / 1000 * thresh;
yihui 0:1b6dab73c06b 866 tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
yihui 0:1b6dab73c06b 867 tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
yihui 0:1b6dab73c06b 868 tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
yihui 0:1b6dab73c06b 869 tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
yihui 0:1b6dab73c06b 870 return mpu_write_mem(D_1_92, 4, tmp);
yihui 0:1b6dab73c06b 871 }
yihui 0:1b6dab73c06b 872
yihui 0:1b6dab73c06b 873 /**
yihui 0:1b6dab73c06b 874 * @brief Set shake rejection time.
yihui 0:1b6dab73c06b 875 * Sets the length of time that the gyro must be outside of the threshold set
yihui 0:1b6dab73c06b 876 * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory
yihui 0:1b6dab73c06b 877 * 60 ms is added to this parameter.
yihui 0:1b6dab73c06b 878 * @param[in] time Time in milliseconds.
yihui 0:1b6dab73c06b 879 * @return 0 if successful.
yihui 0:1b6dab73c06b 880 */
yihui 0:1b6dab73c06b 881 int dmp_set_shake_reject_time(unsigned short time)
yihui 0:1b6dab73c06b 882 {
yihui 0:1b6dab73c06b 883 unsigned char tmp[2];
yihui 0:1b6dab73c06b 884
yihui 0:1b6dab73c06b 885 time /= (1000 / DMP_SAMPLE_RATE);
yihui 0:1b6dab73c06b 886 tmp[0] = time >> 8;
yihui 0:1b6dab73c06b 887 tmp[1] = time & 0xFF;
yihui 0:1b6dab73c06b 888 return mpu_write_mem(D_1_90,2,tmp);
yihui 0:1b6dab73c06b 889 }
yihui 0:1b6dab73c06b 890
yihui 0:1b6dab73c06b 891 /**
yihui 0:1b6dab73c06b 892 * @brief Set shake rejection timeout.
yihui 0:1b6dab73c06b 893 * Sets the length of time after a shake rejection that the gyro must stay
yihui 0:1b6dab73c06b 894 * inside of the threshold before taps can be detected again. A mandatory
yihui 0:1b6dab73c06b 895 * 60 ms is added to this parameter.
yihui 0:1b6dab73c06b 896 * @param[in] time Time in milliseconds.
yihui 0:1b6dab73c06b 897 * @return 0 if successful.
yihui 0:1b6dab73c06b 898 */
yihui 0:1b6dab73c06b 899 int dmp_set_shake_reject_timeout(unsigned short time)
yihui 0:1b6dab73c06b 900 {
yihui 0:1b6dab73c06b 901 unsigned char tmp[2];
yihui 0:1b6dab73c06b 902
yihui 0:1b6dab73c06b 903 time /= (1000 / DMP_SAMPLE_RATE);
yihui 0:1b6dab73c06b 904 tmp[0] = time >> 8;
yihui 0:1b6dab73c06b 905 tmp[1] = time & 0xFF;
yihui 0:1b6dab73c06b 906 return mpu_write_mem(D_1_88,2,tmp);
yihui 0:1b6dab73c06b 907 }
yihui 0:1b6dab73c06b 908
yihui 0:1b6dab73c06b 909 /**
yihui 0:1b6dab73c06b 910 * @brief Get current step count.
yihui 0:1b6dab73c06b 911 * @param[out] count Number of steps detected.
yihui 0:1b6dab73c06b 912 * @return 0 if successful.
yihui 0:1b6dab73c06b 913 */
yihui 0:1b6dab73c06b 914 int dmp_get_pedometer_step_count(unsigned long *count)
yihui 0:1b6dab73c06b 915 {
yihui 0:1b6dab73c06b 916 unsigned char tmp[4];
yihui 0:1b6dab73c06b 917 if (!count)
yihui 0:1b6dab73c06b 918 return -1;
yihui 0:1b6dab73c06b 919
yihui 0:1b6dab73c06b 920 if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
yihui 0:1b6dab73c06b 921 return -1;
yihui 0:1b6dab73c06b 922
yihui 0:1b6dab73c06b 923 count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
yihui 0:1b6dab73c06b 924 ((unsigned long)tmp[2] << 8) | tmp[3];
yihui 0:1b6dab73c06b 925 return 0;
yihui 0:1b6dab73c06b 926 }
yihui 0:1b6dab73c06b 927
yihui 0:1b6dab73c06b 928 /**
yihui 0:1b6dab73c06b 929 * @brief Overwrite current step count.
yihui 0:1b6dab73c06b 930 * WARNING: This function writes to DMP memory and could potentially encounter
yihui 0:1b6dab73c06b 931 * a race condition if called while the pedometer is enabled.
yihui 0:1b6dab73c06b 932 * @param[in] count New step count.
yihui 0:1b6dab73c06b 933 * @return 0 if successful.
yihui 0:1b6dab73c06b 934 */
yihui 0:1b6dab73c06b 935 int dmp_set_pedometer_step_count(unsigned long count)
yihui 0:1b6dab73c06b 936 {
yihui 0:1b6dab73c06b 937 unsigned char tmp[4];
yihui 0:1b6dab73c06b 938
yihui 0:1b6dab73c06b 939 tmp[0] = (unsigned char)((count >> 24) & 0xFF);
yihui 0:1b6dab73c06b 940 tmp[1] = (unsigned char)((count >> 16) & 0xFF);
yihui 0:1b6dab73c06b 941 tmp[2] = (unsigned char)((count >> 8) & 0xFF);
yihui 0:1b6dab73c06b 942 tmp[3] = (unsigned char)(count & 0xFF);
yihui 0:1b6dab73c06b 943 return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);
yihui 0:1b6dab73c06b 944 }
yihui 0:1b6dab73c06b 945
yihui 0:1b6dab73c06b 946 /**
yihui 0:1b6dab73c06b 947 * @brief Get duration of walking time.
yihui 0:1b6dab73c06b 948 * @param[in] time Walk time in milliseconds.
yihui 0:1b6dab73c06b 949 * @return 0 if successful.
yihui 0:1b6dab73c06b 950 */
yihui 0:1b6dab73c06b 951 int dmp_get_pedometer_walk_time(unsigned long *time)
yihui 0:1b6dab73c06b 952 {
yihui 0:1b6dab73c06b 953 unsigned char tmp[4];
yihui 0:1b6dab73c06b 954 if (!time)
yihui 0:1b6dab73c06b 955 return -1;
yihui 0:1b6dab73c06b 956
yihui 0:1b6dab73c06b 957 if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
yihui 0:1b6dab73c06b 958 return -1;
yihui 0:1b6dab73c06b 959
yihui 0:1b6dab73c06b 960 time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
yihui 0:1b6dab73c06b 961 ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
yihui 0:1b6dab73c06b 962 return 0;
yihui 0:1b6dab73c06b 963 }
yihui 0:1b6dab73c06b 964
yihui 0:1b6dab73c06b 965 /**
yihui 0:1b6dab73c06b 966 * @brief Overwrite current walk time.
yihui 0:1b6dab73c06b 967 * WARNING: This function writes to DMP memory and could potentially encounter
yihui 0:1b6dab73c06b 968 * a race condition if called while the pedometer is enabled.
yihui 0:1b6dab73c06b 969 * @param[in] time New walk time in milliseconds.
yihui 0:1b6dab73c06b 970 */
yihui 0:1b6dab73c06b 971 int dmp_set_pedometer_walk_time(unsigned long time)
yihui 0:1b6dab73c06b 972 {
yihui 0:1b6dab73c06b 973 unsigned char tmp[4];
yihui 0:1b6dab73c06b 974
yihui 0:1b6dab73c06b 975 time /= 20;
yihui 0:1b6dab73c06b 976
yihui 0:1b6dab73c06b 977 tmp[0] = (unsigned char)((time >> 24) & 0xFF);
yihui 0:1b6dab73c06b 978 tmp[1] = (unsigned char)((time >> 16) & 0xFF);
yihui 0:1b6dab73c06b 979 tmp[2] = (unsigned char)((time >> 8) & 0xFF);
yihui 0:1b6dab73c06b 980 tmp[3] = (unsigned char)(time & 0xFF);
yihui 0:1b6dab73c06b 981 return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);
yihui 0:1b6dab73c06b 982 }
yihui 0:1b6dab73c06b 983
yihui 0:1b6dab73c06b 984 /**
yihui 0:1b6dab73c06b 985 * @brief Enable DMP features.
yihui 0:1b6dab73c06b 986 * The following \#define's are used in the input mask:
yihui 0:1b6dab73c06b 987 * \n DMP_FEATURE_TAP
yihui 0:1b6dab73c06b 988 * \n DMP_FEATURE_ANDROID_ORIENT
yihui 0:1b6dab73c06b 989 * \n DMP_FEATURE_LP_QUAT
yihui 0:1b6dab73c06b 990 * \n DMP_FEATURE_6X_LP_QUAT
yihui 0:1b6dab73c06b 991 * \n DMP_FEATURE_GYRO_CAL
yihui 0:1b6dab73c06b 992 * \n DMP_FEATURE_SEND_RAW_ACCEL
yihui 0:1b6dab73c06b 993 * \n DMP_FEATURE_SEND_RAW_GYRO
yihui 0:1b6dab73c06b 994 * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
yihui 0:1b6dab73c06b 995 * exclusive.
yihui 0:1b6dab73c06b 996 * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
yihui 0:1b6dab73c06b 997 * mutually exclusive.
yihui 0:1b6dab73c06b 998 * @param[in] mask Mask of features to enable.
yihui 0:1b6dab73c06b 999 * @return 0 if successful.
yihui 0:1b6dab73c06b 1000 */
yihui 0:1b6dab73c06b 1001 int dmp_enable_feature(unsigned short mask)
yihui 0:1b6dab73c06b 1002 {
yihui 0:1b6dab73c06b 1003 unsigned char tmp[10];
yihui 0:1b6dab73c06b 1004
yihui 0:1b6dab73c06b 1005 /* TODO: All of these settings can probably be integrated into the default
yihui 0:1b6dab73c06b 1006 * DMP image.
yihui 0:1b6dab73c06b 1007 */
yihui 0:1b6dab73c06b 1008 /* Set integration scale factor. */
yihui 0:1b6dab73c06b 1009 tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
yihui 0:1b6dab73c06b 1010 tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
yihui 0:1b6dab73c06b 1011 tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
yihui 0:1b6dab73c06b 1012 tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
yihui 0:1b6dab73c06b 1013 mpu_write_mem(D_0_104, 4, tmp);
yihui 0:1b6dab73c06b 1014
yihui 0:1b6dab73c06b 1015 /* Send sensor data to the FIFO. */
yihui 0:1b6dab73c06b 1016 tmp[0] = 0xA3;
yihui 0:1b6dab73c06b 1017 if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
yihui 0:1b6dab73c06b 1018 tmp[1] = 0xC0;
yihui 0:1b6dab73c06b 1019 tmp[2] = 0xC8;
yihui 0:1b6dab73c06b 1020 tmp[3] = 0xC2;
yihui 0:1b6dab73c06b 1021 } else {
yihui 0:1b6dab73c06b 1022 tmp[1] = 0xA3;
yihui 0:1b6dab73c06b 1023 tmp[2] = 0xA3;
yihui 0:1b6dab73c06b 1024 tmp[3] = 0xA3;
yihui 0:1b6dab73c06b 1025 }
yihui 0:1b6dab73c06b 1026 if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
yihui 0:1b6dab73c06b 1027 tmp[4] = 0xC4;
yihui 0:1b6dab73c06b 1028 tmp[5] = 0xCC;
yihui 0:1b6dab73c06b 1029 tmp[6] = 0xC6;
yihui 0:1b6dab73c06b 1030 } else {
yihui 0:1b6dab73c06b 1031 tmp[4] = 0xA3;
yihui 0:1b6dab73c06b 1032 tmp[5] = 0xA3;
yihui 0:1b6dab73c06b 1033 tmp[6] = 0xA3;
yihui 0:1b6dab73c06b 1034 }
yihui 0:1b6dab73c06b 1035 tmp[7] = 0xA3;
yihui 0:1b6dab73c06b 1036 tmp[8] = 0xA3;
yihui 0:1b6dab73c06b 1037 tmp[9] = 0xA3;
yihui 0:1b6dab73c06b 1038 mpu_write_mem(CFG_15,10,tmp);
yihui 0:1b6dab73c06b 1039
yihui 0:1b6dab73c06b 1040 /* Send gesture data to the FIFO. */
yihui 0:1b6dab73c06b 1041 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
yihui 0:1b6dab73c06b 1042 tmp[0] = DINA20;
yihui 0:1b6dab73c06b 1043 else
yihui 0:1b6dab73c06b 1044 tmp[0] = 0xD8;
yihui 0:1b6dab73c06b 1045 mpu_write_mem(CFG_27,1,tmp);
yihui 0:1b6dab73c06b 1046
yihui 0:1b6dab73c06b 1047 if (mask & DMP_FEATURE_GYRO_CAL)
yihui 0:1b6dab73c06b 1048 dmp_enable_gyro_cal(1);
yihui 0:1b6dab73c06b 1049 else
yihui 0:1b6dab73c06b 1050 dmp_enable_gyro_cal(0);
yihui 0:1b6dab73c06b 1051
yihui 0:1b6dab73c06b 1052 if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
yihui 0:1b6dab73c06b 1053 if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
yihui 0:1b6dab73c06b 1054 tmp[0] = 0xB2;
yihui 0:1b6dab73c06b 1055 tmp[1] = 0x8B;
yihui 0:1b6dab73c06b 1056 tmp[2] = 0xB6;
yihui 0:1b6dab73c06b 1057 tmp[3] = 0x9B;
yihui 0:1b6dab73c06b 1058 } else {
yihui 0:1b6dab73c06b 1059 tmp[0] = DINAC0;
yihui 0:1b6dab73c06b 1060 tmp[1] = DINA80;
yihui 0:1b6dab73c06b 1061 tmp[2] = DINAC2;
yihui 0:1b6dab73c06b 1062 tmp[3] = DINA90;
yihui 0:1b6dab73c06b 1063 }
yihui 0:1b6dab73c06b 1064 mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
yihui 0:1b6dab73c06b 1065 }
yihui 0:1b6dab73c06b 1066
yihui 0:1b6dab73c06b 1067 if (mask & DMP_FEATURE_TAP) {
yihui 0:1b6dab73c06b 1068 /* Enable tap. */
yihui 0:1b6dab73c06b 1069 tmp[0] = 0xF8;
yihui 0:1b6dab73c06b 1070 mpu_write_mem(CFG_20, 1, tmp);
yihui 0:1b6dab73c06b 1071 dmp_set_tap_thresh(TAP_XYZ, 250);
yihui 0:1b6dab73c06b 1072 dmp_set_tap_axes(TAP_XYZ);
yihui 0:1b6dab73c06b 1073 dmp_set_tap_count(1);
yihui 0:1b6dab73c06b 1074 dmp_set_tap_time(100);
yihui 0:1b6dab73c06b 1075 dmp_set_tap_time_multi(500);
yihui 0:1b6dab73c06b 1076
yihui 0:1b6dab73c06b 1077 dmp_set_shake_reject_thresh(GYRO_SF, 200);
yihui 0:1b6dab73c06b 1078 dmp_set_shake_reject_time(40);
yihui 0:1b6dab73c06b 1079 dmp_set_shake_reject_timeout(10);
yihui 0:1b6dab73c06b 1080 } else {
yihui 0:1b6dab73c06b 1081 tmp[0] = 0xD8;
yihui 0:1b6dab73c06b 1082 mpu_write_mem(CFG_20, 1, tmp);
yihui 0:1b6dab73c06b 1083 }
yihui 0:1b6dab73c06b 1084
yihui 0:1b6dab73c06b 1085 if (mask & DMP_FEATURE_ANDROID_ORIENT) {
yihui 0:1b6dab73c06b 1086 tmp[0] = 0xD9;
yihui 0:1b6dab73c06b 1087 } else
yihui 0:1b6dab73c06b 1088 tmp[0] = 0xD8;
yihui 0:1b6dab73c06b 1089 mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
yihui 0:1b6dab73c06b 1090
yihui 0:1b6dab73c06b 1091 if (mask & DMP_FEATURE_LP_QUAT)
yihui 0:1b6dab73c06b 1092 dmp_enable_lp_quat(1);
yihui 0:1b6dab73c06b 1093 else
yihui 0:1b6dab73c06b 1094 dmp_enable_lp_quat(0);
yihui 0:1b6dab73c06b 1095
yihui 0:1b6dab73c06b 1096 if (mask & DMP_FEATURE_6X_LP_QUAT)
yihui 0:1b6dab73c06b 1097 dmp_enable_6x_lp_quat(1);
yihui 0:1b6dab73c06b 1098 else
yihui 0:1b6dab73c06b 1099 dmp_enable_6x_lp_quat(0);
yihui 0:1b6dab73c06b 1100
yihui 0:1b6dab73c06b 1101 /* Pedometer is always enabled. */
yihui 0:1b6dab73c06b 1102 dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
yihui 0:1b6dab73c06b 1103 mpu_reset_fifo();
yihui 0:1b6dab73c06b 1104
yihui 0:1b6dab73c06b 1105 dmp.packet_length = 0;
yihui 0:1b6dab73c06b 1106 if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
yihui 0:1b6dab73c06b 1107 dmp.packet_length += 6;
yihui 0:1b6dab73c06b 1108 if (mask & DMP_FEATURE_SEND_ANY_GYRO)
yihui 0:1b6dab73c06b 1109 dmp.packet_length += 6;
yihui 0:1b6dab73c06b 1110 if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
yihui 0:1b6dab73c06b 1111 dmp.packet_length += 16;
yihui 0:1b6dab73c06b 1112 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
yihui 0:1b6dab73c06b 1113 dmp.packet_length += 4;
yihui 0:1b6dab73c06b 1114
yihui 0:1b6dab73c06b 1115 return 0;
yihui 0:1b6dab73c06b 1116 }
yihui 0:1b6dab73c06b 1117
yihui 0:1b6dab73c06b 1118 /**
yihui 0:1b6dab73c06b 1119 * @brief Get list of currently enabled DMP features.
yihui 0:1b6dab73c06b 1120 * @param[out] Mask of enabled features.
yihui 0:1b6dab73c06b 1121 * @return 0 if successful.
yihui 0:1b6dab73c06b 1122 */
yihui 0:1b6dab73c06b 1123 int dmp_get_enabled_features(unsigned short *mask)
yihui 0:1b6dab73c06b 1124 {
yihui 0:1b6dab73c06b 1125 mask[0] = dmp.feature_mask;
yihui 0:1b6dab73c06b 1126 return 0;
yihui 0:1b6dab73c06b 1127 }
yihui 0:1b6dab73c06b 1128
yihui 0:1b6dab73c06b 1129 /**
yihui 0:1b6dab73c06b 1130 * @brief Calibrate the gyro data in the DMP.
yihui 0:1b6dab73c06b 1131 * After eight seconds of no motion, the DMP will compute gyro biases and
yihui 0:1b6dab73c06b 1132 * subtract them from the quaternion output. If @e dmp_enable_feature is
yihui 0:1b6dab73c06b 1133 * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
yihui 0:1b6dab73c06b 1134 * subtracted from the gyro output.
yihui 0:1b6dab73c06b 1135 * @param[in] enable 1 to enable gyro calibration.
yihui 0:1b6dab73c06b 1136 * @return 0 if successful.
yihui 0:1b6dab73c06b 1137 */
yihui 0:1b6dab73c06b 1138 int dmp_enable_gyro_cal(unsigned char enable)
yihui 0:1b6dab73c06b 1139 {
yihui 0:1b6dab73c06b 1140 if (enable) {
yihui 0:1b6dab73c06b 1141 unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
yihui 0:1b6dab73c06b 1142 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
yihui 0:1b6dab73c06b 1143 } else {
yihui 0:1b6dab73c06b 1144 unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
yihui 0:1b6dab73c06b 1145 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
yihui 0:1b6dab73c06b 1146 }
yihui 0:1b6dab73c06b 1147 }
yihui 0:1b6dab73c06b 1148
yihui 0:1b6dab73c06b 1149 /**
yihui 0:1b6dab73c06b 1150 * @brief Generate 3-axis quaternions from the DMP.
yihui 0:1b6dab73c06b 1151 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
yihui 0:1b6dab73c06b 1152 * exclusive.
yihui 0:1b6dab73c06b 1153 * @param[in] enable 1 to enable 3-axis quaternion.
yihui 0:1b6dab73c06b 1154 * @return 0 if successful.
yihui 0:1b6dab73c06b 1155 */
yihui 0:1b6dab73c06b 1156 int dmp_enable_lp_quat(unsigned char enable)
yihui 0:1b6dab73c06b 1157 {
yihui 0:1b6dab73c06b 1158 unsigned char regs[4];
yihui 0:1b6dab73c06b 1159 if (enable) {
yihui 0:1b6dab73c06b 1160 regs[0] = DINBC0;
yihui 0:1b6dab73c06b 1161 regs[1] = DINBC2;
yihui 0:1b6dab73c06b 1162 regs[2] = DINBC4;
yihui 0:1b6dab73c06b 1163 regs[3] = DINBC6;
yihui 0:1b6dab73c06b 1164 }
yihui 0:1b6dab73c06b 1165 else
yihui 0:1b6dab73c06b 1166 memset(regs, 0x8B, 4);
yihui 0:1b6dab73c06b 1167
yihui 0:1b6dab73c06b 1168 mpu_write_mem(CFG_LP_QUAT, 4, regs);
yihui 0:1b6dab73c06b 1169
yihui 0:1b6dab73c06b 1170 return mpu_reset_fifo();
yihui 0:1b6dab73c06b 1171 }
yihui 0:1b6dab73c06b 1172
yihui 0:1b6dab73c06b 1173 /**
yihui 0:1b6dab73c06b 1174 * @brief Generate 6-axis quaternions from the DMP.
yihui 0:1b6dab73c06b 1175 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
yihui 0:1b6dab73c06b 1176 * exclusive.
yihui 0:1b6dab73c06b 1177 * @param[in] enable 1 to enable 6-axis quaternion.
yihui 0:1b6dab73c06b 1178 * @return 0 if successful.
yihui 0:1b6dab73c06b 1179 */
yihui 0:1b6dab73c06b 1180 int dmp_enable_6x_lp_quat(unsigned char enable)
yihui 0:1b6dab73c06b 1181 {
yihui 0:1b6dab73c06b 1182 unsigned char regs[4];
yihui 0:1b6dab73c06b 1183 if (enable) {
yihui 0:1b6dab73c06b 1184 regs[0] = DINA20;
yihui 0:1b6dab73c06b 1185 regs[1] = DINA28;
yihui 0:1b6dab73c06b 1186 regs[2] = DINA30;
yihui 0:1b6dab73c06b 1187 regs[3] = DINA38;
yihui 0:1b6dab73c06b 1188 } else
yihui 0:1b6dab73c06b 1189 memset(regs, 0xA3, 4);
yihui 0:1b6dab73c06b 1190
yihui 0:1b6dab73c06b 1191 mpu_write_mem(CFG_8, 4, regs);
yihui 0:1b6dab73c06b 1192
yihui 0:1b6dab73c06b 1193 return mpu_reset_fifo();
yihui 0:1b6dab73c06b 1194 }
yihui 0:1b6dab73c06b 1195
yihui 0:1b6dab73c06b 1196 /**
yihui 0:1b6dab73c06b 1197 * @brief Decode the four-byte gesture data and execute any callbacks.
yihui 0:1b6dab73c06b 1198 * @param[in] gesture Gesture data from DMP packet.
yihui 0:1b6dab73c06b 1199 * @return 0 if successful.
yihui 0:1b6dab73c06b 1200 */
yihui 0:1b6dab73c06b 1201 static int decode_gesture(unsigned char *gesture)
yihui 0:1b6dab73c06b 1202 {
yihui 0:1b6dab73c06b 1203 unsigned char tap, android_orient;
yihui 0:1b6dab73c06b 1204
yihui 0:1b6dab73c06b 1205 android_orient = gesture[3] & 0xC0;
yihui 0:1b6dab73c06b 1206 tap = 0x3F & gesture[3];
yihui 0:1b6dab73c06b 1207
yihui 0:1b6dab73c06b 1208 if (gesture[1] & INT_SRC_TAP) {
yihui 0:1b6dab73c06b 1209 unsigned char direction, count;
yihui 0:1b6dab73c06b 1210 direction = tap >> 3;
yihui 0:1b6dab73c06b 1211 count = (tap % 8) + 1;
yihui 0:1b6dab73c06b 1212 if (dmp.tap_cb)
yihui 0:1b6dab73c06b 1213 dmp.tap_cb(direction, count);
yihui 0:1b6dab73c06b 1214 }
yihui 0:1b6dab73c06b 1215
yihui 0:1b6dab73c06b 1216 if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
yihui 0:1b6dab73c06b 1217 if (dmp.android_orient_cb)
yihui 0:1b6dab73c06b 1218 dmp.android_orient_cb(android_orient >> 6);
yihui 0:1b6dab73c06b 1219 }
yihui 0:1b6dab73c06b 1220
yihui 0:1b6dab73c06b 1221 return 0;
yihui 0:1b6dab73c06b 1222 }
yihui 0:1b6dab73c06b 1223
yihui 0:1b6dab73c06b 1224 /**
yihui 0:1b6dab73c06b 1225 * @brief Specify when a DMP interrupt should occur.
yihui 0:1b6dab73c06b 1226 * A DMP interrupt can be configured to trigger on either of the two
yihui 0:1b6dab73c06b 1227 * conditions below:
yihui 0:1b6dab73c06b 1228 * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
yihui 0:1b6dab73c06b 1229 * \n b. A tap event has been detected.
yihui 0:1b6dab73c06b 1230 * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
yihui 0:1b6dab73c06b 1231 * @return 0 if successful.
yihui 0:1b6dab73c06b 1232 */
yihui 0:1b6dab73c06b 1233 int dmp_set_interrupt_mode(unsigned char mode)
yihui 0:1b6dab73c06b 1234 {
yihui 0:1b6dab73c06b 1235 const unsigned char regs_continuous[11] =
yihui 0:1b6dab73c06b 1236 {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
yihui 0:1b6dab73c06b 1237 const unsigned char regs_gesture[11] =
yihui 0:1b6dab73c06b 1238 {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
yihui 0:1b6dab73c06b 1239
yihui 0:1b6dab73c06b 1240 switch (mode) {
yihui 0:1b6dab73c06b 1241 case DMP_INT_CONTINUOUS:
yihui 0:1b6dab73c06b 1242 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
yihui 0:1b6dab73c06b 1243 (unsigned char*)regs_continuous);
yihui 0:1b6dab73c06b 1244 case DMP_INT_GESTURE:
yihui 0:1b6dab73c06b 1245 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
yihui 0:1b6dab73c06b 1246 (unsigned char*)regs_gesture);
yihui 0:1b6dab73c06b 1247 default:
yihui 0:1b6dab73c06b 1248 return -1;
yihui 0:1b6dab73c06b 1249 }
yihui 0:1b6dab73c06b 1250 }
yihui 0:1b6dab73c06b 1251
yihui 0:1b6dab73c06b 1252 /**
yihui 0:1b6dab73c06b 1253 * @brief Get one packet from the FIFO.
yihui 0:1b6dab73c06b 1254 * If @e sensors does not contain a particular sensor, disregard the data
yihui 0:1b6dab73c06b 1255 * returned to that pointer.
yihui 0:1b6dab73c06b 1256 * \n @e sensors can contain a combination of the following flags:
yihui 0:1b6dab73c06b 1257 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
yihui 0:1b6dab73c06b 1258 * \n INV_XYZ_GYRO
yihui 0:1b6dab73c06b 1259 * \n INV_XYZ_ACCEL
yihui 0:1b6dab73c06b 1260 * \n INV_WXYZ_QUAT
yihui 0:1b6dab73c06b 1261 * \n If the FIFO has no new data, @e sensors will be zero.
yihui 0:1b6dab73c06b 1262 * \n If the FIFO is disabled, @e sensors will be zero and this function will
yihui 0:1b6dab73c06b 1263 * return a non-zero error code.
yihui 0:1b6dab73c06b 1264 * @param[out] gyro Gyro data in hardware units.
yihui 0:1b6dab73c06b 1265 * @param[out] accel Accel data in hardware units.
yihui 0:1b6dab73c06b 1266 * @param[out] quat 3-axis quaternion data in hardware units.
yihui 0:1b6dab73c06b 1267 * @param[out] timestamp Timestamp in milliseconds.
yihui 0:1b6dab73c06b 1268 * @param[out] sensors Mask of sensors read from FIFO.
yihui 0:1b6dab73c06b 1269 * @param[out] more Number of remaining packets.
yihui 0:1b6dab73c06b 1270 * @return 0 if successful.
yihui 0:1b6dab73c06b 1271 */
yihui 0:1b6dab73c06b 1272 int dmp_read_fifo(short *gyro, short *accel, long *quat,
yihui 0:1b6dab73c06b 1273 unsigned long *timestamp, short *sensors, unsigned char *more)
yihui 0:1b6dab73c06b 1274 {
yihui 0:1b6dab73c06b 1275 unsigned char fifo_data[MAX_PACKET_LENGTH];
yihui 0:1b6dab73c06b 1276 unsigned char ii = 0;
yihui 0:1b6dab73c06b 1277
yihui 0:1b6dab73c06b 1278 /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
yihui 0:1b6dab73c06b 1279 * cache this value and save some cycles.
yihui 0:1b6dab73c06b 1280 */
yihui 0:1b6dab73c06b 1281 sensors[0] = 0;
yihui 0:1b6dab73c06b 1282
yihui 0:1b6dab73c06b 1283 /* Get a packet. */
yihui 0:1b6dab73c06b 1284 if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
yihui 0:1b6dab73c06b 1285 return -1;
yihui 0:1b6dab73c06b 1286
yihui 0:1b6dab73c06b 1287 /* Parse DMP packet. */
yihui 0:1b6dab73c06b 1288 if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
yihui 0:1b6dab73c06b 1289 #ifdef FIFO_CORRUPTION_CHECK
yihui 0:1b6dab73c06b 1290 long quat_q14[4], quat_mag_sq;
yihui 0:1b6dab73c06b 1291 #endif
yihui 0:1b6dab73c06b 1292 quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
yihui 0:1b6dab73c06b 1293 ((long)fifo_data[2] << 8) | fifo_data[3];
yihui 0:1b6dab73c06b 1294 quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
yihui 0:1b6dab73c06b 1295 ((long)fifo_data[6] << 8) | fifo_data[7];
yihui 0:1b6dab73c06b 1296 quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
yihui 0:1b6dab73c06b 1297 ((long)fifo_data[10] << 8) | fifo_data[11];
yihui 0:1b6dab73c06b 1298 quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
yihui 0:1b6dab73c06b 1299 ((long)fifo_data[14] << 8) | fifo_data[15];
yihui 0:1b6dab73c06b 1300 ii += 16;
yihui 0:1b6dab73c06b 1301 #ifdef FIFO_CORRUPTION_CHECK
yihui 0:1b6dab73c06b 1302 /* We can detect a corrupted FIFO by monitoring the quaternion data and
yihui 0:1b6dab73c06b 1303 * ensuring that the magnitude is always normalized to one. This
yihui 0:1b6dab73c06b 1304 * shouldn't happen in normal operation, but if an I2C error occurs,
yihui 0:1b6dab73c06b 1305 * the FIFO reads might become misaligned.
yihui 0:1b6dab73c06b 1306 *
yihui 0:1b6dab73c06b 1307 * Let's start by scaling down the quaternion data to avoid long long
yihui 0:1b6dab73c06b 1308 * math.
yihui 0:1b6dab73c06b 1309 */
yihui 0:1b6dab73c06b 1310 quat_q14[0] = quat[0] >> 16;
yihui 0:1b6dab73c06b 1311 quat_q14[1] = quat[1] >> 16;
yihui 0:1b6dab73c06b 1312 quat_q14[2] = quat[2] >> 16;
yihui 0:1b6dab73c06b 1313 quat_q14[3] = quat[3] >> 16;
yihui 0:1b6dab73c06b 1314 quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
yihui 0:1b6dab73c06b 1315 quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
yihui 0:1b6dab73c06b 1316 if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
yihui 0:1b6dab73c06b 1317 (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
yihui 0:1b6dab73c06b 1318 /* Quaternion is outside of the acceptable threshold. */
yihui 0:1b6dab73c06b 1319 mpu_reset_fifo();
yihui 0:1b6dab73c06b 1320 sensors[0] = 0;
yihui 0:1b6dab73c06b 1321 return -1;
yihui 0:1b6dab73c06b 1322 }
yihui 0:1b6dab73c06b 1323 sensors[0] |= INV_WXYZ_QUAT;
yihui 0:1b6dab73c06b 1324 #endif
yihui 0:1b6dab73c06b 1325 }
yihui 0:1b6dab73c06b 1326
yihui 0:1b6dab73c06b 1327 if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
yihui 0:1b6dab73c06b 1328 accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
yihui 0:1b6dab73c06b 1329 accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
yihui 0:1b6dab73c06b 1330 accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
yihui 0:1b6dab73c06b 1331 ii += 6;
yihui 0:1b6dab73c06b 1332 sensors[0] |= INV_XYZ_ACCEL;
yihui 0:1b6dab73c06b 1333 }
yihui 0:1b6dab73c06b 1334
yihui 0:1b6dab73c06b 1335 if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
yihui 0:1b6dab73c06b 1336 gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
yihui 0:1b6dab73c06b 1337 gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
yihui 0:1b6dab73c06b 1338 gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
yihui 0:1b6dab73c06b 1339 ii += 6;
yihui 0:1b6dab73c06b 1340 sensors[0] |= INV_XYZ_GYRO;
yihui 0:1b6dab73c06b 1341 }
yihui 0:1b6dab73c06b 1342
yihui 0:1b6dab73c06b 1343 /* Gesture data is at the end of the DMP packet. Parse it and call
yihui 0:1b6dab73c06b 1344 * the gesture callbacks (if registered).
yihui 0:1b6dab73c06b 1345 */
yihui 0:1b6dab73c06b 1346 if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
yihui 0:1b6dab73c06b 1347 decode_gesture(fifo_data + ii);
yihui 0:1b6dab73c06b 1348
yihui 0:1b6dab73c06b 1349 get_ms(timestamp);
yihui 0:1b6dab73c06b 1350 return 0;
yihui 0:1b6dab73c06b 1351 }
yihui 0:1b6dab73c06b 1352
yihui 0:1b6dab73c06b 1353 /**
yihui 0:1b6dab73c06b 1354 * @brief Register a function to be executed on a tap event.
yihui 0:1b6dab73c06b 1355 * The tap direction is represented by one of the following:
yihui 0:1b6dab73c06b 1356 * \n TAP_X_UP
yihui 0:1b6dab73c06b 1357 * \n TAP_X_DOWN
yihui 0:1b6dab73c06b 1358 * \n TAP_Y_UP
yihui 0:1b6dab73c06b 1359 * \n TAP_Y_DOWN
yihui 0:1b6dab73c06b 1360 * \n TAP_Z_UP
yihui 0:1b6dab73c06b 1361 * \n TAP_Z_DOWN
yihui 0:1b6dab73c06b 1362 * @param[in] func Callback function.
yihui 0:1b6dab73c06b 1363 * @return 0 if successful.
yihui 0:1b6dab73c06b 1364 */
yihui 0:1b6dab73c06b 1365 int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
yihui 0:1b6dab73c06b 1366 {
yihui 0:1b6dab73c06b 1367 dmp.tap_cb = func;
yihui 0:1b6dab73c06b 1368 return 0;
yihui 0:1b6dab73c06b 1369 }
yihui 0:1b6dab73c06b 1370
yihui 0:1b6dab73c06b 1371 /**
yihui 0:1b6dab73c06b 1372 * @brief Register a function to be executed on a android orientation event.
yihui 0:1b6dab73c06b 1373 * @param[in] func Callback function.
yihui 0:1b6dab73c06b 1374 * @return 0 if successful.
yihui 0:1b6dab73c06b 1375 */
yihui 0:1b6dab73c06b 1376 int dmp_register_android_orient_cb(void (*func)(unsigned char))
yihui 0:1b6dab73c06b 1377 {
yihui 0:1b6dab73c06b 1378 dmp.android_orient_cb = func;
yihui 0:1b6dab73c06b 1379 return 0;
yihui 0:1b6dab73c06b 1380 }
yihui 0:1b6dab73c06b 1381
yihui 0:1b6dab73c06b 1382 /**
yihui 0:1b6dab73c06b 1383 * @}
yihui 0:1b6dab73c06b 1384 */
yihui 0:1b6dab73c06b 1385