eMPL_MPU - library for MPU6050 and MPU9250, supports both I2…

Users » yihui » Code » eMPL_MPU

library for MPU6050 and MPU9250, supports both I2C and SPI

inv_mpu_dmp_motion_driver.c@0:972f3778c19c, 2015-12-10 (annotated)

Committer:: yihui
Date:: Thu Dec 10 07:39:48 2015 +0000
Revision:: 0:972f3778c19c

initial

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	10 Dec 2015
Imports:	127
Forks:	0
Commits:	1
Dependents:	1
Dependencies:	0
Followers:	4

inv_mpu_dmp_motion_driver.c@0:972f3778c19c, 2015-12-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning