標(biāo)題: stm32f103c8t6的mpu6050程序 [打印本頁]
作者: 韋杰123    時間: 2018-8-12 16:02
標(biāo)題: stm32f103c8t6的mpu6050程序
stm32f103c8 mpu6050

單片機(jī)源程序如下:
  1. /*
  2. $License:
  3.     Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
  4.     See included License.txt for License information.
  5. $
  6. */
  7. /**
  8. *  @addtogroup  DRIVERS Sensor Driver Layer
  9. *  @brief       Hardware drivers to communicate with sensors via I2C.
  10. *
  11. *  @{
  12. *      @file       inv_mpu.c
  13. *      @brief      An I2C-based driver for Invensense gyroscopes.
  14. *      @details    This driver currently works for the following devices:
  15. *                  MPU6050
  16. *                  MPU6500
  17. *                  MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)
  18. *                  MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)
  19. */
  20. #include <stdio.h>
  21. #include <stdint.h>
  22. #include <stdlib.h>
  23. #include <string.h>
  24. #include <math.h>
  25. #include "inv_mpu.h"
  26. #include "stm32f10x.h"
  27. #include "I2C.h"
  28. #include "delay.h"
  29. #include "Time.h"
  30. #include "usart.h"
  31. /* The following functions must be defined for this platform:
  32. * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
  33. *      unsigned char length, unsigned char const *data)
  34. * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
  35. *      unsigned char length, unsigned char *data)
  36. * delay_ms(unsigned long num_ms)
  37. * get_ms(unsigned long *count)
  38. * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
  39. * labs(long x)
  40. * fabsf(float x)
  41. * min(int a, int b)
  42. */
  43. #define MPU6050
  44. #define MOTION_DRIVER_TARGET_MSP430

  46. #if defined MOTION_DRIVER_TARGET_MSP430
  47. /*#include "msp430.h"
  48. #include "msp430_i2c.h"
  49. #include "msp430_clock.h"
  50. #include "msp430_interrupt.h" */

  52. #define i2c_write   i2cwrite
  53. #define i2c_read    i2cread
  54. #define delay_ms    delay_ms
  55. #define get_ms      get_ms

  57. //static int reg_int_cb(struct int_param_s *int_param)
  58. //{
  59. //    /*return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
  60. //        int_param->active_low);*/
  61. //                return 0;
  62. //}          
  63. //#define log_i(...)     do {} while (0)
  64. //#define log_e(...)     do {} while (0)
  65. #define log_e    PrintChar
  66. #define log_i    PrintChar
  67. /* labs is already defined by TI's toolchain. */
  68. /* fabs is for doubles. fabsf is for floats. */
  69. #define fabs        fabsf
  70. #define min(a,b) ((a<b)?a:b)

  72. #elif defined EMPL_TARGET_MSP430
  73. #include "msp430.h"
  74. #include "msp430_i2c.h"
  75. #include "msp430_clock.h"
  76. #include "msp430_interrupt.h"
  77. #include "log.h"
  78. #define i2c_write   msp430_i2c_write
  79. #define i2c_read    msp430_i2c_read
  80. #define delay_ms    msp430_delay_ms
  81. #define get_ms      msp430_get_clock_ms
  82. static inline int reg_int_cb(struct int_param_s *int_param)
  83. {
  84.     return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
  85.         int_param->active_low);
  86. }
  87. #define log_i       MPL_LOGI
  88. #define log_e       MPL_LOGE
  89. /* labs is already defined by TI's toolchain. */
  90. /* fabs is for doubles. fabsf is for floats. */
  91. #define fabs        fabsf
  92. #define min(a,b) ((a<b)?a:b)
  93. #elif defined EMPL_TARGET_UC3L0
  94. /* Instead of using the standard TWI driver from the ASF library, we're using
  95. * a TWI driver that follows the slave address + register address convention.
  96. */
  97. #include "twi.h"
  98. #include "delay.h"
  99. #include "sysclk.h"
  100. #include "log.h"
  101. #include "sensors_xplained.h"
  102. #include "uc3l0_clock.h"
  103. #define i2c_write(a, b, c, d)   twi_write(a, b, d, c)
  104. #define i2c_read(a, b, c, d)    twi_read(a, b, d, c)
  105. /* delay_ms is a function already defined in ASF. */
  106. #define get_ms  uc3l0_get_clock_ms
  107. static inline int reg_int_cb(struct int_param_s *int_param)
  108. {
  109.     sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
  110.     return 0;
  111. }
  112. #define log_i       MPL_LOGI
  113. #define log_e       MPL_LOGE
  114. /* UC3 is a 32-bit processor, so abs and labs are equivalent. */
  115. #define labs        abs
  116. #define fabs(x)     (((x)>0)?(x):-(x))
  117. #else
  118. //#error  Gyro driver is missing the system layer implementations.
  119. #endif

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

  125. /* Time for some messy macro work. =]
  126. * #define MPU9150
  127. * is equivalent to..
  128. * #define MPU6050
  129. * #define AK8975_SECONDARY
  130. *
  131. * #define MPU9250
  132. * is equivalent to..
  133. * #define MPU6500
  134. * #define AK8963_SECONDARY
  135. */
  136. #if defined MPU9150
  137. #ifndef MPU6050
  138. #define MPU6050
  139. #endif                          /* #ifndef MPU6050 */
  140. #if defined AK8963_SECONDARY
  141. #error "MPU9150 and AK8963_SECONDARY cannot both be defined."
  142. #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
  143. #define AK8975_SECONDARY
  144. #endif                          /* #if defined AK8963_SECONDARY */
  145. #elif defined MPU9250           /* #if defined MPU9150 */
  146. #ifndef MPU6500
  147. #define MPU6500
  148. #endif                          /* #ifndef MPU6500 */
  149. #if defined AK8975_SECONDARY
  150. #error "MPU9250 and AK8975_SECONDARY cannot both be defined."
  151. #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
  152. #define AK8963_SECONDARY
  153. #endif                          /* #if defined AK8975_SECONDARY */
  154. #endif                          /* #if defined MPU9150 */

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

  162. static int set_int_enable(unsigned char enable);

  164. /* Hardware registers needed by driver. */
  165. struct gyro_reg_s {
  166.     unsigned char who_am_i;
  167.     unsigned char rate_div;
  168.     unsigned char lpf;
  169.     unsigned char prod_id;
  170.     unsigned char user_ctrl;
  171.     unsigned char fifo_en;
  172.     unsigned char gyro_cfg;
  173.     unsigned char accel_cfg;

  175.     //unsigned char accel_cfg2;

  177.     //unsigned char lp_accel_odr;

  179.     unsigned char motion_thr;
  180.     unsigned char motion_dur;
  181.     unsigned char fifo_count_h;
  182.     unsigned char fifo_r_w;
  183.     unsigned char raw_gyro;
  184.     unsigned char raw_accel;
  185.     unsigned char temp;
  186.     unsigned char int_enable;
  187.     unsigned char dmp_int_status;
  188.     unsigned char int_status;

  190.     //unsigned char accel_intel;

  192.     unsigned char pwr_mgmt_1;
  193.     unsigned char pwr_mgmt_2;
  194.     unsigned char int_pin_cfg;
  195.     unsigned char mem_r_w;
  196.     unsigned char accel_offs;
  197.     unsigned char i2c_mst;
  198.     unsigned char bank_sel;
  199.     unsigned char mem_start_addr;
  200.     unsigned char prgm_start_h;
  201. #if defined AK89xx_SECONDARY
  202.     unsigned char s0_addr;
  203.     unsigned char s0_reg;
  204.     unsigned char s0_ctrl;
  205.     unsigned char s1_addr;
  206.     unsigned char s1_reg;
  207.     unsigned char s1_ctrl;
  208.     unsigned char s4_ctrl;
  209.     unsigned char s0_do;
  210.     unsigned char s1_do;
  211.     unsigned char i2c_delay_ctrl;
  212.     unsigned char raw_compass;
  213.     /* The I2C_MST_VDDIO bit is in this register. */
  214.     unsigned char yg_offs_tc;
  215. #endif
  216. };

  218. /* Information specific to a particular device. */
  219. struct hw_s {
  220.     unsigned char addr;
  221.     unsigned short max_fifo;
  222.     unsigned char num_reg;
  223.     unsigned short temp_sens;
  224.     short temp_offset;
  225.     unsigned short bank_size;
  226. #if defined AK89xx_SECONDARY
  227.     unsigned short compass_fsr;
  228. #endif
  229. };

  231. /* When entering motion interrupt mode, the driver keeps track of the
  232. * previous state so that it can be restored at a later time.
  233. * TODO: This is tacky. Fix it.
  234. */
  235. struct motion_int_cache_s {
  236.     unsigned short gyro_fsr;
  237.     unsigned char accel_fsr;
  238.     unsigned short lpf;
  239.     unsigned short sample_rate;
  240.     unsigned char sensors_on;
  241.     unsigned char fifo_sensors;
  242.     unsigned char dmp_on;
  243. };

  245. /* Cached chip configuration data.
  246. * TODO: A lot of these can be handled with a bitmask.
  247. */
  248. struct chip_cfg_s {
  249.     /* Matches gyro_cfg >> 3 & 0x03 */
  250.     unsigned char gyro_fsr;
  251.     /* Matches accel_cfg >> 3 & 0x03 */
  252.     unsigned char accel_fsr;
  253.     /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
  254.     unsigned char sensors;
  255.     /* Matches config register. */
  256.     unsigned char lpf;
  257.     unsigned char clk_src;
  258.     /* Sample rate, NOT rate divider. */
  259.     unsigned short sample_rate;
  260.     /* Matches fifo_en register. */
  261.     unsigned char fifo_enable;
  262.     /* Matches int enable register. */
  263.     unsigned char int_enable;
  264.     /* 1 if devices on auxiliary I2C bus appear on the primary. */
  265.     unsigned char bypass_mode;
  266.     /* 1 if half-sensitivity.
  267.      * NOTE: This doesn't belong here, but everything else in hw_s is const,
  268.      * and this allows us to save some precious RAM.
  269.      */
  270.     unsigned char accel_half;
  271.     /* 1 if device in low-power accel-only mode. */
  272.     unsigned char lp_accel_mode;
  273.     /* 1 if interrupts are only triggered on motion events. */
  274.     unsigned char int_motion_only;
  275.     struct motion_int_cache_s cache;
  276.     /* 1 for active low interrupts. */
  277.     unsigned char active_low_int;
  278.     /* 1 for latched interrupts. */
  279.     unsigned char latched_int;
  280.     /* 1 if DMP is enabled. */
  281.     unsigned char dmp_on;
  282.     /* Ensures that DMP will only be loaded once. */
  283.     unsigned char dmp_loaded;
  284.     /* Sampling rate used when DMP is enabled. */
  285.     unsigned short dmp_sample_rate;
  286. #ifdef AK89xx_SECONDARY
  287.     /* Compass sample rate. */
  288.     unsigned short compass_sample_rate;
  289.     unsigned char compass_addr;
  290.     short mag_sens_adj[3];
  291. #endif
  292. };

  294. /* Information for self-test. */
  295. struct test_s {
  296.     unsigned long gyro_sens;
  297.     unsigned long accel_sens;
  298.     unsigned char reg_rate_div;
  299.     unsigned char reg_lpf;
  300.     unsigned char reg_gyro_fsr;
  301.     unsigned char reg_accel_fsr;
  302.     unsigned short wait_ms;
  303.     unsigned char packet_thresh;
  304.     float min_dps;
  305.     float max_dps;
  306.     float max_gyro_var;
  307.     float min_g;
  308.     float max_g;
  309.     float max_accel_var;
  310. };

  312. /* Gyro driver state variables. */
  313. struct gyro_state_s {
  314.     const struct gyro_reg_s *reg;
  315.     const struct hw_s *hw;
  316.     struct chip_cfg_s chip_cfg;
  317.     const struct test_s *test;
  318. };

  320. /* Filter configurations. */
  321. enum lpf_e {
  322.     INV_FILTER_256HZ_NOLPF2 = 0,
  323.     INV_FILTER_188HZ,
  324.     INV_FILTER_98HZ,
  325.     INV_FILTER_42HZ,
  326.     INV_FILTER_20HZ,
  327.     INV_FILTER_10HZ,
  328.     INV_FILTER_5HZ,
  329.     INV_FILTER_2100HZ_NOLPF,
  330.     NUM_FILTER
  331. };

  333. /* Full scale ranges. */
  334. enum gyro_fsr_e {
  335.     INV_FSR_250DPS = 0,
  336.     INV_FSR_500DPS,
  337.     INV_FSR_1000DPS,
  338.     INV_FSR_2000DPS,
  339.     NUM_GYRO_FSR
  340. };

  342. /* Full scale ranges. */
  343. enum accel_fsr_e {
  344.     INV_FSR_2G = 0,
  345.     INV_FSR_4G,
  346.     INV_FSR_8G,
  347.     INV_FSR_16G,
  348.     NUM_ACCEL_FSR
  349. };

  351. /* Clock sources. */
  352. enum clock_sel_e {
  353.     INV_CLK_INTERNAL = 0,
  354.     INV_CLK_PLL,
  355.     NUM_CLK
  356. };

  358. /* Low-power accel wakeup rates. */
  359. enum lp_accel_rate_e {
  360. #if defined MPU6050
  361.     INV_LPA_1_25HZ,
  362.     INV_LPA_5HZ,
  363.     INV_LPA_20HZ,
  364.     INV_LPA_40HZ
  365. #elif defined MPU6500
  366.     INV_LPA_0_3125HZ,
  367.     INV_LPA_0_625HZ,
  368.     INV_LPA_1_25HZ,
  369.     INV_LPA_2_5HZ,
  370.     INV_LPA_5HZ,
  371.     INV_LPA_10HZ,
  372.     INV_LPA_20HZ,
  373.     INV_LPA_40HZ,
  374.     INV_LPA_80HZ,
  375.     INV_LPA_160HZ,
  376.     INV_LPA_320HZ,
  377.     INV_LPA_640HZ
  378. #endif
  379. };

  381. #define BIT_I2C_MST_VDDIO   (0x80)
  382. #define BIT_FIFO_EN         (0x40)
  383. #define BIT_DMP_EN          (0x80)
  384. #define BIT_FIFO_RST        (0x04)
  385. #define BIT_DMP_RST         (0x08)
  386. #define BIT_FIFO_OVERFLOW   (0x10)
  387. #define BIT_DATA_RDY_EN     (0x01)
  388. #define BIT_DMP_INT_EN      (0x02)
  389. #define BIT_MOT_INT_EN      (0x40)
  390. #define BITS_FSR            (0x18)
  391. #define BITS_LPF            (0x07)
  392. #define BITS_HPF            (0x07)
  393. #define BITS_CLK            (0x07)
  394. #define BIT_FIFO_SIZE_1024  (0x40)
  395. #define BIT_FIFO_SIZE_2048  (0x80)
  396. #define BIT_FIFO_SIZE_4096  (0xC0)
  397. #define BIT_RESET           (0x80)
  398. #define BIT_SLEEP           (0x40)
  399. #define BIT_S0_DELAY_EN     (0x01)
  400. #define BIT_S2_DELAY_EN     (0x04)
  401. #define BITS_SLAVE_LENGTH   (0x0F)
  402. #define BIT_SLAVE_BYTE_SW   (0x40)
  403. #define BIT_SLAVE_GROUP     (0x10)
  404. #define BIT_SLAVE_EN        (0x80)
  405. #define BIT_I2C_READ        (0x80)
  406. #define BITS_I2C_MASTER_DLY (0x1F)
  407. #define BIT_AUX_IF_EN       (0x20)
  408. #define BIT_ACTL            (0x80)
  409. #define BIT_LATCH_EN        (0x20)
  410. #define BIT_ANY_RD_CLR      (0x10)
  411. #define BIT_BYPASS_EN       (0x02)
  412. #define BITS_WOM_EN         (0xC0)
  413. #define BIT_LPA_CYCLE       (0x20)
  414. #define BIT_STBY_XA         (0x20)
  415. #define BIT_STBY_YA         (0x10)
  416. #define BIT_STBY_ZA         (0x08)
  417. #define BIT_STBY_XG         (0x04)
  418. #define BIT_STBY_YG         (0x02)
  419. #define BIT_STBY_ZG         (0x01)
  420. #define BIT_STBY_XYZA       (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
  421. #define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)

  423. #if defined AK8975_SECONDARY
  424. #define SUPPORTS_AK89xx_HIGH_SENS   (0x00)
  425. #define AK89xx_FSR                  (9830)
  426. #elif defined AK8963_SECONDARY
  427. #define SUPPORTS_AK89xx_HIGH_SENS   (0x10)
  428. #define AK89xx_FSR                  (4915)
  429. #endif

  431. #ifdef AK89xx_SECONDARY
  432. #define AKM_REG_WHOAMI      (0x00)

  434. #define AKM_REG_ST1         (0x02)
  435. #define AKM_REG_HXL         (0x03)
  436. #define AKM_REG_ST2         (0x09)

  438. #define AKM_REG_CNTL        (0x0A)
  439. #define AKM_REG_ASTC        (0x0C)
  440. #define AKM_REG_ASAX        (0x10)
  441. #define AKM_REG_ASAY        (0x11)
  442. #define AKM_REG_ASAZ        (0x12)

  444. #define AKM_DATA_READY      (0x01)
  445. #define AKM_DATA_OVERRUN    (0x02)
  446. #define AKM_OVERFLOW        (0x80)
  447. #define AKM_DATA_ERROR      (0x40)

  449. #define AKM_BIT_SELF_TEST   (0x40)

  451. #define AKM_POWER_DOWN          (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
  452. #define AKM_SINGLE_MEASUREMENT  (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
  453. #define AKM_FUSE_ROM_ACCESS     (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
  454. #define AKM_MODE_SELF_TEST      (0x08 | SUPPORTS_AK89xx_HIGH_SENS)

  456. #define AKM_WHOAMI      (0x48)
  457. #endif

  459. #if defined MPU6050
  460. /*
  461. const struct gyro_reg_s reg = {
  462.    .who_am_i       = 0x75,
  463.     .rate_div       = 0x19,
  464.     .lpf            = 0x1A,
  465.     .prod_id        = 0x0C,
  466.     .user_ctrl      = 0x6A,
  467.     .fifo_en        = 0x23,
  468.     .gyro_cfg       = 0x1B,
  469.     .accel_cfg      = 0x1C,
  470.     .motion_thr     = 0x1F,
  471.     .motion_dur     = 0x20,
  472.     .fifo_count_h   = 0x72,
  473.     .fifo_r_w       = 0x74,
  474.     .raw_gyro       = 0x43,
  475.     .raw_accel      = 0x3B,
  476.     .temp           = 0x41,
  477.     .int_enable     = 0x38,
  478.     .dmp_int_status = 0x39,
  479.     .int_status     = 0x3A,
  480.     .pwr_mgmt_1     = 0x6B,
  481.     .pwr_mgmt_2     = 0x6C,
  482.     .int_pin_cfg    = 0x37,
  483.     .mem_r_w        = 0x6F,
  484.     .accel_offs     = 0x06,
  485.     .i2c_mst        = 0x24,
  486.     .bank_sel       = 0x6D,
  487.     .mem_start_addr = 0x6E,
  488.     .prgm_start_h   = 0x70
  489. #ifdef AK89xx_SECONDARY
  490.     ,.raw_compass   = 0x49,
  491.     .yg_offs_tc     = 0x01,
  492.     .s0_addr        = 0x25,
  493.     .s0_reg         = 0x26,
  494.     .s0_ctrl        = 0x27,
  495.     .s1_addr        = 0x28,
  496.     .s1_reg         = 0x29,
  497.     .s1_ctrl        = 0x2A,
  498.     .s4_ctrl        = 0x34,
  499.     .s0_do          = 0x63,
  500.     .s1_do          = 0x64,
  501.     .i2c_delay_ctrl = 0x67
  502. #endif
  503. };
  504. const struct hw_s hw = {
  505.     .addr           = 0x68,
  506.     .max_fifo       = 1024,
  507.     .num_reg        = 118,
  508.     .temp_sens      = 340,
  509.     .temp_offset    = -521,
  510.     .bank_size      = 256
  511. #if defined AK89xx_SECONDARY
  512.     ,.compass_fsr    = AK89xx_FSR
  513. #endif
  514. };
  515. */
  516. const struct hw_s hw={
  517.   0x68,         //addr
  518.   1024,         //max_fifo
  519.   118,         //num_reg
  520.   340,         //temp_sens
  521.   -521,         //temp_offset
  522.   256         //bank_size
  523. };
  524. const struct gyro_reg_s reg = {
  525. 0x75,  //who_am_i
  526. 0x19,  //rate_div
  527. 0x1A,  //lpf
  528. 0x0C,  //prod_id
  529. 0x6A,  //user_ctrl
  530. 0x23,  //fifo_en
  531. 0x1B,  //gyro_cfg
  532. 0x1C,  //accel_cfg
  533. 0x1F,  // motion_thr
  534. 0x20,  // motion_dur
  535. 0x72,  // fifo_count_h
  536. 0x74,  // fifo_r_w
  537. 0x43,  // raw_gyro
  538. 0x3B,  // raw_accel
  539. 0x41,  // temp
  540. 0x38,  // int_enable
  541. 0x39,  //  dmp_int_status
  542. 0x3A,  //  int_status
  543. 0x6B,  // pwr_mgmt_1
  544. 0x6C,  // pwr_mgmt_2
  545. 0x37,  // int_pin_cfg
  546. 0x6F,  // mem_r_w
  547. 0x06,  // accel_offs
  548. 0x24,  // i2c_mst
  549. 0x6D,  // bank_sel
  550. 0x6E,  // mem_start_addr
  551. 0x70   // prgm_start_h
  552. };

  554. //const struct test_s test = {
  555. //    .gyro_sens      = 32768/250,
  556. //    .accel_sens     = 32768/16,          
  557. //    .reg_rate_div   = 0,    /* 1kHz. */
  558. //    .reg_lpf        = 1,    /* 188Hz. */
  559. //    .reg_gyro_fsr   = 0,    /* 250dps. */
  560. //    .reg_accel_fsr  = 0x18, /* 16g. */
  561. //    .wait_ms        = 50,
  562. //    .packet_thresh  = 5,    /* 5% */
  563. //    .min_dps        = 10.f,
  564. //    .max_dps        = 105.f,
  565. //    .max_gyro_var   = 0.14f,
  566. //    .min_g          = 0.3f,
  567. //    .max_g          = 0.95f,
  568. //    .max_accel_var  = 0.14f
  569. //};
  570. const struct test_s test={
  571. 32768/250,                 //gyro_sens
  572. 32768/16,                 //        accel_sens
  573. 0,                                 //        reg_rate_div
  574. 1,                                //        reg_lpf
  575. 0,                                 //        reg_gyro_fsr
  576. 0x18,                        //        reg_accel_fsr
  577. 50,                                //        wait_ms
  578. 5,                                //        packet_thresh
  579. 10.0f,                         //        min_dps
  580. 105.0f,                         //        max_dps
  581. 0.14f,                        //        max_gyro_var
  582. 0.3f,                   //        min_g
  583. 0.95f,                   //        max_g
  584. 0.14f                   //        max_accel_var
  585. };
  586. /*
  587. static struct gyro_state_s st = {
  588.     .reg = ®,
  589.     .hw = &hw,
  590.     .test = &test
  591. };        */
  592. static struct gyro_state_s st={
  593.   ®,
  594.   &hw,
  595.   {0},
  596.   &test
  597. };
  598. //st.chip_cfg.dmp_on = 1;
  599. //st.dhip_cfg.fifo_enabel = 1;
  600. /*
  601. #elif defined MPU6500
  602. const struct gyro_reg_s reg = {
  603.     .who_am_i       = 0x75,
  604.     .rate_div       = 0x19,
  605.     .lpf            = 0x1A,
  606.     .prod_id        = 0x0C,
  607.     .user_ctrl      = 0x6A,
  608.     .fifo_en        = 0x23,
  609.     .gyro_cfg       = 0x1B,
  610.     .accel_cfg      = 0x1C,
  611.     .accel_cfg2     = 0x1D,
  612.     .lp_accel_odr   = 0x1E,
  613.     .motion_thr     = 0x1F,
  614.     .motion_dur     = 0x20,
  615.     .fifo_count_h   = 0x72,
  616.     .fifo_r_w       = 0x74,
  617.     .raw_gyro       = 0x43,
  618.     .raw_accel      = 0x3B,
  619.     .temp           = 0x41,
  620.     .int_enable     = 0x38,
  621.     .dmp_int_status = 0x39,
  622.     .int_status     = 0x3A,
  623.     .accel_intel    = 0x69,
  624.     .pwr_mgmt_1     = 0x6B,
  625.     .pwr_mgmt_2     = 0x6C,
  626.     .int_pin_cfg    = 0x37,
  627.     .mem_r_w        = 0x6F,
  628.     .accel_offs     = 0x77,
  629.     .i2c_mst        = 0x24,
  630.     .bank_sel       = 0x6D,
  631.     .mem_start_addr = 0x6E,
  632.     .prgm_start_h   = 0x70
  633. #ifdef AK89xx_SECONDARY
  634.     ,.raw_compass   = 0x49,
  635.     .s0_addr        = 0x25,
  636.     .s0_reg         = 0x26,
  637.     .s0_ctrl        = 0x27,
  638.     .s1_addr        = 0x28,
  639.     .s1_reg         = 0x29,
  640.     .s1_ctrl        = 0x2A,
  641.     .s4_ctrl        = 0x34,
  642.     .s0_do          = 0x63,
  643.     .s1_do          = 0x64,
  644.     .i2c_delay_ctrl = 0x67
  645. #endif
  646. };
  647. const struct hw_s hw = {
  648.     .addr           = 0x68,
  649.     .max_fifo       = 1024,
  650.     .num_reg        = 128,
  651.     .temp_sens      = 321,
  652.     .temp_offset    = 0,
  653.     .bank_size      = 256
  654. #if defined AK89xx_SECONDARY
  655.     ,.compass_fsr    = AK89xx_FSR
  656. #endif
  657. };
  658. */
  659. //const struct test_s test = {
  660. //    .gyro_sens      = 32768/250,
  661. //    .accel_sens     = 32768/16,
  662. //    .reg_rate_div   = 0,    /* 1kHz. */
  663. //    .reg_lpf        = 1,    /* 188Hz. */
  664. //    .reg_gyro_fsr   = 0,    /* 250dps. */
  665. //    .reg_accel_fsr  = 0x18, /* 16g. */
  666. //    .wait_ms        = 50,
  667. //    .packet_thresh  = 5,    /* 5% */
  668. //    .min_dps        = 10.f,
  669. //    .max_dps        = 105.f,
  670. //    .max_gyro_var   = 0.14f,
  671. //    .min_g          = 0.3f,
  672. //    .max_g          = 0.95f,
  673. //    .max_accel_var  = 0.14f
  674. //};
  675. //
  676. //static struct gyro_state_s st = {
  677. //    .reg = ®,
  678. //    .hw = &hw,
  679. //    .test = &test
  680. //};
  681. #endif

  683. #define MAX_PACKET_LENGTH (12)

  685. #ifdef AK89xx_SECONDARY
  686. static int setup_compass(void);
  687. #define MAX_COMPASS_SAMPLE_RATE (100)
  688. #endif

  690. /**
  691. *  @brief      Enable/disable data ready interrupt.
  692. *  If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
  693. *  interrupt is used.
  694. *  @param[in]  enable      1 to enable interrupt.
  695. *  @return     0 if successful.
  696. */
  697. static int set_int_enable(unsigned char enable)
  698. {
  699.     unsigned char tmp;

  701.     if (st.chip_cfg.dmp_on) {
  702.         if (enable)
  703.             tmp = BIT_DMP_INT_EN;
  704.         else
  705.             tmp = 0x00;
  706.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
  707.             return -1;
  708.         st.chip_cfg.int_enable = tmp;
  709.     } else {
  710.         if (!st.chip_cfg.sensors)
  711.             return -1;
  712.         if (enable && st.chip_cfg.int_enable)
  713.             return 0;
  714.         if (enable)
  715.             tmp = BIT_DATA_RDY_EN;
  716.         else
  717.             tmp = 0x00;
  718.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
  719.             return -1;
  720.         st.chip_cfg.int_enable = tmp;
  721.     }
  722.     return 0;
  723. }

  725. /**
  726. *  @brief      Register dump for testing.
  727. *  @return     0 if successful.
  728. */
  729. int mpu_reg_dump(void)
  730. {
  731.     unsigned char ii;
  732.     unsigned char data;

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

  744. /**
  745. *  @brief      Read from a single register.
  746. *  NOTE: The memory and FIFO read/write registers cannot be accessed.
  747. *  @param[in]  reg     Register address.
  748. *  @param[out] data    Register data.
  749. *  @return     0 if successful.
  750. */
  751. int mpu_read_reg(unsigned char reg, unsigned char *data)
  752. {
  753.     if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
  754.         return -1;
  755.     if (reg >= st.hw->num_reg)
  756.         return -1;
  757.     return i2c_read(st.hw->addr, reg, 1, data);
  758. }

  760. /**
  761. *  @brief      Initialize hardware.
  762. *  Initial configuration:\n
  763. *  Gyro FSR: +/- 2000DPS\n
  764. *  Accel FSR +/- 2G\n
  765. *  DLPF: 42Hz\n
  766. *  FIFO rate: 50Hz\n
  767. *  Clock source: Gyro PLL\n
  768. *  FIFO: Disabled.\n
  769. *  Data ready interrupt: Disabled, active low, unlatched.
  770. *  @param[in]  int_param   Platform-specific parameters to interrupt API.
  771. *  @return     0 if successful.
  772. */
  773. int mpu_init(void)
  774. {
  775.     unsigned char data[6], rev;

  777.     /* Reset device. */
  778.     data[0] = 0x80;//BIT_RESET;
  779.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0])))
  780.         return -1;
  781.     delay_ms(100);

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

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

  795.     if (rev) {
  796.         /* Congrats, these parts are better. */
  797.         if (rev == 1)
  798.             st.chip_cfg.accel_half = 1;
  799.         else if (rev == 2)
  800.             st.chip_cfg.accel_half = 0;
  801.         else {
  802.             log_e("Unsupported software product rev %d.\n");
  803.             return -1;
  804.         }
  805.     }
  806.         else {
  807.         if (i2c_read(st.hw->addr, st.reg->prod_id, 1, &(data[0])))
  808.             return -1;
  809.         rev = data[0] & 0x0F;
  810.         if (!rev) {
  811.             log_e(" Product ID read as 0 indicates device is either "
  812.                 "incompatible or an MPU3050.\n");
  813.             return -1;
  814.         } else if (rev == 4) {
  815.             log_i("Half sensitivity part found.\n");
  816.             st.chip_cfg.accel_half = 1;
  817.         } else
  818.             st.chip_cfg.accel_half = 0;
  819.     }
  820. #elif defined MPU6500

  822. #define MPU6500_MEM_REV_ADDR    (0x17)
  823.     if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
  824.         return -1;
  825.     if (rev == 0x1)
  826.         st.chip_cfg.accel_half = 0;
  827.     else {
  828.         log_e("Unsupported software product rev %d.\n", rev);
  829.         return -1;
  830.     }

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

  840.     /* Set to invalid values to ensure no I2C writes are skipped. */
  841.     st.chip_cfg.sensors = 0xFF;
  842.     st.chip_cfg.gyro_fsr = 0xFF;
  843.     st.chip_cfg.accel_fsr = 0xFF;
  844.     st.chip_cfg.lpf = 0xFF;
  845.     st.chip_cfg.sample_rate = 0xFFFF;
  846.     st.chip_cfg.fifo_enable = 0xFF;
  847.     st.chip_cfg.bypass_mode = 0xFF;
  848. #ifdef AK89xx_SECONDARY
  849.     st.chip_cfg.compass_sample_rate = 0xFFFF;
  850. #endif
  851.     /* mpu_set_sensors always preserves this setting. */
  852.     st.chip_cfg.clk_src = INV_CLK_PLL;
  853.     /* Handled in next call to mpu_set_bypass. */
  854.     st.chip_cfg.active_low_int = 1;
  855.     st.chip_cfg.latched_int = 0;
  856.     st.chip_cfg.int_motion_only = 0;
  857.     st.chip_cfg.lp_accel_mode = 0;
  858.     memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
  859.     st.chip_cfg.dmp_on = 0;
  860.     st.chip_cfg.dmp_loaded = 0;
  861.     st.chip_cfg.dmp_sample_rate = 0;

  863.     if (mpu_set_gyro_fsr(2000))
  864.         return -1;
  865.     if (mpu_set_accel_fsr(2))
  866.         return -1;
  867.     if (mpu_set_lpf(42))
  868.         return -1;
  869.     if (mpu_set_sample_rate(50))
  870.         return -1;
  871.     if (mpu_configure_fifo(0))
  872.         return -1;

  874.     /*if (int_param)
  875.         reg_int_cb(int_param);*/

  877. #ifdef AK89xx_SECONDARY
  878.     setup_compass();
  879.     if (mpu_set_compass_sample_rate(10))
  880.         return -1;
  881. #else
  882.     /* Already disabled by setup_compass. */
  883.     if (mpu_set_bypass(0))
  884.         return -1;
  885. #endif

  887.     mpu_set_sensors(0);
  888.     return 0;
  889. }

  891. /**
  892. *  @brief      Enter low-power accel-only mode.
  893. *  In low-power accel mode, the chip goes to sleep and only wakes up to sample
  894. *  the accelerometer at one of the following frequencies:
  895. *  \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
  896. *  \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
  897. *  \n If the requested rate is not one listed above, the device will be set to
  898. *  the next highest rate. Requesting a rate above the maximum supported
  899. *  frequency will result in an error.
  900. *  \n To select a fractional wake-up frequency, round down the value passed to
  901. *  @e rate.
  902. *  @param[in]  rate        Minimum sampling rate, or zero to disable LP
  903. *                          accel mode.
  904. *  @return     0 if successful.
  905. */
  906. int mpu_lp_accel_mode(unsigned char rate)
  907. {
  908.     unsigned char tmp[2];

  910.     if (rate > 40)
  911.         return -1;

  913.     if (!rate) {
  914.         mpu_set_int_latched(0);
  915.         tmp[0] = 0;
  916.         tmp[1] = BIT_STBY_XYZG;
  917.         if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
  918.             return -1;
  919.         st.chip_cfg.lp_accel_mode = 0;
  920.         return 0;
  921.     }
  922.     /* For LP accel, we automatically configure the hardware to produce latched
  923.      * interrupts. In LP accel mode, the hardware cycles into sleep mode before
  924.      * it gets a chance to deassert the interrupt pin; therefore, we shift this
  925.      * responsibility over to the MCU.
  926.      *
  927.      * Any register read will clear the interrupt.
  928.      */
  929.     mpu_set_int_latched(1);
  930. #if defined MPU6050
  931.     tmp[0] = BIT_LPA_CYCLE;
  932.     if (rate == 1) {
  933.         tmp[1] = INV_LPA_1_25HZ;
  934.         mpu_set_lpf(5);
  935.     } else if (rate <= 5) {
  936.         tmp[1] = INV_LPA_5HZ;
  937.         mpu_set_lpf(5);
  938.     } else if (rate <= 20) {
  939.         tmp[1] = INV_LPA_20HZ;
  940.         mpu_set_lpf(10);
  941.     } else {
  942.         tmp[1] = INV_LPA_40HZ;
  943.         mpu_set_lpf(20);
  944.     }
  945.     tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
  946.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
  947.         return -1;
  948. #elif defined MPU6500
  949.     /* Set wake frequency. */
  950.     if (rate == 1)
  951.         tmp[0] = INV_LPA_1_25HZ;
  952.     else if (rate == 2)
  953.         tmp[0] = INV_LPA_2_5HZ;
  954.     else if (rate <= 5)
  955.         tmp[0] = INV_LPA_5HZ;
  956.     else if (rate <= 10)
  957.         tmp[0] = INV_LPA_10HZ;
  958.     else if (rate <= 20)
  959.         tmp[0] = INV_LPA_20HZ;
  960.     else if (rate <= 40)
  961.         tmp[0] = INV_LPA_40HZ;
  962.     else if (rate <= 80)
  963.         tmp[0] = INV_LPA_80HZ;
  964.     else if (rate <= 160)
  965.         tmp[0] = INV_LPA_160HZ;
  966.     else if (rate <= 320)
  967.         tmp[0] = INV_LPA_320HZ;
  968.     else
  969.         tmp[0] = INV_LPA_640HZ;
  970.     if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
  971.         return -1;
  972.     tmp[0] = BIT_LPA_CYCLE;
  973.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
  974.         return -1;
  975. #endif
  976.     st.chip_cfg.sensors = INV_XYZ_ACCEL;
  977.     st.chip_cfg.clk_src = 0;
  978.     st.chip_cfg.lp_accel_mode = 1;
  979.     mpu_configure_fifo(0);

  981.     return 0;
  982. }

  984. /**
  985. *  @brief      Read raw gyro data directly from the registers.
  986. *  @param[out] data        Raw data in hardware units.
  987. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  988. *  @return     0 if successful.
  989. */
  990. int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
  991. {
  992.     unsigned char tmp[6];

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

  997.     if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
  998.         return -1;
  999.     data[0] = (tmp[0] << 8) | tmp[1];
  1000.     data[1] = (tmp[2] << 8) | tmp[3];
  1001.     data[2] = (tmp[4] << 8) | tmp[5];
  1002.     if (timestamp)
  1003.         get_ms(timestamp);
  1004.     return 0;
  1005. }

  1007. /**
  1008. *  @brief      Read raw accel data directly from the registers.
  1009. *  @param[out] data        Raw data in hardware units.
  1010. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  1011. *  @return     0 if successful.
  1012. */
  1013. int mpu_get_accel_reg(short *data, unsigned long *timestamp)
  1014. {
  1015.     unsigned char tmp[6];

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

  1020.     if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
  1021.         return -1;
  1022.     data[0] = (tmp[0] << 8) | tmp[1];
  1023.     data[1] = (tmp[2] << 8) | tmp[3];
  1024.     data[2] = (tmp[4] << 8) | tmp[5];
  1025.     if (timestamp)
  1026.         get_ms(timestamp);
  1027.     return 0;
  1028. }

  1030. /**
  1031. *  @brief      Read temperature data directly from the registers.
  1032. *  @param[out] data        Data in q16 format.
  1033. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  1034. *  @return     0 if successful.
  1035. */
  1036. int mpu_get_temperature(long *data, unsigned long *timestamp)
  1037. {
  1038.     unsigned char tmp[2];
  1039.     short raw;

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

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

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

  1054. /**
  1055. *  @brief      Push biases to the accel bias registers.
  1056. *  This function expects biases relative to the current sensor output, and
  1057. *  these biases will be added to the factory-supplied values.
  1058. *  @param[in]  accel_bias  New biases.
  1059. *  @return     0 if successful.
  1060. */
  1061. int mpu_set_accel_bias(const long *accel_bias)
  1062. {
  1063.     unsigned char data[6];
  1064.     short accel_hw[3];
  1065.     short got_accel[3];
  1066.     short fg[3];

  1068.     if (!accel_bias)
  1069.         return -1;
  1070.     if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
  1071.         return 0;

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

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

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

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

  1090.     accel_hw[0] += got_accel[0];
  1091.     accel_hw[1] += got_accel[1];
  1092.     accel_hw[2] += got_accel[2];

  1094.     data[0] = (accel_hw[0] >> 8) & 0xff;
  1095.     data[1] = (accel_hw[0]) & 0xff;
  1096.     data[2] = (accel_hw[1] >> 8) & 0xff;
  1097.     data[3] = (accel_hw[1]) & 0xff;
  1098.     data[4] = (accel_hw[2] >> 8) & 0xff;
  1099.     data[5] = (accel_hw[2]) & 0xff;

  1101.     if (i2c_write(st.hw->addr, 0x06, 6, data))
  1102.         return -1;
  1103.     return 0;
  1104. }

  1106. /**
  1107. *  @brief  Reset FIFO read/write pointers.
  1108. *  @return 0 if successful.
  1109. */
  1110. int mpu_reset_fifo(void)
  1111. {
  1112.     unsigned char data;

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

  1117.     data = 0;
  1118.     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
  1119.         return -1;
  1120.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
  1121.         return -1;
  1122.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1123.         return -1;

  1125.     if (st.chip_cfg.dmp_on) {
  1126.         data = BIT_FIFO_RST | BIT_DMP_RST;
  1127.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1128.             return -1;
  1129.         delay_ms(50);
  1130.         data = BIT_DMP_EN | BIT_FIFO_EN;
  1131.         if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
  1132.             data |= BIT_AUX_IF_EN;
  1133.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1134.             return -1;
  1135.         if (st.chip_cfg.int_enable)
  1136.             data = BIT_DMP_INT_EN;
  1137.         else
  1138.             data = 0;
  1139.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
  1140.             return -1;
  1141.         data = 0;
  1142.         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
  1143.             return -1;
  1144.     } else {
  1145.         data = BIT_FIFO_RST;
  1146.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1147.             return -1;
  1148.         if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
  1149.             data = BIT_FIFO_EN;
  1150.         else
  1151.             data = BIT_FIFO_EN | BIT_AUX_IF_EN;
  1152.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1153.             return -1;
  1154.         delay_ms(50);
  1155.         if (st.chip_cfg.int_enable)
  1156.             data = BIT_DATA_RDY_EN;
  1157.         else
  1158.             data = 0;
  1159.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
  1160.             return -1;
  1161.         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
  1162.             return -1;
  1163.     }
  1164.     return 0;
  1165. }

  1167. /**
  1168. *  @brief      Get the gyro full-scale range.
  1169. *  @param[out] fsr Current full-scale range.
  1170. *  @return     0 if successful.
  1171. */
  1172. int mpu_get_gyro_fsr(unsigned short *fsr)
  1173. {
  1174.     switch (st.chip_cfg.gyro_fsr) {
  1175.     case INV_FSR_250DPS:
  1176.         fsr[0] = 250;
  1177.         break;
  1178.     case INV_FSR_500DPS:
  1179.         fsr[0] = 500;
  1180.         break;
  1181.     case INV_FSR_1000DPS:
  1182.         fsr[0] = 1000;
  1183.         break;
  1184.     case INV_FSR_2000DPS:
  1185.         fsr[0] = 2000;
  1186.         break;
  1187.     default:
  1188.         fsr[0] = 0;
  1189.         break;
  1190.     }
  1191.     return 0;
  1192. }

  1194. /**
  1195. *  @brief      Set the gyro full-scale range.
  1196. *  @param[in]  fsr Desired full-scale range.
  1197. *  @return     0 if successful.
  1198. */
  1199. int mpu_set_gyro_fsr(unsigned short fsr)
  1200. {
  1201.     unsigned char data;

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

  1206.     switch (fsr) {
  1207.     case 250:
  1208.         data = INV_FSR_250DPS << 3;
  1209.         break;
  1210.     case 500:
  1211.         data = INV_FSR_500DPS << 3;
  1212.         break;
  1213.     case 1000:
  1214.         data = INV_FSR_1000DPS << 3;
  1215.         break;
  1216.     case 2000:
  1217.         data = INV_FSR_2000DPS << 3;
  1218.         break;
  1219.     default:
  1220.         return -1;
  1221.     }

  1223.     if (st.chip_cfg.gyro_fsr == (data >> 3))
  1224.         return 0;
  1225.     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
  1226.         return -1;
  1227.     st.chip_cfg.gyro_fsr = data >> 3;
  1228.     return 0;
  1229. }

  1231. /**
  1232. *  @brief      Get the accel full-scale range.
  1233. *  @param[out] fsr Current full-scale range.
  1234. *  @return     0 if successful.
  1235. */
  1236. int mpu_get_accel_fsr(unsigned char *fsr)
  1237. {
  1238.     switch (st.chip_cfg.accel_fsr) {
  1239.     case INV_FSR_2G:
  1240.         fsr[0] = 2;
  1241.         break;
  1242.     case INV_FSR_4G:
  1243.         fsr[0] = 4;
  1244.         break;
  1245.     case INV_FSR_8G:
  1246.         fsr[0] = 8;
  1247.         break;
  1248.     case INV_FSR_16G:
  1249.         fsr[0] = 16;
  1250.         break;
  1251.     default:
  1252.         return -1;
  1253.     }
  1254.     if (st.chip_cfg.accel_half)
  1255.         fsr[0] <<= 1;
  1256.     return 0;
  1257. }

  1259. /**
  1260. *  @brief      Set the accel full-scale range.
  1261. *  @param[in]  fsr Desired full-scale range.
  1262. *  @return     0 if successful.
  1263. */
  1264. int mpu_set_accel_fsr(unsigned char fsr)
  1265. {
  1266.     unsigned char data;

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

  1271.     switch (fsr) {
  1272.     case 2:
  1273.         data = INV_FSR_2G << 3;
  1274.         break;
  1275.     case 4:
  1276.         data = INV_FSR_4G << 3;
  1277.         break;
  1278.     case 8:
  1279.         data = INV_FSR_8G << 3;
  1280.         break;
  1281.     case 16:
  1282.         data = INV_FSR_16G << 3;
  1283.         break;
  1284.     default:
  1285.         return -1;
  1286.     }

  1288.     if (st.chip_cfg.accel_fsr == (data >> 3))
  1289.         return 0;
  1290.     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
  1291.         return -1;
  1292.     st.chip_cfg.accel_fsr = data >> 3;
  1293.     return 0;
  1294. }

  1296. /**
  1297. *  @brief      Get the current DLPF setting.
  1298. *  @param[out] lpf Current LPF setting.
  1299. *  0 if successful.
  1300. */
  1301. int mpu_get_lpf(unsigned short *lpf)
  1302. {
  1303.     switch (st.chip_cfg.lpf) {
  1304.     case INV_FILTER_188HZ:
  1305.         lpf[0] = 188;
  1306.         break;
  1307.     case INV_FILTER_98HZ:
  1308.         lpf[0] = 98;
  1309.         break;
  1310.     case INV_FILTER_42HZ:
  1311.         lpf[0] = 42;
  1312.         break;
  1313.     case INV_FILTER_20HZ:
  1314.         lpf[0] = 20;
  1315.         break;
  1316.     case INV_FILTER_10HZ:
  1317.         lpf[0] = 10;
  1318.         break;
  1319.     case INV_FILTER_5HZ:
  1320.         lpf[0] = 5;
  1321.         break;
  1322.     case INV_FILTER_256HZ_NOLPF2:
  1323.     case INV_FILTER_2100HZ_NOLPF:
  1324.     default:
  1325.         lpf[0] = 0;
  1326.         break;
  1327.     }
  1328.     return 0;
  1329. }

  1331. /**
  1332. *  @brief      Set digital low pass filter.
  1333. *  The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
  1334. *  @param[in]  lpf Desired LPF setting.
  1335. *  @return     0 if successful.
  1336. */
  1337. int mpu_set_lpf(unsigned short lpf)
  1338. {
  1339.     unsigned char data;

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

  1344.     if (lpf >= 188)
  1345.         data = INV_FILTER_188HZ;
  1346.     else if (lpf >= 98)
  1347.         data = INV_FILTER_98HZ;
  1348.     else if (lpf >= 42)
  1349.         data = INV_FILTER_42HZ;
  1350.     else if (lpf >= 20)
  1351.         data = INV_FILTER_20HZ;
  1352.     else if (lpf >= 10)
  1353.         data = INV_FILTER_10HZ;
  1354.     else
  1355.         data = INV_FILTER_5HZ;

  1357.     if (st.chip_cfg.lpf == data)
  1358.         return 0;
  1359.     if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
  1360.         return -1;
  1361.     st.chip_cfg.lpf = data;
  1362.     return 0;
  1363. }

  1365. /**
  1366. *  @brief      Get sampling rate.
  1367. *  @param[out] rate    Current sampling rate (Hz).
  1368. *  @return     0 if successful.
  1369. */
  1370. int mpu_get_sample_rate(unsigned short *rate)
  1371. {
  1372.     if (st.chip_cfg.dmp_on)
  1373.         return -1;
  1374.     else
  1375.         rate[0] = st.chip_cfg.sample_rate;
  1376.     return 0;
  1377. }

  1379. /**
  1380. *  @brief      Set sampling rate.
  1381. *  Sampling rate must be between 4Hz and 1kHz.
  1382. *  @param[in]  rate    Desired sampling rate (Hz).
  1383. *  @return     0 if successful.
  1384. */
  1385. int mpu_set_sample_rate(unsigned short rate)
  1386. {
  1387.     unsigned char data;

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

  1392.     if (st.chip_cfg.dmp_on)
  1393.         return -1;
  1394.     else {
  1395.         if (st.chip_cfg.lp_accel_mode) {
  1396.             if (rate && (rate <= 40)) {
  1397.                 /* Just stay in low-power accel mode. */
  1398.                 mpu_lp_accel_mode(rate);
  1399.                 return 0;
  1400.             }
  1401.             /* Requested rate exceeds the allowed frequencies in LP accel mode,
  1402.              * switch back to full-power mode.
  1403.              */
  1404.             mpu_lp_accel_mode(0);
  1405.         }
  1406.         if (rate < 4)
  1407.             rate = 4;
  1408.         else if (rate > 1000)
  1409.             rate = 1000;

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

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

  1417. #ifdef AK89xx_SECONDARY
  1418.         mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
  1419. #endif

  1421.         /* Automatically set LPF to 1/2 sampling rate. */
  1422.         mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
  1423.         return 0;
  1424.     }
  1425. }

  1427. /**
  1428. *  @brief      Get compass sampling rate.
  1429. *  @param[out] rate    Current compass sampling rate (Hz).
  1430. *  @return     0 if successful.
  1431. */
  1432. int mpu_get_compass_sample_rate(unsigned short *rate)
  1433. {
  1434. #ifdef AK89xx_SECONDARY
  1435.     rate[0] = st.chip_cfg.compass_sample_rate;
  1436.     return 0;
  1437. #else
  1438.     rate[0] = 0;
  1439.     return -1;
  1440. #endif
  1441. }

  1443. /**
  1444. *  @brief      Set compass sampling rate.
  1445. *  The compass on the auxiliary I2C bus is read by the MPU hardware at a
  1446. *  maximum of 100Hz. The actual rate can be set to a fraction of the gyro
  1447. *  sampling rate.
  1448. *
  1449. *  \n WARNING: The new rate may be different than what was requested. Call
  1450. *  mpu_get_compass_sample_rate to check the actual setting.
  1451. *  @param[in]  rate    Desired compass sampling rate (Hz).
  1452. *  @return     0 if successful.
  1453. */
  1454. int mpu_set_compass_sample_rate(unsigned short rate)
  1455. {
  1456. #ifdef AK89xx_SECONDARY
  1457.     unsigned char div;
  1458.     if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
  1459.         return -1;

  1461.     div = st.chip_cfg.sample_rate / rate - 1;
  1462.     if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
  1463.         return -1;
  1464.     st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
  1465.     return 0;
  1466. #else
  1467.     return -1;
  1468. #endif
  1469. }

  1471. /**
  1472. *  @brief      Get gyro sensitivity scale factor.
  1473. *  @param[out] sens    Conversion from hardware units to dps.
  1474. *  @return     0 if successful.
  1475. */
  1476. int mpu_get_gyro_sens(float *sens)
  1477. {
  1478.     switch (st.chip_cfg.gyro_fsr) {
  1479.     case INV_FSR_250DPS:
  1480.         sens[0] = 131.f;
  1481.         break;
  1482.     case INV_FSR_500DPS:
  1483.         sens[0] = 65.5f;
  1484.         break;
  1485.     case INV_FSR_1000DPS:
  1486.         sens[0] = 32.8f;
  1487.         break;
  1488.     case INV_FSR_2000DPS:
  1489.         sens[0] = 16.4f;
  1490.         break;
  1491.     default:
  1492.         return -1;
  1493.     }
  1494.     return 0;
  1495. }

  1497. /**
  1498. *  @brief      Get accel sensitivity scale factor.
  1499. *  @param[out] sens    Conversion from hardware units to g's.
  1500. *  @return     0 if successful.
  1501. */
  1502. int mpu_get_accel_sens(unsigned short *sens)
  1503. {
  1504.     switch (st.chip_cfg.accel_fsr) {
  1505.     case INV_FSR_2G:
  1506.         sens[0] = 16384;
  1507.         break;
  1508.     case INV_FSR_4G:
  1509.         sens[0] = 8092;
  1510.         break;
  1511.     case INV_FSR_8G:
  1512.         sens[0] = 4096;
  1513.         break;
  1514.     case INV_FSR_16G:
  1515.         sens[0] = 2048;
  1516.         break;
  1517.     default:
  1518.         return -1;
  1519.     }
  1520.     if (st.chip_cfg.accel_half)
  1521.         sens[0] >>= 1;
  1522.     return 0;
  1523. }

  1525. /**
  1526. *  @brief      Get current FIFO configuration.
  1527. *  @e sensors can contain a combination of the following flags:
  1528. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1529. *  \n INV_XYZ_GYRO
  1530. *  \n INV_XYZ_ACCEL
  1531. *  @param[out] sensors Mask of sensors in FIFO.
  1532. *  @return     0 if successful.
  1533. */
  1534. int mpu_get_fifo_config(unsigned char *sensors)
  1535. {
  1536.     sensors[0] = st.chip_cfg.fifo_enable;
  1537.     return 0;
  1538. }

  1540. /**
  1541. *  @brief      Select which sensors are pushed to FIFO.
  1542. *  @e sensors can contain a combination of the following flags:
  1543. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1544. *  \n INV_XYZ_GYRO
  1545. *  \n INV_XYZ_ACCEL
  1546. *  @param[in]  sensors Mask of sensors to push to FIFO.
  1547. *  @return     0 if successful.
  1548. */
  1549. int mpu_configure_fifo(unsigned char sensors)
  1550. {
  1551.     unsigned char prev;
  1552.     int result = 0;

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

  1557.     if (st.chip_cfg.dmp_on)
  1558.         return 0;
  1559.     else {
  1560.         if (!(st.chip_cfg.sensors))
  1561.             return -1;
  1562.         prev = st.chip_cfg.fifo_enable;
  1563.         st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
  1564.         if (st.chip_cfg.fifo_enable != sensors)
  1565.             /* You're not getting what you asked for. Some sensors are
  1566.              * asleep.
  1567.              */
  1568.             result = -1;
  1569.         else
  1570.             result = 0;
  1571.         if (sensors || st.chip_cfg.lp_accel_mode)
  1572.             set_int_enable(1);
  1573.         else
  1574.             set_int_enable(0);
  1575.         if (sensors) {
  1576.             if (mpu_reset_fifo()) {
  1577.                 st.chip_cfg.fifo_enable = prev;
  1578.                 return -1;
  1579.             }
  1580.         }
  1581.     }

  1583.     return result;
  1584. }

  1586. /**
  1587. *  @brief      Get current power state.
  1588. *  @param[in]  power_on    1 if turned on, 0 if suspended.
  1589. *  @return     0 if successful.
  1590. */
  1591. int mpu_get_power_state(unsigned char *power_on)
  1592. {
  1593.     if (st.chip_cfg.sensors)
  1594.         power_on[0] = 1;
  1595.     else
  1596.         power_on[0] = 0;
  1597.     return 0;
  1598. }

  1600. /**
  1601. *  @brief      Turn specific sensors on/off.
  1602. *  @e sensors can contain a combination of the following flags:
  1603. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1604. *  \n INV_XYZ_GYRO
  1605. *  \n INV_XYZ_ACCEL
  1606. *  \n INV_XYZ_COMPASS
  1607. *  @param[in]  sensors    Mask of sensors to wake.
  1608. *  @return     0 if successful.
  1609. */
  1610. int mpu_set_sensors(unsigned char sensors)
  1611. {
  1612.     unsigned char data;
  1613. #ifdef AK89xx_SECONDARY
  1614.     unsigned char user_ctrl;
  1615. #endif

  1617.     if (sensors & INV_XYZ_GYRO)
  1618.         data = INV_CLK_PLL;
  1619.     else if (sensors)
  1620.         data = 0;
  1621.     else
  1622.         data = BIT_SLEEP;
  1623.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
  1624.         st.chip_cfg.sensors = 0;
  1625.         return -1;
  1626.     }
  1627.     st.chip_cfg.clk_src = data & ~BIT_SLEEP;

  1629.     data = 0;
  1630.     if (!(sensors & INV_X_GYRO))
  1631.         data |= BIT_STBY_XG;
  1632.     if (!(sensors & INV_Y_GYRO))
  1633.         data |= BIT_STBY_YG;
  1634.     if (!(sensors & INV_Z_GYRO))
  1635.         data |= BIT_STBY_ZG;
  1636.     if (!(sensors & INV_XYZ_ACCEL))
  1637.         data |= BIT_STBY_XYZA;
  1638.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
  1639.         st.chip_cfg.sensors = 0;
  1640.         return -1;
  1641.     }

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

  1647. #ifdef AK89xx_SECONDARY
  1648. #ifdef AK89xx_BYPASS
  1649.     if (sensors & INV_XYZ_COMPASS)
  1650.         mpu_set_bypass(1);
  1651.     else
  1652.         mpu_set_bypass(0);
  1653. #else
  1654.     if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
  1655.         return -1;
  1656.     /* Handle AKM power management. */
  1657.     if (sensors & INV_XYZ_COMPASS) {
  1658.         data = AKM_SINGLE_MEASUREMENT;
  1659.         user_ctrl |= BIT_AUX_IF_EN;
  1660.     } else {
  1661.         data = AKM_POWER_DOWN;
  1662.         user_ctrl &= ~BIT_AUX_IF_EN;
  1663.     }
  1664.     if (st.chip_cfg.dmp_on)
  1665.         user_ctrl |= BIT_DMP_EN;
  1666.     else
  1667.         user_ctrl &= ~BIT_DMP_EN;
  1668.     if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
  1669.         return -1;
  1670.     /* Enable/disable I2C master mode. */
  1671.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
  1672.         return -1;
  1673. #endif
  1674. #endif

  1676.     st.chip_cfg.sensors = sensors;
  1677.     st.chip_cfg.lp_accel_mode = 0;
  1678.     delay_ms(50);
  1679.     return 0;
  1680. }

  1682. /**
  1683. *  @brief      Read the MPU interrupt status registers.
  1684. *  @param[out] status  Mask of interrupt bits.
  1685. *  @return     0 if successful.
  1686. */
  1687. int mpu_get_int_status(short *status)
  1688. {
  1689.     unsigned char tmp[2];
  1690.     if (!st.chip_cfg.sensors)
  1691.         return -1;
  1692.     if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
  1693.         return -1;
  1694.     status[0] = (tmp[0] << 8) | tmp[1];
  1695.     return 0;
  1696. }

  1698. /**
  1699. *  @brief      Get one packet from the FIFO.
  1700. *  If @e sensors does not contain a particular sensor, disregard the data
  1701. *  returned to that pointer.
  1702. *  \n @e sensors can contain a combination of the following flags:
  1703. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1704. *  \n INV_XYZ_GYRO
  1705. *  \n INV_XYZ_ACCEL
  1706. *  \n If the FIFO has no new data, @e sensors will be zero.
  1707. *  \n If the FIFO is disabled, @e sensors will be zero and this function will
  1708. *  return a non-zero error code.
  1709. *  @param[out] gyro        Gyro data in hardware units.
  1710. *  @param[out] accel       Accel data in hardware units.
  1711. *  @param[out] timestamp   Timestamp in milliseconds.
  1712. *  @param[out] sensors     Mask of sensors read from FIFO.
  1713. *  @param[out] more        Number of remaining packets.
  1714. *  @return     0 if successful.
  1715. */
  1716. int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
  1717.         unsigned char *sensors, unsigned char *more)
  1718. {
  1719.     /* Assumes maximum packet size is gyro (6) + accel (6). */
  1720.     unsigned char data[MAX_PACKET_LENGTH];
  1721.     unsigned char packet_size = 0;
  1722.     unsigned short fifo_count, index = 0;

  1724.     if (st.chip_cfg.dmp_on)
  1725.         return -1;

  1727.     sensors[0] = 0;
  1728.     if (!st.chip_cfg.sensors)
  1729.         return -1;
  1730.     if (!st.chip_cfg.fifo_enable)
  1731.         return -1;

  1733.     if (st.chip_cfg.fifo_enable & INV_X_GYRO)
  1734.         packet_size += 2;
  1735.     if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
  1736.         packet_size += 2;
  1737.     if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
  1738.         packet_size += 2;
  1739.     if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
  1740.         packet_size += 6;

  1742.     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
  1743.         return -1;
  1744.     fifo_count = (data[0] << 8) | data[1];
  1745.     if (fifo_count < packet_size)
  1746.         return 0;
  1747. //    log_i("FIFO count: %hd\n", fifo_count);
  1748.     if (fifo_count > (st.hw->max_fifo >> 1)) {
  1749.         /* FIFO is 50% full, better check overflow bit. */
  1750.         if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
  1751.             return -1;
  1752.         if (data[0] & BIT_FIFO_OVERFLOW) {
  1753.             mpu_reset_fifo();
  1754.             return -2;
  1755.         }
  1756.     }
  1757.     get_ms((unsigned long*)timestamp);

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

  1764.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
  1765.         accel[0] = (data[index+0] << 8) | data[index+1];
  1766.         accel[1] = (data[index+2] << 8) | data[index+3];
  1767.         accel[2] = (data[index+4] << 8) | data[index+5];
  1768.         sensors[0] |= INV_XYZ_ACCEL;
  1769.         index += 6;
  1770.     }
  1771.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
  1772.         gyro[0] = (data[index+0] << 8) | data[index+1];
  1773.         sensors[0] |= INV_X_GYRO;
  1774.         index += 2;
  1775.     }
  1776.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
  1777.         gyro[1] = (data[index+0] << 8) | data[index+1];
  1778.         sensors[0] |= INV_Y_GYRO;
  1779.         index += 2;
  1780.     }
  1781.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
  1782.         gyro[2] = (data[index+0] << 8) | data[index+1];
  1783.         sensors[0] |= INV_Z_GYRO;
  1784.         index += 2;
  1785.     }

  1787.     return 0;
  1788. }

  1790. /**
  1791. *  @brief      Get one unparsed packet from the FIFO.
  1792. *  This function should be used if the packet is to be parsed elsewhere.
  1793. *  @param[in]  length  Length of one FIFO packet.
  1794. *  @param[in]  data    FIFO packet.
  1795. *  @param[in]  more    Number of remaining packets.
  1796. */
  1797. int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
  1798.     unsigned char *more)
  1799. {
  1800.     unsigned char tmp[2];
  1801.     unsigned short fifo_count;
  1802.     if (!st.chip_cfg.dmp_on)
  1803.         return -1;
  1804.     if (!st.chip_cfg.sensors)
  1805.         return -1;

  1807.     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
  1808.         return -1;
  1809.     fifo_count = (tmp[0] << 8) | tmp[1];
  1810.     if (fifo_count < length) {
  1811.         more[0] = 0;
  1812.         return -1;
  1813.     }
  1814.     if (fifo_count > (st.hw->max_fifo >> 1)) {
  1815.         /* FIFO is 50% full, better check overflow bit. */
  1816.         if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
  1817.             return -1;
  1818.         if (tmp[0] & BIT_FIFO_OVERFLOW) {
  1819.             mpu_reset_fifo();
  1820.             return -2;
  1821.         }
  1822.     }

  1824.     if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
  1825.         return -1;
  1826.     more[0] = fifo_count / length - 1;
  1827.     return 0;
  1828. }

  1830. /**
  1831. *  @brief      Set device to bypass mode.
  1832. *  @param[in]  bypass_on   1 to enable bypass mode.
  1833. *  @return     0 if successful.
  1834. */
  1835. int mpu_set_bypass(unsigned char bypass_on)
  1836. {
  1837.     unsigned char tmp;

  1839.     if (st.chip_cfg.bypass_mode == bypass_on)
  1840.         return 0;

  1842.     if (bypass_on) {
  1843.         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1844.             return -1;
  1845.         tmp &= ~BIT_AUX_IF_EN;
  1846.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1847.             return -1;
  1848.         delay_ms(3);
  1849.         tmp = BIT_BYPASS_EN;
  1850.         if (st.chip_cfg.active_low_int)
  1851.             tmp |= BIT_ACTL;
  1852.         if (st.chip_cfg.latched_int)
  1853.             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
  1854.         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
  1855.             return -1;
  1856.     } else {
  1857.         /* Enable I2C master mode if compass is being used. */
  1858.         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1859.             return -1;
  1860.         if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
  1861.             tmp |= BIT_AUX_IF_EN;
  1862.         else
  1863.             tmp &= ~BIT_AUX_IF_EN;
  1864.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1865.             return -1;
  1866.         delay_ms(3);
  1867.         if (st.chip_cfg.active_low_int)
  1868.             tmp = BIT_ACTL;
  1869.         else
  1870.             tmp = 0;
  1871.         if (st.chip_cfg.latched_int)
  1872.             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
  1873.         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
  1874.             return -1;
  1875.     }
  1876.     st.chip_cfg.bypass_mode = bypass_on;
  1877.     return 0;
  1878. }

  1880. /**
  1881. *  @brief      Set interrupt level.
  1882. *  @param[in]  active_low  1 for active low, 0 for active high.
  1883. *  @return     0 if successful.
  1884. */
  1885. int mpu_set_int_level(unsigned char active_low)
  1886. {
  1887.     st.chip_cfg.active_low_int = active_low;
  1888.     return 0;
  1889. }

  1891. /**
  1892. *  @brief      Enable latched interrupts.
  1893. *  Any MPU register will clear the interrupt.
  1894. *  @param[in]  enable  1 to enable, 0 to disable.
  1895. *  @return     0 if successful.
  1896. */
  1897. int mpu_set_int_latched(unsigned char enable)
  1898. {
  1899.     unsigned char tmp;
  1900.     if (st.chip_cfg.latched_int == enable)
  1901.         return 0;

  1903.     if (enable)
  1904.         tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
  1905.     else
  1906.         tmp = 0;
  1907.     if (st.chip_cfg.bypass_mode)
  1908.         tmp |= BIT_BYPASS_EN;
  1909.     if (st.chip_cfg.active_low_int)
  1910.         tmp |= BIT_ACTL;
  1911.     if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
  1912.         return -1;
  1913.     st.chip_cfg.latched_int = enable;
  1914.     return 0;
  1915. }

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

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

  1925.     shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
  1926.     shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
  1927.     shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
  1928.     for (ii = 0; ii < 3; ii++) {
  1929.         if (!shift_code[ii]) {
  1930.             st_shift[ii] = 0.f;
  1931.             continue;
  1932.         }
  1933.         /* Equivalent to..
  1934.          * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
  1935.          */
  1936.         st_shift[ii] = 0.34f;
  1937.         while (--shift_code[ii])
  1938.             st_shift[ii] *= 1.034f;
  1939.     }
  1940.     return 0;
  1941. }

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

  1948.     get_accel_prod_shift(st_shift);
  1949.     for(jj = 0; jj < 3; jj++) {
  1950.         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
  1951.         if (st_shift[jj]) {
  1952.             st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
  1953.             if (fabs(st_shift_var) > test.max_accel_var)
  1954.                 result |= 1 << jj;
  1955.         } else if ((st_shift_cust < test.min_g) ||
  1956.             (st_shift_cust > test.max_g))
  1957.             result |= 1 << jj;
  1958.     }

  1960.     return result;
  1961. }

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

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

  1972.     tmp[0] &= 0x1F;
  1973.     tmp[1] &= 0x1F;
  1974.     tmp[2] &= 0x1F;

  1976.     for (jj = 0; jj < 3; jj++) {
  1977.         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
  1978.         if (tmp[jj]) {
  1979.             st_shift = 3275.f / test.gyro_sens;
  1980.             while (--tmp[jj])
  1981.                 st_shift *= 1.046f;
  1982.             st_shift_var = st_shift_cust / st_shift - 1.f;
  1983.             if (fabs(st_shift_var) > test.max_gyro_var)
  1984.                 result |= 1 << jj;
  1985.         } else if ((st_shift_cust < test.min_dps) ||
  1986.             (st_shift_cust > test.max_dps))
  1987.             result |= 1 << jj;
  1988.     }
  1989.     return result;
  1990. }

  1992. #ifdef AK89xx_SECONDARY
  1993. static int compass_self_test(void)
  1994. {
  1995.     unsigned char tmp[6];
  1996.     unsigned char tries = 10;
  1997.     int result = 0x07;
  1998.     short data;

  2000.     mpu_set_bypass(1);

  2002.     tmp[0] = AKM_POWER_DOWN;
  2003.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
  2004.         return 0x07;
  2005.     tmp[0] = AKM_BIT_SELF_TEST;
  2006.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
  2007.         goto AKM_restore;
  2008.     tmp[0] = AKM_MODE_SELF_TEST;
  2009.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
  2010.         goto AKM_restore;

  2012.     do {
  2013.         delay_ms(10);
  2014.         if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
  2015.             goto AKM_restore;
  2016.         if (tmp[0] & AKM_DATA_READY)
  2017.             break;
  2018.     } while (tries--);
  2019.     if (!(tmp[0] & AKM_DATA_READY))
  2020.         goto AKM_restore;

  2022.     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
  2023.         goto AKM_restore;

  2025.     result = 0;
  2026.     data = (short)(tmp[1] << 8) | tmp[0];
  2027.     if ((data > 100) || (data < -100))
  2028.         result |= 0x01;
  2029.     data = (short)(tmp[3] << 8) | tmp[2];
  2030.     if ((data > 100) || (data < -100))
  2031.         result |= 0x02;
  2032.     data = (short)(tmp[5] << 8) | tmp[4];
  2033.     if ((data > -300) || (data < -1000))
  2034.         result |= 0x04;

  2036. AKM_restore:
  2037.     tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
  2038.     i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
  2039.     tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
  2040.     i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
  2041.     mpu_set_bypass(0);
  2042.     return result;
  2043. }
  2044. #endif
  2045. #endif

  2047. static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
  2048. {
  2049.     unsigned char data[MAX_PACKET_LENGTH];
  2050.     unsigned char packet_count, ii;
  2051.     unsigned short fifo_count;

  2053.     data[0] = 0x01;
  2054.     data[1] = 0;
  2055.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
  2056.         return -1;
  2057.     delay_ms(200);
  2058.     data[0] = 0;
  2059.     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
  2060.         return -1;
  2061.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
  2062.         return -1;
  2063.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
  2064.         return -1;
  2065.     if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
  2066.         return -1;
  2067.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
  2068.         return -1;
  2069.     data[0] = BIT_FIFO_RST | BIT_DMP_RST;
  2070.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
  2071.         return -1;
  2072.     delay_ms(15);
  2073.     data[0] = st.test->reg_lpf;
  2074.     if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
  2075.         return -1;
  2076.     data[0] = st.test->reg_rate_div;
  2077.     if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
  2078.         return -1;
  2079.     if (hw_test)
  2080.         data[0] = st.test->reg_gyro_fsr | 0xE0;
  2081.     else
  2082.         data[0] = st.test->reg_gyro_fsr;
  2083.     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
  2084.         return -1;

  2086.     if (hw_test)
  2087.         data[0] = st.test->reg_accel_fsr | 0xE0;
  2088.     else
  2089.         data[0] = test.reg_accel_fsr;
  2090.     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
  2091.         return -1;
  2092.     if (hw_test)
  2093.         delay_ms(200);

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

  2100.     data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
  2101.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
  2102.         return -1;
  2103.     delay_ms(test.wait_ms);
  2104.     data[0] = 0;
  2105.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
  2106.         return -1;

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

  2111.     fifo_count = (data[0] << 8) | data[1];
  2112.     packet_count = fifo_count / MAX_PACKET_LENGTH;
  2113.     gyro[0] = gyro[1] = gyro[2] = 0;
  2114.     accel[0] = accel[1] = accel[2] = 0;

  2116.     for (ii = 0; ii < packet_count; ii++) {
  2117.         short accel_cur[3], gyro_cur[3];
  2118.         if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
  2119.             return -1;
  2120.         accel_cur[0] = ((short)data[0] << 8) | data[1];
  2121.         accel_cur[1] = ((short)data[2] << 8) | data[3];
  2122.         accel_cur[2] = ((short)data[4] << 8) | data[5];
  2123.         accel[0] += (long)accel_cur[0];
  2124.         accel[1] += (long)accel_cur[1];
  2125.         accel[2] += (long)accel_cur[2];
  2126.         gyro_cur[0] = (((short)data[6] << 8) | data[7]);
  2127.         gyro_cur[1] = (((short)data[8] << 8) | data[9]);
  2128.         gyro_cur[2] = (((short)data[10] << 8) | data[11]);
  2129.         gyro[0] += (long)gyro_cur[0];
  2130.         gyro[1] += (long)gyro_cur[1];
  2131.         gyro[2] += (long)gyro_cur[2];
  2132.     }
  2133. #ifdef EMPL_NO_64BIT
  2134.     gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
  2135.     gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
  2136.     gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
  2137.     if (has_accel) {
  2138.         accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
  2139.             packet_count);
  2140.         accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
  2141.             packet_count);
  2142.         accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
  2143.             packet_count);
  2144.         /* Don't remove gravity! */
  2145.         accel[2] -= 65536L;
  2146.     }
  2147. #else
  2148.     gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
  2149.     gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
  2150.     gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
  2151.     accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
  2152.         packet_count);
  2153.     accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
  2154.         packet_count);
  2155.     accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
  2156.         packet_count);
  2157.     /* Don't remove gravity! */
  2158.     if (accel[2] > 0L)
  2159.         accel[2] -= 65536L;
  2160.     else
  2161.         accel[2] += 65536L;
  2162. #endif

  2164.     return 0;
  2165. }

  2167. /**
  2168. *  @brief      Trigger gyro/accel/compass self-test.
  2169. *  On success/error, the self-test returns a mask representing the sensor(s)
  2170. *  that failed. For each bit, a one (1) represents a "pass" case; conversely,
  2171. *  a zero (0) indicates a failure.
  2172. *
  2173. *  \n The mask is defined as follows:
  2174. *  \n Bit 0:   Gyro.
  2175. *  \n Bit 1:   Accel.
  2176. *  \n Bit 2:   Compass.
  2177. *
  2178. *  \n Currently, the hardware self-test is unsupported for MPU6500. However,
  2179. *  this function can still be used to obtain the accel and gyro biases.
  2180. *
  2181. *  \n This function must be called with the device either face-up or face-down
  2182. *  (z-axis is parallel to gravity).
  2183. *  @param[out] gyro        Gyro biases in q16 format.
  2184. *  @param[out] accel       Accel biases (if applicable) in q16 format.
  2185. *  @return     Result mask (see above).
  2186. */
  2187. int mpu_run_self_test(long *gyro, long *accel)
  2188. {
  2189. #ifdef MPU6050
  2190.     const unsigned char tries = 2;
  2191.     long gyro_st[3], accel_st[3];
  2192.     unsigned char accel_result, gyro_result;
  2193. #ifdef AK89xx_SECONDARY
  2194.     unsigned char compass_result;
  2195. #endif
  2196.     int ii;
  2197. #endif
  2198.     int result;
  2199.     unsigned char accel_fsr, fifo_sensors, sensors_on;
  2200.     unsigned short gyro_fsr, sample_rate, lpf;
  2201.     unsigned char dmp_was_on;

  2203.     if (st.chip_cfg.dmp_on) {
  2204.         mpu_set_dmp_state(0);
  2205.         dmp_was_on = 1;
  2206.     } else
  2207.         dmp_was_on = 0;

  2209.     /* Get initial settings. */
  2210.     mpu_get_gyro_fsr(&gyro_fsr);
  2211.     mpu_get_accel_fsr(&accel_fsr);
  2212.     mpu_get_lpf(&lpf);
  2213.     mpu_get_sample_rate(&sample_rate);
  2214.     sensors_on = st.chip_cfg.sensors;
  2215.     mpu_get_fifo_config(&fifo_sensors);

  2217.     /* For older chips, the self-test will be different. */
  2218. #if defined MPU6050
  2219.     for (ii = 0; ii < tries; ii++)
  2220.         if (!get_st_biases(gyro, accel, 0))
  2221.             break;
  2222.     if (ii == tries) {
  2223.         /* If we reach this point, we most likely encountered an I2C error.
  2224.          * We'll just report an error for all three sensors.
  2225.          */
  2226.         result = 0;
  2227.         goto restore;
  2228.     }
  2229.     for (ii = 0; ii < tries; ii++)
  2230.         if (!get_st_biases(gyro_st, accel_st, 1))
  2231.             break;
  2232.     if (ii == tries) {
  2233.         /* Again, probably an I2C error. */
  2234.         result = 0;
  2235.         goto restore;
  2236.     }
  2237.     accel_result = accel_self_test(accel, accel_st);
  2238.     gyro_result = gyro_self_test(gyro, gyro_st);

  2240.     result = 0;
  2241.     if (!gyro_result)
  2242.         result |= 0x01;
  2243.     if (!accel_result)
  2244.         result |= 0x02;

  2246. #ifdef AK89xx_SECONDARY
  2247.     compass_result = compass_self_test();
  2248.     if (!compass_result)
  2249.         result |= 0x04;
  2250. #endif
  2251. restore:
  2252. #elif defined MPU6500
  2253.     /* For now, this function will return a "pass" result for all three sensors
  2254.      * for compatibility with current test applications.
  2255.      */
  2256.     get_st_biases(gyro, accel, 0);
  2257.     result = 0x3;
  2258. #endif
  2259.     /* Set to invalid values to ensure no I2C writes are skipped. */
  2260.     st.chip_cfg.gyro_fsr = 0xFF;
  2261.     st.chip_cfg.accel_fsr = 0xFF;
  2262.     st.chip_cfg.lpf = 0xFF;
  2263.     st.chip_cfg.sample_rate = 0xFFFF;
  2264.     st.chip_cfg.sensors = 0xFF;
  2265.     st.chip_cfg.fifo_enable = 0xFF;
  2266.     st.chip_cfg.clk_src = INV_CLK_PLL;
  2267.     mpu_set_gyro_fsr(gyro_fsr);
  2268.     mpu_set_accel_fsr(accel_fsr);
  2269.     mpu_set_lpf(lpf);
  2270.     mpu_set_sample_rate(sample_rate);
  2271.     mpu_set_sensors(sensors_on);
  2272.     mpu_configure_fifo(fifo_sensors);

  2274.     if (dmp_was_on)
  2275.         mpu_set_dmp_state(1);

  2277.     return result;
  2278. }

  2280. /**
  2281. *  @brief      Write to the DMP memory.
  2282. *  This function prevents I2C writes past the bank boundaries. The DMP memory
  2283. *  is only accessible when the chip is awake.
  2284. *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
  2285. *  @param[in]  length      Number of bytes to write.
  2286. *  @param[in]  data        Bytes to write to memory.
  2287. *  @return     0 if successful.
  2288. */
  2289. int mpu_write_mem(unsigned short mem_addr, unsigned short length,
  2290.         unsigned char *data)
  2291. {
  2292.     unsigned char tmp[2];

  2294.     if (!data)
  2295.         return -1;
  2296.     if (!st.chip_cfg.sensors)
  2297.         return -1;

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

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

  2306.     if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
  2307.         return -1;
  2308.     if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
  2309.         return -1;
  2310.     return 0;
  2311. }

  2313. /**
  2314. *  @brief      Read from the DMP memory.
  2315. *  This function prevents I2C reads past the bank boundaries. The DMP memory
  2316. *  is only accessible when the chip is awake.
  2317. *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
  2318. *  @param[in]  length      Number of bytes to read.
  2319. *  @param[out] data        Bytes read from memory.
  2320. *  @return     0 if successful.
  2321. */
  2322. int mpu_read_mem(unsigned short mem_addr, unsigned short length,
  2323.         unsigned char *data)
  2324. {
  2325.     unsigned char tmp[2];

  2327.     if (!data)
  2328.         return -1;
  2329.     if (!st.chip_cfg.sensors)
  2330.         return -1;

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

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

  2339.     if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
  2340.         return -1;
  2341.     if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
  2342.         return -1;
  2343.     return 0;
  2344. }

  2346. /**
  2347. *  @brief      Load and verify DMP image.
  2348. *  @param[in]  length      Length of DMP image.
  2349. *  @param[in]  firmware    DMP code.
  2350. *  @param[in]  start_addr  Starting address of DMP code memory.
  2351. *  @param[in]  sample_rate Fixed sampling rate used when DMP is enabled.
  2352. *  @return     0 if successful.
  2353. */
  2354. int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
  2355.     unsigned short start_addr, unsigned short sample_rate)
  2356. {
  2357.     unsigned short ii;
  2358.     unsigned short this_write;
  2359.     /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
  2360. #define LOAD_CHUNK  (16)
  2361.     unsigned char cur[LOAD_CHUNK], tmp[2];

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

  2367.     if (!firmware)
  2368.         return -1;
  2369.     for (ii = 0; ii < length; ii += this_write) {
  2370.         this_write = min(LOAD_CHUNK, length - ii);
  2371.         if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
  2372.             return -1;
  2373.         if (mpu_read_mem(ii, this_write, cur))
  2374.             return -1;
  2375.         if (memcmp(firmware+ii, cur, this_write))
  2376.             return -2;
  2377.     }

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

  2385.     st.chip_cfg.dmp_loaded = 1;
  2386.     st.chip_cfg.dmp_sample_rate = sample_rate;
  2387.     return 0;
  2388. }

  2390. /**
  2391. *  @brief      Enable/disable DMP support.
  2392. *  @param[in]  enable  1 to turn on the DMP.
  2393. *  @return     0 if successful.
  2394. */
  2395. int mpu_set_dmp_state(unsigned char enable)
  2396. {
  2397.     unsigned char tmp;
  2398.     if (st.chip_cfg.dmp_on == enable)
  2399.         return 0;

  2401.     if (enable) {
  2402.         if (!st.chip_cfg.dmp_loaded)
  2403.             return -1;
  2404.         /* Disable data ready interrupt. */
  2405.         set_int_enable(0);
  2406.         /* Disable bypass mode. */
  2407.         mpu_set_bypass(0);
  2408.         /* Keep constant sample rate, FIFO rate controlled by DMP. */
  2409.         mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
  2410.         /* Remove FIFO elements. */
  2411.         tmp = 0;
  2412.         i2c_write(st.hw->addr, 0x23, 1, &tmp);
  2413.         st.chip_cfg.dmp_on = 1;
  2414.         /* Enable DMP interrupt. */
  2415.         set_int_enable(1);
  2416.         mpu_reset_fifo();
  2417.     } else {
  2418.         /* Disable DMP interrupt. */
  2419.         set_int_enable(0);
  2420.         /* Restore FIFO settings. */
  2421.         tmp = st.chip_cfg.fifo_enable;
  2422.         i2c_write(st.hw->addr, 0x23, 1, &tmp);
  2423.         st.chip_cfg.dmp_on = 0;
  2424.         mpu_reset_fifo();
  2425.     }
  2426.     return 0;
  2427. }

  2429. /**
  2430. *  @brief      Get DMP state.
  2431. *  @param[out] enabled 1 if enabled.
  2432. *  @return     0 if successful.
  2433. */
  2434. int mpu_get_dmp_state(unsigned char *enabled)
  2435. {
  2436.     enabled[0] = st.chip_cfg.dmp_on;
  2437.     return 0;
  2438. }


  2441. /* This initialization is similar to the one in ak8975.c. */
  2442. //static int setup_compass(void)
  2443. //{
  2444. //#ifdef AK89xx_SECONDARY
  2445. //    unsigned char data[4], akm_addr;
  2446. //
  2447. //    mpu_set_bypass(1);
  2448. //
  2449. //    /* Find compass. Possible addresses range from 0x0C to 0x0F. */
  2450. //    for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
  2451. //        int result;
  2452. //        result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
  2453. //        if (!result && (data[0] == AKM_WHOAMI))
  2454. //            break;
  2455. //    }
  2456. //
  2457. //    if (akm_addr > 0x0F) {
  2458. //        /* TODO: Handle this case in all compass-related functions. */
  2459. //        log_e("Compass not found.\n");
  2460. //        return -1;
  2461. //    }
  2462. //
  2463. //    st.chip_cfg.compass_addr = akm_addr;
  2464. //
  2465. //    data[0] = AKM_POWER_DOWN;
  2466. //    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
  2467. //        return -1;
  2468. //    delay_ms(1);
  2469. //
  2470. //    data[0] = AKM_FUSE_ROM_ACCESS;
  2471. //    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
  2472. //        return -1;
  2473. //    delay_ms(1);
  2474. //
  2475. //    /* Get sensitivity adjustment data from fuse ROM. */
  2476. //    if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
  2477. //        return -1;
  2478. //    st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
  2479. //    st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
  2480. //    st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
  2481. //
  2482. //    data[0] = AKM_POWER_DOWN;
  2483. //    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
  2484. //        return -1;
  2485. //    delay_ms(1);
  2486. //
  2487. //    mpu_set_bypass(0);
  2488. //
  2489. //    /* Set up master mode, master clock, and ES bit. */
  2490. //    data[0] = 0x40;
  2491. //    if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
  2492. //        return -1;
  2493. //
  2494. //    /* Slave 0 reads from AKM data registers. */
  2495. //    data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
  2496. //    if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
  2497. //        return -1;
  2498. //
  2499. //    /* Compass reads start at this register. */
  2500. //    data[0] = AKM_REG_ST1;
  2501. //    if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
  2502. //        return -1;
  2503. //
  2504. //    /* Enable slave 0, 8-byte reads. */
  2505. //    data[0] = BIT_SLAVE_EN | 8;
  2506. //    if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
  2507. //        return -1;
  2508. //
  2509. //    /* Slave 1 changes AKM measurement mode. */
  2510. //    data[0] = st.chip_cfg.compass_addr;
  2511. //    if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
  2512. //        return -1;
  2513. //
  2514. //    /* AKM measurement mode register. */
  2515. //    data[0] = AKM_REG_CNTL;
  2516. //    if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
  2517. //        return -1;
  2518. //
  2519. //    /* Enable slave 1, 1-byte writes. */
  2520. //    data[0] = BIT_SLAVE_EN | 1;
  2521. //    if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
  2522. //        return -1;
  2523. //
  2524. //    /* Set slave 1 data. */
  2525. //    data[0] = AKM_SINGLE_MEASUREMENT;
  2526. //    if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
  2527. //        return -1;
  2528. //
  2529. //    /* Trigger slave 0 and slave 1 actions at each sample. */
  2530. //    data[0] = 0x03;
  2531. //    if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
  2532. //        return -1;
  2533. //
  2534. //#ifdef MPU9150
  2535. //    /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
  2536. //    data[0] = BIT_I2C_MST_VDDIO;
  2537. //    if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
  2538. //        return -1;
  2539. //#endif
  2540. //
  2541. //    return 0;
  2542. //#else
  2543. //    return -1;
  2544. //#endif
  2545. //}

  2547. /**
  2548. *  @brief      Read raw compass data.
  2549. *  @param[out] data        Raw data in hardware units.
  2550. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  2551. *  @return     0 if successful.
  2552. */
  2553. int mpu_get_compass_reg(short *data, unsigned long *timestamp)
  2554. {
  2555. #ifdef AK89xx_SECONDARY
  2556.     unsigned char tmp[9];

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

  2561. #ifdef AK89xx_BYPASS
  2562.     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
  2563.         return -1;
  2564.     tmp[8] = AKM_SINGLE_MEASUREMENT;
  2565.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
  2566.         return -1;
  2567. #else
  2568.     if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
  2569.         return -1;
  2570. #endif

  2572. #if defined AK8975_SECONDARY
  2573.     /* AK8975 doesn't have the overrun error bit. */
  2574.     if (!(tmp[0] & AKM_DATA_READY))
  2575.         return -2;
  2576.     if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
  2577.         return -3;
  2578. #elif defined AK8963_SECONDARY
  2579.     /* AK8963 doesn't have the data read error bit. */
  2580.     if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
  2581.         return -2;
  2582.     if (tmp[7] & AKM_OVERFLOW)
  2583. ……………………

  2585. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼

全部資料51hei下載地址：
MPU6050.zip (38.32 KB, 下載次數(shù): 142)


作者: jeffrey.....    時間: 2018-8-18 20:12
請問這個可以用來調(diào)整角度嗎
作者: 韋杰123    時間: 2018-8-19 16:22
jeffrey..... 發(fā)表于 2018-8-18 20:12
請問這個可以用來調(diào)整角度嗎

可以的
作者: dahanzk    時間: 2019-1-30 15:17
下載來試一下。哈哈
作者: YLLDX    時間: 2019-4-22 00:15
有使用說明嗎
作者: YLLDX    時間: 2019-4-22 00:22
你好，那個I2C.h  跟  delay.h   兩個頭文件沒有呀？？？
作者: wstwj    時間: 2019-5-9 15:29
大神，可以測量X、Y兩個方向上的加速度嗎？
作者: jokergcl    時間: 2019-5-15 18:51
感覺根本不行啊，測試不出來啊
作者: xiaozhong1314    時間: 2019-7-19 14:52
測不出來
作者: esmember    時間: 2019-9-30 15:26
這個可以直接用嗎，正糾結(jié)怎么移植到32上呢，雖然我用的是f030，不過沒插，感謝，救星。
作者: 韋杰123    時間: 2019-10-6 12:43
esmember 發(fā)表于 2019-9-30 15:26
這個可以直接用嗎，正糾結(jié)怎么移植到32上呢，雖然我用的是f030，不過沒插，感謝，救星。

可以的a
作者: nba    時間: 2019-10-6 15:06
感謝樓主分想
作者: 吃了嗎    時間: 2020-4-16 13:02
感謝，可以加入OLED顯示嗎




歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1