MotionDriverv512 - InvenSense Motion Driver v5.1.2 ported

Users » oprospero » Code » MotionDriverv512

InvenSense Motion Driver v5.1.2 ported

inv_mpu_dmp_motion_driver.cpp@3:8ac73f9099ab, 2014-10-15 (annotated)

Committer:: oprospero
Date:: Wed Oct 15 04:59:22 2014 +0000
Revision:: 3:8ac73f9099ab
Parent:: 0:4dd021344a6b
Child:: 4:2cb380415dc7

added reset

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	03 Jun 2014
Imports:	41
Forks:	0
Commits:	5
Dependents:	1
Dependencies:	0
Followers:	2

inv_mpu_dmp_motion_driver.cpp@3:8ac73f9099ab, 2014-10-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning