inv_mpu.cpp

Go to the documentation of this file.

/*
 $License:
    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
    See included License.txt for License information.
 $
 */
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"

/* The following functions must be defined for this platform:
 * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
 *      unsigned char length, unsigned char const *data)
 * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
 *      unsigned char length, unsigned char *data)
 * delay_ms(unsigned long num_ms)
 * get_ms(unsigned long *count)
 * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
 * labs(long x)
 * fabsf(float x)
 * min(int a, int b)
 */

#define i2c_write   !I2Cdev::writeBytes
#define i2c_read    !I2Cdev::readBytes
#define delay_ms    delay

static inline int reg_int_cb(struct int_param_s *int_param)
{
}
#define min(a,b) ((a<b)?a:b)

#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
#error  Which gyro are you using? Define MPUxxxx in your compiler options.
#endif

/* Time for some messy macro work. =]
 * #define MPU9150
 * is equivalent to..
 * #define MPU6050
 * #define AK8975_SECONDARY
 *
 * #define MPU9250
 * is equivalent to..
 * #define MPU6500
 * #define AK8963_SECONDARY
 */
#if defined MPU9150
#ifndef MPU6050
#define MPU6050
#endif                          /* #ifndef MPU6050 */
#if defined AK8963_SECONDARY
#error "MPU9150 and AK8963_SECONDARY cannot both be defined."
#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
#define AK8975_SECONDARY
#endif                          /* #if defined AK8963_SECONDARY */
#elif defined MPU9250           /* #if defined MPU9150 */
#ifndef MPU6500
#define MPU6500
#endif                          /* #ifndef MPU6500 */
#if defined AK8975_SECONDARY
#error "MPU9250 and AK8975_SECONDARY cannot both be defined."
#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
#define AK8963_SECONDARY
#endif                          /* #if defined AK8975_SECONDARY */
#endif                          /* #if defined MPU9150 */

#if defined AK8975_SECONDARY || defined AK8963_SECONDARY
#define AK89xx_SECONDARY
#else
/* #warning "No compass = less profit for Invensense. Lame." */
#endif

static int set_int_enable(unsigned char enable);

/* Hardware registers needed by driver. */
struct gyro_reg_s {
    unsigned char who_am_i;
    unsigned char rate_div;
    unsigned char lpf;
    unsigned char prod_id;
    unsigned char user_ctrl;
    unsigned char fifo_en;
    unsigned char gyro_cfg;
    unsigned char accel_cfg;
    unsigned char accel_cfg2;
    unsigned char lp_accel_odr;
    unsigned char motion_thr;
    unsigned char motion_dur;
    unsigned char fifo_count_h;
    unsigned char fifo_r_w;
    unsigned char raw_gyro;
    unsigned char raw_accel;
    unsigned char temp;
    unsigned char int_enable;
    unsigned char dmp_int_status;
    unsigned char int_status;
    unsigned char accel_intel;
    unsigned char pwr_mgmt_1;
    unsigned char pwr_mgmt_2;
    unsigned char int_pin_cfg;
    unsigned char mem_r_w;
    unsigned char accel_offs;
    unsigned char i2c_mst;
    unsigned char bank_sel;
    unsigned char mem_start_addr;
    unsigned char prgm_start_h;
#if defined AK89xx_SECONDARY
    unsigned char s0_addr;
    unsigned char s0_reg;
    unsigned char s0_ctrl;
    unsigned char s1_addr;
    unsigned char s1_reg;
    unsigned char s1_ctrl;
    unsigned char s4_ctrl;
    unsigned char s0_do;
    unsigned char s1_do;
    unsigned char i2c_delay_ctrl;
    unsigned char raw_compass;
    /* The I2C_MST_VDDIO bit is in this register. */
    unsigned char yg_offs_tc;
#endif
};

/* Information specific to a particular device. */
struct hw_s {
    unsigned char addr;
    unsigned short max_fifo;
    unsigned char num_reg;
    unsigned short temp_sens;
    short temp_offset;
    unsigned short bank_size;
#if defined AK89xx_SECONDARY
    unsigned short compass_fsr;
#endif
};

/* When entering motion interrupt mode, the driver keeps track of the
 * previous state so that it can be restored at a later time.
 * TODO: This is tacky. Fix it.
 */
struct motion_int_cache_s {
    unsigned short gyro_fsr;
    unsigned char accel_fsr;
    unsigned short lpf;
    unsigned short sample_rate;
    unsigned char sensors_on;
    unsigned char fifo_sensors;
    unsigned char dmp_on;
};

/* Cached chip configuration data.
 * TODO: A lot of these can be handled with a bitmask.
 */
struct chip_cfg_s {
    /* Matches gyro_cfg >> 3 & 0x03 */
    unsigned char gyro_fsr;
    /* Matches accel_cfg >> 3 & 0x03 */
    unsigned char accel_fsr;
    /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
    unsigned char sensors;
    /* Matches config register. */
    unsigned char lpf;
    unsigned char clk_src;
    /* Sample rate, NOT rate divider. */
    unsigned short sample_rate;
    /* Matches fifo_en register. */
    unsigned char fifo_enable;
    /* Matches int enable register. */
    unsigned char int_enable;
    /* 1 if devices on auxiliary I2C bus appear on the primary. */
    unsigned char bypass_mode;
    /* 1 if half-sensitivity.
     * NOTE: This doesn't belong here, but everything else in hw_s is const,
     * and this allows us to save some precious RAM.
     */
    unsigned char accel_half;
    /* 1 if device in low-power accel-only mode. */
    unsigned char lp_accel_mode;
    /* 1 if interrupts are only triggered on motion events. */
    unsigned char int_motion_only;
    struct motion_int_cache_s cache;
    /* 1 for active low interrupts. */
    unsigned char active_low_int;
    /* 1 for latched interrupts. */
    unsigned char latched_int;
    /* 1 if DMP is enabled. */
    unsigned char dmp_on;
    /* Ensures that DMP will only be loaded once. */
    unsigned char dmp_loaded;
    /* Sampling rate used when DMP is enabled. */
    unsigned short dmp_sample_rate;
#ifdef AK89xx_SECONDARY
    /* Compass sample rate. */
    unsigned short compass_sample_rate;
    unsigned char compass_addr;
    short mag_sens_adj[3];
#endif
};

/* Information for self-test-> */
struct test_s {
    unsigned long gyro_sens;
    unsigned long accel_sens;
    unsigned char reg_rate_div;
    unsigned char reg_lpf;
    unsigned char reg_gyro_fsr;
    unsigned char reg_accel_fsr;
    unsigned short wait_ms;
    unsigned char packet_thresh;
    float min_dps;
    float max_dps;
    float max_gyro_var;
    float min_g;
    float max_g;
    float max_accel_var;
};

/* Gyro driver state variables. */
struct gyro_state_s {
    const struct gyro_reg_s *reg;
    const struct hw_s *hw;
    struct chip_cfg_s chip_cfg;
    const struct test_s *test;
};

/* Filter configurations. */
enum lpf_e {
    INV_FILTER_256HZ_NOLPF2 = 0,
    INV_FILTER_188HZ,
    INV_FILTER_98HZ,
    INV_FILTER_42HZ,
    INV_FILTER_20HZ,
    INV_FILTER_10HZ,
    INV_FILTER_5HZ,
    INV_FILTER_2100HZ_NOLPF,
    NUM_FILTER
};

/* Full scale ranges. */
enum gyro_fsr_e {
    INV_FSR_250DPS = 0,
    INV_FSR_500DPS,
    INV_FSR_1000DPS,
    INV_FSR_2000DPS,
    NUM_GYRO_FSR
};

/* Full scale ranges. */
enum accel_fsr_e {
    INV_FSR_2G = 0,
    INV_FSR_4G,
    INV_FSR_8G,
    INV_FSR_16G,
    NUM_ACCEL_FSR
};

/* Clock sources. */
enum clock_sel_e {
    INV_CLK_INTERNAL = 0,
    INV_CLK_PLL,
    NUM_CLK
};

/* Low-power accel wakeup rates. */
enum lp_accel_rate_e {
#if defined MPU6050
    INV_LPA_1_25HZ,
    INV_LPA_5HZ,
    INV_LPA_20HZ,
    INV_LPA_40HZ
#elif defined MPU6500
    INV_LPA_0_3125HZ,
    INV_LPA_0_625HZ,
    INV_LPA_1_25HZ,
    INV_LPA_2_5HZ,
    INV_LPA_5HZ,
    INV_LPA_10HZ,
    INV_LPA_20HZ,
    INV_LPA_40HZ,
    INV_LPA_80HZ,
    INV_LPA_160HZ,
    INV_LPA_320HZ,
    INV_LPA_640HZ
#endif
};

#define BIT_I2C_MST_VDDIO   (0x80)
#define BIT_FIFO_EN         (0x40)
#define BIT_DMP_EN          (0x80)
#define BIT_FIFO_RST        (0x04)
#define BIT_DMP_RST         (0x08)
#define BIT_FIFO_OVERFLOW   (0x10)
#define BIT_DATA_RDY_EN     (0x01)
#define BIT_DMP_INT_EN      (0x02)
#define BIT_MOT_INT_EN      (0x40)
#define BITS_FSR            (0x18)
#define BITS_LPF            (0x07)
#define BITS_HPF            (0x07)
#define BITS_CLK            (0x07)
#define BIT_FIFO_SIZE_1024  (0x40)
#define BIT_FIFO_SIZE_2048  (0x80)
#define BIT_FIFO_SIZE_4096  (0xC0)
#define BIT_RESET           (0x80)
#define BIT_SLEEP           (0x40)
#define BIT_S0_DELAY_EN     (0x01)
#define BIT_S2_DELAY_EN     (0x04)
#define BITS_SLAVE_LENGTH   (0x0F)
#define BIT_SLAVE_BYTE_SW   (0x40)
#define BIT_SLAVE_GROUP     (0x10)
#define BIT_SLAVE_EN        (0x80)
#define BIT_I2C_READ        (0x80)
#define BITS_I2C_MASTER_DLY (0x1F)
#define BIT_AUX_IF_EN       (0x20)
#define BIT_ACTL            (0x80)
#define BIT_LATCH_EN        (0x20)
#define BIT_ANY_RD_CLR      (0x10)
#define BIT_BYPASS_EN       (0x02)
#define BITS_WOM_EN         (0xC0)
#define BIT_LPA_CYCLE       (0x20)
#define BIT_STBY_XA         (0x20)
#define BIT_STBY_YA         (0x10)
#define BIT_STBY_ZA         (0x08)
#define BIT_STBY_XG         (0x04)
#define BIT_STBY_YG         (0x02)
#define BIT_STBY_ZG         (0x01)
#define BIT_STBY_XYZA       (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
#define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)

#if defined AK8975_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS   (0x00)
#define AK89xx_FSR                  (9830)
#elif defined AK8963_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS   (0x10)
#define AK89xx_FSR                  (4915)
#endif

#ifdef AK89xx_SECONDARY
#define AKM_REG_WHOAMI      (0x00)

#define AKM_REG_ST1         (0x02)
#define AKM_REG_HXL         (0x03)
#define AKM_REG_ST2         (0x09)

#define AKM_REG_CNTL        (0x0A)
#define AKM_REG_ASTC        (0x0C)
#define AKM_REG_ASAX        (0x10)
#define AKM_REG_ASAY        (0x11)
#define AKM_REG_ASAZ        (0x12)

#define AKM_DATA_READY      (0x01)
#define AKM_DATA_OVERRUN    (0x02)
#define AKM_OVERFLOW        (0x80)
#define AKM_DATA_ERROR      (0x40)

#define AKM_BIT_SELF_TEST   (0x40)

#define AKM_POWER_DOWN          (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_SINGLE_MEASUREMENT  (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_FUSE_ROM_ACCESS     (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_MODE_SELF_TEST      (0x08 | SUPPORTS_AK89xx_HIGH_SENS)

#define AKM_WHOAMI      (0x48)
#endif

struct gyro_reg_s regArray[MPU_MAX_DEVICES];
struct hw_s hwArray[MPU_MAX_DEVICES];
struct test_s testArray[MPU_MAX_DEVICES];
struct gyro_state_s gyroArray[MPU_MAX_DEVICES];

//  These variables are set by the mpu_delect_device function

struct gyro_reg_s *reg;
struct hw_s *hw;
struct test_s *test;
struct gyro_state_s *st;
static int deviceIndex = 0;

int mpu_select_device(int device)
{
  if ((device < 0) || (device >= MPU_MAX_DEVICES))
    return -1;

  deviceIndex = device;
  reg = regArray + device;
  hw = hwArray + device;
  test = testArray + device;
  st = gyroArray + device;
  return 0;
}

void mpu_init_structures()
{
    reg->who_am_i         = 0x75;
    reg->rate_div         = 0x19;
    reg->lpf              = 0x1A;
    reg->prod_id          = 0x0C;
    reg->user_ctrl        = 0x6A;
    reg->fifo_en          = 0x23;
    reg->gyro_cfg         = 0x1B;
    reg->accel_cfg        = 0x1C;
    reg->motion_thr       = 0x1F;
    reg->motion_dur       = 0x20;
    reg->fifo_count_h     = 0x72;
    reg->fifo_r_w         = 0x74;
    reg->raw_gyro         = 0x43;
    reg->raw_accel        = 0x3B;
    reg->temp             = 0x41;
    reg->int_enable       = 0x38;
    reg->dmp_int_status   = 0x39;
    reg->int_status       = 0x3A;
    reg->pwr_mgmt_1       = 0x6B;
    reg->pwr_mgmt_2       = 0x6C;
    reg->int_pin_cfg      = 0x37;
    reg->mem_r_w          = 0x6F;
    reg->accel_offs       = 0x06;
    reg->i2c_mst          = 0x24;
    reg->bank_sel         = 0x6D;
    reg->mem_start_addr   = 0x6E;
    reg->prgm_start_h     = 0x70;
#ifdef AK89xx_SECONDARY
    reg->raw_compass      = 0x49;
    reg->yg_offs_tc       = 0x01;
    reg->s0_addr          = 0x25;
    reg->s0_reg           = 0x26;
    reg->s0_ctrl          = 0x27;
    reg->s1_addr          = 0x28;
    reg->s1_reg           = 0x29;
    reg->s1_ctrl          = 0x2A;
    reg->s4_ctrl          = 0x34;
    reg->s0_do            = 0x63;
    reg->s1_do            = 0x64;
    reg->i2c_delay_ctrl   = 0x67;
#endif
    switch (deviceIndex) {
      case 0:
        hw->addr = 0x68;
        break;

      case 1:
        hw->addr = 0x69;
        break;
    }
    hw->max_fifo          = 1024;
    hw->num_reg           = 118;
    hw->temp_sens         = 340;
    hw->temp_offset       = -521;
    hw->bank_size         = 256;
#if defined AK89xx_SECONDARY
    hw->compass_fsr      = AK89xx_FSR;
#endif

    test->gyro_sens      = 32768/250;
    test->accel_sens     = 32768/16;
    test->reg_rate_div   = 0;    /* 1kHz. */
    test->reg_lpf        = 1;    /* 188Hz. */
    test->reg_gyro_fsr   = 0;    /* 250dps. */
    test->reg_accel_fsr  = 0x18; /* 16g. */
    test->wait_ms        = 50;
    test->packet_thresh  = 5;    /* 5% */
    test->min_dps        = 10.f;
    test->max_dps        = 105.f;
    test->max_gyro_var   = 0.14f;
    test->min_g          = 0.3f;
    test->max_g          = 0.95f;
    test->max_accel_var  = 0.14f;

    st->reg = reg;
    st->hw = hw;
    st->test = test;
};

#define MAX_PACKET_LENGTH (12)

#ifdef AK89xx_SECONDARY
static int setup_compass(void);
#define MAX_COMPASS_SAMPLE_RATE (100)
#endif

static int set_int_enable(unsigned char enable)
{
    unsigned char tmp;

    if (st->chip_cfg.dmp_on) {
        if (enable)
            tmp = BIT_DMP_INT_EN;
        else
            tmp = 0x00;
        if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &tmp))
            return -1;
        st->chip_cfg.int_enable = tmp;
    } else {
        if (!st->chip_cfg.sensors)
            return -1;
        if (enable && st->chip_cfg.int_enable)
            return 0;
        if (enable)
            tmp = BIT_DATA_RDY_EN;
        else
            tmp = 0x00;
        if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &tmp))
            return -1;
        st->chip_cfg.int_enable = tmp;
    }
    return 0;
}

int mpu_reg_dump(void)
{
    unsigned char ii;
    unsigned char data;

    for (ii = 0; ii < st->hw->num_reg; ii++) {
        if (ii == st->reg->fifo_r_w || ii == st->reg->mem_r_w)
            continue;
        if (i2c_read(st->hw->addr, ii, 1, &data))
            return -1;
//      log_i("%#5x: %#5x\r\n", ii, data);
    }
    return 0;
}

int mpu_read_reg(unsigned char reg, unsigned char *data)
{
    if (reg == st->reg->fifo_r_w || reg == st->reg->mem_r_w)
        return -1;
    if (reg >= st->hw->num_reg)
        return -1;
    return i2c_read(st->hw->addr, reg, 1, data);
}

int mpu_init(struct int_param_s *int_param)
{
    unsigned char data[6], rev;
    int errCode;
    
    /* Reset device. */
    data[0] = BIT_RESET;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
        return -1;
    delay_ms(100);

    /* Wake up chip. */
    data[0] = 0x00;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
        return -2;

#if defined MPU6050
    /* Check product revision. */
    if (i2c_read(st->hw->addr, st->reg->accel_offs, 6, data))
        return -3;
    rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) |
        (data[1] & 0x01);

    if (rev) {
        /* Congrats, these parts are better. */
        if (rev == 1)
            st->chip_cfg.accel_half = 1;
        else if (rev == 2)
            st->chip_cfg.accel_half = 0;
        else {
#ifdef MPU_DEBUG
            Serial.print("Unsupported software product rev: "); Serial.println(rev);
#endif
            return -4;
        }
    } else {
        if (i2c_read(st->hw->addr, st->reg->prod_id, 1, data))
            return -5;
        rev = data[0] & 0x0F;
        if (!rev) {
#ifdef MPU_DEBUG
            Serial.println("Product ID read as 0 indicates device is either incompatible or an MPU3050");
#endif
            return -6;
        } else if (rev == 4) {
#ifdef MPU_DEBUG
            Serial.println("Half sensitivity part found.");
#endif
            st->chip_cfg.accel_half = 1;
        } else
            st->chip_cfg.accel_half = 0;
    }
#elif defined MPU6500
#define MPU6500_MEM_REV_ADDR    (0x17)
    if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
        return -7;
    if (rev == 0x1)
        st->chip_cfg.accel_half = 0;
    else {
#ifdef MPU_DEBUG
        Serial.print("Unsupported software product rev: "); Serial.println(rev);
#endif
        return -7;
    }

    /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
     * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
     */
    data[0] = BIT_FIFO_SIZE_1024 | 0x8;
    if (i2c_write(st->hw->addr, st->reg->accel_cfg2, 1, data))
        return -1;
#endif

    /* Set to invalid values to ensure no I2C writes are skipped. */
    st->chip_cfg.sensors = 0xFF;
    st->chip_cfg.gyro_fsr = 0xFF;
    st->chip_cfg.accel_fsr = 0xFF;
    st->chip_cfg.lpf = 0xFF;
    st->chip_cfg.sample_rate = 0xFFFF;
    st->chip_cfg.fifo_enable = 0xFF;
    st->chip_cfg.bypass_mode = 0xFF;
#ifdef AK89xx_SECONDARY
    st->chip_cfg.compass_sample_rate = 0xFFFF;
#endif
    /* mpu_set_sensors always preserves this setting. */
    st->chip_cfg.clk_src = INV_CLK_PLL;
    /* Handled in next call to mpu_set_bypass. */
    st->chip_cfg.active_low_int = 1;
    st->chip_cfg.latched_int = 0;
    st->chip_cfg.int_motion_only = 0;
    st->chip_cfg.lp_accel_mode = 0;
    memset(&st->chip_cfg.cache, 0, sizeof(st->chip_cfg.cache));
    st->chip_cfg.dmp_on = 0;
    st->chip_cfg.dmp_loaded = 0;
    st->chip_cfg.dmp_sample_rate = 0;

    if (mpu_set_gyro_fsr(2000))
        return -1;
    if (mpu_set_accel_fsr(2))
        return -1;
    if (mpu_set_lpf(42))
        return -1;
    if (mpu_set_sample_rate(50))
        return -1;
    if (mpu_configure_fifo(0))
        return -1;

    if (int_param)
        reg_int_cb(int_param);

#ifdef AK89xx_SECONDARY
#ifdef MPU_DEBUG
    Serial.println("Setting up compass");
#endif
    errCode = setup_compass();
    if (errCode != 0) {
#ifdef MPU_DEBUG
       Serial.print("Setup compass failed: "); Serial.println(errCode); 
#endif
    }
    if (mpu_set_compass_sample_rate(10))
        return -1;
#else
    /* Already disabled by setup_compass. */
    if (mpu_set_bypass(0))
        return -1;
#endif

    mpu_set_sensors(0);
    return 0;
}

int mpu_lp_accel_mode(unsigned char rate)
{
    unsigned char tmp[2];

    if (rate > 40)
        return -1;

    if (!rate) {
        mpu_set_int_latched(0);
        tmp[0] = 0;
        tmp[1] = BIT_STBY_XYZG;
        if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, tmp))
            return -1;
        st->chip_cfg.lp_accel_mode = 0;
        return 0;
    }
    /* For LP accel, we automatically configure the hardware to produce latched
     * interrupts. In LP accel mode, the hardware cycles into sleep mode before
     * it gets a chance to deassert the interrupt pin; therefore, we shift this
     * responsibility over to the MCU.
     *
     * Any register read will clear the interrupt.
     */
    mpu_set_int_latched(1);
#if defined MPU6050
    tmp[0] = BIT_LPA_CYCLE;
    if (rate == 1) {
        tmp[1] = INV_LPA_1_25HZ;
        mpu_set_lpf(5);
    } else if (rate <= 5) {
        tmp[1] = INV_LPA_5HZ;
        mpu_set_lpf(5);
    } else if (rate <= 20) {
        tmp[1] = INV_LPA_20HZ;
        mpu_set_lpf(10);
    } else {
        tmp[1] = INV_LPA_40HZ;
        mpu_set_lpf(20);
    }
    tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, tmp))
        return -1;
#elif defined MPU6500
    /* Set wake frequency. */
    if (rate == 1)
        tmp[0] = INV_LPA_1_25HZ;
    else if (rate == 2)
        tmp[0] = INV_LPA_2_5HZ;
    else if (rate <= 5)
        tmp[0] = INV_LPA_5HZ;
    else if (rate <= 10)
        tmp[0] = INV_LPA_10HZ;
    else if (rate <= 20)
        tmp[0] = INV_LPA_20HZ;
    else if (rate <= 40)
        tmp[0] = INV_LPA_40HZ;
    else if (rate <= 80)
        tmp[0] = INV_LPA_80HZ;
    else if (rate <= 160)
        tmp[0] = INV_LPA_160HZ;
    else if (rate <= 320)
        tmp[0] = INV_LPA_320HZ;
    else
        tmp[0] = INV_LPA_640HZ;
    if (i2c_write(st->hw->addr, st->reg->lp_accel_odr, 1, tmp))
        return -1;
    tmp[0] = BIT_LPA_CYCLE;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, tmp))
        return -1;
#endif
    st->chip_cfg.sensors = INV_XYZ_ACCEL;
    st->chip_cfg.clk_src = 0;
    st->chip_cfg.lp_accel_mode = 1;
    mpu_configure_fifo(0);

    return 0;
}

int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
{
    unsigned char tmp[6];

    if (!(st->chip_cfg.sensors & INV_XYZ_GYRO))
        return -1;

    if (i2c_read(st->hw->addr, st->reg->raw_gyro, 6, tmp))
        return -1;
    data[0] = (tmp[0] << 8) | tmp[1];
    data[1] = (tmp[2] << 8) | tmp[3];
    data[2] = (tmp[4] << 8) | tmp[5];
    if (timestamp)
        get_ms(timestamp);
    return 0;
}

int mpu_get_accel_reg(short *data, unsigned long *timestamp)
{
    unsigned char tmp[6];

    if (!(st->chip_cfg.sensors & INV_XYZ_ACCEL))
        return -1;

    if (i2c_read(st->hw->addr, st->reg->raw_accel, 6, tmp))
        return -1;
    data[0] = (tmp[0] << 8) | tmp[1];
    data[1] = (tmp[2] << 8) | tmp[3];
    data[2] = (tmp[4] << 8) | tmp[5];
    if (timestamp)
        get_ms(timestamp);
    return 0;
}

int mpu_get_temperature(long *data, unsigned long *timestamp)
{
    unsigned char tmp[2];
    short raw;

    if (!(st->chip_cfg.sensors))
        return -1;

    if (i2c_read(st->hw->addr, st->reg->temp, 2, tmp))
        return -1;
    raw = (tmp[0] << 8) | tmp[1];
    if (timestamp)
        get_ms(timestamp);

    data[0] = (long)((35 + ((raw - (float)st->hw->temp_offset) / st->hw->temp_sens)) * 65536L);
    return 0;
}

int mpu_set_accel_bias(const long *accel_bias)
{
    unsigned char data[6];
    short accel_hw[3];
    short got_accel[3];
    short fg[3];

    if (!accel_bias)
        return -1;
    if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
        return 0;

    if (i2c_read(st->hw->addr, 3, 3, data))
        return -1;
    fg[0] = ((data[0] >> 4) + 8) & 0xf;
    fg[1] = ((data[1] >> 4) + 8) & 0xf;
    fg[2] = ((data[2] >> 4) + 8) & 0xf;

    accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
    accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
    accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));

    if (i2c_read(st->hw->addr, 0x06, 6, data))
        return -1;

    got_accel[0] = ((short)data[0] << 8) | data[1];
    got_accel[1] = ((short)data[2] << 8) | data[3];
    got_accel[2] = ((short)data[4] << 8) | data[5];

    accel_hw[0] += got_accel[0];
    accel_hw[1] += got_accel[1];
    accel_hw[2] += got_accel[2];

    data[0] = (accel_hw[0] >> 8) & 0xff;
    data[1] = (accel_hw[0]) & 0xff;
    data[2] = (accel_hw[1] >> 8) & 0xff;
    data[3] = (accel_hw[1]) & 0xff;
    data[4] = (accel_hw[2] >> 8) & 0xff;
    data[5] = (accel_hw[2]) & 0xff;

    if (i2c_write(st->hw->addr, 0x06, 6, data))
        return -1;
    return 0;
}

int mpu_reset_fifo(void)
{
    unsigned char data;

    if (!(st->chip_cfg.sensors))
        return -1;

    data = 0;
    if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &data))
        return -1;
    if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, &data))
        return -1;
    if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
        return -1;

    if (st->chip_cfg.dmp_on) {
        data = BIT_FIFO_RST | BIT_DMP_RST;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
            return -1;
        delay_ms(50);
        data = BIT_DMP_EN | BIT_FIFO_EN;
        if (st->chip_cfg.sensors & INV_XYZ_COMPASS)
            data |= BIT_AUX_IF_EN;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
            return -1;
        if (st->chip_cfg.int_enable)
            data = BIT_DMP_INT_EN;
        else
            data = 0;
        if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &data))
            return -1;
        data = 0;
        if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, &data))
            return -1;
    } else {
        data = BIT_FIFO_RST;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
            return -1;
        if (st->chip_cfg.bypass_mode || !(st->chip_cfg.sensors & INV_XYZ_COMPASS))
            data = BIT_FIFO_EN;
        else
            data = BIT_FIFO_EN | BIT_AUX_IF_EN;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
            return -1;
        delay_ms(50);
        if (st->chip_cfg.int_enable)
            data = BIT_DATA_RDY_EN;
        else
            data = 0;
        if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &data))
            return -1;
        if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, &st->chip_cfg.fifo_enable))
            return -1;
    }
    return 0;
}

int mpu_get_gyro_fsr(unsigned short *fsr)
{
    switch (st->chip_cfg.gyro_fsr) {
    case INV_FSR_250DPS:
        fsr[0] = 250;
        break;
    case INV_FSR_500DPS:
        fsr[0] = 500;
        break;
    case INV_FSR_1000DPS:
        fsr[0] = 1000;
        break;
    case INV_FSR_2000DPS:
        fsr[0] = 2000;
        break;
    default:
        fsr[0] = 0;
        break;
    }
    return 0;
}

int mpu_set_gyro_fsr(unsigned short fsr)
{
    unsigned char data;

    if (!(st->chip_cfg.sensors))
        return -1;

    switch (fsr) {
    case 250:
        data = INV_FSR_250DPS << 3;
        break;
    case 500:
        data = INV_FSR_500DPS << 3;
        break;
    case 1000:
        data = INV_FSR_1000DPS << 3;
        break;
    case 2000:
        data = INV_FSR_2000DPS << 3;
        break;
    default:
        return -1;
    }

    if (st->chip_cfg.gyro_fsr == (data >> 3))
        return 0;
    if (i2c_write(st->hw->addr, st->reg->gyro_cfg, 1, &data))
        return -1;
    st->chip_cfg.gyro_fsr = data >> 3;
    return 0;
}

int mpu_get_accel_fsr(unsigned char *fsr)
{
    switch (st->chip_cfg.accel_fsr) {
    case INV_FSR_2G:
        fsr[0] = 2;
        break;
    case INV_FSR_4G:
        fsr[0] = 4;
        break;
    case INV_FSR_8G:
        fsr[0] = 8;
        break;
    case INV_FSR_16G:
        fsr[0] = 16;
        break;
    default:
        return -1;
    }
    if (st->chip_cfg.accel_half)
        fsr[0] <<= 1;
    return 0;
}

int mpu_set_accel_fsr(unsigned char fsr)
{
    unsigned char data;

    if (!(st->chip_cfg.sensors))
        return -1;

    switch (fsr) {
    case 2:
        data = INV_FSR_2G << 3;
        break;
    case 4:
        data = INV_FSR_4G << 3;
        break;
    case 8:
        data = INV_FSR_8G << 3;
        break;
    case 16:
        data = INV_FSR_16G << 3;
        break;
    default:
        return -1;
    }

    if (st->chip_cfg.accel_fsr == (data >> 3))
        return 0;
    if (i2c_write(st->hw->addr, st->reg->accel_cfg, 1, &data))
        return -1;
    st->chip_cfg.accel_fsr = data >> 3;
    return 0;
}

int mpu_get_lpf(unsigned short *lpf)
{
    switch (st->chip_cfg.lpf) {
    case INV_FILTER_188HZ:
        lpf[0] = 188;
        break;
    case INV_FILTER_98HZ:
        lpf[0] = 98;
        break;
    case INV_FILTER_42HZ:
        lpf[0] = 42;
        break;
    case INV_FILTER_20HZ:
        lpf[0] = 20;
        break;
    case INV_FILTER_10HZ:
        lpf[0] = 10;
        break;
    case INV_FILTER_5HZ:
        lpf[0] = 5;
        break;
    case INV_FILTER_256HZ_NOLPF2:
    case INV_FILTER_2100HZ_NOLPF:
    default:
        lpf[0] = 0;
        break;
    }
    return 0;
}

int mpu_set_lpf(unsigned short lpf)
{
    unsigned char data;
    if (!(st->chip_cfg.sensors))
        return -1;

    if (lpf >= 188)
        data = INV_FILTER_188HZ;
    else if (lpf >= 98)
        data = INV_FILTER_98HZ;
    else if (lpf >= 42)
        data = INV_FILTER_42HZ;
    else if (lpf >= 20)
        data = INV_FILTER_20HZ;
    else if (lpf >= 10)
        data = INV_FILTER_10HZ;
    else
        data = INV_FILTER_5HZ;

    if (st->chip_cfg.lpf == data)
        return 0;
    if (i2c_write(st->hw->addr, st->reg->lpf, 1, &data))
        return -1;
    st->chip_cfg.lpf = data;
    return 0;
}

int mpu_get_sample_rate(unsigned short *rate)
{
    if (st->chip_cfg.dmp_on)
        return -1;
    else
        rate[0] = st->chip_cfg.sample_rate;
    return 0;
}

int mpu_set_sample_rate(unsigned short rate)
{
    unsigned char data;

    if (!(st->chip_cfg.sensors))
        return -1;

    if (st->chip_cfg.dmp_on)
        return -1;
    else {
        if (st->chip_cfg.lp_accel_mode) {
            if (rate && (rate <= 40)) {
                /* Just stay in low-power accel mode. */
                mpu_lp_accel_mode(rate);
                return 0;
            }
            /* Requested rate exceeds the allowed frequencies in LP accel mode,
             * switch back to full-power mode.
             */
            mpu_lp_accel_mode(0);
        }
        if (rate < 4)
            rate = 4;
        else if (rate > 1000)
            rate = 1000;

        data = 1000 / rate - 1;
        if (i2c_write(st->hw->addr, st->reg->rate_div, 1, &data))
            return -1;

        st->chip_cfg.sample_rate = 1000 / (1 + data);

#ifdef AK89xx_SECONDARY
        mpu_set_compass_sample_rate(min(st->chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
#endif

        /* Automatically set LPF to 1/2 sampling rate. */
        mpu_set_lpf(st->chip_cfg.sample_rate >> 1);
        return 0;
    }
}

int mpu_get_compass_sample_rate(unsigned short *rate)
{
#ifdef AK89xx_SECONDARY
    rate[0] = st->chip_cfg.compass_sample_rate;
    return 0;
#else
    rate[0] = 0;
    return -1;
#endif
}

int mpu_set_compass_sample_rate(unsigned short rate)
{
#ifdef AK89xx_SECONDARY
    unsigned char div;
    if (!rate || rate > st->chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
        return -1;

    div = st->chip_cfg.sample_rate / rate - 1;
    if (i2c_write(st->hw->addr, st->reg->s4_ctrl, 1, &div))
        return -1;
    st->chip_cfg.compass_sample_rate = st->chip_cfg.sample_rate / (div + 1);
    return 0;
#else
    return -1;
#endif
}

int mpu_get_gyro_sens(float *sens)
{
    switch (st->chip_cfg.gyro_fsr) {
    case INV_FSR_250DPS:
        sens[0] = 131.f;
        break;
    case INV_FSR_500DPS:
        sens[0] = 65.5f;
        break;
    case INV_FSR_1000DPS:
        sens[0] = 32.8f;
        break;
    case INV_FSR_2000DPS:
        sens[0] = 16.4f;
        break;
    default:
        return -1;
    }
    return 0;
}

int mpu_get_accel_sens(unsigned short *sens)
{
    switch (st->chip_cfg.accel_fsr) {
    case INV_FSR_2G:
        sens[0] = 16384;
        break;
    case INV_FSR_4G:
        sens[0] = 8092;
        break;
    case INV_FSR_8G:
        sens[0] = 4096;
        break;
    case INV_FSR_16G:
        sens[0] = 2048;
        break;
    default:
        return -1;
    }
    if (st->chip_cfg.accel_half)
        sens[0] >>= 1;
    return 0;
}

int mpu_get_fifo_config(unsigned char *sensors)
{
    sensors[0] = st->chip_cfg.fifo_enable;
    return 0;
}

int mpu_configure_fifo(unsigned char sensors)
{
    unsigned char prev;
    int result = 0;

    /* Compass data isn't going into the FIFO. Stop trying. */
    sensors &= ~INV_XYZ_COMPASS;

    if (st->chip_cfg.dmp_on)
        return 0;
    else {
        if (!(st->chip_cfg.sensors))
            return -1;
        prev = st->chip_cfg.fifo_enable;
        st->chip_cfg.fifo_enable = sensors & st->chip_cfg.sensors;
        if (st->chip_cfg.fifo_enable != sensors)
            /* You're not getting what you asked for. Some sensors are
             * asleep.
             */
            result = -1;
        else
            result = 0;
        if (sensors || st->chip_cfg.lp_accel_mode)
            set_int_enable(1);
        else
            set_int_enable(0);
        if (sensors) {
            if (mpu_reset_fifo()) {
                st->chip_cfg.fifo_enable = prev;
                return -1;
            }
        }
    }

    return result;
}

int mpu_get_power_state(unsigned char *power_on)
{
    if (st->chip_cfg.sensors)
        power_on[0] = 1;
    else
        power_on[0] = 0;
    return 0;
}

int mpu_set_sensors(unsigned char sensors)
{
    unsigned char data;
#ifdef AK89xx_SECONDARY
    unsigned char user_ctrl;
#endif

    if (sensors & INV_XYZ_GYRO)
        data = INV_CLK_PLL;
    else if (sensors)
        data = 0;
    else
        data = BIT_SLEEP;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, &data)) {
        st->chip_cfg.sensors = 0;
        return -1;
    }
    st->chip_cfg.clk_src = data & ~BIT_SLEEP;

    data = 0;
    if (!(sensors & INV_X_GYRO))
        data |= BIT_STBY_XG;
    if (!(sensors & INV_Y_GYRO))
        data |= BIT_STBY_YG;
    if (!(sensors & INV_Z_GYRO))
        data |= BIT_STBY_ZG;
    if (!(sensors & INV_XYZ_ACCEL))
        data |= BIT_STBY_XYZA;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_2, 1, &data)) {
        st->chip_cfg.sensors = 0;
        return -1;
    }

    if (sensors && (sensors != INV_XYZ_ACCEL))
        /* Latched interrupts only used in LP accel mode. */
        mpu_set_int_latched(0);

#ifdef AK89xx_SECONDARY
#ifdef AK89xx_BYPASS
    if (sensors & INV_XYZ_COMPASS)
        mpu_set_bypass(1);
    else
        mpu_set_bypass(0);
#else
    if (i2c_read(st->hw->addr, st->reg->user_ctrl, 1, &user_ctrl))
        return -1;
    /* Handle AKM power management. */
    if (sensors & INV_XYZ_COMPASS) {
        data = AKM_SINGLE_MEASUREMENT;
        user_ctrl |= BIT_AUX_IF_EN;
    } else {
        data = AKM_POWER_DOWN;
        user_ctrl &= ~BIT_AUX_IF_EN;
    }
    if (st->chip_cfg.dmp_on)
        user_ctrl |= BIT_DMP_EN;
    else
        user_ctrl &= ~BIT_DMP_EN;
    if (i2c_write(st->hw->addr, st->reg->s1_do, 1, &data))
        return -1;
    /* Enable/disable I2C master mode. */
    if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &user_ctrl))
        return -1;
#endif
#endif

    st->chip_cfg.sensors = sensors;
    st->chip_cfg.lp_accel_mode = 0;
    delay_ms(50);
    return 0;
}

int mpu_get_int_status(short *status)
{
    unsigned char tmp[2];
    if (!st->chip_cfg.sensors)
        return -1;
    if (i2c_read(st->hw->addr, st->reg->dmp_int_status, 2, tmp))
        return -1;
    status[0] = (tmp[0] << 8) | tmp[1];
    return 0;
}

int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
        unsigned char *sensors, unsigned char *more)
{
    /* Assumes maximum packet size is gyro (6) + accel (6). */
    unsigned char data[MAX_PACKET_LENGTH];
    unsigned char packet_size = 0;
    unsigned short fifo_count, index = 0;

    if (st->chip_cfg.dmp_on)
        return -1;

    sensors[0] = 0;
    if (!st->chip_cfg.sensors)
        return -2;
    if (!st->chip_cfg.fifo_enable)
        return -3;

    if (st->chip_cfg.fifo_enable & INV_X_GYRO)
        packet_size += 2;
    if (st->chip_cfg.fifo_enable & INV_Y_GYRO)
        packet_size += 2;
    if (st->chip_cfg.fifo_enable & INV_Z_GYRO)
        packet_size += 2;
    if (st->chip_cfg.fifo_enable & INV_XYZ_ACCEL)
        packet_size += 6;

    if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 2, data))
        return -4;
    fifo_count = (data[0] << 8) | data[1];
    if (fifo_count < packet_size)
        return 0;
//    log_i("FIFO count: %hd\n", fifo_count);
    if (fifo_count > (st->hw->max_fifo >> 1)) {
        /* FIFO is 50% full, better check overflow bit. */
        if (i2c_read(st->hw->addr, st->reg->int_status, 1, data))
            return -5;
        if (data[0] & BIT_FIFO_OVERFLOW) {
            mpu_reset_fifo();
            return -6;
        }
    }
    get_ms((unsigned long*)timestamp);

    if (i2c_read(st->hw->addr, st->reg->fifo_r_w, packet_size, data))
        return -7;
    more[0] = fifo_count / packet_size - 1;
    sensors[0] = 0;

    if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
        accel[0] = (data[index+0] << 8) | data[index+1];
        accel[1] = (data[index+2] << 8) | data[index+3];
        accel[2] = (data[index+4] << 8) | data[index+5];
        sensors[0] |= INV_XYZ_ACCEL;
        index += 6;
    }
    if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_X_GYRO) {
        gyro[0] = (data[index+0] << 8) | data[index+1];
        sensors[0] |= INV_X_GYRO;
        index += 2;
    }
    if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_Y_GYRO) {
        gyro[1] = (data[index+0] << 8) | data[index+1];
        sensors[0] |= INV_Y_GYRO;
        index += 2;
    }
    if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_Z_GYRO) {
        gyro[2] = (data[index+0] << 8) | data[index+1];
        sensors[0] |= INV_Z_GYRO;
        index += 2;
    }

    return 0;
}

int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
    unsigned char *more)
{
    unsigned char tmp[2];
    unsigned short fifo_count;
    if (!st->chip_cfg.dmp_on)
        return -1;
    if (!st->chip_cfg.sensors)
        return -2;

    if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 2, tmp))
        return -3;
    fifo_count = (tmp[0] << 8) | tmp[1];
    if (fifo_count < length) {
        more[0] = 0;
        return -4;
    }
    if (fifo_count > (st->hw->max_fifo >> 1)) {
        /* FIFO is 50% full, better check overflow bit. */
        if (i2c_read(st->hw->addr, st->reg->int_status, 1, tmp))
            return -5;
        if (tmp[0] & BIT_FIFO_OVERFLOW) {
            mpu_reset_fifo();
            return -6;
        }
    }

    if (i2c_read(st->hw->addr, st->reg->fifo_r_w, length, data))
        return -7;
    more[0] = fifo_count / length - 1;
    return 0;
}

int mpu_set_bypass(unsigned char bypass_on)
{
    unsigned char tmp;

    if (st->chip_cfg.bypass_mode == bypass_on)
        return 0;

    if (bypass_on) {
        if (i2c_read(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
            return -1;
        tmp &= ~BIT_AUX_IF_EN;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
            return -1;
        delay_ms(3);
        tmp = BIT_BYPASS_EN;
        if (st->chip_cfg.active_low_int)
            tmp |= BIT_ACTL;
        if (st->chip_cfg.latched_int)
            tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
        if (i2c_write(st->hw->addr, st->reg->int_pin_cfg, 1, &tmp))
            return -1;
    } else {
        /* Enable I2C master mode if compass is being used. */
        if (i2c_read(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
            return -1;
        if (st->chip_cfg.sensors & INV_XYZ_COMPASS)
            tmp |= BIT_AUX_IF_EN;
        else
            tmp &= ~BIT_AUX_IF_EN;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
            return -1;
        delay_ms(3);
        if (st->chip_cfg.active_low_int)
            tmp = BIT_ACTL;
        else
            tmp = 0;
        if (st->chip_cfg.latched_int)
            tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
        if (i2c_write(st->hw->addr, st->reg->int_pin_cfg, 1, &tmp))
            return -1;
    }
    st->chip_cfg.bypass_mode = bypass_on;
    return 0;
}

int mpu_set_int_level(unsigned char active_low)
{
    st->chip_cfg.active_low_int = active_low;
    return 0;
}

int mpu_set_int_latched(unsigned char enable)
{
    unsigned char tmp;
    if (st->chip_cfg.latched_int == enable)
        return 0;

    if (enable)
        tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
    else
        tmp = 0;
    if (st->chip_cfg.bypass_mode)
        tmp |= BIT_BYPASS_EN;
    if (st->chip_cfg.active_low_int)
        tmp |= BIT_ACTL;
    if (i2c_write(st->hw->addr, st->reg->int_pin_cfg, 1, &tmp))
        return -1;
    st->chip_cfg.latched_int = enable;
    return 0;
}

#ifdef MPU_MAXIMAL

#ifdef MPU6050
static int get_accel_prod_shift(float *st_shift)
{
    unsigned char tmp[4], shift_code[3], ii;

    if (i2c_read(st->hw->addr, 0x0D, 4, tmp))
        return 0x07;

    shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
    shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
    shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
    for (ii = 0; ii < 3; ii++) {
        if (!shift_code[ii]) {
            st_shift[ii] = 0.f;
            continue;
        }
        /* Equivalent to..
         * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
         */
        st_shift[ii] = 0.34f;
        while (--shift_code[ii])
            st_shift[ii] *= 1.034f;
    }
    return 0;
}

static int accel_self_test(long *bias_regular, long *bias_st)
{
    int jj, result = 0;
    float st_shift[3], st_shift_cust, st_shift_var;

    get_accel_prod_shift(st_shift);
    for(jj = 0; jj < 3; jj++) {
        st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
        if (st_shift[jj]) {
            st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
            if (fabs(st_shift_var) > test->max_accel_var)
                result |= 1 << jj;
        } else if ((st_shift_cust < test->min_g) ||
            (st_shift_cust > test->max_g))
            result |= 1 << jj;
    }

    return result;
}

static int gyro_self_test(long *bias_regular, long *bias_st)
{
    int jj, result = 0;
    unsigned char tmp[3];
    float st_shift, st_shift_cust, st_shift_var;

    if (i2c_read(st->hw->addr, 0x0D, 3, tmp))
        return 0x07;

    tmp[0] &= 0x1F;
    tmp[1] &= 0x1F;
    tmp[2] &= 0x1F;

    for (jj = 0; jj < 3; jj++) {
        st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
        if (tmp[jj]) {
            st_shift = 3275.f / test->gyro_sens;
            while (--tmp[jj])
                st_shift *= 1.046f;
            st_shift_var = st_shift_cust / st_shift - 1.f;
            if (fabs(st_shift_var) > test->max_gyro_var)
                result |= 1 << jj;
        } else if ((st_shift_cust < test->min_dps) ||
            (st_shift_cust > test->max_dps))
            result |= 1 << jj;
    }
    return result;
}

#ifdef AK89xx_SECONDARY
static int compass_self_test(void)
{
    unsigned char tmp[6];
    unsigned char tries = 10;
    int result = 0x07;
    short data;

    mpu_set_bypass(1);

    tmp[0] = AKM_POWER_DOWN;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
        return 0x07;
    tmp[0] = AKM_BIT_SELF_TEST;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
        goto AKM_restore;
    tmp[0] = AKM_MODE_SELF_TEST;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
        goto AKM_restore;

    do {
        delay_ms(10);
        if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
            goto AKM_restore;
        if (tmp[0] & AKM_DATA_READY)
            break;
    } while (tries--);
    if (!(tmp[0] & AKM_DATA_READY))
        goto AKM_restore;

    if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
        goto AKM_restore;

    result = 0;
    data = (short)(tmp[1] << 8) | tmp[0];
    if ((data > 100) || (data < -100))
        result |= 0x01;
    data = (short)(tmp[3] << 8) | tmp[2];
    if ((data > 100) || (data < -100))
        result |= 0x02;
    data = (short)(tmp[5] << 8) | tmp[4];
    if ((data > -300) || (data < -1000))
        result |= 0x04;

AKM_restore:
    tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
    i2c_write(st->chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
    tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
    i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
    mpu_set_bypass(0);
    return result;
}
#endif
#endif

static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
{
    unsigned char data[MAX_PACKET_LENGTH];
    unsigned char packet_count, ii;
    unsigned short fifo_count;

    data[0] = 0x01;
    data[1] = 0;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, data))
        return -1;
    delay_ms(200);
    data[0] = 0;
    if (i2c_write(st->hw->addr, st->reg->int_enable, 1, data))
        return -1;
    if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, data))
        return -1;
    if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
        return -1;
    if (i2c_write(st->hw->addr, st->reg->i2c_mst, 1, data))
        return -1;
    if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, data))
        return -1;
    data[0] = BIT_FIFO_RST | BIT_DMP_RST;
    if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, data))
        return -1;
    delay_ms(15);
    data[0] = st->test->reg_lpf;
    if (i2c_write(st->hw->addr, st->reg->lpf, 1, data))
        return -1;
    data[0] = st->test->reg_rate_div;
    if (i2c_write(st->hw->addr, st->reg->rate_div, 1, data))
        return -1;
    if (hw_test)
        data[0] = st->test->reg_gyro_fsr | 0xE0;
    else
        data[0] = st->test->reg_gyro_fsr;
    if (i2c_write(st->hw->addr, st->reg->gyro_cfg, 1, data))
        return -1;

    if (hw_test)
        data[0] = st->test->reg_accel_fsr | 0xE0;
    else
        data[0] = test->reg_accel_fsr;
    if (i2c_write(st->hw->addr, st->reg->accel_cfg, 1, data))
        return -1;
    if (hw_test)
        delay_ms(200);

    /* Fill FIFO for test->wait_ms milliseconds. */
    data[0] = BIT_FIFO_EN;
    if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, data))
        return -1;

    data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
    if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, data))
        return -1;
    delay_ms(test->wait_ms);
    data[0] = 0;
    if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, data))
        return -1;

    if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 2, data))
        return -1;

    fifo_count = (data[0] << 8) | data[1];
    packet_count = fifo_count / MAX_PACKET_LENGTH;
    gyro[0] = gyro[1] = gyro[2] = 0;
    accel[0] = accel[1] = accel[2] = 0;

    for (ii = 0; ii < packet_count; ii++) {
        short accel_cur[3], gyro_cur[3];
        if (i2c_read(st->hw->addr, st->reg->fifo_r_w, MAX_PACKET_LENGTH, data))
            return -1;
        accel_cur[0] = ((short)data[0] << 8) | data[1];
        accel_cur[1] = ((short)data[2] << 8) | data[3];
        accel_cur[2] = ((short)data[4] << 8) | data[5];
        accel[0] += (long)accel_cur[0];
        accel[1] += (long)accel_cur[1];
        accel[2] += (long)accel_cur[2];
        gyro_cur[0] = (((short)data[6] << 8) | data[7]);
        gyro_cur[1] = (((short)data[8] << 8) | data[9]);
        gyro_cur[2] = (((short)data[10] << 8) | data[11]);
        gyro[0] += (long)gyro_cur[0];
        gyro[1] += (long)gyro_cur[1];
        gyro[2] += (long)gyro_cur[2];
    }
#ifdef EMPL_NO_64BIT
    gyro[0] = (long)(((float)gyro[0]*65536.f) / test->gyro_sens / packet_count);
    gyro[1] = (long)(((float)gyro[1]*65536.f) / test->gyro_sens / packet_count);
    gyro[2] = (long)(((float)gyro[2]*65536.f) / test->gyro_sens / packet_count);
    if (has_accel) {
        accel[0] = (long)(((float)accel[0]*65536.f) / test->accel_sens /
            packet_count);
        accel[1] = (long)(((float)accel[1]*65536.f) / test->accel_sens /
            packet_count);
        accel[2] = (long)(((float)accel[2]*65536.f) / test->accel_sens /
            packet_count);
        /* Don't remove gravity! */
        accel[2] -= 65536L;
    }
#else
    gyro[0] = (long)(((long long)gyro[0]<<16) / test->gyro_sens / packet_count);
    gyro[1] = (long)(((long long)gyro[1]<<16) / test->gyro_sens / packet_count);
    gyro[2] = (long)(((long long)gyro[2]<<16) / test->gyro_sens / packet_count);
    accel[0] = (long)(((long long)accel[0]<<16) / test->accel_sens /
        packet_count);
    accel[1] = (long)(((long long)accel[1]<<16) / test->accel_sens /
        packet_count);
    accel[2] = (long)(((long long)accel[2]<<16) / test->accel_sens /
        packet_count);
    /* Don't remove gravity! */
    if (accel[2] > 0L)
        accel[2] -= 65536L;
    else
        accel[2] += 65536L;
#endif

    return 0;
}

int mpu_run_self_test(long *gyro, long *accel)
{
#ifdef MPU6050
    const unsigned char tries = 2;
    long gyro_st[3], accel_st[3];
    unsigned char accel_result, gyro_result;
#ifdef AK89xx_SECONDARY
    unsigned char compass_result;
#endif
    int ii;
#endif
    int result;
    unsigned char accel_fsr, fifo_sensors, sensors_on;
    unsigned short gyro_fsr, sample_rate, lpf;
    unsigned char dmp_was_on;

    if (st->chip_cfg.dmp_on) {
        mpu_set_dmp_state(0);
        dmp_was_on = 1;
    } else
        dmp_was_on = 0;

    /* Get initial settings. */
    mpu_get_gyro_fsr(&gyro_fsr);
    mpu_get_accel_fsr(&accel_fsr);
    mpu_get_lpf(&lpf);
    mpu_get_sample_rate(&sample_rate);
    sensors_on = st->chip_cfg.sensors;
    mpu_get_fifo_config(&fifo_sensors);

    /* For older chips, the self-test will be different. */
#if defined MPU6050
    for (ii = 0; ii < tries; ii++)
        if (!get_st_biases(gyro, accel, 0))
            break;
    if (ii == tries) {
        /* If we reach this point, we most likely encountered an I2C error.
         * We'll just report an error for all three sensors.
         */
        result = 0;
        goto restore;
    }
    for (ii = 0; ii < tries; ii++)
        if (!get_st_biases(gyro_st, accel_st, 1))
            break;
    if (ii == tries) {
        /* Again, probably an I2C error. */
        result = 0;
        goto restore;
    }
    accel_result = accel_self_test(accel, accel_st);
    gyro_result = gyro_self_test(gyro, gyro_st);

    result = 0;
    if (!gyro_result)
        result |= 0x01;
    if (!accel_result)
        result |= 0x02;

#ifdef AK89xx_SECONDARY
    compass_result = compass_self_test();
    if (!compass_result)
        result |= 0x04;
#endif
restore:
#elif defined MPU6500
    /* For now, this function will return a "pass" result for all three sensors
     * for compatibility with current test applications.
     */
    get_st_biases(gyro, accel, 0);
    result = 0x7;
#endif
    /* Set to invalid values to ensure no I2C writes are skipped. */
    st->chip_cfg.gyro_fsr = 0xFF;
    st->chip_cfg.accel_fsr = 0xFF;
    st->chip_cfg.lpf = 0xFF;
    st->chip_cfg.sample_rate = 0xFFFF;
    st->chip_cfg.sensors = 0xFF;
    st->chip_cfg.fifo_enable = 0xFF;
    st->chip_cfg.clk_src = INV_CLK_PLL;
    mpu_set_gyro_fsr(gyro_fsr);
    mpu_set_accel_fsr(accel_fsr);
    mpu_set_lpf(lpf);
    mpu_set_sample_rate(sample_rate);
    mpu_set_sensors(sensors_on);
    mpu_configure_fifo(fifo_sensors);

    if (dmp_was_on)
        mpu_set_dmp_state(1);

    return result;
}
#endif // MPU_MAXIMAL

int mpu_write_mem(unsigned short mem_addr, unsigned short length,
        unsigned char *data)
{
    unsigned char tmp[2];

    if (!data)
        return -1;
    if (!st->chip_cfg.sensors)
        return -2;

    tmp[0] = (unsigned char)(mem_addr >> 8);
    tmp[1] = (unsigned char)(mem_addr & 0xFF);

    /* Check bank boundaries. */
    if (tmp[1] + length > st->hw->bank_size)
        return -3;

    if (i2c_write(st->hw->addr, st->reg->bank_sel, 2, tmp))
        return -4;
    if (i2c_write(st->hw->addr, st->reg->mem_r_w, length, data))
        return -5;
    return 0;
}

int mpu_read_mem(unsigned short mem_addr, unsigned short length,
        unsigned char *data)
{
    unsigned char tmp[2];

    if (!data)
        return -1;
    if (!st->chip_cfg.sensors)
        return -1;

    tmp[0] = (unsigned char)(mem_addr >> 8);
    tmp[1] = (unsigned char)(mem_addr & 0xFF);

    /* Check bank boundaries. */
    if (tmp[1] + length > st->hw->bank_size)
        return -1;

    if (i2c_write(st->hw->addr, st->reg->bank_sel, 2, tmp))
        return -1;
    if (i2c_read(st->hw->addr, st->reg->mem_r_w, length, data))
        return -1;
    return 0;
}

int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
    unsigned short start_addr, unsigned short sample_rate)
{
    unsigned short ii;
    unsigned short this_write;
    int errCode;
    uint8_t *progBuffer;
    /* Must divide evenly into st->hw->bank_size to avoid bank crossings. */
#define LOAD_CHUNK  (16)
    unsigned char cur[LOAD_CHUNK], tmp[2];

    if (st->chip_cfg.dmp_loaded)
        /* DMP should only be loaded once. */
        return -1;

    if (!firmware)
        return -2;
        
    progBuffer = (uint8_t *)malloc(LOAD_CHUNK);
    for (ii = 0; ii < length; ii += this_write) {
        this_write = min(LOAD_CHUNK, length - ii);
        
        for (int progIndex = 0; progIndex < this_write; progIndex++)
#ifdef __SAM3X8E__
            progBuffer[progIndex] = firmware[ii + progIndex];
#else
            progBuffer[progIndex] = pgm_read_byte(firmware + ii + progIndex);
#endif
            
        if ((errCode = mpu_write_mem(ii, this_write, progBuffer))) {
#ifdef MPU_DEBUG
            Serial.print("fimrware write failed: ");
            Serial.println(errCode);
#endif
            return -3;
        }
        
        if (mpu_read_mem(ii, this_write, cur))
            return -4;
            
        if (memcmp(progBuffer, cur, this_write)) {
#ifdef MPU_DEBUG
            Serial.print("Firmware compare failed addr "); Serial.println(ii);
            for (int i = 0; i < 10; i++) {
              Serial.print(progBuffer[i]); 
              Serial.print(" ");
            }
            Serial.println();
            for (int i = 0; i < 10; i++) {
              Serial.print(cur[i]); 
              Serial.print(" ");
            }
            Serial.println();
#endif
            return -5;
        }
    }

    /* Set program start address. */
    tmp[0] = start_addr >> 8;
    tmp[1] = start_addr & 0xFF;
    if (i2c_write(st->hw->addr, st->reg->prgm_start_h, 2, tmp))
        return -6;

    st->chip_cfg.dmp_loaded = 1;
    st->chip_cfg.dmp_sample_rate = sample_rate;
#ifdef MPU_DEBUG
    Serial.println("Firmware loaded");
#endif
    return 0;
}

int mpu_set_dmp_state(unsigned char enable)
{
    unsigned char tmp;
    if (st->chip_cfg.dmp_on == enable)
        return 0;

    if (enable) {
        if (!st->chip_cfg.dmp_loaded)
            return -1;
        /* Disable data ready interrupt. */
        set_int_enable(0);
        /* Disable bypass mode. */
        mpu_set_bypass(0);
        /* Keep constant sample rate, FIFO rate controlled by DMP. */
        mpu_set_sample_rate(st->chip_cfg.dmp_sample_rate);
        /* Remove FIFO elements. */
        tmp = 0;
        i2c_write(st->hw->addr, 0x23, 1, &tmp);
        st->chip_cfg.dmp_on = 1;
        /* Enable DMP interrupt. */
        set_int_enable(1);
        mpu_reset_fifo();
    } else {
        /* Disable DMP interrupt. */
        set_int_enable(0);
        /* Restore FIFO settings. */
        tmp = st->chip_cfg.fifo_enable;
        i2c_write(st->hw->addr, 0x23, 1, &tmp);
        st->chip_cfg.dmp_on = 0;
        mpu_reset_fifo();
    }
    return 0;
}

int mpu_get_dmp_state(unsigned char *enabled)
{
    enabled[0] = st->chip_cfg.dmp_on;
    return 0;
}


/* This initialization is similar to the one in ak8975.c. */
static int setup_compass(void)
{
#ifdef AK89xx_SECONDARY
    unsigned char data[4], akm_addr;

    mpu_set_bypass(1);

    /* Find compass. Possible addresses range from 0x0C to 0x0F. */
    for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
        int result;
        result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
        if (!result && (data[0] == AKM_WHOAMI))
            break;
    }

    if (akm_addr > 0x0F) {
        /* TODO: Handle this case in all compass-related functions. */
#ifdef MPU_DEBUG
        Serial.println("Compass not found.");
#endif
        return -1;
    }

    st->chip_cfg.compass_addr = akm_addr;

    data[0] = AKM_POWER_DOWN;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
        return -2;
    delay_ms(1);

    data[0] = AKM_FUSE_ROM_ACCESS;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
        return -3;
    delay_ms(1);

    /* Get sensitivity adjustment data from fuse ROM. */
    if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
        return -4;
    st->chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
    st->chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
    st->chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
#ifdef MPU_DEBUG
    Serial.print("Compass sens: "); Serial.print(st->chip_cfg.mag_sens_adj[0]); Serial.print(" ");
    Serial.print(st->chip_cfg.mag_sens_adj[1]); Serial.print(" ");
    Serial.print(st->chip_cfg.mag_sens_adj[2]); Serial.println();
#endif

    data[0] = AKM_POWER_DOWN;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
        return -5;
    delay_ms(1);

    mpu_set_bypass(0);

    /* Set up master mode, master clock, and ES bit. */
    data[0] = 0x40;
    if (i2c_write(st->hw->addr, st->reg->i2c_mst, 1, data))
        return -6;

    /* Slave 0 reads from AKM data registers. */
    data[0] = BIT_I2C_READ | st->chip_cfg.compass_addr;
    if (i2c_write(st->hw->addr, st->reg->s0_addr, 1, data))
        return -7;

    /* Compass reads start at this register. */
    data[0] = AKM_REG_ST1;
    if (i2c_write(st->hw->addr, st->reg->s0_reg, 1, data))
        return -8;

    /* Enable slave 0, 8-byte reads. */
    data[0] = BIT_SLAVE_EN | 8;
    if (i2c_write(st->hw->addr, st->reg->s0_ctrl, 1, data))
        return -9;

    /* Slave 1 changes AKM measurement mode. */
    data[0] = st->chip_cfg.compass_addr;
    if (i2c_write(st->hw->addr, st->reg->s1_addr, 1, data))
        return -10;

    /* AKM measurement mode register. */
    data[0] = AKM_REG_CNTL;
    if (i2c_write(st->hw->addr, st->reg->s1_reg, 1, data))
        return -11;

    /* Enable slave 1, 1-byte writes. */
    data[0] = BIT_SLAVE_EN | 1;
    if (i2c_write(st->hw->addr, st->reg->s1_ctrl, 1, data))
        return -12;

    /* Set slave 1 data. */
    data[0] = AKM_SINGLE_MEASUREMENT;
    if (i2c_write(st->hw->addr, st->reg->s1_do, 1, data))
        return -13;

    /* Trigger slave 0 and slave 1 actions at each sample. */
    data[0] = 0x03;
    if (i2c_write(st->hw->addr, st->reg->i2c_delay_ctrl, 1, data))
        return -14;

#ifdef MPU9150
    /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
    data[0] = BIT_I2C_MST_VDDIO;
    if (i2c_write(st->hw->addr, st->reg->yg_offs_tc, 1, data))
        return -15;
#endif

    return 0;
#else
    return -16;
#endif
}

int mpu_get_compass_reg(short *data, unsigned long *timestamp)
{
#ifdef AK89xx_SECONDARY
    unsigned char tmp[9];

    if (!(st->chip_cfg.sensors & INV_XYZ_COMPASS))
        return -1;

#ifdef AK89xx_BYPASS
    if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
        return -2;
    tmp[8] = AKM_SINGLE_MEASUREMENT;
    if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
        return -3;
#else
    if (i2c_read(st->hw->addr, st->reg->raw_compass, 8, tmp))
        return -4;
#endif

#if defined AK8975_SECONDARY
    /* AK8975 doesn't have the overrun error bit. */
    if (!(tmp[0] & AKM_DATA_READY))
        return -5;
    if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
        return -6;
#elif defined AK8963_SECONDARY
    /* AK8963 doesn't have the data read error bit. */
    if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
        return -7;
    if (tmp[7] & AKM_OVERFLOW)
        return -8;
#endif
    data[0] = ((unsigned short)tmp[2] << 8) | (unsigned short)tmp[1];
    data[1] = ((unsigned short)tmp[4] << 8) | (unsigned short)tmp[3];
    data[2] = ((unsigned short)tmp[6] << 8) | (unsigned short)tmp[5];

    data[0] = ((long)data[0] * st->chip_cfg.mag_sens_adj[0]) >> 8;
    data[1] = ((long)data[1] * st->chip_cfg.mag_sens_adj[1]) >> 8;
    data[2] = ((long)data[2] * st->chip_cfg.mag_sens_adj[2]) >> 8;

    if (timestamp)
        get_ms(timestamp);
    return 0;
#else
    return -9;
#endif
}

int mpu_get_compass_fsr(unsigned short *fsr)
{
#ifdef AK89xx_SECONDARY
    fsr[0] = st->hw->compass_fsr;
    return 0;
#else
    return -1;
#endif
}

int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
    unsigned char lpa_freq)
{
    unsigned char data[3];

    if (lpa_freq) {
        unsigned char thresh_hw;

#if defined MPU6050
        /* TODO: Make these const/#defines. */
        /* 1LSb = 32mg. */
        if (thresh > 8160)
            thresh_hw = 255;
        else if (thresh < 32)
            thresh_hw = 1;
        else
            thresh_hw = thresh >> 5;
#elif defined MPU6500
        /* 1LSb = 4mg. */
        if (thresh > 1020)
            thresh_hw = 255;
        else if (thresh < 4)
            thresh_hw = 1;
        else
            thresh_hw = thresh >> 2;
#endif

        if (!time)
            /* Minimum duration must be 1ms. */
            time = 1;

#if defined MPU6050
        if (lpa_freq > 40)
#elif defined MPU6500
        if (lpa_freq > 640)
#endif
            /* At this point, the chip has not been re-configured, so the
             * function can safely exit.
             */
            return -1;

        if (!st->chip_cfg.int_motion_only) {
            /* Store current settings for later. */
            if (st->chip_cfg.dmp_on) {
                mpu_set_dmp_state(0);
                st->chip_cfg.cache.dmp_on = 1;
            } else
                st->chip_cfg.cache.dmp_on = 0;
            mpu_get_gyro_fsr(&st->chip_cfg.cache.gyro_fsr);
            mpu_get_accel_fsr(&st->chip_cfg.cache.accel_fsr);
            mpu_get_lpf(&st->chip_cfg.cache.lpf);
            mpu_get_sample_rate(&st->chip_cfg.cache.sample_rate);
            st->chip_cfg.cache.sensors_on = st->chip_cfg.sensors;
            mpu_get_fifo_config(&st->chip_cfg.cache.fifo_sensors);
        }

#ifdef MPU6050
        /* Disable hardware interrupts for now. */
        set_int_enable(0);

        /* Enter full-power accel-only mode. */
        mpu_lp_accel_mode(0);

        /* Override current LPF (and HPF) settings to obtain a valid accel
         * reading.
         */
        data[0] = INV_FILTER_256HZ_NOLPF2;
        if (i2c_write(st->hw->addr, st->reg->lpf, 1, data))
            return -1;

        /* NOTE: Digital high pass filter should be configured here. Since this
         * driver doesn't modify those bits anywhere, they should already be
         * cleared by default.
         */

        /* Configure the device to send motion interrupts. */
        /* Enable motion interrupt. */
        data[0] = BIT_MOT_INT_EN;
        if (i2c_write(st->hw->addr, st->reg->int_enable, 1, data))
            goto lp_int_restore;

        /* Set motion interrupt parameters. */
        data[0] = thresh_hw;
        data[1] = time;
        if (i2c_write(st->hw->addr, st->reg->motion_thr, 2, data))
            goto lp_int_restore;

        /* Force hardware to "lock" current accel sample. */
        delay_ms(5);
        data[0] = (st->chip_cfg.accel_fsr << 3) | BITS_HPF;
        if (i2c_write(st->hw->addr, st->reg->accel_cfg, 1, data))
            goto lp_int_restore;

        /* Set up LP accel mode. */
        data[0] = BIT_LPA_CYCLE;
        if (lpa_freq == 1)
            data[1] = INV_LPA_1_25HZ;
        else if (lpa_freq <= 5)
            data[1] = INV_LPA_5HZ;
        else if (lpa_freq <= 20)
            data[1] = INV_LPA_20HZ;
        else
            data[1] = INV_LPA_40HZ;
        data[1] = (data[1] << 6) | BIT_STBY_XYZG;
        if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, data))
            goto lp_int_restore;

        st->chip_cfg.int_motion_only = 1;
        return 0;
#elif defined MPU6500
        /* Disable hardware interrupts. */
        set_int_enable(0);

        /* Enter full-power accel-only mode, no FIFO/DMP. */
        data[0] = 0;
        data[1] = 0;
        data[2] = BIT_STBY_XYZG;
        if (i2c_write(st->hw->addr, st->reg->user_ctrl, 3, data))
            goto lp_int_restore;

        /* Set motion threshold. */
        data[0] = thresh_hw;
        if (i2c_write(st->hw->addr, st->reg->motion_thr, 1, data))
            goto lp_int_restore;

        /* Set wake frequency. */
        if (lpa_freq == 1)
            data[0] = INV_LPA_1_25HZ;
        else if (lpa_freq == 2)
            data[0] = INV_LPA_2_5HZ;
        else if (lpa_freq <= 5)
            data[0] = INV_LPA_5HZ;
        else if (lpa_freq <= 10)
            data[0] = INV_LPA_10HZ;
        else if (lpa_freq <= 20)
            data[0] = INV_LPA_20HZ;
        else if (lpa_freq <= 40)
            data[0] = INV_LPA_40HZ;
        else if (lpa_freq <= 80)
            data[0] = INV_LPA_80HZ;
        else if (lpa_freq <= 160)
            data[0] = INV_LPA_160HZ;
        else if (lpa_freq <= 320)
            data[0] = INV_LPA_320HZ;
        else
            data[0] = INV_LPA_640HZ;
        if (i2c_write(st->hw->addr, st->reg->lp_accel_odr, 1, data))
            goto lp_int_restore;

        /* Enable motion interrupt (MPU6500 version). */
        data[0] = BITS_WOM_EN;
        if (i2c_write(st->hw->addr, st->reg->accel_intel, 1, data))
            goto lp_int_restore;

        /* Enable cycle mode. */
        data[0] = BIT_LPA_CYCLE;
        if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
            goto lp_int_restore;

        /* Enable interrupt. */
        data[0] = BIT_MOT_INT_EN;
        if (i2c_write(st->hw->addr, st->reg->int_enable, 1, data))
            goto lp_int_restore;

        st->chip_cfg.int_motion_only = 1;
        return 0;
#endif
    } else {
        /* Don't "restore" the previous state if no state has been saved. */
        int ii;
        char *cache_ptr = (char*)&st->chip_cfg.cache;
        for (ii = 0; ii < sizeof(st->chip_cfg.cache); ii++) {
            if (cache_ptr[ii] != 0)
                goto lp_int_restore;
        }
        /* If we reach this point, motion interrupt mode hasn't been used yet. */
        return -1;
    }
lp_int_restore:
    /* Set to invalid values to ensure no I2C writes are skipped. */
    st->chip_cfg.gyro_fsr = 0xFF;
    st->chip_cfg.accel_fsr = 0xFF;
    st->chip_cfg.lpf = 0xFF;
    st->chip_cfg.sample_rate = 0xFFFF;
    st->chip_cfg.sensors = 0xFF;
    st->chip_cfg.fifo_enable = 0xFF;
    st->chip_cfg.clk_src = INV_CLK_PLL;
    mpu_set_sensors(st->chip_cfg.cache.sensors_on);
    mpu_set_gyro_fsr(st->chip_cfg.cache.gyro_fsr);
    mpu_set_accel_fsr(st->chip_cfg.cache.accel_fsr);
    mpu_set_lpf(st->chip_cfg.cache.lpf);
    mpu_set_sample_rate(st->chip_cfg.cache.sample_rate);
    mpu_configure_fifo(st->chip_cfg.cache.fifo_sensors);

    if (st->chip_cfg.cache.dmp_on)
        mpu_set_dmp_state(1);

#ifdef MPU6500
    /* Disable motion interrupt (MPU6500 version). */
    data[0] = 0;
    if (i2c_write(st->hw->addr, st->reg->accel_intel, 1, data))
        goto lp_int_restore;
#endif

    st->chip_cfg.int_motion_only = 0;
    return 0;
}