InvenSense Motion Driver v5.1.2 ported

Dependents:   Sensorv2

Committer:
oprospero
Date:
Tue Jun 03 07:54:12 2014 +0000
Revision:
0:4dd021344a6b
Child:
3:8ac73f9099ab
Initial MotionDriver v5.1.2 ported from InvenSense

Who changed what in which revision?

UserRevisionLine numberNew 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 0:4dd021344a6b 546
oprospero 0:4dd021344a6b 547 /**
oprospero 0:4dd021344a6b 548 * @brief Load the DMP with this image.
oprospero 0:4dd021344a6b 549 * @return 0 if successful.
oprospero 0:4dd021344a6b 550 */
oprospero 0:4dd021344a6b 551 int DMP_Motion_Driver::dmp_load_motion_driver_firmware(void)
oprospero 0:4dd021344a6b 552 {
oprospero 0:4dd021344a6b 553 return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
oprospero 0:4dd021344a6b 554 DMP_SAMPLE_RATE);
oprospero 0:4dd021344a6b 555 }
oprospero 0:4dd021344a6b 556
oprospero 0:4dd021344a6b 557 /**
oprospero 0:4dd021344a6b 558 * @brief Push gyro and accel orientation to the DMP.
oprospero 0:4dd021344a6b 559 * The orientation is represented here as the output of
oprospero 0:4dd021344a6b 560 * @e inv_orientation_matrix_to_scalar.
oprospero 0:4dd021344a6b 561 * @param[in] orient Gyro and accel orientation in body frame.
oprospero 0:4dd021344a6b 562 * @return 0 if successful.
oprospero 0:4dd021344a6b 563 */
oprospero 0:4dd021344a6b 564 int DMP_Motion_Driver::dmp_set_orientation(unsigned short orient)
oprospero 0:4dd021344a6b 565 {
oprospero 0:4dd021344a6b 566 unsigned char gyro_regs[3], accel_regs[3];
oprospero 0:4dd021344a6b 567 const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
oprospero 0:4dd021344a6b 568 const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
oprospero 0:4dd021344a6b 569 const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
oprospero 0:4dd021344a6b 570 const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
oprospero 0:4dd021344a6b 571
oprospero 0:4dd021344a6b 572 gyro_regs[0] = gyro_axes[orient & 3];
oprospero 0:4dd021344a6b 573 gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
oprospero 0:4dd021344a6b 574 gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
oprospero 0:4dd021344a6b 575 accel_regs[0] = accel_axes[orient & 3];
oprospero 0:4dd021344a6b 576 accel_regs[1] = accel_axes[(orient >> 3) & 3];
oprospero 0:4dd021344a6b 577 accel_regs[2] = accel_axes[(orient >> 6) & 3];
oprospero 0:4dd021344a6b 578
oprospero 0:4dd021344a6b 579 /* Chip-to-body, axes only. */
oprospero 0:4dd021344a6b 580 if (mpu_write_mem(FCFG_1, 3, gyro_regs))
oprospero 0:4dd021344a6b 581 return -1;
oprospero 0:4dd021344a6b 582 if (mpu_write_mem(FCFG_2, 3, accel_regs))
oprospero 0:4dd021344a6b 583 return -1;
oprospero 0:4dd021344a6b 584
oprospero 0:4dd021344a6b 585 memcpy(gyro_regs, gyro_sign, 3);
oprospero 0:4dd021344a6b 586 memcpy(accel_regs, accel_sign, 3);
oprospero 0:4dd021344a6b 587 if (orient & 4) {
oprospero 0:4dd021344a6b 588 gyro_regs[0] |= 1;
oprospero 0:4dd021344a6b 589 accel_regs[0] |= 1;
oprospero 0:4dd021344a6b 590 }
oprospero 0:4dd021344a6b 591 if (orient & 0x20) {
oprospero 0:4dd021344a6b 592 gyro_regs[1] |= 1;
oprospero 0:4dd021344a6b 593 accel_regs[1] |= 1;
oprospero 0:4dd021344a6b 594 }
oprospero 0:4dd021344a6b 595 if (orient & 0x100) {
oprospero 0:4dd021344a6b 596 gyro_regs[2] |= 1;
oprospero 0:4dd021344a6b 597 accel_regs[2] |= 1;
oprospero 0:4dd021344a6b 598 }
oprospero 0:4dd021344a6b 599
oprospero 0:4dd021344a6b 600 /* Chip-to-body, sign only. */
oprospero 0:4dd021344a6b 601 if (mpu_write_mem(FCFG_3, 3, gyro_regs))
oprospero 0:4dd021344a6b 602 return -1;
oprospero 0:4dd021344a6b 603 if (mpu_write_mem(FCFG_7, 3, accel_regs))
oprospero 0:4dd021344a6b 604 return -1;
oprospero 0:4dd021344a6b 605 dmp.orient = orient;
oprospero 0:4dd021344a6b 606 return 0;
oprospero 0:4dd021344a6b 607 }
oprospero 0:4dd021344a6b 608
oprospero 0:4dd021344a6b 609 /**
oprospero 0:4dd021344a6b 610 * @brief Push gyro biases to the DMP.
oprospero 0:4dd021344a6b 611 * Because the gyro integration is handled in the DMP, any gyro biases
oprospero 0:4dd021344a6b 612 * calculated by the MPL should be pushed down to DMP memory to remove
oprospero 0:4dd021344a6b 613 * 3-axis quaternion drift.
oprospero 0:4dd021344a6b 614 * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
oprospero 0:4dd021344a6b 615 * overwrite the biases written to this location once a new one is computed.
oprospero 0:4dd021344a6b 616 * @param[in] bias Gyro biases in q16.
oprospero 0:4dd021344a6b 617 * @return 0 if successful.
oprospero 0:4dd021344a6b 618 */
oprospero 0:4dd021344a6b 619 int DMP_Motion_Driver::dmp_set_gyro_bias(long *bias)
oprospero 0:4dd021344a6b 620 {
oprospero 0:4dd021344a6b 621 long gyro_bias_body[3];
oprospero 0:4dd021344a6b 622 unsigned char regs[4];
oprospero 0:4dd021344a6b 623
oprospero 0:4dd021344a6b 624 gyro_bias_body[0] = bias[dmp.orient & 3];
oprospero 0:4dd021344a6b 625 if (dmp.orient & 4)
oprospero 0:4dd021344a6b 626 gyro_bias_body[0] *= -1;
oprospero 0:4dd021344a6b 627 gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];
oprospero 0:4dd021344a6b 628 if (dmp.orient & 0x20)
oprospero 0:4dd021344a6b 629 gyro_bias_body[1] *= -1;
oprospero 0:4dd021344a6b 630 gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];
oprospero 0:4dd021344a6b 631 if (dmp.orient & 0x100)
oprospero 0:4dd021344a6b 632 gyro_bias_body[2] *= -1;
oprospero 0:4dd021344a6b 633
oprospero 0:4dd021344a6b 634 #ifdef EMPL_NO_64BIT
oprospero 0:4dd021344a6b 635 gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
oprospero 0:4dd021344a6b 636 gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
oprospero 0:4dd021344a6b 637 gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
oprospero 0:4dd021344a6b 638 #else
oprospero 0:4dd021344a6b 639 gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
oprospero 0:4dd021344a6b 640 gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
oprospero 0:4dd021344a6b 641 gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
oprospero 0:4dd021344a6b 642 #endif
oprospero 0:4dd021344a6b 643
oprospero 0:4dd021344a6b 644 regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
oprospero 0:4dd021344a6b 645 regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
oprospero 0:4dd021344a6b 646 regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
oprospero 0:4dd021344a6b 647 regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
oprospero 0:4dd021344a6b 648 if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
oprospero 0:4dd021344a6b 649 return -1;
oprospero 0:4dd021344a6b 650
oprospero 0:4dd021344a6b 651 regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
oprospero 0:4dd021344a6b 652 regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
oprospero 0:4dd021344a6b 653 regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
oprospero 0:4dd021344a6b 654 regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
oprospero 0:4dd021344a6b 655 if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
oprospero 0:4dd021344a6b 656 return -1;
oprospero 0:4dd021344a6b 657
oprospero 0:4dd021344a6b 658 regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
oprospero 0:4dd021344a6b 659 regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
oprospero 0:4dd021344a6b 660 regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
oprospero 0:4dd021344a6b 661 regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
oprospero 0:4dd021344a6b 662 return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
oprospero 0:4dd021344a6b 663 }
oprospero 0:4dd021344a6b 664
oprospero 0:4dd021344a6b 665 /**
oprospero 0:4dd021344a6b 666 * @brief Push accel biases to the DMP.
oprospero 0:4dd021344a6b 667 * These biases will be removed from the DMP 6-axis quaternion.
oprospero 0:4dd021344a6b 668 * @param[in] bias Accel biases in q16.
oprospero 0:4dd021344a6b 669 * @return 0 if successful.
oprospero 0:4dd021344a6b 670 */
oprospero 0:4dd021344a6b 671 int DMP_Motion_Driver::dmp_set_accel_bias(long *bias)
oprospero 0:4dd021344a6b 672 {
oprospero 0:4dd021344a6b 673 long accel_bias_body[3];
oprospero 0:4dd021344a6b 674 unsigned char regs[12];
oprospero 0:4dd021344a6b 675 long long accel_sf;
oprospero 0:4dd021344a6b 676 unsigned short accel_sens;
oprospero 0:4dd021344a6b 677
oprospero 0:4dd021344a6b 678 mpu_get_accel_sens(&accel_sens);
oprospero 0:4dd021344a6b 679 accel_sf = (long long)accel_sens << 15;
oprospero 0:4dd021344a6b 680 // __no_operation();
oprospero 0:4dd021344a6b 681
oprospero 0:4dd021344a6b 682 accel_bias_body[0] = bias[dmp.orient & 3];
oprospero 0:4dd021344a6b 683 if (dmp.orient & 4)
oprospero 0:4dd021344a6b 684 accel_bias_body[0] *= -1;
oprospero 0:4dd021344a6b 685 accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];
oprospero 0:4dd021344a6b 686 if (dmp.orient & 0x20)
oprospero 0:4dd021344a6b 687 accel_bias_body[1] *= -1;
oprospero 0:4dd021344a6b 688 accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];
oprospero 0:4dd021344a6b 689 if (dmp.orient & 0x100)
oprospero 0:4dd021344a6b 690 accel_bias_body[2] *= -1;
oprospero 0:4dd021344a6b 691
oprospero 0:4dd021344a6b 692 #ifdef EMPL_NO_64BIT
oprospero 0:4dd021344a6b 693 accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
oprospero 0:4dd021344a6b 694 accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
oprospero 0:4dd021344a6b 695 accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
oprospero 0:4dd021344a6b 696 #else
oprospero 0:4dd021344a6b 697 accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
oprospero 0:4dd021344a6b 698 accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
oprospero 0:4dd021344a6b 699 accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
oprospero 0:4dd021344a6b 700 #endif
oprospero 0:4dd021344a6b 701
oprospero 0:4dd021344a6b 702 regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
oprospero 0:4dd021344a6b 703 regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
oprospero 0:4dd021344a6b 704 regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
oprospero 0:4dd021344a6b 705 regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
oprospero 0:4dd021344a6b 706 regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
oprospero 0:4dd021344a6b 707 regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
oprospero 0:4dd021344a6b 708 regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
oprospero 0:4dd021344a6b 709 regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
oprospero 0:4dd021344a6b 710 regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
oprospero 0:4dd021344a6b 711 regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
oprospero 0:4dd021344a6b 712 regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
oprospero 0:4dd021344a6b 713 regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
oprospero 0:4dd021344a6b 714 return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
oprospero 0:4dd021344a6b 715 }
oprospero 0:4dd021344a6b 716
oprospero 0:4dd021344a6b 717 /**
oprospero 0:4dd021344a6b 718 * @brief Set DMP output rate.
oprospero 0:4dd021344a6b 719 * Only used when DMP is on.
oprospero 0:4dd021344a6b 720 * @param[in] rate Desired fifo rate (Hz).
oprospero 0:4dd021344a6b 721 * @return 0 if successful.
oprospero 0:4dd021344a6b 722 */
oprospero 0:4dd021344a6b 723 int DMP_Motion_Driver::dmp_set_fifo_rate(unsigned short rate)
oprospero 0:4dd021344a6b 724 {
oprospero 0:4dd021344a6b 725 const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
oprospero 0:4dd021344a6b 726 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
oprospero 0:4dd021344a6b 727 unsigned short div;
oprospero 0:4dd021344a6b 728 unsigned char tmp[8];
oprospero 0:4dd021344a6b 729
oprospero 0:4dd021344a6b 730 if (rate > DMP_SAMPLE_RATE)
oprospero 0:4dd021344a6b 731 return -1;
oprospero 0:4dd021344a6b 732 div = DMP_SAMPLE_RATE / rate - 1;
oprospero 0:4dd021344a6b 733 tmp[0] = (unsigned char)((div >> 8) & 0xFF);
oprospero 0:4dd021344a6b 734 tmp[1] = (unsigned char)(div & 0xFF);
oprospero 0:4dd021344a6b 735 if (mpu_write_mem(D_0_22, 2, tmp))
oprospero 0:4dd021344a6b 736 return -1;
oprospero 0:4dd021344a6b 737 if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
oprospero 0:4dd021344a6b 738 return -1;
oprospero 0:4dd021344a6b 739
oprospero 0:4dd021344a6b 740 dmp.fifo_rate = rate;
oprospero 0:4dd021344a6b 741 return 0;
oprospero 0:4dd021344a6b 742 }
oprospero 0:4dd021344a6b 743
oprospero 0:4dd021344a6b 744 /**
oprospero 0:4dd021344a6b 745 * @brief Get DMP output rate.
oprospero 0:4dd021344a6b 746 * @param[out] rate Current fifo rate (Hz).
oprospero 0:4dd021344a6b 747 * @return 0 if successful.
oprospero 0:4dd021344a6b 748 */
oprospero 0:4dd021344a6b 749 int DMP_Motion_Driver::dmp_get_fifo_rate(unsigned short *rate)
oprospero 0:4dd021344a6b 750 {
oprospero 0:4dd021344a6b 751 rate[0] = dmp.fifo_rate;
oprospero 0:4dd021344a6b 752 return 0;
oprospero 0:4dd021344a6b 753 }
oprospero 0:4dd021344a6b 754
oprospero 0:4dd021344a6b 755 /**
oprospero 0:4dd021344a6b 756 * @brief Set tap threshold for a specific axis.
oprospero 0:4dd021344a6b 757 * @param[in] axis 1, 2, and 4 for XYZ accel, respectively.
oprospero 0:4dd021344a6b 758 * @param[in] thresh Tap threshold, in mg/ms.
oprospero 0:4dd021344a6b 759 * @return 0 if successful.
oprospero 0:4dd021344a6b 760 */
oprospero 0:4dd021344a6b 761 int DMP_Motion_Driver::dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
oprospero 0:4dd021344a6b 762 {
oprospero 0:4dd021344a6b 763 unsigned char tmp[4], accel_fsr;
oprospero 0:4dd021344a6b 764 float scaled_thresh;
oprospero 0:4dd021344a6b 765 unsigned short dmp_thresh, dmp_thresh_2;
oprospero 0:4dd021344a6b 766 if (!(axis & TAP_XYZ) || thresh > 1600)
oprospero 0:4dd021344a6b 767 return -1;
oprospero 0:4dd021344a6b 768
oprospero 0:4dd021344a6b 769 scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
oprospero 0:4dd021344a6b 770
oprospero 0:4dd021344a6b 771 mpu_get_accel_fsr(&accel_fsr);
oprospero 0:4dd021344a6b 772 switch (accel_fsr) {
oprospero 0:4dd021344a6b 773 case 2:
oprospero 0:4dd021344a6b 774 dmp_thresh = (unsigned short)(scaled_thresh * 16384);
oprospero 0:4dd021344a6b 775 /* dmp_thresh * 0.75 */
oprospero 0:4dd021344a6b 776 dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
oprospero 0:4dd021344a6b 777 break;
oprospero 0:4dd021344a6b 778 case 4:
oprospero 0:4dd021344a6b 779 dmp_thresh = (unsigned short)(scaled_thresh * 8192);
oprospero 0:4dd021344a6b 780 /* dmp_thresh * 0.75 */
oprospero 0:4dd021344a6b 781 dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
oprospero 0:4dd021344a6b 782 break;
oprospero 0:4dd021344a6b 783 case 8:
oprospero 0:4dd021344a6b 784 dmp_thresh = (unsigned short)(scaled_thresh * 4096);
oprospero 0:4dd021344a6b 785 /* dmp_thresh * 0.75 */
oprospero 0:4dd021344a6b 786 dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
oprospero 0:4dd021344a6b 787 break;
oprospero 0:4dd021344a6b 788 case 16:
oprospero 0:4dd021344a6b 789 dmp_thresh = (unsigned short)(scaled_thresh * 2048);
oprospero 0:4dd021344a6b 790 /* dmp_thresh * 0.75 */
oprospero 0:4dd021344a6b 791 dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
oprospero 0:4dd021344a6b 792 break;
oprospero 0:4dd021344a6b 793 default:
oprospero 0:4dd021344a6b 794 return -1;
oprospero 0:4dd021344a6b 795 }
oprospero 0:4dd021344a6b 796 tmp[0] = (unsigned char)(dmp_thresh >> 8);
oprospero 0:4dd021344a6b 797 tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
oprospero 0:4dd021344a6b 798 tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
oprospero 0:4dd021344a6b 799 tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
oprospero 0:4dd021344a6b 800
oprospero 0:4dd021344a6b 801 if (axis & TAP_X) {
oprospero 0:4dd021344a6b 802 if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
oprospero 0:4dd021344a6b 803 return -1;
oprospero 0:4dd021344a6b 804 if (mpu_write_mem(D_1_36, 2, tmp+2))
oprospero 0:4dd021344a6b 805 return -1;
oprospero 0:4dd021344a6b 806 }
oprospero 0:4dd021344a6b 807 if (axis & TAP_Y) {
oprospero 0:4dd021344a6b 808 if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
oprospero 0:4dd021344a6b 809 return -1;
oprospero 0:4dd021344a6b 810 if (mpu_write_mem(D_1_40, 2, tmp+2))
oprospero 0:4dd021344a6b 811 return -1;
oprospero 0:4dd021344a6b 812 }
oprospero 0:4dd021344a6b 813 if (axis & TAP_Z) {
oprospero 0:4dd021344a6b 814 if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
oprospero 0:4dd021344a6b 815 return -1;
oprospero 0:4dd021344a6b 816 if (mpu_write_mem(D_1_44, 2, tmp+2))
oprospero 0:4dd021344a6b 817 return -1;
oprospero 0:4dd021344a6b 818 }
oprospero 0:4dd021344a6b 819 return 0;
oprospero 0:4dd021344a6b 820 }
oprospero 0:4dd021344a6b 821
oprospero 0:4dd021344a6b 822 /**
oprospero 0:4dd021344a6b 823 * @brief Set which axes will register a tap.
oprospero 0:4dd021344a6b 824 * @param[in] axis 1, 2, and 4 for XYZ, respectively.
oprospero 0:4dd021344a6b 825 * @return 0 if successful.
oprospero 0:4dd021344a6b 826 */
oprospero 0:4dd021344a6b 827 int DMP_Motion_Driver::dmp_set_tap_axes(unsigned char axis)
oprospero 0:4dd021344a6b 828 {
oprospero 0:4dd021344a6b 829 unsigned char tmp = 0;
oprospero 0:4dd021344a6b 830
oprospero 0:4dd021344a6b 831 if (axis & TAP_X)
oprospero 0:4dd021344a6b 832 tmp |= 0x30;
oprospero 0:4dd021344a6b 833 if (axis & TAP_Y)
oprospero 0:4dd021344a6b 834 tmp |= 0x0C;
oprospero 0:4dd021344a6b 835 if (axis & TAP_Z)
oprospero 0:4dd021344a6b 836 tmp |= 0x03;
oprospero 0:4dd021344a6b 837 return mpu_write_mem(D_1_72, 1, &tmp);
oprospero 0:4dd021344a6b 838 }
oprospero 0:4dd021344a6b 839
oprospero 0:4dd021344a6b 840 /**
oprospero 0:4dd021344a6b 841 * @brief Set minimum number of taps needed for an interrupt.
oprospero 0:4dd021344a6b 842 * @param[in] min_taps Minimum consecutive taps (1-4).
oprospero 0:4dd021344a6b 843 * @return 0 if successful.
oprospero 0:4dd021344a6b 844 */
oprospero 0:4dd021344a6b 845 int DMP_Motion_Driver::dmp_set_tap_count(unsigned char min_taps)
oprospero 0:4dd021344a6b 846 {
oprospero 0:4dd021344a6b 847 unsigned char tmp;
oprospero 0:4dd021344a6b 848
oprospero 0:4dd021344a6b 849 if (min_taps < 1)
oprospero 0:4dd021344a6b 850 min_taps = 1;
oprospero 0:4dd021344a6b 851 else if (min_taps > 4)
oprospero 0:4dd021344a6b 852 min_taps = 4;
oprospero 0:4dd021344a6b 853
oprospero 0:4dd021344a6b 854 tmp = min_taps - 1;
oprospero 0:4dd021344a6b 855 return mpu_write_mem(D_1_79, 1, &tmp);
oprospero 0:4dd021344a6b 856 }
oprospero 0:4dd021344a6b 857
oprospero 0:4dd021344a6b 858 /**
oprospero 0:4dd021344a6b 859 * @brief Set length between valid taps.
oprospero 0:4dd021344a6b 860 * @param[in] time Milliseconds between taps.
oprospero 0:4dd021344a6b 861 * @return 0 if successful.
oprospero 0:4dd021344a6b 862 */
oprospero 0:4dd021344a6b 863 int DMP_Motion_Driver::dmp_set_tap_time(unsigned short time)
oprospero 0:4dd021344a6b 864 {
oprospero 0:4dd021344a6b 865 unsigned short dmp_time;
oprospero 0:4dd021344a6b 866 unsigned char tmp[2];
oprospero 0:4dd021344a6b 867
oprospero 0:4dd021344a6b 868 dmp_time = time / (1000 / DMP_SAMPLE_RATE);
oprospero 0:4dd021344a6b 869 tmp[0] = (unsigned char)(dmp_time >> 8);
oprospero 0:4dd021344a6b 870 tmp[1] = (unsigned char)(dmp_time & 0xFF);
oprospero 0:4dd021344a6b 871 return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
oprospero 0:4dd021344a6b 872 }
oprospero 0:4dd021344a6b 873
oprospero 0:4dd021344a6b 874 /**
oprospero 0:4dd021344a6b 875 * @brief Set max time between taps to register as a multi-tap.
oprospero 0:4dd021344a6b 876 * @param[in] time Max milliseconds between taps.
oprospero 0:4dd021344a6b 877 * @return 0 if successful.
oprospero 0:4dd021344a6b 878 */
oprospero 0:4dd021344a6b 879 int DMP_Motion_Driver::dmp_set_tap_time_multi(unsigned short time)
oprospero 0:4dd021344a6b 880 {
oprospero 0:4dd021344a6b 881 unsigned short dmp_time;
oprospero 0:4dd021344a6b 882 unsigned char tmp[2];
oprospero 0:4dd021344a6b 883
oprospero 0:4dd021344a6b 884 dmp_time = time / (1000 / DMP_SAMPLE_RATE);
oprospero 0:4dd021344a6b 885 tmp[0] = (unsigned char)(dmp_time >> 8);
oprospero 0:4dd021344a6b 886 tmp[1] = (unsigned char)(dmp_time & 0xFF);
oprospero 0:4dd021344a6b 887 return mpu_write_mem(D_1_218, 2, tmp);
oprospero 0:4dd021344a6b 888 }
oprospero 0:4dd021344a6b 889
oprospero 0:4dd021344a6b 890 /**
oprospero 0:4dd021344a6b 891 * @brief Set shake rejection threshold.
oprospero 0:4dd021344a6b 892 * If the DMP detects a gyro sample larger than @e thresh, taps are rejected.
oprospero 0:4dd021344a6b 893 * @param[in] sf Gyro scale factor.
oprospero 0:4dd021344a6b 894 * @param[in] thresh Gyro threshold in dps.
oprospero 0:4dd021344a6b 895 * @return 0 if successful.
oprospero 0:4dd021344a6b 896 */
oprospero 0:4dd021344a6b 897 int DMP_Motion_Driver::dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
oprospero 0:4dd021344a6b 898 {
oprospero 0:4dd021344a6b 899 unsigned char tmp[4];
oprospero 0:4dd021344a6b 900 long thresh_scaled = sf / 1000 * thresh;
oprospero 0:4dd021344a6b 901 tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
oprospero 0:4dd021344a6b 902 tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
oprospero 0:4dd021344a6b 903 tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
oprospero 0:4dd021344a6b 904 tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
oprospero 0:4dd021344a6b 905 return mpu_write_mem(D_1_92, 4, tmp);
oprospero 0:4dd021344a6b 906 }
oprospero 0:4dd021344a6b 907
oprospero 0:4dd021344a6b 908 /**
oprospero 0:4dd021344a6b 909 * @brief Set shake rejection time.
oprospero 0:4dd021344a6b 910 * Sets the length of time that the gyro must be outside of the threshold set
oprospero 0:4dd021344a6b 911 * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory
oprospero 0:4dd021344a6b 912 * 60 ms is added to this parameter.
oprospero 0:4dd021344a6b 913 * @param[in] time Time in milliseconds.
oprospero 0:4dd021344a6b 914 * @return 0 if successful.
oprospero 0:4dd021344a6b 915 */
oprospero 0:4dd021344a6b 916 int DMP_Motion_Driver::dmp_set_shake_reject_time(unsigned short time)
oprospero 0:4dd021344a6b 917 {
oprospero 0:4dd021344a6b 918 unsigned char tmp[2];
oprospero 0:4dd021344a6b 919
oprospero 0:4dd021344a6b 920 time /= (1000 / DMP_SAMPLE_RATE);
oprospero 0:4dd021344a6b 921 tmp[0] = time >> 8;
oprospero 0:4dd021344a6b 922 tmp[1] = time & 0xFF;
oprospero 0:4dd021344a6b 923 return mpu_write_mem(D_1_90,2,tmp);
oprospero 0:4dd021344a6b 924 }
oprospero 0:4dd021344a6b 925
oprospero 0:4dd021344a6b 926 /**
oprospero 0:4dd021344a6b 927 * @brief Set shake rejection timeout.
oprospero 0:4dd021344a6b 928 * Sets the length of time after a shake rejection that the gyro must stay
oprospero 0:4dd021344a6b 929 * inside of the threshold before taps can be detected again. A mandatory
oprospero 0:4dd021344a6b 930 * 60 ms is added to this parameter.
oprospero 0:4dd021344a6b 931 * @param[in] time Time in milliseconds.
oprospero 0:4dd021344a6b 932 * @return 0 if successful.
oprospero 0:4dd021344a6b 933 */
oprospero 0:4dd021344a6b 934 int DMP_Motion_Driver::dmp_set_shake_reject_timeout(unsigned short time)
oprospero 0:4dd021344a6b 935 {
oprospero 0:4dd021344a6b 936 unsigned char tmp[2];
oprospero 0:4dd021344a6b 937
oprospero 0:4dd021344a6b 938 time /= (1000 / DMP_SAMPLE_RATE);
oprospero 0:4dd021344a6b 939 tmp[0] = time >> 8;
oprospero 0:4dd021344a6b 940 tmp[1] = time & 0xFF;
oprospero 0:4dd021344a6b 941 return mpu_write_mem(D_1_88,2,tmp);
oprospero 0:4dd021344a6b 942 }
oprospero 0:4dd021344a6b 943
oprospero 0:4dd021344a6b 944 /**
oprospero 0:4dd021344a6b 945 * @brief Get current step count.
oprospero 0:4dd021344a6b 946 * @param[out] count Number of steps detected.
oprospero 0:4dd021344a6b 947 * @return 0 if successful.
oprospero 0:4dd021344a6b 948 */
oprospero 0:4dd021344a6b 949 int DMP_Motion_Driver::dmp_get_pedometer_step_count(unsigned long *count)
oprospero 0:4dd021344a6b 950 {
oprospero 0:4dd021344a6b 951 unsigned char tmp[4];
oprospero 0:4dd021344a6b 952 if (!count)
oprospero 0:4dd021344a6b 953 return -1;
oprospero 0:4dd021344a6b 954
oprospero 0:4dd021344a6b 955 if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
oprospero 0:4dd021344a6b 956 return -1;
oprospero 0:4dd021344a6b 957
oprospero 0:4dd021344a6b 958 count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
oprospero 0:4dd021344a6b 959 ((unsigned long)tmp[2] << 8) | tmp[3];
oprospero 0:4dd021344a6b 960 return 0;
oprospero 0:4dd021344a6b 961 }
oprospero 0:4dd021344a6b 962
oprospero 0:4dd021344a6b 963 /**
oprospero 0:4dd021344a6b 964 * @brief Overwrite current step count.
oprospero 0:4dd021344a6b 965 * WARNING: This function writes to DMP memory and could potentially encounter
oprospero 0:4dd021344a6b 966 * a race condition if called while the pedometer is enabled.
oprospero 0:4dd021344a6b 967 * @param[in] count New step count.
oprospero 0:4dd021344a6b 968 * @return 0 if successful.
oprospero 0:4dd021344a6b 969 */
oprospero 0:4dd021344a6b 970 int DMP_Motion_Driver::dmp_set_pedometer_step_count(unsigned long count)
oprospero 0:4dd021344a6b 971 {
oprospero 0:4dd021344a6b 972 unsigned char tmp[4];
oprospero 0:4dd021344a6b 973
oprospero 0:4dd021344a6b 974 tmp[0] = (unsigned char)((count >> 24) & 0xFF);
oprospero 0:4dd021344a6b 975 tmp[1] = (unsigned char)((count >> 16) & 0xFF);
oprospero 0:4dd021344a6b 976 tmp[2] = (unsigned char)((count >> 8) & 0xFF);
oprospero 0:4dd021344a6b 977 tmp[3] = (unsigned char)(count & 0xFF);
oprospero 0:4dd021344a6b 978 return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);
oprospero 0:4dd021344a6b 979 }
oprospero 0:4dd021344a6b 980
oprospero 0:4dd021344a6b 981 /**
oprospero 0:4dd021344a6b 982 * @brief Get duration of walking time.
oprospero 0:4dd021344a6b 983 * @param[in] time Walk time in milliseconds.
oprospero 0:4dd021344a6b 984 * @return 0 if successful.
oprospero 0:4dd021344a6b 985 */
oprospero 0:4dd021344a6b 986 int DMP_Motion_Driver::dmp_get_pedometer_walk_time(unsigned long *time)
oprospero 0:4dd021344a6b 987 {
oprospero 0:4dd021344a6b 988 unsigned char tmp[4];
oprospero 0:4dd021344a6b 989 if (!time)
oprospero 0:4dd021344a6b 990 return -1;
oprospero 0:4dd021344a6b 991
oprospero 0:4dd021344a6b 992 if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
oprospero 0:4dd021344a6b 993 return -1;
oprospero 0:4dd021344a6b 994
oprospero 0:4dd021344a6b 995 time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
oprospero 0:4dd021344a6b 996 ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
oprospero 0:4dd021344a6b 997 return 0;
oprospero 0:4dd021344a6b 998 }
oprospero 0:4dd021344a6b 999
oprospero 0:4dd021344a6b 1000 /**
oprospero 0:4dd021344a6b 1001 * @brief Overwrite current walk time.
oprospero 0:4dd021344a6b 1002 * WARNING: This function writes to DMP memory and could potentially encounter
oprospero 0:4dd021344a6b 1003 * a race condition if called while the pedometer is enabled.
oprospero 0:4dd021344a6b 1004 * @param[in] time New walk time in milliseconds.
oprospero 0:4dd021344a6b 1005 */
oprospero 0:4dd021344a6b 1006 int DMP_Motion_Driver::dmp_set_pedometer_walk_time(unsigned long time)
oprospero 0:4dd021344a6b 1007 {
oprospero 0:4dd021344a6b 1008 unsigned char tmp[4];
oprospero 0:4dd021344a6b 1009
oprospero 0:4dd021344a6b 1010 time /= 20;
oprospero 0:4dd021344a6b 1011
oprospero 0:4dd021344a6b 1012 tmp[0] = (unsigned char)((time >> 24) & 0xFF);
oprospero 0:4dd021344a6b 1013 tmp[1] = (unsigned char)((time >> 16) & 0xFF);
oprospero 0:4dd021344a6b 1014 tmp[2] = (unsigned char)((time >> 8) & 0xFF);
oprospero 0:4dd021344a6b 1015 tmp[3] = (unsigned char)(time & 0xFF);
oprospero 0:4dd021344a6b 1016 return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);
oprospero 0:4dd021344a6b 1017 }
oprospero 0:4dd021344a6b 1018
oprospero 0:4dd021344a6b 1019 /**
oprospero 0:4dd021344a6b 1020 * @brief Enable DMP features.
oprospero 0:4dd021344a6b 1021 * The following \#define's are used in the input mask:
oprospero 0:4dd021344a6b 1022 * \n DMP_FEATURE_TAP
oprospero 0:4dd021344a6b 1023 * \n DMP_FEATURE_ANDROID_ORIENT
oprospero 0:4dd021344a6b 1024 * \n DMP_FEATURE_LP_QUAT
oprospero 0:4dd021344a6b 1025 * \n DMP_FEATURE_6X_LP_QUAT
oprospero 0:4dd021344a6b 1026 * \n DMP_FEATURE_GYRO_CAL
oprospero 0:4dd021344a6b 1027 * \n DMP_FEATURE_SEND_RAW_ACCEL
oprospero 0:4dd021344a6b 1028 * \n DMP_FEATURE_SEND_RAW_GYRO
oprospero 0:4dd021344a6b 1029 * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
oprospero 0:4dd021344a6b 1030 * exclusive.
oprospero 0:4dd021344a6b 1031 * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
oprospero 0:4dd021344a6b 1032 * mutually exclusive.
oprospero 0:4dd021344a6b 1033 * @param[in] mask Mask of features to enable.
oprospero 0:4dd021344a6b 1034 * @return 0 if successful.
oprospero 0:4dd021344a6b 1035 */
oprospero 0:4dd021344a6b 1036 int DMP_Motion_Driver::dmp_enable_feature(unsigned short mask)
oprospero 0:4dd021344a6b 1037 {
oprospero 0:4dd021344a6b 1038 unsigned char tmp[10];
oprospero 0:4dd021344a6b 1039
oprospero 0:4dd021344a6b 1040 /* TODO: All of these settings can probably be integrated into the default
oprospero 0:4dd021344a6b 1041 * DMP image.
oprospero 0:4dd021344a6b 1042 */
oprospero 0:4dd021344a6b 1043 /* Set integration scale factor. */
oprospero 0:4dd021344a6b 1044 tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
oprospero 0:4dd021344a6b 1045 tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
oprospero 0:4dd021344a6b 1046 tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
oprospero 0:4dd021344a6b 1047 tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
oprospero 0:4dd021344a6b 1048 mpu_write_mem(D_0_104, 4, tmp);
oprospero 0:4dd021344a6b 1049
oprospero 0:4dd021344a6b 1050 /* Send sensor data to the FIFO. */
oprospero 0:4dd021344a6b 1051 tmp[0] = 0xA3;
oprospero 0:4dd021344a6b 1052 if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
oprospero 0:4dd021344a6b 1053 tmp[1] = 0xC0;
oprospero 0:4dd021344a6b 1054 tmp[2] = 0xC8;
oprospero 0:4dd021344a6b 1055 tmp[3] = 0xC2;
oprospero 0:4dd021344a6b 1056 } else {
oprospero 0:4dd021344a6b 1057 tmp[1] = 0xA3;
oprospero 0:4dd021344a6b 1058 tmp[2] = 0xA3;
oprospero 0:4dd021344a6b 1059 tmp[3] = 0xA3;
oprospero 0:4dd021344a6b 1060 }
oprospero 0:4dd021344a6b 1061 if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
oprospero 0:4dd021344a6b 1062 tmp[4] = 0xC4;
oprospero 0:4dd021344a6b 1063 tmp[5] = 0xCC;
oprospero 0:4dd021344a6b 1064 tmp[6] = 0xC6;
oprospero 0:4dd021344a6b 1065 } else {
oprospero 0:4dd021344a6b 1066 tmp[4] = 0xA3;
oprospero 0:4dd021344a6b 1067 tmp[5] = 0xA3;
oprospero 0:4dd021344a6b 1068 tmp[6] = 0xA3;
oprospero 0:4dd021344a6b 1069 }
oprospero 0:4dd021344a6b 1070 tmp[7] = 0xA3;
oprospero 0:4dd021344a6b 1071 tmp[8] = 0xA3;
oprospero 0:4dd021344a6b 1072 tmp[9] = 0xA3;
oprospero 0:4dd021344a6b 1073 mpu_write_mem(CFG_15,10,tmp);
oprospero 0:4dd021344a6b 1074
oprospero 0:4dd021344a6b 1075 /* Send gesture data to the FIFO. */
oprospero 0:4dd021344a6b 1076 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
oprospero 0:4dd021344a6b 1077 tmp[0] = DINA20;
oprospero 0:4dd021344a6b 1078 else
oprospero 0:4dd021344a6b 1079 tmp[0] = 0xD8;
oprospero 0:4dd021344a6b 1080 mpu_write_mem(CFG_27,1,tmp);
oprospero 0:4dd021344a6b 1081
oprospero 0:4dd021344a6b 1082 if (mask & DMP_FEATURE_GYRO_CAL)
oprospero 0:4dd021344a6b 1083 dmp_enable_gyro_cal(1);
oprospero 0:4dd021344a6b 1084 else
oprospero 0:4dd021344a6b 1085 dmp_enable_gyro_cal(0);
oprospero 0:4dd021344a6b 1086
oprospero 0:4dd021344a6b 1087 if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
oprospero 0:4dd021344a6b 1088 if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
oprospero 0:4dd021344a6b 1089 tmp[0] = 0xB2;
oprospero 0:4dd021344a6b 1090 tmp[1] = 0x8B;
oprospero 0:4dd021344a6b 1091 tmp[2] = 0xB6;
oprospero 0:4dd021344a6b 1092 tmp[3] = 0x9B;
oprospero 0:4dd021344a6b 1093 } else {
oprospero 0:4dd021344a6b 1094 tmp[0] = DINAC0;
oprospero 0:4dd021344a6b 1095 tmp[1] = DINA80;
oprospero 0:4dd021344a6b 1096 tmp[2] = DINAC2;
oprospero 0:4dd021344a6b 1097 tmp[3] = DINA90;
oprospero 0:4dd021344a6b 1098 }
oprospero 0:4dd021344a6b 1099 mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
oprospero 0:4dd021344a6b 1100 }
oprospero 0:4dd021344a6b 1101
oprospero 0:4dd021344a6b 1102 if (mask & DMP_FEATURE_TAP) {
oprospero 0:4dd021344a6b 1103 /* Enable tap. */
oprospero 0:4dd021344a6b 1104 tmp[0] = 0xF8;
oprospero 0:4dd021344a6b 1105 mpu_write_mem(CFG_20, 1, tmp);
oprospero 0:4dd021344a6b 1106 dmp_set_tap_thresh(TAP_XYZ, 250);
oprospero 0:4dd021344a6b 1107 dmp_set_tap_axes(TAP_XYZ);
oprospero 0:4dd021344a6b 1108 dmp_set_tap_count(1);
oprospero 0:4dd021344a6b 1109 dmp_set_tap_time(100);
oprospero 0:4dd021344a6b 1110 dmp_set_tap_time_multi(500);
oprospero 0:4dd021344a6b 1111
oprospero 0:4dd021344a6b 1112 dmp_set_shake_reject_thresh(GYRO_SF, 200);
oprospero 0:4dd021344a6b 1113 dmp_set_shake_reject_time(40);
oprospero 0:4dd021344a6b 1114 dmp_set_shake_reject_timeout(10);
oprospero 0:4dd021344a6b 1115 } else {
oprospero 0:4dd021344a6b 1116 tmp[0] = 0xD8;
oprospero 0:4dd021344a6b 1117 mpu_write_mem(CFG_20, 1, tmp);
oprospero 0:4dd021344a6b 1118 }
oprospero 0:4dd021344a6b 1119
oprospero 0:4dd021344a6b 1120 if (mask & DMP_FEATURE_ANDROID_ORIENT) {
oprospero 0:4dd021344a6b 1121 tmp[0] = 0xD9;
oprospero 0:4dd021344a6b 1122 } else
oprospero 0:4dd021344a6b 1123 tmp[0] = 0xD8;
oprospero 0:4dd021344a6b 1124 mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
oprospero 0:4dd021344a6b 1125
oprospero 0:4dd021344a6b 1126 if (mask & DMP_FEATURE_LP_QUAT)
oprospero 0:4dd021344a6b 1127 dmp_enable_lp_quat(1);
oprospero 0:4dd021344a6b 1128 else
oprospero 0:4dd021344a6b 1129 dmp_enable_lp_quat(0);
oprospero 0:4dd021344a6b 1130
oprospero 0:4dd021344a6b 1131 if (mask & DMP_FEATURE_6X_LP_QUAT)
oprospero 0:4dd021344a6b 1132 dmp_enable_6x_lp_quat(1);
oprospero 0:4dd021344a6b 1133 else
oprospero 0:4dd021344a6b 1134 dmp_enable_6x_lp_quat(0);
oprospero 0:4dd021344a6b 1135
oprospero 0:4dd021344a6b 1136 /* Pedometer is always enabled. */
oprospero 0:4dd021344a6b 1137 dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
oprospero 0:4dd021344a6b 1138 mpu_reset_fifo();
oprospero 0:4dd021344a6b 1139
oprospero 0:4dd021344a6b 1140 dmp.packet_length = 0;
oprospero 0:4dd021344a6b 1141 if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
oprospero 0:4dd021344a6b 1142 dmp.packet_length += 6;
oprospero 0:4dd021344a6b 1143 if (mask & DMP_FEATURE_SEND_ANY_GYRO)
oprospero 0:4dd021344a6b 1144 dmp.packet_length += 6;
oprospero 0:4dd021344a6b 1145 if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
oprospero 0:4dd021344a6b 1146 dmp.packet_length += 16;
oprospero 0:4dd021344a6b 1147 if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
oprospero 0:4dd021344a6b 1148 dmp.packet_length += 4;
oprospero 0:4dd021344a6b 1149
oprospero 0:4dd021344a6b 1150 return 0;
oprospero 0:4dd021344a6b 1151 }
oprospero 0:4dd021344a6b 1152
oprospero 0:4dd021344a6b 1153 int DMP_Motion_Driver::dmp_get_packet_length()
oprospero 0:4dd021344a6b 1154 {
oprospero 0:4dd021344a6b 1155 return dmp.packet_length;
oprospero 0:4dd021344a6b 1156 }
oprospero 0:4dd021344a6b 1157
oprospero 0:4dd021344a6b 1158 /**
oprospero 0:4dd021344a6b 1159 * @brief Get list of currently enabled DMP features.
oprospero 0:4dd021344a6b 1160 * @param[out] Mask of enabled features.
oprospero 0:4dd021344a6b 1161 * @return 0 if successful.
oprospero 0:4dd021344a6b 1162 */
oprospero 0:4dd021344a6b 1163 int DMP_Motion_Driver::dmp_get_enabled_features(unsigned short *mask)
oprospero 0:4dd021344a6b 1164 {
oprospero 0:4dd021344a6b 1165 mask[0] = dmp.feature_mask;
oprospero 0:4dd021344a6b 1166 return 0;
oprospero 0:4dd021344a6b 1167 }
oprospero 0:4dd021344a6b 1168
oprospero 0:4dd021344a6b 1169 /**
oprospero 0:4dd021344a6b 1170 * @brief Calibrate the gyro data in the DMP.
oprospero 0:4dd021344a6b 1171 * After eight seconds of no motion, the DMP will compute gyro biases and
oprospero 0:4dd021344a6b 1172 * subtract them from the quaternion output. If @e dmp_enable_feature is
oprospero 0:4dd021344a6b 1173 * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
oprospero 0:4dd021344a6b 1174 * subtracted from the gyro output.
oprospero 0:4dd021344a6b 1175 * @param[in] enable 1 to enable gyro calibration.
oprospero 0:4dd021344a6b 1176 * @return 0 if successful.
oprospero 0:4dd021344a6b 1177 */
oprospero 0:4dd021344a6b 1178 int DMP_Motion_Driver::dmp_enable_gyro_cal(unsigned char enable)
oprospero 0:4dd021344a6b 1179 {
oprospero 0:4dd021344a6b 1180 if (enable) {
oprospero 0:4dd021344a6b 1181 unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
oprospero 0:4dd021344a6b 1182 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
oprospero 0:4dd021344a6b 1183 } else {
oprospero 0:4dd021344a6b 1184 unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
oprospero 0:4dd021344a6b 1185 return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
oprospero 0:4dd021344a6b 1186 }
oprospero 0:4dd021344a6b 1187 }
oprospero 0:4dd021344a6b 1188
oprospero 0:4dd021344a6b 1189 /**
oprospero 0:4dd021344a6b 1190 * @brief Generate 3-axis quaternions from the DMP.
oprospero 0:4dd021344a6b 1191 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
oprospero 0:4dd021344a6b 1192 * exclusive.
oprospero 0:4dd021344a6b 1193 * @param[in] enable 1 to enable 3-axis quaternion.
oprospero 0:4dd021344a6b 1194 * @return 0 if successful.
oprospero 0:4dd021344a6b 1195 */
oprospero 0:4dd021344a6b 1196 int DMP_Motion_Driver::dmp_enable_lp_quat(unsigned char enable)
oprospero 0:4dd021344a6b 1197 {
oprospero 0:4dd021344a6b 1198 unsigned char regs[4];
oprospero 0:4dd021344a6b 1199 if (enable) {
oprospero 0:4dd021344a6b 1200 regs[0] = DINBC0;
oprospero 0:4dd021344a6b 1201 regs[1] = DINBC2;
oprospero 0:4dd021344a6b 1202 regs[2] = DINBC4;
oprospero 0:4dd021344a6b 1203 regs[3] = DINBC6;
oprospero 0:4dd021344a6b 1204 }
oprospero 0:4dd021344a6b 1205 else
oprospero 0:4dd021344a6b 1206 memset(regs, 0x8B, 4);
oprospero 0:4dd021344a6b 1207
oprospero 0:4dd021344a6b 1208 mpu_write_mem(CFG_LP_QUAT, 4, regs);
oprospero 0:4dd021344a6b 1209
oprospero 0:4dd021344a6b 1210 return mpu_reset_fifo();
oprospero 0:4dd021344a6b 1211 }
oprospero 0:4dd021344a6b 1212
oprospero 0:4dd021344a6b 1213 /**
oprospero 0:4dd021344a6b 1214 * @brief Generate 6-axis quaternions from the DMP.
oprospero 0:4dd021344a6b 1215 * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
oprospero 0:4dd021344a6b 1216 * exclusive.
oprospero 0:4dd021344a6b 1217 * @param[in] enable 1 to enable 6-axis quaternion.
oprospero 0:4dd021344a6b 1218 * @return 0 if successful.
oprospero 0:4dd021344a6b 1219 */
oprospero 0:4dd021344a6b 1220 int DMP_Motion_Driver::dmp_enable_6x_lp_quat(unsigned char enable)
oprospero 0:4dd021344a6b 1221 {
oprospero 0:4dd021344a6b 1222 unsigned char regs[4];
oprospero 0:4dd021344a6b 1223 if (enable) {
oprospero 0:4dd021344a6b 1224 regs[0] = DINA20;
oprospero 0:4dd021344a6b 1225 regs[1] = DINA28;
oprospero 0:4dd021344a6b 1226 regs[2] = DINA30;
oprospero 0:4dd021344a6b 1227 regs[3] = DINA38;
oprospero 0:4dd021344a6b 1228 } else
oprospero 0:4dd021344a6b 1229 memset(regs, 0xA3, 4);
oprospero 0:4dd021344a6b 1230
oprospero 0:4dd021344a6b 1231 mpu_write_mem(CFG_8, 4, regs);
oprospero 0:4dd021344a6b 1232
oprospero 0:4dd021344a6b 1233 return mpu_reset_fifo();
oprospero 0:4dd021344a6b 1234 }
oprospero 0:4dd021344a6b 1235
oprospero 0:4dd021344a6b 1236 /**
oprospero 0:4dd021344a6b 1237 * @brief Decode the four-byte gesture data and execute any callbacks.
oprospero 0:4dd021344a6b 1238 * @param[in] gesture Gesture data from DMP packet.
oprospero 0:4dd021344a6b 1239 * @return 0 if successful.
oprospero 0:4dd021344a6b 1240 */
oprospero 0:4dd021344a6b 1241 //static int decode_gesture(unsigned char *gesture)
oprospero 0:4dd021344a6b 1242 int DMP_Motion_Driver::decode_gesture(unsigned char *gesture)
oprospero 0:4dd021344a6b 1243 {
oprospero 0:4dd021344a6b 1244 unsigned char tap, android_orient;
oprospero 0:4dd021344a6b 1245
oprospero 0:4dd021344a6b 1246 android_orient = gesture[3] & 0xC0;
oprospero 0:4dd021344a6b 1247 tap = 0x3F & gesture[3];
oprospero 0:4dd021344a6b 1248
oprospero 0:4dd021344a6b 1249 if (gesture[1] & INT_SRC_TAP) {
oprospero 0:4dd021344a6b 1250 unsigned char direction, count;
oprospero 0:4dd021344a6b 1251 direction = tap >> 3;
oprospero 0:4dd021344a6b 1252 count = (tap % 8) + 1;
oprospero 0:4dd021344a6b 1253 if (dmp.tap_cb)
oprospero 0:4dd021344a6b 1254 dmp.tap_cb(direction, count);
oprospero 0:4dd021344a6b 1255 }
oprospero 0:4dd021344a6b 1256
oprospero 0:4dd021344a6b 1257 if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
oprospero 0:4dd021344a6b 1258 if (dmp.android_orient_cb)
oprospero 0:4dd021344a6b 1259 dmp.android_orient_cb(android_orient >> 6);
oprospero 0:4dd021344a6b 1260 }
oprospero 0:4dd021344a6b 1261
oprospero 0:4dd021344a6b 1262 return 0;
oprospero 0:4dd021344a6b 1263 }
oprospero 0:4dd021344a6b 1264
oprospero 0:4dd021344a6b 1265 /**
oprospero 0:4dd021344a6b 1266 * @brief Specify when a DMP interrupt should occur.
oprospero 0:4dd021344a6b 1267 * A DMP interrupt can be configured to trigger on either of the two
oprospero 0:4dd021344a6b 1268 * conditions below:
oprospero 0:4dd021344a6b 1269 * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
oprospero 0:4dd021344a6b 1270 * \n b. A tap event has been detected.
oprospero 0:4dd021344a6b 1271 * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
oprospero 0:4dd021344a6b 1272 * @return 0 if successful.
oprospero 0:4dd021344a6b 1273 */
oprospero 0:4dd021344a6b 1274 int DMP_Motion_Driver::dmp_set_interrupt_mode(unsigned char mode)
oprospero 0:4dd021344a6b 1275 {
oprospero 0:4dd021344a6b 1276 const unsigned char regs_continuous[11] =
oprospero 0:4dd021344a6b 1277 {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
oprospero 0:4dd021344a6b 1278 const unsigned char regs_gesture[11] =
oprospero 0:4dd021344a6b 1279 {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
oprospero 0:4dd021344a6b 1280
oprospero 0:4dd021344a6b 1281 switch (mode) {
oprospero 0:4dd021344a6b 1282 case DMP_INT_CONTINUOUS:
oprospero 0:4dd021344a6b 1283 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
oprospero 0:4dd021344a6b 1284 (unsigned char*)regs_continuous);
oprospero 0:4dd021344a6b 1285 case DMP_INT_GESTURE:
oprospero 0:4dd021344a6b 1286 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
oprospero 0:4dd021344a6b 1287 (unsigned char*)regs_gesture);
oprospero 0:4dd021344a6b 1288 default:
oprospero 0:4dd021344a6b 1289 return -1;
oprospero 0:4dd021344a6b 1290 }
oprospero 0:4dd021344a6b 1291 }
oprospero 0:4dd021344a6b 1292
oprospero 0:4dd021344a6b 1293 /**
oprospero 0:4dd021344a6b 1294 * @brief Get one packet from the FIFO.
oprospero 0:4dd021344a6b 1295 * If @e sensors does not contain a particular sensor, disregard the data
oprospero 0:4dd021344a6b 1296 * returned to that pointer.
oprospero 0:4dd021344a6b 1297 * \n @e sensors can contain a combination of the following flags:
oprospero 0:4dd021344a6b 1298 * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
oprospero 0:4dd021344a6b 1299 * \n INV_XYZ_GYRO
oprospero 0:4dd021344a6b 1300 * \n INV_XYZ_ACCEL
oprospero 0:4dd021344a6b 1301 * \n INV_WXYZ_QUAT
oprospero 0:4dd021344a6b 1302 * \n If the FIFO has no new data, @e sensors will be zero.
oprospero 0:4dd021344a6b 1303 * \n If the FIFO is disabled, @e sensors will be zero and this function will
oprospero 0:4dd021344a6b 1304 * return a non-zero error code.
oprospero 0:4dd021344a6b 1305 * @param[out] gyro Gyro data in hardware units.
oprospero 0:4dd021344a6b 1306 * @param[out] accel Accel data in hardware units.
oprospero 0:4dd021344a6b 1307 * @param[out] quat 3-axis quaternion data in hardware units.
oprospero 0:4dd021344a6b 1308 * @param[out] timestamp Timestamp in milliseconds.
oprospero 0:4dd021344a6b 1309 * @param[out] sensors Mask of sensors read from FIFO.
oprospero 0:4dd021344a6b 1310 * @param[out] more Number of remaining packets.
oprospero 0:4dd021344a6b 1311 * @return 0 if successful.
oprospero 0:4dd021344a6b 1312 */
oprospero 0:4dd021344a6b 1313 int DMP_Motion_Driver::dmp_read_fifo(short *gyro, short *accel, long *quat,
oprospero 0:4dd021344a6b 1314 unsigned long *timestamp, short *sensors, unsigned char *more)
oprospero 0:4dd021344a6b 1315 {
oprospero 0:4dd021344a6b 1316 unsigned char fifo_data[MAX_PACKET_LENGTH];
oprospero 0:4dd021344a6b 1317 unsigned char ii = 0;
oprospero 0:4dd021344a6b 1318
oprospero 0:4dd021344a6b 1319 /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
oprospero 0:4dd021344a6b 1320 * cache this value and save some cycles.
oprospero 0:4dd021344a6b 1321 */
oprospero 0:4dd021344a6b 1322 sensors[0] = 0;
oprospero 0:4dd021344a6b 1323
oprospero 0:4dd021344a6b 1324 /* Get a packet. */
oprospero 0:4dd021344a6b 1325 int result = mpu_read_fifo_stream(dmp.packet_length, fifo_data, more);
oprospero 0:4dd021344a6b 1326 if (result != 0)
oprospero 0:4dd021344a6b 1327 return result;
oprospero 0:4dd021344a6b 1328
oprospero 0:4dd021344a6b 1329 /* Parse DMP packet. */
oprospero 0:4dd021344a6b 1330 if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
oprospero 0:4dd021344a6b 1331 #ifdef FIFO_CORRUPTION_CHECK
oprospero 0:4dd021344a6b 1332 long quat_q14[4], quat_mag_sq;
oprospero 0:4dd021344a6b 1333 #endif
oprospero 0:4dd021344a6b 1334 quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
oprospero 0:4dd021344a6b 1335 ((long)fifo_data[2] << 8) | fifo_data[3];
oprospero 0:4dd021344a6b 1336 quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
oprospero 0:4dd021344a6b 1337 ((long)fifo_data[6] << 8) | fifo_data[7];
oprospero 0:4dd021344a6b 1338 quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
oprospero 0:4dd021344a6b 1339 ((long)fifo_data[10] << 8) | fifo_data[11];
oprospero 0:4dd021344a6b 1340 quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
oprospero 0:4dd021344a6b 1341 ((long)fifo_data[14] << 8) | fifo_data[15];
oprospero 0:4dd021344a6b 1342 ii += 16;
oprospero 0:4dd021344a6b 1343 #ifdef FIFO_CORRUPTION_CHECK
oprospero 0:4dd021344a6b 1344 /* We can detect a corrupted FIFO by monitoring the quaternion data and
oprospero 0:4dd021344a6b 1345 * ensuring that the magnitude is always normalized to one. This
oprospero 0:4dd021344a6b 1346 * shouldn't happen in normal operation, but if an I2C error occurs,
oprospero 0:4dd021344a6b 1347 * the FIFO reads might become misaligned.
oprospero 0:4dd021344a6b 1348 *
oprospero 0:4dd021344a6b 1349 * Let's start by scaling down the quaternion data to avoid long long
oprospero 0:4dd021344a6b 1350 * math.
oprospero 0:4dd021344a6b 1351 */
oprospero 0:4dd021344a6b 1352 quat_q14[0] = quat[0] >> 16;
oprospero 0:4dd021344a6b 1353 quat_q14[1] = quat[1] >> 16;
oprospero 0:4dd021344a6b 1354 quat_q14[2] = quat[2] >> 16;
oprospero 0:4dd021344a6b 1355 quat_q14[3] = quat[3] >> 16;
oprospero 0:4dd021344a6b 1356 quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
oprospero 0:4dd021344a6b 1357 quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
oprospero 0:4dd021344a6b 1358 if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
oprospero 0:4dd021344a6b 1359 (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
oprospero 0:4dd021344a6b 1360 /* Quaternion is outside of the acceptable threshold. */
oprospero 0:4dd021344a6b 1361 mpu_reset_fifo();
oprospero 0:4dd021344a6b 1362 sensors[0] = 0;
oprospero 0:4dd021344a6b 1363 return -4;
oprospero 0:4dd021344a6b 1364 }
oprospero 0:4dd021344a6b 1365 sensors[0] |= INV_WXYZ_QUAT;
oprospero 0:4dd021344a6b 1366 #endif
oprospero 0:4dd021344a6b 1367 }
oprospero 0:4dd021344a6b 1368
oprospero 0:4dd021344a6b 1369 if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
oprospero 0:4dd021344a6b 1370 accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
oprospero 0:4dd021344a6b 1371 accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
oprospero 0:4dd021344a6b 1372 accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
oprospero 0:4dd021344a6b 1373 ii += 6;
oprospero 0:4dd021344a6b 1374 sensors[0] |= INV_XYZ_ACCEL;
oprospero 0:4dd021344a6b 1375 }
oprospero 0:4dd021344a6b 1376
oprospero 0:4dd021344a6b 1377 if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
oprospero 0:4dd021344a6b 1378 gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
oprospero 0:4dd021344a6b 1379 gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
oprospero 0:4dd021344a6b 1380 gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
oprospero 0:4dd021344a6b 1381 ii += 6;
oprospero 0:4dd021344a6b 1382 sensors[0] |= INV_XYZ_GYRO;
oprospero 0:4dd021344a6b 1383 }
oprospero 0:4dd021344a6b 1384
oprospero 0:4dd021344a6b 1385 /* Gesture data is at the end of the DMP packet. Parse it and call
oprospero 0:4dd021344a6b 1386 * the gesture callbacks (if registered).
oprospero 0:4dd021344a6b 1387 */
oprospero 0:4dd021344a6b 1388 if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
oprospero 0:4dd021344a6b 1389 decode_gesture(fifo_data + ii);
oprospero 0:4dd021344a6b 1390
oprospero 0:4dd021344a6b 1391 get_ms(timestamp);
oprospero 0:4dd021344a6b 1392 return 0;
oprospero 0:4dd021344a6b 1393 }
oprospero 0:4dd021344a6b 1394
oprospero 0:4dd021344a6b 1395 /**
oprospero 0:4dd021344a6b 1396 * @brief Register a function to be executed on a tap event.
oprospero 0:4dd021344a6b 1397 * The tap direction is represented by one of the following:
oprospero 0:4dd021344a6b 1398 * \n TAP_X_UP
oprospero 0:4dd021344a6b 1399 * \n TAP_X_DOWN
oprospero 0:4dd021344a6b 1400 * \n TAP_Y_UP
oprospero 0:4dd021344a6b 1401 * \n TAP_Y_DOWN
oprospero 0:4dd021344a6b 1402 * \n TAP_Z_UP
oprospero 0:4dd021344a6b 1403 * \n TAP_Z_DOWN
oprospero 0:4dd021344a6b 1404 * @param[in] func Callback function.
oprospero 0:4dd021344a6b 1405 * @return 0 if successful.
oprospero 0:4dd021344a6b 1406 */
oprospero 0:4dd021344a6b 1407 int DMP_Motion_Driver::dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
oprospero 0:4dd021344a6b 1408 {
oprospero 0:4dd021344a6b 1409 dmp.tap_cb = func;
oprospero 0:4dd021344a6b 1410 return 0;
oprospero 0:4dd021344a6b 1411 }
oprospero 0:4dd021344a6b 1412
oprospero 0:4dd021344a6b 1413 /**
oprospero 0:4dd021344a6b 1414 * @brief Register a function to be executed on a android orientation event.
oprospero 0:4dd021344a6b 1415 * @param[in] func Callback function.
oprospero 0:4dd021344a6b 1416 * @return 0 if successful.
oprospero 0:4dd021344a6b 1417 */
oprospero 0:4dd021344a6b 1418 int DMP_Motion_Driver::dmp_register_android_orient_cb(void (*func)(unsigned char))
oprospero 0:4dd021344a6b 1419 {
oprospero 0:4dd021344a6b 1420 dmp.android_orient_cb = func;
oprospero 0:4dd021344a6b 1421 return 0;
oprospero 0:4dd021344a6b 1422 }
oprospero 0:4dd021344a6b 1423
oprospero 0:4dd021344a6b 1424
oprospero 0:4dd021344a6b 1425 int DMP_Motion_Driver::dmpGetGravity(VectorFloat *v, Quaternion *q)
oprospero 0:4dd021344a6b 1426 {
oprospero 0:4dd021344a6b 1427 v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
oprospero 0:4dd021344a6b 1428 v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
oprospero 0:4dd021344a6b 1429 v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
oprospero 0:4dd021344a6b 1430 return 0;
oprospero 0:4dd021344a6b 1431 }
oprospero 0:4dd021344a6b 1432
oprospero 0:4dd021344a6b 1433 int DMP_Motion_Driver::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity)
oprospero 0:4dd021344a6b 1434 {
oprospero 0:4dd021344a6b 1435 // yaw: (about Z axis)
oprospero 0:4dd021344a6b 1436 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
oprospero 0:4dd021344a6b 1437 // pitch: (nose up/down, about Y axis)
oprospero 0:4dd021344a6b 1438 data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
oprospero 0:4dd021344a6b 1439 // roll: (tilt left/right, about X axis)
oprospero 0:4dd021344a6b 1440 data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
oprospero 0:4dd021344a6b 1441 return 0;
oprospero 0:4dd021344a6b 1442 }
oprospero 0:4dd021344a6b 1443
oprospero 0:4dd021344a6b 1444 /**
oprospero 0:4dd021344a6b 1445 * @}
oprospero 0:4dd021344a6b 1446 */