\u7b80\u5355\u6613\u4e0a\u624b\u7684MPU6050 dmp\u5e93\u6253\u5305<\/p>\n
\u63d0\u4f9b\u4e94\u4e2a\u6ee1\u8db3\u8981\u6c42\u7684\u51fd\u6570\uff1a \u5728 \u7b80\u4ecb \u7b80\u5355\u6613\u4e0a\u624b\u7684MPU6050 dmp\u5e93\u6253\u5305 \u4f7f\u7528\u65b9\u5f0f \u63d0\u4f9b\u4e94\u4e2a\u6ee1\u8db3\u8981\u6c42\u7684\u51fd\u6570\uff1a – int i2c_write(unsigne …<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"emotion":"","emotion_color":"","title_style":"","license":"","footnotes":""},"categories":[4],"tags":[],"class_list":["post-422","post","type-post","status-publish","format-standard","hentry","category-esp32"],"_links":{"self":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/422","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=422"}],"version-history":[{"count":1,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/422\/revisions"}],"predecessor-version":[{"id":423,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/422\/revisions\/423"}],"wp:attachment":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=422"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=422"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=422"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}
\n- int i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data);<\/code> I2C\u53d1\u9001
\n- int i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data);<\/code> I2C\u63a5\u6536
\n- int delay_ms(unsigned long num_ms);<\/code> \u5ef6\u65f6\uff08\u7cbe\u51c6\u5230\u6beb\u79d2\uff09
\n- int get_ms(unsigned long *count);<\/code> \u83b7\u53d6\u5f53\u524d\u65f6\u95f4\uff08\u7cbe\u51c6\u5230\u6beb\u79d2\uff09
\n- void system_reset(void);<\/code> \u7cfb\u7edf\u91cd\u542f<\/p>\nmpu.h<\/code>\u4e2d\u4fee\u6539\u578b\u53f7\u548c\u5237\u65b0\u7387<\/p>\nmpu_dmp_init()<\/code> \u542f\u52a8
\n\u4e2d\u65ad\u89e6\u53d1\u540e\uff0c\u4f7f\u7528mpu_dmp_read_raw()<\/code>\u8bfb\u53d6\u56db\u5143\u6570\u548c\u539f\u59cb\u7684\u52a0\u901f\u5ea6\u53ca\u89d2\u901f\u5ea6\u503c,\u8fd4\u56detrue\u8bf4\u660e\u6210\u529f<\/p>\n\u9644\u4ef6<\/h2>\n
mpu.c<\/h3>\n
\/**\n * @file mpu.c\n * @author YIN\n * @brief \u57fa\u4e8e\u5b98\u65b9v6.12\u7684dmp\u5e93\uff0c\u5220\u53bb\u4e0d\u5fc5\u8981\u7684\u90e8\u5206\uff0c\u4fdd\u7559\u4e86dmp\u76f8\u5173\u7684\u51fd\u6570\uff0c\u5e76\u6253\u5305\u5230\u4e00\u4e2a\u6587\u4ef6\u5185\n * @version 1.0\n * @date 2025-07-25\n * \n * @copyright Copyright (c) 2025\n * \n *\/\n\n#include <stdio.h>\n#include <stdint.h>\n#include <stdlib.h>\n#include <string.h>\n#include <math.h>\n\n#include \"mpu.h\"\n\n#define log_i(...) do {} while (0)\n#define log_e(...) do {} while (0)\n#define fabs fabsf\n#define min(a,b) ((a<b)?a:b)\n\n\/* ---------------------- ---------------------- *\/\n\/* ---------------------- \u9700\u8981\u63d0\u4f9b\u7684\u63a5\u53e3\uff08\u5728\u4e0b\u65b9\u6ce8\u91ca\u4e2d\uff09 ---------------------- *\/\n\n\/\/ int i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data);\n\/\/ int i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data);\n\/\/ int delay_ms(unsigned long num_ms);\n\/\/ int get_ms(unsigned long *count);\n\/\/ void system_reset(void);\n\n\n\/* ---------------------- \u539f\u751fdmp\u5e93\u51fd\u6570\u6253\u5305 ---------------------- *\/\n\/* ---------------------- iic\u9a71\u52a8\u5c42 ---------------------- *\/\n\n\/\/ \u5408\u6cd5\u6027\u5224\u65ad\n\n#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250\n#error Which gyro are you using? Define MPUxxxx in your compiler options.\n#endif\n\n\/* Time for some messy macro work. =]\n * #define MPU9150\n * is equivalent to..\n * #define MPU6050\n * #define AK8975_SECONDARY\n *\n * #define MPU9250\n * is equivalent to..\n * #define MPU6500\n * #define AK8963_SECONDARY\n *\/\n#if defined MPU9150\n#ifndef MPU6050\n#define MPU6050\n#endif \/* #ifndef MPU6050 *\/\n#if defined AK8963_SECONDARY\n#error \"MPU9150 and AK8963_SECONDARY cannot both be defined.\"\n#elif !defined AK8975_SECONDARY \/* #if defined AK8963_SECONDARY *\/\n#define AK8975_SECONDARY\n#endif \/* #if defined AK8963_SECONDARY *\/\n#elif defined MPU9250 \/* #if defined MPU9150 *\/\n#ifndef MPU6500\n#define MPU6500\n#endif \/* #ifndef MPU6500 *\/\n#if defined AK8975_SECONDARY\n#error \"MPU9250 and AK8975_SECONDARY cannot both be defined.\"\n#elif !defined AK8963_SECONDARY \/* #if defined AK8975_SECONDARY *\/\n#define AK8963_SECONDARY\n#endif \/* #if defined AK8975_SECONDARY *\/\n#endif \/* #if defined MPU9150 *\/\n\n#if defined AK8975_SECONDARY || defined AK8963_SECONDARY\n#define AK89xx_SECONDARY\n#else\n\/* #warning \"No compass = less profit for Invensense. Lame.\" *\/\n#endif\n\nstatic int set_int_enable(unsigned char enable);\n\n\/* Hardware registers needed by driver. *\/\nstruct gyro_reg_s {\n unsigned char who_am_i;\n unsigned char rate_div;\n unsigned char lpf;\n unsigned char prod_id;\n unsigned char user_ctrl;\n unsigned char fifo_en;\n unsigned char gyro_cfg;\n unsigned char accel_cfg;\n \/\/ unsigned char accel_cfg2;\n \/\/ unsigned char lp_accel_odr;\n unsigned char motion_thr;\n unsigned char motion_dur;\n unsigned char fifo_count_h;\n unsigned char fifo_r_w;\n unsigned char raw_gyro;\n unsigned char raw_accel;\n unsigned char temp;\n unsigned char int_enable;\n unsigned char dmp_int_status;\n unsigned char int_status;\n \/\/ unsigned char accel_intel;\n unsigned char pwr_mgmt_1;\n unsigned char pwr_mgmt_2;\n unsigned char int_pin_cfg;\n unsigned char mem_r_w;\n unsigned char accel_offs;\n unsigned char i2c_mst;\n unsigned char bank_sel;\n unsigned char mem_start_addr;\n unsigned char prgm_start_h;\n#if defined AK89xx_SECONDARY\n unsigned char s0_addr;\n unsigned char s0_reg;\n unsigned char s0_ctrl;\n unsigned char s1_addr;\n unsigned char s1_reg;\n unsigned char s1_ctrl;\n unsigned char s4_ctrl;\n unsigned char s0_do;\n unsigned char s1_do;\n unsigned char i2c_delay_ctrl;\n unsigned char raw_compass;\n \/* The I2C_MST_VDDIO bit is in this register. *\/\n unsigned char yg_offs_tc;\n#endif\n};\n\n\/* Information specific to a particular device. *\/\nstruct hw_s {\n unsigned char addr;\n unsigned short max_fifo;\n unsigned char num_reg;\n unsigned short temp_sens;\n short temp_offset;\n unsigned short bank_size;\n#if defined AK89xx_SECONDARY\n unsigned short compass_fsr;\n#endif\n};\n\n\/* When entering motion interrupt mode, the driver keeps track of the\n * previous state so that it can be restored at a later time.\n * TODO: This is tacky. Fix it.\n *\/\nstruct motion_int_cache_s {\n unsigned short gyro_fsr;\n unsigned char accel_fsr;\n unsigned short lpf;\n unsigned short sample_rate;\n unsigned char sensors_on;\n unsigned char fifo_sensors;\n unsigned char dmp_on;\n};\n\n\/* Cached chip configuration data.\n * TODO: A lot of these can be handled with a bitmask.\n *\/\nstruct chip_cfg_s {\n \/* Matches gyro_cfg >> 3 & 0x03 *\/\n unsigned char gyro_fsr;\n \/* Matches accel_cfg >> 3 & 0x03 *\/\n unsigned char accel_fsr;\n \/* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. *\/\n unsigned char sensors;\n \/* Matches config register. *\/\n unsigned char lpf;\n unsigned char clk_src;\n \/* Sample rate, NOT rate divider. *\/\n unsigned short sample_rate;\n \/* Matches fifo_en register. *\/\n unsigned char fifo_enable;\n \/* Matches int enable register. *\/\n unsigned char int_enable;\n \/* 1 if devices on auxiliary I2C bus appear on the primary. *\/\n unsigned char bypass_mode;\n \/* 1 if half-sensitivity.\n * NOTE: This doesn't belong here, but everything else in hw_s is const,\n * and this allows us to save some precious RAM.\n *\/\n unsigned char accel_half;\n \/* 1 if device in low-power accel-only mode. *\/\n unsigned char lp_accel_mode;\n \/* 1 if interrupts are only triggered on motion events. *\/\n unsigned char int_motion_only;\n struct motion_int_cache_s cache;\n \/* 1 for active low interrupts. *\/\n unsigned char active_low_int;\n \/* 1 for latched interrupts. *\/\n unsigned char latched_int;\n \/* 1 if DMP is enabled. *\/\n unsigned char dmp_on;\n \/* Ensures that DMP will only be loaded once. *\/\n unsigned char dmp_loaded;\n \/* Sampling rate used when DMP is enabled. *\/\n unsigned short dmp_sample_rate;\n#ifdef AK89xx_SECONDARY\n \/* Compass sample rate. *\/\n unsigned short compass_sample_rate;\n unsigned char compass_addr;\n short mag_sens_adj[3];\n#endif\n};\n\n\/* Information for self-test. *\/\nstruct test_s {\n unsigned long gyro_sens;\n unsigned long accel_sens;\n unsigned char reg_rate_div;\n unsigned char reg_lpf;\n unsigned char reg_gyro_fsr;\n unsigned char reg_accel_fsr;\n unsigned short wait_ms;\n unsigned char packet_thresh;\n float min_dps;\n float max_dps;\n float max_gyro_var;\n float min_g;\n float max_g;\n float max_accel_var;\n#ifdef MPU6500\n float max_g_offset;\n unsigned short sample_wait_ms;\n#endif\n};\n\n\/* Gyro driver state variables. *\/\nstruct gyro_state_s {\n const struct gyro_reg_s *reg;\n const struct hw_s *hw;\n struct chip_cfg_s chip_cfg;\n const struct test_s *test;\n};\n\n\/* Filter configurations. *\/\nenum lpf_e {\n INV_FILTER_256HZ_NOLPF2 = 0,\n INV_FILTER_188HZ,\n INV_FILTER_98HZ,\n INV_FILTER_42HZ,\n INV_FILTER_20HZ,\n INV_FILTER_10HZ,\n INV_FILTER_5HZ,\n INV_FILTER_2100HZ_NOLPF,\n NUM_FILTER\n};\n\n\/* Full scale ranges. *\/\nenum gyro_fsr_e {\n INV_FSR_250DPS = 0,\n INV_FSR_500DPS,\n INV_FSR_1000DPS,\n INV_FSR_2000DPS,\n NUM_GYRO_FSR\n};\n\n\/* Full scale ranges. *\/\nenum accel_fsr_e {\n INV_FSR_2G = 0,\n INV_FSR_4G,\n INV_FSR_8G,\n INV_FSR_16G,\n NUM_ACCEL_FSR\n};\n\n\/* Clock sources. *\/\nenum clock_sel_e {\n INV_CLK_INTERNAL = 0,\n INV_CLK_PLL,\n NUM_CLK\n};\n\n\/* Low-power accel wakeup rates. *\/\nenum lp_accel_rate_e {\n#if defined MPU6050\n INV_LPA_1_25HZ,\n INV_LPA_5HZ,\n INV_LPA_20HZ,\n INV_LPA_40HZ\n#elif defined MPU6500\n INV_LPA_0_3125HZ,\n INV_LPA_0_625HZ,\n INV_LPA_1_25HZ,\n INV_LPA_2_5HZ,\n INV_LPA_5HZ,\n INV_LPA_10HZ,\n INV_LPA_20HZ,\n INV_LPA_40HZ,\n INV_LPA_80HZ,\n INV_LPA_160HZ,\n INV_LPA_320HZ,\n INV_LPA_640HZ\n#endif\n};\n\n#define BIT_I2C_MST_VDDIO (0x80)\n#define BIT_FIFO_EN (0x40)\n#define BIT_DMP_EN (0x80)\n#define BIT_FIFO_RST (0x04)\n#define BIT_DMP_RST (0x08)\n#define BIT_FIFO_OVERFLOW (0x10)\n#define BIT_DATA_RDY_EN (0x01)\n#define BIT_DMP_INT_EN (0x02)\n#define BIT_MOT_INT_EN (0x40)\n#define BITS_FSR (0x18)\n#define BITS_LPF (0x07)\n#define BITS_HPF (0x07)\n#define BITS_CLK (0x07)\n#define BIT_FIFO_SIZE_1024 (0x40)\n#define BIT_FIFO_SIZE_2048 (0x80)\n#define BIT_FIFO_SIZE_4096 (0xC0)\n#define BIT_RESET (0x80)\n#define BIT_SLEEP (0x40)\n#define BIT_S0_DELAY_EN (0x01)\n#define BIT_S2_DELAY_EN (0x04)\n#define BITS_SLAVE_LENGTH (0x0F)\n#define BIT_SLAVE_BYTE_SW (0x40)\n#define BIT_SLAVE_GROUP (0x10)\n#define BIT_SLAVE_EN (0x80)\n#define BIT_I2C_READ (0x80)\n#define BITS_I2C_MASTER_DLY (0x1F)\n#define BIT_AUX_IF_EN (0x20)\n#define BIT_ACTL (0x80)\n#define BIT_LATCH_EN (0x20)\n#define BIT_ANY_RD_CLR (0x10)\n#define BIT_BYPASS_EN (0x02)\n#define BITS_WOM_EN (0xC0)\n#define BIT_LPA_CYCLE (0x20)\n#define BIT_STBY_XA (0x20)\n#define BIT_STBY_YA (0x10)\n#define BIT_STBY_ZA (0x08)\n#define BIT_STBY_XG (0x04)\n#define BIT_STBY_YG (0x02)\n#define BIT_STBY_ZG (0x01)\n#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)\n#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)\n\n#if defined AK8975_SECONDARY\n#define SUPPORTS_AK89xx_HIGH_SENS (0x00)\n#define AK89xx_FSR (9830)\n#elif defined AK8963_SECONDARY\n#define SUPPORTS_AK89xx_HIGH_SENS (0x10)\n#define AK89xx_FSR (4915)\n#endif\n\n#ifdef AK89xx_SECONDARY\n#define AKM_REG_WHOAMI (0x00)\n\n#define AKM_REG_ST1 (0x02)\n#define AKM_REG_HXL (0x03)\n#define AKM_REG_ST2 (0x09)\n\n#define AKM_REG_CNTL (0x0A)\n#define AKM_REG_ASTC (0x0C)\n#define AKM_REG_ASAX (0x10)\n#define AKM_REG_ASAY (0x11)\n#define AKM_REG_ASAZ (0x12)\n\n#define AKM_DATA_READY (0x01)\n#define AKM_DATA_OVERRUN (0x02)\n#define AKM_OVERFLOW (0x80)\n#define AKM_DATA_ERROR (0x40)\n\n#define AKM_BIT_SELF_TEST (0x40)\n\n#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS)\n#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS)\n#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS)\n#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS)\n\n#define AKM_WHOAMI (0x48)\n#endif\n\n#if defined MPU6050\nconst struct gyro_reg_s reg = {\n .who_am_i = 0x75,\n .rate_div = 0x19,\n .lpf = 0x1A,\n .prod_id = 0x0C,\n .user_ctrl = 0x6A,\n .fifo_en = 0x23,\n .gyro_cfg = 0x1B,\n .accel_cfg = 0x1C,\n .motion_thr = 0x1F,\n .motion_dur = 0x20,\n .fifo_count_h = 0x72,\n .fifo_r_w = 0x74,\n .raw_gyro = 0x43,\n .raw_accel = 0x3B,\n .temp = 0x41,\n .int_enable = 0x38,\n .dmp_int_status = 0x39,\n .int_status = 0x3A,\n .pwr_mgmt_1 = 0x6B,\n .pwr_mgmt_2 = 0x6C,\n .int_pin_cfg = 0x37,\n .mem_r_w = 0x6F,\n .accel_offs = 0x06,\n .i2c_mst = 0x24,\n .bank_sel = 0x6D,\n .mem_start_addr = 0x6E,\n .prgm_start_h = 0x70\n#ifdef AK89xx_SECONDARY\n ,.raw_compass = 0x49,\n .yg_offs_tc = 0x01,\n .s0_addr = 0x25,\n .s0_reg = 0x26,\n .s0_ctrl = 0x27,\n .s1_addr = 0x28,\n .s1_reg = 0x29,\n .s1_ctrl = 0x2A,\n .s4_ctrl = 0x34,\n .s0_do = 0x63,\n .s1_do = 0x64,\n .i2c_delay_ctrl = 0x67\n#endif\n};\nconst struct hw_s hw = {\n .addr = 0x68,\n .max_fifo = 1024,\n .num_reg = 118,\n .temp_sens = 340,\n .temp_offset = -521,\n .bank_size = 256\n#if defined AK89xx_SECONDARY\n ,.compass_fsr = AK89xx_FSR\n#endif\n};\n\nconst struct test_s test = {\n .gyro_sens = 32768\/250,\n .accel_sens = 32768\/16,\n .reg_rate_div = 0, \/* 1kHz. *\/\n .reg_lpf = 1, \/* 188Hz. *\/\n .reg_gyro_fsr = 0, \/* 250dps. *\/\n .reg_accel_fsr = 0x18, \/* 16g. *\/\n .wait_ms = 50,\n .packet_thresh = 5, \/* 5% *\/\n .min_dps = 10.f,\n .max_dps = 105.f,\n .max_gyro_var = 0.14f,\n .min_g = 0.3f,\n .max_g = 0.95f,\n .max_accel_var = 0.14f\n};\n\nstatic struct gyro_state_s st = {\n .reg = ®,\n .hw = &hw,\n .test = &test\n};\n#elif defined MPU6500\nconst struct gyro_reg_s reg = {\n .who_am_i = 0x75,\n .rate_div = 0x19,\n .lpf = 0x1A,\n .prod_id = 0x0C,\n .user_ctrl = 0x6A,\n .fifo_en = 0x23,\n .gyro_cfg = 0x1B,\n .accel_cfg = 0x1C,\n .accel_cfg2 = 0x1D,\n .lp_accel_odr = 0x1E,\n .motion_thr = 0x1F,\n .motion_dur = 0x20,\n .fifo_count_h = 0x72,\n .fifo_r_w = 0x74,\n .raw_gyro = 0x43,\n .raw_accel = 0x3B,\n .temp = 0x41,\n .int_enable = 0x38,\n .dmp_int_status = 0x39,\n .int_status = 0x3A,\n .accel_intel = 0x69,\n .pwr_mgmt_1 = 0x6B,\n .pwr_mgmt_2 = 0x6C,\n .int_pin_cfg = 0x37,\n .mem_r_w = 0x6F,\n .accel_offs = 0x77,\n .i2c_mst = 0x24,\n .bank_sel = 0x6D,\n .mem_start_addr = 0x6E,\n .prgm_start_h = 0x70\n#ifdef AK89xx_SECONDARY\n ,.raw_compass = 0x49,\n .s0_addr = 0x25,\n .s0_reg = 0x26,\n .s0_ctrl = 0x27,\n .s1_addr = 0x28,\n .s1_reg = 0x29,\n .s1_ctrl = 0x2A,\n .s4_ctrl = 0x34,\n .s0_do = 0x63,\n .s1_do = 0x64,\n .i2c_delay_ctrl = 0x67\n#endif\n};\nconst struct hw_s hw = {\n .addr = 0x68,\n .max_fifo = 1024,\n .num_reg = 128,\n .temp_sens = 321,\n .temp_offset = 0,\n .bank_size = 256\n#if defined AK89xx_SECONDARY\n ,.compass_fsr = AK89xx_FSR\n#endif\n};\n\nconst struct test_s test = {\n .gyro_sens = 32768\/250,\n .accel_sens = 32768\/2, \/\/FSR = +-2G = 16384 LSB\/G\n .reg_rate_div = 0, \/* 1kHz. *\/\n .reg_lpf = 2, \/* 92Hz low pass filter*\/\n .reg_gyro_fsr = 0, \/* 250dps. *\/\n .reg_accel_fsr = 0x0, \/* Accel FSR setting = 2g. *\/\n .wait_ms = 200, \/\/200ms stabilization time\n .packet_thresh = 200, \/* 200 samples *\/\n .min_dps = 20.f, \/\/20 dps for Gyro Criteria C\n .max_dps = 60.f, \/\/Must exceed 60 dps threshold for Gyro Criteria B\n .max_gyro_var = .5f, \/\/Must exceed +50% variation for Gyro Criteria A\n .min_g = .225f, \/\/Accel must exceed Min 225 mg for Criteria B\n .max_g = .675f, \/\/Accel cannot exceed Max 675 mg for Criteria B\n .max_accel_var = .5f, \/\/Accel must be within 50% variation for Criteria A\n .max_g_offset = .5f, \/\/500 mg for Accel Criteria C\n .sample_wait_ms = 10 \/\/10ms sample time wait\n};\n\nstatic struct gyro_state_s st = {\n .reg = ®,\n .hw = &hw,\n .test = &test\n};\n#endif\n\n#define MAX_PACKET_LENGTH (12)\n#ifdef MPU6500\n#define HWST_MAX_PACKET_LENGTH (512)\n#endif\n\n#ifdef AK89xx_SECONDARY\nstatic int setup_compass(void);\n#define MAX_COMPASS_SAMPLE_RATE (100)\n#endif\n\n\/**\n * @brief Enable\/disable data ready interrupt.\n * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready\n * interrupt is used.\n * @param[in] enable 1 to enable interrupt.\n * @return 0 if successful.\n *\/\nstatic int set_int_enable(unsigned char enable)\n{\n unsigned char tmp;\n\n if (st.chip_cfg.dmp_on) {\n if (enable)\n tmp = BIT_DMP_INT_EN;\n else\n tmp = 0x00;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))\n return -1;\n st.chip_cfg.int_enable = tmp;\n } else {\n if (!st.chip_cfg.sensors)\n return -1;\n if (enable && st.chip_cfg.int_enable)\n return 0;\n if (enable)\n tmp = BIT_DATA_RDY_EN;\n else\n tmp = 0x00;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))\n return -1;\n st.chip_cfg.int_enable = tmp;\n }\n return 0;\n}\n\n\/**\n * @brief Register dump for testing.\n * @return 0 if successful.\n *\/\nint mpu_reg_dump(void)\n{\n unsigned char ii;\n unsigned char data;\n\n for (ii = 0; ii < st.hw->num_reg; ii++) {\n if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)\n continue;\n if (i2c_read(st.hw->addr, ii, 1, &data))\n return -1;\n log_i(\"%#5x: %#5x\\r\\n\", ii, data);\n }\n return 0;\n}\n\n\/**\n * @brief Read from a single register.\n * NOTE: The memory and FIFO read\/write registers cannot be accessed.\n * @param[in] reg Register address.\n * @param[out] data Register data.\n * @return 0 if successful.\n *\/\nint mpu_read_reg(unsigned char reg, unsigned char *data)\n{\n if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)\n return -1;\n if (reg >= st.hw->num_reg)\n return -1;\n return i2c_read(st.hw->addr, reg, 1, data);\n}\n\n\/**\n * @brief Enable latched interrupts.\n * Any MPU register will clear the interrupt.\n * @param[in] enable 1 to enable, 0 to disable.\n * @return 0 if successful.\n *\/\nint mpu_set_int_latched(unsigned char enable)\n{\n unsigned char tmp;\n if (st.chip_cfg.latched_int == enable)\n return 0;\n\n if (enable)\n tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;\n else\n tmp = 0;\n if (st.chip_cfg.bypass_mode)\n tmp |= BIT_BYPASS_EN;\n if (st.chip_cfg.active_low_int)\n tmp |= BIT_ACTL;\n if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))\n return -1;\n st.chip_cfg.latched_int = enable;\n return 0;\n}\n\n\/**\n * @brief Set digital low pass filter.\n * The following LPF settings are supported: 188, 98, 42, 20, 10, 5.\n * @param[in] lpf Desired LPF setting.\n * @return 0 if successful.\n *\/\nint mpu_set_lpf(unsigned short lpf)\n{\n unsigned char data;\n\n if (!(st.chip_cfg.sensors))\n return -1;\n\n if (lpf >= 188)\n data = INV_FILTER_188HZ;\n else if (lpf >= 98)\n data = INV_FILTER_98HZ;\n else if (lpf >= 42)\n data = INV_FILTER_42HZ;\n else if (lpf >= 20)\n data = INV_FILTER_20HZ;\n else if (lpf >= 10)\n data = INV_FILTER_10HZ;\n else\n data = INV_FILTER_5HZ;\n\n if (st.chip_cfg.lpf == data)\n return 0;\n if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))\n return -1;\n st.chip_cfg.lpf = data;\n return 0;\n}\n\n\/**\n * @brief Read raw gyro data directly from the registers.\n * @param[out] data Raw data in hardware units.\n * @param[out] timestamp Timestamp in milliseconds. Null if not needed.\n * @return 0 if successful.\n *\/\nint mpu_get_gyro_reg(short *data, unsigned long *timestamp)\n{\n unsigned char tmp[6];\n\n if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))\n return -1;\n\n if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))\n return -1;\n data[0] = (tmp[0] << 8) | tmp[1];\n data[1] = (tmp[2] << 8) | tmp[3];\n data[2] = (tmp[4] << 8) | tmp[5];\n if (timestamp)\n get_ms(timestamp);\n return 0;\n}\n\n\/**\n * @brief Read raw accel data directly from the registers.\n * @param[out] data Raw data in hardware units.\n * @param[out] timestamp Timestamp in milliseconds. Null if not needed.\n * @return 0 if successful.\n *\/\nint mpu_get_accel_reg(short *data, unsigned long *timestamp)\n{\n unsigned char tmp[6];\n\n if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))\n return -1;\n\n if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))\n return -1;\n data[0] = (tmp[0] << 8) | tmp[1];\n data[1] = (tmp[2] << 8) | tmp[3];\n data[2] = (tmp[4] << 8) | tmp[5];\n if (timestamp)\n get_ms(timestamp);\n return 0;\n}\n\n\/**\n * @brief Read temperature data directly from the registers.\n * @param[out] data Data in q16 format.\n * @param[out] timestamp Timestamp in milliseconds. Null if not needed.\n * @return 0 if successful.\n *\/\nint mpu_get_temperature(long *data, unsigned long *timestamp)\n{\n unsigned char tmp[2];\n short raw;\n\n if (!(st.chip_cfg.sensors))\n return -1;\n\n if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))\n return -1;\n raw = (tmp[0] << 8) | tmp[1];\n if (timestamp)\n get_ms(timestamp);\n\n data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) \/ st.hw->temp_sens)) * 65536L);\n return 0;\n}\n\n\/**\n * @brief Read biases to the accel bias 6500 registers.\n * This function reads from the MPU6500 accel offset cancellations registers.\n * The format are G in +-8G format. The register is initialized with OTP \n * factory trim values.\n * @param[in] accel_bias returned structure with the accel bias\n * @return 0 if successful.\n *\/\nint mpu_read_6500_accel_bias(long *accel_bias) {\n unsigned char data[6];\n if (i2c_read(st.hw->addr, 0x77, 2, &data[0]))\n return -1;\n if (i2c_read(st.hw->addr, 0x7A, 2, &data[2]))\n return -1;\n if (i2c_read(st.hw->addr, 0x7D, 2, &data[4]))\n return -1;\n accel_bias[0] = ((long)data[0]<<8) | data[1];\n accel_bias[1] = ((long)data[2]<<8) | data[3];\n accel_bias[2] = ((long)data[4]<<8) | data[5];\n return 0;\n}\n\n\/**\n * @brief Read biases to the accel bias 6050 registers.\n * This function reads from the MPU6050 accel offset cancellations registers.\n * The format are G in +-8G format. The register is initialized with OTP \n * factory trim values.\n * @param[in] accel_bias returned structure with the accel bias\n * @return 0 if successful.\n *\/\nint mpu_read_6050_accel_bias(long *accel_bias) {\n unsigned char data[6];\n if (i2c_read(st.hw->addr, 0x06, 2, &data[0]))\n return -1;\n if (i2c_read(st.hw->addr, 0x08, 2, &data[2]))\n return -1;\n if (i2c_read(st.hw->addr, 0x0A, 2, &data[4]))\n return -1;\n accel_bias[0] = ((long)data[0]<<8) | data[1];\n accel_bias[1] = ((long)data[2]<<8) | data[3];\n accel_bias[2] = ((long)data[4]<<8) | data[5];\n return 0;\n}\n\nint mpu_read_6500_gyro_bias(long *gyro_bias) {\n unsigned char data[6];\n if (i2c_read(st.hw->addr, 0x13, 2, &data[0]))\n return -1;\n if (i2c_read(st.hw->addr, 0x15, 2, &data[2]))\n return -1;\n if (i2c_read(st.hw->addr, 0x17, 2, &data[4]))\n return -1;\n gyro_bias[0] = ((long)data[0]<<8) | data[1];\n gyro_bias[1] = ((long)data[2]<<8) | data[3];\n gyro_bias[2] = ((long)data[4]<<8) | data[5];\n return 0;\n}\n\n\/**\n * @brief Push biases to the gyro bias 6500\/6050 registers.\n * This function expects biases relative to the current sensor output, and\n * these biases will be added to the factory-supplied values. Bias inputs are LSB\n * in +-1000dps format.\n * @param[in] gyro_bias New biases.\n * @return 0 if successful.\n *\/\nint mpu_set_gyro_bias_reg(long *gyro_bias)\n{\n unsigned char data[6] = {0, 0, 0, 0, 0, 0};\n int i=0;\n for(i=0;i<3;i++) {\n gyro_bias[i]= (-gyro_bias[i]);\n }\n data[0] = (gyro_bias[0] >> 8) & 0xff;\n data[1] = (gyro_bias[0]) & 0xff;\n data[2] = (gyro_bias[1] >> 8) & 0xff;\n data[3] = (gyro_bias[1]) & 0xff;\n data[4] = (gyro_bias[2] >> 8) & 0xff;\n data[5] = (gyro_bias[2]) & 0xff;\n if (i2c_write(st.hw->addr, 0x13, 2, &data[0]))\n return -1;\n if (i2c_write(st.hw->addr, 0x15, 2, &data[2]))\n return -1;\n if (i2c_write(st.hw->addr, 0x17, 2, &data[4]))\n return -1;\n return 0;\n}\n\n\/**\n * @brief Push biases to the accel bias 6050 registers.\n * This function expects biases relative to the current sensor output, and\n * these biases will be added to the factory-supplied values. Bias inputs are LSB\n * in +-16G format.\n * @param[in] accel_bias New biases.\n * @return 0 if successful.\n *\/\nint mpu_set_accel_bias_6050_reg(const long *accel_bias) {\n unsigned char data[6] = {0, 0, 0, 0, 0, 0};\n long accel_reg_bias[3] = {0, 0, 0};\n\n if(mpu_read_6050_accel_bias(accel_reg_bias))\n return -1;\n\n accel_reg_bias[0] -= (accel_bias[0] & ~1);\n accel_reg_bias[1] -= (accel_bias[1] & ~1);\n accel_reg_bias[2] -= (accel_bias[2] & ~1);\n\n data[0] = (accel_reg_bias[0] >> 8) & 0xff;\n data[1] = (accel_reg_bias[0]) & 0xff;\n data[2] = (accel_reg_bias[1] >> 8) & 0xff;\n data[3] = (accel_reg_bias[1]) & 0xff;\n data[4] = (accel_reg_bias[2] >> 8) & 0xff;\n data[5] = (accel_reg_bias[2]) & 0xff;\n\n if (i2c_write(st.hw->addr, 0x06, 2, &data[0]))\n return -1;\n if (i2c_write(st.hw->addr, 0x08, 2, &data[2]))\n return -1;\n if (i2c_write(st.hw->addr, 0x0A, 2, &data[4]))\n return -1;\n\n return 0;\n}\n\n\n\n\/**\n * @brief Push biases to the accel bias 6500 registers.\n * This function expects biases relative to the current sensor output, and\n * these biases will be added to the factory-supplied values. Bias inputs are LSB\n * in +-16G format.\n * @param[in] accel_bias New biases.\n * @return 0 if successful.\n *\/\nint mpu_set_accel_bias_6500_reg(const long *accel_bias) {\n unsigned char data[6] = {0, 0, 0, 0, 0, 0};\n long accel_reg_bias[3] = {0, 0, 0};\n\n if(mpu_read_6500_accel_bias(accel_reg_bias))\n return -1;\n\n \/\/ Preserve bit 0 of factory value (for temperature compensation)\n accel_reg_bias[0] -= (accel_bias[0] & ~1);\n accel_reg_bias[1] -= (accel_bias[1] & ~1);\n accel_reg_bias[2] -= (accel_bias[2] & ~1);\n\n data[0] = (accel_reg_bias[0] >> 8) & 0xff;\n data[1] = (accel_reg_bias[0]) & 0xff;\n data[2] = (accel_reg_bias[1] >> 8) & 0xff;\n data[3] = (accel_reg_bias[1]) & 0xff;\n data[4] = (accel_reg_bias[2] >> 8) & 0xff;\n data[5] = (accel_reg_bias[2]) & 0xff;\n\n if (i2c_write(st.hw->addr, 0x77, 2, &data[0]))\n return -1;\n if (i2c_write(st.hw->addr, 0x7A, 2, &data[2]))\n return -1;\n if (i2c_write(st.hw->addr, 0x7D, 2, &data[4]))\n return -1;\n\n return 0;\n}\n\n\n\/**\n * @brief Reset FIFO read\/write pointers.\n * @return 0 if successful.\n *\/\nint mpu_reset_fifo(void)\n{\n unsigned char data;\n\n if (!(st.chip_cfg.sensors))\n return -1;\n\n data = 0;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))\n return -1;\n\n if (st.chip_cfg.dmp_on) {\n data = BIT_FIFO_RST | BIT_DMP_RST;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))\n return -1;\n delay_ms(50);\n data = BIT_DMP_EN | BIT_FIFO_EN;\n if (st.chip_cfg.sensors & INV_XYZ_COMPASS)\n data |= BIT_AUX_IF_EN;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))\n return -1;\n if (st.chip_cfg.int_enable)\n data = BIT_DMP_INT_EN;\n else\n data = 0;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))\n return -1;\n data = 0;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))\n return -1;\n } else {\n data = BIT_FIFO_RST;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))\n return -1;\n if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))\n data = BIT_FIFO_EN;\n else\n data = BIT_FIFO_EN | BIT_AUX_IF_EN;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))\n return -1;\n delay_ms(50);\n if (st.chip_cfg.int_enable)\n data = BIT_DATA_RDY_EN;\n else\n data = 0;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))\n return -1;\n }\n return 0;\n}\n\n\/**\n * @brief Get the gyro full-scale range.\n * @param[out] fsr Current full-scale range.\n * @return 0 if successful.\n *\/\nint mpu_get_gyro_fsr(unsigned short *fsr)\n{\n switch (st.chip_cfg.gyro_fsr) {\n case INV_FSR_250DPS:\n fsr[0] = 250;\n break;\n case INV_FSR_500DPS:\n fsr[0] = 500;\n break;\n case INV_FSR_1000DPS:\n fsr[0] = 1000;\n break;\n case INV_FSR_2000DPS:\n fsr[0] = 2000;\n break;\n default:\n fsr[0] = 0;\n break;\n }\n return 0;\n}\n\n\/**\n * @brief Set the gyro full-scale range.\n * @param[in] fsr Desired full-scale range.\n * @return 0 if successful.\n *\/\nint mpu_set_gyro_fsr(unsigned short fsr)\n{\n unsigned char data;\n\n if (!(st.chip_cfg.sensors))\n return -1;\n\n switch (fsr) {\n case 250:\n data = INV_FSR_250DPS << 3;\n break;\n case 500:\n data = INV_FSR_500DPS << 3;\n break;\n case 1000:\n data = INV_FSR_1000DPS << 3;\n break;\n case 2000:\n data = INV_FSR_2000DPS << 3;\n break;\n default:\n return -1;\n }\n\n if (st.chip_cfg.gyro_fsr == (data >> 3))\n return 0;\n if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))\n return -1;\n st.chip_cfg.gyro_fsr = data >> 3;\n return 0;\n}\n\n\/**\n * @brief Get the accel full-scale range.\n * @param[out] fsr Current full-scale range.\n * @return 0 if successful.\n *\/\nint mpu_get_accel_fsr(unsigned char *fsr)\n{\n switch (st.chip_cfg.accel_fsr) {\n case INV_FSR_2G:\n fsr[0] = 2;\n break;\n case INV_FSR_4G:\n fsr[0] = 4;\n break;\n case INV_FSR_8G:\n fsr[0] = 8;\n break;\n case INV_FSR_16G:\n fsr[0] = 16;\n break;\n default:\n return -1;\n }\n if (st.chip_cfg.accel_half)\n fsr[0] <<= 1;\n return 0;\n}\n\n\/**\n * @brief Set the accel full-scale range.\n * @param[in] fsr Desired full-scale range.\n * @return 0 if successful.\n *\/\nint mpu_set_accel_fsr(unsigned char fsr)\n{\n unsigned char data;\n\n if (!(st.chip_cfg.sensors))\n return -1;\n\n switch (fsr) {\n case 2:\n data = INV_FSR_2G << 3;\n break;\n case 4:\n data = INV_FSR_4G << 3;\n break;\n case 8:\n data = INV_FSR_8G << 3;\n break;\n case 16:\n data = INV_FSR_16G << 3;\n break;\n default:\n return -1;\n }\n\n if (st.chip_cfg.accel_fsr == (data >> 3))\n return 0;\n if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))\n return -1;\n st.chip_cfg.accel_fsr = data >> 3;\n return 0;\n}\n\n\/**\n * @brief Get the current DLPF setting.\n * @param[out] lpf Current LPF setting.\n * 0 if successful.\n *\/\nint mpu_get_lpf(unsigned short *lpf)\n{\n switch (st.chip_cfg.lpf) {\n case INV_FILTER_188HZ:\n lpf[0] = 188;\n break;\n case INV_FILTER_98HZ:\n lpf[0] = 98;\n break;\n case INV_FILTER_42HZ:\n lpf[0] = 42;\n break;\n case INV_FILTER_20HZ:\n lpf[0] = 20;\n break;\n case INV_FILTER_10HZ:\n lpf[0] = 10;\n break;\n case INV_FILTER_5HZ:\n lpf[0] = 5;\n break;\n case INV_FILTER_256HZ_NOLPF2:\n case INV_FILTER_2100HZ_NOLPF:\n default:\n lpf[0] = 0;\n break;\n }\n return 0;\n}\n\n\/**\n * @brief Get sampling rate.\n * @param[out] rate Current sampling rate (Hz).\n * @return 0 if successful.\n *\/\nint mpu_get_sample_rate(unsigned short *rate)\n{\n if (st.chip_cfg.dmp_on)\n return -1;\n else\n rate[0] = st.chip_cfg.sample_rate;\n return 0;\n}\n\n\/**\n * @brief Get compass sampling rate.\n * @param[out] rate Current compass sampling rate (Hz).\n * @return 0 if successful.\n *\/\nint mpu_get_compass_sample_rate(unsigned short *rate)\n{\n#ifdef AK89xx_SECONDARY\n rate[0] = st.chip_cfg.compass_sample_rate;\n return 0;\n#else\n rate[0] = 0;\n return -1;\n#endif\n}\n\n\/**\n * @brief Set compass sampling rate.\n * The compass on the auxiliary I2C bus is read by the MPU hardware at a\n * maximum of 100Hz. The actual rate can be set to a fraction of the gyro\n * sampling rate.\n *\n * \\n WARNING: The new rate may be different than what was requested. Call\n * mpu_get_compass_sample_rate to check the actual setting.\n * @param[in] rate Desired compass sampling rate (Hz).\n * @return 0 if successful.\n *\/\nint mpu_set_compass_sample_rate(unsigned short rate)\n{\n#ifdef AK89xx_SECONDARY\n unsigned char div;\n if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)\n return -1;\n\n div = st.chip_cfg.sample_rate \/ rate - 1;\n if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))\n return -1;\n st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate \/ (div + 1);\n return 0;\n#else\n return -1;\n#endif\n}\n\n\/**\n * @brief Get gyro sensitivity scale factor.\n * @param[out] sens Conversion from hardware units to dps.\n * @return 0 if successful.\n *\/\nint mpu_get_gyro_sens(float *sens)\n{\n switch (st.chip_cfg.gyro_fsr) {\n case INV_FSR_250DPS:\n sens[0] = 131.f;\n break;\n case INV_FSR_500DPS:\n sens[0] = 65.5f;\n break;\n case INV_FSR_1000DPS:\n sens[0] = 32.8f;\n break;\n case INV_FSR_2000DPS:\n sens[0] = 16.4f;\n break;\n default:\n return -1;\n }\n return 0;\n}\n\n\/**\n * @brief Get accel sensitivity scale factor.\n * @param[out] sens Conversion from hardware units to g's.\n * @return 0 if successful.\n *\/\nint mpu_get_accel_sens(unsigned short *sens)\n{\n switch (st.chip_cfg.accel_fsr) {\n case INV_FSR_2G:\n sens[0] = 16384;\n break;\n case INV_FSR_4G:\n sens[0] = 8192;\n break;\n case INV_FSR_8G:\n sens[0] = 4096;\n break;\n case INV_FSR_16G:\n sens[0] = 2048;\n break;\n default:\n return -1;\n }\n if (st.chip_cfg.accel_half)\n sens[0] >>= 1;\n return 0;\n}\n\n\/**\n * @brief Get current FIFO configuration.\n * @e sensors can contain a combination of the following flags:\n * \\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO\n * \\n INV_XYZ_GYRO\n * \\n INV_XYZ_ACCEL\n * @param[out] sensors Mask of sensors in FIFO.\n * @return 0 if successful.\n *\/\nint mpu_get_fifo_config(unsigned char *sensors)\n{\n sensors[0] = st.chip_cfg.fifo_enable;\n return 0;\n}\n\n\/**\n * @brief Select which sensors are pushed to FIFO.\n * @e sensors can contain a combination of the following flags:\n * \\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO\n * \\n INV_XYZ_GYRO\n * \\n INV_XYZ_ACCEL\n * @param[in] sensors Mask of sensors to push to FIFO.\n * @return 0 if successful.\n *\/\nint mpu_configure_fifo(unsigned char sensors)\n{\n unsigned char prev;\n int result = 0;\n\n \/* Compass data isn't going into the FIFO. Stop trying. *\/\n sensors &= ~INV_XYZ_COMPASS;\n\n if (st.chip_cfg.dmp_on)\n return 0;\n else {\n if (!(st.chip_cfg.sensors))\n return -1;\n prev = st.chip_cfg.fifo_enable;\n st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;\n if (st.chip_cfg.fifo_enable != sensors)\n \/* You're not getting what you asked for. Some sensors are\n * asleep.\n *\/\n result = -1;\n else\n result = 0;\n if (sensors || st.chip_cfg.lp_accel_mode)\n set_int_enable(1);\n else\n set_int_enable(0);\n if (sensors) {\n if (mpu_reset_fifo()) {\n st.chip_cfg.fifo_enable = prev;\n return -1;\n }\n }\n }\n\n return result;\n}\n\n\/**\n * @brief Enter low-power accel-only mode.\n * In low-power accel mode, the chip goes to sleep and only wakes up to sample\n * the accelerometer at one of the following frequencies:\n * \\n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz\n * \\n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz\n * \\n If the requested rate is not one listed above, the device will be set to\n * the next highest rate. Requesting a rate above the maximum supported\n * frequency will result in an error.\n * \\n To select a fractional wake-up frequency, round down the value passed to\n * @e rate.\n * @param[in] rate Minimum sampling rate, or zero to disable LP\n * accel mode.\n * @return 0 if successful.\n *\/\nint mpu_lp_accel_mode(unsigned short rate)\n{\n unsigned char tmp[2];\n\n if (rate > 40)\n return -1;\n\n if (!rate) {\n mpu_set_int_latched(0);\n tmp[0] = 0;\n tmp[1] = BIT_STBY_XYZG;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))\n return -1;\n st.chip_cfg.lp_accel_mode = 0;\n return 0;\n }\n \/* For LP accel, we automatically configure the hardware to produce latched\n * interrupts. In LP accel mode, the hardware cycles into sleep mode before\n * it gets a chance to deassert the interrupt pin; therefore, we shift this\n * responsibility over to the MCU.\n *\n * Any register read will clear the interrupt.\n *\/\n mpu_set_int_latched(1);\n#if defined MPU6050\n tmp[0] = BIT_LPA_CYCLE;\n if (rate == 1) {\n tmp[1] = INV_LPA_1_25HZ;\n mpu_set_lpf(5);\n } else if (rate <= 5) {\n tmp[1] = INV_LPA_5HZ;\n mpu_set_lpf(5);\n } else if (rate <= 20) {\n tmp[1] = INV_LPA_20HZ;\n mpu_set_lpf(10);\n } else {\n tmp[1] = INV_LPA_40HZ;\n mpu_set_lpf(20);\n }\n tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))\n return -1;\n#elif defined MPU6500\n \/* Set wake frequency. *\/\n if (rate == 1)\n tmp[0] = INV_LPA_1_25HZ;\n else if (rate == 2)\n tmp[0] = INV_LPA_2_5HZ;\n else if (rate <= 5)\n tmp[0] = INV_LPA_5HZ;\n else if (rate <= 10)\n tmp[0] = INV_LPA_10HZ;\n else if (rate <= 20)\n tmp[0] = INV_LPA_20HZ;\n else if (rate <= 40)\n tmp[0] = INV_LPA_40HZ;\n else if (rate <= 80)\n tmp[0] = INV_LPA_80HZ;\n else if (rate <= 160)\n tmp[0] = INV_LPA_160HZ;\n else if (rate <= 320)\n tmp[0] = INV_LPA_320HZ;\n else\n tmp[0] = INV_LPA_640HZ;\n if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))\n return -1;\n tmp[0] = BIT_LPA_CYCLE;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))\n return -1;\n#endif\n st.chip_cfg.sensors = INV_XYZ_ACCEL;\n st.chip_cfg.clk_src = 0;\n st.chip_cfg.lp_accel_mode = 1;\n mpu_configure_fifo(0);\n\n return 0;\n}\n\n\/**\n * @brief Set sampling rate.\n * Sampling rate must be between 4Hz and 1kHz.\n * @param[in] rate Desired sampling rate (Hz).\n * @return 0 if successful.\n *\/\nint mpu_set_sample_rate(unsigned short rate)\n{\n unsigned char data;\n\n if (!(st.chip_cfg.sensors))\n return -1;\n\n if (st.chip_cfg.dmp_on)\n return -1;\n else {\n if (st.chip_cfg.lp_accel_mode) {\n if (rate && (rate <= 40)) {\n \/* Just stay in low-power accel mode. *\/\n mpu_lp_accel_mode(rate);\n return 0;\n }\n \/* Requested rate exceeds the allowed frequencies in LP accel mode,\n * switch back to full-power mode.\n *\/\n mpu_lp_accel_mode(0);\n }\n if (rate < 4)\n rate = 4;\n else if (rate > 1000)\n rate = 1000;\n\n data = 1000 \/ rate - 1;\n if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data))\n return -1;\n\n st.chip_cfg.sample_rate = 1000 \/ (1 + data);\n\n#ifdef AK89xx_SECONDARY\n mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));\n#endif\n\n \/* Automatically set LPF to 1\/2 sampling rate. *\/\n mpu_set_lpf(st.chip_cfg.sample_rate >> 1);\n return 0;\n }\n}\n\n\/**\n * @brief Get current power state.\n * @param[in] power_on 1 if turned on, 0 if suspended.\n * @return 0 if successful.\n *\/\nint mpu_get_power_state(unsigned char *power_on)\n{\n if (st.chip_cfg.sensors)\n power_on[0] = 1;\n else\n power_on[0] = 0;\n return 0;\n}\n\n\/**\n * @brief Turn specific sensors on\/off.\n * @e sensors can contain a combination of the following flags:\n * \\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO\n * \\n INV_XYZ_GYRO\n * \\n INV_XYZ_ACCEL\n * \\n INV_XYZ_COMPASS\n * @param[in] sensors Mask of sensors to wake.\n * @return 0 if successful.\n *\/\nint mpu_set_sensors(unsigned char sensors)\n{\n unsigned char data;\n#ifdef AK89xx_SECONDARY\n unsigned char user_ctrl;\n#endif\n\n if (sensors & INV_XYZ_GYRO)\n data = INV_CLK_PLL;\n else if (sensors)\n data = 0;\n else\n data = BIT_SLEEP;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {\n st.chip_cfg.sensors = 0;\n return -1;\n }\n st.chip_cfg.clk_src = data & ~BIT_SLEEP;\n\n data = 0;\n if (!(sensors & INV_X_GYRO))\n data |= BIT_STBY_XG;\n if (!(sensors & INV_Y_GYRO))\n data |= BIT_STBY_YG;\n if (!(sensors & INV_Z_GYRO))\n data |= BIT_STBY_ZG;\n if (!(sensors & INV_XYZ_ACCEL))\n data |= BIT_STBY_XYZA;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {\n st.chip_cfg.sensors = 0;\n return -1;\n }\n\n if (sensors && (sensors != INV_XYZ_ACCEL))\n \/* Latched interrupts only used in LP accel mode. *\/\n mpu_set_int_latched(0);\n\n#ifdef AK89xx_SECONDARY\n#ifdef AK89xx_BYPASS\n if (sensors & INV_XYZ_COMPASS)\n mpu_set_bypass(1);\n else\n mpu_set_bypass(0);\n#else\n if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))\n return -1;\n \/* Handle AKM power management. *\/\n if (sensors & INV_XYZ_COMPASS) {\n data = AKM_SINGLE_MEASUREMENT;\n user_ctrl |= BIT_AUX_IF_EN;\n } else {\n data = AKM_POWER_DOWN;\n user_ctrl &= ~BIT_AUX_IF_EN;\n }\n if (st.chip_cfg.dmp_on)\n user_ctrl |= BIT_DMP_EN;\n else\n user_ctrl &= ~BIT_DMP_EN;\n if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))\n return -1;\n \/* Enable\/disable I2C master mode. *\/\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))\n return -1;\n#endif\n#endif\n\n st.chip_cfg.sensors = sensors;\n st.chip_cfg.lp_accel_mode = 0;\n delay_ms(50);\n return 0;\n}\n\n\/**\n * @brief Read the MPU interrupt status registers.\n * @param[out] status Mask of interrupt bits.\n * @return 0 if successful.\n *\/\nint mpu_get_int_status(short *status)\n{\n unsigned char tmp[2];\n if (!st.chip_cfg.sensors)\n return -1;\n if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))\n return -1;\n status[0] = (tmp[0] << 8) | tmp[1];\n return 0;\n}\n\n\/**\n * @brief Get one packet from the FIFO.\n * If @e sensors does not contain a particular sensor, disregard the data\n * returned to that pointer.\n * \\n @e sensors can contain a combination of the following flags:\n * \\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO\n * \\n INV_XYZ_GYRO\n * \\n INV_XYZ_ACCEL\n * \\n If the FIFO has no new data, @e sensors will be zero.\n * \\n If the FIFO is disabled, @e sensors will be zero and this function will\n * return a non-zero error code.\n * @param[out] gyro Gyro data in hardware units.\n * @param[out] accel Accel data in hardware units.\n * @param[out] timestamp Timestamp in milliseconds.\n * @param[out] sensors Mask of sensors read from FIFO.\n * @param[out] more Number of remaining packets.\n * @return 0 if successful.\n *\/\nint mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,\n unsigned char *sensors, unsigned char *more)\n{\n \/* Assumes maximum packet size is gyro (6) + accel (6). *\/\n unsigned char data[MAX_PACKET_LENGTH];\n unsigned char packet_size = 0;\n unsigned short fifo_count, index = 0;\n\n if (st.chip_cfg.dmp_on)\n return -1;\n\n sensors[0] = 0;\n if (!st.chip_cfg.sensors)\n return -1;\n if (!st.chip_cfg.fifo_enable)\n return -1;\n\n if (st.chip_cfg.fifo_enable & INV_X_GYRO)\n packet_size += 2;\n if (st.chip_cfg.fifo_enable & INV_Y_GYRO)\n packet_size += 2;\n if (st.chip_cfg.fifo_enable & INV_Z_GYRO)\n packet_size += 2;\n if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)\n packet_size += 6;\n\n if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))\n return -1;\n fifo_count = (data[0] << 8) | data[1];\n if (fifo_count < packet_size)\n return 0;\n\/\/ log_i(\"FIFO count: %hd\\n\", fifo_count);\n if (fifo_count > (st.hw->max_fifo >> 1)) {\n \/* FIFO is 50% full, better check overflow bit. *\/\n if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))\n return -1;\n if (data[0] & BIT_FIFO_OVERFLOW) {\n mpu_reset_fifo();\n return -2;\n }\n }\n get_ms((unsigned long*)timestamp);\n\n if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))\n return -1;\n more[0] = fifo_count \/ packet_size - 1;\n sensors[0] = 0;\n\n if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {\n accel[0] = (data[index+0] << 8) | data[index+1];\n accel[1] = (data[index+2] << 8) | data[index+3];\n accel[2] = (data[index+4] << 8) | data[index+5];\n sensors[0] |= INV_XYZ_ACCEL;\n index += 6;\n }\n if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {\n gyro[0] = (data[index+0] << 8) | data[index+1];\n sensors[0] |= INV_X_GYRO;\n index += 2;\n }\n if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {\n gyro[1] = (data[index+0] << 8) | data[index+1];\n sensors[0] |= INV_Y_GYRO;\n index += 2;\n }\n if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {\n gyro[2] = (data[index+0] << 8) | data[index+1];\n sensors[0] |= INV_Z_GYRO;\n index += 2;\n }\n\n return 0;\n}\n\n\/**\n * @brief Get one unparsed packet from the FIFO.\n * This function should be used if the packet is to be parsed elsewhere.\n * @param[in] length Length of one FIFO packet.\n * @param[in] data FIFO packet.\n * @param[in] more Number of remaining packets.\n *\/\nint mpu_read_fifo_stream(unsigned short length, unsigned char *data,\n unsigned char *more)\n{\n unsigned char tmp[2];\n unsigned short fifo_count;\n if (!st.chip_cfg.dmp_on)\n return -1;\n if (!st.chip_cfg.sensors)\n return -1;\n\n if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))\n return -1;\n fifo_count = (tmp[0] << 8) | tmp[1];\n if (fifo_count < length) {\n more[0] = 0;\n return -1;\n }\n if (fifo_count > (st.hw->max_fifo >> 1)) {\n \/* FIFO is 50% full, better check overflow bit. *\/\n if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))\n return -1;\n if (tmp[0] & BIT_FIFO_OVERFLOW) {\n mpu_reset_fifo();\n return -2;\n }\n }\n\n if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))\n return -1;\n more[0] = fifo_count \/ length - 1;\n return 0;\n}\n\n\/**\n * @brief Set device to bypass mode.\n * @param[in] bypass_on 1 to enable bypass mode.\n * @return 0 if successful.\n *\/\nint mpu_set_bypass(unsigned char bypass_on)\n{\n unsigned char tmp;\n\n if (st.chip_cfg.bypass_mode == bypass_on)\n return 0;\n\n if (bypass_on) {\n if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))\n return -1;\n tmp &= ~BIT_AUX_IF_EN;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))\n return -1;\n delay_ms(3);\n tmp = BIT_BYPASS_EN;\n if (st.chip_cfg.active_low_int)\n tmp |= BIT_ACTL;\n if (st.chip_cfg.latched_int)\n tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;\n if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))\n return -1;\n } else {\n \/* Enable I2C master mode if compass is being used. *\/\n if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))\n return -1;\n if (st.chip_cfg.sensors & INV_XYZ_COMPASS)\n tmp |= BIT_AUX_IF_EN;\n else\n tmp &= ~BIT_AUX_IF_EN;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))\n return -1;\n delay_ms(3);\n if (st.chip_cfg.active_low_int)\n tmp = BIT_ACTL;\n else\n tmp = 0;\n if (st.chip_cfg.latched_int)\n tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;\n if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))\n return -1;\n }\n st.chip_cfg.bypass_mode = bypass_on;\n return 0;\n}\n\n\/**\n * @brief Set interrupt level.\n * @param[in] active_low 1 for active low, 0 for active high.\n * @return 0 if successful.\n *\/\nint mpu_set_int_level(unsigned char active_low)\n{\n st.chip_cfg.active_low_int = active_low;\n return 0;\n}\n\n#ifdef MPU6050\nstatic int get_accel_prod_shift(float *st_shift)\n{\n unsigned char tmp[4], shift_code[3], ii;\n\n if (i2c_read(st.hw->addr, 0x0D, 4, tmp))\n return 0x07;\n\n shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);\n shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);\n shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);\n for (ii = 0; ii < 3; ii++) {\n if (!shift_code[ii]) {\n st_shift[ii] = 0.f;\n continue;\n }\n \/* Equivalent to..\n * st_shift[ii] = 0.34f * powf(0.92f\/0.34f, (shift_code[ii]-1) \/ 30.f)\n *\/\n st_shift[ii] = 0.34f;\n while (--shift_code[ii])\n st_shift[ii] *= 1.034f;\n }\n return 0;\n}\n\nstatic int accel_self_test(long *bias_regular, long *bias_st)\n{\n int jj, result = 0;\n float st_shift[3], st_shift_cust, st_shift_var;\n\n get_accel_prod_shift(st_shift);\n for(jj = 0; jj < 3; jj++) {\n st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) \/ 65536.f;\n if (st_shift[jj]) {\n st_shift_var = st_shift_cust \/ st_shift[jj] - 1.f;\n if (fabs(st_shift_var) > test.max_accel_var)\n result |= 1 << jj;\n } else if ((st_shift_cust < test.min_g) ||\n (st_shift_cust > test.max_g))\n result |= 1 << jj;\n }\n\n return result;\n}\n\nstatic int gyro_self_test(long *bias_regular, long *bias_st)\n{\n int jj, result = 0;\n unsigned char tmp[3];\n float st_shift, st_shift_cust, st_shift_var;\n\n if (i2c_read(st.hw->addr, 0x0D, 3, tmp))\n return 0x07;\n\n tmp[0] &= 0x1F;\n tmp[1] &= 0x1F;\n tmp[2] &= 0x1F;\n\n for (jj = 0; jj < 3; jj++) {\n st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) \/ 65536.f;\n if (tmp[jj]) {\n st_shift = 3275.f \/ test.gyro_sens;\n while (--tmp[jj])\n st_shift *= 1.046f;\n st_shift_var = st_shift_cust \/ st_shift - 1.f;\n if (fabs(st_shift_var) > test.max_gyro_var)\n result |= 1 << jj;\n } else if ((st_shift_cust < test.min_dps) ||\n (st_shift_cust > test.max_dps))\n result |= 1 << jj;\n }\n return result;\n}\n\n#endif \n#ifdef AK89xx_SECONDARY\nstatic int compass_self_test(void)\n{\n unsigned char tmp[6];\n unsigned char tries = 10;\n int result = 0x07;\n short data;\n\n mpu_set_bypass(1);\n\n tmp[0] = AKM_POWER_DOWN;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))\n return 0x07;\n tmp[0] = AKM_BIT_SELF_TEST;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))\n goto AKM_restore;\n tmp[0] = AKM_MODE_SELF_TEST;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))\n goto AKM_restore;\n\n do {\n delay_ms(10);\n if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))\n goto AKM_restore;\n if (tmp[0] & AKM_DATA_READY)\n break;\n } while (tries--);\n if (!(tmp[0] & AKM_DATA_READY))\n goto AKM_restore;\n\n if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))\n goto AKM_restore;\n\n result = 0;\n#if defined MPU9150\n data = (short)(tmp[1] << 8) | tmp[0];\n if ((data > 100) || (data < -100))\n result |= 0x01;\n data = (short)(tmp[3] << 8) | tmp[2];\n if ((data > 100) || (data < -100))\n result |= 0x02;\n data = (short)(tmp[5] << 8) | tmp[4];\n if ((data > -300) || (data < -1000))\n result |= 0x04;\n#elif defined MPU9250\n data = (short)(tmp[1] << 8) | tmp[0];\n if ((data > 200) || (data < -200)) \n result |= 0x01;\n data = (short)(tmp[3] << 8) | tmp[2];\n if ((data > 200) || (data < -200)) \n result |= 0x02;\n data = (short)(tmp[5] << 8) | tmp[4];\n if ((data > -800) || (data < -3200)) \n result |= 0x04;\n#endif\nAKM_restore:\n tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;\n i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);\n tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;\n i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);\n mpu_set_bypass(0);\n return result;\n}\n#endif\n\nstatic int get_st_biases(long *gyro, long *accel, unsigned char hw_test)\n{\n unsigned char data[MAX_PACKET_LENGTH];\n unsigned char packet_count, ii;\n unsigned short fifo_count;\n\n data[0] = 0x01;\n data[1] = 0;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))\n return -1;\n delay_ms(200);\n data[0] = 0;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))\n return -1;\n data[0] = BIT_FIFO_RST | BIT_DMP_RST;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))\n return -1;\n delay_ms(15);\n data[0] = st.test->reg_lpf;\n if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))\n return -1;\n data[0] = st.test->reg_rate_div;\n if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))\n return -1;\n if (hw_test)\n data[0] = st.test->reg_gyro_fsr | 0xE0;\n else\n data[0] = st.test->reg_gyro_fsr;\n if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))\n return -1;\n\n if (hw_test)\n data[0] = st.test->reg_accel_fsr | 0xE0;\n else\n data[0] = test.reg_accel_fsr;\n if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))\n return -1;\n if (hw_test)\n delay_ms(200);\n\n \/* Fill FIFO for test.wait_ms milliseconds. *\/\n data[0] = BIT_FIFO_EN;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))\n return -1;\n\n data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))\n return -1;\n delay_ms(test.wait_ms);\n data[0] = 0;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))\n return -1;\n\n if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))\n return -1;\n\n fifo_count = (data[0] << 8) | data[1];\n packet_count = fifo_count \/ MAX_PACKET_LENGTH;\n gyro[0] = gyro[1] = gyro[2] = 0;\n accel[0] = accel[1] = accel[2] = 0;\n\n for (ii = 0; ii < packet_count; ii++) {\n short accel_cur[3], gyro_cur[3];\n if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))\n return -1;\n accel_cur[0] = ((short)data[0] << 8) | data[1];\n accel_cur[1] = ((short)data[2] << 8) | data[3];\n accel_cur[2] = ((short)data[4] << 8) | data[5];\n accel[0] += (long)accel_cur[0];\n accel[1] += (long)accel_cur[1];\n accel[2] += (long)accel_cur[2];\n gyro_cur[0] = (((short)data[6] << 8) | data[7]);\n gyro_cur[1] = (((short)data[8] << 8) | data[9]);\n gyro_cur[2] = (((short)data[10] << 8) | data[11]);\n gyro[0] += (long)gyro_cur[0];\n gyro[1] += (long)gyro_cur[1];\n gyro[2] += (long)gyro_cur[2];\n }\n#ifdef EMPL_NO_64BIT\n gyro[0] = (long)(((float)gyro[0]*65536.f) \/ test.gyro_sens \/ packet_count);\n gyro[1] = (long)(((float)gyro[1]*65536.f) \/ test.gyro_sens \/ packet_count);\n gyro[2] = (long)(((float)gyro[2]*65536.f) \/ test.gyro_sens \/ packet_count);\n if (has_accel) {\n accel[0] = (long)(((float)accel[0]*65536.f) \/ test.accel_sens \/\n packet_count);\n accel[1] = (long)(((float)accel[1]*65536.f) \/ test.accel_sens \/\n packet_count);\n accel[2] = (long)(((float)accel[2]*65536.f) \/ test.accel_sens \/\n packet_count);\n \/* Don't remove gravity! *\/\n accel[2] -= 65536L;\n }\n#else\n gyro[0] = (long)(((long long)gyro[0]<<16) \/ test.gyro_sens \/ packet_count);\n gyro[1] = (long)(((long long)gyro[1]<<16) \/ test.gyro_sens \/ packet_count);\n gyro[2] = (long)(((long long)gyro[2]<<16) \/ test.gyro_sens \/ packet_count);\n accel[0] = (long)(((long long)accel[0]<<16) \/ test.accel_sens \/\n packet_count);\n accel[1] = (long)(((long long)accel[1]<<16) \/ test.accel_sens \/\n packet_count);\n accel[2] = (long)(((long long)accel[2]<<16) \/ test.accel_sens \/\n packet_count);\n \/* Don't remove gravity! *\/\n if (accel[2] > 0L)\n accel[2] -= 65536L;\n else\n accel[2] += 65536L;\n#endif\n\n return 0;\n}\n\n\/**\n * @brief Enable\/disable DMP support.\n * @param[in] enable 1 to turn on the DMP.\n * @return 0 if successful.\n *\/\nint mpu_set_dmp_state(unsigned char enable)\n{\n unsigned char tmp;\n if (st.chip_cfg.dmp_on == enable)\n return 0;\n\n if (enable) {\n if (!st.chip_cfg.dmp_loaded)\n return -1;\n \/* Disable data ready interrupt. *\/\n set_int_enable(0);\n \/* Disable bypass mode. *\/\n mpu_set_bypass(0);\n \/* Keep constant sample rate, FIFO rate controlled by DMP. *\/\n mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);\n \/* Remove FIFO elements. *\/\n tmp = 0;\n i2c_write(st.hw->addr, 0x23, 1, &tmp);\n st.chip_cfg.dmp_on = 1;\n \/* Enable DMP interrupt. *\/\n set_int_enable(1);\n mpu_reset_fifo();\n } else {\n \/* Disable DMP interrupt. *\/\n set_int_enable(0);\n \/* Restore FIFO settings. *\/\n tmp = st.chip_cfg.fifo_enable;\n i2c_write(st.hw->addr, 0x23, 1, &tmp);\n st.chip_cfg.dmp_on = 0;\n mpu_reset_fifo();\n }\n return 0;\n}\n\n#ifdef MPU6500\n#define REG_6500_XG_ST_DATA 0x0\n#define REG_6500_XA_ST_DATA 0xD\nstatic const unsigned short mpu_6500_st_tb[256] = {\n 2620,2646,2672,2699,2726,2753,2781,2808, \/\/7\n 2837,2865,2894,2923,2952,2981,3011,3041, \/\/15\n 3072,3102,3133,3165,3196,3228,3261,3293, \/\/23\n 3326,3359,3393,3427,3461,3496,3531,3566, \/\/31\n 3602,3638,3674,3711,3748,3786,3823,3862, \/\/39\n 3900,3939,3979,4019,4059,4099,4140,4182, \/\/47\n 4224,4266,4308,4352,4395,4439,4483,4528, \/\/55\n 4574,4619,4665,4712,4759,4807,4855,4903, \/\/63\n 4953,5002,5052,5103,5154,5205,5257,5310, \/\/71\n 5363,5417,5471,5525,5581,5636,5693,5750, \/\/79\n 5807,5865,5924,5983,6043,6104,6165,6226, \/\/87\n 6289,6351,6415,6479,6544,6609,6675,6742, \/\/95\n 6810,6878,6946,7016,7086,7157,7229,7301, \/\/103\n 7374,7448,7522,7597,7673,7750,7828,7906, \/\/111\n 7985,8065,8145,8227,8309,8392,8476,8561, \/\/119\n 8647,8733,8820,8909,8998,9088,9178,9270,\n 9363,9457,9551,9647,9743,9841,9939,10038,\n 10139,10240,10343,10446,10550,10656,10763,10870,\n 10979,11089,11200,11312,11425,11539,11654,11771,\n 11889,12008,12128,12249,12371,12495,12620,12746,\n 12874,13002,13132,13264,13396,13530,13666,13802,\n 13940,14080,14221,14363,14506,14652,14798,14946,\n 15096,15247,15399,15553,15709,15866,16024,16184,\n 16346,16510,16675,16842,17010,17180,17352,17526,\n 17701,17878,18057,18237,18420,18604,18790,18978,\n 19167,19359,19553,19748,19946,20145,20347,20550,\n 20756,20963,21173,21385,21598,21814,22033,22253,\n 22475,22700,22927,23156,23388,23622,23858,24097,\n 24338,24581,24827,25075,25326,25579,25835,26093,\n 26354,26618,26884,27153,27424,27699,27976,28255,\n 28538,28823,29112,29403,29697,29994,30294,30597,\n 30903,31212,31524,31839,32157,32479,32804,33132\n};\nstatic int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)\n{\n int i, result = 0, otp_value_zero = 0;\n float accel_st_al_min, accel_st_al_max;\n float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max;\n unsigned char regs[3];\n if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) {\n if(debug)\n log_i(\"Reading OTP Register Error.\\n\");\n return 0x07;\n }\n if(debug)\n log_i(\"Accel OTP:%d, %d, %d\\n\", regs[0], regs[1], regs[2]);\n for (i = 0; i < 3; i++) {\n if (regs[i] != 0) {\n ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];\n ct_shift_prod[i] *= 65536.f;\n ct_shift_prod[i] \/= test.accel_sens;\n }\n else {\n ct_shift_prod[i] = 0;\n otp_value_zero = 1;\n }\n }\n if(otp_value_zero == 0) {\n if(debug)\n log_i(\"ACCEL:CRITERIA A\\n\");\n for (i = 0; i < 3; i++) {\n st_shift_cust[i] = bias_st[i] - bias_regular[i];\n if(debug) {\n log_i(\"Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\\r\\n\",\n st_shift_cust[i]\/1.f, bias_regular[i]\/1.f,\n bias_st[i]\/1.f);\n log_i(\"OTP value: %7.4f\\r\\n\", ct_shift_prod[i]\/1.f);\n }\n\n st_shift_ratio[i] = st_shift_cust[i] \/ ct_shift_prod[i] - 1.f;\n\n if(debug)\n log_i(\"ratio=%7.4f, threshold=%7.4f\\r\\n\", st_shift_ratio[i]\/1.f,\n test.max_accel_var\/1.f);\n\n if (fabs(st_shift_ratio[i]) > test.max_accel_var) {\n if(debug)\n log_i(\"ACCEL Fail Axis = %d\\n\", i);\n result |= 1 << i; \/\/Error condition\n }\n }\n }\n else {\n \/* Self Test Pass\/Fail Criteria B *\/\n accel_st_al_min = test.min_g * 65536.f;\n accel_st_al_max = test.max_g * 65536.f;\n\n if(debug) {\n log_i(\"ACCEL:CRITERIA B\\r\\n\");\n log_i(\"Min MG: %7.4f\\r\\n\", accel_st_al_min\/1.f);\n log_i(\"Max MG: %7.4f\\r\\n\", accel_st_al_max\/1.f);\n }\n\n for (i = 0; i < 3; i++) {\n st_shift_cust[i] = bias_st[i] - bias_regular[i];\n\n if(debug)\n log_i(\"Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\\n\", st_shift_cust[i]\/1.f, bias_st[i]\/1.f, bias_regular[i]\/1.f);\n if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) {\n if(debug)\n log_i(\"Accel FAIL axis:%d <= 225mg or >= 675mg\\n\", i);\n result |= 1 << i; \/\/Error condition\n }\n }\n }\n\n if(result == 0) {\n \/* Self Test Pass\/Fail Criteria C *\/\n accel_offset_max = test.max_g_offset * 65536.f;\n if(debug)\n log_i(\"Accel:CRITERIA C: bias less than %7.4f\\n\", accel_offset_max\/1.f);\n for (i = 0; i < 3; i++) {\n if(fabs(bias_regular[i]) > accel_offset_max) {\n if(debug)\n log_i(\"FAILED: Accel axis:%d = %ld > 500mg\\n\", i, bias_regular[i]);\n result |= 1 << i; \/\/Error condition\n }\n }\n }\n\n return result;\n}\n\nstatic int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)\n{\n int i, result = 0, otp_value_zero = 0;\n float gyro_st_al_max;\n float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max;\n unsigned char regs[3];\n\n if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) {\n if(debug)\n log_i(\"Reading OTP Register Error.\\n\");\n return 0x07;\n }\n\n if(debug)\n log_i(\"Gyro OTP:%d, %d, %d\\r\\n\", regs[0], regs[1], regs[2]);\n\n for (i = 0; i < 3; i++) {\n if (regs[i] != 0) {\n ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];\n ct_shift_prod[i] *= 65536.f;\n ct_shift_prod[i] \/= test.gyro_sens;\n }\n else {\n ct_shift_prod[i] = 0;\n otp_value_zero = 1;\n }\n }\n\n if(otp_value_zero == 0) {\n if(debug)\n log_i(\"GYRO:CRITERIA A\\n\");\n \/* Self Test Pass\/Fail Criteria A *\/\n for (i = 0; i < 3; i++) {\n st_shift_cust[i] = bias_st[i] - bias_regular[i];\n\n if(debug) {\n log_i(\"Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\\r\\n\",\n st_shift_cust[i]\/1.f, bias_regular[i]\/1.f,\n bias_st[i]\/1.f);\n log_i(\"OTP value: %7.4f\\r\\n\", ct_shift_prod[i]\/1.f);\n }\n\n st_shift_ratio[i] = st_shift_cust[i] \/ ct_shift_prod[i];\n\n if(debug)\n log_i(\"ratio=%7.4f, threshold=%7.4f\\r\\n\", st_shift_ratio[i]\/1.f,\n test.max_gyro_var\/1.f);\n\n if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {\n if(debug)\n log_i(\"Gyro Fail Axis = %d\\n\", i);\n result |= 1 << i; \/\/Error condition\n }\n }\n }\n else {\n \/* Self Test Pass\/Fail Criteria B *\/\n gyro_st_al_max = test.max_dps * 65536.f;\n\n if(debug) {\n log_i(\"GYRO:CRITERIA B\\r\\n\");\n log_i(\"Max DPS: %7.4f\\r\\n\", gyro_st_al_max\/1.f);\n }\n\n for (i = 0; i < 3; i++) {\n st_shift_cust[i] = bias_st[i] - bias_regular[i];\n\n if(debug)\n log_i(\"Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\\n\", st_shift_cust[i]\/1.f, bias_st[i]\/1.f, bias_regular[i]\/1.f);\n if(st_shift_cust[i] < gyro_st_al_max) {\n if(debug)\n log_i(\"GYRO FAIL axis:%d greater than 60dps\\n\", i);\n result |= 1 << i; \/\/Error condition\n }\n }\n }\n\n if(result == 0) {\n \/* Self Test Pass\/Fail Criteria C *\/\n gyro_offset_max = test.min_dps * 65536.f;\n if(debug)\n log_i(\"Gyro:CRITERIA C: bias less than %7.4f\\n\", gyro_offset_max\/1.f);\n for (i = 0; i < 3; i++) {\n if(fabs(bias_regular[i]) > gyro_offset_max) {\n if(debug)\n log_i(\"FAILED: Gyro axis:%d = %ld > 20dps\\n\", i, bias_regular[i]);\n result |= 1 << i; \/\/Error condition\n }\n }\n }\n return result;\n}\n\nstatic int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, int debug)\n{\n unsigned char data[HWST_MAX_PACKET_LENGTH];\n unsigned char packet_count, ii;\n unsigned short fifo_count;\n int s = 0, read_size = 0, ind;\n\n data[0] = 0x01;\n data[1] = 0;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))\n return -1;\n delay_ms(200);\n data[0] = 0;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))\n return -1;\n data[0] = BIT_FIFO_RST | BIT_DMP_RST;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))\n return -1;\n delay_ms(15);\n data[0] = st.test->reg_lpf;\n if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))\n return -1;\n data[0] = st.test->reg_rate_div;\n if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))\n return -1;\n if (hw_test)\n data[0] = st.test->reg_gyro_fsr | 0xE0;\n else\n data[0] = st.test->reg_gyro_fsr;\n if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))\n return -1;\n\n if (hw_test)\n data[0] = st.test->reg_accel_fsr | 0xE0;\n else\n data[0] = test.reg_accel_fsr;\n if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))\n return -1;\n\n delay_ms(test.wait_ms); \/\/wait 200ms for sensors to stabilize\n\n \/* Enable FIFO *\/\n data[0] = BIT_FIFO_EN;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))\n return -1;\n data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))\n return -1;\n\n \/\/initialize the bias return values\n gyro[0] = gyro[1] = gyro[2] = 0;\n accel[0] = accel[1] = accel[2] = 0;\n\n if(debug)\n log_i(\"Starting Bias Loop Reads\\n\");\n\n \/\/start reading samples\n while (s < test.packet_thresh) {\n delay_ms(test.sample_wait_ms); \/\/wait 10ms to fill FIFO\n if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))\n return -1;\n fifo_count = (data[0] << 8) | data[1];\n packet_count = fifo_count \/ MAX_PACKET_LENGTH;\n if ((test.packet_thresh - s) < packet_count)\n packet_count = test.packet_thresh - s;\n read_size = packet_count * MAX_PACKET_LENGTH;\n\n \/\/burst read from FIFO\n if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data))\n return -1;\n ind = 0;\n for (ii = 0; ii < packet_count; ii++) {\n short accel_cur[3], gyro_cur[3];\n accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1];\n accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3];\n accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5];\n accel[0] += (long)accel_cur[0];\n accel[1] += (long)accel_cur[1];\n accel[2] += (long)accel_cur[2];\n gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]);\n gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]);\n gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]);\n gyro[0] += (long)gyro_cur[0];\n gyro[1] += (long)gyro_cur[1];\n gyro[2] += (long)gyro_cur[2];\n ind += MAX_PACKET_LENGTH;\n }\n s += packet_count;\n }\n\n if(debug)\n log_i(\"Samples: %d\\n\", s);\n\n \/\/stop FIFO\n data[0] = 0;\n if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))\n return -1;\n\n gyro[0] = (long)(((long long)gyro[0]<<16) \/ test.gyro_sens \/ s);\n gyro[1] = (long)(((long long)gyro[1]<<16) \/ test.gyro_sens \/ s);\n gyro[2] = (long)(((long long)gyro[2]<<16) \/ test.gyro_sens \/ s);\n accel[0] = (long)(((long long)accel[0]<<16) \/ test.accel_sens \/ s);\n accel[1] = (long)(((long long)accel[1]<<16) \/ test.accel_sens \/ s);\n accel[2] = (long)(((long long)accel[2]<<16) \/ test.accel_sens \/ s);\n \/* remove gravity from bias calculation *\/\n if (accel[2] > 0L)\n accel[2] -= 65536L;\n else\n accel[2] += 65536L;\n\n\n if(debug) {\n log_i(\"Accel offset data HWST bit=%d: %7.4f %7.4f %7.4f\\r\\n\", hw_test, accel[0]\/65536.f, accel[1]\/65536.f, accel[2]\/65536.f);\n log_i(\"Gyro offset data HWST bit=%d: %7.4f %7.4f %7.4f\\r\\n\", hw_test, gyro[0]\/65536.f, gyro[1]\/65536.f, gyro[2]\/65536.f);\n }\n\n return 0;\n}\n\n\/**\n * @brief Trigger gyro\/accel\/compass self-test for MPU6500\/MPU9250\n * On success\/error, the self-test returns a mask representing the sensor(s)\n * that failed. For each bit, a one (1) represents a \"pass\" case; conversely,\n * a zero (0) indicates a failure.\n *\n * \\n The mask is defined as follows:\n * \\n Bit 0: Gyro.\n * \\n Bit 1: Accel.\n * \\n Bit 2: Compass.\n *\n * @param[out] gyro Gyro biases in q16 format.\n * @param[out] accel Accel biases (if applicable) in q16 format.\n * @param[in] debug Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver.\n * @return Result mask (see above).\n *\/\nint mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)\n{\n const unsigned char tries = 2;\n long gyro_st[3], accel_st[3];\n unsigned char accel_result, gyro_result;\n#ifdef AK89xx_SECONDARY\n unsigned char compass_result;\n#endif\n int ii;\n\n int result;\n unsigned char accel_fsr, fifo_sensors, sensors_on;\n unsigned short gyro_fsr, sample_rate, lpf;\n unsigned char dmp_was_on;\n\n\n\n if(debug)\n log_i(\"Starting MPU6500 HWST!\\r\\n\");\n\n if (st.chip_cfg.dmp_on) {\n mpu_set_dmp_state(0);\n dmp_was_on = 1;\n } else\n dmp_was_on = 0;\n\n \/* Get initial settings. *\/\n mpu_get_gyro_fsr(&gyro_fsr);\n mpu_get_accel_fsr(&accel_fsr);\n mpu_get_lpf(&lpf);\n mpu_get_sample_rate(&sample_rate);\n sensors_on = st.chip_cfg.sensors;\n mpu_get_fifo_config(&fifo_sensors);\n\n if(debug)\n log_i(\"Retrieving Biases\\r\\n\");\n\n for (ii = 0; ii < tries; ii++)\n if (!get_st_6500_biases(gyro, accel, 0, debug))\n break;\n if (ii == tries) {\n \/* If we reach this point, we most likely encountered an I2C error.\n * We'll just report an error for all three sensors.\n *\/\n if(debug)\n log_i(\"Retrieving Biases Error - possible I2C error\\n\");\n\n result = 0;\n goto restore;\n }\n\n if(debug)\n log_i(\"Retrieving ST Biases\\n\");\n\n for (ii = 0; ii < tries; ii++)\n if (!get_st_6500_biases(gyro_st, accel_st, 1, debug))\n break;\n if (ii == tries) {\n\n if(debug)\n log_i(\"Retrieving ST Biases Error - possible I2C error\\n\");\n\n \/* Again, probably an I2C error. *\/\n result = 0;\n goto restore;\n }\n\n accel_result = accel_6500_self_test(accel, accel_st, debug);\n if(debug)\n log_i(\"Accel Self Test Results: %d\\n\", accel_result);\n\n gyro_result = gyro_6500_self_test(gyro, gyro_st, debug);\n if(debug)\n log_i(\"Gyro Self Test Results: %d\\n\", gyro_result);\n\n result = 0;\n if (!gyro_result)\n result |= 0x01;\n if (!accel_result)\n result |= 0x02;\n\n#ifdef AK89xx_SECONDARY\n compass_result = compass_self_test();\n if(debug)\n log_i(\"Compass Self Test Results: %d\\n\", compass_result);\n if (!compass_result)\n result |= 0x04;\n#else\n result |= 0x04;\n#endif\nrestore:\n if(debug)\n log_i(\"Exiting HWST\\n\");\n \/* Set to invalid values to ensure no I2C writes are skipped. *\/\n st.chip_cfg.gyro_fsr = 0xFF;\n st.chip_cfg.accel_fsr = 0xFF;\n st.chip_cfg.lpf = 0xFF;\n st.chip_cfg.sample_rate = 0xFFFF;\n st.chip_cfg.sensors = 0xFF;\n st.chip_cfg.fifo_enable = 0xFF;\n st.chip_cfg.clk_src = INV_CLK_PLL;\n mpu_set_gyro_fsr(gyro_fsr);\n mpu_set_accel_fsr(accel_fsr);\n mpu_set_lpf(lpf);\n mpu_set_sample_rate(sample_rate);\n mpu_set_sensors(sensors_on);\n mpu_configure_fifo(fifo_sensors);\n\n if (dmp_was_on)\n mpu_set_dmp_state(1);\n\n return result;\n}\n#endif\n\n \/*\n * \\n This function must be called with the device either face-up or face-down\n * (z-axis is parallel to gravity).\n * @param[out] gyro Gyro biases in q16 format.\n * @param[out] accel Accel biases (if applicable) in q16 format.\n * @return Result mask (see above).\n *\/\nint mpu_run_self_test(long *gyro, long *accel)\n{\n#ifdef MPU6050\n const unsigned char tries = 2;\n long gyro_st[3], accel_st[3];\n unsigned char accel_result, gyro_result;\n#ifdef AK89xx_SECONDARY\n unsigned char compass_result;\n#endif\n int ii;\n#endif\n int result;\n unsigned char accel_fsr, fifo_sensors, sensors_on;\n unsigned short gyro_fsr, sample_rate, lpf;\n unsigned char dmp_was_on;\n\n if (st.chip_cfg.dmp_on) {\n mpu_set_dmp_state(0);\n dmp_was_on = 1;\n } else\n dmp_was_on = 0;\n\n \/* Get initial settings. *\/\n mpu_get_gyro_fsr(&gyro_fsr);\n mpu_get_accel_fsr(&accel_fsr);\n mpu_get_lpf(&lpf);\n mpu_get_sample_rate(&sample_rate);\n sensors_on = st.chip_cfg.sensors;\n mpu_get_fifo_config(&fifo_sensors);\n\n \/* For older chips, the self-test will be different. *\/\n#if defined MPU6050\n for (ii = 0; ii < tries; ii++)\n if (!get_st_biases(gyro, accel, 0))\n break;\n if (ii == tries) {\n \/* If we reach this point, we most likely encountered an I2C error.\n * We'll just report an error for all three sensors.\n *\/\n result = 0;\n goto restore;\n }\n for (ii = 0; ii < tries; ii++)\n if (!get_st_biases(gyro_st, accel_st, 1))\n break;\n if (ii == tries) {\n \/* Again, probably an I2C error. *\/\n result = 0;\n goto restore;\n }\n accel_result = accel_self_test(accel, accel_st);\n gyro_result = gyro_self_test(gyro, gyro_st);\n\n result = 0;\n if (!gyro_result)\n result |= 0x01;\n if (!accel_result)\n result |= 0x02;\n\n#ifdef AK89xx_SECONDARY\n compass_result = compass_self_test();\n if (!compass_result)\n result |= 0x04;\n#else\n result |= 0x04;\n#endif\nrestore:\n#elif defined MPU6500\n \/* For now, this function will return a \"pass\" result for all three sensors\n * for compatibility with current test applications.\n *\/\n get_st_biases(gyro, accel, 0);\n result = 0x7;\n#endif\n \/* Set to invalid values to ensure no I2C writes are skipped. *\/\n st.chip_cfg.gyro_fsr = 0xFF;\n st.chip_cfg.accel_fsr = 0xFF;\n st.chip_cfg.lpf = 0xFF;\n st.chip_cfg.sample_rate = 0xFFFF;\n st.chip_cfg.sensors = 0xFF;\n st.chip_cfg.fifo_enable = 0xFF;\n st.chip_cfg.clk_src = INV_CLK_PLL;\n mpu_set_gyro_fsr(gyro_fsr);\n mpu_set_accel_fsr(accel_fsr);\n mpu_set_lpf(lpf);\n mpu_set_sample_rate(sample_rate);\n mpu_set_sensors(sensors_on);\n mpu_configure_fifo(fifo_sensors);\n\n if (dmp_was_on)\n mpu_set_dmp_state(1);\n\n return result;\n}\n\n\/**\n * @brief Write to the DMP memory.\n * This function prevents I2C writes past the bank boundaries. The DMP memory\n * is only accessible when the chip is awake.\n * @param[in] mem_addr Memory location (bank << 8 | start address)\n * @param[in] length Number of bytes to write.\n * @param[in] data Bytes to write to memory.\n * @return 0 if successful.\n *\/\nint mpu_write_mem(unsigned short mem_addr, unsigned short length,\n unsigned char *data)\n{\n unsigned char tmp[2];\n\n if (!data)\n return -1;\n if (!st.chip_cfg.sensors)\n return -1;\n\n tmp[0] = (unsigned char)(mem_addr >> 8);\n tmp[1] = (unsigned char)(mem_addr & 0xFF);\n\n \/* Check bank boundaries. *\/\n if (tmp[1] + length > st.hw->bank_size)\n return -1;\n\n if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))\n return -1;\n if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))\n return -1;\n return 0;\n}\n\n\/**\n * @brief Read from the DMP memory.\n * This function prevents I2C reads past the bank boundaries. The DMP memory\n * is only accessible when the chip is awake.\n * @param[in] mem_addr Memory location (bank << 8 | start address)\n * @param[in] length Number of bytes to read.\n * @param[out] data Bytes read from memory.\n * @return 0 if successful.\n *\/\nint mpu_read_mem(unsigned short mem_addr, unsigned short length,\n unsigned char *data)\n{\n unsigned char tmp[2];\n\n if (!data)\n return -1;\n if (!st.chip_cfg.sensors)\n return -1;\n\n tmp[0] = (unsigned char)(mem_addr >> 8);\n tmp[1] = (unsigned char)(mem_addr & 0xFF);\n\n \/* Check bank boundaries. *\/\n if (tmp[1] + length > st.hw->bank_size)\n return -1;\n\n if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))\n return -1;\n if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))\n return -1;\n return 0;\n}\n\n\/**\n * @brief Load and verify DMP image.\n * @param[in] length Length of DMP image.\n * @param[in] firmware DMP code.\n * @param[in] start_addr Starting address of DMP code memory.\n * @param[in] sample_rate Fixed sampling rate used when DMP is enabled.\n * @return 0 if successful.\n *\/\nint mpu_load_firmware(unsigned short length, const unsigned char *firmware,\n unsigned short start_addr, unsigned short sample_rate)\n{\n unsigned short ii;\n unsigned short this_write;\n \/* Must divide evenly into st.hw->bank_size to avoid bank crossings. *\/\n#define LOAD_CHUNK (16)\n unsigned char cur[LOAD_CHUNK], tmp[2];\n\n if (st.chip_cfg.dmp_loaded)\n \/* DMP should only be loaded once. *\/\n return -1;\n\n if (!firmware)\n return -1;\n for (ii = 0; ii < length; ii += this_write) {\n this_write = min(LOAD_CHUNK, length - ii);\n if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))\n return -1;\n if (mpu_read_mem(ii, this_write, cur))\n return -1;\n if (memcmp(firmware+ii, cur, this_write))\n return -2;\n }\n\n \/* Set program start address. *\/\n tmp[0] = start_addr >> 8;\n tmp[1] = start_addr & 0xFF;\n if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))\n return -1;\n\n st.chip_cfg.dmp_loaded = 1;\n st.chip_cfg.dmp_sample_rate = sample_rate;\n return 0;\n}\n\n\/**\n * @brief Get DMP state.\n * @param[out] enabled 1 if enabled.\n * @return 0 if successful.\n *\/\nint mpu_get_dmp_state(unsigned char *enabled)\n{\n enabled[0] = st.chip_cfg.dmp_on;\n return 0;\n}\n\n#ifdef AK89xx_SECONDARY\n\/* This initialization is similar to the one in ak8975.c. *\/\nstatic int setup_compass(void)\n{\n unsigned char data[4], akm_addr;\n\n mpu_set_bypass(1);\n\n \/* Find compass. Possible addresses range from 0x0C to 0x0F. *\/\n for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {\n int result;\n result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);\n if (!result && (data[0] == AKM_WHOAMI))\n break;\n }\n\n if (akm_addr > 0x0F) {\n \/* TODO: Handle this case in all compass-related functions. *\/\n log_e(\"Compass not found.\\n\");\n return -1;\n }\n\n st.chip_cfg.compass_addr = akm_addr;\n\n data[0] = AKM_POWER_DOWN;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))\n return -1;\n delay_ms(1);\n\n data[0] = AKM_FUSE_ROM_ACCESS;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))\n return -1;\n delay_ms(1);\n\n \/* Get sensitivity adjustment data from fuse ROM. *\/\n if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))\n return -1;\n st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;\n st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;\n st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;\n\n data[0] = AKM_POWER_DOWN;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))\n return -1;\n delay_ms(1);\n\n mpu_set_bypass(0);\n\n \/* Set up master mode, master clock, and ES bit. *\/\n data[0] = 0x40;\n if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))\n return -1;\n\n \/* Slave 0 reads from AKM data registers. *\/\n data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;\n if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))\n return -1;\n\n \/* Compass reads start at this register. *\/\n data[0] = AKM_REG_ST1;\n if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))\n return -1;\n\n \/* Enable slave 0, 8-byte reads. *\/\n data[0] = BIT_SLAVE_EN | 8;\n if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))\n return -1;\n\n \/* Slave 1 changes AKM measurement mode. *\/\n data[0] = st.chip_cfg.compass_addr;\n if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))\n return -1;\n\n \/* AKM measurement mode register. *\/\n data[0] = AKM_REG_CNTL;\n if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))\n return -1;\n\n \/* Enable slave 1, 1-byte writes. *\/\n data[0] = BIT_SLAVE_EN | 1;\n if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))\n return -1;\n\n \/* Set slave 1 data. *\/\n data[0] = AKM_SINGLE_MEASUREMENT;\n if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))\n return -1;\n\n \/* Trigger slave 0 and slave 1 actions at each sample. *\/\n data[0] = 0x03;\n if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))\n return -1;\n\n#ifdef MPU9150\n \/* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. *\/\n data[0] = BIT_I2C_MST_VDDIO;\n if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))\n return -1;\n#endif\n\n return 0;\n}\n#endif\n\n\/**\n * @brief Read raw compass data.\n * @param[out] data Raw data in hardware units.\n * @param[out] timestamp Timestamp in milliseconds. Null if not needed.\n * @return 0 if successful.\n *\/\nint mpu_get_compass_reg(short *data, unsigned long *timestamp)\n{\n#ifdef AK89xx_SECONDARY\n unsigned char tmp[9];\n\n if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))\n return -1;\n\n#ifdef AK89xx_BYPASS\n if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))\n return -1;\n tmp[8] = AKM_SINGLE_MEASUREMENT;\n if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))\n return -1;\n#else\n if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))\n return -1;\n#endif\n\n#if defined AK8975_SECONDARY\n \/* AK8975 doesn't have the overrun error bit. *\/\n if (!(tmp[0] & AKM_DATA_READY))\n return -2;\n if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))\n return -3;\n#elif defined AK8963_SECONDARY\n \/* AK8963 doesn't have the data read error bit. *\/\n if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))\n return -2;\n if (tmp[7] & AKM_OVERFLOW)\n return -3;\n#endif\n data[0] = (tmp[2] << 8) | tmp[1];\n data[1] = (tmp[4] << 8) | tmp[3];\n data[2] = (tmp[6] << 8) | tmp[5];\n\n data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;\n data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;\n data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;\n\n if (timestamp)\n get_ms(timestamp);\n return 0;\n#else\n return -1;\n#endif\n}\n\n\/**\n * @brief Get the compass full-scale range.\n * @param[out] fsr Current full-scale range.\n * @return 0 if successful.\n *\/\nint mpu_get_compass_fsr(unsigned short *fsr)\n{\n#ifdef AK89xx_SECONDARY\n fsr[0] = st.hw->compass_fsr;\n return 0;\n#else\n return -1;\n#endif\n}\n\n\/**\n * @brief Enters LP accel motion interrupt mode.\n * The behaviour of this feature is very different between the MPU6050 and the\n * MPU6500. Each chip's version of this feature is explained below.\n *\n * \\n The hardware motion threshold can be between 32mg and 8160mg in 32mg\n * increments.\n *\n * \\n Low-power accel mode supports the following frequencies:\n * \\n 1.25Hz, 5Hz, 20Hz, 40Hz\n *\n * \\n MPU6500:\n * \\n Unlike the MPU6050 version, the hardware does not \"lock in\" a reference\n * sample. The hardware monitors the accel data and detects any large change\n * over a short period of time.\n *\n * \\n The hardware motion threshold can be between 4mg and 1020mg in 4mg\n * increments.\n *\n * \\n MPU6500 Low-power accel mode supports the following frequencies:\n * \\n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz\n *\n * \\n\\n NOTES:\n * \\n The driver will round down @e thresh to the nearest supported value if\n * an unsupported threshold is selected.\n * \\n To select a fractional wake-up frequency, round down the value passed to\n * @e lpa_freq.\n * \\n The MPU6500 does not support a delay parameter. If this function is used\n * for the MPU6500, the value passed to @e time will be ignored.\n * \\n To disable this mode, set @e lpa_freq to zero. The driver will restore\n * the previous configuration.\n *\n * @param[in] thresh Motion threshold in mg.\n * @param[in] time Duration in milliseconds that the accel data must\n * exceed @e thresh before motion is reported.\n * @param[in] lpa_freq Minimum sampling rate, or zero to disable.\n * @return 0 if successful.\n *\/\nint mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,\n unsigned short lpa_freq)\n{\n\n#if defined MPU6500\n unsigned char data[3];\n#endif\n if (lpa_freq) {\n#if defined MPU6500\n unsigned char thresh_hw;\n\n \/* 1LSb = 4mg. *\/\n if (thresh > 1020)\n thresh_hw = 255;\n else if (thresh < 4)\n thresh_hw = 1;\n else\n thresh_hw = thresh >> 2;\n#endif\n\n if (!time)\n \/* Minimum duration must be 1ms. *\/\n time = 1;\n\n#if defined MPU6500\n if (lpa_freq > 640)\n \/* At this point, the chip has not been re-configured, so the\n * function can safely exit.\n *\/\n return -1;\n#endif\n\n if (!st.chip_cfg.int_motion_only) {\n \/* Store current settings for later. *\/\n if (st.chip_cfg.dmp_on) {\n mpu_set_dmp_state(0);\n st.chip_cfg.cache.dmp_on = 1;\n } else\n st.chip_cfg.cache.dmp_on = 0;\n mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);\n mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);\n mpu_get_lpf(&st.chip_cfg.cache.lpf);\n mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);\n st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;\n mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);\n }\n\n#if defined MPU6500\n \/* Disable hardware interrupts. *\/\n set_int_enable(0);\n\n \/* Enter full-power accel-only mode, no FIFO\/DMP. *\/\n data[0] = 0;\n data[1] = 0;\n data[2] = BIT_STBY_XYZG;\n if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))\n goto lp_int_restore;\n\n \/* Set motion threshold. *\/\n data[0] = thresh_hw;\n if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))\n goto lp_int_restore;\n\n \/* Set wake frequency. *\/\n if (lpa_freq == 1)\n data[0] = INV_LPA_1_25HZ;\n else if (lpa_freq == 2)\n data[0] = INV_LPA_2_5HZ;\n else if (lpa_freq <= 5)\n data[0] = INV_LPA_5HZ;\n else if (lpa_freq <= 10)\n data[0] = INV_LPA_10HZ;\n else if (lpa_freq <= 20)\n data[0] = INV_LPA_20HZ;\n else if (lpa_freq <= 40)\n data[0] = INV_LPA_40HZ;\n else if (lpa_freq <= 80)\n data[0] = INV_LPA_80HZ;\n else if (lpa_freq <= 160)\n data[0] = INV_LPA_160HZ;\n else if (lpa_freq <= 320)\n data[0] = INV_LPA_320HZ;\n else\n data[0] = INV_LPA_640HZ;\n if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))\n goto lp_int_restore;\n\n \/* Enable motion interrupt (MPU6500 version). *\/\n data[0] = BITS_WOM_EN;\n if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))\n goto lp_int_restore;\n\n \/* Enable cycle mode. *\/\n data[0] = BIT_LPA_CYCLE;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))\n goto lp_int_restore;\n\n \/* Enable interrupt. *\/\n data[0] = BIT_MOT_INT_EN;\n if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))\n goto lp_int_restore;\n\n st.chip_cfg.int_motion_only = 1;\n return 0;\n#endif\n } else {\n \/* Don't \"restore\" the previous state if no state has been saved. *\/\n unsigned int ii;\n char *cache_ptr = (char*)&st.chip_cfg.cache;\n for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {\n if (cache_ptr[ii] != 0)\n goto lp_int_restore;\n }\n \/* If we reach this point, motion interrupt mode hasn't been used yet. *\/\n return -1;\n }\nlp_int_restore:\n \/* Set to invalid values to ensure no I2C writes are skipped. *\/\n st.chip_cfg.gyro_fsr = 0xFF;\n st.chip_cfg.accel_fsr = 0xFF;\n st.chip_cfg.lpf = 0xFF;\n st.chip_cfg.sample_rate = 0xFFFF;\n st.chip_cfg.sensors = 0xFF;\n st.chip_cfg.fifo_enable = 0xFF;\n st.chip_cfg.clk_src = INV_CLK_PLL;\n mpu_set_sensors(st.chip_cfg.cache.sensors_on);\n mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);\n mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);\n mpu_set_lpf(st.chip_cfg.cache.lpf);\n mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);\n mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);\n\n if (st.chip_cfg.cache.dmp_on)\n mpu_set_dmp_state(1);\n\n#ifdef MPU6500\n \/* Disable motion interrupt (MPU6500 version). *\/\n data[0] = 0;\n if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))\n goto lp_int_restore;\n#endif\n\n st.chip_cfg.int_motion_only = 0;\n return 0;\n}\n\n\/**\n * @brief Initialize hardware.\n * Initial configuration:\\n\n * Gyro FSR: +\/- 2000DPS\\n\n * Accel FSR +\/- 2G\\n\n * DLPF: 42Hz\\n\n * FIFO rate: 50Hz\\n\n * Clock source: Gyro PLL\\n\n * FIFO: Disabled.\\n\n * Data ready interrupt: Disabled, active low, unlatched.\n * @param[in] int_param Platform-specific parameters to interrupt API.\n * @return 0 if successful.\n *\/\nint mpu_init()\n{\n unsigned char data[6];\n\n \/* Reset device. *\/\n data[0] = BIT_RESET;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))\n return -1;\n delay_ms(100);\n\n \/* Wake up chip. *\/\n data[0] = 0x00;\n if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))\n return -1;\n\n st.chip_cfg.accel_half = 0;\n\n#ifdef MPU6500\n \/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the\n * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.\n *\/\n data[0] = BIT_FIFO_SIZE_1024 | 0x8;\n if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))\n return -1;\n#endif\n\n \/* Set to invalid values to ensure no I2C writes are skipped. *\/\n st.chip_cfg.sensors = 0xFF;\n st.chip_cfg.gyro_fsr = 0xFF;\n st.chip_cfg.accel_fsr = 0xFF;\n st.chip_cfg.lpf = 0xFF;\n st.chip_cfg.sample_rate = 0xFFFF;\n st.chip_cfg.fifo_enable = 0xFF;\n st.chip_cfg.bypass_mode = 0xFF;\n#ifdef AK89xx_SECONDARY\n st.chip_cfg.compass_sample_rate = 0xFFFF;\n#endif\n \/* mpu_set_sensors always preserves this setting. *\/\n st.chip_cfg.clk_src = INV_CLK_PLL;\n \/* Handled in next call to mpu_set_bypass. *\/\n st.chip_cfg.active_low_int = 1;\n st.chip_cfg.latched_int = 0;\n st.chip_cfg.int_motion_only = 0;\n st.chip_cfg.lp_accel_mode = 0;\n memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));\n st.chip_cfg.dmp_on = 0;\n st.chip_cfg.dmp_loaded = 0;\n st.chip_cfg.dmp_sample_rate = 0;\n\n if (mpu_set_gyro_fsr(2000))\n return -1;\n if (mpu_set_accel_fsr(2))\n return -1;\n if (mpu_set_lpf(42))\n return -1;\n if (mpu_set_sample_rate(50))\n return -1;\n if (mpu_configure_fifo(0))\n return -1;\n#ifdef AK89xx_SECONDARY\n setup_compass();\n if (mpu_set_compass_sample_rate(10))\n return -1;\n#else\n \/* Already disabled by setup_compass. *\/\n if (mpu_set_bypass(0))\n return -1;\n#endif\n\n mpu_set_sensors(0);\n return 0;\n}\n\n\/* ---------------------- \u59ff\u6001\u89e3\u7b97\u5c42 ---------------------- *\/\n\n\/* These defines are copied from dmpDefaultMPU6050.c in the general MPL\n * releases. These defines may change for each DMP image, so be sure to modify\n * these values when switching to a new image.\n *\/\n#define CFG_LP_QUAT (2712)\n#define END_ORIENT_TEMP (1866)\n#define CFG_27 (2742)\n#define CFG_20 (2224)\n#define CFG_23 (2745)\n#define CFG_FIFO_ON_EVENT (2690)\n#define END_PREDICTION_UPDATE (1761)\n#define CGNOTICE_INTR (2620)\n#define X_GRT_Y_TMP (1358)\n#define CFG_DR_INT (1029)\n#define CFG_AUTH (1035)\n#define UPDATE_PROP_ROT (1835)\n#define END_COMPARE_Y_X_TMP2 (1455)\n#define SKIP_X_GRT_Y_TMP (1359)\n#define SKIP_END_COMPARE (1435)\n#define FCFG_3 (1088)\n#define FCFG_2 (1066)\n#define FCFG_1 (1062)\n#define END_COMPARE_Y_X_TMP3 (1434)\n#define FCFG_7 (1073)\n#define FCFG_6 (1106)\n#define FLAT_STATE_END (1713)\n#define SWING_END_4 (1616)\n#define SWING_END_2 (1565)\n#define SWING_END_3 (1587)\n#define SWING_END_1 (1550)\n#define CFG_8 (2718)\n#define CFG_15 (2727)\n#define CFG_16 (2746)\n#define CFG_EXT_GYRO_BIAS (1189)\n#define END_COMPARE_Y_X_TMP (1407)\n#define DO_NOT_UPDATE_PROP_ROT (1839)\n#define CFG_7 (1205)\n#define FLAT_STATE_END_TEMP (1683)\n#define END_COMPARE_Y_X (1484)\n#define SKIP_SWING_END_1 (1551)\n#define SKIP_SWING_END_3 (1588)\n#define SKIP_SWING_END_2 (1566)\n#define TILTG75_START (1672)\n#define CFG_6 (2753)\n#define TILTL75_END (1669)\n#define END_ORIENT (1884)\n#define CFG_FLICK_IN (2573)\n#define TILTL75_START (1643)\n#define CFG_MOTION_BIAS (1208)\n#define X_GRT_Y (1408)\n#define TEMPLABEL (2324)\n#define CFG_ANDROID_ORIENT_INT (1853)\n#define CFG_GYRO_RAW_DATA (2722)\n#define X_GRT_Y_TMP2 (1379)\n\n#define D_0_22 (22+512)\n#define D_0_24 (24+512)\n\n#define D_0_36 (36)\n#define D_0_52 (52)\n#define D_0_96 (96)\n#define D_0_104 (104)\n#define D_0_108 (108)\n#define D_0_163 (163)\n#define D_0_188 (188)\n#define D_0_192 (192)\n#define D_0_224 (224)\n#define D_0_228 (228)\n#define D_0_232 (232)\n#define D_0_236 (236)\n\n#define D_1_2 (256 + 2)\n#define D_1_4 (256 + 4)\n#define D_1_8 (256 + 8)\n#define D_1_10 (256 + 10)\n#define D_1_24 (256 + 24)\n#define D_1_28 (256 + 28)\n#define D_1_36 (256 + 36)\n#define D_1_40 (256 + 40)\n#define D_1_44 (256 + 44)\n#define D_1_72 (256 + 72)\n#define D_1_74 (256 + 74)\n#define D_1_79 (256 + 79)\n#define D_1_88 (256 + 88)\n#define D_1_90 (256 + 90)\n#define D_1_92 (256 + 92)\n#define D_1_96 (256 + 96)\n#define D_1_98 (256 + 98)\n#define D_1_106 (256 + 106)\n#define D_1_108 (256 + 108)\n#define D_1_112 (256 + 112)\n#define D_1_128 (256 + 144)\n#define D_1_152 (256 + 12)\n#define D_1_160 (256 + 160)\n#define D_1_176 (256 + 176)\n#define D_1_178 (256 + 178)\n#define D_1_218 (256 + 218)\n#define D_1_232 (256 + 232)\n#define D_1_236 (256 + 236)\n#define D_1_240 (256 + 240)\n#define D_1_244 (256 + 244)\n#define D_1_250 (256 + 250)\n#define D_1_252 (256 + 252)\n#define D_2_12 (512 + 12)\n#define D_2_96 (512 + 96)\n#define D_2_108 (512 + 108)\n#define D_2_208 (512 + 208)\n#define D_2_224 (512 + 224)\n#define D_2_236 (512 + 236)\n#define D_2_244 (512 + 244)\n#define D_2_248 (512 + 248)\n#define D_2_252 (512 + 252)\n\n#define CPASS_BIAS_X (35 * 16 + 4)\n#define CPASS_BIAS_Y (35 * 16 + 8)\n#define CPASS_BIAS_Z (35 * 16 + 12)\n#define CPASS_MTX_00 (36 * 16)\n#define CPASS_MTX_01 (36 * 16 + 4)\n#define CPASS_MTX_02 (36 * 16 + 8)\n#define CPASS_MTX_10 (36 * 16 + 12)\n#define CPASS_MTX_11 (37 * 16)\n#define CPASS_MTX_12 (37 * 16 + 4)\n#define CPASS_MTX_20 (37 * 16 + 8)\n#define CPASS_MTX_21 (37 * 16 + 12)\n#define CPASS_MTX_22 (43 * 16 + 12)\n#define D_EXT_GYRO_BIAS_X (61 * 16)\n#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4\n#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8\n#define D_ACT0 (40 * 16)\n#define D_ACSX (40 * 16 + 4)\n#define D_ACSY (40 * 16 + 8)\n#define D_ACSZ (40 * 16 + 12)\n\n#define FLICK_MSG (45 * 16 + 4)\n#define FLICK_COUNTER (45 * 16 + 8)\n#define FLICK_LOWER (45 * 16 + 12)\n#define FLICK_UPPER (46 * 16 + 12)\n\n#define D_AUTH_OUT (992)\n#define D_AUTH_IN (996)\n#define D_AUTH_A (1000)\n#define D_AUTH_B (1004)\n\n#define D_PEDSTD_BP_B (768 + 0x1C)\n#define D_PEDSTD_HP_A (768 + 0x78)\n#define D_PEDSTD_HP_B (768 + 0x7C)\n#define D_PEDSTD_BP_A4 (768 + 0x40)\n#define D_PEDSTD_BP_A3 (768 + 0x44)\n#define D_PEDSTD_BP_A2 (768 + 0x48)\n#define D_PEDSTD_BP_A1 (768 + 0x4C)\n#define D_PEDSTD_INT_THRSH (768 + 0x68)\n#define D_PEDSTD_CLIP (768 + 0x6C)\n#define D_PEDSTD_SB (768 + 0x28)\n#define D_PEDSTD_SB_TIME (768 + 0x2C)\n#define D_PEDSTD_PEAKTHRSH (768 + 0x98)\n#define D_PEDSTD_TIML (768 + 0x2A)\n#define D_PEDSTD_TIMH (768 + 0x2E)\n#define D_PEDSTD_PEAK (768 + 0X94)\n#define D_PEDSTD_STEPCTR (768 + 0x60)\n#define D_PEDSTD_TIMECTR (964)\n#define D_PEDSTD_DECI (768 + 0xA0)\n\n#define D_HOST_NO_MOT (976)\n#define D_ACCEL_BIAS (660)\n\n#define D_ORIENT_GAP (76)\n\n#define D_TILT0_H (48)\n#define D_TILT0_L (50)\n#define D_TILT1_H (52)\n#define D_TILT1_L (54)\n#define D_TILT2_H (56)\n#define D_TILT2_L (58)\n#define D_TILT3_H (60)\n#define D_TILT3_L (62)\n\n#define DMP_CODE_SIZE (3062)\n\nstatic const unsigned char dmp_memory[DMP_CODE_SIZE] = {\n \/* bank # 0 *\/\n 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,\n 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,\n 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,\n 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,\n 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,\n 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,\n 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,\n 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,\n 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,\n 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,\n 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,\n 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,\n 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,\n 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,\n 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n \/* bank # 1 *\/\n 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,\n 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,\n 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,\n 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,\n 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,\n 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,\n 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,\n 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,\n 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,\n 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,\n \/* bank # 2 *\/\n 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,\n 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,\n 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,\n 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,\n 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,\n 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n \/* bank # 3 *\/\n 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,\n 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,\n 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,\n\n \/* bank # 4 *\/\n 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,\n 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,\n 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,\n 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,\n 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,\n 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,\n 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,\n 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,\n 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,\n 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,\n 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,\n 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,\n 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,\n 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,\n 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,\n 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,\n \/* bank # 5 *\/\n 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,\n 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,\n 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,\n 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,\n 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,\n 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,\n 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,\n 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,\n 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,\n 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,\n 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,\n 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,\n 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,\n 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,\n 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,\n 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,\n \/* bank # 6 *\/\n 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,\n 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,\n 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,\n 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,\n 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,\n 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,\n 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,\n 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,\n 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,\n 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,\n 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,\n 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,\n 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,\n 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,\n 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,\n 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,\n \/* bank # 7 *\/\n 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,\n 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,\n 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,\n 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,\n 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,\n 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,\n 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,\n 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,\n 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,\n 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,\n 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,\n 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,\n 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,\n 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,\n 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,\n 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,\n \/* bank # 8 *\/\n 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,\n 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,\n 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,\n 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,\n 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,\n 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,\n 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,\n 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,\n 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,\n 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,\n 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,\n 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,\n 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,\n 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,\n 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,\n 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,\n \/* bank # 9 *\/\n 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,\n 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,\n 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,\n 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,\n 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,\n 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,\n 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,\n 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,\n 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,\n 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,\n 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,\n 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,\n 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,\n 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,\n 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,\n 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,\n \/* bank # 10 *\/\n 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,\n 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,\n 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,\n 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,\n 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,\n 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,\n 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,\n 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,\n 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,\n 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,\n 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,\n 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,\n 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,\n 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,\n 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,\n 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,\n \/* bank # 11 *\/\n 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,\n 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,\n 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,\n 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,\n 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,\n 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,\n 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,\n 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,\n 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,\n 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,\n 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,\n 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,\n 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,\n 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,\n 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,\n 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff\n};\n\nstatic const unsigned short sStartAddress = 0x0400;\n\n\/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c *\/\n\n#define INT_SRC_TAP (0x01)\n#define INT_SRC_ANDROID_ORIENT (0x08)\n\n#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \\\n DMP_FEATURE_SEND_CAL_GYRO)\n\n#define MAX_PACKET_LENGTH_2 (32)\n\n#define DMP_SAMPLE_RATE (200)\n#define GYRO_SF (46850825LL * 200 \/ DMP_SAMPLE_RATE)\n\n#define FIFO_CORRUPTION_CHECK\n#ifdef FIFO_CORRUPTION_CHECK\n#define QUAT_ERROR_THRESH (1L<<24)\n#define QUAT_MAG_SQ_NORMALIZED (1L<<28)\n#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)\n#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)\n#endif\n\nstruct dmp_s {\n void (*tap_cb)(unsigned char count, unsigned char direction);\n void (*android_orient_cb)(unsigned char orientation);\n unsigned short orient;\n unsigned short feature_mask;\n unsigned short fifo_rate;\n unsigned char packet_length;\n};\n\nstatic struct dmp_s dmp = {\n .tap_cb = NULL,\n .android_orient_cb = NULL,\n .orient = 0,\n .feature_mask = 0,\n .fifo_rate = 0,\n .packet_length = 0\n};\n\n\/**\n * @brief Load the DMP with this image.\n * @return 0 if successful.\n *\/\nint dmp_load_motion_driver_firmware(void)\n{\n return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,\n DMP_SAMPLE_RATE);\n}\n\n\/**\n * @brief Push gyro and accel orientation to the DMP.\n * The orientation is represented here as the output of\n * @e inv_orientation_matrix_to_scalar.\n * @param[in] orient Gyro and accel orientation in body frame.\n * @return 0 if successful.\n *\/\nint dmp_set_orientation(unsigned short orient)\n{\n unsigned char gyro_regs[3], accel_regs[3];\n const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};\n const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};\n const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};\n const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};\n\n gyro_regs[0] = gyro_axes[orient & 3];\n gyro_regs[1] = gyro_axes[(orient >> 3) & 3];\n gyro_regs[2] = gyro_axes[(orient >> 6) & 3];\n accel_regs[0] = accel_axes[orient & 3];\n accel_regs[1] = accel_axes[(orient >> 3) & 3];\n accel_regs[2] = accel_axes[(orient >> 6) & 3];\n\n \/* Chip-to-body, axes only. *\/\n if (mpu_write_mem(FCFG_1, 3, gyro_regs))\n return -1;\n if (mpu_write_mem(FCFG_2, 3, accel_regs))\n return -1;\n\n memcpy(gyro_regs, gyro_sign, 3);\n memcpy(accel_regs, accel_sign, 3);\n if (orient & 4) {\n gyro_regs[0] |= 1;\n accel_regs[0] |= 1;\n }\n if (orient & 0x20) {\n gyro_regs[1] |= 1;\n accel_regs[1] |= 1;\n }\n if (orient & 0x100) {\n gyro_regs[2] |= 1;\n accel_regs[2] |= 1;\n }\n\n \/* Chip-to-body, sign only. *\/\n if (mpu_write_mem(FCFG_3, 3, gyro_regs))\n return -1;\n if (mpu_write_mem(FCFG_7, 3, accel_regs))\n return -1;\n dmp.orient = orient;\n return 0;\n}\n\n\/**\n * @brief Push gyro biases to the DMP.\n * Because the gyro integration is handled in the DMP, any gyro biases\n * calculated by the MPL should be pushed down to DMP memory to remove\n * 3-axis quaternion drift.\n * \\n NOTE: If the DMP-based gyro calibration is enabled, the DMP will\n * overwrite the biases written to this location once a new one is computed.\n * @param[in] bias Gyro biases in q16.\n * @return 0 if successful.\n *\/\nint dmp_set_gyro_bias(long *bias)\n{\n long gyro_bias_body[3];\n unsigned char regs[4];\n\n gyro_bias_body[0] = bias[dmp.orient & 3];\n if (dmp.orient & 4)\n gyro_bias_body[0] *= -1;\n gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];\n if (dmp.orient & 0x20)\n gyro_bias_body[1] *= -1;\n gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];\n if (dmp.orient & 0x100)\n gyro_bias_body[2] *= -1;\n\n#ifdef EMPL_NO_64BIT\n gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) \/ 1073741824.f);\n gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) \/ 1073741824.f);\n gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) \/ 1073741824.f);\n#else\n gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);\n gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);\n gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);\n#endif\n\n regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);\n regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);\n regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);\n regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);\n if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))\n return -1;\n\n regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);\n regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);\n regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);\n regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);\n if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))\n return -1;\n\n regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);\n regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);\n regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);\n regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);\n return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);\n}\n\n\/**\n * @brief Push accel biases to the DMP.\n * These biases will be removed from the DMP 6-axis quaternion.\n * @param[in] bias Accel biases in q16.\n * @return 0 if successful.\n *\/\nint dmp_set_accel_bias(long *bias)\n{\n long accel_bias_body[3];\n unsigned char regs[12];\n long long accel_sf;\n unsigned short accel_sens;\n\n mpu_get_accel_sens(&accel_sens);\n accel_sf = (long long)accel_sens << 15;\n \/\/ __no_operation();\n\n accel_bias_body[0] = bias[dmp.orient & 3];\n if (dmp.orient & 4)\n accel_bias_body[0] *= -1;\n accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];\n if (dmp.orient & 0x20)\n accel_bias_body[1] *= -1;\n accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];\n if (dmp.orient & 0x100)\n accel_bias_body[2] *= -1;\n\n#ifdef EMPL_NO_64BIT\n accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) \/ 1073741824.f);\n accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) \/ 1073741824.f);\n accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) \/ 1073741824.f);\n#else\n accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);\n accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);\n accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);\n#endif\n\n regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);\n regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);\n regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);\n regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);\n regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);\n regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);\n regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);\n regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);\n regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);\n regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);\n regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);\n regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);\n return mpu_write_mem(D_ACCEL_BIAS, 12, regs);\n}\n\n\/**\n * @brief Set DMP output rate.\n * Only used when DMP is on.\n * @param[in] rate Desired fifo rate (Hz).\n * @return 0 if successful.\n *\/\nint dmp_set_fifo_rate(unsigned short rate)\n{\n const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,\n 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};\n unsigned short div;\n unsigned char tmp[8];\n\n if (rate > DMP_SAMPLE_RATE)\n return -1;\n div = DMP_SAMPLE_RATE \/ rate - 1;\n tmp[0] = (unsigned char)((div >> 8) & 0xFF);\n tmp[1] = (unsigned char)(div & 0xFF);\n if (mpu_write_mem(D_0_22, 2, tmp))\n return -1;\n if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))\n return -1;\n\n dmp.fifo_rate = rate;\n return 0;\n}\n\n\/**\n * @brief Get DMP output rate.\n * @param[out] rate Current fifo rate (Hz).\n * @return 0 if successful.\n *\/\nint dmp_get_fifo_rate(unsigned short *rate)\n{\n rate[0] = dmp.fifo_rate;\n return 0;\n}\n\n\/**\n * @brief Set tap threshold for a specific axis.\n * @param[in] axis 1, 2, and 4 for XYZ accel, respectively.\n * @param[in] thresh Tap threshold, in mg\/ms.\n * @return 0 if successful.\n *\/\nint dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)\n{\n unsigned char tmp[4], accel_fsr;\n float scaled_thresh;\n unsigned short dmp_thresh, dmp_thresh_2;\n if (!(axis & TAP_XYZ) || thresh > 1600)\n return -1;\n\n scaled_thresh = (float)thresh \/ DMP_SAMPLE_RATE;\n\n mpu_get_accel_fsr(&accel_fsr);\n switch (accel_fsr) {\n case 2:\n dmp_thresh = (unsigned short)(scaled_thresh * 16384);\n \/* dmp_thresh * 0.75 *\/\n dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);\n break;\n case 4:\n dmp_thresh = (unsigned short)(scaled_thresh * 8192);\n \/* dmp_thresh * 0.75 *\/\n dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);\n break;\n case 8:\n dmp_thresh = (unsigned short)(scaled_thresh * 4096);\n \/* dmp_thresh * 0.75 *\/\n dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);\n break;\n case 16:\n dmp_thresh = (unsigned short)(scaled_thresh * 2048);\n \/* dmp_thresh * 0.75 *\/\n dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);\n break;\n default:\n return -1;\n }\n tmp[0] = (unsigned char)(dmp_thresh >> 8);\n tmp[1] = (unsigned char)(dmp_thresh & 0xFF);\n tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);\n tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);\n\n if (axis & TAP_X) {\n if (mpu_write_mem(DMP_TAP_THX, 2, tmp))\n return -1;\n if (mpu_write_mem(D_1_36, 2, tmp+2))\n return -1;\n }\n if (axis & TAP_Y) {\n if (mpu_write_mem(DMP_TAP_THY, 2, tmp))\n return -1;\n if (mpu_write_mem(D_1_40, 2, tmp+2))\n return -1;\n }\n if (axis & TAP_Z) {\n if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))\n return -1;\n if (mpu_write_mem(D_1_44, 2, tmp+2))\n return -1;\n }\n return 0;\n}\n\n\/**\n * @brief Set which axes will register a tap.\n * @param[in] axis 1, 2, and 4 for XYZ, respectively.\n * @return 0 if successful.\n *\/\nint dmp_set_tap_axes(unsigned char axis)\n{\n unsigned char tmp = 0;\n\n if (axis & TAP_X)\n tmp |= 0x30;\n if (axis & TAP_Y)\n tmp |= 0x0C;\n if (axis & TAP_Z)\n tmp |= 0x03;\n return mpu_write_mem(D_1_72, 1, &tmp);\n}\n\n\/**\n * @brief Set minimum number of taps needed for an interrupt.\n * @param[in] min_taps Minimum consecutive taps (1-4).\n * @return 0 if successful.\n *\/\nint dmp_set_tap_count(unsigned char min_taps)\n{\n unsigned char tmp;\n\n if (min_taps < 1)\n min_taps = 1;\n else if (min_taps > 4)\n min_taps = 4;\n\n tmp = min_taps - 1;\n return mpu_write_mem(D_1_79, 1, &tmp);\n}\n\n\/**\n * @brief Set length between valid taps.\n * @param[in] time Milliseconds between taps.\n * @return 0 if successful.\n *\/\nint dmp_set_tap_time(unsigned short time)\n{\n unsigned short dmp_time;\n unsigned char tmp[2];\n\n dmp_time = time \/ (1000 \/ DMP_SAMPLE_RATE);\n tmp[0] = (unsigned char)(dmp_time >> 8);\n tmp[1] = (unsigned char)(dmp_time & 0xFF);\n return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);\n}\n\n\/**\n * @brief Set max time between taps to register as a multi-tap.\n * @param[in] time Max milliseconds between taps.\n * @return 0 if successful.\n *\/\nint dmp_set_tap_time_multi(unsigned short time)\n{\n unsigned short dmp_time;\n unsigned char tmp[2];\n\n dmp_time = time \/ (1000 \/ DMP_SAMPLE_RATE);\n tmp[0] = (unsigned char)(dmp_time >> 8);\n tmp[1] = (unsigned char)(dmp_time & 0xFF);\n return mpu_write_mem(D_1_218, 2, tmp);\n}\n\n\/**\n * @brief Set shake rejection threshold.\n * If the DMP detects a gyro sample larger than @e thresh, taps are rejected.\n * @param[in] sf Gyro scale factor.\n * @param[in] thresh Gyro threshold in dps.\n * @return 0 if successful.\n *\/\nint dmp_set_shake_reject_thresh(long sf, unsigned short thresh)\n{\n unsigned char tmp[4];\n long thresh_scaled = sf \/ 1000 * thresh;\n tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);\n tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);\n tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);\n tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);\n return mpu_write_mem(D_1_92, 4, tmp);\n}\n\n\/**\n * @brief Set shake rejection time.\n * Sets the length of time that the gyro must be outside of the threshold set\n * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory\n * 60 ms is added to this parameter.\n * @param[in] time Time in milliseconds.\n * @return 0 if successful.\n *\/\nint dmp_set_shake_reject_time(unsigned short time)\n{\n unsigned char tmp[2];\n\n time \/= (1000 \/ DMP_SAMPLE_RATE);\n tmp[0] = time >> 8;\n tmp[1] = time & 0xFF;\n return mpu_write_mem(D_1_90,2,tmp);\n}\n\n\/**\n * @brief Set shake rejection timeout.\n * Sets the length of time after a shake rejection that the gyro must stay\n * inside of the threshold before taps can be detected again. A mandatory\n * 60 ms is added to this parameter.\n * @param[in] time Time in milliseconds.\n * @return 0 if successful.\n *\/\nint dmp_set_shake_reject_timeout(unsigned short time)\n{\n unsigned char tmp[2];\n\n time \/= (1000 \/ DMP_SAMPLE_RATE);\n tmp[0] = time >> 8;\n tmp[1] = time & 0xFF;\n return mpu_write_mem(D_1_88,2,tmp);\n}\n\n\/**\n * @brief Get current step count.\n * @param[out] count Number of steps detected.\n * @return 0 if successful.\n *\/\nint dmp_get_pedometer_step_count(unsigned long *count)\n{\n unsigned char tmp[4];\n if (!count)\n return -1;\n\n if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))\n return -1;\n\n count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |\n ((unsigned long)tmp[2] << 8) | tmp[3];\n return 0;\n}\n\n\/**\n * @brief Overwrite current step count.\n * WARNING: This function writes to DMP memory and could potentially encounter\n * a race condition if called while the pedometer is enabled.\n * @param[in] count New step count.\n * @return 0 if successful.\n *\/\nint dmp_set_pedometer_step_count(unsigned long count)\n{\n unsigned char tmp[4];\n\n tmp[0] = (unsigned char)((count >> 24) & 0xFF);\n tmp[1] = (unsigned char)((count >> 16) & 0xFF);\n tmp[2] = (unsigned char)((count >> 8) & 0xFF);\n tmp[3] = (unsigned char)(count & 0xFF);\n return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);\n}\n\n\/**\n * @brief Get duration of walking time.\n * @param[in] time Walk time in milliseconds.\n * @return 0 if successful.\n *\/\nint dmp_get_pedometer_walk_time(unsigned long *time)\n{\n unsigned char tmp[4];\n if (!time)\n return -1;\n\n if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))\n return -1;\n\n time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |\n ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;\n return 0;\n}\n\n\/**\n * @brief Overwrite current walk time.\n * WARNING: This function writes to DMP memory and could potentially encounter\n * a race condition if called while the pedometer is enabled.\n * @param[in] time New walk time in milliseconds.\n *\/\nint dmp_set_pedometer_walk_time(unsigned long time)\n{\n unsigned char tmp[4];\n\n time \/= 20;\n\n tmp[0] = (unsigned char)((time >> 24) & 0xFF);\n tmp[1] = (unsigned char)((time >> 16) & 0xFF);\n tmp[2] = (unsigned char)((time >> 8) & 0xFF);\n tmp[3] = (unsigned char)(time & 0xFF);\n return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);\n}\n\n\/**\n * @brief Get list of currently enabled DMP features.\n * @param[out] Mask of enabled features.\n * @return 0 if successful.\n *\/\nint dmp_get_enabled_features(unsigned short *mask)\n{\n mask[0] = dmp.feature_mask;\n return 0;\n}\n\n\/**\n * @brief Calibrate the gyro data in the DMP.\n * After eight seconds of no motion, the DMP will compute gyro biases and\n * subtract them from the quaternion output. If @e dmp_enable_feature is\n * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be\n * subtracted from the gyro output.\n * @param[in] enable 1 to enable gyro calibration.\n * @return 0 if successful.\n *\/\nint dmp_enable_gyro_cal(unsigned char enable)\n{\n if (enable) {\n unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};\n return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);\n } else {\n unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};\n return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);\n }\n}\n\n\/**\n * @brief Generate 3-axis quaternions from the DMP.\n * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually\n * exclusive.\n * @param[in] enable 1 to enable 3-axis quaternion.\n * @return 0 if successful.\n *\/\nint dmp_enable_lp_quat(unsigned char enable)\n{\n unsigned char regs[4];\n if (enable) {\n regs[0] = DINBC0;\n regs[1] = DINBC2;\n regs[2] = DINBC4;\n regs[3] = DINBC6;\n }\n else\n memset(regs, 0x8B, 4);\n\n mpu_write_mem(CFG_LP_QUAT, 4, regs);\n\n return mpu_reset_fifo();\n}\n\n\/**\n * @brief Generate 6-axis quaternions from the DMP.\n * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually\n * exclusive.\n * @param[in] enable 1 to enable 6-axis quaternion.\n * @return 0 if successful.\n *\/\nint dmp_enable_6x_lp_quat(unsigned char enable)\n{\n unsigned char regs[4];\n if (enable) {\n regs[0] = DINA20;\n regs[1] = DINA28;\n regs[2] = DINA30;\n regs[3] = DINA38;\n } else\n memset(regs, 0xA3, 4);\n\n mpu_write_mem(CFG_8, 4, regs);\n\n return mpu_reset_fifo();\n}\n\n\/**\n * @brief Decode the four-byte gesture data and execute any callbacks.\n * @param[in] gesture Gesture data from DMP packet.\n * @return 0 if successful.\n *\/\nstatic int decode_gesture(unsigned char *gesture)\n{\n unsigned char tap, android_orient;\n\n android_orient = gesture[3] & 0xC0;\n tap = 0x3F & gesture[3];\n\n if (gesture[1] & INT_SRC_TAP) {\n unsigned char direction, count;\n direction = tap >> 3;\n count = (tap % 8) + 1;\n if (dmp.tap_cb)\n dmp.tap_cb(direction, count);\n }\n\n if (gesture[1] & INT_SRC_ANDROID_ORIENT) {\n if (dmp.android_orient_cb)\n dmp.android_orient_cb(android_orient >> 6);\n }\n\n return 0;\n}\n\n\/**\n * @brief Specify when a DMP interrupt should occur.\n * A DMP interrupt can be configured to trigger on either of the two\n * conditions below:\n * \\n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).\n * \\n b. A tap event has been detected.\n * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.\n * @return 0 if successful.\n *\/\nint dmp_set_interrupt_mode(unsigned char mode)\n{\n const unsigned char regs_continuous[11] =\n {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};\n const unsigned char regs_gesture[11] =\n {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};\n\n switch (mode) {\n case DMP_INT_CONTINUOUS:\n return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,\n (unsigned char*)regs_continuous);\n case DMP_INT_GESTURE:\n return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,\n (unsigned char*)regs_gesture);\n default:\n return -1;\n }\n}\n\n\/**\n * @brief Get one packet from the FIFO.\n * If @e sensors does not contain a particular sensor, disregard the data\n * returned to that pointer.\n * \\n @e sensors can contain a combination of the following flags:\n * \\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO\n * \\n INV_XYZ_GYRO\n * \\n INV_XYZ_ACCEL\n * \\n INV_WXYZ_QUAT\n * \\n If the FIFO has no new data, @e sensors will be zero.\n * \\n If the FIFO is disabled, @e sensors will be zero and this function will\n * return a non-zero error code.\n * @param[out] gyro Gyro data in hardware units.\n * @param[out] accel Accel data in hardware units.\n * @param[out] quat 3-axis quaternion data in hardware units.\n * @param[out] timestamp Timestamp in milliseconds.\n * @param[out] sensors Mask of sensors read from FIFO.\n * @param[out] more Number of remaining packets.\n * @return 0 if successful.\n *\/\nint dmp_read_fifo(short *gyro, short *accel, long *quat,\n unsigned long *timestamp, short *sensors, unsigned char *more)\n{\n unsigned char fifo_data[MAX_PACKET_LENGTH_2];\n unsigned char ii = 0;\n\n \/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can\n * cache this value and save some cycles.\n *\/\n sensors[0] = 0;\n\n \/* Get a packet. *\/\n if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))\n return -1;\n\n \/* Parse DMP packet. *\/\n if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {\n#ifdef FIFO_CORRUPTION_CHECK\n long quat_q14[4], quat_mag_sq;\n#endif\n quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |\n ((long)fifo_data[2] << 8) | fifo_data[3];\n quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |\n ((long)fifo_data[6] << 8) | fifo_data[7];\n quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |\n ((long)fifo_data[10] << 8) | fifo_data[11];\n quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |\n ((long)fifo_data[14] << 8) | fifo_data[15];\n ii += 16;\n#ifdef FIFO_CORRUPTION_CHECK\n \/* We can detect a corrupted FIFO by monitoring the quaternion data and\n * ensuring that the magnitude is always normalized to one. This\n * shouldn't happen in normal operation, but if an I2C error occurs,\n * the FIFO reads might become misaligned.\n *\n * Let's start by scaling down the quaternion data to avoid long long\n * math.\n *\/\n quat_q14[0] = quat[0] >> 16;\n quat_q14[1] = quat[1] >> 16;\n quat_q14[2] = quat[2] >> 16;\n quat_q14[3] = quat[3] >> 16;\n quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +\n quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];\n if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||\n (quat_mag_sq > QUAT_MAG_SQ_MAX)) {\n \/* Quaternion is outside of the acceptable threshold. *\/\n mpu_reset_fifo();\n sensors[0] = 0;\n return -1;\n }\n sensors[0] |= INV_WXYZ_QUAT;\n#endif\n }\n\n if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {\n accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];\n accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];\n accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];\n ii += 6;\n sensors[0] |= INV_XYZ_ACCEL;\n }\n\n if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {\n gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];\n gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];\n gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];\n ii += 6;\n sensors[0] |= INV_XYZ_GYRO;\n }\n\n \/* Gesture data is at the end of the DMP packet. Parse it and call\n * the gesture callbacks (if registered).\n *\/\n if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))\n decode_gesture(fifo_data + ii);\n\n get_ms(timestamp);\n return 0;\n}\n\n\/**\n * @brief Enable DMP features.\n * The following \\#define's are used in the input mask:\n * \\n DMP_FEATURE_TAP\n * \\n DMP_FEATURE_ANDROID_ORIENT\n * \\n DMP_FEATURE_LP_QUAT\n * \\n DMP_FEATURE_6X_LP_QUAT\n * \\n DMP_FEATURE_GYRO_CAL\n * \\n DMP_FEATURE_SEND_RAW_ACCEL\n * \\n DMP_FEATURE_SEND_RAW_GYRO\n * \\n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually\n * exclusive.\n * \\n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also\n * mutually exclusive.\n * @param[in] mask Mask of features to enable.\n * @return 0 if successful.\n *\/\nint dmp_enable_feature(unsigned short mask)\n{\n unsigned char tmp[10];\n\n \/* TODO: All of these settings can probably be integrated into the default\n * DMP image.\n *\/\n \/* Set integration scale factor. *\/\n tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);\n tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);\n tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);\n tmp[3] = (unsigned char)(GYRO_SF & 0xFF);\n mpu_write_mem(D_0_104, 4, tmp);\n\n \/* Send sensor data to the FIFO. *\/\n tmp[0] = 0xA3;\n if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {\n tmp[1] = 0xC0;\n tmp[2] = 0xC8;\n tmp[3] = 0xC2;\n } else {\n tmp[1] = 0xA3;\n tmp[2] = 0xA3;\n tmp[3] = 0xA3;\n }\n if (mask & DMP_FEATURE_SEND_ANY_GYRO) {\n tmp[4] = 0xC4;\n tmp[5] = 0xCC;\n tmp[6] = 0xC6;\n } else {\n tmp[4] = 0xA3;\n tmp[5] = 0xA3;\n tmp[6] = 0xA3;\n }\n tmp[7] = 0xA3;\n tmp[8] = 0xA3;\n tmp[9] = 0xA3;\n mpu_write_mem(CFG_15,10,tmp);\n\n \/* Send gesture data to the FIFO. *\/\n if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))\n tmp[0] = DINA20;\n else\n tmp[0] = 0xD8;\n mpu_write_mem(CFG_27,1,tmp);\n\n if (mask & DMP_FEATURE_GYRO_CAL)\n dmp_enable_gyro_cal(1);\n else\n dmp_enable_gyro_cal(0);\n\n if (mask & DMP_FEATURE_SEND_ANY_GYRO) {\n if (mask & DMP_FEATURE_SEND_CAL_GYRO) {\n tmp[0] = 0xB2;\n tmp[1] = 0x8B;\n tmp[2] = 0xB6;\n tmp[3] = 0x9B;\n } else {\n tmp[0] = DINAC0;\n tmp[1] = DINA80;\n tmp[2] = DINAC2;\n tmp[3] = DINA90;\n }\n mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);\n }\n\n if (mask & DMP_FEATURE_TAP) {\n \/* Enable tap. *\/\n tmp[0] = 0xF8;\n mpu_write_mem(CFG_20, 1, tmp);\n dmp_set_tap_thresh(TAP_XYZ, 250);\n dmp_set_tap_axes(TAP_XYZ);\n dmp_set_tap_count(1);\n dmp_set_tap_time(100);\n dmp_set_tap_time_multi(500);\n\n dmp_set_shake_reject_thresh(GYRO_SF, 200);\n dmp_set_shake_reject_time(40);\n dmp_set_shake_reject_timeout(10);\n } else {\n tmp[0] = 0xD8;\n mpu_write_mem(CFG_20, 1, tmp);\n }\n\n if (mask & DMP_FEATURE_ANDROID_ORIENT) {\n tmp[0] = 0xD9;\n } else\n tmp[0] = 0xD8;\n mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);\n\n if (mask & DMP_FEATURE_LP_QUAT)\n dmp_enable_lp_quat(1);\n else\n dmp_enable_lp_quat(0);\n\n if (mask & DMP_FEATURE_6X_LP_QUAT)\n dmp_enable_6x_lp_quat(1);\n else\n dmp_enable_6x_lp_quat(0);\n\n \/* Pedometer is always enabled. *\/\n dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;\n mpu_reset_fifo();\n\n dmp.packet_length = 0;\n if (mask & DMP_FEATURE_SEND_RAW_ACCEL)\n dmp.packet_length += 6;\n if (mask & DMP_FEATURE_SEND_ANY_GYRO)\n dmp.packet_length += 6;\n if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))\n dmp.packet_length += 16;\n if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))\n dmp.packet_length += 4;\n\n return 0;\n}\n\n\/* ---------------------- dmp\u5e94\u7528\u5c42 ---------------------- *\/\n\n#define PRINT_ACCEL (0x01)\n#define PRINT_GYRO (0x02)\n#define PRINT_QUAT (0x04)\n\n#define ACCEL_ON (0x01)\n#define GYRO_ON (0x02)\n\n#define MOTION (0)\n#define NO_MOTION (1)\n\nstruct rx_s {\n unsigned char header[3];\n unsigned char cmd;\n};\nstruct hal_s {\n unsigned char sensors;\n unsigned char dmp_on;\n unsigned char wait_for_tap;\n volatile unsigned char new_gyro;\n unsigned short report;\n unsigned short dmp_features;\n unsigned char motion_int_mode;\n struct rx_s rx;\n};\n\nstatic struct hal_s hal = {0};\n\n\/* The sensors can be mounted onto the board in any orientation. The mounting\n * matrix seen below tells the MPL how to rotate the raw data from thei\n * driver(s).\n * TODO: The following matrices refer to the configuration on an internal test\n * board at Invensense. If needed, please modify the matrices to match the\n * chip-to-body matrix for your particular set up.\n *\/\nstatic signed char gyro_orientation[9] = {-1, 0, 0,\n 0,-1, 0,\n 0, 0, 1};\n\n\/* These next two functions converts the orientation matrix (see\n * gyro_orientation) to a scalar representation for use by the DMP.\n * NOTE: These functions are borrowed from Invensense's MPL.\n *\/\nstatic inline unsigned short inv_row_2_scale(const signed char *row)\n{\n unsigned short b;\n\n if (row[0] > 0)\n b = 0;\n else if (row[0] < 0)\n b = 4;\n else if (row[1] > 0)\n b = 1;\n else if (row[1] < 0)\n b = 5;\n else if (row[2] > 0)\n b = 2;\n else if (row[2] < 0)\n b = 6;\n else\n b = 7; \/\/ error\n return b;\n}\n\nstatic inline unsigned short inv_orientation_matrix_to_scalar(\n const signed char *mtx)\n{\n unsigned short scalar;\n\n \/*\n XYZ 010_001_000 Identity Matrix\n XZY 001_010_000\n YXZ 010_000_001\n YZX 000_010_001\n ZXY 001_000_010\n ZYX 000_001_010\n *\/\n\n scalar = inv_row_2_scale(mtx);\n scalar |= inv_row_2_scale(mtx + 3) << 3;\n scalar |= inv_row_2_scale(mtx + 6) << 6;\n\n\n return scalar;\n}\n\nstatic inline void run_self_test(void)\n{\n int result;\n long gyro[3], accel[3];\n unsigned char i = 0;\n\n#if defined (MPU6500) || defined (MPU9250)\n result = mpu_run_6500_self_test(gyro, accel, 0);\n#elif defined (MPU6050) || defined (MPU9150)\n result = mpu_run_self_test(gyro, accel);\n#endif\n if (result == 0x7) {\n \/* Test passed. We can trust the gyro data here, so let's push it down\n * to the DMP.\n *\/\n for(i = 0; i<3; i++) {\n gyro[i] = (long)(gyro[i] * 32.8f); \/\/convert to +-1000dps\n accel[i] *= 2048.f; \/\/convert to +-16G\n accel[i] = accel[i] >> 16;\n gyro[i] = (long)(gyro[i] >> 16);\n }\n\n mpu_set_gyro_bias_reg(gyro);\n\n#if defined (MPU6500) || defined (MPU9250)\n mpu_set_accel_bias_6500_reg(accel);\n#elif defined (MPU6050) || defined (MPU9150)\n mpu_set_accel_bias_6050_reg(accel);\n#endif\n }\n}\n\n\nvoid mpu_dmp_init()\n{\n int result;\n unsigned char accel_fsr;\n unsigned short gyro_rate, gyro_fsr;\n\n \/* Set up gyro.\n * Every function preceded by mpu_ is a driver function and can be found\n * in inv_mpu.h.\n *\/\n\n result = mpu_init();\n if (result)\n system_reset();\n \/* If you're not using an MPU9150 AND you're not using DMP features, this\n * function will place all slaves on the primary bus.\n * mpu_set_bypass(1);\n *\/\n\n \/* Get\/set hardware configuration. Start gyro. *\/\n \/* Wake up all sensors. *\/\n mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);\n \/* Push both gyro and accel data into the FIFO. *\/\n mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);\n mpu_set_sample_rate(DEFAULT_MPU_HZ);\n \/* Read back configuration in case it was set improperly. *\/\n mpu_get_sample_rate(&gyro_rate);\n mpu_get_gyro_fsr(&gyro_fsr);\n mpu_get_accel_fsr(&accel_fsr);\n\n \/* To initialize the DMP:\n * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in\n * inv_mpu_dmp_motion_driver.h into the MPU memory.\n * 2. Push the gyro and accel orientation matrix to the DMP.\n * 3. Register gesture callbacks. Don't worry, these callbacks won't be\n * executed unless the corresponding feature is enabled.\n * 4. Call dmp_enable_feature(mask) to enable different features.\n * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.\n * 6. Call any feature-specific control functions.\n *\n * To enable the DMP, just call mpu_set_dmp_state(1). This function can\n * be called repeatedly to enable and disable the DMP at runtime.\n *\n * The following is a short summary of the features supported in the DMP\n * image provided in inv_mpu_dmp_motion_driver.c:\n * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at\n * 200Hz. Integrating the gyro data at higher rates reduces numerical\n * errors (compared to integration on the MCU at a lower sampling rate).\n * DMP_FEATURE_6X_LP_QUAT: Generate a gyro\/accel quaternion on the DMP at\n * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.\n * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.\n * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers\n * an event at the four orientations where the screen should rotate.\n * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of\n * no motion.\n * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.\n * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.\n * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot\n * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.\n *\/\n dmp_load_motion_driver_firmware();\n dmp_set_orientation(\n inv_orientation_matrix_to_scalar(gyro_orientation));\n \/*\n * Known Bug -\n * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate\n * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once\n * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz\n * there will be a 25Hz interrupt from the MPU device.\n *\n * There is a known issue in which if you do not enable DMP_FEATURE_TAP\n * then the interrupts will be at 200Hz even if fifo rate\n * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP\n *\/\n hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |\n DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |\n DMP_FEATURE_GYRO_CAL;\n dmp_enable_feature(hal.dmp_features);\n dmp_set_fifo_rate(DEFAULT_MPU_HZ);\n mpu_set_dmp_state(1);\n \/* Wait for enumeration. *\/\n}\n\nbool mpu_dmp_read_raw(long* quat, short* gyro, short* accel)\n{\n unsigned long sensor_timestamp;\n unsigned char more;\n short sensors;\n\n if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more))\n return false;\n\n return true;\n}\n\nshort gyro[3], accel[3], sensors;\n#define q30 1073741824.0f \nfloat q0, q1, q2, q3; \nbool mpu_dmp_convert_euler(float *pitch, float *roll, float *yaw)\n{\n unsigned long sensor_timestamp;\n unsigned char more;\n long quat[4];\n if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more))\n return false;\n\n \/\/ \u56db\u5143\u6570\u6570\u636e\u89e3\u6790\uff08q30\u683c\u5f0f\u8f6c\u6d6e\u70b9\uff09\n if (sensors & INV_WXYZ_QUAT) \n {\n q0 = quat[0] \/ q30;\n q1 = quat[1] \/ q30;\n q2 = quat[2] \/ q30;\n q3 = quat[3] \/ q30;\n\n \/\/ \u6b27\u62c9\u89d2\u8ba1\u7b97\uff08\u5355\u4f4d\uff1a\u5ea6\uff09\n *pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; \/\/ \u4fef\u4ef0\u89d2\n *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, \n -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; \/\/ \u6a2a\u6eda\u89d2\n *yaw = atan2(2 * (q1 * q2 + q0 * q3),\n q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; \/\/ \u504f\u822a\u89d2\n }\n else {\n return false;\n }\n return true;\n}\n\n<\/code><\/pre>\nmpu.h<\/h3>\n
#ifndef MPU_H\n#define MPU_H\n\n#include <stdbool.h>\n\n\n\/* ---------------------- \u6309\u9700\u66f4\u6539\u7684\u53c2\u6570 ---------------------- *\/\n#define DEFAULT_MPU_HZ (200)\n\n\/\/ \u5173\u4e8e\u9009\u578b\uff1a\n\/\/ \u53c2\u89c1mpu.c \u5408\u6cd5\u6027\u5224\u65ad\uff08iic\u9a71\u52a8\u5c42\u5f00\u5934\u90e8\u5206\uff09\uff0c\u81ea\u884c\u51b3\u5b9a\n#define MPU6050\n\n\nvoid mpu_dmp_init();\nbool mpu_dmp_read_raw(long* quat, short* gyro, short* accel);\nbool mpu_dmp_convert_euler(float *pitch, float *roll, float *yaw);\n\n\/* ---------------------- \u4e0d\u8981\u52a8 ---------------------- *\/\n\/* ---------------------- dmpKey.h ---------------------- *\/\n#define KEY_CFG_25 (0)\n#define KEY_CFG_24 (KEY_CFG_25 + 1)\n#define KEY_CFG_26 (KEY_CFG_24 + 1)\n#define KEY_CFG_27 (KEY_CFG_26 + 1)\n#define KEY_CFG_21 (KEY_CFG_27 + 1)\n#define KEY_CFG_20 (KEY_CFG_21 + 1)\n#define KEY_CFG_TAP4 (KEY_CFG_20 + 1)\n#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1)\n#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1)\n#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1)\n#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1)\n#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1)\n#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1)\n#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1)\n#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1)\n#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1)\n#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1)\n#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1)\n#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1)\n#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1)\n#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1)\n#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1)\n#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1)\n#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1)\n#define KEY_FCFG_1 (KEY_CFG_23 + 1)\n#define KEY_FCFG_3 (KEY_FCFG_1 + 1)\n#define KEY_FCFG_2 (KEY_FCFG_3 + 1)\n#define KEY_CFG_3D (KEY_FCFG_2 + 1)\n#define KEY_CFG_3B (KEY_CFG_3D + 1)\n#define KEY_CFG_3C (KEY_CFG_3B + 1)\n#define KEY_FCFG_5 (KEY_CFG_3C + 1)\n#define KEY_FCFG_4 (KEY_FCFG_5 + 1)\n#define KEY_FCFG_7 (KEY_FCFG_4 + 1)\n#define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1)\n#define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1)\n#define KEY_FCFG_6 (KEY_FCFG_AZ + 1)\n#define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1)\n#define KEY_CFG_12 (KEY_FCFG_LSB4 + 1)\n#define KEY_CFG_14 (KEY_CFG_12 + 1)\n#define KEY_CFG_15 (KEY_CFG_14 + 1)\n#define KEY_CFG_16 (KEY_CFG_15 + 1)\n#define KEY_CFG_18 (KEY_CFG_16 + 1)\n#define KEY_CFG_6 (KEY_CFG_18 + 1)\n#define KEY_CFG_7 (KEY_CFG_6 + 1)\n#define KEY_CFG_4 (KEY_CFG_7 + 1)\n#define KEY_CFG_5 (KEY_CFG_4 + 1)\n#define KEY_CFG_2 (KEY_CFG_5 + 1)\n#define KEY_CFG_3 (KEY_CFG_2 + 1)\n#define KEY_CFG_1 (KEY_CFG_3 + 1)\n#define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1)\n#define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1)\n#define KEY_CFG_9 (KEY_CFG_8 + 1)\n#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1)\n#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1)\n#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1)\n#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1)\n#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1)\n#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1)\n#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1)\n#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1)\n#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1)\n#define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1)\n\n\/* MPU6050 keys *\/\n#define KEY_CFG_ACCEL_FILTER (KEY_CFG_LP_QUAT + 1)\n#define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1)\n#define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1)\n\n#define KEY_D_0_22 (KEY_TEMPLABEL + 1)\n#define KEY_D_0_24 (KEY_D_0_22 + 1)\n#define KEY_D_0_36 (KEY_D_0_24 + 1)\n#define KEY_D_0_52 (KEY_D_0_36 + 1)\n#define KEY_D_0_96 (KEY_D_0_52 + 1)\n#define KEY_D_0_104 (KEY_D_0_96 + 1)\n#define KEY_D_0_108 (KEY_D_0_104 + 1)\n#define KEY_D_0_163 (KEY_D_0_108 + 1)\n#define KEY_D_0_188 (KEY_D_0_163 + 1)\n#define KEY_D_0_192 (KEY_D_0_188 + 1)\n#define KEY_D_0_224 (KEY_D_0_192 + 1)\n#define KEY_D_0_228 (KEY_D_0_224 + 1)\n#define KEY_D_0_232 (KEY_D_0_228 + 1)\n#define KEY_D_0_236 (KEY_D_0_232 + 1)\n\n#define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1)\n#define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1)\n#define KEY_D_1_4 (KEY_D_1_2 + 1)\n#define KEY_D_1_8 (KEY_D_1_4 + 1)\n#define KEY_D_1_10 (KEY_D_1_8 + 1)\n#define KEY_D_1_24 (KEY_D_1_10 + 1)\n#define KEY_D_1_28 (KEY_D_1_24 + 1)\n#define KEY_D_1_36 (KEY_D_1_28 + 1)\n#define KEY_D_1_40 (KEY_D_1_36 + 1)\n#define KEY_D_1_44 (KEY_D_1_40 + 1)\n#define KEY_D_1_72 (KEY_D_1_44 + 1)\n#define KEY_D_1_74 (KEY_D_1_72 + 1)\n#define KEY_D_1_79 (KEY_D_1_74 + 1)\n#define KEY_D_1_88 (KEY_D_1_79 + 1)\n#define KEY_D_1_90 (KEY_D_1_88 + 1)\n#define KEY_D_1_92 (KEY_D_1_90 + 1)\n#define KEY_D_1_96 (KEY_D_1_92 + 1)\n#define KEY_D_1_98 (KEY_D_1_96 + 1)\n#define KEY_D_1_100 (KEY_D_1_98 + 1)\n#define KEY_D_1_106 (KEY_D_1_100 + 1)\n#define KEY_D_1_108 (KEY_D_1_106 + 1)\n#define KEY_D_1_112 (KEY_D_1_108 + 1)\n#define KEY_D_1_128 (KEY_D_1_112 + 1)\n#define KEY_D_1_152 (KEY_D_1_128 + 1)\n#define KEY_D_1_160 (KEY_D_1_152 + 1)\n#define KEY_D_1_168 (KEY_D_1_160 + 1)\n#define KEY_D_1_175 (KEY_D_1_168 + 1)\n#define KEY_D_1_176 (KEY_D_1_175 + 1)\n#define KEY_D_1_178 (KEY_D_1_176 + 1)\n#define KEY_D_1_179 (KEY_D_1_178 + 1)\n#define KEY_D_1_218 (KEY_D_1_179 + 1)\n#define KEY_D_1_232 (KEY_D_1_218 + 1)\n#define KEY_D_1_236 (KEY_D_1_232 + 1)\n#define KEY_D_1_240 (KEY_D_1_236 + 1)\n#define KEY_D_1_244 (KEY_D_1_240 + 1)\n#define KEY_D_1_250 (KEY_D_1_244 + 1)\n#define KEY_D_1_252 (KEY_D_1_250 + 1)\n#define KEY_D_2_12 (KEY_D_1_252 + 1)\n#define KEY_D_2_96 (KEY_D_2_12 + 1)\n#define KEY_D_2_108 (KEY_D_2_96 + 1)\n#define KEY_D_2_208 (KEY_D_2_108 + 1)\n#define KEY_FLICK_MSG (KEY_D_2_208 + 1)\n#define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1)\n#define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1)\n#define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1)\n#define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1)\n#define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1)\n#define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1)\n#define KEY_D_2_244 (KEY_D_2_224 + 1)\n#define KEY_D_2_248 (KEY_D_2_244 + 1)\n#define KEY_D_2_252 (KEY_D_2_248 + 1)\n\n#define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1)\n#define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1)\n#define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1)\n#define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1)\n#define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1)\n#define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1)\n#define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1)\n#define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1)\n#define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE +1)\n#define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1)\n#define KEY_D_CR_TIME_G (KEY_D_OUTPUT_ENABLE + 1)\n#define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1)\n#define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1)\n#define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1)\n#define KEY_D_CS_TAY (KEY_D_CS_TAX + 1)\n#define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1)\n#define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1)\n#define KEY_D_CS_TGY (KEY_D_CS_TGX + 1)\n#define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1)\n#define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1)\n#define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1)\n#define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1)\n#define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1)\n\n\/* Compass keys *\/\n#define KEY_CPASS_BIAS_X (KEY_D_CS_TQ3 + 1)\n#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1)\n#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1)\n#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1)\n#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1)\n#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1)\n#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1)\n#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1)\n#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1)\n#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1)\n#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1)\n#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1)\n\n\/* Gesture Keys *\/\n#define KEY_DMP_TAPW_MIN (KEY_CPASS_MTX_22 + 1)\n#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN + 1)\n#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X + 1)\n#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y + 1)\n#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z + 1)\n#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1)\n#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1)\n#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1)\n#define KEY_D_ACT0 (KEY_DMP_ORIENT + 1)\n#define KEY_D_ACSX (KEY_D_ACT0 + 1)\n#define KEY_D_ACSY (KEY_D_ACSX + 1)\n#define KEY_D_ACSZ (KEY_D_ACSY + 1)\n\n#define KEY_X_GRT_Y_TMP (KEY_D_ACSZ + 1)\n#define KEY_SKIP_X_GRT_Y_TMP (KEY_X_GRT_Y_TMP + 1)\n#define KEY_SKIP_END_COMPARE (KEY_SKIP_X_GRT_Y_TMP + 1)\n#define KEY_END_COMPARE_Y_X_TMP2 (KEY_SKIP_END_COMPARE + 1) \n#define KEY_CFG_ANDROID_ORIENT_INT (KEY_END_COMPARE_Y_X_TMP2 + 1)\n#define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_ANDROID_ORIENT_INT + 1)\n#define KEY_END_COMPARE_Y_X_TMP (KEY_NO_ORIENT_INTERRUPT + 1)\n#define KEY_END_ORIENT_1 (KEY_END_COMPARE_Y_X_TMP + 1)\n#define KEY_END_COMPARE_Y_X (KEY_END_ORIENT_1 + 1) \n#define KEY_END_ORIENT (KEY_END_COMPARE_Y_X + 1)\n#define KEY_X_GRT_Y (KEY_END_ORIENT + 1)\n#define KEY_NOT_TIME_MINUS_1 (KEY_X_GRT_Y + 1) \n#define KEY_END_COMPARE_Y_X_TMP3 (KEY_NOT_TIME_MINUS_1 + 1) \n#define KEY_X_GRT_Y_TMP2 (KEY_END_COMPARE_Y_X_TMP3 + 1)\n\n\/* Authenticate Keys *\/\n#define KEY_D_AUTH_OUT (KEY_X_GRT_Y_TMP2 + 1)\n#define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1)\n#define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1)\n#define KEY_D_AUTH_B (KEY_D_AUTH_A + 1)\n\n\/* Pedometer standalone only keys *\/\n#define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1)\n#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1)\n#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1)\n#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1)\n#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1)\n#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1)\n#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1)\n#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1)\n#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1)\n#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1)\n#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1)\n#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1)\n#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1)\n#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1)\n#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1)\n#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1)\n#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1)\n#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR + 1)\n#define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1)\n\n\/*Host Based No Motion*\/\n#define KEY_D_HOST_NO_MOT (KEY_D_PEDSTD_DECI + 1)\n\n\/* EIS keys *\/\n#define KEY_P_EIS_FIFO_FOOTER (KEY_D_HOST_NO_MOT + 1)\n#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER + 1)\n#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT + 1)\n#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE + 1)\n#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT + 1)\n#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC + 1)\n#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT + 1)\n#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY + 1)\n#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER + 1)\n#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC + 1)\n#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH + 1)\n#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH + 1)\n#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH + 1)\n#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH + 1)\n#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH + 1)\n#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H + 1)\n#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H + 1)\n#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H + 1)\n#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H + 1)\n#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H + 1)\n#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H + 1)\n#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH + 1)\n#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH + 1)\n#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH + 1)\n#define KEY_DMP_INTY_H (KEY_DMP_INTX_H + 1)\n#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H + 1)\n\/\/#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H + 1)\n\/\/#define KEY_DMP_HPY_H (KEY_DMP_HPX_H + 1)\n\/\/#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H + 1)\n\n\/* Stream keys *\/\n#define KEY_STREAM_P_GYRO_Z (KEY_DMP_INTZ_H + 1)\n#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z + 1)\n#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y + 1)\n#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X + 1)\n#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP + 1)\n#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y + 1)\n#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X + 1)\n#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z + 1)\n#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y + 1)\n#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X + 1)\n#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER + 1)\n\n#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z + 1)\n\ntypedef struct {\n unsigned short key;\n unsigned short addr;\n} tKeyLabel;\n\n#define DINA0A 0x0a\n#define DINA22 0x22\n#define DINA42 0x42\n#define DINA5A 0x5a\n\n#define DINA06 0x06\n#define DINA0E 0x0e\n#define DINA16 0x16\n#define DINA1E 0x1e\n#define DINA26 0x26\n#define DINA2E 0x2e\n#define DINA36 0x36\n#define DINA3E 0x3e\n#define DINA46 0x46\n#define DINA4E 0x4e\n#define DINA56 0x56\n#define DINA5E 0x5e\n#define DINA66 0x66\n#define DINA6E 0x6e\n#define DINA76 0x76\n#define DINA7E 0x7e\n\n#define DINA00 0x00\n#define DINA08 0x08\n#define DINA10 0x10\n#define DINA18 0x18\n#define DINA20 0x20\n#define DINA28 0x28\n#define DINA30 0x30\n#define DINA38 0x38\n#define DINA40 0x40\n#define DINA48 0x48\n#define DINA50 0x50\n#define DINA58 0x58\n#define DINA60 0x60\n#define DINA68 0x68\n#define DINA70 0x70\n#define DINA78 0x78\n\n#define DINA04 0x04\n#define DINA0C 0x0c\n#define DINA14 0x14\n#define DINA1C 0x1C\n#define DINA24 0x24\n#define DINA2C 0x2c\n#define DINA34 0x34\n#define DINA3C 0x3c\n#define DINA44 0x44\n#define DINA4C 0x4c\n#define DINA54 0x54\n#define DINA5C 0x5c\n#define DINA64 0x64\n#define DINA6C 0x6c\n#define DINA74 0x74\n#define DINA7C 0x7c\n\n#define DINA01 0x01\n#define DINA09 0x09\n#define DINA11 0x11\n#define DINA19 0x19\n#define DINA21 0x21\n#define DINA29 0x29\n#define DINA31 0x31\n#define DINA39 0x39\n#define DINA41 0x41\n#define DINA49 0x49\n#define DINA51 0x51\n#define DINA59 0x59\n#define DINA61 0x61\n#define DINA69 0x69\n#define DINA71 0x71\n#define DINA79 0x79\n\n#define DINA25 0x25\n#define DINA2D 0x2d\n#define DINA35 0x35\n#define DINA3D 0x3d\n#define DINA4D 0x4d\n#define DINA55 0x55\n#define DINA5D 0x5D\n#define DINA6D 0x6d\n#define DINA75 0x75\n#define DINA7D 0x7d\n\n#define DINADC 0xdc\n#define DINAF2 0xf2\n#define DINAAB 0xab\n#define DINAAA 0xaa\n#define DINAF1 0xf1\n#define DINADF 0xdf\n#define DINADA 0xda\n#define DINAB1 0xb1\n#define DINAB9 0xb9\n#define DINAF3 0xf3\n#define DINA8B 0x8b\n#define DINAA3 0xa3\n#define DINA91 0x91\n#define DINAB6 0xb6\n#define DINAB4 0xb4\n\n\n#define DINC00 0x00\n#define DINC01 0x01\n#define DINC02 0x02\n#define DINC03 0x03\n#define DINC08 0x08\n#define DINC09 0x09\n#define DINC0A 0x0a\n#define DINC0B 0x0b\n#define DINC10 0x10\n#define DINC11 0x11\n#define DINC12 0x12\n#define DINC13 0x13\n#define DINC18 0x18\n#define DINC19 0x19\n#define DINC1A 0x1a\n#define DINC1B 0x1b\n\n#define DINC20 0x20\n#define DINC21 0x21\n#define DINC22 0x22\n#define DINC23 0x23\n#define DINC28 0x28\n#define DINC29 0x29\n#define DINC2A 0x2a\n#define DINC2B 0x2b\n#define DINC30 0x30\n#define DINC31 0x31\n#define DINC32 0x32\n#define DINC33 0x33\n#define DINC38 0x38\n#define DINC39 0x39\n#define DINC3A 0x3a\n#define DINC3B 0x3b\n\n#define DINC40 0x40\n#define DINC41 0x41\n#define DINC42 0x42\n#define DINC43 0x43\n#define DINC48 0x48\n#define DINC49 0x49\n#define DINC4A 0x4a\n#define DINC4B 0x4b\n#define DINC50 0x50\n#define DINC51 0x51\n#define DINC52 0x52\n#define DINC53 0x53\n#define DINC58 0x58\n#define DINC59 0x59\n#define DINC5A 0x5a\n#define DINC5B 0x5b\n\n#define DINC60 0x60\n#define DINC61 0x61\n#define DINC62 0x62\n#define DINC63 0x63\n#define DINC68 0x68\n#define DINC69 0x69\n#define DINC6A 0x6a\n#define DINC6B 0x6b\n#define DINC70 0x70\n#define DINC71 0x71\n#define DINC72 0x72\n#define DINC73 0x73\n#define DINC78 0x78\n#define DINC79 0x79\n#define DINC7A 0x7a\n#define DINC7B 0x7b\n\n#define DIND40 0x40\n\n\n#define DINA80 0x80\n#define DINA90 0x90\n#define DINAA0 0xa0\n#define DINAC9 0xc9\n#define DINACB 0xcb\n#define DINACD 0xcd\n#define DINACF 0xcf\n#define DINAC8 0xc8\n#define DINACA 0xca\n#define DINACC 0xcc\n#define DINACE 0xce\n#define DINAD8 0xd8\n#define DINADD 0xdd\n#define DINAF8 0xf0\n#define DINAFE 0xfe\n\n#define DINBF8 0xf8\n#define DINAC0 0xb0\n#define DINAC1 0xb1\n#define DINAC2 0xb4\n#define DINAC3 0xb5\n#define DINAC4 0xb8\n#define DINAC5 0xb9\n#define DINBC0 0xc0\n#define DINBC2 0xc2\n#define DINBC4 0xc4\n#define DINBC6 0xc6\n\n\n\/* ---------------------- dmpmap.h ---------------------- *\/\n\n#ifdef __cplusplus\nextern \"C\"\n{\n#endif\n\n#define DMP_PTAT 0\n#define DMP_XGYR 2\n#define DMP_YGYR 4\n#define DMP_ZGYR 6\n#define DMP_XACC 8\n#define DMP_YACC 10\n#define DMP_ZACC 12\n#define DMP_ADC1 14\n#define DMP_ADC2 16\n#define DMP_ADC3 18\n#define DMP_BIASUNC 20\n#define DMP_FIFORT 22\n#define DMP_INVGSFH 24\n#define DMP_INVGSFL 26\n#define DMP_1H 28\n#define DMP_1L 30\n#define DMP_BLPFSTCH 32\n#define DMP_BLPFSTCL 34\n#define DMP_BLPFSXH 36\n#define DMP_BLPFSXL 38\n#define DMP_BLPFSYH 40\n#define DMP_BLPFSYL 42\n#define DMP_BLPFSZH 44\n#define DMP_BLPFSZL 46\n#define DMP_BLPFMTC 48\n#define DMP_SMC 50\n#define DMP_BLPFMXH 52\n#define DMP_BLPFMXL 54\n#define DMP_BLPFMYH 56\n#define DMP_BLPFMYL 58\n#define DMP_BLPFMZH 60\n#define DMP_BLPFMZL 62\n#define DMP_BLPFC 64\n#define DMP_SMCTH 66\n#define DMP_0H2 68\n#define DMP_0L2 70\n#define DMP_BERR2H 72\n#define DMP_BERR2L 74\n#define DMP_BERR2NH 76\n#define DMP_SMCINC 78\n#define DMP_ANGVBXH 80\n#define DMP_ANGVBXL 82\n#define DMP_ANGVBYH 84\n#define DMP_ANGVBYL 86\n#define DMP_ANGVBZH 88\n#define DMP_ANGVBZL 90\n#define DMP_BERR1H 92\n#define DMP_BERR1L 94\n#define DMP_ATCH 96\n#define DMP_BIASUNCSF 98\n#define DMP_ACT2H 100\n#define DMP_ACT2L 102\n#define DMP_GSFH 104\n#define DMP_GSFL 106\n#define DMP_GH 108\n#define DMP_GL 110\n#define DMP_0_5H 112\n#define DMP_0_5L 114\n#define DMP_0_0H 116\n#define DMP_0_0L 118\n#define DMP_1_0H 120\n#define DMP_1_0L 122\n#define DMP_1_5H 124\n#define DMP_1_5L 126\n#define DMP_TMP1AH 128\n#define DMP_TMP1AL 130\n#define DMP_TMP2AH 132\n#define DMP_TMP2AL 134\n#define DMP_TMP3AH 136\n#define DMP_TMP3AL 138\n#define DMP_TMP4AH 140\n#define DMP_TMP4AL 142\n#define DMP_XACCW 144\n#define DMP_TMP5 146\n#define DMP_XACCB 148\n#define DMP_TMP8 150\n#define DMP_YACCB 152\n#define DMP_TMP9 154\n#define DMP_ZACCB 156\n#define DMP_TMP10 158\n#define DMP_DZH 160\n#define DMP_DZL 162\n#define DMP_XGCH 164\n#define DMP_XGCL 166\n#define DMP_YGCH 168\n#define DMP_YGCL 170\n#define DMP_ZGCH 172\n#define DMP_ZGCL 174\n#define DMP_YACCW 176\n#define DMP_TMP7 178\n#define DMP_AFB1H 180\n#define DMP_AFB1L 182\n#define DMP_AFB2H 184\n#define DMP_AFB2L 186\n#define DMP_MAGFBH 188\n#define DMP_MAGFBL 190\n#define DMP_QT1H 192\n#define DMP_QT1L 194\n#define DMP_QT2H 196\n#define DMP_QT2L 198\n#define DMP_QT3H 200\n#define DMP_QT3L 202\n#define DMP_QT4H 204\n#define DMP_QT4L 206\n#define DMP_CTRL1H 208\n#define DMP_CTRL1L 210\n#define DMP_CTRL2H 212\n#define DMP_CTRL2L 214\n#define DMP_CTRL3H 216\n#define DMP_CTRL3L 218\n#define DMP_CTRL4H 220\n#define DMP_CTRL4L 222\n#define DMP_CTRLS1 224\n#define DMP_CTRLSF1 226\n#define DMP_CTRLS2 228\n#define DMP_CTRLSF2 230\n#define DMP_CTRLS3 232\n#define DMP_CTRLSFNLL 234\n#define DMP_CTRLS4 236\n#define DMP_CTRLSFNL2 238\n#define DMP_CTRLSFNL 240\n#define DMP_TMP30 242\n#define DMP_CTRLSFJT 244\n#define DMP_TMP31 246\n#define DMP_TMP11 248\n#define DMP_CTRLSF2_2 250\n#define DMP_TMP12 252\n#define DMP_CTRLSF1_2 254\n#define DMP_PREVPTAT 256\n#define DMP_ACCZB 258\n#define DMP_ACCXB 264\n#define DMP_ACCYB 266\n#define DMP_1HB 272\n#define DMP_1LB 274\n#define DMP_0H 276\n#define DMP_0L 278\n#define DMP_ASR22H 280\n#define DMP_ASR22L 282\n#define DMP_ASR6H 284\n#define DMP_ASR6L 286\n#define DMP_TMP13 288\n#define DMP_TMP14 290\n#define DMP_FINTXH 292\n#define DMP_FINTXL 294\n#define DMP_FINTYH 296\n#define DMP_FINTYL 298\n#define DMP_FINTZH 300\n#define DMP_FINTZL 302\n#define DMP_TMP1BH 304\n#define DMP_TMP1BL 306\n#define DMP_TMP2BH 308\n#define DMP_TMP2BL 310\n#define DMP_TMP3BH 312\n#define DMP_TMP3BL 314\n#define DMP_TMP4BH 316\n#define DMP_TMP4BL 318\n#define DMP_STXG 320\n#define DMP_ZCTXG 322\n#define DMP_STYG 324\n#define DMP_ZCTYG 326\n#define DMP_STZG 328\n#define DMP_ZCTZG 330\n#define DMP_CTRLSFJT2 332\n#define DMP_CTRLSFJTCNT 334\n#define DMP_PVXG 336\n#define DMP_TMP15 338\n#define DMP_PVYG 340\n#define DMP_TMP16 342\n#define DMP_PVZG 344\n#define DMP_TMP17 346\n#define DMP_MNMFLAGH 352\n#define DMP_MNMFLAGL 354\n#define DMP_MNMTMH 356\n#define DMP_MNMTML 358\n#define DMP_MNMTMTHRH 360\n#define DMP_MNMTMTHRL 362\n#define DMP_MNMTHRH 364\n#define DMP_MNMTHRL 366\n#define DMP_ACCQD4H 368\n#define DMP_ACCQD4L 370\n#define DMP_ACCQD5H 372\n#define DMP_ACCQD5L 374\n#define DMP_ACCQD6H 376\n#define DMP_ACCQD6L 378\n#define DMP_ACCQD7H 380\n#define DMP_ACCQD7L 382\n#define DMP_ACCQD0H 384\n#define DMP_ACCQD0L 386\n#define DMP_ACCQD1H 388\n#define DMP_ACCQD1L 390\n#define DMP_ACCQD2H 392\n#define DMP_ACCQD2L 394\n#define DMP_ACCQD3H 396\n#define DMP_ACCQD3L 398\n#define DMP_XN2H 400\n#define DMP_XN2L 402\n#define DMP_XN1H 404\n#define DMP_XN1L 406\n#define DMP_YN2H 408\n#define DMP_YN2L 410\n#define DMP_YN1H 412\n#define DMP_YN1L 414\n#define DMP_YH 416\n#define DMP_YL 418\n#define DMP_B0H 420\n#define DMP_B0L 422\n#define DMP_A1H 424\n#define DMP_A1L 426\n#define DMP_A2H 428\n#define DMP_A2L 430\n#define DMP_SEM1 432\n#define DMP_FIFOCNT 434\n#define DMP_SH_TH_X 436\n#define DMP_PACKET 438\n#define DMP_SH_TH_Y 440\n#define DMP_FOOTER 442\n#define DMP_SH_TH_Z 444\n#define DMP_TEMP29 448\n#define DMP_TEMP30 450\n#define DMP_XACCB_PRE 452\n#define DMP_XACCB_PREL 454\n#define DMP_YACCB_PRE 456\n#define DMP_YACCB_PREL 458\n#define DMP_ZACCB_PRE 460\n#define DMP_ZACCB_PREL 462\n#define DMP_TMP22 464\n#define DMP_TAP_TIMER 466\n#define DMP_TAP_THX 468\n#define DMP_TAP_THY 472\n#define DMP_TAP_THZ 476\n#define DMP_TAPW_MIN 478\n#define DMP_TMP25 480\n#define DMP_TMP26 482\n#define DMP_TMP27 484\n#define DMP_TMP28 486\n#define DMP_ORIENT 488\n#define DMP_THRSH 490\n#define DMP_ENDIANH 492\n#define DMP_ENDIANL 494\n#define DMP_BLPFNMTCH 496\n#define DMP_BLPFNMTCL 498\n#define DMP_BLPFNMXH 500\n#define DMP_BLPFNMXL 502\n#define DMP_BLPFNMYH 504\n#define DMP_BLPFNMYL 506\n#define DMP_BLPFNMZH 508\n#define DMP_BLPFNMZL 510\n#ifdef __cplusplus\n}\n#endif\n\n\/* ---------------------- inv_mpu.h ---------------------- *\/\n#define INV_X_GYRO (0x40)\n#define INV_Y_GYRO (0x20)\n#define INV_Z_GYRO (0x10)\n#define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)\n#define INV_XYZ_ACCEL (0x08)\n#define INV_XYZ_COMPASS (0x01)\n\n#define MPU_INT_STATUS_DATA_READY (0x0001)\n#define MPU_INT_STATUS_DMP (0x0002)\n#define MPU_INT_STATUS_PLL_READY (0x0004)\n#define MPU_INT_STATUS_I2C_MST (0x0008)\n#define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010)\n#define MPU_INT_STATUS_ZMOT (0x0020)\n#define MPU_INT_STATUS_MOT (0x0040)\n#define MPU_INT_STATUS_FREE_FALL (0x0080)\n#define MPU_INT_STATUS_DMP_0 (0x0100)\n#define MPU_INT_STATUS_DMP_1 (0x0200)\n#define MPU_INT_STATUS_DMP_2 (0x0400)\n#define MPU_INT_STATUS_DMP_3 (0x0800)\n#define MPU_INT_STATUS_DMP_4 (0x1000)\n#define MPU_INT_STATUS_DMP_5 (0x2000)\n\n\/* ---------------------- inv_mpu_dmp_motion_driver.h ---------------------- *\/\n#define TAP_X (0x01)\n#define TAP_Y (0x02)\n#define TAP_Z (0x04)\n#define TAP_XYZ (0x07)\n\n#define DMP_INT_GESTURE (0x01)\n#define DMP_INT_CONTINUOUS (0x02)\n\n#define DMP_FEATURE_TAP (0x001)\n#define DMP_FEATURE_ANDROID_ORIENT (0x002)\n#define DMP_FEATURE_LP_QUAT (0x004)\n#define DMP_FEATURE_PEDOMETER (0x008)\n#define DMP_FEATURE_6X_LP_QUAT (0x010)\n#define DMP_FEATURE_GYRO_CAL (0x020)\n#define DMP_FEATURE_SEND_RAW_ACCEL (0x040)\n#define DMP_FEATURE_SEND_RAW_GYRO (0x080)\n#define DMP_FEATURE_SEND_CAL_GYRO (0x100)\n\n#define INV_WXYZ_QUAT (0x100)\n\n\n\n\n#endif\n<\/code><\/pre>\n","protected":false},"excerpt":{"rendered":"