标题: stm32f103c8t6的mpu6050程序 [打印本页]

作者: 韦杰123    时间: 2018-8-12 16:02
标题: stm32f103c8t6的mpu6050程序
stm32f103c8 mpu6050

单片机源程序如下:
  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

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

  50. #define i2c_write   i2cwrite
  51. #define i2c_read    i2cread
  52. #define delay_ms    delay_ms
  53. #define get_ms      get_ms

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

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

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

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

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

  154. static int set_int_enable(unsigned char enable);

  155. /* Hardware registers needed by driver. */
  156. struct gyro_reg_s {
  157.     unsigned char who_am_i;
  158.     unsigned char rate_div;
  159.     unsigned char lpf;
  160.     unsigned char prod_id;
  161.     unsigned char user_ctrl;
  162.     unsigned char fifo_en;
  163.     unsigned char gyro_cfg;
  164.     unsigned char accel_cfg;

  165.     //unsigned char accel_cfg2;

  166.     //unsigned char lp_accel_odr;

  167.     unsigned char motion_thr;
  168.     unsigned char motion_dur;
  169.     unsigned char fifo_count_h;
  170.     unsigned char fifo_r_w;
  171.     unsigned char raw_gyro;
  172.     unsigned char raw_accel;
  173.     unsigned char temp;
  174.     unsigned char int_enable;
  175.     unsigned char dmp_int_status;
  176.     unsigned char int_status;

  177.     //unsigned char accel_intel;

  178.     unsigned char pwr_mgmt_1;
  179.     unsigned char pwr_mgmt_2;
  180.     unsigned char int_pin_cfg;
  181.     unsigned char mem_r_w;
  182.     unsigned char accel_offs;
  183.     unsigned char i2c_mst;
  184.     unsigned char bank_sel;
  185.     unsigned char mem_start_addr;
  186.     unsigned char prgm_start_h;
  187. #if defined AK89xx_SECONDARY
  188.     unsigned char s0_addr;
  189.     unsigned char s0_reg;
  190.     unsigned char s0_ctrl;
  191.     unsigned char s1_addr;
  192.     unsigned char s1_reg;
  193.     unsigned char s1_ctrl;
  194.     unsigned char s4_ctrl;
  195.     unsigned char s0_do;
  196.     unsigned char s1_do;
  197.     unsigned char i2c_delay_ctrl;
  198.     unsigned char raw_compass;
  199.     /* The I2C_MST_VDDIO bit is in this register. */
  200.     unsigned char yg_offs_tc;
  201. #endif
  202. };

  203. /* Information specific to a particular device. */
  204. struct hw_s {
  205.     unsigned char addr;
  206.     unsigned short max_fifo;
  207.     unsigned char num_reg;
  208.     unsigned short temp_sens;
  209.     short temp_offset;
  210.     unsigned short bank_size;
  211. #if defined AK89xx_SECONDARY
  212.     unsigned short compass_fsr;
  213. #endif
  214. };

  215. /* When entering motion interrupt mode, the driver keeps track of the
  216. * previous state so that it can be restored at a later time.
  217. * TODO: This is tacky. Fix it.
  218. */
  219. struct motion_int_cache_s {
  220.     unsigned short gyro_fsr;
  221.     unsigned char accel_fsr;
  222.     unsigned short lpf;
  223.     unsigned short sample_rate;
  224.     unsigned char sensors_on;
  225.     unsigned char fifo_sensors;
  226.     unsigned char dmp_on;
  227. };

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

  276. /* Information for self-test. */
  277. struct test_s {
  278.     unsigned long gyro_sens;
  279.     unsigned long accel_sens;
  280.     unsigned char reg_rate_div;
  281.     unsigned char reg_lpf;
  282.     unsigned char reg_gyro_fsr;
  283.     unsigned char reg_accel_fsr;
  284.     unsigned short wait_ms;
  285.     unsigned char packet_thresh;
  286.     float min_dps;
  287.     float max_dps;
  288.     float max_gyro_var;
  289.     float min_g;
  290.     float max_g;
  291.     float max_accel_var;
  292. };

  293. /* Gyro driver state variables. */
  294. struct gyro_state_s {
  295.     const struct gyro_reg_s *reg;
  296.     const struct hw_s *hw;
  297.     struct chip_cfg_s chip_cfg;
  298.     const struct test_s *test;
  299. };

  300. /* Filter configurations. */
  301. enum lpf_e {
  302.     INV_FILTER_256HZ_NOLPF2 = 0,
  303.     INV_FILTER_188HZ,
  304.     INV_FILTER_98HZ,
  305.     INV_FILTER_42HZ,
  306.     INV_FILTER_20HZ,
  307.     INV_FILTER_10HZ,
  308.     INV_FILTER_5HZ,
  309.     INV_FILTER_2100HZ_NOLPF,
  310.     NUM_FILTER
  311. };

  312. /* Full scale ranges. */
  313. enum gyro_fsr_e {
  314.     INV_FSR_250DPS = 0,
  315.     INV_FSR_500DPS,
  316.     INV_FSR_1000DPS,
  317.     INV_FSR_2000DPS,
  318.     NUM_GYRO_FSR
  319. };

  320. /* Full scale ranges. */
  321. enum accel_fsr_e {
  322.     INV_FSR_2G = 0,
  323.     INV_FSR_4G,
  324.     INV_FSR_8G,
  325.     INV_FSR_16G,
  326.     NUM_ACCEL_FSR
  327. };

  328. /* Clock sources. */
  329. enum clock_sel_e {
  330.     INV_CLK_INTERNAL = 0,
  331.     INV_CLK_PLL,
  332.     NUM_CLK
  333. };

  334. /* Low-power accel wakeup rates. */
  335. enum lp_accel_rate_e {
  336. #if defined MPU6050
  337.     INV_LPA_1_25HZ,
  338.     INV_LPA_5HZ,
  339.     INV_LPA_20HZ,
  340.     INV_LPA_40HZ
  341. #elif defined MPU6500
  342.     INV_LPA_0_3125HZ,
  343.     INV_LPA_0_625HZ,
  344.     INV_LPA_1_25HZ,
  345.     INV_LPA_2_5HZ,
  346.     INV_LPA_5HZ,
  347.     INV_LPA_10HZ,
  348.     INV_LPA_20HZ,
  349.     INV_LPA_40HZ,
  350.     INV_LPA_80HZ,
  351.     INV_LPA_160HZ,
  352.     INV_LPA_320HZ,
  353.     INV_LPA_640HZ
  354. #endif
  355. };

  356. #define BIT_I2C_MST_VDDIO   (0x80)
  357. #define BIT_FIFO_EN         (0x40)
  358. #define BIT_DMP_EN          (0x80)
  359. #define BIT_FIFO_RST        (0x04)
  360. #define BIT_DMP_RST         (0x08)
  361. #define BIT_FIFO_OVERFLOW   (0x10)
  362. #define BIT_DATA_RDY_EN     (0x01)
  363. #define BIT_DMP_INT_EN      (0x02)
  364. #define BIT_MOT_INT_EN      (0x40)
  365. #define BITS_FSR            (0x18)
  366. #define BITS_LPF            (0x07)
  367. #define BITS_HPF            (0x07)
  368. #define BITS_CLK            (0x07)
  369. #define BIT_FIFO_SIZE_1024  (0x40)
  370. #define BIT_FIFO_SIZE_2048  (0x80)
  371. #define BIT_FIFO_SIZE_4096  (0xC0)
  372. #define BIT_RESET           (0x80)
  373. #define BIT_SLEEP           (0x40)
  374. #define BIT_S0_DELAY_EN     (0x01)
  375. #define BIT_S2_DELAY_EN     (0x04)
  376. #define BITS_SLAVE_LENGTH   (0x0F)
  377. #define BIT_SLAVE_BYTE_SW   (0x40)
  378. #define BIT_SLAVE_GROUP     (0x10)
  379. #define BIT_SLAVE_EN        (0x80)
  380. #define BIT_I2C_READ        (0x80)
  381. #define BITS_I2C_MASTER_DLY (0x1F)
  382. #define BIT_AUX_IF_EN       (0x20)
  383. #define BIT_ACTL            (0x80)
  384. #define BIT_LATCH_EN        (0x20)
  385. #define BIT_ANY_RD_CLR      (0x10)
  386. #define BIT_BYPASS_EN       (0x02)
  387. #define BITS_WOM_EN         (0xC0)
  388. #define BIT_LPA_CYCLE       (0x20)
  389. #define BIT_STBY_XA         (0x20)
  390. #define BIT_STBY_YA         (0x10)
  391. #define BIT_STBY_ZA         (0x08)
  392. #define BIT_STBY_XG         (0x04)
  393. #define BIT_STBY_YG         (0x02)
  394. #define BIT_STBY_ZG         (0x01)
  395. #define BIT_STBY_XYZA       (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
  396. #define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)

  397. #if defined AK8975_SECONDARY
  398. #define SUPPORTS_AK89xx_HIGH_SENS   (0x00)
  399. #define AK89xx_FSR                  (9830)
  400. #elif defined AK8963_SECONDARY
  401. #define SUPPORTS_AK89xx_HIGH_SENS   (0x10)
  402. #define AK89xx_FSR                  (4915)
  403. #endif

  404. #ifdef AK89xx_SECONDARY
  405. #define AKM_REG_WHOAMI      (0x00)

  406. #define AKM_REG_ST1         (0x02)
  407. #define AKM_REG_HXL         (0x03)
  408. #define AKM_REG_ST2         (0x09)

  409. #define AKM_REG_CNTL        (0x0A)
  410. #define AKM_REG_ASTC        (0x0C)
  411. #define AKM_REG_ASAX        (0x10)
  412. #define AKM_REG_ASAY        (0x11)
  413. #define AKM_REG_ASAZ        (0x12)

  414. #define AKM_DATA_READY      (0x01)
  415. #define AKM_DATA_OVERRUN    (0x02)
  416. #define AKM_OVERFLOW        (0x80)
  417. #define AKM_DATA_ERROR      (0x40)

  418. #define AKM_BIT_SELF_TEST   (0x40)

  419. #define AKM_POWER_DOWN          (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
  420. #define AKM_SINGLE_MEASUREMENT  (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
  421. #define AKM_FUSE_ROM_ACCESS     (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
  422. #define AKM_MODE_SELF_TEST      (0x08 | SUPPORTS_AK89xx_HIGH_SENS)

  423. #define AKM_WHOAMI      (0x48)
  424. #endif

  425. #if defined MPU6050
  426. /*
  427. const struct gyro_reg_s reg = {
  428.    .who_am_i       = 0x75,
  429.     .rate_div       = 0x19,
  430.     .lpf            = 0x1A,
  431.     .prod_id        = 0x0C,
  432.     .user_ctrl      = 0x6A,
  433.     .fifo_en        = 0x23,
  434.     .gyro_cfg       = 0x1B,
  435.     .accel_cfg      = 0x1C,
  436.     .motion_thr     = 0x1F,
  437.     .motion_dur     = 0x20,
  438.     .fifo_count_h   = 0x72,
  439.     .fifo_r_w       = 0x74,
  440.     .raw_gyro       = 0x43,
  441.     .raw_accel      = 0x3B,
  442.     .temp           = 0x41,
  443.     .int_enable     = 0x38,
  444.     .dmp_int_status = 0x39,
  445.     .int_status     = 0x3A,
  446.     .pwr_mgmt_1     = 0x6B,
  447.     .pwr_mgmt_2     = 0x6C,
  448.     .int_pin_cfg    = 0x37,
  449.     .mem_r_w        = 0x6F,
  450.     .accel_offs     = 0x06,
  451.     .i2c_mst        = 0x24,
  452.     .bank_sel       = 0x6D,
  453.     .mem_start_addr = 0x6E,
  454.     .prgm_start_h   = 0x70
  455. #ifdef AK89xx_SECONDARY
  456.     ,.raw_compass   = 0x49,
  457.     .yg_offs_tc     = 0x01,
  458.     .s0_addr        = 0x25,
  459.     .s0_reg         = 0x26,
  460.     .s0_ctrl        = 0x27,
  461.     .s1_addr        = 0x28,
  462.     .s1_reg         = 0x29,
  463.     .s1_ctrl        = 0x2A,
  464.     .s4_ctrl        = 0x34,
  465.     .s0_do          = 0x63,
  466.     .s1_do          = 0x64,
  467.     .i2c_delay_ctrl = 0x67
  468. #endif
  469. };
  470. const struct hw_s hw = {
  471.     .addr           = 0x68,
  472.     .max_fifo       = 1024,
  473.     .num_reg        = 118,
  474.     .temp_sens      = 340,
  475.     .temp_offset    = -521,
  476.     .bank_size      = 256
  477. #if defined AK89xx_SECONDARY
  478.     ,.compass_fsr    = AK89xx_FSR
  479. #endif
  480. };
  481. */
  482. const struct hw_s hw={
  483.   0x68,         //addr
  484.   1024,         //max_fifo
  485.   118,         //num_reg
  486.   340,         //temp_sens
  487.   -521,         //temp_offset
  488.   256         //bank_size
  489. };
  490. const struct gyro_reg_s reg = {
  491. 0x75,  //who_am_i
  492. 0x19,  //rate_div
  493. 0x1A,  //lpf
  494. 0x0C,  //prod_id
  495. 0x6A,  //user_ctrl
  496. 0x23,  //fifo_en
  497. 0x1B,  //gyro_cfg
  498. 0x1C,  //accel_cfg
  499. 0x1F,  // motion_thr
  500. 0x20,  // motion_dur
  501. 0x72,  // fifo_count_h
  502. 0x74,  // fifo_r_w
  503. 0x43,  // raw_gyro
  504. 0x3B,  // raw_accel
  505. 0x41,  // temp
  506. 0x38,  // int_enable
  507. 0x39,  //  dmp_int_status
  508. 0x3A,  //  int_status
  509. 0x6B,  // pwr_mgmt_1
  510. 0x6C,  // pwr_mgmt_2
  511. 0x37,  // int_pin_cfg
  512. 0x6F,  // mem_r_w
  513. 0x06,  // accel_offs
  514. 0x24,  // i2c_mst
  515. 0x6D,  // bank_sel
  516. 0x6E,  // mem_start_addr
  517. 0x70   // prgm_start_h
  518. };

  519. //const struct test_s test = {
  520. //    .gyro_sens      = 32768/250,
  521. //    .accel_sens     = 32768/16,          
  522. //    .reg_rate_div   = 0,    /* 1kHz. */
  523. //    .reg_lpf        = 1,    /* 188Hz. */
  524. //    .reg_gyro_fsr   = 0,    /* 250dps. */
  525. //    .reg_accel_fsr  = 0x18, /* 16g. */
  526. //    .wait_ms        = 50,
  527. //    .packet_thresh  = 5,    /* 5% */
  528. //    .min_dps        = 10.f,
  529. //    .max_dps        = 105.f,
  530. //    .max_gyro_var   = 0.14f,
  531. //    .min_g          = 0.3f,
  532. //    .max_g          = 0.95f,
  533. //    .max_accel_var  = 0.14f
  534. //};
  535. const struct test_s test={
  536. 32768/250,                 //gyro_sens
  537. 32768/16,                 //        accel_sens
  538. 0,                                 //        reg_rate_div
  539. 1,                                //        reg_lpf
  540. 0,                                 //        reg_gyro_fsr
  541. 0x18,                        //        reg_accel_fsr
  542. 50,                                //        wait_ms
  543. 5,                                //        packet_thresh
  544. 10.0f,                         //        min_dps
  545. 105.0f,                         //        max_dps
  546. 0.14f,                        //        max_gyro_var
  547. 0.3f,                   //        min_g
  548. 0.95f,                   //        max_g
  549. 0.14f                   //        max_accel_var
  550. };
  551. /*
  552. static struct gyro_state_s st = {
  553.     .reg = ®,
  554.     .hw = &hw,
  555.     .test = &test
  556. };        */
  557. static struct gyro_state_s st={
  558.   ®,
  559.   &hw,
  560.   {0},
  561.   &test
  562. };
  563. //st.chip_cfg.dmp_on = 1;
  564. //st.dhip_cfg.fifo_enabel = 1;
  565. /*
  566. #elif defined MPU6500
  567. const struct gyro_reg_s reg = {
  568.     .who_am_i       = 0x75,
  569.     .rate_div       = 0x19,
  570.     .lpf            = 0x1A,
  571.     .prod_id        = 0x0C,
  572.     .user_ctrl      = 0x6A,
  573.     .fifo_en        = 0x23,
  574.     .gyro_cfg       = 0x1B,
  575.     .accel_cfg      = 0x1C,
  576.     .accel_cfg2     = 0x1D,
  577.     .lp_accel_odr   = 0x1E,
  578.     .motion_thr     = 0x1F,
  579.     .motion_dur     = 0x20,
  580.     .fifo_count_h   = 0x72,
  581.     .fifo_r_w       = 0x74,
  582.     .raw_gyro       = 0x43,
  583.     .raw_accel      = 0x3B,
  584.     .temp           = 0x41,
  585.     .int_enable     = 0x38,
  586.     .dmp_int_status = 0x39,
  587.     .int_status     = 0x3A,
  588.     .accel_intel    = 0x69,
  589.     .pwr_mgmt_1     = 0x6B,
  590.     .pwr_mgmt_2     = 0x6C,
  591.     .int_pin_cfg    = 0x37,
  592.     .mem_r_w        = 0x6F,
  593.     .accel_offs     = 0x77,
  594.     .i2c_mst        = 0x24,
  595.     .bank_sel       = 0x6D,
  596.     .mem_start_addr = 0x6E,
  597.     .prgm_start_h   = 0x70
  598. #ifdef AK89xx_SECONDARY
  599.     ,.raw_compass   = 0x49,
  600.     .s0_addr        = 0x25,
  601.     .s0_reg         = 0x26,
  602.     .s0_ctrl        = 0x27,
  603.     .s1_addr        = 0x28,
  604.     .s1_reg         = 0x29,
  605.     .s1_ctrl        = 0x2A,
  606.     .s4_ctrl        = 0x34,
  607.     .s0_do          = 0x63,
  608.     .s1_do          = 0x64,
  609.     .i2c_delay_ctrl = 0x67
  610. #endif
  611. };
  612. const struct hw_s hw = {
  613.     .addr           = 0x68,
  614.     .max_fifo       = 1024,
  615.     .num_reg        = 128,
  616.     .temp_sens      = 321,
  617.     .temp_offset    = 0,
  618.     .bank_size      = 256
  619. #if defined AK89xx_SECONDARY
  620.     ,.compass_fsr    = AK89xx_FSR
  621. #endif
  622. };
  623. */
  624. //const struct test_s test = {
  625. //    .gyro_sens      = 32768/250,
  626. //    .accel_sens     = 32768/16,
  627. //    .reg_rate_div   = 0,    /* 1kHz. */
  628. //    .reg_lpf        = 1,    /* 188Hz. */
  629. //    .reg_gyro_fsr   = 0,    /* 250dps. */
  630. //    .reg_accel_fsr  = 0x18, /* 16g. */
  631. //    .wait_ms        = 50,
  632. //    .packet_thresh  = 5,    /* 5% */
  633. //    .min_dps        = 10.f,
  634. //    .max_dps        = 105.f,
  635. //    .max_gyro_var   = 0.14f,
  636. //    .min_g          = 0.3f,
  637. //    .max_g          = 0.95f,
  638. //    .max_accel_var  = 0.14f
  639. //};
  640. //
  641. //static struct gyro_state_s st = {
  642. //    .reg = ®,
  643. //    .hw = &hw,
  644. //    .test = &test
  645. //};
  646. #endif

  647. #define MAX_PACKET_LENGTH (12)

  648. #ifdef AK89xx_SECONDARY
  649. static int setup_compass(void);
  650. #define MAX_COMPASS_SAMPLE_RATE (100)
  651. #endif

  652. /**
  653. *  @brief      Enable/disable data ready interrupt.
  654. *  If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
  655. *  interrupt is used.
  656. *  @param[in]  enable      1 to enable interrupt.
  657. *  @return     0 if successful.
  658. */
  659. static int set_int_enable(unsigned char enable)
  660. {
  661.     unsigned char tmp;

  662.     if (st.chip_cfg.dmp_on) {
  663.         if (enable)
  664.             tmp = BIT_DMP_INT_EN;
  665.         else
  666.             tmp = 0x00;
  667.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
  668.             return -1;
  669.         st.chip_cfg.int_enable = tmp;
  670.     } else {
  671.         if (!st.chip_cfg.sensors)
  672.             return -1;
  673.         if (enable && st.chip_cfg.int_enable)
  674.             return 0;
  675.         if (enable)
  676.             tmp = BIT_DATA_RDY_EN;
  677.         else
  678.             tmp = 0x00;
  679.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
  680.             return -1;
  681.         st.chip_cfg.int_enable = tmp;
  682.     }
  683.     return 0;
  684. }

  685. /**
  686. *  @brief      Register dump for testing.
  687. *  @return     0 if successful.
  688. */
  689. int mpu_reg_dump(void)
  690. {
  691.     unsigned char ii;
  692.     unsigned char data;

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

  702. /**
  703. *  @brief      Read from a single register.
  704. *  NOTE: The memory and FIFO read/write registers cannot be accessed.
  705. *  @param[in]  reg     Register address.
  706. *  @param[out] data    Register data.
  707. *  @return     0 if successful.
  708. */
  709. int mpu_read_reg(unsigned char reg, unsigned char *data)
  710. {
  711.     if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
  712.         return -1;
  713.     if (reg >= st.hw->num_reg)
  714.         return -1;
  715.     return i2c_read(st.hw->addr, reg, 1, data);
  716. }

  717. /**
  718. *  @brief      Initialize hardware.
  719. *  Initial configuration:\n
  720. *  Gyro FSR: +/- 2000DPS\n
  721. *  Accel FSR +/- 2G\n
  722. *  DLPF: 42Hz\n
  723. *  FIFO rate: 50Hz\n
  724. *  Clock source: Gyro PLL\n
  725. *  FIFO: Disabled.\n
  726. *  Data ready interrupt: Disabled, active low, unlatched.
  727. *  @param[in]  int_param   Platform-specific parameters to interrupt API.
  728. *  @return     0 if successful.
  729. */
  730. int mpu_init(void)
  731. {
  732.     unsigned char data[6], rev;

  733.     /* Reset device. */
  734.     data[0] = 0x80;//BIT_RESET;
  735.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0])))
  736.         return -1;
  737.     delay_ms(100);

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

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

  748.     if (rev) {
  749.         /* Congrats, these parts are better. */
  750.         if (rev == 1)
  751.             st.chip_cfg.accel_half = 1;
  752.         else if (rev == 2)
  753.             st.chip_cfg.accel_half = 0;
  754.         else {
  755.             log_e("Unsupported software product rev %d.\n");
  756.             return -1;
  757.         }
  758.     }
  759.         else {
  760.         if (i2c_read(st.hw->addr, st.reg->prod_id, 1, &(data[0])))
  761.             return -1;
  762.         rev = data[0] & 0x0F;
  763.         if (!rev) {
  764.             log_e(" Product ID read as 0 indicates device is either "
  765.                 "incompatible or an MPU3050.\n");
  766.             return -1;
  767.         } else if (rev == 4) {
  768.             log_i("Half sensitivity part found.\n");
  769.             st.chip_cfg.accel_half = 1;
  770.         } else
  771.             st.chip_cfg.accel_half = 0;
  772.     }
  773. #elif defined MPU6500

  774. #define MPU6500_MEM_REV_ADDR    (0x17)
  775.     if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
  776.         return -1;
  777.     if (rev == 0x1)
  778.         st.chip_cfg.accel_half = 0;
  779.     else {
  780.         log_e("Unsupported software product rev %d.\n", rev);
  781.         return -1;
  782.     }

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

  790.     /* Set to invalid values to ensure no I2C writes are skipped. */
  791.     st.chip_cfg.sensors = 0xFF;
  792.     st.chip_cfg.gyro_fsr = 0xFF;
  793.     st.chip_cfg.accel_fsr = 0xFF;
  794.     st.chip_cfg.lpf = 0xFF;
  795.     st.chip_cfg.sample_rate = 0xFFFF;
  796.     st.chip_cfg.fifo_enable = 0xFF;
  797.     st.chip_cfg.bypass_mode = 0xFF;
  798. #ifdef AK89xx_SECONDARY
  799.     st.chip_cfg.compass_sample_rate = 0xFFFF;
  800. #endif
  801.     /* mpu_set_sensors always preserves this setting. */
  802.     st.chip_cfg.clk_src = INV_CLK_PLL;
  803.     /* Handled in next call to mpu_set_bypass. */
  804.     st.chip_cfg.active_low_int = 1;
  805.     st.chip_cfg.latched_int = 0;
  806.     st.chip_cfg.int_motion_only = 0;
  807.     st.chip_cfg.lp_accel_mode = 0;
  808.     memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
  809.     st.chip_cfg.dmp_on = 0;
  810.     st.chip_cfg.dmp_loaded = 0;
  811.     st.chip_cfg.dmp_sample_rate = 0;

  812.     if (mpu_set_gyro_fsr(2000))
  813.         return -1;
  814.     if (mpu_set_accel_fsr(2))
  815.         return -1;
  816.     if (mpu_set_lpf(42))
  817.         return -1;
  818.     if (mpu_set_sample_rate(50))
  819.         return -1;
  820.     if (mpu_configure_fifo(0))
  821.         return -1;

  822.     /*if (int_param)
  823.         reg_int_cb(int_param);*/

  824. #ifdef AK89xx_SECONDARY
  825.     setup_compass();
  826.     if (mpu_set_compass_sample_rate(10))
  827.         return -1;
  828. #else
  829.     /* Already disabled by setup_compass. */
  830.     if (mpu_set_bypass(0))
  831.         return -1;
  832. #endif

  833.     mpu_set_sensors(0);
  834.     return 0;
  835. }

  836. /**
  837. *  @brief      Enter low-power accel-only mode.
  838. *  In low-power accel mode, the chip goes to sleep and only wakes up to sample
  839. *  the accelerometer at one of the following frequencies:
  840. *  \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
  841. *  \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
  842. *  \n If the requested rate is not one listed above, the device will be set to
  843. *  the next highest rate. Requesting a rate above the maximum supported
  844. *  frequency will result in an error.
  845. *  \n To select a fractional wake-up frequency, round down the value passed to
  846. *  @e rate.
  847. *  @param[in]  rate        Minimum sampling rate, or zero to disable LP
  848. *                          accel mode.
  849. *  @return     0 if successful.
  850. */
  851. int mpu_lp_accel_mode(unsigned char rate)
  852. {
  853.     unsigned char tmp[2];

  854.     if (rate > 40)
  855.         return -1;

  856.     if (!rate) {
  857.         mpu_set_int_latched(0);
  858.         tmp[0] = 0;
  859.         tmp[1] = BIT_STBY_XYZG;
  860.         if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
  861.             return -1;
  862.         st.chip_cfg.lp_accel_mode = 0;
  863.         return 0;
  864.     }
  865.     /* For LP accel, we automatically configure the hardware to produce latched
  866.      * interrupts. In LP accel mode, the hardware cycles into sleep mode before
  867.      * it gets a chance to deassert the interrupt pin; therefore, we shift this
  868.      * responsibility over to the MCU.
  869.      *
  870.      * Any register read will clear the interrupt.
  871.      */
  872.     mpu_set_int_latched(1);
  873. #if defined MPU6050
  874.     tmp[0] = BIT_LPA_CYCLE;
  875.     if (rate == 1) {
  876.         tmp[1] = INV_LPA_1_25HZ;
  877.         mpu_set_lpf(5);
  878.     } else if (rate <= 5) {
  879.         tmp[1] = INV_LPA_5HZ;
  880.         mpu_set_lpf(5);
  881.     } else if (rate <= 20) {
  882.         tmp[1] = INV_LPA_20HZ;
  883.         mpu_set_lpf(10);
  884.     } else {
  885.         tmp[1] = INV_LPA_40HZ;
  886.         mpu_set_lpf(20);
  887.     }
  888.     tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
  889.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
  890.         return -1;
  891. #elif defined MPU6500
  892.     /* Set wake frequency. */
  893.     if (rate == 1)
  894.         tmp[0] = INV_LPA_1_25HZ;
  895.     else if (rate == 2)
  896.         tmp[0] = INV_LPA_2_5HZ;
  897.     else if (rate <= 5)
  898.         tmp[0] = INV_LPA_5HZ;
  899.     else if (rate <= 10)
  900.         tmp[0] = INV_LPA_10HZ;
  901.     else if (rate <= 20)
  902.         tmp[0] = INV_LPA_20HZ;
  903.     else if (rate <= 40)
  904.         tmp[0] = INV_LPA_40HZ;
  905.     else if (rate <= 80)
  906.         tmp[0] = INV_LPA_80HZ;
  907.     else if (rate <= 160)
  908.         tmp[0] = INV_LPA_160HZ;
  909.     else if (rate <= 320)
  910.         tmp[0] = INV_LPA_320HZ;
  911.     else
  912.         tmp[0] = INV_LPA_640HZ;
  913.     if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
  914.         return -1;
  915.     tmp[0] = BIT_LPA_CYCLE;
  916.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
  917.         return -1;
  918. #endif
  919.     st.chip_cfg.sensors = INV_XYZ_ACCEL;
  920.     st.chip_cfg.clk_src = 0;
  921.     st.chip_cfg.lp_accel_mode = 1;
  922.     mpu_configure_fifo(0);

  923.     return 0;
  924. }

  925. /**
  926. *  @brief      Read raw gyro data directly from the registers.
  927. *  @param[out] data        Raw data in hardware units.
  928. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  929. *  @return     0 if successful.
  930. */
  931. int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
  932. {
  933.     unsigned char tmp[6];

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

  936.     if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
  937.         return -1;
  938.     data[0] = (tmp[0] << 8) | tmp[1];
  939.     data[1] = (tmp[2] << 8) | tmp[3];
  940.     data[2] = (tmp[4] << 8) | tmp[5];
  941.     if (timestamp)
  942.         get_ms(timestamp);
  943.     return 0;
  944. }

  945. /**
  946. *  @brief      Read raw accel data directly from the registers.
  947. *  @param[out] data        Raw data in hardware units.
  948. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  949. *  @return     0 if successful.
  950. */
  951. int mpu_get_accel_reg(short *data, unsigned long *timestamp)
  952. {
  953.     unsigned char tmp[6];

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

  956.     if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
  957.         return -1;
  958.     data[0] = (tmp[0] << 8) | tmp[1];
  959.     data[1] = (tmp[2] << 8) | tmp[3];
  960.     data[2] = (tmp[4] << 8) | tmp[5];
  961.     if (timestamp)
  962.         get_ms(timestamp);
  963.     return 0;
  964. }

  965. /**
  966. *  @brief      Read temperature data directly from the registers.
  967. *  @param[out] data        Data in q16 format.
  968. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  969. *  @return     0 if successful.
  970. */
  971. int mpu_get_temperature(long *data, unsigned long *timestamp)
  972. {
  973.     unsigned char tmp[2];
  974.     short raw;

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

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

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

  985. /**
  986. *  @brief      Push biases to the accel bias registers.
  987. *  This function expects biases relative to the current sensor output, and
  988. *  these biases will be added to the factory-supplied values.
  989. *  @param[in]  accel_bias  New biases.
  990. *  @return     0 if successful.
  991. */
  992. int mpu_set_accel_bias(const long *accel_bias)
  993. {
  994.     unsigned char data[6];
  995.     short accel_hw[3];
  996.     short got_accel[3];
  997.     short fg[3];

  998.     if (!accel_bias)
  999.         return -1;
  1000.     if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
  1001.         return 0;

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

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

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

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

  1015.     accel_hw[0] += got_accel[0];
  1016.     accel_hw[1] += got_accel[1];
  1017.     accel_hw[2] += got_accel[2];

  1018.     data[0] = (accel_hw[0] >> 8) & 0xff;
  1019.     data[1] = (accel_hw[0]) & 0xff;
  1020.     data[2] = (accel_hw[1] >> 8) & 0xff;
  1021.     data[3] = (accel_hw[1]) & 0xff;
  1022.     data[4] = (accel_hw[2] >> 8) & 0xff;
  1023.     data[5] = (accel_hw[2]) & 0xff;

  1024.     if (i2c_write(st.hw->addr, 0x06, 6, data))
  1025.         return -1;
  1026.     return 0;
  1027. }

  1028. /**
  1029. *  @brief  Reset FIFO read/write pointers.
  1030. *  @return 0 if successful.
  1031. */
  1032. int mpu_reset_fifo(void)
  1033. {
  1034.     unsigned char data;

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

  1037.     data = 0;
  1038.     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
  1039.         return -1;
  1040.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
  1041.         return -1;
  1042.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1043.         return -1;

  1044.     if (st.chip_cfg.dmp_on) {
  1045.         data = BIT_FIFO_RST | BIT_DMP_RST;
  1046.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1047.             return -1;
  1048.         delay_ms(50);
  1049.         data = BIT_DMP_EN | BIT_FIFO_EN;
  1050.         if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
  1051.             data |= BIT_AUX_IF_EN;
  1052.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1053.             return -1;
  1054.         if (st.chip_cfg.int_enable)
  1055.             data = BIT_DMP_INT_EN;
  1056.         else
  1057.             data = 0;
  1058.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
  1059.             return -1;
  1060.         data = 0;
  1061.         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
  1062.             return -1;
  1063.     } else {
  1064.         data = BIT_FIFO_RST;
  1065.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1066.             return -1;
  1067.         if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
  1068.             data = BIT_FIFO_EN;
  1069.         else
  1070.             data = BIT_FIFO_EN | BIT_AUX_IF_EN;
  1071.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
  1072.             return -1;
  1073.         delay_ms(50);
  1074.         if (st.chip_cfg.int_enable)
  1075.             data = BIT_DATA_RDY_EN;
  1076.         else
  1077.             data = 0;
  1078.         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
  1079.             return -1;
  1080.         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
  1081.             return -1;
  1082.     }
  1083.     return 0;
  1084. }

  1085. /**
  1086. *  @brief      Get the gyro full-scale range.
  1087. *  @param[out] fsr Current full-scale range.
  1088. *  @return     0 if successful.
  1089. */
  1090. int mpu_get_gyro_fsr(unsigned short *fsr)
  1091. {
  1092.     switch (st.chip_cfg.gyro_fsr) {
  1093.     case INV_FSR_250DPS:
  1094.         fsr[0] = 250;
  1095.         break;
  1096.     case INV_FSR_500DPS:
  1097.         fsr[0] = 500;
  1098.         break;
  1099.     case INV_FSR_1000DPS:
  1100.         fsr[0] = 1000;
  1101.         break;
  1102.     case INV_FSR_2000DPS:
  1103.         fsr[0] = 2000;
  1104.         break;
  1105.     default:
  1106.         fsr[0] = 0;
  1107.         break;
  1108.     }
  1109.     return 0;
  1110. }

  1111. /**
  1112. *  @brief      Set the gyro full-scale range.
  1113. *  @param[in]  fsr Desired full-scale range.
  1114. *  @return     0 if successful.
  1115. */
  1116. int mpu_set_gyro_fsr(unsigned short fsr)
  1117. {
  1118.     unsigned char data;

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

  1121.     switch (fsr) {
  1122.     case 250:
  1123.         data = INV_FSR_250DPS << 3;
  1124.         break;
  1125.     case 500:
  1126.         data = INV_FSR_500DPS << 3;
  1127.         break;
  1128.     case 1000:
  1129.         data = INV_FSR_1000DPS << 3;
  1130.         break;
  1131.     case 2000:
  1132.         data = INV_FSR_2000DPS << 3;
  1133.         break;
  1134.     default:
  1135.         return -1;
  1136.     }

  1137.     if (st.chip_cfg.gyro_fsr == (data >> 3))
  1138.         return 0;
  1139.     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
  1140.         return -1;
  1141.     st.chip_cfg.gyro_fsr = data >> 3;
  1142.     return 0;
  1143. }

  1144. /**
  1145. *  @brief      Get the accel full-scale range.
  1146. *  @param[out] fsr Current full-scale range.
  1147. *  @return     0 if successful.
  1148. */
  1149. int mpu_get_accel_fsr(unsigned char *fsr)
  1150. {
  1151.     switch (st.chip_cfg.accel_fsr) {
  1152.     case INV_FSR_2G:
  1153.         fsr[0] = 2;
  1154.         break;
  1155.     case INV_FSR_4G:
  1156.         fsr[0] = 4;
  1157.         break;
  1158.     case INV_FSR_8G:
  1159.         fsr[0] = 8;
  1160.         break;
  1161.     case INV_FSR_16G:
  1162.         fsr[0] = 16;
  1163.         break;
  1164.     default:
  1165.         return -1;
  1166.     }
  1167.     if (st.chip_cfg.accel_half)
  1168.         fsr[0] <<= 1;
  1169.     return 0;
  1170. }

  1171. /**
  1172. *  @brief      Set the accel full-scale range.
  1173. *  @param[in]  fsr Desired full-scale range.
  1174. *  @return     0 if successful.
  1175. */
  1176. int mpu_set_accel_fsr(unsigned char fsr)
  1177. {
  1178.     unsigned char data;

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

  1181.     switch (fsr) {
  1182.     case 2:
  1183.         data = INV_FSR_2G << 3;
  1184.         break;
  1185.     case 4:
  1186.         data = INV_FSR_4G << 3;
  1187.         break;
  1188.     case 8:
  1189.         data = INV_FSR_8G << 3;
  1190.         break;
  1191.     case 16:
  1192.         data = INV_FSR_16G << 3;
  1193.         break;
  1194.     default:
  1195.         return -1;
  1196.     }

  1197.     if (st.chip_cfg.accel_fsr == (data >> 3))
  1198.         return 0;
  1199.     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
  1200.         return -1;
  1201.     st.chip_cfg.accel_fsr = data >> 3;
  1202.     return 0;
  1203. }

  1204. /**
  1205. *  @brief      Get the current DLPF setting.
  1206. *  @param[out] lpf Current LPF setting.
  1207. *  0 if successful.
  1208. */
  1209. int mpu_get_lpf(unsigned short *lpf)
  1210. {
  1211.     switch (st.chip_cfg.lpf) {
  1212.     case INV_FILTER_188HZ:
  1213.         lpf[0] = 188;
  1214.         break;
  1215.     case INV_FILTER_98HZ:
  1216.         lpf[0] = 98;
  1217.         break;
  1218.     case INV_FILTER_42HZ:
  1219.         lpf[0] = 42;
  1220.         break;
  1221.     case INV_FILTER_20HZ:
  1222.         lpf[0] = 20;
  1223.         break;
  1224.     case INV_FILTER_10HZ:
  1225.         lpf[0] = 10;
  1226.         break;
  1227.     case INV_FILTER_5HZ:
  1228.         lpf[0] = 5;
  1229.         break;
  1230.     case INV_FILTER_256HZ_NOLPF2:
  1231.     case INV_FILTER_2100HZ_NOLPF:
  1232.     default:
  1233.         lpf[0] = 0;
  1234.         break;
  1235.     }
  1236.     return 0;
  1237. }

  1238. /**
  1239. *  @brief      Set digital low pass filter.
  1240. *  The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
  1241. *  @param[in]  lpf Desired LPF setting.
  1242. *  @return     0 if successful.
  1243. */
  1244. int mpu_set_lpf(unsigned short lpf)
  1245. {
  1246.     unsigned char data;

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

  1249.     if (lpf >= 188)
  1250.         data = INV_FILTER_188HZ;
  1251.     else if (lpf >= 98)
  1252.         data = INV_FILTER_98HZ;
  1253.     else if (lpf >= 42)
  1254.         data = INV_FILTER_42HZ;
  1255.     else if (lpf >= 20)
  1256.         data = INV_FILTER_20HZ;
  1257.     else if (lpf >= 10)
  1258.         data = INV_FILTER_10HZ;
  1259.     else
  1260.         data = INV_FILTER_5HZ;

  1261.     if (st.chip_cfg.lpf == data)
  1262.         return 0;
  1263.     if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
  1264.         return -1;
  1265.     st.chip_cfg.lpf = data;
  1266.     return 0;
  1267. }

  1268. /**
  1269. *  @brief      Get sampling rate.
  1270. *  @param[out] rate    Current sampling rate (Hz).
  1271. *  @return     0 if successful.
  1272. */
  1273. int mpu_get_sample_rate(unsigned short *rate)
  1274. {
  1275.     if (st.chip_cfg.dmp_on)
  1276.         return -1;
  1277.     else
  1278.         rate[0] = st.chip_cfg.sample_rate;
  1279.     return 0;
  1280. }

  1281. /**
  1282. *  @brief      Set sampling rate.
  1283. *  Sampling rate must be between 4Hz and 1kHz.
  1284. *  @param[in]  rate    Desired sampling rate (Hz).
  1285. *  @return     0 if successful.
  1286. */
  1287. int mpu_set_sample_rate(unsigned short rate)
  1288. {
  1289.     unsigned char data;

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

  1292.     if (st.chip_cfg.dmp_on)
  1293.         return -1;
  1294.     else {
  1295.         if (st.chip_cfg.lp_accel_mode) {
  1296.             if (rate && (rate <= 40)) {
  1297.                 /* Just stay in low-power accel mode. */
  1298.                 mpu_lp_accel_mode(rate);
  1299.                 return 0;
  1300.             }
  1301.             /* Requested rate exceeds the allowed frequencies in LP accel mode,
  1302.              * switch back to full-power mode.
  1303.              */
  1304.             mpu_lp_accel_mode(0);
  1305.         }
  1306.         if (rate < 4)
  1307.             rate = 4;
  1308.         else if (rate > 1000)
  1309.             rate = 1000;

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

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

  1314. #ifdef AK89xx_SECONDARY
  1315.         mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
  1316. #endif

  1317.         /* Automatically set LPF to 1/2 sampling rate. */
  1318.         mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
  1319.         return 0;
  1320.     }
  1321. }

  1322. /**
  1323. *  @brief      Get compass sampling rate.
  1324. *  @param[out] rate    Current compass sampling rate (Hz).
  1325. *  @return     0 if successful.
  1326. */
  1327. int mpu_get_compass_sample_rate(unsigned short *rate)
  1328. {
  1329. #ifdef AK89xx_SECONDARY
  1330.     rate[0] = st.chip_cfg.compass_sample_rate;
  1331.     return 0;
  1332. #else
  1333.     rate[0] = 0;
  1334.     return -1;
  1335. #endif
  1336. }

  1337. /**
  1338. *  @brief      Set compass sampling rate.
  1339. *  The compass on the auxiliary I2C bus is read by the MPU hardware at a
  1340. *  maximum of 100Hz. The actual rate can be set to a fraction of the gyro
  1341. *  sampling rate.
  1342. *
  1343. *  \n WARNING: The new rate may be different than what was requested. Call
  1344. *  mpu_get_compass_sample_rate to check the actual setting.
  1345. *  @param[in]  rate    Desired compass sampling rate (Hz).
  1346. *  @return     0 if successful.
  1347. */
  1348. int mpu_set_compass_sample_rate(unsigned short rate)
  1349. {
  1350. #ifdef AK89xx_SECONDARY
  1351.     unsigned char div;
  1352.     if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
  1353.         return -1;

  1354.     div = st.chip_cfg.sample_rate / rate - 1;
  1355.     if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
  1356.         return -1;
  1357.     st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
  1358.     return 0;
  1359. #else
  1360.     return -1;
  1361. #endif
  1362. }

  1363. /**
  1364. *  @brief      Get gyro sensitivity scale factor.
  1365. *  @param[out] sens    Conversion from hardware units to dps.
  1366. *  @return     0 if successful.
  1367. */
  1368. int mpu_get_gyro_sens(float *sens)
  1369. {
  1370.     switch (st.chip_cfg.gyro_fsr) {
  1371.     case INV_FSR_250DPS:
  1372.         sens[0] = 131.f;
  1373.         break;
  1374.     case INV_FSR_500DPS:
  1375.         sens[0] = 65.5f;
  1376.         break;
  1377.     case INV_FSR_1000DPS:
  1378.         sens[0] = 32.8f;
  1379.         break;
  1380.     case INV_FSR_2000DPS:
  1381.         sens[0] = 16.4f;
  1382.         break;
  1383.     default:
  1384.         return -1;
  1385.     }
  1386.     return 0;
  1387. }

  1388. /**
  1389. *  @brief      Get accel sensitivity scale factor.
  1390. *  @param[out] sens    Conversion from hardware units to g's.
  1391. *  @return     0 if successful.
  1392. */
  1393. int mpu_get_accel_sens(unsigned short *sens)
  1394. {
  1395.     switch (st.chip_cfg.accel_fsr) {
  1396.     case INV_FSR_2G:
  1397.         sens[0] = 16384;
  1398.         break;
  1399.     case INV_FSR_4G:
  1400.         sens[0] = 8092;
  1401.         break;
  1402.     case INV_FSR_8G:
  1403.         sens[0] = 4096;
  1404.         break;
  1405.     case INV_FSR_16G:
  1406.         sens[0] = 2048;
  1407.         break;
  1408.     default:
  1409.         return -1;
  1410.     }
  1411.     if (st.chip_cfg.accel_half)
  1412.         sens[0] >>= 1;
  1413.     return 0;
  1414. }

  1415. /**
  1416. *  @brief      Get current FIFO configuration.
  1417. *  @e sensors can contain a combination of the following flags:
  1418. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1419. *  \n INV_XYZ_GYRO
  1420. *  \n INV_XYZ_ACCEL
  1421. *  @param[out] sensors Mask of sensors in FIFO.
  1422. *  @return     0 if successful.
  1423. */
  1424. int mpu_get_fifo_config(unsigned char *sensors)
  1425. {
  1426.     sensors[0] = st.chip_cfg.fifo_enable;
  1427.     return 0;
  1428. }

  1429. /**
  1430. *  @brief      Select which sensors are pushed to FIFO.
  1431. *  @e sensors can contain a combination of the following flags:
  1432. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1433. *  \n INV_XYZ_GYRO
  1434. *  \n INV_XYZ_ACCEL
  1435. *  @param[in]  sensors Mask of sensors to push to FIFO.
  1436. *  @return     0 if successful.
  1437. */
  1438. int mpu_configure_fifo(unsigned char sensors)
  1439. {
  1440.     unsigned char prev;
  1441.     int result = 0;

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

  1444.     if (st.chip_cfg.dmp_on)
  1445.         return 0;
  1446.     else {
  1447.         if (!(st.chip_cfg.sensors))
  1448.             return -1;
  1449.         prev = st.chip_cfg.fifo_enable;
  1450.         st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
  1451.         if (st.chip_cfg.fifo_enable != sensors)
  1452.             /* You're not getting what you asked for. Some sensors are
  1453.              * asleep.
  1454.              */
  1455.             result = -1;
  1456.         else
  1457.             result = 0;
  1458.         if (sensors || st.chip_cfg.lp_accel_mode)
  1459.             set_int_enable(1);
  1460.         else
  1461.             set_int_enable(0);
  1462.         if (sensors) {
  1463.             if (mpu_reset_fifo()) {
  1464.                 st.chip_cfg.fifo_enable = prev;
  1465.                 return -1;
  1466.             }
  1467.         }
  1468.     }

  1469.     return result;
  1470. }

  1471. /**
  1472. *  @brief      Get current power state.
  1473. *  @param[in]  power_on    1 if turned on, 0 if suspended.
  1474. *  @return     0 if successful.
  1475. */
  1476. int mpu_get_power_state(unsigned char *power_on)
  1477. {
  1478.     if (st.chip_cfg.sensors)
  1479.         power_on[0] = 1;
  1480.     else
  1481.         power_on[0] = 0;
  1482.     return 0;
  1483. }

  1484. /**
  1485. *  @brief      Turn specific sensors on/off.
  1486. *  @e sensors can contain a combination of the following flags:
  1487. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1488. *  \n INV_XYZ_GYRO
  1489. *  \n INV_XYZ_ACCEL
  1490. *  \n INV_XYZ_COMPASS
  1491. *  @param[in]  sensors    Mask of sensors to wake.
  1492. *  @return     0 if successful.
  1493. */
  1494. int mpu_set_sensors(unsigned char sensors)
  1495. {
  1496.     unsigned char data;
  1497. #ifdef AK89xx_SECONDARY
  1498.     unsigned char user_ctrl;
  1499. #endif

  1500.     if (sensors & INV_XYZ_GYRO)
  1501.         data = INV_CLK_PLL;
  1502.     else if (sensors)
  1503.         data = 0;
  1504.     else
  1505.         data = BIT_SLEEP;
  1506.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
  1507.         st.chip_cfg.sensors = 0;
  1508.         return -1;
  1509.     }
  1510.     st.chip_cfg.clk_src = data & ~BIT_SLEEP;

  1511.     data = 0;
  1512.     if (!(sensors & INV_X_GYRO))
  1513.         data |= BIT_STBY_XG;
  1514.     if (!(sensors & INV_Y_GYRO))
  1515.         data |= BIT_STBY_YG;
  1516.     if (!(sensors & INV_Z_GYRO))
  1517.         data |= BIT_STBY_ZG;
  1518.     if (!(sensors & INV_XYZ_ACCEL))
  1519.         data |= BIT_STBY_XYZA;
  1520.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
  1521.         st.chip_cfg.sensors = 0;
  1522.         return -1;
  1523.     }

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

  1527. #ifdef AK89xx_SECONDARY
  1528. #ifdef AK89xx_BYPASS
  1529.     if (sensors & INV_XYZ_COMPASS)
  1530.         mpu_set_bypass(1);
  1531.     else
  1532.         mpu_set_bypass(0);
  1533. #else
  1534.     if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
  1535.         return -1;
  1536.     /* Handle AKM power management. */
  1537.     if (sensors & INV_XYZ_COMPASS) {
  1538.         data = AKM_SINGLE_MEASUREMENT;
  1539.         user_ctrl |= BIT_AUX_IF_EN;
  1540.     } else {
  1541.         data = AKM_POWER_DOWN;
  1542.         user_ctrl &= ~BIT_AUX_IF_EN;
  1543.     }
  1544.     if (st.chip_cfg.dmp_on)
  1545.         user_ctrl |= BIT_DMP_EN;
  1546.     else
  1547.         user_ctrl &= ~BIT_DMP_EN;
  1548.     if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
  1549.         return -1;
  1550.     /* Enable/disable I2C master mode. */
  1551.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
  1552.         return -1;
  1553. #endif
  1554. #endif

  1555.     st.chip_cfg.sensors = sensors;
  1556.     st.chip_cfg.lp_accel_mode = 0;
  1557.     delay_ms(50);
  1558.     return 0;
  1559. }

  1560. /**
  1561. *  @brief      Read the MPU interrupt status registers.
  1562. *  @param[out] status  Mask of interrupt bits.
  1563. *  @return     0 if successful.
  1564. */
  1565. int mpu_get_int_status(short *status)
  1566. {
  1567.     unsigned char tmp[2];
  1568.     if (!st.chip_cfg.sensors)
  1569.         return -1;
  1570.     if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
  1571.         return -1;
  1572.     status[0] = (tmp[0] << 8) | tmp[1];
  1573.     return 0;
  1574. }

  1575. /**
  1576. *  @brief      Get one packet from the FIFO.
  1577. *  If @e sensors does not contain a particular sensor, disregard the data
  1578. *  returned to that pointer.
  1579. *  \n @e sensors can contain a combination of the following flags:
  1580. *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
  1581. *  \n INV_XYZ_GYRO
  1582. *  \n INV_XYZ_ACCEL
  1583. *  \n If the FIFO has no new data, @e sensors will be zero.
  1584. *  \n If the FIFO is disabled, @e sensors will be zero and this function will
  1585. *  return a non-zero error code.
  1586. *  @param[out] gyro        Gyro data in hardware units.
  1587. *  @param[out] accel       Accel data in hardware units.
  1588. *  @param[out] timestamp   Timestamp in milliseconds.
  1589. *  @param[out] sensors     Mask of sensors read from FIFO.
  1590. *  @param[out] more        Number of remaining packets.
  1591. *  @return     0 if successful.
  1592. */
  1593. int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
  1594.         unsigned char *sensors, unsigned char *more)
  1595. {
  1596.     /* Assumes maximum packet size is gyro (6) + accel (6). */
  1597.     unsigned char data[MAX_PACKET_LENGTH];
  1598.     unsigned char packet_size = 0;
  1599.     unsigned short fifo_count, index = 0;

  1600.     if (st.chip_cfg.dmp_on)
  1601.         return -1;

  1602.     sensors[0] = 0;
  1603.     if (!st.chip_cfg.sensors)
  1604.         return -1;
  1605.     if (!st.chip_cfg.fifo_enable)
  1606.         return -1;

  1607.     if (st.chip_cfg.fifo_enable & INV_X_GYRO)
  1608.         packet_size += 2;
  1609.     if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
  1610.         packet_size += 2;
  1611.     if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
  1612.         packet_size += 2;
  1613.     if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
  1614.         packet_size += 6;

  1615.     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
  1616.         return -1;
  1617.     fifo_count = (data[0] << 8) | data[1];
  1618.     if (fifo_count < packet_size)
  1619.         return 0;
  1620. //    log_i("FIFO count: %hd\n", fifo_count);
  1621.     if (fifo_count > (st.hw->max_fifo >> 1)) {
  1622.         /* FIFO is 50% full, better check overflow bit. */
  1623.         if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
  1624.             return -1;
  1625.         if (data[0] & BIT_FIFO_OVERFLOW) {
  1626.             mpu_reset_fifo();
  1627.             return -2;
  1628.         }
  1629.     }
  1630.     get_ms((unsigned long*)timestamp);

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

  1635.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
  1636.         accel[0] = (data[index+0] << 8) | data[index+1];
  1637.         accel[1] = (data[index+2] << 8) | data[index+3];
  1638.         accel[2] = (data[index+4] << 8) | data[index+5];
  1639.         sensors[0] |= INV_XYZ_ACCEL;
  1640.         index += 6;
  1641.     }
  1642.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
  1643.         gyro[0] = (data[index+0] << 8) | data[index+1];
  1644.         sensors[0] |= INV_X_GYRO;
  1645.         index += 2;
  1646.     }
  1647.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
  1648.         gyro[1] = (data[index+0] << 8) | data[index+1];
  1649.         sensors[0] |= INV_Y_GYRO;
  1650.         index += 2;
  1651.     }
  1652.     if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
  1653.         gyro[2] = (data[index+0] << 8) | data[index+1];
  1654.         sensors[0] |= INV_Z_GYRO;
  1655.         index += 2;
  1656.     }

  1657.     return 0;
  1658. }

  1659. /**
  1660. *  @brief      Get one unparsed packet from the FIFO.
  1661. *  This function should be used if the packet is to be parsed elsewhere.
  1662. *  @param[in]  length  Length of one FIFO packet.
  1663. *  @param[in]  data    FIFO packet.
  1664. *  @param[in]  more    Number of remaining packets.
  1665. */
  1666. int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
  1667.     unsigned char *more)
  1668. {
  1669.     unsigned char tmp[2];
  1670.     unsigned short fifo_count;
  1671.     if (!st.chip_cfg.dmp_on)
  1672.         return -1;
  1673.     if (!st.chip_cfg.sensors)
  1674.         return -1;

  1675.     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
  1676.         return -1;
  1677.     fifo_count = (tmp[0] << 8) | tmp[1];
  1678.     if (fifo_count < length) {
  1679.         more[0] = 0;
  1680.         return -1;
  1681.     }
  1682.     if (fifo_count > (st.hw->max_fifo >> 1)) {
  1683.         /* FIFO is 50% full, better check overflow bit. */
  1684.         if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
  1685.             return -1;
  1686.         if (tmp[0] & BIT_FIFO_OVERFLOW) {
  1687.             mpu_reset_fifo();
  1688.             return -2;
  1689.         }
  1690.     }

  1691.     if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
  1692.         return -1;
  1693.     more[0] = fifo_count / length - 1;
  1694.     return 0;
  1695. }

  1696. /**
  1697. *  @brief      Set device to bypass mode.
  1698. *  @param[in]  bypass_on   1 to enable bypass mode.
  1699. *  @return     0 if successful.
  1700. */
  1701. int mpu_set_bypass(unsigned char bypass_on)
  1702. {
  1703.     unsigned char tmp;

  1704.     if (st.chip_cfg.bypass_mode == bypass_on)
  1705.         return 0;

  1706.     if (bypass_on) {
  1707.         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1708.             return -1;
  1709.         tmp &= ~BIT_AUX_IF_EN;
  1710.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1711.             return -1;
  1712.         delay_ms(3);
  1713.         tmp = BIT_BYPASS_EN;
  1714.         if (st.chip_cfg.active_low_int)
  1715.             tmp |= BIT_ACTL;
  1716.         if (st.chip_cfg.latched_int)
  1717.             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
  1718.         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
  1719.             return -1;
  1720.     } else {
  1721.         /* Enable I2C master mode if compass is being used. */
  1722.         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1723.             return -1;
  1724.         if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
  1725.             tmp |= BIT_AUX_IF_EN;
  1726.         else
  1727.             tmp &= ~BIT_AUX_IF_EN;
  1728.         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
  1729.             return -1;
  1730.         delay_ms(3);
  1731.         if (st.chip_cfg.active_low_int)
  1732.             tmp = BIT_ACTL;
  1733.         else
  1734.             tmp = 0;
  1735.         if (st.chip_cfg.latched_int)
  1736.             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
  1737.         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
  1738.             return -1;
  1739.     }
  1740.     st.chip_cfg.bypass_mode = bypass_on;
  1741.     return 0;
  1742. }

  1743. /**
  1744. *  @brief      Set interrupt level.
  1745. *  @param[in]  active_low  1 for active low, 0 for active high.
  1746. *  @return     0 if successful.
  1747. */
  1748. int mpu_set_int_level(unsigned char active_low)
  1749. {
  1750.     st.chip_cfg.active_low_int = active_low;
  1751.     return 0;
  1752. }

  1753. /**
  1754. *  @brief      Enable latched interrupts.
  1755. *  Any MPU register will clear the interrupt.
  1756. *  @param[in]  enable  1 to enable, 0 to disable.
  1757. *  @return     0 if successful.
  1758. */
  1759. int mpu_set_int_latched(unsigned char enable)
  1760. {
  1761.     unsigned char tmp;
  1762.     if (st.chip_cfg.latched_int == enable)
  1763.         return 0;

  1764.     if (enable)
  1765.         tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
  1766.     else
  1767.         tmp = 0;
  1768.     if (st.chip_cfg.bypass_mode)
  1769.         tmp |= BIT_BYPASS_EN;
  1770.     if (st.chip_cfg.active_low_int)
  1771.         tmp |= BIT_ACTL;
  1772.     if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
  1773.         return -1;
  1774.     st.chip_cfg.latched_int = enable;
  1775.     return 0;
  1776. }

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

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

  1783.     shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
  1784.     shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
  1785.     shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
  1786.     for (ii = 0; ii < 3; ii++) {
  1787.         if (!shift_code[ii]) {
  1788.             st_shift[ii] = 0.f;
  1789.             continue;
  1790.         }
  1791.         /* Equivalent to..
  1792.          * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
  1793.          */
  1794.         st_shift[ii] = 0.34f;
  1795.         while (--shift_code[ii])
  1796.             st_shift[ii] *= 1.034f;
  1797.     }
  1798.     return 0;
  1799. }

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

  1804.     get_accel_prod_shift(st_shift);
  1805.     for(jj = 0; jj < 3; jj++) {
  1806.         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
  1807.         if (st_shift[jj]) {
  1808.             st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
  1809.             if (fabs(st_shift_var) > test.max_accel_var)
  1810.                 result |= 1 << jj;
  1811.         } else if ((st_shift_cust < test.min_g) ||
  1812.             (st_shift_cust > test.max_g))
  1813.             result |= 1 << jj;
  1814.     }

  1815.     return result;
  1816. }

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

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

  1824.     tmp[0] &= 0x1F;
  1825.     tmp[1] &= 0x1F;
  1826.     tmp[2] &= 0x1F;

  1827.     for (jj = 0; jj < 3; jj++) {
  1828.         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
  1829.         if (tmp[jj]) {
  1830.             st_shift = 3275.f / test.gyro_sens;
  1831.             while (--tmp[jj])
  1832.                 st_shift *= 1.046f;
  1833.             st_shift_var = st_shift_cust / st_shift - 1.f;
  1834.             if (fabs(st_shift_var) > test.max_gyro_var)
  1835.                 result |= 1 << jj;
  1836.         } else if ((st_shift_cust < test.min_dps) ||
  1837.             (st_shift_cust > test.max_dps))
  1838.             result |= 1 << jj;
  1839.     }
  1840.     return result;
  1841. }

  1842. #ifdef AK89xx_SECONDARY
  1843. static int compass_self_test(void)
  1844. {
  1845.     unsigned char tmp[6];
  1846.     unsigned char tries = 10;
  1847.     int result = 0x07;
  1848.     short data;

  1849.     mpu_set_bypass(1);

  1850.     tmp[0] = AKM_POWER_DOWN;
  1851.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
  1852.         return 0x07;
  1853.     tmp[0] = AKM_BIT_SELF_TEST;
  1854.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
  1855.         goto AKM_restore;
  1856.     tmp[0] = AKM_MODE_SELF_TEST;
  1857.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
  1858.         goto AKM_restore;

  1859.     do {
  1860.         delay_ms(10);
  1861.         if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
  1862.             goto AKM_restore;
  1863.         if (tmp[0] & AKM_DATA_READY)
  1864.             break;
  1865.     } while (tries--);
  1866.     if (!(tmp[0] & AKM_DATA_READY))
  1867.         goto AKM_restore;

  1868.     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
  1869.         goto AKM_restore;

  1870.     result = 0;
  1871.     data = (short)(tmp[1] << 8) | tmp[0];
  1872.     if ((data > 100) || (data < -100))
  1873.         result |= 0x01;
  1874.     data = (short)(tmp[3] << 8) | tmp[2];
  1875.     if ((data > 100) || (data < -100))
  1876.         result |= 0x02;
  1877.     data = (short)(tmp[5] << 8) | tmp[4];
  1878.     if ((data > -300) || (data < -1000))
  1879.         result |= 0x04;

  1880. AKM_restore:
  1881.     tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
  1882.     i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
  1883.     tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
  1884.     i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
  1885.     mpu_set_bypass(0);
  1886.     return result;
  1887. }
  1888. #endif
  1889. #endif

  1890. static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
  1891. {
  1892.     unsigned char data[MAX_PACKET_LENGTH];
  1893.     unsigned char packet_count, ii;
  1894.     unsigned short fifo_count;

  1895.     data[0] = 0x01;
  1896.     data[1] = 0;
  1897.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
  1898.         return -1;
  1899.     delay_ms(200);
  1900.     data[0] = 0;
  1901.     if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
  1902.         return -1;
  1903.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
  1904.         return -1;
  1905.     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
  1906.         return -1;
  1907.     if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
  1908.         return -1;
  1909.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
  1910.         return -1;
  1911.     data[0] = BIT_FIFO_RST | BIT_DMP_RST;
  1912.     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
  1913.         return -1;
  1914.     delay_ms(15);
  1915.     data[0] = st.test->reg_lpf;
  1916.     if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
  1917.         return -1;
  1918.     data[0] = st.test->reg_rate_div;
  1919.     if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
  1920.         return -1;
  1921.     if (hw_test)
  1922.         data[0] = st.test->reg_gyro_fsr | 0xE0;
  1923.     else
  1924.         data[0] = st.test->reg_gyro_fsr;
  1925.     if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
  1926.         return -1;

  1927.     if (hw_test)
  1928.         data[0] = st.test->reg_accel_fsr | 0xE0;
  1929.     else
  1930.         data[0] = test.reg_accel_fsr;
  1931.     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
  1932.         return -1;
  1933.     if (hw_test)
  1934.         delay_ms(200);

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

  1939.     data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
  1940.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
  1941.         return -1;
  1942.     delay_ms(test.wait_ms);
  1943.     data[0] = 0;
  1944.     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
  1945.         return -1;

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

  1948.     fifo_count = (data[0] << 8) | data[1];
  1949.     packet_count = fifo_count / MAX_PACKET_LENGTH;
  1950.     gyro[0] = gyro[1] = gyro[2] = 0;
  1951.     accel[0] = accel[1] = accel[2] = 0;

  1952.     for (ii = 0; ii < packet_count; ii++) {
  1953.         short accel_cur[3], gyro_cur[3];
  1954.         if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
  1955.             return -1;
  1956.         accel_cur[0] = ((short)data[0] << 8) | data[1];
  1957.         accel_cur[1] = ((short)data[2] << 8) | data[3];
  1958.         accel_cur[2] = ((short)data[4] << 8) | data[5];
  1959.         accel[0] += (long)accel_cur[0];
  1960.         accel[1] += (long)accel_cur[1];
  1961.         accel[2] += (long)accel_cur[2];
  1962.         gyro_cur[0] = (((short)data[6] << 8) | data[7]);
  1963.         gyro_cur[1] = (((short)data[8] << 8) | data[9]);
  1964.         gyro_cur[2] = (((short)data[10] << 8) | data[11]);
  1965.         gyro[0] += (long)gyro_cur[0];
  1966.         gyro[1] += (long)gyro_cur[1];
  1967.         gyro[2] += (long)gyro_cur[2];
  1968.     }
  1969. #ifdef EMPL_NO_64BIT
  1970.     gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
  1971.     gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
  1972.     gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
  1973.     if (has_accel) {
  1974.         accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
  1975.             packet_count);
  1976.         accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
  1977.             packet_count);
  1978.         accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
  1979.             packet_count);
  1980.         /* Don't remove gravity! */
  1981.         accel[2] -= 65536L;
  1982.     }
  1983. #else
  1984.     gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
  1985.     gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
  1986.     gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
  1987.     accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
  1988.         packet_count);
  1989.     accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
  1990.         packet_count);
  1991.     accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
  1992.         packet_count);
  1993.     /* Don't remove gravity! */
  1994.     if (accel[2] > 0L)
  1995.         accel[2] -= 65536L;
  1996.     else
  1997.         accel[2] += 65536L;
  1998. #endif

  1999.     return 0;
  2000. }

  2001. /**
  2002. *  @brief      Trigger gyro/accel/compass self-test.
  2003. *  On success/error, the self-test returns a mask representing the sensor(s)
  2004. *  that failed. For each bit, a one (1) represents a "pass" case; conversely,
  2005. *  a zero (0) indicates a failure.
  2006. *
  2007. *  \n The mask is defined as follows:
  2008. *  \n Bit 0:   Gyro.
  2009. *  \n Bit 1:   Accel.
  2010. *  \n Bit 2:   Compass.
  2011. *
  2012. *  \n Currently, the hardware self-test is unsupported for MPU6500. However,
  2013. *  this function can still be used to obtain the accel and gyro biases.
  2014. *
  2015. *  \n This function must be called with the device either face-up or face-down
  2016. *  (z-axis is parallel to gravity).
  2017. *  @param[out] gyro        Gyro biases in q16 format.
  2018. *  @param[out] accel       Accel biases (if applicable) in q16 format.
  2019. *  @return     Result mask (see above).
  2020. */
  2021. int mpu_run_self_test(long *gyro, long *accel)
  2022. {
  2023. #ifdef MPU6050
  2024.     const unsigned char tries = 2;
  2025.     long gyro_st[3], accel_st[3];
  2026.     unsigned char accel_result, gyro_result;
  2027. #ifdef AK89xx_SECONDARY
  2028.     unsigned char compass_result;
  2029. #endif
  2030.     int ii;
  2031. #endif
  2032.     int result;
  2033.     unsigned char accel_fsr, fifo_sensors, sensors_on;
  2034.     unsigned short gyro_fsr, sample_rate, lpf;
  2035.     unsigned char dmp_was_on;

  2036.     if (st.chip_cfg.dmp_on) {
  2037.         mpu_set_dmp_state(0);
  2038.         dmp_was_on = 1;
  2039.     } else
  2040.         dmp_was_on = 0;

  2041.     /* Get initial settings. */
  2042.     mpu_get_gyro_fsr(&gyro_fsr);
  2043.     mpu_get_accel_fsr(&accel_fsr);
  2044.     mpu_get_lpf(&lpf);
  2045.     mpu_get_sample_rate(&sample_rate);
  2046.     sensors_on = st.chip_cfg.sensors;
  2047.     mpu_get_fifo_config(&fifo_sensors);

  2048.     /* For older chips, the self-test will be different. */
  2049. #if defined MPU6050
  2050.     for (ii = 0; ii < tries; ii++)
  2051.         if (!get_st_biases(gyro, accel, 0))
  2052.             break;
  2053.     if (ii == tries) {
  2054.         /* If we reach this point, we most likely encountered an I2C error.
  2055.          * We'll just report an error for all three sensors.
  2056.          */
  2057.         result = 0;
  2058.         goto restore;
  2059.     }
  2060.     for (ii = 0; ii < tries; ii++)
  2061.         if (!get_st_biases(gyro_st, accel_st, 1))
  2062.             break;
  2063.     if (ii == tries) {
  2064.         /* Again, probably an I2C error. */
  2065.         result = 0;
  2066.         goto restore;
  2067.     }
  2068.     accel_result = accel_self_test(accel, accel_st);
  2069.     gyro_result = gyro_self_test(gyro, gyro_st);

  2070.     result = 0;
  2071.     if (!gyro_result)
  2072.         result |= 0x01;
  2073.     if (!accel_result)
  2074.         result |= 0x02;

  2075. #ifdef AK89xx_SECONDARY
  2076.     compass_result = compass_self_test();
  2077.     if (!compass_result)
  2078.         result |= 0x04;
  2079. #endif
  2080. restore:
  2081. #elif defined MPU6500
  2082.     /* For now, this function will return a "pass" result for all three sensors
  2083.      * for compatibility with current test applications.
  2084.      */
  2085.     get_st_biases(gyro, accel, 0);
  2086.     result = 0x3;
  2087. #endif
  2088.     /* Set to invalid values to ensure no I2C writes are skipped. */
  2089.     st.chip_cfg.gyro_fsr = 0xFF;
  2090.     st.chip_cfg.accel_fsr = 0xFF;
  2091.     st.chip_cfg.lpf = 0xFF;
  2092.     st.chip_cfg.sample_rate = 0xFFFF;
  2093.     st.chip_cfg.sensors = 0xFF;
  2094.     st.chip_cfg.fifo_enable = 0xFF;
  2095.     st.chip_cfg.clk_src = INV_CLK_PLL;
  2096.     mpu_set_gyro_fsr(gyro_fsr);
  2097.     mpu_set_accel_fsr(accel_fsr);
  2098.     mpu_set_lpf(lpf);
  2099.     mpu_set_sample_rate(sample_rate);
  2100.     mpu_set_sensors(sensors_on);
  2101.     mpu_configure_fifo(fifo_sensors);

  2102.     if (dmp_was_on)
  2103.         mpu_set_dmp_state(1);

  2104.     return result;
  2105. }

  2106. /**
  2107. *  @brief      Write to the DMP memory.
  2108. *  This function prevents I2C writes past the bank boundaries. The DMP memory
  2109. *  is only accessible when the chip is awake.
  2110. *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
  2111. *  @param[in]  length      Number of bytes to write.
  2112. *  @param[in]  data        Bytes to write to memory.
  2113. *  @return     0 if successful.
  2114. */
  2115. int mpu_write_mem(unsigned short mem_addr, unsigned short length,
  2116.         unsigned char *data)
  2117. {
  2118.     unsigned char tmp[2];

  2119.     if (!data)
  2120.         return -1;
  2121.     if (!st.chip_cfg.sensors)
  2122.         return -1;

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

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

  2128.     if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
  2129.         return -1;
  2130.     if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
  2131.         return -1;
  2132.     return 0;
  2133. }

  2134. /**
  2135. *  @brief      Read from the DMP memory.
  2136. *  This function prevents I2C reads past the bank boundaries. The DMP memory
  2137. *  is only accessible when the chip is awake.
  2138. *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
  2139. *  @param[in]  length      Number of bytes to read.
  2140. *  @param[out] data        Bytes read from memory.
  2141. *  @return     0 if successful.
  2142. */
  2143. int mpu_read_mem(unsigned short mem_addr, unsigned short length,
  2144.         unsigned char *data)
  2145. {
  2146.     unsigned char tmp[2];

  2147.     if (!data)
  2148.         return -1;
  2149.     if (!st.chip_cfg.sensors)
  2150.         return -1;

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

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

  2156.     if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
  2157.         return -1;
  2158.     if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
  2159.         return -1;
  2160.     return 0;
  2161. }

  2162. /**
  2163. *  @brief      Load and verify DMP image.
  2164. *  @param[in]  length      Length of DMP image.
  2165. *  @param[in]  firmware    DMP code.
  2166. *  @param[in]  start_addr  Starting address of DMP code memory.
  2167. *  @param[in]  sample_rate Fixed sampling rate used when DMP is enabled.
  2168. *  @return     0 if successful.
  2169. */
  2170. int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
  2171.     unsigned short start_addr, unsigned short sample_rate)
  2172. {
  2173.     unsigned short ii;
  2174.     unsigned short this_write;
  2175.     /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
  2176. #define LOAD_CHUNK  (16)
  2177.     unsigned char cur[LOAD_CHUNK], tmp[2];

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

  2181.     if (!firmware)
  2182.         return -1;
  2183.     for (ii = 0; ii < length; ii += this_write) {
  2184.         this_write = min(LOAD_CHUNK, length - ii);
  2185.         if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
  2186.             return -1;
  2187.         if (mpu_read_mem(ii, this_write, cur))
  2188.             return -1;
  2189.         if (memcmp(firmware+ii, cur, this_write))
  2190.             return -2;
  2191.     }

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

  2197.     st.chip_cfg.dmp_loaded = 1;
  2198.     st.chip_cfg.dmp_sample_rate = sample_rate;
  2199.     return 0;
  2200. }

  2201. /**
  2202. *  @brief      Enable/disable DMP support.
  2203. *  @param[in]  enable  1 to turn on the DMP.
  2204. *  @return     0 if successful.
  2205. */
  2206. int mpu_set_dmp_state(unsigned char enable)
  2207. {
  2208.     unsigned char tmp;
  2209.     if (st.chip_cfg.dmp_on == enable)
  2210.         return 0;

  2211.     if (enable) {
  2212.         if (!st.chip_cfg.dmp_loaded)
  2213.             return -1;
  2214.         /* Disable data ready interrupt. */
  2215.         set_int_enable(0);
  2216.         /* Disable bypass mode. */
  2217.         mpu_set_bypass(0);
  2218.         /* Keep constant sample rate, FIFO rate controlled by DMP. */
  2219.         mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
  2220.         /* Remove FIFO elements. */
  2221.         tmp = 0;
  2222.         i2c_write(st.hw->addr, 0x23, 1, &tmp);
  2223.         st.chip_cfg.dmp_on = 1;
  2224.         /* Enable DMP interrupt. */
  2225.         set_int_enable(1);
  2226.         mpu_reset_fifo();
  2227.     } else {
  2228.         /* Disable DMP interrupt. */
  2229.         set_int_enable(0);
  2230.         /* Restore FIFO settings. */
  2231.         tmp = st.chip_cfg.fifo_enable;
  2232.         i2c_write(st.hw->addr, 0x23, 1, &tmp);
  2233.         st.chip_cfg.dmp_on = 0;
  2234.         mpu_reset_fifo();
  2235.     }
  2236.     return 0;
  2237. }

  2238. /**
  2239. *  @brief      Get DMP state.
  2240. *  @param[out] enabled 1 if enabled.
  2241. *  @return     0 if successful.
  2242. */
  2243. int mpu_get_dmp_state(unsigned char *enabled)
  2244. {
  2245.     enabled[0] = st.chip_cfg.dmp_on;
  2246.     return 0;
  2247. }


  2248. /* This initialization is similar to the one in ak8975.c. */
  2249. //static int setup_compass(void)
  2250. //{
  2251. //#ifdef AK89xx_SECONDARY
  2252. //    unsigned char data[4], akm_addr;
  2253. //
  2254. //    mpu_set_bypass(1);
  2255. //
  2256. //    /* Find compass. Possible addresses range from 0x0C to 0x0F. */
  2257. //    for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
  2258. //        int result;
  2259. //        result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
  2260. //        if (!result && (data[0] == AKM_WHOAMI))
  2261. //            break;
  2262. //    }
  2263. //
  2264. //    if (akm_addr > 0x0F) {
  2265. //        /* TODO: Handle this case in all compass-related functions. */
  2266. //        log_e("Compass not found.\n");
  2267. //        return -1;
  2268. //    }
  2269. //
  2270. //    st.chip_cfg.compass_addr = akm_addr;
  2271. //
  2272. //    data[0] = AKM_POWER_DOWN;
  2273. //    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
  2274. //        return -1;
  2275. //    delay_ms(1);
  2276. //
  2277. //    data[0] = AKM_FUSE_ROM_ACCESS;
  2278. //    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
  2279. //        return -1;
  2280. //    delay_ms(1);
  2281. //
  2282. //    /* Get sensitivity adjustment data from fuse ROM. */
  2283. //    if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
  2284. //        return -1;
  2285. //    st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
  2286. //    st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
  2287. //    st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
  2288. //
  2289. //    data[0] = AKM_POWER_DOWN;
  2290. //    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
  2291. //        return -1;
  2292. //    delay_ms(1);
  2293. //
  2294. //    mpu_set_bypass(0);
  2295. //
  2296. //    /* Set up master mode, master clock, and ES bit. */
  2297. //    data[0] = 0x40;
  2298. //    if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
  2299. //        return -1;
  2300. //
  2301. //    /* Slave 0 reads from AKM data registers. */
  2302. //    data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
  2303. //    if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
  2304. //        return -1;
  2305. //
  2306. //    /* Compass reads start at this register. */
  2307. //    data[0] = AKM_REG_ST1;
  2308. //    if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
  2309. //        return -1;
  2310. //
  2311. //    /* Enable slave 0, 8-byte reads. */
  2312. //    data[0] = BIT_SLAVE_EN | 8;
  2313. //    if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
  2314. //        return -1;
  2315. //
  2316. //    /* Slave 1 changes AKM measurement mode. */
  2317. //    data[0] = st.chip_cfg.compass_addr;
  2318. //    if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
  2319. //        return -1;
  2320. //
  2321. //    /* AKM measurement mode register. */
  2322. //    data[0] = AKM_REG_CNTL;
  2323. //    if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
  2324. //        return -1;
  2325. //
  2326. //    /* Enable slave 1, 1-byte writes. */
  2327. //    data[0] = BIT_SLAVE_EN | 1;
  2328. //    if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
  2329. //        return -1;
  2330. //
  2331. //    /* Set slave 1 data. */
  2332. //    data[0] = AKM_SINGLE_MEASUREMENT;
  2333. //    if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
  2334. //        return -1;
  2335. //
  2336. //    /* Trigger slave 0 and slave 1 actions at each sample. */
  2337. //    data[0] = 0x03;
  2338. //    if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
  2339. //        return -1;
  2340. //
  2341. //#ifdef MPU9150
  2342. //    /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
  2343. //    data[0] = BIT_I2C_MST_VDDIO;
  2344. //    if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
  2345. //        return -1;
  2346. //#endif
  2347. //
  2348. //    return 0;
  2349. //#else
  2350. //    return -1;
  2351. //#endif
  2352. //}

  2353. /**
  2354. *  @brief      Read raw compass data.
  2355. *  @param[out] data        Raw data in hardware units.
  2356. *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
  2357. *  @return     0 if successful.
  2358. */
  2359. int mpu_get_compass_reg(short *data, unsigned long *timestamp)
  2360. {
  2361. #ifdef AK89xx_SECONDARY
  2362.     unsigned char tmp[9];

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

  2365. #ifdef AK89xx_BYPASS
  2366.     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
  2367.         return -1;
  2368.     tmp[8] = AKM_SINGLE_MEASUREMENT;
  2369.     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
  2370.         return -1;
  2371. #else
  2372.     if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
  2373.         return -1;
  2374. #endif

  2375. #if defined AK8975_SECONDARY
  2376.     /* AK8975 doesn't have the overrun error bit. */
  2377.     if (!(tmp[0] & AKM_DATA_READY))
  2378.         return -2;
  2379.     if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
  2380.         return -3;
  2381. #elif defined AK8963_SECONDARY
  2382.     /* AK8963 doesn't have the data read error bit. */
  2383.     if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
  2384.         return -2;
  2385.     if (tmp[7] & AKM_OVERFLOW)
  2386. ……………………

  2387. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

全部资料51hei下载地址:
MPU6050.zip (38.32 KB, 下载次数: 139)



作者: jeffrey.....    时间: 2018-8-18 20:12
请问这个可以用来调整角度吗
作者: 韦杰123    时间: 2018-8-19 16:22
jeffrey..... 发表于 2018-8-18 20:12
请问这个可以用来调整角度吗

可以的

作者: 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
这个可以直接用吗,正纠结怎么移植到32上呢,虽然我用的是f030,不过没插,感谢,救星。
作者: 韦杰123    时间: 2019-10-6 12:43
esmember 发表于 2019-9-30 15:26
这个可以直接用吗,正纠结怎么移植到32上呢,虽然我用的是f030,不过没插,感谢,救星。

可以的a
作者: nba    时间: 2019-10-6 15:06
感谢楼主分想
作者: 吃了吗    时间: 2020-4-16 13:02
感谢,可以加入OLED显示吗





欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1