MPL6_1 - Motion Processing Library adapted to mbed

Users » janekm » Code » MPL6_1

Motion Processing Library adapted to mbed

Dependents: openwear-lifelogger-example

inv_mpu_dmp_motion_driver.c@1:6221b0518f94, 2014-09-16 (annotated)

Committer:: janekm
Date:: Tue Sep 16 22:42:12 2014 +0000
Revision:: 1:6221b0518f94
Parent:: 0:61a7cadab29e

I forgot

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	16 Sep 2014
Imports:	1
Forks:	0
Commits:	2
Dependents:	1
Dependencies:	0
Followers:	1

This repository is Public (Unlisted).

inv_mpu_dmp_motion_driver.c@1:6221b0518f94, 2014-09-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning