inv_mpu.cpp
Go to the documentation of this file.
00001 /*
00002  $License:
00003     Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
00004     See included License.txt for License information.
00005  $
00006  */
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 
00040 #define i2c_write   !I2Cdev::writeBytes
00041 #define i2c_read    !I2Cdev::readBytes
00042 #define delay_ms    delay
00043 
00044 static inline int reg_int_cb(struct int_param_s *int_param)
00045 {
00046 }
00047 #define min(a,b) ((a<b)?a:b)
00048 
00049 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
00050 #error  Which gyro are you using? Define MPUxxxx in your compiler options.
00051 #endif
00052 
00053 /* Time for some messy macro work. =]
00054  * #define MPU9150
00055  * is equivalent to..
00056  * #define MPU6050
00057  * #define AK8975_SECONDARY
00058  *
00059  * #define MPU9250
00060  * is equivalent to..
00061  * #define MPU6500
00062  * #define AK8963_SECONDARY
00063  */
00064 #if defined MPU9150
00065 #ifndef MPU6050
00066 #define MPU6050
00067 #endif                          /* #ifndef MPU6050 */
00068 #if defined AK8963_SECONDARY
00069 #error "MPU9150 and AK8963_SECONDARY cannot both be defined."
00070 #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
00071 #define AK8975_SECONDARY
00072 #endif                          /* #if defined AK8963_SECONDARY */
00073 #elif defined MPU9250           /* #if defined MPU9150 */
00074 #ifndef MPU6500
00075 #define MPU6500
00076 #endif                          /* #ifndef MPU6500 */
00077 #if defined AK8975_SECONDARY
00078 #error "MPU9250 and AK8975_SECONDARY cannot both be defined."
00079 #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
00080 #define AK8963_SECONDARY
00081 #endif                          /* #if defined AK8975_SECONDARY */
00082 #endif                          /* #if defined MPU9150 */
00083 
00084 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY
00085 #define AK89xx_SECONDARY
00086 #else
00087 /* #warning "No compass = less profit for Invensense. Lame." */
00088 #endif
00089 
00090 static int set_int_enable(unsigned char enable);
00091 
00092 /* Hardware registers needed by driver. */
00093 struct gyro_reg_s {
00094     unsigned char who_am_i;
00095     unsigned char rate_div;
00096     unsigned char lpf;
00097     unsigned char prod_id;
00098     unsigned char user_ctrl;
00099     unsigned char fifo_en;
00100     unsigned char gyro_cfg;
00101     unsigned char accel_cfg;
00102     unsigned char accel_cfg2;
00103     unsigned char lp_accel_odr;
00104     unsigned char motion_thr;
00105     unsigned char motion_dur;
00106     unsigned char fifo_count_h;
00107     unsigned char fifo_r_w;
00108     unsigned char raw_gyro;
00109     unsigned char raw_accel;
00110     unsigned char temp;
00111     unsigned char int_enable;
00112     unsigned char dmp_int_status;
00113     unsigned char int_status;
00114     unsigned char accel_intel;
00115     unsigned char pwr_mgmt_1;
00116     unsigned char pwr_mgmt_2;
00117     unsigned char int_pin_cfg;
00118     unsigned char mem_r_w;
00119     unsigned char accel_offs;
00120     unsigned char i2c_mst;
00121     unsigned char bank_sel;
00122     unsigned char mem_start_addr;
00123     unsigned char prgm_start_h;
00124 #if defined AK89xx_SECONDARY
00125     unsigned char s0_addr;
00126     unsigned char s0_reg;
00127     unsigned char s0_ctrl;
00128     unsigned char s1_addr;
00129     unsigned char s1_reg;
00130     unsigned char s1_ctrl;
00131     unsigned char s4_ctrl;
00132     unsigned char s0_do;
00133     unsigned char s1_do;
00134     unsigned char i2c_delay_ctrl;
00135     unsigned char raw_compass;
00136     /* The I2C_MST_VDDIO bit is in this register. */
00137     unsigned char yg_offs_tc;
00138 #endif
00139 };
00140 
00141 /* Information specific to a particular device. */
00142 struct hw_s {
00143     unsigned char addr;
00144     unsigned short max_fifo;
00145     unsigned char num_reg;
00146     unsigned short temp_sens;
00147     short temp_offset;
00148     unsigned short bank_size;
00149 #if defined AK89xx_SECONDARY
00150     unsigned short compass_fsr;
00151 #endif
00152 };
00153 
00154 /* When entering motion interrupt mode, the driver keeps track of the
00155  * previous state so that it can be restored at a later time.
00156  * TODO: This is tacky. Fix it.
00157  */
00158 struct motion_int_cache_s {
00159     unsigned short gyro_fsr;
00160     unsigned char accel_fsr;
00161     unsigned short lpf;
00162     unsigned short sample_rate;
00163     unsigned char sensors_on;
00164     unsigned char fifo_sensors;
00165     unsigned char dmp_on;
00166 };
00167 
00168 /* Cached chip configuration data.
00169  * TODO: A lot of these can be handled with a bitmask.
00170  */
00171 struct chip_cfg_s {
00172     /* Matches gyro_cfg >> 3 & 0x03 */
00173     unsigned char gyro_fsr;
00174     /* Matches accel_cfg >> 3 & 0x03 */
00175     unsigned char accel_fsr;
00176     /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
00177     unsigned char sensors;
00178     /* Matches config register. */
00179     unsigned char lpf;
00180     unsigned char clk_src;
00181     /* Sample rate, NOT rate divider. */
00182     unsigned short sample_rate;
00183     /* Matches fifo_en register. */
00184     unsigned char fifo_enable;
00185     /* Matches int enable register. */
00186     unsigned char int_enable;
00187     /* 1 if devices on auxiliary I2C bus appear on the primary. */
00188     unsigned char bypass_mode;
00189     /* 1 if half-sensitivity.
00190      * NOTE: This doesn't belong here, but everything else in hw_s is const,
00191      * and this allows us to save some precious RAM.
00192      */
00193     unsigned char accel_half;
00194     /* 1 if device in low-power accel-only mode. */
00195     unsigned char lp_accel_mode;
00196     /* 1 if interrupts are only triggered on motion events. */
00197     unsigned char int_motion_only;
00198     struct motion_int_cache_s cache;
00199     /* 1 for active low interrupts. */
00200     unsigned char active_low_int;
00201     /* 1 for latched interrupts. */
00202     unsigned char latched_int;
00203     /* 1 if DMP is enabled. */
00204     unsigned char dmp_on;
00205     /* Ensures that DMP will only be loaded once. */
00206     unsigned char dmp_loaded;
00207     /* Sampling rate used when DMP is enabled. */
00208     unsigned short dmp_sample_rate;
00209 #ifdef AK89xx_SECONDARY
00210     /* Compass sample rate. */
00211     unsigned short compass_sample_rate;
00212     unsigned char compass_addr;
00213     short mag_sens_adj[3];
00214 #endif
00215 };
00216 
00217 /* Information for self-test-> */
00218 struct test_s {
00219     unsigned long gyro_sens;
00220     unsigned long accel_sens;
00221     unsigned char reg_rate_div;
00222     unsigned char reg_lpf;
00223     unsigned char reg_gyro_fsr;
00224     unsigned char reg_accel_fsr;
00225     unsigned short wait_ms;
00226     unsigned char packet_thresh;
00227     float min_dps;
00228     float max_dps;
00229     float max_gyro_var;
00230     float min_g;
00231     float max_g;
00232     float max_accel_var;
00233 };
00234 
00235 /* Gyro driver state variables. */
00236 struct gyro_state_s {
00237     const struct gyro_reg_s *reg;
00238     const struct hw_s *hw;
00239     struct chip_cfg_s chip_cfg;
00240     const struct test_s *test;
00241 };
00242 
00243 /* Filter configurations. */
00244 enum lpf_e {
00245     INV_FILTER_256HZ_NOLPF2 = 0,
00246     INV_FILTER_188HZ,
00247     INV_FILTER_98HZ,
00248     INV_FILTER_42HZ,
00249     INV_FILTER_20HZ,
00250     INV_FILTER_10HZ,
00251     INV_FILTER_5HZ,
00252     INV_FILTER_2100HZ_NOLPF,
00253     NUM_FILTER
00254 };
00255 
00256 /* Full scale ranges. */
00257 enum gyro_fsr_e {
00258     INV_FSR_250DPS = 0,
00259     INV_FSR_500DPS,
00260     INV_FSR_1000DPS,
00261     INV_FSR_2000DPS,
00262     NUM_GYRO_FSR
00263 };
00264 
00265 /* Full scale ranges. */
00266 enum accel_fsr_e {
00267     INV_FSR_2G = 0,
00268     INV_FSR_4G,
00269     INV_FSR_8G,
00270     INV_FSR_16G,
00271     NUM_ACCEL_FSR
00272 };
00273 
00274 /* Clock sources. */
00275 enum clock_sel_e {
00276     INV_CLK_INTERNAL = 0,
00277     INV_CLK_PLL,
00278     NUM_CLK
00279 };
00280 
00281 /* Low-power accel wakeup rates. */
00282 enum lp_accel_rate_e {
00283 #if defined MPU6050
00284     INV_LPA_1_25HZ,
00285     INV_LPA_5HZ,
00286     INV_LPA_20HZ,
00287     INV_LPA_40HZ
00288 #elif defined MPU6500
00289     INV_LPA_0_3125HZ,
00290     INV_LPA_0_625HZ,
00291     INV_LPA_1_25HZ,
00292     INV_LPA_2_5HZ,
00293     INV_LPA_5HZ,
00294     INV_LPA_10HZ,
00295     INV_LPA_20HZ,
00296     INV_LPA_40HZ,
00297     INV_LPA_80HZ,
00298     INV_LPA_160HZ,
00299     INV_LPA_320HZ,
00300     INV_LPA_640HZ
00301 #endif
00302 };
00303 
00304 #define BIT_I2C_MST_VDDIO   (0x80)
00305 #define BIT_FIFO_EN         (0x40)
00306 #define BIT_DMP_EN          (0x80)
00307 #define BIT_FIFO_RST        (0x04)
00308 #define BIT_DMP_RST         (0x08)
00309 #define BIT_FIFO_OVERFLOW   (0x10)
00310 #define BIT_DATA_RDY_EN     (0x01)
00311 #define BIT_DMP_INT_EN      (0x02)
00312 #define BIT_MOT_INT_EN      (0x40)
00313 #define BITS_FSR            (0x18)
00314 #define BITS_LPF            (0x07)
00315 #define BITS_HPF            (0x07)
00316 #define BITS_CLK            (0x07)
00317 #define BIT_FIFO_SIZE_1024  (0x40)
00318 #define BIT_FIFO_SIZE_2048  (0x80)
00319 #define BIT_FIFO_SIZE_4096  (0xC0)
00320 #define BIT_RESET           (0x80)
00321 #define BIT_SLEEP           (0x40)
00322 #define BIT_S0_DELAY_EN     (0x01)
00323 #define BIT_S2_DELAY_EN     (0x04)
00324 #define BITS_SLAVE_LENGTH   (0x0F)
00325 #define BIT_SLAVE_BYTE_SW   (0x40)
00326 #define BIT_SLAVE_GROUP     (0x10)
00327 #define BIT_SLAVE_EN        (0x80)
00328 #define BIT_I2C_READ        (0x80)
00329 #define BITS_I2C_MASTER_DLY (0x1F)
00330 #define BIT_AUX_IF_EN       (0x20)
00331 #define BIT_ACTL            (0x80)
00332 #define BIT_LATCH_EN        (0x20)
00333 #define BIT_ANY_RD_CLR      (0x10)
00334 #define BIT_BYPASS_EN       (0x02)
00335 #define BITS_WOM_EN         (0xC0)
00336 #define BIT_LPA_CYCLE       (0x20)
00337 #define BIT_STBY_XA         (0x20)
00338 #define BIT_STBY_YA         (0x10)
00339 #define BIT_STBY_ZA         (0x08)
00340 #define BIT_STBY_XG         (0x04)
00341 #define BIT_STBY_YG         (0x02)
00342 #define BIT_STBY_ZG         (0x01)
00343 #define BIT_STBY_XYZA       (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
00344 #define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
00345 
00346 #if defined AK8975_SECONDARY
00347 #define SUPPORTS_AK89xx_HIGH_SENS   (0x00)
00348 #define AK89xx_FSR                  (9830)
00349 #elif defined AK8963_SECONDARY
00350 #define SUPPORTS_AK89xx_HIGH_SENS   (0x10)
00351 #define AK89xx_FSR                  (4915)
00352 #endif
00353 
00354 #ifdef AK89xx_SECONDARY
00355 #define AKM_REG_WHOAMI      (0x00)
00356 
00357 #define AKM_REG_ST1         (0x02)
00358 #define AKM_REG_HXL         (0x03)
00359 #define AKM_REG_ST2         (0x09)
00360 
00361 #define AKM_REG_CNTL        (0x0A)
00362 #define AKM_REG_ASTC        (0x0C)
00363 #define AKM_REG_ASAX        (0x10)
00364 #define AKM_REG_ASAY        (0x11)
00365 #define AKM_REG_ASAZ        (0x12)
00366 
00367 #define AKM_DATA_READY      (0x01)
00368 #define AKM_DATA_OVERRUN    (0x02)
00369 #define AKM_OVERFLOW        (0x80)
00370 #define AKM_DATA_ERROR      (0x40)
00371 
00372 #define AKM_BIT_SELF_TEST   (0x40)
00373 
00374 #define AKM_POWER_DOWN          (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
00375 #define AKM_SINGLE_MEASUREMENT  (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
00376 #define AKM_FUSE_ROM_ACCESS     (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
00377 #define AKM_MODE_SELF_TEST      (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
00378 
00379 #define AKM_WHOAMI      (0x48)
00380 #endif
00381 
00382 struct gyro_reg_s regArray[MPU_MAX_DEVICES];
00383 struct hw_s hwArray[MPU_MAX_DEVICES];
00384 struct test_s testArray[MPU_MAX_DEVICES];
00385 struct gyro_state_s gyroArray[MPU_MAX_DEVICES];
00386 
00387 //  These variables are set by the mpu_delect_device function
00388 
00389 struct gyro_reg_s *reg;
00390 struct hw_s *hw;
00391 struct test_s *test;
00392 struct gyro_state_s *st;
00393 static int deviceIndex = 0;
00394 
00395 int mpu_select_device(int device)
00396 {
00397   if ((device < 0) || (device >= MPU_MAX_DEVICES))
00398     return -1;
00399 
00400   deviceIndex = device;
00401   reg = regArray + device;
00402   hw = hwArray + device;
00403   test = testArray + device;
00404   st = gyroArray + device;
00405   return 0;
00406 }
00407 
00408 void mpu_init_structures()
00409 {
00410     reg->who_am_i         = 0x75;
00411     reg->rate_div         = 0x19;
00412     reg->lpf              = 0x1A;
00413     reg->prod_id          = 0x0C;
00414     reg->user_ctrl        = 0x6A;
00415     reg->fifo_en          = 0x23;
00416     reg->gyro_cfg         = 0x1B;
00417     reg->accel_cfg        = 0x1C;
00418     reg->motion_thr       = 0x1F;
00419     reg->motion_dur       = 0x20;
00420     reg->fifo_count_h     = 0x72;
00421     reg->fifo_r_w         = 0x74;
00422     reg->raw_gyro         = 0x43;
00423     reg->raw_accel        = 0x3B;
00424     reg->temp             = 0x41;
00425     reg->int_enable       = 0x38;
00426     reg->dmp_int_status   = 0x39;
00427     reg->int_status       = 0x3A;
00428     reg->pwr_mgmt_1       = 0x6B;
00429     reg->pwr_mgmt_2       = 0x6C;
00430     reg->int_pin_cfg      = 0x37;
00431     reg->mem_r_w          = 0x6F;
00432     reg->accel_offs       = 0x06;
00433     reg->i2c_mst          = 0x24;
00434     reg->bank_sel         = 0x6D;
00435     reg->mem_start_addr   = 0x6E;
00436     reg->prgm_start_h     = 0x70;
00437 #ifdef AK89xx_SECONDARY
00438     reg->raw_compass      = 0x49;
00439     reg->yg_offs_tc       = 0x01;
00440     reg->s0_addr          = 0x25;
00441     reg->s0_reg           = 0x26;
00442     reg->s0_ctrl          = 0x27;
00443     reg->s1_addr          = 0x28;
00444     reg->s1_reg           = 0x29;
00445     reg->s1_ctrl          = 0x2A;
00446     reg->s4_ctrl          = 0x34;
00447     reg->s0_do            = 0x63;
00448     reg->s1_do            = 0x64;
00449     reg->i2c_delay_ctrl   = 0x67;
00450 #endif
00451     switch (deviceIndex) {
00452       case 0:
00453         hw->addr = 0x68;
00454         break;
00455 
00456       case 1:
00457         hw->addr = 0x69;
00458         break;
00459     }
00460     hw->max_fifo          = 1024;
00461     hw->num_reg           = 118;
00462     hw->temp_sens         = 340;
00463     hw->temp_offset       = -521;
00464     hw->bank_size         = 256;
00465 #if defined AK89xx_SECONDARY
00466     hw->compass_fsr      = AK89xx_FSR;
00467 #endif
00468 
00469     test->gyro_sens      = 32768/250;
00470     test->accel_sens     = 32768/16;
00471     test->reg_rate_div   = 0;    /* 1kHz. */
00472     test->reg_lpf        = 1;    /* 188Hz. */
00473     test->reg_gyro_fsr   = 0;    /* 250dps. */
00474     test->reg_accel_fsr  = 0x18; /* 16g. */
00475     test->wait_ms        = 50;
00476     test->packet_thresh  = 5;    /* 5% */
00477     test->min_dps        = 10.f;
00478     test->max_dps        = 105.f;
00479     test->max_gyro_var   = 0.14f;
00480     test->min_g          = 0.3f;
00481     test->max_g          = 0.95f;
00482     test->max_accel_var  = 0.14f;
00483 
00484     st->reg = reg;
00485     st->hw = hw;
00486     st->test = test;
00487 };
00488 
00489 #define MAX_PACKET_LENGTH (12)
00490 
00491 #ifdef AK89xx_SECONDARY
00492 static int setup_compass(void);
00493 #define MAX_COMPASS_SAMPLE_RATE (100)
00494 #endif
00495 
00503 static int set_int_enable(unsigned char enable)
00504 {
00505     unsigned char tmp;
00506 
00507     if (st->chip_cfg.dmp_on) {
00508         if (enable)
00509             tmp = BIT_DMP_INT_EN;
00510         else
00511             tmp = 0x00;
00512         if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &tmp))
00513             return -1;
00514         st->chip_cfg.int_enable = tmp;
00515     } else {
00516         if (!st->chip_cfg.sensors)
00517             return -1;
00518         if (enable && st->chip_cfg.int_enable)
00519             return 0;
00520         if (enable)
00521             tmp = BIT_DATA_RDY_EN;
00522         else
00523             tmp = 0x00;
00524         if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &tmp))
00525             return -1;
00526         st->chip_cfg.int_enable = tmp;
00527     }
00528     return 0;
00529 }
00530 
00535 int mpu_reg_dump(void)
00536 {
00537     unsigned char ii;
00538     unsigned char data;
00539 
00540     for (ii = 0; ii < st->hw->num_reg; ii++) {
00541         if (ii == st->reg->fifo_r_w || ii == st->reg->mem_r_w)
00542             continue;
00543         if (i2c_read(st->hw->addr, ii, 1, &data))
00544             return -1;
00545 //      log_i("%#5x: %#5x\r\n", ii, data);
00546     }
00547     return 0;
00548 }
00549 
00557 int mpu_read_reg(unsigned char reg, unsigned char *data)
00558 {
00559     if (reg == st->reg->fifo_r_w || reg == st->reg->mem_r_w)
00560         return -1;
00561     if (reg >= st->hw->num_reg)
00562         return -1;
00563     return i2c_read(st->hw->addr, reg, 1, data);
00564 }
00565 
00580 int mpu_init(struct int_param_s *int_param)
00581 {
00582     unsigned char data[6], rev;
00583     int errCode;
00584     
00585     /* Reset device. */
00586     data[0] = BIT_RESET;
00587     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
00588         return -1;
00589     delay_ms(100);
00590 
00591     /* Wake up chip. */
00592     data[0] = 0x00;
00593     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
00594         return -2;
00595 
00596 #if defined MPU6050
00597     /* Check product revision. */
00598     if (i2c_read(st->hw->addr, st->reg->accel_offs, 6, data))
00599         return -3;
00600     rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) |
00601         (data[1] & 0x01);
00602 
00603     if (rev) {
00604         /* Congrats, these parts are better. */
00605         if (rev == 1)
00606             st->chip_cfg.accel_half = 1;
00607         else if (rev == 2)
00608             st->chip_cfg.accel_half = 0;
00609         else {
00610 #ifdef MPU_DEBUG
00611             Serial.print("Unsupported software product rev: "); Serial.println(rev);
00612 #endif
00613             return -4;
00614         }
00615     } else {
00616         if (i2c_read(st->hw->addr, st->reg->prod_id, 1, data))
00617             return -5;
00618         rev = data[0] & 0x0F;
00619         if (!rev) {
00620 #ifdef MPU_DEBUG
00621             Serial.println("Product ID read as 0 indicates device is either incompatible or an MPU3050");
00622 #endif
00623             return -6;
00624         } else if (rev == 4) {
00625 #ifdef MPU_DEBUG
00626             Serial.println("Half sensitivity part found.");
00627 #endif
00628             st->chip_cfg.accel_half = 1;
00629         } else
00630             st->chip_cfg.accel_half = 0;
00631     }
00632 #elif defined MPU6500
00633 #define MPU6500_MEM_REV_ADDR    (0x17)
00634     if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
00635         return -7;
00636     if (rev == 0x1)
00637         st->chip_cfg.accel_half = 0;
00638     else {
00639 #ifdef MPU_DEBUG
00640         Serial.print("Unsupported software product rev: "); Serial.println(rev);
00641 #endif
00642         return -7;
00643     }
00644 
00645     /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
00646      * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
00647      */
00648     data[0] = BIT_FIFO_SIZE_1024 | 0x8;
00649     if (i2c_write(st->hw->addr, st->reg->accel_cfg2, 1, data))
00650         return -1;
00651 #endif
00652 
00653     /* Set to invalid values to ensure no I2C writes are skipped. */
00654     st->chip_cfg.sensors = 0xFF;
00655     st->chip_cfg.gyro_fsr = 0xFF;
00656     st->chip_cfg.accel_fsr = 0xFF;
00657     st->chip_cfg.lpf = 0xFF;
00658     st->chip_cfg.sample_rate = 0xFFFF;
00659     st->chip_cfg.fifo_enable = 0xFF;
00660     st->chip_cfg.bypass_mode = 0xFF;
00661 #ifdef AK89xx_SECONDARY
00662     st->chip_cfg.compass_sample_rate = 0xFFFF;
00663 #endif
00664     /* mpu_set_sensors always preserves this setting. */
00665     st->chip_cfg.clk_src = INV_CLK_PLL;
00666     /* Handled in next call to mpu_set_bypass. */
00667     st->chip_cfg.active_low_int = 1;
00668     st->chip_cfg.latched_int = 0;
00669     st->chip_cfg.int_motion_only = 0;
00670     st->chip_cfg.lp_accel_mode = 0;
00671     memset(&st->chip_cfg.cache, 0, sizeof(st->chip_cfg.cache));
00672     st->chip_cfg.dmp_on = 0;
00673     st->chip_cfg.dmp_loaded = 0;
00674     st->chip_cfg.dmp_sample_rate = 0;
00675 
00676     if (mpu_set_gyro_fsr(2000))
00677         return -1;
00678     if (mpu_set_accel_fsr(2))
00679         return -1;
00680     if (mpu_set_lpf(42))
00681         return -1;
00682     if (mpu_set_sample_rate(50))
00683         return -1;
00684     if (mpu_configure_fifo(0))
00685         return -1;
00686 
00687     if (int_param)
00688         reg_int_cb(int_param);
00689 
00690 #ifdef AK89xx_SECONDARY
00691 #ifdef MPU_DEBUG
00692     Serial.println("Setting up compass");
00693 #endif
00694     errCode = setup_compass();
00695     if (errCode != 0) {
00696 #ifdef MPU_DEBUG
00697        Serial.print("Setup compass failed: "); Serial.println(errCode); 
00698 #endif
00699     }
00700     if (mpu_set_compass_sample_rate(10))
00701         return -1;
00702 #else
00703     /* Already disabled by setup_compass. */
00704     if (mpu_set_bypass(0))
00705         return -1;
00706 #endif
00707 
00708     mpu_set_sensors(0);
00709     return 0;
00710 }
00711 
00727 int mpu_lp_accel_mode(unsigned char rate)
00728 {
00729     unsigned char tmp[2];
00730 
00731     if (rate > 40)
00732         return -1;
00733 
00734     if (!rate) {
00735         mpu_set_int_latched(0);
00736         tmp[0] = 0;
00737         tmp[1] = BIT_STBY_XYZG;
00738         if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, tmp))
00739             return -1;
00740         st->chip_cfg.lp_accel_mode = 0;
00741         return 0;
00742     }
00743     /* For LP accel, we automatically configure the hardware to produce latched
00744      * interrupts. In LP accel mode, the hardware cycles into sleep mode before
00745      * it gets a chance to deassert the interrupt pin; therefore, we shift this
00746      * responsibility over to the MCU.
00747      *
00748      * Any register read will clear the interrupt.
00749      */
00750     mpu_set_int_latched(1);
00751 #if defined MPU6050
00752     tmp[0] = BIT_LPA_CYCLE;
00753     if (rate == 1) {
00754         tmp[1] = INV_LPA_1_25HZ;
00755         mpu_set_lpf(5);
00756     } else if (rate <= 5) {
00757         tmp[1] = INV_LPA_5HZ;
00758         mpu_set_lpf(5);
00759     } else if (rate <= 20) {
00760         tmp[1] = INV_LPA_20HZ;
00761         mpu_set_lpf(10);
00762     } else {
00763         tmp[1] = INV_LPA_40HZ;
00764         mpu_set_lpf(20);
00765     }
00766     tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
00767     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, tmp))
00768         return -1;
00769 #elif defined MPU6500
00770     /* Set wake frequency. */
00771     if (rate == 1)
00772         tmp[0] = INV_LPA_1_25HZ;
00773     else if (rate == 2)
00774         tmp[0] = INV_LPA_2_5HZ;
00775     else if (rate <= 5)
00776         tmp[0] = INV_LPA_5HZ;
00777     else if (rate <= 10)
00778         tmp[0] = INV_LPA_10HZ;
00779     else if (rate <= 20)
00780         tmp[0] = INV_LPA_20HZ;
00781     else if (rate <= 40)
00782         tmp[0] = INV_LPA_40HZ;
00783     else if (rate <= 80)
00784         tmp[0] = INV_LPA_80HZ;
00785     else if (rate <= 160)
00786         tmp[0] = INV_LPA_160HZ;
00787     else if (rate <= 320)
00788         tmp[0] = INV_LPA_320HZ;
00789     else
00790         tmp[0] = INV_LPA_640HZ;
00791     if (i2c_write(st->hw->addr, st->reg->lp_accel_odr, 1, tmp))
00792         return -1;
00793     tmp[0] = BIT_LPA_CYCLE;
00794     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, tmp))
00795         return -1;
00796 #endif
00797     st->chip_cfg.sensors = INV_XYZ_ACCEL;
00798     st->chip_cfg.clk_src = 0;
00799     st->chip_cfg.lp_accel_mode = 1;
00800     mpu_configure_fifo(0);
00801 
00802     return 0;
00803 }
00804 
00811 int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
00812 {
00813     unsigned char tmp[6];
00814 
00815     if (!(st->chip_cfg.sensors & INV_XYZ_GYRO))
00816         return -1;
00817 
00818     if (i2c_read(st->hw->addr, st->reg->raw_gyro, 6, tmp))
00819         return -1;
00820     data[0] = (tmp[0] << 8) | tmp[1];
00821     data[1] = (tmp[2] << 8) | tmp[3];
00822     data[2] = (tmp[4] << 8) | tmp[5];
00823     if (timestamp)
00824         get_ms(timestamp);
00825     return 0;
00826 }
00827 
00834 int mpu_get_accel_reg(short *data, unsigned long *timestamp)
00835 {
00836     unsigned char tmp[6];
00837 
00838     if (!(st->chip_cfg.sensors & INV_XYZ_ACCEL))
00839         return -1;
00840 
00841     if (i2c_read(st->hw->addr, st->reg->raw_accel, 6, tmp))
00842         return -1;
00843     data[0] = (tmp[0] << 8) | tmp[1];
00844     data[1] = (tmp[2] << 8) | tmp[3];
00845     data[2] = (tmp[4] << 8) | tmp[5];
00846     if (timestamp)
00847         get_ms(timestamp);
00848     return 0;
00849 }
00850 
00857 int mpu_get_temperature(long *data, unsigned long *timestamp)
00858 {
00859     unsigned char tmp[2];
00860     short raw;
00861 
00862     if (!(st->chip_cfg.sensors))
00863         return -1;
00864 
00865     if (i2c_read(st->hw->addr, st->reg->temp, 2, tmp))
00866         return -1;
00867     raw = (tmp[0] << 8) | tmp[1];
00868     if (timestamp)
00869         get_ms(timestamp);
00870 
00871     data[0] = (long)((35 + ((raw - (float)st->hw->temp_offset) / st->hw->temp_sens)) * 65536L);
00872     return 0;
00873 }
00874 
00882 int mpu_set_accel_bias(const long *accel_bias)
00883 {
00884     unsigned char data[6];
00885     short accel_hw[3];
00886     short got_accel[3];
00887     short fg[3];
00888 
00889     if (!accel_bias)
00890         return -1;
00891     if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
00892         return 0;
00893 
00894     if (i2c_read(st->hw->addr, 3, 3, data))
00895         return -1;
00896     fg[0] = ((data[0] >> 4) + 8) & 0xf;
00897     fg[1] = ((data[1] >> 4) + 8) & 0xf;
00898     fg[2] = ((data[2] >> 4) + 8) & 0xf;
00899 
00900     accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
00901     accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
00902     accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));
00903 
00904     if (i2c_read(st->hw->addr, 0x06, 6, data))
00905         return -1;
00906 
00907     got_accel[0] = ((short)data[0] << 8) | data[1];
00908     got_accel[1] = ((short)data[2] << 8) | data[3];
00909     got_accel[2] = ((short)data[4] << 8) | data[5];
00910 
00911     accel_hw[0] += got_accel[0];
00912     accel_hw[1] += got_accel[1];
00913     accel_hw[2] += got_accel[2];
00914 
00915     data[0] = (accel_hw[0] >> 8) & 0xff;
00916     data[1] = (accel_hw[0]) & 0xff;
00917     data[2] = (accel_hw[1] >> 8) & 0xff;
00918     data[3] = (accel_hw[1]) & 0xff;
00919     data[4] = (accel_hw[2] >> 8) & 0xff;
00920     data[5] = (accel_hw[2]) & 0xff;
00921 
00922     if (i2c_write(st->hw->addr, 0x06, 6, data))
00923         return -1;
00924     return 0;
00925 }
00926 
00931 int mpu_reset_fifo(void)
00932 {
00933     unsigned char data;
00934 
00935     if (!(st->chip_cfg.sensors))
00936         return -1;
00937 
00938     data = 0;
00939     if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &data))
00940         return -1;
00941     if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, &data))
00942         return -1;
00943     if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
00944         return -1;
00945 
00946     if (st->chip_cfg.dmp_on) {
00947         data = BIT_FIFO_RST | BIT_DMP_RST;
00948         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
00949             return -1;
00950         delay_ms(50);
00951         data = BIT_DMP_EN | BIT_FIFO_EN;
00952         if (st->chip_cfg.sensors & INV_XYZ_COMPASS)
00953             data |= BIT_AUX_IF_EN;
00954         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
00955             return -1;
00956         if (st->chip_cfg.int_enable)
00957             data = BIT_DMP_INT_EN;
00958         else
00959             data = 0;
00960         if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &data))
00961             return -1;
00962         data = 0;
00963         if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, &data))
00964             return -1;
00965     } else {
00966         data = BIT_FIFO_RST;
00967         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
00968             return -1;
00969         if (st->chip_cfg.bypass_mode || !(st->chip_cfg.sensors & INV_XYZ_COMPASS))
00970             data = BIT_FIFO_EN;
00971         else
00972             data = BIT_FIFO_EN | BIT_AUX_IF_EN;
00973         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &data))
00974             return -1;
00975         delay_ms(50);
00976         if (st->chip_cfg.int_enable)
00977             data = BIT_DATA_RDY_EN;
00978         else
00979             data = 0;
00980         if (i2c_write(st->hw->addr, st->reg->int_enable, 1, &data))
00981             return -1;
00982         if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, &st->chip_cfg.fifo_enable))
00983             return -1;
00984     }
00985     return 0;
00986 }
00987 
00993 int mpu_get_gyro_fsr(unsigned short *fsr)
00994 {
00995     switch (st->chip_cfg.gyro_fsr) {
00996     case INV_FSR_250DPS:
00997         fsr[0] = 250;
00998         break;
00999     case INV_FSR_500DPS:
01000         fsr[0] = 500;
01001         break;
01002     case INV_FSR_1000DPS:
01003         fsr[0] = 1000;
01004         break;
01005     case INV_FSR_2000DPS:
01006         fsr[0] = 2000;
01007         break;
01008     default:
01009         fsr[0] = 0;
01010         break;
01011     }
01012     return 0;
01013 }
01014 
01020 int mpu_set_gyro_fsr(unsigned short fsr)
01021 {
01022     unsigned char data;
01023 
01024     if (!(st->chip_cfg.sensors))
01025         return -1;
01026 
01027     switch (fsr) {
01028     case 250:
01029         data = INV_FSR_250DPS << 3;
01030         break;
01031     case 500:
01032         data = INV_FSR_500DPS << 3;
01033         break;
01034     case 1000:
01035         data = INV_FSR_1000DPS << 3;
01036         break;
01037     case 2000:
01038         data = INV_FSR_2000DPS << 3;
01039         break;
01040     default:
01041         return -1;
01042     }
01043 
01044     if (st->chip_cfg.gyro_fsr == (data >> 3))
01045         return 0;
01046     if (i2c_write(st->hw->addr, st->reg->gyro_cfg, 1, &data))
01047         return -1;
01048     st->chip_cfg.gyro_fsr = data >> 3;
01049     return 0;
01050 }
01051 
01057 int mpu_get_accel_fsr(unsigned char *fsr)
01058 {
01059     switch (st->chip_cfg.accel_fsr) {
01060     case INV_FSR_2G:
01061         fsr[0] = 2;
01062         break;
01063     case INV_FSR_4G:
01064         fsr[0] = 4;
01065         break;
01066     case INV_FSR_8G:
01067         fsr[0] = 8;
01068         break;
01069     case INV_FSR_16G:
01070         fsr[0] = 16;
01071         break;
01072     default:
01073         return -1;
01074     }
01075     if (st->chip_cfg.accel_half)
01076         fsr[0] <<= 1;
01077     return 0;
01078 }
01079 
01085 int mpu_set_accel_fsr(unsigned char fsr)
01086 {
01087     unsigned char data;
01088 
01089     if (!(st->chip_cfg.sensors))
01090         return -1;
01091 
01092     switch (fsr) {
01093     case 2:
01094         data = INV_FSR_2G << 3;
01095         break;
01096     case 4:
01097         data = INV_FSR_4G << 3;
01098         break;
01099     case 8:
01100         data = INV_FSR_8G << 3;
01101         break;
01102     case 16:
01103         data = INV_FSR_16G << 3;
01104         break;
01105     default:
01106         return -1;
01107     }
01108 
01109     if (st->chip_cfg.accel_fsr == (data >> 3))
01110         return 0;
01111     if (i2c_write(st->hw->addr, st->reg->accel_cfg, 1, &data))
01112         return -1;
01113     st->chip_cfg.accel_fsr = data >> 3;
01114     return 0;
01115 }
01116 
01122 int mpu_get_lpf(unsigned short *lpf)
01123 {
01124     switch (st->chip_cfg.lpf) {
01125     case INV_FILTER_188HZ:
01126         lpf[0] = 188;
01127         break;
01128     case INV_FILTER_98HZ:
01129         lpf[0] = 98;
01130         break;
01131     case INV_FILTER_42HZ:
01132         lpf[0] = 42;
01133         break;
01134     case INV_FILTER_20HZ:
01135         lpf[0] = 20;
01136         break;
01137     case INV_FILTER_10HZ:
01138         lpf[0] = 10;
01139         break;
01140     case INV_FILTER_5HZ:
01141         lpf[0] = 5;
01142         break;
01143     case INV_FILTER_256HZ_NOLPF2:
01144     case INV_FILTER_2100HZ_NOLPF:
01145     default:
01146         lpf[0] = 0;
01147         break;
01148     }
01149     return 0;
01150 }
01151 
01158 int mpu_set_lpf(unsigned short lpf)
01159 {
01160     unsigned char data;
01161     if (!(st->chip_cfg.sensors))
01162         return -1;
01163 
01164     if (lpf >= 188)
01165         data = INV_FILTER_188HZ;
01166     else if (lpf >= 98)
01167         data = INV_FILTER_98HZ;
01168     else if (lpf >= 42)
01169         data = INV_FILTER_42HZ;
01170     else if (lpf >= 20)
01171         data = INV_FILTER_20HZ;
01172     else if (lpf >= 10)
01173         data = INV_FILTER_10HZ;
01174     else
01175         data = INV_FILTER_5HZ;
01176 
01177     if (st->chip_cfg.lpf == data)
01178         return 0;
01179     if (i2c_write(st->hw->addr, st->reg->lpf, 1, &data))
01180         return -1;
01181     st->chip_cfg.lpf = data;
01182     return 0;
01183 }
01184 
01190 int mpu_get_sample_rate(unsigned short *rate)
01191 {
01192     if (st->chip_cfg.dmp_on)
01193         return -1;
01194     else
01195         rate[0] = st->chip_cfg.sample_rate;
01196     return 0;
01197 }
01198 
01205 int mpu_set_sample_rate(unsigned short rate)
01206 {
01207     unsigned char data;
01208 
01209     if (!(st->chip_cfg.sensors))
01210         return -1;
01211 
01212     if (st->chip_cfg.dmp_on)
01213         return -1;
01214     else {
01215         if (st->chip_cfg.lp_accel_mode) {
01216             if (rate && (rate <= 40)) {
01217                 /* Just stay in low-power accel mode. */
01218                 mpu_lp_accel_mode(rate);
01219                 return 0;
01220             }
01221             /* Requested rate exceeds the allowed frequencies in LP accel mode,
01222              * switch back to full-power mode.
01223              */
01224             mpu_lp_accel_mode(0);
01225         }
01226         if (rate < 4)
01227             rate = 4;
01228         else if (rate > 1000)
01229             rate = 1000;
01230 
01231         data = 1000 / rate - 1;
01232         if (i2c_write(st->hw->addr, st->reg->rate_div, 1, &data))
01233             return -1;
01234 
01235         st->chip_cfg.sample_rate = 1000 / (1 + data);
01236 
01237 #ifdef AK89xx_SECONDARY
01238         mpu_set_compass_sample_rate(min(st->chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
01239 #endif
01240 
01241         /* Automatically set LPF to 1/2 sampling rate. */
01242         mpu_set_lpf(st->chip_cfg.sample_rate >> 1);
01243         return 0;
01244     }
01245 }
01246 
01252 int mpu_get_compass_sample_rate(unsigned short *rate)
01253 {
01254 #ifdef AK89xx_SECONDARY
01255     rate[0] = st->chip_cfg.compass_sample_rate;
01256     return 0;
01257 #else
01258     rate[0] = 0;
01259     return -1;
01260 #endif
01261 }
01262 
01274 int mpu_set_compass_sample_rate(unsigned short rate)
01275 {
01276 #ifdef AK89xx_SECONDARY
01277     unsigned char div;
01278     if (!rate || rate > st->chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
01279         return -1;
01280 
01281     div = st->chip_cfg.sample_rate / rate - 1;
01282     if (i2c_write(st->hw->addr, st->reg->s4_ctrl, 1, &div))
01283         return -1;
01284     st->chip_cfg.compass_sample_rate = st->chip_cfg.sample_rate / (div + 1);
01285     return 0;
01286 #else
01287     return -1;
01288 #endif
01289 }
01290 
01296 int mpu_get_gyro_sens(float *sens)
01297 {
01298     switch (st->chip_cfg.gyro_fsr) {
01299     case INV_FSR_250DPS:
01300         sens[0] = 131.f;
01301         break;
01302     case INV_FSR_500DPS:
01303         sens[0] = 65.5f;
01304         break;
01305     case INV_FSR_1000DPS:
01306         sens[0] = 32.8f;
01307         break;
01308     case INV_FSR_2000DPS:
01309         sens[0] = 16.4f;
01310         break;
01311     default:
01312         return -1;
01313     }
01314     return 0;
01315 }
01316 
01322 int mpu_get_accel_sens(unsigned short *sens)
01323 {
01324     switch (st->chip_cfg.accel_fsr) {
01325     case INV_FSR_2G:
01326         sens[0] = 16384;
01327         break;
01328     case INV_FSR_4G:
01329         sens[0] = 8092;
01330         break;
01331     case INV_FSR_8G:
01332         sens[0] = 4096;
01333         break;
01334     case INV_FSR_16G:
01335         sens[0] = 2048;
01336         break;
01337     default:
01338         return -1;
01339     }
01340     if (st->chip_cfg.accel_half)
01341         sens[0] >>= 1;
01342     return 0;
01343 }
01344 
01354 int mpu_get_fifo_config(unsigned char *sensors)
01355 {
01356     sensors[0] = st->chip_cfg.fifo_enable;
01357     return 0;
01358 }
01359 
01369 int mpu_configure_fifo(unsigned char sensors)
01370 {
01371     unsigned char prev;
01372     int result = 0;
01373 
01374     /* Compass data isn't going into the FIFO. Stop trying. */
01375     sensors &= ~INV_XYZ_COMPASS;
01376 
01377     if (st->chip_cfg.dmp_on)
01378         return 0;
01379     else {
01380         if (!(st->chip_cfg.sensors))
01381             return -1;
01382         prev = st->chip_cfg.fifo_enable;
01383         st->chip_cfg.fifo_enable = sensors & st->chip_cfg.sensors;
01384         if (st->chip_cfg.fifo_enable != sensors)
01385             /* You're not getting what you asked for. Some sensors are
01386              * asleep.
01387              */
01388             result = -1;
01389         else
01390             result = 0;
01391         if (sensors || st->chip_cfg.lp_accel_mode)
01392             set_int_enable(1);
01393         else
01394             set_int_enable(0);
01395         if (sensors) {
01396             if (mpu_reset_fifo()) {
01397                 st->chip_cfg.fifo_enable = prev;
01398                 return -1;
01399             }
01400         }
01401     }
01402 
01403     return result;
01404 }
01405 
01411 int mpu_get_power_state(unsigned char *power_on)
01412 {
01413     if (st->chip_cfg.sensors)
01414         power_on[0] = 1;
01415     else
01416         power_on[0] = 0;
01417     return 0;
01418 }
01419 
01430 int mpu_set_sensors(unsigned char sensors)
01431 {
01432     unsigned char data;
01433 #ifdef AK89xx_SECONDARY
01434     unsigned char user_ctrl;
01435 #endif
01436 
01437     if (sensors & INV_XYZ_GYRO)
01438         data = INV_CLK_PLL;
01439     else if (sensors)
01440         data = 0;
01441     else
01442         data = BIT_SLEEP;
01443     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, &data)) {
01444         st->chip_cfg.sensors = 0;
01445         return -1;
01446     }
01447     st->chip_cfg.clk_src = data & ~BIT_SLEEP;
01448 
01449     data = 0;
01450     if (!(sensors & INV_X_GYRO))
01451         data |= BIT_STBY_XG;
01452     if (!(sensors & INV_Y_GYRO))
01453         data |= BIT_STBY_YG;
01454     if (!(sensors & INV_Z_GYRO))
01455         data |= BIT_STBY_ZG;
01456     if (!(sensors & INV_XYZ_ACCEL))
01457         data |= BIT_STBY_XYZA;
01458     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_2, 1, &data)) {
01459         st->chip_cfg.sensors = 0;
01460         return -1;
01461     }
01462 
01463     if (sensors && (sensors != INV_XYZ_ACCEL))
01464         /* Latched interrupts only used in LP accel mode. */
01465         mpu_set_int_latched(0);
01466 
01467 #ifdef AK89xx_SECONDARY
01468 #ifdef AK89xx_BYPASS
01469     if (sensors & INV_XYZ_COMPASS)
01470         mpu_set_bypass(1);
01471     else
01472         mpu_set_bypass(0);
01473 #else
01474     if (i2c_read(st->hw->addr, st->reg->user_ctrl, 1, &user_ctrl))
01475         return -1;
01476     /* Handle AKM power management. */
01477     if (sensors & INV_XYZ_COMPASS) {
01478         data = AKM_SINGLE_MEASUREMENT;
01479         user_ctrl |= BIT_AUX_IF_EN;
01480     } else {
01481         data = AKM_POWER_DOWN;
01482         user_ctrl &= ~BIT_AUX_IF_EN;
01483     }
01484     if (st->chip_cfg.dmp_on)
01485         user_ctrl |= BIT_DMP_EN;
01486     else
01487         user_ctrl &= ~BIT_DMP_EN;
01488     if (i2c_write(st->hw->addr, st->reg->s1_do, 1, &data))
01489         return -1;
01490     /* Enable/disable I2C master mode. */
01491     if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &user_ctrl))
01492         return -1;
01493 #endif
01494 #endif
01495 
01496     st->chip_cfg.sensors = sensors;
01497     st->chip_cfg.lp_accel_mode = 0;
01498     delay_ms(50);
01499     return 0;
01500 }
01501 
01507 int mpu_get_int_status(short *status)
01508 {
01509     unsigned char tmp[2];
01510     if (!st->chip_cfg.sensors)
01511         return -1;
01512     if (i2c_read(st->hw->addr, st->reg->dmp_int_status, 2, tmp))
01513         return -1;
01514     status[0] = (tmp[0] << 8) | tmp[1];
01515     return 0;
01516 }
01517 
01536 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
01537         unsigned char *sensors, unsigned char *more)
01538 {
01539     /* Assumes maximum packet size is gyro (6) + accel (6). */
01540     unsigned char data[MAX_PACKET_LENGTH];
01541     unsigned char packet_size = 0;
01542     unsigned short fifo_count, index = 0;
01543 
01544     if (st->chip_cfg.dmp_on)
01545         return -1;
01546 
01547     sensors[0] = 0;
01548     if (!st->chip_cfg.sensors)
01549         return -2;
01550     if (!st->chip_cfg.fifo_enable)
01551         return -3;
01552 
01553     if (st->chip_cfg.fifo_enable & INV_X_GYRO)
01554         packet_size += 2;
01555     if (st->chip_cfg.fifo_enable & INV_Y_GYRO)
01556         packet_size += 2;
01557     if (st->chip_cfg.fifo_enable & INV_Z_GYRO)
01558         packet_size += 2;
01559     if (st->chip_cfg.fifo_enable & INV_XYZ_ACCEL)
01560         packet_size += 6;
01561 
01562     if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 2, data))
01563         return -4;
01564     fifo_count = (data[0] << 8) | data[1];
01565     if (fifo_count < packet_size)
01566         return 0;
01567 //    log_i("FIFO count: %hd\n", fifo_count);
01568     if (fifo_count > (st->hw->max_fifo >> 1)) {
01569         /* FIFO is 50% full, better check overflow bit. */
01570         if (i2c_read(st->hw->addr, st->reg->int_status, 1, data))
01571             return -5;
01572         if (data[0] & BIT_FIFO_OVERFLOW) {
01573             mpu_reset_fifo();
01574             return -6;
01575         }
01576     }
01577     get_ms((unsigned long*)timestamp);
01578 
01579     if (i2c_read(st->hw->addr, st->reg->fifo_r_w, packet_size, data))
01580         return -7;
01581     more[0] = fifo_count / packet_size - 1;
01582     sensors[0] = 0;
01583 
01584     if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
01585         accel[0] = (data[index+0] << 8) | data[index+1];
01586         accel[1] = (data[index+2] << 8) | data[index+3];
01587         accel[2] = (data[index+4] << 8) | data[index+5];
01588         sensors[0] |= INV_XYZ_ACCEL;
01589         index += 6;
01590     }
01591     if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_X_GYRO) {
01592         gyro[0] = (data[index+0] << 8) | data[index+1];
01593         sensors[0] |= INV_X_GYRO;
01594         index += 2;
01595     }
01596     if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_Y_GYRO) {
01597         gyro[1] = (data[index+0] << 8) | data[index+1];
01598         sensors[0] |= INV_Y_GYRO;
01599         index += 2;
01600     }
01601     if ((index != packet_size) && st->chip_cfg.fifo_enable & INV_Z_GYRO) {
01602         gyro[2] = (data[index+0] << 8) | data[index+1];
01603         sensors[0] |= INV_Z_GYRO;
01604         index += 2;
01605     }
01606 
01607     return 0;
01608 }
01609 
01617 int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
01618     unsigned char *more)
01619 {
01620     unsigned char tmp[2];
01621     unsigned short fifo_count;
01622     if (!st->chip_cfg.dmp_on)
01623         return -1;
01624     if (!st->chip_cfg.sensors)
01625         return -2;
01626 
01627     if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 2, tmp))
01628         return -3;
01629     fifo_count = (tmp[0] << 8) | tmp[1];
01630     if (fifo_count < length) {
01631         more[0] = 0;
01632         return -4;
01633     }
01634     if (fifo_count > (st->hw->max_fifo >> 1)) {
01635         /* FIFO is 50% full, better check overflow bit. */
01636         if (i2c_read(st->hw->addr, st->reg->int_status, 1, tmp))
01637             return -5;
01638         if (tmp[0] & BIT_FIFO_OVERFLOW) {
01639             mpu_reset_fifo();
01640             return -6;
01641         }
01642     }
01643 
01644     if (i2c_read(st->hw->addr, st->reg->fifo_r_w, length, data))
01645         return -7;
01646     more[0] = fifo_count / length - 1;
01647     return 0;
01648 }
01649 
01655 int mpu_set_bypass(unsigned char bypass_on)
01656 {
01657     unsigned char tmp;
01658 
01659     if (st->chip_cfg.bypass_mode == bypass_on)
01660         return 0;
01661 
01662     if (bypass_on) {
01663         if (i2c_read(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
01664             return -1;
01665         tmp &= ~BIT_AUX_IF_EN;
01666         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
01667             return -1;
01668         delay_ms(3);
01669         tmp = BIT_BYPASS_EN;
01670         if (st->chip_cfg.active_low_int)
01671             tmp |= BIT_ACTL;
01672         if (st->chip_cfg.latched_int)
01673             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
01674         if (i2c_write(st->hw->addr, st->reg->int_pin_cfg, 1, &tmp))
01675             return -1;
01676     } else {
01677         /* Enable I2C master mode if compass is being used. */
01678         if (i2c_read(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
01679             return -1;
01680         if (st->chip_cfg.sensors & INV_XYZ_COMPASS)
01681             tmp |= BIT_AUX_IF_EN;
01682         else
01683             tmp &= ~BIT_AUX_IF_EN;
01684         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, &tmp))
01685             return -1;
01686         delay_ms(3);
01687         if (st->chip_cfg.active_low_int)
01688             tmp = BIT_ACTL;
01689         else
01690             tmp = 0;
01691         if (st->chip_cfg.latched_int)
01692             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
01693         if (i2c_write(st->hw->addr, st->reg->int_pin_cfg, 1, &tmp))
01694             return -1;
01695     }
01696     st->chip_cfg.bypass_mode = bypass_on;
01697     return 0;
01698 }
01699 
01705 int mpu_set_int_level(unsigned char active_low)
01706 {
01707     st->chip_cfg.active_low_int = active_low;
01708     return 0;
01709 }
01710 
01717 int mpu_set_int_latched(unsigned char enable)
01718 {
01719     unsigned char tmp;
01720     if (st->chip_cfg.latched_int == enable)
01721         return 0;
01722 
01723     if (enable)
01724         tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
01725     else
01726         tmp = 0;
01727     if (st->chip_cfg.bypass_mode)
01728         tmp |= BIT_BYPASS_EN;
01729     if (st->chip_cfg.active_low_int)
01730         tmp |= BIT_ACTL;
01731     if (i2c_write(st->hw->addr, st->reg->int_pin_cfg, 1, &tmp))
01732         return -1;
01733     st->chip_cfg.latched_int = enable;
01734     return 0;
01735 }
01736 
01737 #ifdef MPU_MAXIMAL
01738 
01739 #ifdef MPU6050
01740 static int get_accel_prod_shift(float *st_shift)
01741 {
01742     unsigned char tmp[4], shift_code[3], ii;
01743 
01744     if (i2c_read(st->hw->addr, 0x0D, 4, tmp))
01745         return 0x07;
01746 
01747     shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
01748     shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
01749     shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
01750     for (ii = 0; ii < 3; ii++) {
01751         if (!shift_code[ii]) {
01752             st_shift[ii] = 0.f;
01753             continue;
01754         }
01755         /* Equivalent to..
01756          * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
01757          */
01758         st_shift[ii] = 0.34f;
01759         while (--shift_code[ii])
01760             st_shift[ii] *= 1.034f;
01761     }
01762     return 0;
01763 }
01764 
01765 static int accel_self_test(long *bias_regular, long *bias_st)
01766 {
01767     int jj, result = 0;
01768     float st_shift[3], st_shift_cust, st_shift_var;
01769 
01770     get_accel_prod_shift(st_shift);
01771     for(jj = 0; jj < 3; jj++) {
01772         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
01773         if (st_shift[jj]) {
01774             st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
01775             if (fabs(st_shift_var) > test->max_accel_var)
01776                 result |= 1 << jj;
01777         } else if ((st_shift_cust < test->min_g) ||
01778             (st_shift_cust > test->max_g))
01779             result |= 1 << jj;
01780     }
01781 
01782     return result;
01783 }
01784 
01785 static int gyro_self_test(long *bias_regular, long *bias_st)
01786 {
01787     int jj, result = 0;
01788     unsigned char tmp[3];
01789     float st_shift, st_shift_cust, st_shift_var;
01790 
01791     if (i2c_read(st->hw->addr, 0x0D, 3, tmp))
01792         return 0x07;
01793 
01794     tmp[0] &= 0x1F;
01795     tmp[1] &= 0x1F;
01796     tmp[2] &= 0x1F;
01797 
01798     for (jj = 0; jj < 3; jj++) {
01799         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
01800         if (tmp[jj]) {
01801             st_shift = 3275.f / test->gyro_sens;
01802             while (--tmp[jj])
01803                 st_shift *= 1.046f;
01804             st_shift_var = st_shift_cust / st_shift - 1.f;
01805             if (fabs(st_shift_var) > test->max_gyro_var)
01806                 result |= 1 << jj;
01807         } else if ((st_shift_cust < test->min_dps) ||
01808             (st_shift_cust > test->max_dps))
01809             result |= 1 << jj;
01810     }
01811     return result;
01812 }
01813 
01814 #ifdef AK89xx_SECONDARY
01815 static int compass_self_test(void)
01816 {
01817     unsigned char tmp[6];
01818     unsigned char tries = 10;
01819     int result = 0x07;
01820     short data;
01821 
01822     mpu_set_bypass(1);
01823 
01824     tmp[0] = AKM_POWER_DOWN;
01825     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
01826         return 0x07;
01827     tmp[0] = AKM_BIT_SELF_TEST;
01828     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
01829         goto AKM_restore;
01830     tmp[0] = AKM_MODE_SELF_TEST;
01831     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
01832         goto AKM_restore;
01833 
01834     do {
01835         delay_ms(10);
01836         if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
01837             goto AKM_restore;
01838         if (tmp[0] & AKM_DATA_READY)
01839             break;
01840     } while (tries--);
01841     if (!(tmp[0] & AKM_DATA_READY))
01842         goto AKM_restore;
01843 
01844     if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
01845         goto AKM_restore;
01846 
01847     result = 0;
01848     data = (short)(tmp[1] << 8) | tmp[0];
01849     if ((data > 100) || (data < -100))
01850         result |= 0x01;
01851     data = (short)(tmp[3] << 8) | tmp[2];
01852     if ((data > 100) || (data < -100))
01853         result |= 0x02;
01854     data = (short)(tmp[5] << 8) | tmp[4];
01855     if ((data > -300) || (data < -1000))
01856         result |= 0x04;
01857 
01858 AKM_restore:
01859     tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
01860     i2c_write(st->chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
01861     tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
01862     i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
01863     mpu_set_bypass(0);
01864     return result;
01865 }
01866 #endif
01867 #endif
01868 
01869 static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
01870 {
01871     unsigned char data[MAX_PACKET_LENGTH];
01872     unsigned char packet_count, ii;
01873     unsigned short fifo_count;
01874 
01875     data[0] = 0x01;
01876     data[1] = 0;
01877     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, data))
01878         return -1;
01879     delay_ms(200);
01880     data[0] = 0;
01881     if (i2c_write(st->hw->addr, st->reg->int_enable, 1, data))
01882         return -1;
01883     if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, data))
01884         return -1;
01885     if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
01886         return -1;
01887     if (i2c_write(st->hw->addr, st->reg->i2c_mst, 1, data))
01888         return -1;
01889     if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, data))
01890         return -1;
01891     data[0] = BIT_FIFO_RST | BIT_DMP_RST;
01892     if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, data))
01893         return -1;
01894     delay_ms(15);
01895     data[0] = st->test->reg_lpf;
01896     if (i2c_write(st->hw->addr, st->reg->lpf, 1, data))
01897         return -1;
01898     data[0] = st->test->reg_rate_div;
01899     if (i2c_write(st->hw->addr, st->reg->rate_div, 1, data))
01900         return -1;
01901     if (hw_test)
01902         data[0] = st->test->reg_gyro_fsr | 0xE0;
01903     else
01904         data[0] = st->test->reg_gyro_fsr;
01905     if (i2c_write(st->hw->addr, st->reg->gyro_cfg, 1, data))
01906         return -1;
01907 
01908     if (hw_test)
01909         data[0] = st->test->reg_accel_fsr | 0xE0;
01910     else
01911         data[0] = test->reg_accel_fsr;
01912     if (i2c_write(st->hw->addr, st->reg->accel_cfg, 1, data))
01913         return -1;
01914     if (hw_test)
01915         delay_ms(200);
01916 
01917     /* Fill FIFO for test->wait_ms milliseconds. */
01918     data[0] = BIT_FIFO_EN;
01919     if (i2c_write(st->hw->addr, st->reg->user_ctrl, 1, data))
01920         return -1;
01921 
01922     data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
01923     if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, data))
01924         return -1;
01925     delay_ms(test->wait_ms);
01926     data[0] = 0;
01927     if (i2c_write(st->hw->addr, st->reg->fifo_en, 1, data))
01928         return -1;
01929 
01930     if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 2, data))
01931         return -1;
01932 
01933     fifo_count = (data[0] << 8) | data[1];
01934     packet_count = fifo_count / MAX_PACKET_LENGTH;
01935     gyro[0] = gyro[1] = gyro[2] = 0;
01936     accel[0] = accel[1] = accel[2] = 0;
01937 
01938     for (ii = 0; ii < packet_count; ii++) {
01939         short accel_cur[3], gyro_cur[3];
01940         if (i2c_read(st->hw->addr, st->reg->fifo_r_w, MAX_PACKET_LENGTH, data))
01941             return -1;
01942         accel_cur[0] = ((short)data[0] << 8) | data[1];
01943         accel_cur[1] = ((short)data[2] << 8) | data[3];
01944         accel_cur[2] = ((short)data[4] << 8) | data[5];
01945         accel[0] += (long)accel_cur[0];
01946         accel[1] += (long)accel_cur[1];
01947         accel[2] += (long)accel_cur[2];
01948         gyro_cur[0] = (((short)data[6] << 8) | data[7]);
01949         gyro_cur[1] = (((short)data[8] << 8) | data[9]);
01950         gyro_cur[2] = (((short)data[10] << 8) | data[11]);
01951         gyro[0] += (long)gyro_cur[0];
01952         gyro[1] += (long)gyro_cur[1];
01953         gyro[2] += (long)gyro_cur[2];
01954     }
01955 #ifdef EMPL_NO_64BIT
01956     gyro[0] = (long)(((float)gyro[0]*65536.f) / test->gyro_sens / packet_count);
01957     gyro[1] = (long)(((float)gyro[1]*65536.f) / test->gyro_sens / packet_count);
01958     gyro[2] = (long)(((float)gyro[2]*65536.f) / test->gyro_sens / packet_count);
01959     if (has_accel) {
01960         accel[0] = (long)(((float)accel[0]*65536.f) / test->accel_sens /
01961             packet_count);
01962         accel[1] = (long)(((float)accel[1]*65536.f) / test->accel_sens /
01963             packet_count);
01964         accel[2] = (long)(((float)accel[2]*65536.f) / test->accel_sens /
01965             packet_count);
01966         /* Don't remove gravity! */
01967         accel[2] -= 65536L;
01968     }
01969 #else
01970     gyro[0] = (long)(((long long)gyro[0]<<16) / test->gyro_sens / packet_count);
01971     gyro[1] = (long)(((long long)gyro[1]<<16) / test->gyro_sens / packet_count);
01972     gyro[2] = (long)(((long long)gyro[2]<<16) / test->gyro_sens / packet_count);
01973     accel[0] = (long)(((long long)accel[0]<<16) / test->accel_sens /
01974         packet_count);
01975     accel[1] = (long)(((long long)accel[1]<<16) / test->accel_sens /
01976         packet_count);
01977     accel[2] = (long)(((long long)accel[2]<<16) / test->accel_sens /
01978         packet_count);
01979     /* Don't remove gravity! */
01980     if (accel[2] > 0L)
01981         accel[2] -= 65536L;
01982     else
01983         accel[2] += 65536L;
01984 #endif
01985 
01986     return 0;
01987 }
01988 
02009 int mpu_run_self_test(long *gyro, long *accel)
02010 {
02011 #ifdef MPU6050
02012     const unsigned char tries = 2;
02013     long gyro_st[3], accel_st[3];
02014     unsigned char accel_result, gyro_result;
02015 #ifdef AK89xx_SECONDARY
02016     unsigned char compass_result;
02017 #endif
02018     int ii;
02019 #endif
02020     int result;
02021     unsigned char accel_fsr, fifo_sensors, sensors_on;
02022     unsigned short gyro_fsr, sample_rate, lpf;
02023     unsigned char dmp_was_on;
02024 
02025     if (st->chip_cfg.dmp_on) {
02026         mpu_set_dmp_state(0);
02027         dmp_was_on = 1;
02028     } else
02029         dmp_was_on = 0;
02030 
02031     /* Get initial settings. */
02032     mpu_get_gyro_fsr(&gyro_fsr);
02033     mpu_get_accel_fsr(&accel_fsr);
02034     mpu_get_lpf(&lpf);
02035     mpu_get_sample_rate(&sample_rate);
02036     sensors_on = st->chip_cfg.sensors;
02037     mpu_get_fifo_config(&fifo_sensors);
02038 
02039     /* For older chips, the self-test will be different. */
02040 #if defined MPU6050
02041     for (ii = 0; ii < tries; ii++)
02042         if (!get_st_biases(gyro, accel, 0))
02043             break;
02044     if (ii == tries) {
02045         /* If we reach this point, we most likely encountered an I2C error.
02046          * We'll just report an error for all three sensors.
02047          */
02048         result = 0;
02049         goto restore;
02050     }
02051     for (ii = 0; ii < tries; ii++)
02052         if (!get_st_biases(gyro_st, accel_st, 1))
02053             break;
02054     if (ii == tries) {
02055         /* Again, probably an I2C error. */
02056         result = 0;
02057         goto restore;
02058     }
02059     accel_result = accel_self_test(accel, accel_st);
02060     gyro_result = gyro_self_test(gyro, gyro_st);
02061 
02062     result = 0;
02063     if (!gyro_result)
02064         result |= 0x01;
02065     if (!accel_result)
02066         result |= 0x02;
02067 
02068 #ifdef AK89xx_SECONDARY
02069     compass_result = compass_self_test();
02070     if (!compass_result)
02071         result |= 0x04;
02072 #endif
02073 restore:
02074 #elif defined MPU6500
02075     /* For now, this function will return a "pass" result for all three sensors
02076      * for compatibility with current test applications.
02077      */
02078     get_st_biases(gyro, accel, 0);
02079     result = 0x7;
02080 #endif
02081     /* Set to invalid values to ensure no I2C writes are skipped. */
02082     st->chip_cfg.gyro_fsr = 0xFF;
02083     st->chip_cfg.accel_fsr = 0xFF;
02084     st->chip_cfg.lpf = 0xFF;
02085     st->chip_cfg.sample_rate = 0xFFFF;
02086     st->chip_cfg.sensors = 0xFF;
02087     st->chip_cfg.fifo_enable = 0xFF;
02088     st->chip_cfg.clk_src = INV_CLK_PLL;
02089     mpu_set_gyro_fsr(gyro_fsr);
02090     mpu_set_accel_fsr(accel_fsr);
02091     mpu_set_lpf(lpf);
02092     mpu_set_sample_rate(sample_rate);
02093     mpu_set_sensors(sensors_on);
02094     mpu_configure_fifo(fifo_sensors);
02095 
02096     if (dmp_was_on)
02097         mpu_set_dmp_state(1);
02098 
02099     return result;
02100 }
02101 #endif // MPU_MAXIMAL
02102 
02112 int mpu_write_mem(unsigned short mem_addr, unsigned short length,
02113         unsigned char *data)
02114 {
02115     unsigned char tmp[2];
02116 
02117     if (!data)
02118         return -1;
02119     if (!st->chip_cfg.sensors)
02120         return -2;
02121 
02122     tmp[0] = (unsigned char)(mem_addr >> 8);
02123     tmp[1] = (unsigned char)(mem_addr & 0xFF);
02124 
02125     /* Check bank boundaries. */
02126     if (tmp[1] + length > st->hw->bank_size)
02127         return -3;
02128 
02129     if (i2c_write(st->hw->addr, st->reg->bank_sel, 2, tmp))
02130         return -4;
02131     if (i2c_write(st->hw->addr, st->reg->mem_r_w, length, data))
02132         return -5;
02133     return 0;
02134 }
02135 
02145 int mpu_read_mem(unsigned short mem_addr, unsigned short length,
02146         unsigned char *data)
02147 {
02148     unsigned char tmp[2];
02149 
02150     if (!data)
02151         return -1;
02152     if (!st->chip_cfg.sensors)
02153         return -1;
02154 
02155     tmp[0] = (unsigned char)(mem_addr >> 8);
02156     tmp[1] = (unsigned char)(mem_addr & 0xFF);
02157 
02158     /* Check bank boundaries. */
02159     if (tmp[1] + length > st->hw->bank_size)
02160         return -1;
02161 
02162     if (i2c_write(st->hw->addr, st->reg->bank_sel, 2, tmp))
02163         return -1;
02164     if (i2c_read(st->hw->addr, st->reg->mem_r_w, length, data))
02165         return -1;
02166     return 0;
02167 }
02168 
02177 int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
02178     unsigned short start_addr, unsigned short sample_rate)
02179 {
02180     unsigned short ii;
02181     unsigned short this_write;
02182     int errCode;
02183     uint8_t *progBuffer;
02184     /* Must divide evenly into st->hw->bank_size to avoid bank crossings. */
02185 #define LOAD_CHUNK  (16)
02186     unsigned char cur[LOAD_CHUNK], tmp[2];
02187 
02188     if (st->chip_cfg.dmp_loaded)
02189         /* DMP should only be loaded once. */
02190         return -1;
02191 
02192     if (!firmware)
02193         return -2;
02194         
02195     progBuffer = (uint8_t *)malloc(LOAD_CHUNK);
02196     for (ii = 0; ii < length; ii += this_write) {
02197         this_write = min(LOAD_CHUNK, length - ii);
02198         
02199         for (int progIndex = 0; progIndex < this_write; progIndex++)
02200 #ifdef __SAM3X8E__
02201             progBuffer[progIndex] = firmware[ii + progIndex];
02202 #else
02203             progBuffer[progIndex] = pgm_read_byte(firmware + ii + progIndex);
02204 #endif
02205             
02206         if ((errCode = mpu_write_mem(ii, this_write, progBuffer))) {
02207 #ifdef MPU_DEBUG
02208             Serial.print("fimrware write failed: ");
02209             Serial.println(errCode);
02210 #endif
02211             return -3;
02212         }
02213         
02214         if (mpu_read_mem(ii, this_write, cur))
02215             return -4;
02216             
02217         if (memcmp(progBuffer, cur, this_write)) {
02218 #ifdef MPU_DEBUG
02219             Serial.print("Firmware compare failed addr "); Serial.println(ii);
02220             for (int i = 0; i < 10; i++) {
02221               Serial.print(progBuffer[i]); 
02222               Serial.print(" ");
02223             }
02224             Serial.println();
02225             for (int i = 0; i < 10; i++) {
02226               Serial.print(cur[i]); 
02227               Serial.print(" ");
02228             }
02229             Serial.println();
02230 #endif
02231             return -5;
02232         }
02233     }
02234 
02235     /* Set program start address. */
02236     tmp[0] = start_addr >> 8;
02237     tmp[1] = start_addr & 0xFF;
02238     if (i2c_write(st->hw->addr, st->reg->prgm_start_h, 2, tmp))
02239         return -6;
02240 
02241     st->chip_cfg.dmp_loaded = 1;
02242     st->chip_cfg.dmp_sample_rate = sample_rate;
02243 #ifdef MPU_DEBUG
02244     Serial.println("Firmware loaded");
02245 #endif
02246     return 0;
02247 }
02248 
02254 int mpu_set_dmp_state(unsigned char enable)
02255 {
02256     unsigned char tmp;
02257     if (st->chip_cfg.dmp_on == enable)
02258         return 0;
02259 
02260     if (enable) {
02261         if (!st->chip_cfg.dmp_loaded)
02262             return -1;
02263         /* Disable data ready interrupt. */
02264         set_int_enable(0);
02265         /* Disable bypass mode. */
02266         mpu_set_bypass(0);
02267         /* Keep constant sample rate, FIFO rate controlled by DMP. */
02268         mpu_set_sample_rate(st->chip_cfg.dmp_sample_rate);
02269         /* Remove FIFO elements. */
02270         tmp = 0;
02271         i2c_write(st->hw->addr, 0x23, 1, &tmp);
02272         st->chip_cfg.dmp_on = 1;
02273         /* Enable DMP interrupt. */
02274         set_int_enable(1);
02275         mpu_reset_fifo();
02276     } else {
02277         /* Disable DMP interrupt. */
02278         set_int_enable(0);
02279         /* Restore FIFO settings. */
02280         tmp = st->chip_cfg.fifo_enable;
02281         i2c_write(st->hw->addr, 0x23, 1, &tmp);
02282         st->chip_cfg.dmp_on = 0;
02283         mpu_reset_fifo();
02284     }
02285     return 0;
02286 }
02287 
02293 int mpu_get_dmp_state(unsigned char *enabled)
02294 {
02295     enabled[0] = st->chip_cfg.dmp_on;
02296     return 0;
02297 }
02298 
02299 
02300 /* This initialization is similar to the one in ak8975.c. */
02301 static int setup_compass(void)
02302 {
02303 #ifdef AK89xx_SECONDARY
02304     unsigned char data[4], akm_addr;
02305 
02306     mpu_set_bypass(1);
02307 
02308     /* Find compass. Possible addresses range from 0x0C to 0x0F. */
02309     for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
02310         int result;
02311         result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
02312         if (!result && (data[0] == AKM_WHOAMI))
02313             break;
02314     }
02315 
02316     if (akm_addr > 0x0F) {
02317         /* TODO: Handle this case in all compass-related functions. */
02318 #ifdef MPU_DEBUG
02319         Serial.println("Compass not found.");
02320 #endif
02321         return -1;
02322     }
02323 
02324     st->chip_cfg.compass_addr = akm_addr;
02325 
02326     data[0] = AKM_POWER_DOWN;
02327     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
02328         return -2;
02329     delay_ms(1);
02330 
02331     data[0] = AKM_FUSE_ROM_ACCESS;
02332     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
02333         return -3;
02334     delay_ms(1);
02335 
02336     /* Get sensitivity adjustment data from fuse ROM. */
02337     if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
02338         return -4;
02339     st->chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
02340     st->chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
02341     st->chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
02342 #ifdef MPU_DEBUG
02343     Serial.print("Compass sens: "); Serial.print(st->chip_cfg.mag_sens_adj[0]); Serial.print(" ");
02344     Serial.print(st->chip_cfg.mag_sens_adj[1]); Serial.print(" ");
02345     Serial.print(st->chip_cfg.mag_sens_adj[2]); Serial.println();
02346 #endif
02347 
02348     data[0] = AKM_POWER_DOWN;
02349     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
02350         return -5;
02351     delay_ms(1);
02352 
02353     mpu_set_bypass(0);
02354 
02355     /* Set up master mode, master clock, and ES bit. */
02356     data[0] = 0x40;
02357     if (i2c_write(st->hw->addr, st->reg->i2c_mst, 1, data))
02358         return -6;
02359 
02360     /* Slave 0 reads from AKM data registers. */
02361     data[0] = BIT_I2C_READ | st->chip_cfg.compass_addr;
02362     if (i2c_write(st->hw->addr, st->reg->s0_addr, 1, data))
02363         return -7;
02364 
02365     /* Compass reads start at this register. */
02366     data[0] = AKM_REG_ST1;
02367     if (i2c_write(st->hw->addr, st->reg->s0_reg, 1, data))
02368         return -8;
02369 
02370     /* Enable slave 0, 8-byte reads. */
02371     data[0] = BIT_SLAVE_EN | 8;
02372     if (i2c_write(st->hw->addr, st->reg->s0_ctrl, 1, data))
02373         return -9;
02374 
02375     /* Slave 1 changes AKM measurement mode. */
02376     data[0] = st->chip_cfg.compass_addr;
02377     if (i2c_write(st->hw->addr, st->reg->s1_addr, 1, data))
02378         return -10;
02379 
02380     /* AKM measurement mode register. */
02381     data[0] = AKM_REG_CNTL;
02382     if (i2c_write(st->hw->addr, st->reg->s1_reg, 1, data))
02383         return -11;
02384 
02385     /* Enable slave 1, 1-byte writes. */
02386     data[0] = BIT_SLAVE_EN | 1;
02387     if (i2c_write(st->hw->addr, st->reg->s1_ctrl, 1, data))
02388         return -12;
02389 
02390     /* Set slave 1 data. */
02391     data[0] = AKM_SINGLE_MEASUREMENT;
02392     if (i2c_write(st->hw->addr, st->reg->s1_do, 1, data))
02393         return -13;
02394 
02395     /* Trigger slave 0 and slave 1 actions at each sample. */
02396     data[0] = 0x03;
02397     if (i2c_write(st->hw->addr, st->reg->i2c_delay_ctrl, 1, data))
02398         return -14;
02399 
02400 #ifdef MPU9150
02401     /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
02402     data[0] = BIT_I2C_MST_VDDIO;
02403     if (i2c_write(st->hw->addr, st->reg->yg_offs_tc, 1, data))
02404         return -15;
02405 #endif
02406 
02407     return 0;
02408 #else
02409     return -16;
02410 #endif
02411 }
02412 
02419 int mpu_get_compass_reg(short *data, unsigned long *timestamp)
02420 {
02421 #ifdef AK89xx_SECONDARY
02422     unsigned char tmp[9];
02423 
02424     if (!(st->chip_cfg.sensors & INV_XYZ_COMPASS))
02425         return -1;
02426 
02427 #ifdef AK89xx_BYPASS
02428     if (i2c_read(st->chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
02429         return -2;
02430     tmp[8] = AKM_SINGLE_MEASUREMENT;
02431     if (i2c_write(st->chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
02432         return -3;
02433 #else
02434     if (i2c_read(st->hw->addr, st->reg->raw_compass, 8, tmp))
02435         return -4;
02436 #endif
02437 
02438 #if defined AK8975_SECONDARY
02439     /* AK8975 doesn't have the overrun error bit. */
02440     if (!(tmp[0] & AKM_DATA_READY))
02441         return -5;
02442     if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
02443         return -6;
02444 #elif defined AK8963_SECONDARY
02445     /* AK8963 doesn't have the data read error bit. */
02446     if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
02447         return -7;
02448     if (tmp[7] & AKM_OVERFLOW)
02449         return -8;
02450 #endif
02451     data[0] = ((unsigned short)tmp[2] << 8) | (unsigned short)tmp[1];
02452     data[1] = ((unsigned short)tmp[4] << 8) | (unsigned short)tmp[3];
02453     data[2] = ((unsigned short)tmp[6] << 8) | (unsigned short)tmp[5];
02454 
02455     data[0] = ((long)data[0] * st->chip_cfg.mag_sens_adj[0]) >> 8;
02456     data[1] = ((long)data[1] * st->chip_cfg.mag_sens_adj[1]) >> 8;
02457     data[2] = ((long)data[2] * st->chip_cfg.mag_sens_adj[2]) >> 8;
02458 
02459     if (timestamp)
02460         get_ms(timestamp);
02461     return 0;
02462 #else
02463     return -9;
02464 #endif
02465 }
02466 
02472 int mpu_get_compass_fsr(unsigned short *fsr)
02473 {
02474 #ifdef AK89xx_SECONDARY
02475     fsr[0] = st->hw->compass_fsr;
02476     return 0;
02477 #else
02478     return -1;
02479 #endif
02480 }
02481 
02526 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
02527     unsigned char lpa_freq)
02528 {
02529     unsigned char data[3];
02530 
02531     if (lpa_freq) {
02532         unsigned char thresh_hw;
02533 
02534 #if defined MPU6050
02535         /* TODO: Make these const/#defines. */
02536         /* 1LSb = 32mg. */
02537         if (thresh > 8160)
02538             thresh_hw = 255;
02539         else if (thresh < 32)
02540             thresh_hw = 1;
02541         else
02542             thresh_hw = thresh >> 5;
02543 #elif defined MPU6500
02544         /* 1LSb = 4mg. */
02545         if (thresh > 1020)
02546             thresh_hw = 255;
02547         else if (thresh < 4)
02548             thresh_hw = 1;
02549         else
02550             thresh_hw = thresh >> 2;
02551 #endif
02552 
02553         if (!time)
02554             /* Minimum duration must be 1ms. */
02555             time = 1;
02556 
02557 #if defined MPU6050
02558         if (lpa_freq > 40)
02559 #elif defined MPU6500
02560         if (lpa_freq > 640)
02561 #endif
02562             /* At this point, the chip has not been re-configured, so the
02563              * function can safely exit.
02564              */
02565             return -1;
02566 
02567         if (!st->chip_cfg.int_motion_only) {
02568             /* Store current settings for later. */
02569             if (st->chip_cfg.dmp_on) {
02570                 mpu_set_dmp_state(0);
02571                 st->chip_cfg.cache.dmp_on = 1;
02572             } else
02573                 st->chip_cfg.cache.dmp_on = 0;
02574             mpu_get_gyro_fsr(&st->chip_cfg.cache.gyro_fsr);
02575             mpu_get_accel_fsr(&st->chip_cfg.cache.accel_fsr);
02576             mpu_get_lpf(&st->chip_cfg.cache.lpf);
02577             mpu_get_sample_rate(&st->chip_cfg.cache.sample_rate);
02578             st->chip_cfg.cache.sensors_on = st->chip_cfg.sensors;
02579             mpu_get_fifo_config(&st->chip_cfg.cache.fifo_sensors);
02580         }
02581 
02582 #ifdef MPU6050
02583         /* Disable hardware interrupts for now. */
02584         set_int_enable(0);
02585 
02586         /* Enter full-power accel-only mode. */
02587         mpu_lp_accel_mode(0);
02588 
02589         /* Override current LPF (and HPF) settings to obtain a valid accel
02590          * reading.
02591          */
02592         data[0] = INV_FILTER_256HZ_NOLPF2;
02593         if (i2c_write(st->hw->addr, st->reg->lpf, 1, data))
02594             return -1;
02595 
02596         /* NOTE: Digital high pass filter should be configured here. Since this
02597          * driver doesn't modify those bits anywhere, they should already be
02598          * cleared by default.
02599          */
02600 
02601         /* Configure the device to send motion interrupts. */
02602         /* Enable motion interrupt. */
02603         data[0] = BIT_MOT_INT_EN;
02604         if (i2c_write(st->hw->addr, st->reg->int_enable, 1, data))
02605             goto lp_int_restore;
02606 
02607         /* Set motion interrupt parameters. */
02608         data[0] = thresh_hw;
02609         data[1] = time;
02610         if (i2c_write(st->hw->addr, st->reg->motion_thr, 2, data))
02611             goto lp_int_restore;
02612 
02613         /* Force hardware to "lock" current accel sample. */
02614         delay_ms(5);
02615         data[0] = (st->chip_cfg.accel_fsr << 3) | BITS_HPF;
02616         if (i2c_write(st->hw->addr, st->reg->accel_cfg, 1, data))
02617             goto lp_int_restore;
02618 
02619         /* Set up LP accel mode. */
02620         data[0] = BIT_LPA_CYCLE;
02621         if (lpa_freq == 1)
02622             data[1] = INV_LPA_1_25HZ;
02623         else if (lpa_freq <= 5)
02624             data[1] = INV_LPA_5HZ;
02625         else if (lpa_freq <= 20)
02626             data[1] = INV_LPA_20HZ;
02627         else
02628             data[1] = INV_LPA_40HZ;
02629         data[1] = (data[1] << 6) | BIT_STBY_XYZG;
02630         if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 2, data))
02631             goto lp_int_restore;
02632 
02633         st->chip_cfg.int_motion_only = 1;
02634         return 0;
02635 #elif defined MPU6500
02636         /* Disable hardware interrupts. */
02637         set_int_enable(0);
02638 
02639         /* Enter full-power accel-only mode, no FIFO/DMP. */
02640         data[0] = 0;
02641         data[1] = 0;
02642         data[2] = BIT_STBY_XYZG;
02643         if (i2c_write(st->hw->addr, st->reg->user_ctrl, 3, data))
02644             goto lp_int_restore;
02645 
02646         /* Set motion threshold. */
02647         data[0] = thresh_hw;
02648         if (i2c_write(st->hw->addr, st->reg->motion_thr, 1, data))
02649             goto lp_int_restore;
02650 
02651         /* Set wake frequency. */
02652         if (lpa_freq == 1)
02653             data[0] = INV_LPA_1_25HZ;
02654         else if (lpa_freq == 2)
02655             data[0] = INV_LPA_2_5HZ;
02656         else if (lpa_freq <= 5)
02657             data[0] = INV_LPA_5HZ;
02658         else if (lpa_freq <= 10)
02659             data[0] = INV_LPA_10HZ;
02660         else if (lpa_freq <= 20)
02661             data[0] = INV_LPA_20HZ;
02662         else if (lpa_freq <= 40)
02663             data[0] = INV_LPA_40HZ;
02664         else if (lpa_freq <= 80)
02665             data[0] = INV_LPA_80HZ;
02666         else if (lpa_freq <= 160)
02667             data[0] = INV_LPA_160HZ;
02668         else if (lpa_freq <= 320)
02669             data[0] = INV_LPA_320HZ;
02670         else
02671             data[0] = INV_LPA_640HZ;
02672         if (i2c_write(st->hw->addr, st->reg->lp_accel_odr, 1, data))
02673             goto lp_int_restore;
02674 
02675         /* Enable motion interrupt (MPU6500 version). */
02676         data[0] = BITS_WOM_EN;
02677         if (i2c_write(st->hw->addr, st->reg->accel_intel, 1, data))
02678             goto lp_int_restore;
02679 
02680         /* Enable cycle mode. */
02681         data[0] = BIT_LPA_CYCLE;
02682         if (i2c_write(st->hw->addr, st->reg->pwr_mgmt_1, 1, data))
02683             goto lp_int_restore;
02684 
02685         /* Enable interrupt. */
02686         data[0] = BIT_MOT_INT_EN;
02687         if (i2c_write(st->hw->addr, st->reg->int_enable, 1, data))
02688             goto lp_int_restore;
02689 
02690         st->chip_cfg.int_motion_only = 1;
02691         return 0;
02692 #endif
02693     } else {
02694         /* Don't "restore" the previous state if no state has been saved. */
02695         int ii;
02696         char *cache_ptr = (char*)&st->chip_cfg.cache;
02697         for (ii = 0; ii < sizeof(st->chip_cfg.cache); ii++) {
02698             if (cache_ptr[ii] != 0)
02699                 goto lp_int_restore;
02700         }
02701         /* If we reach this point, motion interrupt mode hasn't been used yet. */
02702         return -1;
02703     }
02704 lp_int_restore:
02705     /* Set to invalid values to ensure no I2C writes are skipped. */
02706     st->chip_cfg.gyro_fsr = 0xFF;
02707     st->chip_cfg.accel_fsr = 0xFF;
02708     st->chip_cfg.lpf = 0xFF;
02709     st->chip_cfg.sample_rate = 0xFFFF;
02710     st->chip_cfg.sensors = 0xFF;
02711     st->chip_cfg.fifo_enable = 0xFF;
02712     st->chip_cfg.clk_src = INV_CLK_PLL;
02713     mpu_set_sensors(st->chip_cfg.cache.sensors_on);
02714     mpu_set_gyro_fsr(st->chip_cfg.cache.gyro_fsr);
02715     mpu_set_accel_fsr(st->chip_cfg.cache.accel_fsr);
02716     mpu_set_lpf(st->chip_cfg.cache.lpf);
02717     mpu_set_sample_rate(st->chip_cfg.cache.sample_rate);
02718     mpu_configure_fifo(st->chip_cfg.cache.fifo_sensors);
02719 
02720     if (st->chip_cfg.cache.dmp_on)
02721         mpu_set_dmp_state(1);
02722 
02723 #ifdef MPU6500
02724     /* Disable motion interrupt (MPU6500 version). */
02725     data[0] = 0;
02726     if (i2c_write(st->hw->addr, st->reg->accel_intel, 1, data))
02727         goto lp_int_restore;
02728 #endif
02729 
02730     st->chip_cfg.int_motion_only = 0;
02731     return 0;
02732 }
02733 


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:55