Yihui Xiong / Mbed 2 deprecated Seeed_nRF51822_Motion

initial

Dependencies:   mbed

eMPL_MPU/inv_mpu_dmp_motion_driver.c@1:9d3b497333c0, 2016-01-11 (annotated)

Committer:
yihui
Date:
Mon Jan 11 02:49:25 2016 +0000
Revision:
1:9d3b497333c0
Parent:
0:638edba3adf6 
use mbed lib

Who changed what in which revision?

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