inv_mpu.c

00001 /*
00002  $License:
00003     Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
00004     See included License.txt for License information.
00005  $
00006  */
00007 /**
00008  *  @addtogroup  DRIVERS Sensor Driver Layer
00009  *  @brief       Hardware drivers to communicate with sensors via I2C.
00010  *
00011  *  @{
00012  *      @file       inv_mpu.c
00013  *      @brief      An I2C-based driver for Invensense gyroscopes.
00014  *      @details    This driver currently works for the following devices:
00015  *                  MPU6050
00016  *                  MPU6500
00017  *                  MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)
00018  *                  MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)
00019  */
00020 #include <stdio.h>
00021 #include <stdint.h>
00022 #include <stdlib.h>
00023 #include <string.h>
00024 #include <math.h>
00025 #include "inv_mpu.h"
00026 
00027 /* The following functions must be defined for this platform:
00028  * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
00029  *      unsigned char length, unsigned char const *data)
00030  * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
00031  *      unsigned char length, unsigned char *data)
00032  * delay_ms(unsigned long num_ms)
00033  * get_ms(unsigned long *count)
00034  * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
00035  * labs(long x)
00036  * fabsf(float x)
00037  * min(int a, int b)
00038  */
00039 #if defined __MBED__            // for mbed platform
00040 #include "mbed_i2c.h"
00041 #include "wait_api.h"
00042 #include "us_ticker_api.h"
00043 
00044 #define MPU6050
00045 
00046 #define i2c_write(a, b, c, d)   mbed_i2c_write(a, b, c, d)
00047 #define i2c_read(a, b, c, d)    mbed_i2c_read(a, b, c, d)
00048 #define delay_ms    wait_ms
00049 #define log_i(...)
00050 #define log_e(...)
00051 #define labs        abs
00052 #define fabs(x)     (((x)>0)?(x):-(x))
00053 #define min(x, y)   (((x) < (y)) ? (x) : (y))
00054 
00055 static inline unsigned long get_ms(unsigned long *t)
00056 {
00057     unsigned long ms = us_ticker_read() / 1000;
00058     *t = ms;
00059     return ms;
00060 }
00061 static inline int reg_int_cb(struct int_param_s *int_param)
00062 {
00063     return 0;
00064 }
00065 
00066 #elif defined MOTION_DRIVER_TARGET_MSP430
00067 #include "msp430.h"
00068 #include "msp430_i2c.h"
00069 #include "msp430_clock.h"
00070 #include "msp430_interrupt.h"
00071 #define i2c_write   msp430_i2c_write
00072 #define i2c_read    msp430_i2c_read
00073 #define delay_ms    msp430_delay_ms
00074 #define get_ms      msp430_get_clock_ms
00075 static inline int reg_int_cb(struct int_param_s *int_param)
00076 {
00077     return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
00078         int_param->active_low);
00079 }
00080 #define log_i(...)     do {} while (0)
00081 #define log_e(...)     do {} while (0)
00082 /* labs is already defined by TI's toolchain. */
00083 /* fabs is for doubles. fabsf is for floats. */
00084 #define fabs        fabsf
00085 #define min(a,b) ((a<b)?a:b)
00086 #elif defined EMPL_TARGET_MSP430
00087 #include "msp430.h"
00088 #include "msp430_i2c.h"
00089 #include "msp430_clock.h"
00090 #include "msp430_interrupt.h"
00091 #include "log.h"
00092 #define i2c_write   msp430_i2c_write
00093 #define i2c_read    msp430_i2c_read
00094 #define delay_ms    msp430_delay_ms
00095 #define get_ms      msp430_get_clock_ms
00096 static inline int reg_int_cb(struct int_param_s *int_param)
00097 {
00098     return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
00099         int_param->active_low);
00100 }
00101 #define log_i       MPL_LOGI
00102 #define log_e       MPL_LOGE
00103 /* labs is already defined by TI's toolchain. */
00104 /* fabs is for doubles. fabsf is for floats. */
00105 #define fabs        fabsf
00106 #define min(a,b) ((a<b)?a:b)
00107 #elif defined EMPL_TARGET_UC3L0
00108 /* Instead of using the standard TWI driver from the ASF library, we're using
00109  * a TWI driver that follows the slave address + register address convention.
00110  */
00111 #include "twi.h"
00112 #include "delay.h"
00113 #include "sysclk.h"
00114 #include "log.h"
00115 #include "sensors_xplained.h"
00116 #include "uc3l0_clock.h"
00117 #define i2c_write(a, b, c, d)   twi_write(a, b, d, c)
00118 #define i2c_read(a, b, c, d)    twi_read(a, b, d, c)
00119 /* delay_ms is a function already defined in ASF. */
00120 #define get_ms  uc3l0_get_clock_ms
00121 static inline int reg_int_cb(struct int_param_s *int_param)
00122 {
00123     sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
00124     return 0;
00125 }
00126 #define log_i       MPL_LOGI
00127 #define log_e       MPL_LOGE
00128 /* UC3 is a 32-bit processor, so abs and labs are equivalent. */
00129 #define labs        abs
00130 #define fabs(x)     (((x)>0)?(x):-(x))
00131 
00132 #else
00133 #error  Gyro driver is missing the system layer implementations.
00134 #endif
00135 
00136 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
00137 #error  Which gyro are you using? Define MPUxxxx in your compiler options.
00138 #endif
00139 
00140 /* Time for some messy macro work. =]
00141  * #define MPU9150
00142  * is equivalent to..
00143  * #define MPU6050
00144  * #define AK8975_SECONDARY
00145  *
00146  * #define MPU9250
00147  * is equivalent to..
00148  * #define MPU6500
00149  * #define AK8963_SECONDARY
00150  */
00151 #if defined MPU9150
00152 #ifndef MPU6050
00153 #define MPU6050
00154 #endif                          /* #ifndef MPU6050 */
00155 #if defined AK8963_SECONDARY
00156 #error "MPU9150 and AK8963_SECONDARY cannot both be defined."
00157 #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
00158 #define AK8975_SECONDARY
00159 #endif                          /* #if defined AK8963_SECONDARY */
00160 #elif defined MPU9250           /* #if defined MPU9150 */
00161 #ifndef MPU6500
00162 #define MPU6500
00163 #endif                          /* #ifndef MPU6500 */
00164 #if defined AK8975_SECONDARY
00165 #error "MPU9250 and AK8975_SECONDARY cannot both be defined."
00166 #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
00167 #define AK8963_SECONDARY
00168 #endif                          /* #if defined AK8975_SECONDARY */
00169 #endif                          /* #if defined MPU9150 */
00170 
00171 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY
00172 #define AK89xx_SECONDARY
00173 #else
00174 /* #warning "No compass = less profit for Invensense. Lame." */
00175 #endif
00176 
00177 static int set_int_enable(unsigned char enable);
00178 
00179 /* Hardware registers needed by driver. */
00180 struct gyro_reg_s {
00181     unsigned char who_am_i;
00182     unsigned char rate_div;
00183     unsigned char lpf;
00184     unsigned char prod_id;
00185     unsigned char user_ctrl;
00186     unsigned char fifo_en;
00187     unsigned char gyro_cfg;
00188     unsigned char accel_cfg;
00189     unsigned char accel_cfg2;
00190     unsigned char lp_accel_odr;
00191     unsigned char motion_thr;
00192     unsigned char motion_dur;
00193     unsigned char fifo_count_h;
00194     unsigned char fifo_r_w;
00195     unsigned char raw_gyro;
00196     unsigned char raw_accel;
00197     unsigned char temp;
00198     unsigned char int_enable;
00199     unsigned char dmp_int_status;
00200     unsigned char int_status;
00201     unsigned char accel_intel;
00202     unsigned char pwr_mgmt_1;
00203     unsigned char pwr_mgmt_2;
00204     unsigned char int_pin_cfg;
00205     unsigned char mem_r_w;
00206     unsigned char accel_offs;
00207     unsigned char i2c_mst;
00208     unsigned char bank_sel;
00209     unsigned char mem_start_addr;
00210     unsigned char prgm_start_h;
00211 #if defined AK89xx_SECONDARY
00212     unsigned char s0_addr;
00213     unsigned char s0_reg;
00214     unsigned char s0_ctrl;
00215     unsigned char s1_addr;
00216     unsigned char s1_reg;
00217     unsigned char s1_ctrl;
00218     unsigned char s4_ctrl;
00219     unsigned char s0_do;
00220     unsigned char s1_do;
00221     unsigned char i2c_delay_ctrl;
00222     unsigned char raw_compass;
00223     /* The I2C_MST_VDDIO bit is in this register. */
00224     unsigned char yg_offs_tc;
00225 #endif
00226 };
00227 
00228 /* Information specific to a particular device. */
00229 struct hw_s {
00230     unsigned char addr;
00231     unsigned short max_fifo;
00232     unsigned char num_reg;
00233     unsigned short temp_sens;
00234     short temp_offset;
00235     unsigned short bank_size;
00236 #if defined AK89xx_SECONDARY
00237     unsigned short compass_fsr;
00238 #endif
00239 };
00240 
00241 /* When entering motion interrupt mode, the driver keeps track of the
00242  * previous state so that it can be restored at a later time.
00243  * TODO: This is tacky. Fix it.
00244  */
00245 struct motion_int_cache_s {
00246     unsigned short gyro_fsr;
00247     unsigned char accel_fsr;
00248     unsigned short lpf;
00249     unsigned short sample_rate;
00250     unsigned char sensors_on;
00251     unsigned char fifo_sensors;
00252     unsigned char dmp_on;
00253 };
00254 
00255 /* Cached chip configuration data.
00256  * TODO: A lot of these can be handled with a bitmask.
00257  */
00258 struct chip_cfg_s {
00259     /* Matches gyro_cfg >> 3 & 0x03 */
00260     unsigned char gyro_fsr;
00261     /* Matches accel_cfg >> 3 & 0x03 */
00262     unsigned char accel_fsr;
00263     /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
00264     unsigned char sensors;
00265     /* Matches config register. */
00266     unsigned char lpf;
00267     unsigned char clk_src;
00268     /* Sample rate, NOT rate divider. */
00269     unsigned short sample_rate;
00270     /* Matches fifo_en register. */
00271     unsigned char fifo_enable;
00272     /* Matches int enable register. */
00273     unsigned char int_enable;
00274     /* 1 if devices on auxiliary I2C bus appear on the primary. */
00275     unsigned char bypass_mode;
00276     /* 1 if half-sensitivity.
00277      * NOTE: This doesn't belong here, but everything else in hw_s is const,
00278      * and this allows us to save some precious RAM.
00279      */
00280     unsigned char accel_half;
00281     /* 1 if device in low-power accel-only mode. */
00282     unsigned char lp_accel_mode;
00283     /* 1 if interrupts are only triggered on motion events. */
00284     unsigned char int_motion_only;
00285     struct motion_int_cache_s cache;
00286     /* 1 for active low interrupts. */
00287     unsigned char active_low_int;
00288     /* 1 for latched interrupts. */
00289     unsigned char latched_int;
00290     /* 1 if DMP is enabled. */
00291     unsigned char dmp_on;
00292     /* Ensures that DMP will only be loaded once. */
00293     unsigned char dmp_loaded;
00294     /* Sampling rate used when DMP is enabled. */
00295     unsigned short dmp_sample_rate;
00296 #ifdef AK89xx_SECONDARY
00297     /* Compass sample rate. */
00298     unsigned short compass_sample_rate;
00299     unsigned char compass_addr;
00300     short mag_sens_adj[3];
00301 #endif
00302 };
00303 
00304 /* Information for self-test. */
00305 struct test_s {
00306     unsigned long gyro_sens;
00307     unsigned long accel_sens;
00308     unsigned char reg_rate_div;
00309     unsigned char reg_lpf;
00310     unsigned char reg_gyro_fsr;
00311     unsigned char reg_accel_fsr;
00312     unsigned short wait_ms;
00313     unsigned char packet_thresh;
00314     float min_dps;
00315     float max_dps;
00316     float max_gyro_var;
00317     float min_g;
00318     float max_g;
00319     float max_accel_var;
00320 #ifdef MPU6500
00321     float max_g_offset;
00322     unsigned short sample_wait_ms;
00323 #endif
00324 };
00325 
00326 /* Gyro driver state variables. */
00327 struct gyro_state_s {
00328     const struct gyro_reg_s *reg;
00329     const struct hw_s *hw;
00330     struct chip_cfg_s chip_cfg;
00331     const struct test_s *test;
00332 };
00333 
00334 /* Filter configurations. */
00335 enum lpf_e {
00336     INV_FILTER_256HZ_NOLPF2 = 0,
00337     INV_FILTER_188HZ,
00338     INV_FILTER_98HZ,
00339     INV_FILTER_42HZ,
00340     INV_FILTER_20HZ,
00341     INV_FILTER_10HZ,
00342     INV_FILTER_5HZ,
00343     INV_FILTER_2100HZ_NOLPF,
00344     NUM_FILTER
00345 };
00346 
00347 /* Full scale ranges. */
00348 enum gyro_fsr_e {
00349     INV_FSR_250DPS = 0,
00350     INV_FSR_500DPS,
00351     INV_FSR_1000DPS,
00352     INV_FSR_2000DPS,
00353     NUM_GYRO_FSR
00354 };
00355 
00356 /* Full scale ranges. */
00357 enum accel_fsr_e {
00358     INV_FSR_2G = 0,
00359     INV_FSR_4G,
00360     INV_FSR_8G,
00361     INV_FSR_16G,
00362     NUM_ACCEL_FSR
00363 };
00364 
00365 /* Clock sources. */
00366 enum clock_sel_e {
00367     INV_CLK_INTERNAL = 0,
00368     INV_CLK_PLL,
00369     NUM_CLK
00370 };
00371 
00372 /* Low-power accel wakeup rates. */
00373 enum lp_accel_rate_e {
00374 #if defined MPU6050
00375     INV_LPA_1_25HZ,
00376     INV_LPA_5HZ,
00377     INV_LPA_20HZ,
00378     INV_LPA_40HZ
00379 #elif defined MPU6500
00380     INV_LPA_0_3125HZ,
00381     INV_LPA_0_625HZ,
00382     INV_LPA_1_25HZ,
00383     INV_LPA_2_5HZ,
00384     INV_LPA_5HZ,
00385     INV_LPA_10HZ,
00386     INV_LPA_20HZ,
00387     INV_LPA_40HZ,
00388     INV_LPA_80HZ,
00389     INV_LPA_160HZ,
00390     INV_LPA_320HZ,
00391     INV_LPA_640HZ
00392 #endif
00393 };
00394 
00395 #define BIT_I2C_MST_VDDIO   (0x80)
00396 #define BIT_FIFO_EN         (0x40)
00397 #define BIT_DMP_EN          (0x80)
00398 #define BIT_FIFO_RST        (0x04)
00399 #define BIT_DMP_RST         (0x08)
00400 #define BIT_FIFO_OVERFLOW   (0x10)
00401 #define BIT_DATA_RDY_EN     (0x01)
00402 #define BIT_DMP_INT_EN      (0x02)
00403 #define BIT_MOT_INT_EN      (0x40)
00404 #define BITS_FSR            (0x18)
00405 #define BITS_LPF            (0x07)
00406 #define BITS_HPF            (0x07)
00407 #define BITS_CLK            (0x07)
00408 #define BIT_FIFO_SIZE_1024  (0x40)
00409 #define BIT_FIFO_SIZE_2048  (0x80)
00410 #define BIT_FIFO_SIZE_4096  (0xC0)
00411 #define BIT_RESET           (0x80)
00412 #define BIT_SLEEP           (0x40)
00413 #define BIT_S0_DELAY_EN     (0x01)
00414 #define BIT_S2_DELAY_EN     (0x04)
00415 #define BITS_SLAVE_LENGTH   (0x0F)
00416 #define BIT_SLAVE_BYTE_SW   (0x40)
00417 #define BIT_SLAVE_GROUP     (0x10)
00418 #define BIT_SLAVE_EN        (0x80)
00419 #define BIT_I2C_READ        (0x80)
00420 #define BITS_I2C_MASTER_DLY (0x1F)
00421 #define BIT_AUX_IF_EN       (0x20)
00422 #define BIT_ACTL            (0x80)
00423 #define BIT_LATCH_EN        (0x20)
00424 #define BIT_ANY_RD_CLR      (0x10)
00425 #define BIT_BYPASS_EN       (0x02)
00426 #define BITS_WOM_EN         (0xC0)
00427 #define BIT_LPA_CYCLE       (0x20)
00428 #define BIT_STBY_XA         (0x20)
00429 #define BIT_STBY_YA         (0x10)
00430 #define BIT_STBY_ZA         (0x08)
00431 #define BIT_STBY_XG         (0x04)
00432 #define BIT_STBY_YG         (0x02)
00433 #define BIT_STBY_ZG         (0x01)
00434 #define BIT_STBY_XYZA       (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
00435 #define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
00436 
00437 #if defined AK8975_SECONDARY
00438 #define SUPPORTS_AK89xx_HIGH_SENS   (0x00)
00439 #define AK89xx_FSR                  (9830)
00440 #elif defined AK8963_SECONDARY
00441 #define SUPPORTS_AK89xx_HIGH_SENS   (0x10)
00442 #define AK89xx_FSR                  (4915)
00443 #endif
00444 
00445 #ifdef AK89xx_SECONDARY
00446 #define AKM_REG_WHOAMI      (0x00)
00447 
00448 #define AKM_REG_ST1         (0x02)
00449 #define AKM_REG_HXL         (0x03)
00450 #define AKM_REG_ST2         (0x09)
00451 
00452 #define AKM_REG_CNTL        (0x0A)
00453 #define AKM_REG_ASTC        (0x0C)
00454 #define AKM_REG_ASAX        (0x10)
00455 #define AKM_REG_ASAY        (0x11)
00456 #define AKM_REG_ASAZ        (0x12)
00457 
00458 #define AKM_DATA_READY      (0x01)
00459 #define AKM_DATA_OVERRUN    (0x02)
00460 #define AKM_OVERFLOW        (0x80)
00461 #define AKM_DATA_ERROR      (0x40)
00462 
00463 #define AKM_BIT_SELF_TEST   (0x40)
00464 
00465 #define AKM_POWER_DOWN          (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
00466 #define AKM_SINGLE_MEASUREMENT  (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
00467 #define AKM_FUSE_ROM_ACCESS     (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
00468 #define AKM_MODE_SELF_TEST      (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
00469 
00470 #define AKM_WHOAMI      (0x48)
00471 #endif
00472 
00473 #if defined MPU6050
00474 const struct gyro_reg_s reg = {
00475     .who_am_i       = 0x75,
00476     .rate_div       = 0x19,
00477     .lpf            = 0x1A,
00478     .prod_id        = 0x0C,
00479     .user_ctrl      = 0x6A,
00480     .fifo_en        = 0x23,
00481     .gyro_cfg       = 0x1B,
00482     .accel_cfg      = 0x1C,
00483     .motion_thr     = 0x1F,
00484     .motion_dur     = 0x20,
00485     .fifo_count_h   = 0x72,
00486     .fifo_r_w       = 0x74,
00487     .raw_gyro       = 0x43,
00488     .raw_accel      = 0x3B,
00489     .temp           = 0x41,
00490     .int_enable     = 0x38,
00491     .dmp_int_status = 0x39,
00492     .int_status     = 0x3A,
00493     .pwr_mgmt_1     = 0x6B,
00494     .pwr_mgmt_2     = 0x6C,
00495     .int_pin_cfg    = 0x37,
00496     .mem_r_w        = 0x6F,
00497     .accel_offs     = 0x06,
00498     .i2c_mst        = 0x24,
00499     .bank_sel       = 0x6D,
00500     .mem_start_addr = 0x6E,
00501     .prgm_start_h   = 0x70
00502 #ifdef AK89xx_SECONDARY
00503     ,.raw_compass   = 0x49,
00504     .yg_offs_tc     = 0x01,
00505     .s0_addr        = 0x25,
00506     .s0_reg         = 0x26,
00507     .s0_ctrl        = 0x27,
00508     .s1_addr        = 0x28,
00509     .s1_reg         = 0x29,
00510     .s1_ctrl        = 0x2A,
00511     .s4_ctrl        = 0x34,
00512     .s0_do          = 0x63,
00513     .s1_do          = 0x64,
00514     .i2c_delay_ctrl = 0x67
00515 #endif
00516 };
00517 const struct hw_s hw = {
00518     .addr           = 0x69,
00519     .max_fifo       = 1024,
00520     .num_reg        = 118,
00521     .temp_sens      = 340,
00522     .temp_offset    = -521,
00523     .bank_size      = 256
00524 #if defined AK89xx_SECONDARY
00525     ,.compass_fsr    = AK89xx_FSR
00526 #endif
00527 };
00528 
00529 const struct test_s test = {
00530     .gyro_sens      = 32768/250,
00531     .accel_sens     = 32768/16,
00532     .reg_rate_div   = 0,    /* 1kHz. */
00533     .reg_lpf        = 1,    /* 188Hz. */
00534     .reg_gyro_fsr   = 0,    /* 250dps. */
00535     .reg_accel_fsr  = 0x18, /* 16g. */
00536     .wait_ms        = 50,
00537     .packet_thresh  = 5,    /* 5% */
00538     .min_dps        = 10.f,
00539     .max_dps        = 105.f,
00540     .max_gyro_var   = 0.14f,
00541     .min_g          = 0.3f,
00542     .max_g          = 0.95f,
00543     .max_accel_var  = 0.14f
00544 };
00545 
00546 static struct gyro_state_s st = {
00547     .reg = &reg,
00548     .hw = &hw,
00549     .test = &test
00550 };
00551 #elif defined MPU6500
00552 const struct gyro_reg_s reg = {
00553     .who_am_i       = 0x75,
00554     .rate_div       = 0x19,
00555     .lpf            = 0x1A,
00556     .prod_id        = 0x0C,
00557     .user_ctrl      = 0x6A,
00558     .fifo_en        = 0x23,
00559     .gyro_cfg       = 0x1B,
00560     .accel_cfg      = 0x1C,
00561     .accel_cfg2     = 0x1D,
00562     .lp_accel_odr   = 0x1E,
00563     .motion_thr     = 0x1F,
00564     .motion_dur     = 0x20,
00565     .fifo_count_h   = 0x72,
00566     .fifo_r_w       = 0x74,
00567     .raw_gyro       = 0x43,
00568     .raw_accel      = 0x3B,
00569     .temp           = 0x41,
00570     .int_enable     = 0x38,
00571     .dmp_int_status = 0x39,
00572     .int_status     = 0x3A,
00573     .accel_intel    = 0x69,
00574     .pwr_mgmt_1     = 0x6B,
00575     .pwr_mgmt_2     = 0x6C,
00576     .int_pin_cfg    = 0x37,
00577     .mem_r_w        = 0x6F,
00578     .accel_offs     = 0x77,
00579     .i2c_mst        = 0x24,
00580     .bank_sel       = 0x6D,
00581     .mem_start_addr = 0x6E,
00582     .prgm_start_h   = 0x70
00583 #ifdef AK89xx_SECONDARY
00584     ,.raw_compass   = 0x49,
00585     .s0_addr        = 0x25,
00586     .s0_reg         = 0x26,
00587     .s0_ctrl        = 0x27,
00588     .s1_addr        = 0x28,
00589     .s1_reg         = 0x29,
00590     .s1_ctrl        = 0x2A,
00591     .s4_ctrl        = 0x34,
00592     .s0_do          = 0x63,
00593     .s1_do          = 0x64,
00594     .i2c_delay_ctrl = 0x67
00595 #endif
00596 };
00597 const struct hw_s hw = {
00598     .addr           = 0x68,
00599     .max_fifo       = 1024,
00600     .num_reg        = 128,
00601     .temp_sens      = 321,
00602     .temp_offset    = 0,
00603     .bank_size      = 256
00604 #if defined AK89xx_SECONDARY
00605     ,.compass_fsr    = AK89xx_FSR
00606 #endif
00607 };
00608 
00609 const struct test_s test = {
00610     .gyro_sens      = 32768/250,
00611     .accel_sens     = 32768/2,  //FSR = +-2G = 16384 LSB/G
00612     .reg_rate_div   = 0,    /* 1kHz. */
00613     .reg_lpf        = 2,    /* 92Hz low pass filter*/
00614     .reg_gyro_fsr   = 0,    /* 250dps. */
00615     .reg_accel_fsr  = 0x0,  /* Accel FSR setting = 2g. */
00616     .wait_ms        = 200,   //200ms stabilization time
00617     .packet_thresh  = 200,    /* 200 samples */
00618     .min_dps        = 20.f,  //20 dps for Gyro Criteria C
00619     .max_dps        = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B
00620     .max_gyro_var   = .5f, //Must exceed +50% variation for Gyro Criteria A
00621     .min_g          = .225f, //Accel must exceed Min 225 mg for Criteria B
00622     .max_g          = .675f, //Accel cannot exceed Max 675 mg for Criteria B
00623     .max_accel_var  = .5f,  //Accel must be within 50% variation for Criteria A
00624     .max_g_offset   = .5f,   //500 mg for Accel Criteria C
00625     .sample_wait_ms = 10    //10ms sample time wait
00626 };
00627 
00628 static struct gyro_state_s st = {
00629     .reg = &reg,
00630     .hw = &hw,
00631     .test = &test
00632 };
00633 #endif
00634 
00635 #define MAX_PACKET_LENGTH (12)
00636 #ifdef MPU6500
00637 #define HWST_MAX_PACKET_LENGTH (512)
00638 #endif
00639 
00640 #ifdef AK89xx_SECONDARY
00641 static int setup_compass(void);
00642 #define MAX_COMPASS_SAMPLE_RATE (100)
00643 #endif
00644 
00645 /**
00646  *  @brief      Enable/disable data ready interrupt.
00647  *  If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
00648  *  interrupt is used.
00649  *  @param[in]  enable      1 to enable interrupt.
00650  *  @return     0 if successful.
00651  */
00652 static int set_int_enable(unsigned char enable)
00653 {
00654     unsigned char tmp;
00655 
00656     if (st.chip_cfg.dmp_on) {
00657         if (enable)
00658             tmp = BIT_DMP_INT_EN;
00659         else
00660             tmp = 0x00;
00661         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
00662             return -1;
00663         st.chip_cfg.int_enable = tmp;
00664     } else {
00665         if (!st.chip_cfg.sensors)
00666             return -1;
00667         if (enable && st.chip_cfg.int_enable)
00668             return 0;
00669         if (enable)
00670             tmp = BIT_DATA_RDY_EN;
00671         else
00672             tmp = 0x00;
00673         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
00674             return -1;
00675         st.chip_cfg.int_enable = tmp;
00676     }
00677     return 0;
00678 }
00679 
00680 /**
00681  *  @brief      Register dump for testing.
00682  *  @return     0 if successful.
00683  */
00684 int mpu_reg_dump(void)
00685 {
00686     unsigned char ii;
00687     unsigned char data;
00688 
00689     for (ii = 0; ii < st.hw->num_reg; ii++) {
00690         if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
00691             continue;
00692         if (i2c_read(st.hw->addr, ii, 1, &data))
00693             return -1;
00694         log_i("%#5x: %#5x\r\n", ii, data);
00695     }
00696     return 0;
00697 }
00698 
00699 /**
00700  *  @brief      Read from a single register.
00701  *  NOTE: The memory and FIFO read/write registers cannot be accessed.
00702  *  @param[in]  reg     Register address.
00703  *  @param[out] data    Register data.
00704  *  @return     0 if successful.
00705  */
00706 int mpu_read_reg(unsigned char reg, unsigned char *data)
00707 {
00708     if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
00709         return -1;
00710     if (reg >= st.hw->num_reg)
00711         return -1;
00712     return i2c_read(st.hw->addr, reg, 1, data);
00713 }
00714 
00715 /**
00716  *  @brief      Initialize hardware.
00717  *  Initial configuration:\n
00718  *  Gyro FSR: +/- 2000DPS\n
00719  *  Accel FSR +/- 2G\n
00720  *  DLPF: 42Hz\n
00721  *  FIFO rate: 50Hz\n
00722  *  Clock source: Gyro PLL\n
00723  *  FIFO: Disabled.\n
00724  *  Data ready interrupt: Disabled, active low, unlatched.
00725  *  @param[in]  int_param   Platform-specific parameters to interrupt API.
00726  *  @return     0 if successful.
00727  */
00728 int mpu_init(struct int_param_s *int_param)
00729 {
00730     unsigned char data[6];
00731 
00732     /* Reset device. */
00733     data[0] = BIT_RESET;
00734     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
00735         return -1;
00736     delay_ms(100);
00737 
00738     /* Wake up chip. */
00739     data[0] = 0x00;
00740     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
00741         return -1;
00742 
00743    st.chip_cfg.accel_half = 0;
00744 
00745 #ifdef MPU6500
00746     /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
00747      * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
00748      */
00749     data[0] = BIT_FIFO_SIZE_1024 | 0x8;
00750     if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
00751         return -1;
00752 #endif
00753 
00754     /* Set to invalid values to ensure no I2C writes are skipped. */
00755     st.chip_cfg.sensors = 0xFF;
00756     st.chip_cfg.gyro_fsr = 0xFF;
00757     st.chip_cfg.accel_fsr = 0xFF;
00758     st.chip_cfg.lpf = 0xFF;
00759     st.chip_cfg.sample_rate = 0xFFFF;
00760     st.chip_cfg.fifo_enable = 0xFF;
00761     st.chip_cfg.bypass_mode = 0xFF;
00762 #ifdef AK89xx_SECONDARY
00763     st.chip_cfg.compass_sample_rate = 0xFFFF;
00764 #endif
00765     /* mpu_set_sensors always preserves this setting. */
00766     st.chip_cfg.clk_src = INV_CLK_PLL;
00767     /* Handled in next call to mpu_set_bypass. */
00768     st.chip_cfg.active_low_int = 1;
00769     st.chip_cfg.latched_int = 0;
00770     st.chip_cfg.int_motion_only = 0;
00771     st.chip_cfg.lp_accel_mode = 0;
00772     memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
00773     st.chip_cfg.dmp_on = 0;
00774     st.chip_cfg.dmp_loaded = 0;
00775     st.chip_cfg.dmp_sample_rate = 0;
00776 
00777     if (mpu_set_gyro_fsr(2000))
00778         return -1;
00779     if (mpu_set_accel_fsr(2))
00780         return -1;
00781     if (mpu_set_lpf(42))
00782         return -1;
00783     if (mpu_set_sample_rate(50))
00784         return -1;
00785     if (mpu_configure_fifo(0))
00786         return -1;
00787 
00788     if (int_param)
00789         reg_int_cb(int_param);
00790 
00791 #ifdef AK89xx_SECONDARY
00792     setup_compass();
00793     if (mpu_set_compass_sample_rate(10))
00794         return -1;
00795 #else
00796     /* Already disabled by setup_compass. */
00797     if (mpu_set_bypass(0))
00798         return -1;
00799 #endif
00800 
00801     mpu_set_sensors(0);
00802     return 0;
00803 }
00804 
00805 /**
00806  *  @brief      Enter low-power accel-only mode.
00807  *  In low-power accel mode, the chip goes to sleep and only wakes up to sample
00808  *  the accelerometer at one of the following frequencies:
00809  *  \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
00810  *  \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
00811  *  \n If the requested rate is not one listed above, the device will be set to
00812  *  the next highest rate. Requesting a rate above the maximum supported
00813  *  frequency will result in an error.
00814  *  \n To select a fractional wake-up frequency, round down the value passed to
00815  *  @e rate.
00816  *  @param[in]  rate        Minimum sampling rate, or zero to disable LP
00817  *                          accel mode.
00818  *  @return     0 if successful.
00819  */
00820 int mpu_lp_accel_mode(unsigned char rate)
00821 {
00822     unsigned char tmp[2];
00823 
00824     if (rate > 40)
00825         return -1;
00826 
00827     if (!rate) {
00828         mpu_set_int_latched(0);
00829         tmp[0] = 0;
00830         tmp[1] = BIT_STBY_XYZG;
00831         if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
00832             return -1;
00833         st.chip_cfg.lp_accel_mode = 0;
00834         return 0;
00835     }
00836     /* For LP accel, we automatically configure the hardware to produce latched
00837      * interrupts. In LP accel mode, the hardware cycles into sleep mode before
00838      * it gets a chance to deassert the interrupt pin; therefore, we shift this
00839      * responsibility over to the MCU.
00840      *
00841      * Any register read will clear the interrupt.
00842      */
00843     mpu_set_int_latched(1);
00844 #if defined MPU6050
00845     tmp[0] = BIT_LPA_CYCLE;
00846     if (rate == 1) {
00847         tmp[1] = INV_LPA_1_25HZ;
00848         mpu_set_lpf(5);
00849     } else if (rate <= 5) {
00850         tmp[1] = INV_LPA_5HZ;
00851         mpu_set_lpf(5);
00852     } else if (rate <= 20) {
00853         tmp[1] = INV_LPA_20HZ;
00854         mpu_set_lpf(10);
00855     } else {
00856         tmp[1] = INV_LPA_40HZ;
00857         mpu_set_lpf(20);
00858     }
00859     tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
00860     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
00861         return -1;
00862 #elif defined MPU6500
00863     /* Set wake frequency. */
00864     if (rate == 1)
00865         tmp[0] = INV_LPA_1_25HZ;
00866     else if (rate == 2)
00867         tmp[0] = INV_LPA_2_5HZ;
00868     else if (rate <= 5)
00869         tmp[0] = INV_LPA_5HZ;
00870     else if (rate <= 10)
00871         tmp[0] = INV_LPA_10HZ;
00872     else if (rate <= 20)
00873         tmp[0] = INV_LPA_20HZ;
00874     else if (rate <= 40)
00875         tmp[0] = INV_LPA_40HZ;
00876     else if (rate <= 80)
00877         tmp[0] = INV_LPA_80HZ;
00878     else if (rate <= 160)
00879         tmp[0] = INV_LPA_160HZ;
00880     else if (rate <= 320)
00881         tmp[0] = INV_LPA_320HZ;
00882     else
00883         tmp[0] = INV_LPA_640HZ;
00884     if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
00885         return -1;
00886     tmp[0] = BIT_LPA_CYCLE;
00887     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
00888         return -1;
00889 #endif
00890     st.chip_cfg.sensors = INV_XYZ_ACCEL;
00891     st.chip_cfg.clk_src = 0;
00892     st.chip_cfg.lp_accel_mode = 1;
00893     mpu_configure_fifo(0);
00894 
00895     return 0;
00896 }
00897 
00898 /**
00899  *  @brief      Read raw gyro data directly from the registers.
00900  *  @param[out] data        Raw data in hardware units.
00901  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
00902  *  @return     0 if successful.
00903  */
00904 int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
00905 {
00906     unsigned char tmp[6];
00907 
00908     if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
00909         return -1;
00910 
00911     if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
00912         return -1;
00913     data[0] = (tmp[0] << 8) | tmp[1];
00914     data[1] = (tmp[2] << 8) | tmp[3];
00915     data[2] = (tmp[4] << 8) | tmp[5];
00916     if (timestamp)
00917         get_ms(timestamp);
00918     return 0;
00919 }
00920 
00921 /**
00922  *  @brief      Read raw accel data directly from the registers.
00923  *  @param[out] data        Raw data in hardware units.
00924  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
00925  *  @return     0 if successful.
00926  */
00927 int mpu_get_accel_reg(short *data, unsigned long *timestamp)
00928 {
00929     unsigned char tmp[6];
00930 
00931     if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
00932         return -1;
00933 
00934     if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
00935         return -1;
00936     data[0] = (tmp[0] << 8) | tmp[1];
00937     data[1] = (tmp[2] << 8) | tmp[3];
00938     data[2] = (tmp[4] << 8) | tmp[5];
00939     if (timestamp)
00940         get_ms(timestamp);
00941     return 0;
00942 }
00943 
00944 /**
00945  *  @brief      Read temperature data directly from the registers.
00946  *  @param[out] data        Data in q16 format.
00947  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
00948  *  @return     0 if successful.
00949  */
00950 int mpu_get_temperature(long *data, unsigned long *timestamp)
00951 {
00952     unsigned char tmp[2];
00953     short raw;
00954 
00955     if (!(st.chip_cfg.sensors))
00956         return -1;
00957 
00958     if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))
00959         return -1;
00960     raw = (tmp[0] << 8) | tmp[1];
00961     if (timestamp)
00962         get_ms(timestamp);
00963 
00964     data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
00965     return 0;
00966 }
00967 
00968 /**
00969  *  @brief      Read biases to the accel bias 6500 registers.
00970  *  This function reads from the MPU6500 accel offset cancellations registers.
00971  *  The format are G in +-8G format. The register is initialized with OTP 
00972  *  factory trim values.
00973  *  @param[in]  accel_bias  returned structure with the accel bias
00974  *  @return     0 if successful.
00975  */
00976 int mpu_read_6500_accel_bias(long *accel_bias) {
00977     unsigned char data[6];
00978     if (i2c_read(st.hw->addr, 0x77, 2, &data[0]))
00979         return -1;
00980     if (i2c_read(st.hw->addr, 0x7A, 2, &data[2]))
00981         return -1;
00982     if (i2c_read(st.hw->addr, 0x7D, 2, &data[4]))
00983         return -1;
00984     accel_bias[0] = ((long)data[0]<<8) | data[1];
00985     accel_bias[1] = ((long)data[2]<<8) | data[3];
00986     accel_bias[2] = ((long)data[4]<<8) | data[5];
00987     return 0;
00988 }
00989 
00990 /**
00991  *  @brief      Read biases to the accel bias 6050 registers.
00992  *  This function reads from the MPU6050 accel offset cancellations registers.
00993  *  The format are G in +-8G format. The register is initialized with OTP 
00994  *  factory trim values.
00995  *  @param[in]  accel_bias  returned structure with the accel bias
00996  *  @return     0 if successful.
00997  */
00998 int mpu_read_6050_accel_bias(long *accel_bias) {
00999     unsigned char data[6];
01000     if (i2c_read(st.hw->addr, 0x06, 2, &data[0]))
01001         return -1;
01002     if (i2c_read(st.hw->addr, 0x08, 2, &data[2]))
01003         return -1;
01004     if (i2c_read(st.hw->addr, 0x0A, 2, &data[4]))
01005         return -1;
01006     accel_bias[0] = ((long)data[0]<<8) | data[1];
01007     accel_bias[1] = ((long)data[2]<<8) | data[3];
01008     accel_bias[2] = ((long)data[4]<<8) | data[5];
01009     return 0;
01010 }
01011 
01012 int mpu_read_6500_gyro_bias(long *gyro_bias) {
01013     unsigned char data[6];
01014     if (i2c_read(st.hw->addr, 0x13, 2, &data[0]))
01015         return -1;
01016     if (i2c_read(st.hw->addr, 0x15, 2, &data[2]))
01017         return -1;
01018     if (i2c_read(st.hw->addr, 0x17, 2, &data[4]))
01019         return -1;
01020     gyro_bias[0] = ((long)data[0]<<8) | data[1];
01021     gyro_bias[1] = ((long)data[2]<<8) | data[3];
01022     gyro_bias[2] = ((long)data[4]<<8) | data[5];
01023     return 0;
01024 }
01025 
01026 /**
01027  *  @brief      Push biases to the gyro bias 6500/6050 registers.
01028  *  This function expects biases relative to the current sensor output, and
01029  *  these biases will be added to the factory-supplied values. Bias inputs are LSB
01030  *  in +-1000dps format.
01031  *  @param[in]  gyro_bias  New biases.
01032  *  @return     0 if successful.
01033  */
01034 int mpu_set_gyro_bias_reg(long *gyro_bias)
01035 {
01036     unsigned char data[6] = {0, 0, 0, 0, 0, 0};
01037     int i=0;
01038     for(i=0;i<3;i++) {
01039         gyro_bias[i]= (-gyro_bias[i]);
01040     }
01041     data[0] = (gyro_bias[0] >> 8) & 0xff;
01042     data[1] = (gyro_bias[0]) & 0xff;
01043     data[2] = (gyro_bias[1] >> 8) & 0xff;
01044     data[3] = (gyro_bias[1]) & 0xff;
01045     data[4] = (gyro_bias[2] >> 8) & 0xff;
01046     data[5] = (gyro_bias[2]) & 0xff;
01047     if (i2c_write(st.hw->addr, 0x13, 2, &data[0]))
01048         return -1;
01049     if (i2c_write(st.hw->addr, 0x15, 2, &data[2]))
01050         return -1;
01051     if (i2c_write(st.hw->addr, 0x17, 2, &data[4]))
01052         return -1;
01053     return 0;
01054 }
01055 
01056 /**
01057  *  @brief      Push biases to the accel bias 6050 registers.
01058  *  This function expects biases relative to the current sensor output, and
01059  *  these biases will be added to the factory-supplied values. Bias inputs are LSB
01060  *  in +-8G format.
01061  *  @param[in]  accel_bias  New biases.
01062  *  @return     0 if successful.
01063  */
01064 int mpu_set_accel_bias_6050_reg(const long *accel_bias)
01065 {
01066     unsigned char data[6] = {0, 0, 0, 0, 0, 0};
01067     long accel_reg_bias[3] = {0, 0, 0};
01068     long mask = 0x0001;
01069     unsigned char mask_bit[3] = {0, 0, 0};
01070     unsigned char i = 0;
01071     if(mpu_read_6050_accel_bias(accel_reg_bias))
01072         return -1;
01073 
01074     //bit 0 of the 2 byte bias is for temp comp
01075     //calculations need to compensate for this and not change it
01076     for(i=0; i<3; i++) {
01077         if(accel_reg_bias[i]&mask)
01078             mask_bit[i] = 0x01;
01079     }
01080 
01081     accel_reg_bias[0] -= accel_bias[0];
01082     accel_reg_bias[1] -= accel_bias[1];
01083     accel_reg_bias[2] -= accel_bias[2];
01084 
01085     data[0] = (accel_reg_bias[0] >> 8) & 0xff;
01086     data[1] = (accel_reg_bias[0]) & 0xff;
01087     data[1] = data[1]|mask_bit[0];
01088     data[2] = (accel_reg_bias[1] >> 8) & 0xff;
01089     data[3] = (accel_reg_bias[1]) & 0xff;
01090     data[3] = data[3]|mask_bit[1];
01091     data[4] = (accel_reg_bias[2] >> 8) & 0xff;
01092     data[5] = (accel_reg_bias[2]) & 0xff;
01093     data[5] = data[5]|mask_bit[2];
01094 
01095     if (i2c_write(st.hw->addr, 0x06, 2, &data[0]))
01096         return -1;
01097     if (i2c_write(st.hw->addr, 0x08, 2, &data[2]))
01098         return -1;
01099     if (i2c_write(st.hw->addr, 0x0A, 2, &data[4]))
01100         return -1;
01101 
01102     return 0;
01103 }
01104 
01105 
01106 /**
01107  *  @brief      Push biases to the accel bias 6500 registers.
01108  *  This function expects biases relative to the current sensor output, and
01109  *  these biases will be added to the factory-supplied values. Bias inputs are LSB
01110  *  in +-8G format.
01111  *  @param[in]  accel_bias  New biases.
01112  *  @return     0 if successful.
01113  */
01114 int mpu_set_accel_bias_6500_reg(const long *accel_bias)
01115 {
01116     unsigned char data[6] = {0, 0, 0, 0, 0, 0};
01117     long accel_reg_bias[3] = {0, 0, 0};
01118     long mask = 0x0001;
01119     unsigned char mask_bit[3] = {0, 0, 0};
01120     unsigned char i = 0;
01121 
01122     if(mpu_read_6500_accel_bias(accel_reg_bias))
01123         return -1;
01124 
01125     //bit 0 of the 2 byte bias is for temp comp
01126     //calculations need to compensate for this
01127     for(i=0; i<3; i++) {
01128         if(accel_reg_bias[i]&mask)
01129             mask_bit[i] = 0x01;
01130     }
01131 
01132     accel_reg_bias[0] -= accel_bias[0];
01133     accel_reg_bias[1] -= accel_bias[1];
01134     accel_reg_bias[2] -= accel_bias[2];
01135 
01136     data[0] = (accel_reg_bias[0] >> 8) & 0xff;
01137     data[1] = (accel_reg_bias[0]) & 0xff;
01138     data[1] = data[1]|mask_bit[0];
01139     data[2] = (accel_reg_bias[1] >> 8) & 0xff;
01140     data[3] = (accel_reg_bias[1]) & 0xff;
01141     data[3] = data[3]|mask_bit[1];
01142     data[4] = (accel_reg_bias[2] >> 8) & 0xff;
01143     data[5] = (accel_reg_bias[2]) & 0xff;
01144     data[5] = data[5]|mask_bit[2];
01145 
01146     if (i2c_write(st.hw->addr, 0x77, 2, &data[0]))
01147         return -1;
01148     if (i2c_write(st.hw->addr, 0x7A, 2, &data[2]))
01149         return -1;
01150     if (i2c_write(st.hw->addr, 0x7D, 2, &data[4]))
01151         return -1;
01152 
01153     return 0;
01154 }
01155 
01156 /**
01157  *  @brief  Reset FIFO read/write pointers.
01158  *  @return 0 if successful.
01159  */
01160 int mpu_reset_fifo(void)
01161 {
01162     unsigned char data;
01163 
01164     if (!(st.chip_cfg.sensors))
01165         return -1;
01166 
01167     data = 0;
01168     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
01169         return -1;
01170     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
01171         return -1;
01172     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
01173         return -1;
01174 
01175     if (st.chip_cfg.dmp_on) {
01176         data = BIT_FIFO_RST | BIT_DMP_RST;
01177         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
01178             return -1;
01179         delay_ms(50);
01180         data = BIT_DMP_EN | BIT_FIFO_EN;
01181         if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
01182             data |= BIT_AUX_IF_EN;
01183         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
01184             return -1;
01185         if (st.chip_cfg.int_enable)
01186             data = BIT_DMP_INT_EN;
01187         else
01188             data = 0;
01189         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
01190             return -1;
01191         data = 0;
01192         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
01193             return -1;
01194     } else {
01195         data = BIT_FIFO_RST;
01196         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
01197             return -1;
01198         if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
01199             data = BIT_FIFO_EN;
01200         else
01201             data = BIT_FIFO_EN | BIT_AUX_IF_EN;
01202         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
01203             return -1;
01204         delay_ms(50);
01205         if (st.chip_cfg.int_enable)
01206             data = BIT_DATA_RDY_EN;
01207         else
01208             data = 0;
01209         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
01210             return -1;
01211         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
01212             return -1;
01213     }
01214     return 0;
01215 }
01216 
01217 /**
01218  *  @brief      Get the gyro full-scale range.
01219  *  @param[out] fsr Current full-scale range.
01220  *  @return     0 if successful.
01221  */
01222 int mpu_get_gyro_fsr(unsigned short *fsr)
01223 {
01224     switch (st.chip_cfg.gyro_fsr) {
01225     case INV_FSR_250DPS:
01226         fsr[0] = 250;
01227         break;
01228     case INV_FSR_500DPS:
01229         fsr[0] = 500;
01230         break;
01231     case INV_FSR_1000DPS:
01232         fsr[0] = 1000;
01233         break;
01234     case INV_FSR_2000DPS:
01235         fsr[0] = 2000;
01236         break;
01237     default:
01238         fsr[0] = 0;
01239         break;
01240     }
01241     return 0;
01242 }
01243 
01244 /**
01245  *  @brief      Set the gyro full-scale range.
01246  *  @param[in]  fsr Desired full-scale range.
01247  *  @return     0 if successful.
01248  */
01249 int mpu_set_gyro_fsr(unsigned short fsr)
01250 {
01251     unsigned char data;
01252 
01253     if (!(st.chip_cfg.sensors))
01254         return -1;
01255 
01256     switch (fsr) {
01257     case 250:
01258         data = INV_FSR_250DPS << 3;
01259         break;
01260     case 500:
01261         data = INV_FSR_500DPS << 3;
01262         break;
01263     case 1000:
01264         data = INV_FSR_1000DPS << 3;
01265         break;
01266     case 2000:
01267         data = INV_FSR_2000DPS << 3;
01268         break;
01269     default:
01270         return -1;
01271     }
01272 
01273     if (st.chip_cfg.gyro_fsr == (data >> 3))
01274         return 0;
01275     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
01276         return -1;
01277     st.chip_cfg.gyro_fsr = data >> 3;
01278     return 0;
01279 }
01280 
01281 /**
01282  *  @brief      Get the accel full-scale range.
01283  *  @param[out] fsr Current full-scale range.
01284  *  @return     0 if successful.
01285  */
01286 int mpu_get_accel_fsr(unsigned char *fsr)
01287 {
01288     switch (st.chip_cfg.accel_fsr) {
01289     case INV_FSR_2G:
01290         fsr[0] = 2;
01291         break;
01292     case INV_FSR_4G:
01293         fsr[0] = 4;
01294         break;
01295     case INV_FSR_8G:
01296         fsr[0] = 8;
01297         break;
01298     case INV_FSR_16G:
01299         fsr[0] = 16;
01300         break;
01301     default:
01302         return -1;
01303     }
01304     if (st.chip_cfg.accel_half)
01305         fsr[0] <<= 1;
01306     return 0;
01307 }
01308 
01309 /**
01310  *  @brief      Set the accel full-scale range.
01311  *  @param[in]  fsr Desired full-scale range.
01312  *  @return     0 if successful.
01313  */
01314 int mpu_set_accel_fsr(unsigned char fsr)
01315 {
01316     unsigned char data;
01317 
01318     if (!(st.chip_cfg.sensors))
01319         return -1;
01320 
01321     switch (fsr) {
01322     case 2:
01323         data = INV_FSR_2G << 3;
01324         break;
01325     case 4:
01326         data = INV_FSR_4G << 3;
01327         break;
01328     case 8:
01329         data = INV_FSR_8G << 3;
01330         break;
01331     case 16:
01332         data = INV_FSR_16G << 3;
01333         break;
01334     default:
01335         return -1;
01336     }
01337 
01338     if (st.chip_cfg.accel_fsr == (data >> 3))
01339         return 0;
01340     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
01341         return -1;
01342     st.chip_cfg.accel_fsr = data >> 3;
01343     return 0;
01344 }
01345 
01346 /**
01347  *  @brief      Get the current DLPF setting.
01348  *  @param[out] lpf Current LPF setting.
01349  *  0 if successful.
01350  */
01351 int mpu_get_lpf(unsigned short *lpf)
01352 {
01353     switch (st.chip_cfg.lpf) {
01354     case INV_FILTER_188HZ:
01355         lpf[0] = 188;
01356         break;
01357     case INV_FILTER_98HZ:
01358         lpf[0] = 98;
01359         break;
01360     case INV_FILTER_42HZ:
01361         lpf[0] = 42;
01362         break;
01363     case INV_FILTER_20HZ:
01364         lpf[0] = 20;
01365         break;
01366     case INV_FILTER_10HZ:
01367         lpf[0] = 10;
01368         break;
01369     case INV_FILTER_5HZ:
01370         lpf[0] = 5;
01371         break;
01372     case INV_FILTER_256HZ_NOLPF2:
01373     case INV_FILTER_2100HZ_NOLPF:
01374     default:
01375         lpf[0] = 0;
01376         break;
01377     }
01378     return 0;
01379 }
01380 
01381 /**
01382  *  @brief      Set digital low pass filter.
01383  *  The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
01384  *  @param[in]  lpf Desired LPF setting.
01385  *  @return     0 if successful.
01386  */
01387 int mpu_set_lpf(unsigned short lpf)
01388 {
01389     unsigned char data;
01390 
01391     if (!(st.chip_cfg.sensors))
01392         return -1;
01393 
01394     if (lpf >= 188)
01395         data = INV_FILTER_188HZ;
01396     else if (lpf >= 98)
01397         data = INV_FILTER_98HZ;
01398     else if (lpf >= 42)
01399         data = INV_FILTER_42HZ;
01400     else if (lpf >= 20)
01401         data = INV_FILTER_20HZ;
01402     else if (lpf >= 10)
01403         data = INV_FILTER_10HZ;
01404     else
01405         data = INV_FILTER_5HZ;
01406 
01407     if (st.chip_cfg.lpf == data)
01408         return 0;
01409     if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
01410         return -1;
01411     st.chip_cfg.lpf = data;
01412     return 0;
01413 }
01414 
01415 /**
01416  *  @brief      Get sampling rate.
01417  *  @param[out] rate    Current sampling rate (Hz).
01418  *  @return     0 if successful.
01419  */
01420 int mpu_get_sample_rate(unsigned short *rate)
01421 {
01422     if (st.chip_cfg.dmp_on)
01423         return -1;
01424     else
01425         rate[0] = st.chip_cfg.sample_rate;
01426     return 0;
01427 }
01428 
01429 /**
01430  *  @brief      Set sampling rate.
01431  *  Sampling rate must be between 4Hz and 1kHz.
01432  *  @param[in]  rate    Desired sampling rate (Hz).
01433  *  @return     0 if successful.
01434  */
01435 int mpu_set_sample_rate(unsigned short rate)
01436 {
01437     unsigned char data;
01438 
01439     if (!(st.chip_cfg.sensors))
01440         return -1;
01441 
01442     if (st.chip_cfg.dmp_on)
01443         return -1;
01444     else {
01445         if (st.chip_cfg.lp_accel_mode) {
01446             if (rate && (rate <= 40)) {
01447                 /* Just stay in low-power accel mode. */
01448                 mpu_lp_accel_mode(rate);
01449                 return 0;
01450             }
01451             /* Requested rate exceeds the allowed frequencies in LP accel mode,
01452              * switch back to full-power mode.
01453              */
01454             mpu_lp_accel_mode(0);
01455         }
01456         if (rate < 4)
01457             rate = 4;
01458         else if (rate > 1000)
01459             rate = 1000;
01460 
01461         data = 1000 / rate - 1;
01462         if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data))
01463             return -1;
01464 
01465         st.chip_cfg.sample_rate = 1000 / (1 + data);
01466 
01467 #ifdef AK89xx_SECONDARY
01468         mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
01469 #endif
01470 
01471         /* Automatically set LPF to 1/2 sampling rate. */
01472         mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
01473         return 0;
01474     }
01475 }
01476 
01477 /**
01478  *  @brief      Get compass sampling rate.
01479  *  @param[out] rate    Current compass sampling rate (Hz).
01480  *  @return     0 if successful.
01481  */
01482 int mpu_get_compass_sample_rate(unsigned short *rate)
01483 {
01484 #ifdef AK89xx_SECONDARY
01485     rate[0] = st.chip_cfg.compass_sample_rate;
01486     return 0;
01487 #else
01488     rate[0] = 0;
01489     return -1;
01490 #endif
01491 }
01492 
01493 /**
01494  *  @brief      Set compass sampling rate.
01495  *  The compass on the auxiliary I2C bus is read by the MPU hardware at a
01496  *  maximum of 100Hz. The actual rate can be set to a fraction of the gyro
01497  *  sampling rate.
01498  *
01499  *  \n WARNING: The new rate may be different than what was requested. Call
01500  *  mpu_get_compass_sample_rate to check the actual setting.
01501  *  @param[in]  rate    Desired compass sampling rate (Hz).
01502  *  @return     0 if successful.
01503  */
01504 int mpu_set_compass_sample_rate(unsigned short rate)
01505 {
01506 #ifdef AK89xx_SECONDARY
01507     unsigned char div;
01508     if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
01509         return -1;
01510 
01511     div = st.chip_cfg.sample_rate / rate - 1;
01512     if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
01513         return -1;
01514     st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
01515     return 0;
01516 #else
01517     return -1;
01518 #endif
01519 }
01520 
01521 /**
01522  *  @brief      Get gyro sensitivity scale factor.
01523  *  @param[out] sens    Conversion from hardware units to dps.
01524  *  @return     0 if successful.
01525  */
01526 int mpu_get_gyro_sens(float *sens)
01527 {
01528     switch (st.chip_cfg.gyro_fsr) {
01529     case INV_FSR_250DPS:
01530         sens[0] = 131.f;
01531         break;
01532     case INV_FSR_500DPS:
01533         sens[0] = 65.5f;
01534         break;
01535     case INV_FSR_1000DPS:
01536         sens[0] = 32.8f;
01537         break;
01538     case INV_FSR_2000DPS:
01539         sens[0] = 16.4f;
01540         break;
01541     default:
01542         return -1;
01543     }
01544     return 0;
01545 }
01546 
01547 /**
01548  *  @brief      Get accel sensitivity scale factor.
01549  *  @param[out] sens    Conversion from hardware units to g's.
01550  *  @return     0 if successful.
01551  */
01552 int mpu_get_accel_sens(unsigned short *sens)
01553 {
01554     switch (st.chip_cfg.accel_fsr) {
01555     case INV_FSR_2G:
01556         sens[0] = 16384;
01557         break;
01558     case INV_FSR_4G:
01559         sens[0] = 8092;
01560         break;
01561     case INV_FSR_8G:
01562         sens[0] = 4096;
01563         break;
01564     case INV_FSR_16G:
01565         sens[0] = 2048;
01566         break;
01567     default:
01568         return -1;
01569     }
01570     if (st.chip_cfg.accel_half)
01571         sens[0] >>= 1;
01572     return 0;
01573 }
01574 
01575 /**
01576  *  @brief      Get current FIFO configuration.
01577  *  @e sensors can contain a combination of the following flags:
01578  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
01579  *  \n INV_XYZ_GYRO
01580  *  \n INV_XYZ_ACCEL
01581  *  @param[out] sensors Mask of sensors in FIFO.
01582  *  @return     0 if successful.
01583  */
01584 int mpu_get_fifo_config(unsigned char *sensors)
01585 {
01586     sensors[0] = st.chip_cfg.fifo_enable;
01587     return 0;
01588 }
01589 
01590 /**
01591  *  @brief      Select which sensors are pushed to FIFO.
01592  *  @e sensors can contain a combination of the following flags:
01593  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
01594  *  \n INV_XYZ_GYRO
01595  *  \n INV_XYZ_ACCEL
01596  *  @param[in]  sensors Mask of sensors to push to FIFO.
01597  *  @return     0 if successful.
01598  */
01599 int mpu_configure_fifo(unsigned char sensors)
01600 {
01601     unsigned char prev;
01602     int result = 0;
01603 
01604     /* Compass data isn't going into the FIFO. Stop trying. */
01605     sensors &= ~INV_XYZ_COMPASS;
01606 
01607     if (st.chip_cfg.dmp_on)
01608         return 0;
01609     else {
01610         if (!(st.chip_cfg.sensors))
01611             return -1;
01612         prev = st.chip_cfg.fifo_enable;
01613         st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
01614         if (st.chip_cfg.fifo_enable != sensors)
01615             /* You're not getting what you asked for. Some sensors are
01616              * asleep.
01617              */
01618             result = -1;
01619         else
01620             result = 0;
01621         if (sensors || st.chip_cfg.lp_accel_mode)
01622             set_int_enable(1);
01623         else
01624             set_int_enable(0);
01625         if (sensors) {
01626             if (mpu_reset_fifo()) {
01627                 st.chip_cfg.fifo_enable = prev;
01628                 return -1;
01629             }
01630         }
01631     }
01632 
01633     return result;
01634 }
01635 
01636 /**
01637  *  @brief      Get current power state.
01638  *  @param[in]  power_on    1 if turned on, 0 if suspended.
01639  *  @return     0 if successful.
01640  */
01641 int mpu_get_power_state(unsigned char *power_on)
01642 {
01643     if (st.chip_cfg.sensors)
01644         power_on[0] = 1;
01645     else
01646         power_on[0] = 0;
01647     return 0;
01648 }
01649 
01650 /**
01651  *  @brief      Turn specific sensors on/off.
01652  *  @e sensors can contain a combination of the following flags:
01653  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
01654  *  \n INV_XYZ_GYRO
01655  *  \n INV_XYZ_ACCEL
01656  *  \n INV_XYZ_COMPASS
01657  *  @param[in]  sensors    Mask of sensors to wake.
01658  *  @return     0 if successful.
01659  */
01660 int mpu_set_sensors(unsigned char sensors)
01661 {
01662     unsigned char data;
01663 #ifdef AK89xx_SECONDARY
01664     unsigned char user_ctrl;
01665 #endif
01666 
01667     if (sensors & INV_XYZ_GYRO)
01668         data = INV_CLK_PLL;
01669     else if (sensors)
01670         data = 0;
01671     else
01672         data = BIT_SLEEP;
01673     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
01674         st.chip_cfg.sensors = 0;
01675         return -1;
01676     }
01677     st.chip_cfg.clk_src = data & ~BIT_SLEEP;
01678 
01679     data = 0;
01680     if (!(sensors & INV_X_GYRO))
01681         data |= BIT_STBY_XG;
01682     if (!(sensors & INV_Y_GYRO))
01683         data |= BIT_STBY_YG;
01684     if (!(sensors & INV_Z_GYRO))
01685         data |= BIT_STBY_ZG;
01686     if (!(sensors & INV_XYZ_ACCEL))
01687         data |= BIT_STBY_XYZA;
01688     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
01689         st.chip_cfg.sensors = 0;
01690         return -1;
01691     }
01692 
01693     if (sensors && (sensors != INV_XYZ_ACCEL))
01694         /* Latched interrupts only used in LP accel mode. */
01695         mpu_set_int_latched(0);
01696 
01697 #ifdef AK89xx_SECONDARY
01698 #ifdef AK89xx_BYPASS
01699     if (sensors & INV_XYZ_COMPASS)
01700         mpu_set_bypass(1);
01701     else
01702         mpu_set_bypass(0);
01703 #else
01704     if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
01705         return -1;
01706     /* Handle AKM power management. */
01707     if (sensors & INV_XYZ_COMPASS) {
01708         data = AKM_SINGLE_MEASUREMENT;
01709         user_ctrl |= BIT_AUX_IF_EN;
01710     } else {
01711         data = AKM_POWER_DOWN;
01712         user_ctrl &= ~BIT_AUX_IF_EN;
01713     }
01714     if (st.chip_cfg.dmp_on)
01715         user_ctrl |= BIT_DMP_EN;
01716     else
01717         user_ctrl &= ~BIT_DMP_EN;
01718     if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
01719         return -1;
01720     /* Enable/disable I2C master mode. */
01721     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
01722         return -1;
01723 #endif
01724 #endif
01725 
01726     st.chip_cfg.sensors = sensors;
01727     st.chip_cfg.lp_accel_mode = 0;
01728     delay_ms(50);
01729     return 0;
01730 }
01731 
01732 /**
01733  *  @brief      Read the MPU interrupt status registers.
01734  *  @param[out] status  Mask of interrupt bits.
01735  *  @return     0 if successful.
01736  */
01737 int mpu_get_int_status(short *status)
01738 {
01739     unsigned char tmp[2];
01740     if (!st.chip_cfg.sensors)
01741         return -1;
01742     if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
01743         return -1;
01744     status[0] = (tmp[0] << 8) | tmp[1];
01745     return 0;
01746 }
01747 
01748 /**
01749  *  @brief      Get one packet from the FIFO.
01750  *  If @e sensors does not contain a particular sensor, disregard the data
01751  *  returned to that pointer.
01752  *  \n @e sensors can contain a combination of the following flags:
01753  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
01754  *  \n INV_XYZ_GYRO
01755  *  \n INV_XYZ_ACCEL
01756  *  \n If the FIFO has no new data, @e sensors will be zero.
01757  *  \n If the FIFO is disabled, @e sensors will be zero and this function will
01758  *  return a non-zero error code.
01759  *  @param[out] gyro        Gyro data in hardware units.
01760  *  @param[out] accel       Accel data in hardware units.
01761  *  @param[out] timestamp   Timestamp in milliseconds.
01762  *  @param[out] sensors     Mask of sensors read from FIFO.
01763  *  @param[out] more        Number of remaining packets.
01764  *  @return     0 if successful.
01765  */
01766 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
01767         unsigned char *sensors, unsigned char *more)
01768 {
01769     /* Assumes maximum packet size is gyro (6) + accel (6). */
01770     unsigned char data[MAX_PACKET_LENGTH];
01771     unsigned char packet_size = 0;
01772     unsigned short fifo_count, index = 0;
01773 
01774     if (st.chip_cfg.dmp_on)
01775         return -1;
01776 
01777     sensors[0] = 0;
01778     if (!st.chip_cfg.sensors)
01779         return -1;
01780     if (!st.chip_cfg.fifo_enable)
01781         return -1;
01782 
01783     if (st.chip_cfg.fifo_enable & INV_X_GYRO)
01784         packet_size += 2;
01785     if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
01786         packet_size += 2;
01787     if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
01788         packet_size += 2;
01789     if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
01790         packet_size += 6;
01791 
01792     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
01793         return -1;
01794     fifo_count = (data[0] << 8) | data[1];
01795     if (fifo_count < packet_size)
01796         return 0;
01797 //    log_i("FIFO count: %hd\n", fifo_count);
01798     if (fifo_count > (st.hw->max_fifo >> 1)) {
01799         /* FIFO is 50% full, better check overflow bit. */
01800         if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
01801             return -1;
01802         if (data[0] & BIT_FIFO_OVERFLOW) {
01803             mpu_reset_fifo();
01804             return -2;
01805         }
01806     }
01807     get_ms((unsigned long*)timestamp);
01808 
01809     if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
01810         return -1;
01811     more[0] = fifo_count / packet_size - 1;
01812     sensors[0] = 0;
01813 
01814     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
01815         accel[0] = (data[index+0] << 8) | data[index+1];
01816         accel[1] = (data[index+2] << 8) | data[index+3];
01817         accel[2] = (data[index+4] << 8) | data[index+5];
01818         sensors[0] |= INV_XYZ_ACCEL;
01819         index += 6;
01820     }
01821     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
01822         gyro[0] = (data[index+0] << 8) | data[index+1];
01823         sensors[0] |= INV_X_GYRO;
01824         index += 2;
01825     }
01826     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
01827         gyro[1] = (data[index+0] << 8) | data[index+1];
01828         sensors[0] |= INV_Y_GYRO;
01829         index += 2;
01830     }
01831     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
01832         gyro[2] = (data[index+0] << 8) | data[index+1];
01833         sensors[0] |= INV_Z_GYRO;
01834         index += 2;
01835     }
01836 
01837     return 0;
01838 }
01839 
01840 /**
01841  *  @brief      Get one unparsed packet from the FIFO.
01842  *  This function should be used if the packet is to be parsed elsewhere.
01843  *  @param[in]  length  Length of one FIFO packet.
01844  *  @param[in]  data    FIFO packet.
01845  *  @param[in]  more    Number of remaining packets.
01846  */
01847 int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
01848     unsigned char *more)
01849 {
01850     unsigned char tmp[2];
01851     unsigned short fifo_count;
01852     if (!st.chip_cfg.dmp_on)
01853         return -1;
01854     if (!st.chip_cfg.sensors)
01855         return -1;
01856 
01857     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
01858         return -1;
01859     fifo_count = (tmp[0] << 8) | tmp[1];
01860     if (fifo_count < length) {
01861         more[0] = 0;
01862         return -1;
01863     }
01864     if (fifo_count > (st.hw->max_fifo >> 1)) {
01865         /* FIFO is 50% full, better check overflow bit. */
01866         if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
01867             return -1;
01868         if (tmp[0] & BIT_FIFO_OVERFLOW) {
01869             mpu_reset_fifo();
01870             return -2;
01871         }
01872     }
01873 
01874     if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
01875         return -1;
01876     more[0] = fifo_count / length - 1;
01877     return 0;
01878 }
01879 
01880 /**
01881  *  @brief      Set device to bypass mode.
01882  *  @param[in]  bypass_on   1 to enable bypass mode.
01883  *  @return     0 if successful.
01884  */
01885 int mpu_set_bypass(unsigned char bypass_on)
01886 {
01887     unsigned char tmp;
01888 
01889     if (st.chip_cfg.bypass_mode == bypass_on)
01890         return 0;
01891 
01892     if (bypass_on) {
01893         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
01894             return -1;
01895         tmp &= ~BIT_AUX_IF_EN;
01896         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
01897             return -1;
01898         delay_ms(3);
01899         tmp = BIT_BYPASS_EN;
01900         if (st.chip_cfg.active_low_int)
01901             tmp |= BIT_ACTL;
01902         if (st.chip_cfg.latched_int)
01903             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
01904         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
01905             return -1;
01906     } else {
01907         /* Enable I2C master mode if compass is being used. */
01908         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
01909             return -1;
01910         if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
01911             tmp |= BIT_AUX_IF_EN;
01912         else
01913             tmp &= ~BIT_AUX_IF_EN;
01914         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
01915             return -1;
01916         delay_ms(3);
01917         if (st.chip_cfg.active_low_int)
01918             tmp = BIT_ACTL;
01919         else
01920             tmp = 0;
01921         if (st.chip_cfg.latched_int)
01922             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
01923         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
01924             return -1;
01925     }
01926     st.chip_cfg.bypass_mode = bypass_on;
01927     return 0;
01928 }
01929 
01930 /**
01931  *  @brief      Set interrupt level.
01932  *  @param[in]  active_low  1 for active low, 0 for active high.
01933  *  @return     0 if successful.
01934  */
01935 int mpu_set_int_level(unsigned char active_low)
01936 {
01937     st.chip_cfg.active_low_int = active_low;
01938     return 0;
01939 }
01940 
01941 /**
01942  *  @brief      Enable latched interrupts.
01943  *  Any MPU register will clear the interrupt.
01944  *  @param[in]  enable  1 to enable, 0 to disable.
01945  *  @return     0 if successful.
01946  */
01947 int mpu_set_int_latched(unsigned char enable)
01948 {
01949     unsigned char tmp;
01950     if (st.chip_cfg.latched_int == enable)
01951         return 0;
01952 
01953     if (enable)
01954         tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
01955     else
01956         tmp = 0;
01957     if (st.chip_cfg.bypass_mode)
01958         tmp |= BIT_BYPASS_EN;
01959     if (st.chip_cfg.active_low_int)
01960         tmp |= BIT_ACTL;
01961     if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
01962         return -1;
01963     st.chip_cfg.latched_int = enable;
01964     return 0;
01965 }
01966 
01967 #ifdef MPU6050
01968 static int get_accel_prod_shift(float *st_shift)
01969 {
01970     unsigned char tmp[4], shift_code[3], ii;
01971 
01972     if (i2c_read(st.hw->addr, 0x0D, 4, tmp))
01973         return 0x07;
01974 
01975     shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
01976     shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
01977     shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
01978     for (ii = 0; ii < 3; ii++) {
01979         if (!shift_code[ii]) {
01980             st_shift[ii] = 0.f;
01981             continue;
01982         }
01983         /* Equivalent to..
01984          * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
01985          */
01986         st_shift[ii] = 0.34f;
01987         while (--shift_code[ii])
01988             st_shift[ii] *= 1.034f;
01989     }
01990     return 0;
01991 }
01992 
01993 static int accel_self_test(long *bias_regular, long *bias_st)
01994 {
01995     int jj, result = 0;
01996     float st_shift[3], st_shift_cust, st_shift_var;
01997 
01998     get_accel_prod_shift(st_shift);
01999     for(jj = 0; jj < 3; jj++) {
02000         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
02001         if (st_shift[jj]) {
02002             st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
02003             if (fabs(st_shift_var) > test.max_accel_var)
02004                 result |= 1 << jj;
02005         } else if ((st_shift_cust < test.min_g) ||
02006             (st_shift_cust > test.max_g))
02007             result |= 1 << jj;
02008     }
02009 
02010     return result;
02011 }
02012 
02013 static int gyro_self_test(long *bias_regular, long *bias_st)
02014 {
02015     int jj, result = 0;
02016     unsigned char tmp[3];
02017     float st_shift, st_shift_cust, st_shift_var;
02018 
02019     if (i2c_read(st.hw->addr, 0x0D, 3, tmp))
02020         return 0x07;
02021 
02022     tmp[0] &= 0x1F;
02023     tmp[1] &= 0x1F;
02024     tmp[2] &= 0x1F;
02025 
02026     for (jj = 0; jj < 3; jj++) {
02027         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
02028         if (tmp[jj]) {
02029             st_shift = 3275.f / test.gyro_sens;
02030             while (--tmp[jj])
02031                 st_shift *= 1.046f;
02032             st_shift_var = st_shift_cust / st_shift - 1.f;
02033             if (fabs(st_shift_var) > test.max_gyro_var)
02034                 result |= 1 << jj;
02035         } else if ((st_shift_cust < test.min_dps) ||
02036             (st_shift_cust > test.max_dps))
02037             result |= 1 << jj;
02038     }
02039     return result;
02040 }
02041 
02042 #endif 
02043 #ifdef AK89xx_SECONDARY
02044 static int compass_self_test(void)
02045 {
02046     unsigned char tmp[6];
02047     unsigned char tries = 10;
02048     int result = 0x07;
02049     short data;
02050 
02051     mpu_set_bypass(1);
02052 
02053     tmp[0] = AKM_POWER_DOWN;
02054     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
02055         return 0x07;
02056     tmp[0] = AKM_BIT_SELF_TEST;
02057     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
02058         goto AKM_restore;
02059     tmp[0] = AKM_MODE_SELF_TEST;
02060     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
02061         goto AKM_restore;
02062 
02063     do {
02064         delay_ms(10);
02065         if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
02066             goto AKM_restore;
02067         if (tmp[0] & AKM_DATA_READY)
02068             break;
02069     } while (tries--);
02070     if (!(tmp[0] & AKM_DATA_READY))
02071         goto AKM_restore;
02072 
02073     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
02074         goto AKM_restore;
02075 
02076     result = 0;
02077 #if defined MPU9150
02078     data = (short)(tmp[1] << 8) | tmp[0];
02079     if ((data > 100) || (data < -100))
02080         result |= 0x01;
02081     data = (short)(tmp[3] << 8) | tmp[2];
02082     if ((data > 100) || (data < -100))
02083         result |= 0x02;
02084     data = (short)(tmp[5] << 8) | tmp[4];
02085     if ((data > -300) || (data < -1000))
02086         result |= 0x04;
02087 #elif defined MPU9250
02088     data = (short)(tmp[1] << 8) | tmp[0];
02089     if ((data > 200) || (data < -200))  
02090         result |= 0x01;
02091     data = (short)(tmp[3] << 8) | tmp[2];
02092     if ((data > 200) || (data < -200))  
02093         result |= 0x02;
02094     data = (short)(tmp[5] << 8) | tmp[4];
02095     if ((data > -800) || (data < -3200))  
02096         result |= 0x04;
02097 #endif
02098 AKM_restore:
02099     tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
02100     i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
02101     tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
02102     i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
02103     mpu_set_bypass(0);
02104     return result;
02105 }
02106 #endif
02107 
02108 static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
02109 {
02110     unsigned char data[MAX_PACKET_LENGTH];
02111     unsigned char packet_count, ii;
02112     unsigned short fifo_count;
02113 
02114     data[0] = 0x01;
02115     data[1] = 0;
02116     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
02117         return -1;
02118     delay_ms(200);
02119     data[0] = 0;
02120     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
02121         return -1;
02122     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
02123         return -1;
02124     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
02125         return -1;
02126     if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
02127         return -1;
02128     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
02129         return -1;
02130     data[0] = BIT_FIFO_RST | BIT_DMP_RST;
02131     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
02132         return -1;
02133     delay_ms(15);
02134     data[0] = st.test->reg_lpf;
02135     if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
02136         return -1;
02137     data[0] = st.test->reg_rate_div;
02138     if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
02139         return -1;
02140     if (hw_test)
02141         data[0] = st.test->reg_gyro_fsr | 0xE0;
02142     else
02143         data[0] = st.test->reg_gyro_fsr;
02144     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
02145         return -1;
02146 
02147     if (hw_test)
02148         data[0] = st.test->reg_accel_fsr | 0xE0;
02149     else
02150         data[0] = test.reg_accel_fsr;
02151     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
02152         return -1;
02153     if (hw_test)
02154         delay_ms(200);
02155 
02156     /* Fill FIFO for test.wait_ms milliseconds. */
02157     data[0] = BIT_FIFO_EN;
02158     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
02159         return -1;
02160 
02161     data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
02162     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
02163         return -1;
02164     delay_ms(test.wait_ms);
02165     data[0] = 0;
02166     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
02167         return -1;
02168 
02169     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
02170         return -1;
02171 
02172     fifo_count = (data[0] << 8) | data[1];
02173     packet_count = fifo_count / MAX_PACKET_LENGTH;
02174     gyro[0] = gyro[1] = gyro[2] = 0;
02175     accel[0] = accel[1] = accel[2] = 0;
02176 
02177     for (ii = 0; ii < packet_count; ii++) {
02178         short accel_cur[3], gyro_cur[3];
02179         if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
02180             return -1;
02181         accel_cur[0] = ((short)data[0] << 8) | data[1];
02182         accel_cur[1] = ((short)data[2] << 8) | data[3];
02183         accel_cur[2] = ((short)data[4] << 8) | data[5];
02184         accel[0] += (long)accel_cur[0];
02185         accel[1] += (long)accel_cur[1];
02186         accel[2] += (long)accel_cur[2];
02187         gyro_cur[0] = (((short)data[6] << 8) | data[7]);
02188         gyro_cur[1] = (((short)data[8] << 8) | data[9]);
02189         gyro_cur[2] = (((short)data[10] << 8) | data[11]);
02190         gyro[0] += (long)gyro_cur[0];
02191         gyro[1] += (long)gyro_cur[1];
02192         gyro[2] += (long)gyro_cur[2];
02193     }
02194 #ifdef EMPL_NO_64BIT
02195     gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
02196     gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
02197     gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
02198     if (has_accel) {
02199         accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
02200             packet_count);
02201         accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
02202             packet_count);
02203         accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
02204             packet_count);
02205         /* Don't remove gravity! */
02206         accel[2] -= 65536L;
02207     }
02208 #else
02209     gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
02210     gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
02211     gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
02212     accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
02213         packet_count);
02214     accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
02215         packet_count);
02216     accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
02217         packet_count);
02218     /* Don't remove gravity! */
02219     if (accel[2] > 0L)
02220         accel[2] -= 65536L;
02221     else
02222         accel[2] += 65536L;
02223 #endif
02224 
02225     return 0;
02226 }
02227 
02228 #ifdef MPU6500
02229 #define REG_6500_XG_ST_DATA     0x0
02230 #define REG_6500_XA_ST_DATA     0xD
02231 static const unsigned short mpu_6500_st_tb[256] = {
02232     2620,2646,2672,2699,2726,2753,2781,2808, //7
02233     2837,2865,2894,2923,2952,2981,3011,3041, //15
02234     3072,3102,3133,3165,3196,3228,3261,3293, //23
02235     3326,3359,3393,3427,3461,3496,3531,3566, //31
02236     3602,3638,3674,3711,3748,3786,3823,3862, //39
02237     3900,3939,3979,4019,4059,4099,4140,4182, //47
02238     4224,4266,4308,4352,4395,4439,4483,4528, //55
02239     4574,4619,4665,4712,4759,4807,4855,4903, //63
02240     4953,5002,5052,5103,5154,5205,5257,5310, //71
02241     5363,5417,5471,5525,5581,5636,5693,5750, //79
02242     5807,5865,5924,5983,6043,6104,6165,6226, //87
02243     6289,6351,6415,6479,6544,6609,6675,6742, //95
02244     6810,6878,6946,7016,7086,7157,7229,7301, //103
02245     7374,7448,7522,7597,7673,7750,7828,7906, //111
02246     7985,8065,8145,8227,8309,8392,8476,8561, //119
02247     8647,8733,8820,8909,8998,9088,9178,9270,
02248     9363,9457,9551,9647,9743,9841,9939,10038,
02249     10139,10240,10343,10446,10550,10656,10763,10870,
02250     10979,11089,11200,11312,11425,11539,11654,11771,
02251     11889,12008,12128,12249,12371,12495,12620,12746,
02252     12874,13002,13132,13264,13396,13530,13666,13802,
02253     13940,14080,14221,14363,14506,14652,14798,14946,
02254     15096,15247,15399,15553,15709,15866,16024,16184,
02255     16346,16510,16675,16842,17010,17180,17352,17526,
02256     17701,17878,18057,18237,18420,18604,18790,18978,
02257     19167,19359,19553,19748,19946,20145,20347,20550,
02258     20756,20963,21173,21385,21598,21814,22033,22253,
02259     22475,22700,22927,23156,23388,23622,23858,24097,
02260     24338,24581,24827,25075,25326,25579,25835,26093,
02261     26354,26618,26884,27153,27424,27699,27976,28255,
02262     28538,28823,29112,29403,29697,29994,30294,30597,
02263     30903,31212,31524,31839,32157,32479,32804,33132
02264 };
02265 static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
02266 {
02267     int i, result = 0, otp_value_zero = 0;
02268     float accel_st_al_min, accel_st_al_max;
02269     float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max;
02270     unsigned char regs[3];
02271     if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) {
02272         if(debug)
02273             log_i("Reading OTP Register Error.\n");
02274         return 0x07;
02275     }
02276     if(debug)
02277         log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]);
02278     for (i = 0; i < 3; i++) {
02279         if (regs[i] != 0) {
02280             ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
02281             ct_shift_prod[i] *= 65536.f;
02282             ct_shift_prod[i] /= test.accel_sens;
02283         }
02284         else {
02285             ct_shift_prod[i] = 0;
02286             otp_value_zero = 1;
02287         }
02288     }
02289     if(otp_value_zero == 0) {
02290         if(debug)
02291             log_i("ACCEL:CRITERIA A\n");
02292         for (i = 0; i < 3; i++) {
02293             st_shift_cust[i] = bias_st[i] - bias_regular[i];
02294             if(debug) {
02295                 log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
02296                         st_shift_cust[i]/1.f, bias_regular[i]/1.f,
02297                         bias_st[i]/1.f);
02298                 log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
02299             }
02300 
02301             st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
02302 
02303             if(debug)
02304                 log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
02305                             test.max_accel_var/1.f);
02306 
02307             if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
02308                 if(debug)
02309                     log_i("ACCEL Fail Axis = %d\n", i);
02310                 result |= 1 << i;   //Error condition
02311             }
02312         }
02313     }
02314     else {
02315         /* Self Test Pass/Fail Criteria B */
02316         accel_st_al_min = test.min_g * 65536.f;
02317         accel_st_al_max = test.max_g * 65536.f;
02318 
02319         if(debug) {
02320             log_i("ACCEL:CRITERIA B\r\n");
02321             log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f);
02322             log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f);
02323         }
02324 
02325         for (i = 0; i < 3; i++) {
02326             st_shift_cust[i] = bias_st[i] - bias_regular[i];
02327 
02328             if(debug)
02329                 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);
02330             if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) {
02331                 if(debug)
02332                     log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i);
02333                 result |= 1 << i;   //Error condition
02334             }
02335         }
02336     }
02337 
02338     if(result == 0) {
02339     /* Self Test Pass/Fail Criteria C */
02340         accel_offset_max = test.max_g_offset * 65536.f;
02341         if(debug)
02342             log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f);
02343         for (i = 0; i < 3; i++) {
02344             if(fabs(bias_regular[i]) > accel_offset_max) {
02345                 if(debug)
02346                     log_i("FAILED: Accel axis:%d = %d > 500mg\n", i, bias_regular[i]);
02347                 result |= 1 << i;   //Error condition
02348             }
02349         }
02350     }
02351 
02352     return result;
02353 }
02354 
02355 static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
02356 {
02357     int i, result = 0, otp_value_zero = 0;
02358     float gyro_st_al_max;
02359     float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max;
02360     unsigned char regs[3];
02361 
02362     if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) {
02363         if(debug)
02364             log_i("Reading OTP Register Error.\n");
02365         return 0x07;
02366     }
02367 
02368     if(debug)
02369         log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]);
02370 
02371     for (i = 0; i < 3; i++) {
02372         if (regs[i] != 0) {
02373             ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
02374             ct_shift_prod[i] *= 65536.f;
02375             ct_shift_prod[i] /= test.gyro_sens;
02376         }
02377         else {
02378             ct_shift_prod[i] = 0;
02379             otp_value_zero = 1;
02380         }
02381     }
02382 
02383     if(otp_value_zero == 0) {
02384         if(debug)
02385             log_i("GYRO:CRITERIA A\n");
02386         /* Self Test Pass/Fail Criteria A */
02387         for (i = 0; i < 3; i++) {
02388             st_shift_cust[i] = bias_st[i] - bias_regular[i];
02389 
02390             if(debug) {
02391                 log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
02392                         st_shift_cust[i]/1.f, bias_regular[i]/1.f,
02393                         bias_st[i]/1.f);
02394                 log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
02395             }
02396 
02397             st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
02398 
02399             if(debug)
02400                 log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
02401                             test.max_gyro_var/1.f);
02402 
02403             if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
02404                 if(debug)
02405                     log_i("Gyro Fail Axis = %d\n", i);
02406                 result |= 1 << i;   //Error condition
02407             }
02408         }
02409     }
02410     else {
02411         /* Self Test Pass/Fail Criteria B */
02412         gyro_st_al_max = test.max_dps * 65536.f;
02413 
02414         if(debug) {
02415             log_i("GYRO:CRITERIA B\r\n");
02416             log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f);
02417         }
02418 
02419         for (i = 0; i < 3; i++) {
02420             st_shift_cust[i] = bias_st[i] - bias_regular[i];
02421 
02422             if(debug)
02423                 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);
02424             if(st_shift_cust[i] < gyro_st_al_max) {
02425                 if(debug)
02426                     log_i("GYRO FAIL axis:%d greater than 60dps\n", i);
02427                 result |= 1 << i;   //Error condition
02428             }
02429         }
02430     }
02431 
02432     if(result == 0) {
02433     /* Self Test Pass/Fail Criteria C */
02434         gyro_offset_max = test.min_dps * 65536.f;
02435         if(debug)
02436             log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f);
02437         for (i = 0; i < 3; i++) {
02438             if(fabs(bias_regular[i]) > gyro_offset_max) {
02439                 if(debug)
02440                     log_i("FAILED: Gyro axis:%d = %d > 20dps\n", i, bias_regular[i]);
02441                 result |= 1 << i;   //Error condition
02442             }
02443         }
02444     }
02445     return result;
02446 }
02447 
02448 static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, int debug)
02449 {
02450     unsigned char data[HWST_MAX_PACKET_LENGTH];
02451     unsigned char packet_count, ii;
02452     unsigned short fifo_count;
02453     int s = 0, read_size = 0, ind;
02454 
02455     data[0] = 0x01;
02456     data[1] = 0;
02457     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
02458         return -1;
02459     delay_ms(200);
02460     data[0] = 0;
02461     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
02462         return -1;
02463     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
02464         return -1;
02465     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
02466         return -1;
02467     if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
02468         return -1;
02469     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
02470         return -1;
02471     data[0] = BIT_FIFO_RST | BIT_DMP_RST;
02472     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
02473         return -1;
02474     delay_ms(15);
02475     data[0] = st.test->reg_lpf;
02476     if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
02477         return -1;
02478     data[0] = st.test->reg_rate_div;
02479     if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
02480         return -1;
02481     if (hw_test)
02482         data[0] = st.test->reg_gyro_fsr | 0xE0;
02483     else
02484         data[0] = st.test->reg_gyro_fsr;
02485     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
02486         return -1;
02487 
02488     if (hw_test)
02489         data[0] = st.test->reg_accel_fsr | 0xE0;
02490     else
02491         data[0] = test.reg_accel_fsr;
02492     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
02493         return -1;
02494 
02495     delay_ms(test.wait_ms);  //wait 200ms for sensors to stabilize
02496 
02497     /* Enable FIFO */
02498     data[0] = BIT_FIFO_EN;
02499     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
02500         return -1;
02501     data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
02502     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
02503         return -1;
02504 
02505     //initialize the bias return values
02506     gyro[0] = gyro[1] = gyro[2] = 0;
02507     accel[0] = accel[1] = accel[2] = 0;
02508 
02509     if(debug)
02510         log_i("Starting Bias Loop Reads\n");
02511 
02512     //start reading samples
02513     while (s < test.packet_thresh) {
02514         delay_ms(test.sample_wait_ms); //wait 10ms to fill FIFO
02515         if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
02516             return -1;
02517         fifo_count = (data[0] << 8) | data[1];
02518         packet_count = fifo_count / MAX_PACKET_LENGTH;
02519         if ((test.packet_thresh - s) < packet_count)
02520                     packet_count = test.packet_thresh - s;
02521         read_size = packet_count * MAX_PACKET_LENGTH;
02522 
02523         //burst read from FIFO
02524         if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data))
02525                         return -1;
02526         ind = 0;
02527         for (ii = 0; ii < packet_count; ii++) {
02528             short accel_cur[3], gyro_cur[3];
02529             accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1];
02530             accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3];
02531             accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5];
02532             accel[0] += (long)accel_cur[0];
02533             accel[1] += (long)accel_cur[1];
02534             accel[2] += (long)accel_cur[2];
02535             gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]);
02536             gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]);
02537             gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]);
02538             gyro[0] += (long)gyro_cur[0];
02539             gyro[1] += (long)gyro_cur[1];
02540             gyro[2] += (long)gyro_cur[2];
02541             ind += MAX_PACKET_LENGTH;
02542         }
02543         s += packet_count;
02544     }
02545 
02546     if(debug)
02547         log_i("Samples: %d\n", s);
02548 
02549     //stop FIFO
02550     data[0] = 0;
02551     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
02552         return -1;
02553 
02554     gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s);
02555     gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s);
02556     gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s);
02557     accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s);
02558     accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s);
02559     accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s);
02560     /* remove gravity from bias calculation */
02561     if (accel[2] > 0L)
02562         accel[2] -= 65536L;
02563     else
02564         accel[2] += 65536L;
02565 
02566 
02567     if(debug) {
02568         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);
02569         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);
02570     }
02571 
02572     return 0;
02573 }
02574 /**
02575  *  @brief      Trigger gyro/accel/compass self-test for MPU6500/MPU9250
02576  *  On success/error, the self-test returns a mask representing the sensor(s)
02577  *  that failed. For each bit, a one (1) represents a "pass" case; conversely,
02578  *  a zero (0) indicates a failure.
02579  *
02580  *  \n The mask is defined as follows:
02581  *  \n Bit 0:   Gyro.
02582  *  \n Bit 1:   Accel.
02583  *  \n Bit 2:   Compass.
02584  *
02585  *  @param[out] gyro        Gyro biases in q16 format.
02586  *  @param[out] accel       Accel biases (if applicable) in q16 format.
02587  *  @param[in]  debug       Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver.
02588  *  @return     Result mask (see above).
02589  */
02590 int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
02591 {
02592     const unsigned char tries = 2;
02593     long gyro_st[3], accel_st[3];
02594     unsigned char accel_result, gyro_result;
02595 #ifdef AK89xx_SECONDARY
02596     unsigned char compass_result;
02597 #endif
02598     int ii;
02599 
02600     int result;
02601     unsigned char accel_fsr, fifo_sensors, sensors_on;
02602     unsigned short gyro_fsr, sample_rate, lpf;
02603     unsigned char dmp_was_on;
02604 
02605 
02606 
02607     if(debug)
02608         log_i("Starting MPU6500 HWST!\r\n");
02609 
02610     if (st.chip_cfg.dmp_on) {
02611         mpu_set_dmp_state(0);
02612         dmp_was_on = 1;
02613     } else
02614         dmp_was_on = 0;
02615 
02616     /* Get initial settings. */
02617     mpu_get_gyro_fsr(&gyro_fsr);
02618     mpu_get_accel_fsr(&accel_fsr);
02619     mpu_get_lpf(&lpf);
02620     mpu_get_sample_rate(&sample_rate);
02621     sensors_on = st.chip_cfg.sensors;
02622     mpu_get_fifo_config(&fifo_sensors);
02623 
02624     if(debug)
02625         log_i("Retrieving Biases\r\n");
02626 
02627     for (ii = 0; ii < tries; ii++)
02628         if (!get_st_6500_biases(gyro, accel, 0, debug))
02629             break;
02630     if (ii == tries) {
02631         /* If we reach this point, we most likely encountered an I2C error.
02632          * We'll just report an error for all three sensors.
02633          */
02634         if(debug)
02635             log_i("Retrieving Biases Error - possible I2C error\n");
02636 
02637         result = 0;
02638         goto restore;
02639     }
02640 
02641     if(debug)
02642         log_i("Retrieving ST Biases\n");
02643 
02644     for (ii = 0; ii < tries; ii++)
02645         if (!get_st_6500_biases(gyro_st, accel_st, 1, debug))
02646             break;
02647     if (ii == tries) {
02648 
02649         if(debug)
02650             log_i("Retrieving ST Biases Error - possible I2C error\n");
02651 
02652         /* Again, probably an I2C error. */
02653         result = 0;
02654         goto restore;
02655     }
02656 
02657     accel_result = accel_6500_self_test(accel, accel_st, debug);
02658     if(debug)
02659         log_i("Accel Self Test Results: %d\n", accel_result);
02660 
02661     gyro_result = gyro_6500_self_test(gyro, gyro_st, debug);
02662     if(debug)
02663         log_i("Gyro Self Test Results: %d\n", gyro_result);
02664 
02665     result = 0;
02666     if (!gyro_result)
02667         result |= 0x01;
02668     if (!accel_result)
02669         result |= 0x02;
02670 
02671 #ifdef AK89xx_SECONDARY
02672     compass_result = compass_self_test();
02673     if(debug)
02674         log_i("Compass Self Test Results: %d\n", compass_result);
02675     if (!compass_result)
02676         result |= 0x04;
02677 #else
02678     result |= 0x04;
02679 #endif
02680 restore:
02681     if(debug)
02682         log_i("Exiting HWST\n");
02683     /* Set to invalid values to ensure no I2C writes are skipped. */
02684     st.chip_cfg.gyro_fsr = 0xFF;
02685     st.chip_cfg.accel_fsr = 0xFF;
02686     st.chip_cfg.lpf = 0xFF;
02687     st.chip_cfg.sample_rate = 0xFFFF;
02688     st.chip_cfg.sensors = 0xFF;
02689     st.chip_cfg.fifo_enable = 0xFF;
02690     st.chip_cfg.clk_src = INV_CLK_PLL;
02691     mpu_set_gyro_fsr(gyro_fsr);
02692     mpu_set_accel_fsr(accel_fsr);
02693     mpu_set_lpf(lpf);
02694     mpu_set_sample_rate(sample_rate);
02695     mpu_set_sensors(sensors_on);
02696     mpu_configure_fifo(fifo_sensors);
02697 
02698     if (dmp_was_on)
02699         mpu_set_dmp_state(1);
02700 
02701     return result;
02702 }
02703 #endif
02704  /*
02705  *  \n This function must be called with the device either face-up or face-down
02706  *  (z-axis is parallel to gravity).
02707  *  @param[out] gyro        Gyro biases in q16 format.
02708  *  @param[out] accel       Accel biases (if applicable) in q16 format.
02709  *  @return     Result mask (see above).
02710  */
02711 int mpu_run_self_test(long *gyro, long *accel)
02712 {
02713 #ifdef MPU6050
02714     const unsigned char tries = 2;
02715     long gyro_st[3], accel_st[3];
02716     unsigned char accel_result, gyro_result;
02717 #ifdef AK89xx_SECONDARY
02718     unsigned char compass_result;
02719 #endif
02720     int ii;
02721 #endif
02722     int result;
02723     unsigned char accel_fsr, fifo_sensors, sensors_on;
02724     unsigned short gyro_fsr, sample_rate, lpf;
02725     unsigned char dmp_was_on;
02726 
02727     if (st.chip_cfg.dmp_on) {
02728         mpu_set_dmp_state(0);
02729         dmp_was_on = 1;
02730     } else
02731         dmp_was_on = 0;
02732 
02733     /* Get initial settings. */
02734     mpu_get_gyro_fsr(&gyro_fsr);
02735     mpu_get_accel_fsr(&accel_fsr);
02736     mpu_get_lpf(&lpf);
02737     mpu_get_sample_rate(&sample_rate);
02738     sensors_on = st.chip_cfg.sensors;
02739     mpu_get_fifo_config(&fifo_sensors);
02740 
02741     /* For older chips, the self-test will be different. */
02742 #if defined MPU6050
02743     for (ii = 0; ii < tries; ii++)
02744         if (!get_st_biases(gyro, accel, 0))
02745             break;
02746     if (ii == tries) {
02747         /* If we reach this point, we most likely encountered an I2C error.
02748          * We'll just report an error for all three sensors.
02749          */
02750         result = 0;
02751         goto restore;
02752     }
02753     for (ii = 0; ii < tries; ii++)
02754         if (!get_st_biases(gyro_st, accel_st, 1))
02755             break;
02756     if (ii == tries) {
02757         /* Again, probably an I2C error. */
02758         result = 0;
02759         goto restore;
02760     }
02761     accel_result = accel_self_test(accel, accel_st);
02762     gyro_result = gyro_self_test(gyro, gyro_st);
02763 
02764     result = 0;
02765     if (!gyro_result)
02766         result |= 0x01;
02767     if (!accel_result)
02768         result |= 0x02;
02769 
02770 #ifdef AK89xx_SECONDARY
02771     compass_result = compass_self_test();
02772     if (!compass_result)
02773         result |= 0x04;
02774 #else
02775         result |= 0x04;
02776 #endif
02777 restore:
02778 #elif defined MPU6500
02779     /* For now, this function will return a "pass" result for all three sensors
02780      * for compatibility with current test applications.
02781      */
02782     get_st_biases(gyro, accel, 0);
02783     result = 0x7;
02784 #endif
02785     /* Set to invalid values to ensure no I2C writes are skipped. */
02786     st.chip_cfg.gyro_fsr = 0xFF;
02787     st.chip_cfg.accel_fsr = 0xFF;
02788     st.chip_cfg.lpf = 0xFF;
02789     st.chip_cfg.sample_rate = 0xFFFF;
02790     st.chip_cfg.sensors = 0xFF;
02791     st.chip_cfg.fifo_enable = 0xFF;
02792     st.chip_cfg.clk_src = INV_CLK_PLL;
02793     mpu_set_gyro_fsr(gyro_fsr);
02794     mpu_set_accel_fsr(accel_fsr);
02795     mpu_set_lpf(lpf);
02796     mpu_set_sample_rate(sample_rate);
02797     mpu_set_sensors(sensors_on);
02798     mpu_configure_fifo(fifo_sensors);
02799 
02800     if (dmp_was_on)
02801         mpu_set_dmp_state(1);
02802 
02803     return result;
02804 }
02805 
02806 /**
02807  *  @brief      Write to the DMP memory.
02808  *  This function prevents I2C writes past the bank boundaries. The DMP memory
02809  *  is only accessible when the chip is awake.
02810  *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
02811  *  @param[in]  length      Number of bytes to write.
02812  *  @param[in]  data        Bytes to write to memory.
02813  *  @return     0 if successful.
02814  */
02815 int mpu_write_mem(unsigned short mem_addr, unsigned short length,
02816         unsigned char *data)
02817 {
02818     unsigned char tmp[2];
02819 
02820     if (!data)
02821         return -1;
02822     if (!st.chip_cfg.sensors)
02823         return -1;
02824 
02825     tmp[0] = (unsigned char)(mem_addr >> 8);
02826     tmp[1] = (unsigned char)(mem_addr & 0xFF);
02827 
02828     /* Check bank boundaries. */
02829     if (tmp[1] + length > st.hw->bank_size)
02830         return -1;
02831 
02832     if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
02833         return -1;
02834     if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
02835         return -1;
02836     return 0;
02837 }
02838 
02839 /**
02840  *  @brief      Read from the DMP memory.
02841  *  This function prevents I2C reads past the bank boundaries. The DMP memory
02842  *  is only accessible when the chip is awake.
02843  *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
02844  *  @param[in]  length      Number of bytes to read.
02845  *  @param[out] data        Bytes read from memory.
02846  *  @return     0 if successful.
02847  */
02848 int mpu_read_mem(unsigned short mem_addr, unsigned short length,
02849         unsigned char *data)
02850 {
02851     unsigned char tmp[2];
02852 
02853     if (!data)
02854         return -1;
02855     if (!st.chip_cfg.sensors)
02856         return -1;
02857 
02858     tmp[0] = (unsigned char)(mem_addr >> 8);
02859     tmp[1] = (unsigned char)(mem_addr & 0xFF);
02860 
02861     /* Check bank boundaries. */
02862     if (tmp[1] + length > st.hw->bank_size)
02863         return -1;
02864 
02865     if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
02866         return -1;
02867     if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
02868         return -1;
02869     return 0;
02870 }
02871 
02872 /**
02873  *  @brief      Load and verify DMP image.
02874  *  @param[in]  length      Length of DMP image.
02875  *  @param[in]  firmware    DMP code.
02876  *  @param[in]  start_addr  Starting address of DMP code memory.
02877  *  @param[in]  sample_rate Fixed sampling rate used when DMP is enabled.
02878  *  @return     0 if successful.
02879  */
02880 int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
02881     unsigned short start_addr, unsigned short sample_rate)
02882 {
02883     unsigned short ii;
02884     unsigned short this_write;
02885     /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
02886 #define LOAD_CHUNK  (16)
02887     static unsigned char cur[LOAD_CHUNK], tmp[2];
02888 
02889     if (st.chip_cfg.dmp_loaded)
02890         /* DMP should only be loaded once. */
02891         return -1;
02892 
02893     if (!firmware)
02894         return -1;
02895     for (ii = 0; ii < length; ii += this_write) {
02896         this_write = min(LOAD_CHUNK, length - ii);
02897         if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
02898             return -1;
02899         if (mpu_read_mem(ii, this_write, cur))
02900             return -1;
02901         if (memcmp(firmware+ii, cur, this_write))
02902             return -2;
02903     }
02904 
02905     /* Set program start address. */
02906     tmp[0] = start_addr >> 8;
02907     tmp[1] = start_addr & 0xFF;
02908     if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
02909         return -1;
02910 
02911     st.chip_cfg.dmp_loaded = 1;
02912     st.chip_cfg.dmp_sample_rate = sample_rate;
02913     return 0;
02914 }
02915 
02916 /**
02917  *  @brief      Enable/disable DMP support.
02918  *  @param[in]  enable  1 to turn on the DMP.
02919  *  @return     0 if successful.
02920  */
02921 int mpu_set_dmp_state(unsigned char enable)
02922 {
02923     unsigned char tmp;
02924     if (st.chip_cfg.dmp_on == enable)
02925         return 0;
02926 
02927     if (enable) {
02928         if (!st.chip_cfg.dmp_loaded)
02929             return -1;
02930         /* Disable data ready interrupt. */
02931         set_int_enable(0);
02932         /* Disable bypass mode. */
02933         mpu_set_bypass(0);
02934         /* Keep constant sample rate, FIFO rate controlled by DMP. */
02935         mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
02936         /* Remove FIFO elements. */
02937         tmp = 0;
02938         i2c_write(st.hw->addr, 0x23, 1, &tmp);
02939         st.chip_cfg.dmp_on = 1;
02940         /* Enable DMP interrupt. */
02941         set_int_enable(1);
02942         mpu_reset_fifo();
02943     } else {
02944         /* Disable DMP interrupt. */
02945         set_int_enable(0);
02946         /* Restore FIFO settings. */
02947         tmp = st.chip_cfg.fifo_enable;
02948         i2c_write(st.hw->addr, 0x23, 1, &tmp);
02949         st.chip_cfg.dmp_on = 0;
02950         mpu_reset_fifo();
02951     }
02952     return 0;
02953 }
02954 
02955 /**
02956  *  @brief      Get DMP state.
02957  *  @param[out] enabled 1 if enabled.
02958  *  @return     0 if successful.
02959  */
02960 int mpu_get_dmp_state(unsigned char *enabled)
02961 {
02962     enabled[0] = st.chip_cfg.dmp_on;
02963     return 0;
02964 }
02965 
02966 #ifdef AK89xx_SECONDARY
02967 /* This initialization is similar to the one in ak8975.c. */
02968 static int setup_compass(void)
02969 {
02970     unsigned char data[4], akm_addr;
02971 
02972     mpu_set_bypass(1);
02973 
02974     /* Find compass. Possible addresses range from 0x0C to 0x0F. */
02975     for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
02976         int result;
02977         result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
02978         if (!result && (data[0] == AKM_WHOAMI))
02979             break;
02980     }
02981 
02982     if (akm_addr > 0x0F) {
02983         /* TODO: Handle this case in all compass-related functions. */
02984         log_e("Compass not found.\n");
02985         return -1;
02986     }
02987 
02988     st.chip_cfg.compass_addr = akm_addr;
02989 
02990     data[0] = AKM_POWER_DOWN;
02991     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
02992         return -1;
02993     delay_ms(1);
02994 
02995     data[0] = AKM_FUSE_ROM_ACCESS;
02996     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
02997         return -1;
02998     delay_ms(1);
02999 
03000     /* Get sensitivity adjustment data from fuse ROM. */
03001     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
03002         return -1;
03003     st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
03004     st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
03005     st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
03006 
03007     data[0] = AKM_POWER_DOWN;
03008     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
03009         return -1;
03010     delay_ms(1);
03011 
03012     mpu_set_bypass(0);
03013 
03014     /* Set up master mode, master clock, and ES bit. */
03015     data[0] = 0x40;
03016     if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
03017         return -1;
03018 
03019     /* Slave 0 reads from AKM data registers. */
03020     data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
03021     if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
03022         return -1;
03023 
03024     /* Compass reads start at this register. */
03025     data[0] = AKM_REG_ST1;
03026     if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
03027         return -1;
03028 
03029     /* Enable slave 0, 8-byte reads. */
03030     data[0] = BIT_SLAVE_EN | 8;
03031     if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
03032         return -1;
03033 
03034     /* Slave 1 changes AKM measurement mode. */
03035     data[0] = st.chip_cfg.compass_addr;
03036     if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
03037         return -1;
03038 
03039     /* AKM measurement mode register. */
03040     data[0] = AKM_REG_CNTL;
03041     if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
03042         return -1;
03043 
03044     /* Enable slave 1, 1-byte writes. */
03045     data[0] = BIT_SLAVE_EN | 1;
03046     if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
03047         return -1;
03048 
03049     /* Set slave 1 data. */
03050     data[0] = AKM_SINGLE_MEASUREMENT;
03051     if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
03052         return -1;
03053 
03054     /* Trigger slave 0 and slave 1 actions at each sample. */
03055     data[0] = 0x03;
03056     if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
03057         return -1;
03058 
03059 #ifdef MPU9150
03060     /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
03061     data[0] = BIT_I2C_MST_VDDIO;
03062     if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
03063         return -1;
03064 #endif
03065 
03066     return 0;
03067 }
03068 #endif
03069 
03070 /**
03071  *  @brief      Read raw compass data.
03072  *  @param[out] data        Raw data in hardware units.
03073  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
03074  *  @return     0 if successful.
03075  */
03076 int mpu_get_compass_reg(short *data, unsigned long *timestamp)
03077 {
03078 #ifdef AK89xx_SECONDARY
03079     unsigned char tmp[9];
03080 
03081     if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
03082         return -1;
03083 
03084 #ifdef AK89xx_BYPASS
03085     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
03086         return -1;
03087     tmp[8] = AKM_SINGLE_MEASUREMENT;
03088     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
03089         return -1;
03090 #else
03091     if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
03092         return -1;
03093 #endif
03094 
03095 #if defined AK8975_SECONDARY
03096     /* AK8975 doesn't have the overrun error bit. */
03097     if (!(tmp[0] & AKM_DATA_READY))
03098         return -2;
03099     if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
03100         return -3;
03101 #elif defined AK8963_SECONDARY
03102     /* AK8963 doesn't have the data read error bit. */
03103     if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
03104         return -2;
03105     if (tmp[7] & AKM_OVERFLOW)
03106         return -3;
03107 #endif
03108     data[0] = (tmp[2] << 8) | tmp[1];
03109     data[1] = (tmp[4] << 8) | tmp[3];
03110     data[2] = (tmp[6] << 8) | tmp[5];
03111 
03112     data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
03113     data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
03114     data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
03115 
03116     if (timestamp)
03117         get_ms(timestamp);
03118     return 0;
03119 #else
03120     return -1;
03121 #endif
03122 }
03123 
03124 /**
03125  *  @brief      Get the compass full-scale range.
03126  *  @param[out] fsr Current full-scale range.
03127  *  @return     0 if successful.
03128  */
03129 int mpu_get_compass_fsr(unsigned short *fsr)
03130 {
03131 #ifdef AK89xx_SECONDARY
03132     fsr[0] = st.hw->compass_fsr;
03133     return 0;
03134 #else
03135     return -1;
03136 #endif
03137 }
03138 
03139 /**
03140  *  @brief      Enters LP accel motion interrupt mode.
03141  *  The behaviour of this feature is very different between the MPU6050 and the
03142  *  MPU6500. Each chip's version of this feature is explained below.
03143  *
03144  *  \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
03145  *  increments.
03146  *
03147  *  \n Low-power accel mode supports the following frequencies:
03148  *  \n 1.25Hz, 5Hz, 20Hz, 40Hz
03149  *
03150  *  \n MPU6500:
03151  *  \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
03152  *  sample. The hardware monitors the accel data and detects any large change
03153  *  over a short period of time.
03154  *
03155  *  \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
03156  *  increments.
03157  *
03158  *  \n MPU6500 Low-power accel mode supports the following frequencies:
03159  *  \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
03160  *
03161  *  \n\n NOTES:
03162  *  \n The driver will round down @e thresh to the nearest supported value if
03163  *  an unsupported threshold is selected.
03164  *  \n To select a fractional wake-up frequency, round down the value passed to
03165  *  @e lpa_freq.
03166  *  \n The MPU6500 does not support a delay parameter. If this function is used
03167  *  for the MPU6500, the value passed to @e time will be ignored.
03168  *  \n To disable this mode, set @e lpa_freq to zero. The driver will restore
03169  *  the previous configuration.
03170  *
03171  *  @param[in]  thresh      Motion threshold in mg.
03172  *  @param[in]  time        Duration in milliseconds that the accel data must
03173  *                          exceed @e thresh before motion is reported.
03174  *  @param[in]  lpa_freq    Minimum sampling rate, or zero to disable.
03175  *  @return     0 if successful.
03176  */
03177 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
03178     unsigned char lpa_freq)
03179 {
03180 
03181 #if defined MPU6500
03182     unsigned char data[3];
03183 #endif
03184     if (lpa_freq) {
03185 #if defined MPU6500
03186         unsigned char thresh_hw;
03187 
03188         /* 1LSb = 4mg. */
03189         if (thresh > 1020)
03190             thresh_hw = 255;
03191         else if (thresh < 4)
03192             thresh_hw = 1;
03193         else
03194             thresh_hw = thresh >> 2;
03195 #endif
03196 
03197         if (!time)
03198             /* Minimum duration must be 1ms. */
03199             time = 1;
03200 
03201 #if defined MPU6500
03202         if (lpa_freq > 640)
03203             /* At this point, the chip has not been re-configured, so the
03204              * function can safely exit.
03205              */
03206             return -1;
03207 #endif
03208 
03209         if (!st.chip_cfg.int_motion_only) {
03210             /* Store current settings for later. */
03211             if (st.chip_cfg.dmp_on) {
03212                 mpu_set_dmp_state(0);
03213                 st.chip_cfg.cache.dmp_on = 1;
03214             } else
03215                 st.chip_cfg.cache.dmp_on = 0;
03216             mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
03217             mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
03218             mpu_get_lpf(&st.chip_cfg.cache.lpf);
03219             mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
03220             st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
03221             mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
03222         }
03223 
03224 #if defined MPU6500
03225         /* Disable hardware interrupts. */
03226         set_int_enable(0);
03227 
03228         /* Enter full-power accel-only mode, no FIFO/DMP. */
03229         data[0] = 0;
03230         data[1] = 0;
03231         data[2] = BIT_STBY_XYZG;
03232         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))
03233             goto lp_int_restore;
03234 
03235         /* Set motion threshold. */
03236         data[0] = thresh_hw;
03237         if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))
03238             goto lp_int_restore;
03239 
03240         /* Set wake frequency. */
03241         if (lpa_freq == 1)
03242             data[0] = INV_LPA_1_25HZ;
03243         else if (lpa_freq == 2)
03244             data[0] = INV_LPA_2_5HZ;
03245         else if (lpa_freq <= 5)
03246             data[0] = INV_LPA_5HZ;
03247         else if (lpa_freq <= 10)
03248             data[0] = INV_LPA_10HZ;
03249         else if (lpa_freq <= 20)
03250             data[0] = INV_LPA_20HZ;
03251         else if (lpa_freq <= 40)
03252             data[0] = INV_LPA_40HZ;
03253         else if (lpa_freq <= 80)
03254             data[0] = INV_LPA_80HZ;
03255         else if (lpa_freq <= 160)
03256             data[0] = INV_LPA_160HZ;
03257         else if (lpa_freq <= 320)
03258             data[0] = INV_LPA_320HZ;
03259         else
03260             data[0] = INV_LPA_640HZ;
03261         if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
03262             goto lp_int_restore;
03263 
03264         /* Enable motion interrupt (MPU6500 version). */
03265         data[0] = BITS_WOM_EN;
03266         if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
03267             goto lp_int_restore;
03268 
03269         /* Enable cycle mode. */
03270         data[0] = BIT_LPA_CYCLE;
03271         if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
03272             goto lp_int_restore;
03273 
03274         /* Enable interrupt. */
03275         data[0] = BIT_MOT_INT_EN;
03276         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
03277             goto lp_int_restore;
03278 
03279         st.chip_cfg.int_motion_only = 1;
03280         return 0;
03281 #endif
03282     } else {
03283         /* Don't "restore" the previous state if no state has been saved. */
03284         int ii;
03285         char *cache_ptr = (char*)&st.chip_cfg.cache;
03286         for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
03287             if (cache_ptr[ii] != 0)
03288                 goto lp_int_restore;
03289         }
03290         /* If we reach this point, motion interrupt mode hasn't been used yet. */
03291         return -1;
03292     }
03293 lp_int_restore:
03294     /* Set to invalid values to ensure no I2C writes are skipped. */
03295     st.chip_cfg.gyro_fsr = 0xFF;
03296     st.chip_cfg.accel_fsr = 0xFF;
03297     st.chip_cfg.lpf = 0xFF;
03298     st.chip_cfg.sample_rate = 0xFFFF;
03299     st.chip_cfg.sensors = 0xFF;
03300     st.chip_cfg.fifo_enable = 0xFF;
03301     st.chip_cfg.clk_src = INV_CLK_PLL;
03302     mpu_set_sensors(st.chip_cfg.cache.sensors_on);
03303     mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
03304     mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
03305     mpu_set_lpf(st.chip_cfg.cache.lpf);
03306     mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
03307     mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
03308 
03309     if (st.chip_cfg.cache.dmp_on)
03310         mpu_set_dmp_state(1);
03311 
03312 #ifdef MPU6500
03313     /* Disable motion interrupt (MPU6500 version). */
03314     data[0] = 0;
03315     if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
03316         goto lp_int_restore;
03317 #endif
03318 
03319     st.chip_cfg.int_motion_only = 0;
03320     return 0;
03321 }
03322 
03323 /**
03324  *  @}
03325  */
03326