openXsensor/inv_mpu.c

   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 (support for other devices have been deleted in order to save flash memory)
  16  */
  17
  18 #define EMPL_TARGET_ATMEGA328 // initially the library supported several mcu, some code has been removed; this define is kept for saferty
  19 #define MPU6050 // initially the library supported several mpu, some code has been removed; this define is kept for saferty
  20
  21 #include <stdio.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 //#include <string.h>
  25 //#include <math.h>
  26 #include "inv_mpu.h"
  27
  28 //#include "I2C.h"
  29 /* The following functions must be defined for this platform:
  30  * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
  31  *      unsigned char length, unsigned char const *data)
  32  * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
  33  *      unsigned char length, unsigned char *data)
  34  * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
  35  * labs(long x)
  36  * fabsf(float x)
  37  */
  38 #include "arduino_shim.h"
  39
  40 //#define DEBUG_READ_FIFO    // this function has been added to create a function get_fifo_count_debug to print it in oXs_imu if required
  41 //#define i2c_write(a,b,c,d) shim_i2c_write(a,b,c,d)
  42 //#define i2c_write(a,b,c,d)  I2c.write( (uint8_t) a , (uint8_t) b ,  (uint8_t) c  , (uint8_t *) d ) ; // return 0 on success
  43 #define i2c_writeByte(b,d) shim_i2c_writeByte(b,d)
  44 #define i2c_read(a,b,c,d) shim_i2c_read(a,b,c,d)
  45 #define i2c_writeBlock(b,c,d) shim_i2c_writeBlock(b,c,d)
  46
  47 /*
  48 static inline int reg_int_cb(struct int_param_s *int_param)
  49 {
  50         attachInterrupt(int_param->pin, int_param->cb, RISING);
  51         return 0;
  52 }
  53 */
  54
  55 #define fabs(x)     (((x)>0)?(x):-(x))
  56
  57
  58 struct chip_cfg_s {   // keep trace of mpu6050 config.
  59     /* Matches gyro_cfg >> 3 & 0x03 */
  60 //    unsigned char gyro_fsr;
  61     /* Matches accel_cfg >> 3 & 0x03 */
  62 //    unsigned char accel_fsr;
  63     /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
  64 //    unsigned char sensors;
  65     /* Matches config register. */
  66 //    unsigned char lpf;
  67 //    unsigned char clk_src;
  68     /* Sample rate, NOT rate divider. */
  69 //    unsigned short sample_rate;
  70     /* Matches fifo_en register. */
  71 //    unsigned char fifo_enable;
  72     /* Matches int enable register. */
  73 //    unsigned char int_enable;
  74     /* 1 if devices on auxiliary I2C bus appear on the primary. */
  75 //    unsigned char bypass_mode;
  76     /* 1 if half-sensitivity.
  77      * NOTE: This doesn't belong here, but everything else in hw_s is const,
  78      * and this allows us to save some precious RAM.
  79      */
  80     unsigned char accel_half;
  81     /* 1 if device in low-power accel-only mode. */
  82 //    unsigned char lp_accel_mode;
  83     /* 1 if interrupts are only triggered on motion events. */
  84 //    unsigned char int_motion_only;
  85 //    struct motion_int_cache_s cache;
  86     /* 1 for active low interrupts. */
  87 //    unsigned char active_low_int;
  88     /* 1 for latched interrupts. */
  89 //    unsigned char latched_int;
  90     /* 1 if DMP is enabled. */
  91 //    unsigned char dmp_on;
  92     /* Ensures that DMP will only be loaded once. */
  93 //    unsigned char dmp_loaded;
  94     /* Sampling rate used when DMP is enabled. */
  95 //    unsigned short dmp_sample_rate;
  96 };
  97
  98
  99 /* Gyro driver state variables. */
 100 //struct gyro_state_s {
 101 //    struct chip_cfg_s chip_cfg;
 102 //};
 103
 104
 105     struct chip_cfg_s chip_cfg;
 106
 107
 108
 109 /* Filter configurations. */
 110 enum lpf_e {
 111     INV_FILTER_256HZ_NOLPF2 = 0,
 112     INV_FILTER_188HZ,
 113     INV_FILTER_98HZ,
 114     INV_FILTER_42HZ,
 115     INV_FILTER_20HZ,
 116     INV_FILTER_10HZ,
 117     INV_FILTER_5HZ,
 118     INV_FILTER_2100HZ_NOLPF,
 119     NUM_FILTER
 120 };
 121
 122 /* Full scale ranges. */
 123 enum gyro_fsr_e {
 124     INV_FSR_250DPS = 0,
 125     INV_FSR_500DPS,
 126     INV_FSR_1000DPS,
 127     INV_FSR_2000DPS,
 128     NUM_GYRO_FSR
 129 };
 130
 131 /* Full scale ranges. */
 132 enum accel_fsr_e {
 133     INV_FSR_2G = 0,
 134     INV_FSR_4G,
 135     INV_FSR_8G,
 136     INV_FSR_16G,
 137     NUM_ACCEL_FSR
 138 };
 139
 140 /* Clock sources. */
 141 enum clock_sel_e {
 142     INV_CLK_INTERNAL = 0,
 143     INV_CLK_PLL,
 144     NUM_CLK
 145 };
 146
 147 /* Low-power accel wakeup rates. */
 148 enum lp_accel_rate_e {
 149     INV_LPA_1_25HZ,
 150     INV_LPA_5HZ,
 151     INV_LPA_20HZ,
 152     INV_LPA_40HZ
 153 };
 154
 155 #define BIT_I2C_MST_VDDIO   (0x80)
 156 #define BIT_FIFO_EN         (0x40)
 157 #define BIT_DMP_EN          (0x80)
 158 #define BIT_FIFO_RST        (0x04)
 159 #define BIT_DMP_RST         (0x08)
 160 #define BIT_FIFO_OVERFLOW   (0x10)
 161 #define BIT_DATA_RDY_EN     (0x01)
 162 #define BIT_DMP_INT_EN      (0x02)
 163 #define BIT_MOT_INT_EN      (0x40)
 164 #define BITS_FSR            (0x18)
 165 #define BITS_LPF            (0x07)
 166 #define BITS_HPF            (0x07)
 167 #define BITS_CLK            (0x07)
 168 #define BIT_FIFO_SIZE_1024  (0x40)
 169 #define BIT_FIFO_SIZE_2048  (0x80)
 170 #define BIT_FIFO_SIZE_4096  (0xC0)
 171 #define BIT_RESET           (0x80)
 172 #define BIT_SLEEP           (0x40)
 173 #define BIT_S0_DELAY_EN     (0x01)
 174 #define BIT_S2_DELAY_EN     (0x04)
 175 #define BITS_SLAVE_LENGTH   (0x0F)
 176 #define BIT_SLAVE_BYTE_SW   (0x40)
 177 #define BIT_SLAVE_GROUP     (0x10)
 178 #define BIT_SLAVE_EN        (0x80)
 179 #define BIT_I2C_READ        (0x80)
 180 #define BITS_I2C_MASTER_DLY (0x1F)
 181 #define BIT_AUX_IF_EN       (0x20)
 182 #define BIT_ACTL            (0x80)
 183 #define BIT_LATCH_EN        (0x20)
 184 #define BIT_ANY_RD_CLR      (0x10)
 185 #define BIT_BYPASS_EN       (0x02)
 186 #define BITS_WOM_EN         (0xC0)
 187 #define BIT_LPA_CYCLE       (0x20)
 188 #define BIT_STBY_XA         (0x20)
 189 #define BIT_STBY_YA         (0x10)
 190 #define BIT_STBY_ZA         (0x08)
 191 #define BIT_STBY_XG         (0x04)
 192 #define BIT_STBY_YG         (0x02)
 193 #define BIT_STBY_ZG         (0x01)
 194 #define BIT_STBY_XYZA       (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
 195 #define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
 196
 197
 198 /* Hardware registers needed by driver. */
 199 #define GYRO_REG_who_am_i         0x75
 200 #define GYRO_REG_rate_div         0x19
 201 #define GYRO_REG_lpf              0x1A
 202 #define GYRO_REG_prod_id          0x0C
 203 #define GYRO_REG_user_ctrl        0x6A
 204 #define GYRO_REG_fifo_en          0x23
 205 #define GYRO_REG_gyro_cfg         0x1B
 206 #define GYRO_REG_accel_cfg        0x1C
 207 #define GYRO_REG_motion_thr       0x1F
 208 #define GYRO_REG_motion_dur       0x20
 209 #define GYRO_REG_fifo_count_h     0x72
 210 #define GYRO_REG_fifo_r_w         0x74
 211 #define GYRO_REG_raw_gyro         0x43
 212 #define GYRO_REG_raw_accel        0x3B
 213 #define GYRO_REG_temp             0x41
 214 #define GYRO_REG_int_enable       0x38
 215 #define GYRO_REG_dmp_int_status   0x39
 216 #define GYRO_REG_int_status       0x3A
 217 #define GYRO_REG_pwr_mgmt_1       0x6B
 218 #define GYRO_REG_pwr_mgmt_2       0x6C
 219 #define GYRO_REG_int_pin_cfg      0x37
 220 #define GYRO_REG_mem_r_w          0x6F
 221 #define GYRO_REG_accel_offs       0x06
 222 #define GYRO_REG_i2c_mst          0x24
 223 #define GYRO_REG_bank_sel         0x6D
 224 #define GYRO_REG_mem_start_addr   0x6E
 225 #define GYRO_REG_prgm_start_h     0x70
 226
 227 // Information specific to a particular device. //
 228 #define INV6050_addr              0x68
 229 #define INV6050_max_fifo          1024
 230 #define INV6050_num_reg           118
 231 #define INV6050_temp_sens         340
 232 #define INV6050_temp_offset       -521
 233 #define INV6050_bank_size         256
 234
 235 // Information for self test I think //
 236 #define TEST_gyro_sens        32768/250
 237 #define TEST_accel_sens       32768/16
 238 #define TEST_reg_rate_div     0    /* 1kHz. */
 239 #define TEST_reg_lpf          1    /* 188Hz. */
 240 #define TEST_reg_gyro_fsr     0    /* 250dps. */
 241 #define TEST_reg_accel_fsr    0x18 /* 16g. */
 242 #define TEST_wait_ms          50
 243 #define TEST_packet_thresh    5    /* 5% */
 244 #define TEST_min_dps          10.f
 245 #define TEST_max_dps          105.f
 246 #define TEST_max_gyro_var     0.14f
 247 #define TEST_min_g            0.3f
 248 #define TEST_max_g            0.95f
 249 #define TEST_max_accel_var    0.14f
 250
 251
 252 #define MAX_PACKET_LENGTH (12)
 253
 254
 255 //static struct gyro_state_s st ; // store the status and config of MPU6050
 256
 257
 258 /**
 259  *  @brief      Enable/disable data ready interrupt.                                // saving : if dmp is always used this code could be simplified
 260  *  If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
 261  *  interrupt is used.
 262  *  @param[in]  enable      1 to enable interrupt.
 263  *  @return     0 if successful.
 264  */
 265 /*
 266 static int set_int_enable(unsigned char enable)
 267 {
 268     unsigned char tmp;
 269     if (chip_cfg.dmp_on) {
 270         if (enable)
 271             tmp = BIT_DMP_INT_EN;
 272         else
 273             tmp = 0x00;
 274     //        if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, &tmp))
 275           if (i2c_writeByte( GYRO_REG_int_enable,  tmp))
 276             return -1;
 277         chip_cfg.int_enable = tmp;
 278     } else {
 279             if (!chip_cfg.sensors)
 280                 return -1;
 281         if (enable && chip_cfg.int_enable)
 282             return 0;
 283         if (enable)
 284             tmp = BIT_DATA_RDY_EN;
 285         else
 286             tmp = 0x00;
 287     //        if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, &tmp))
 288           if (i2c_writeByte(  GYRO_REG_int_enable,  tmp))
 289             return -1;
 290         chip_cfg.int_enable = tmp;
 291     }
 292     return 0;
 293 }
 294 */
 295 /*
 296 static int set_int_enablexx(unsigned char enable)
 297 {
 298        return  i2c_writeByte( GYRO_REG_int_enable,  BIT_DMP_INT_EN ) ;     // it is 0x02 ; it is not documented in the datashee
 299 }
 300 */
 301
 302 /**
 303  *  @brief      Register dump for testing.
 304  *  @return     0 if successful.
 305  */
 306 /*                                            // this does not work if all registers are defined with #define instead of a structure; so this code is commented
 307 int mpu_reg_dump(void)
 308 {
 309     unsigned char ii;
 310     unsigned char data;
 311
 312     for (ii = 0; ii < INV6050_num_reg; ii++) {
 313         if (ii == GYRO_REG_fifo_r_w || ii == GYRO_REG_mem_r_w)
 314             continue;
 315         if (i2c_read( INV6050_addr, ii, 1, &data))
 316             return -1;
 317         log_i("%#5x: %#5x\r\n", ii, data);
 318     }
 319     return 0;
 320 }
 321 */
 322
 323 /**
 324  *  @brief      Read from a single register.                                    // saving : this code could be reduced if we remove the checks
 325  *  NOTE: The memory and FIFO read/write registers cannot be accessed.          // this code is not used by oXs
 326  *  @param[in]  reg     Register address.
 327  *  @param[out] data    Register data.
 328  *  @return     0 if successful.
 329  */
 330 int mpu_read_reg(unsigned char reg, unsigned char *data)
 331 {
 332     if (reg == GYRO_REG_fifo_r_w || reg == GYRO_REG_mem_r_w)
 333         return -1;
 334     if (reg >= INV6050_num_reg)
 335         return -1;
 336     return i2c_read( INV6050_addr, reg, 1, data);
 337 }
 338
 339 /*
 340 void mpu_force_reset()                                         // this code is currently not used; so it does not take flash memory
 341 {
 342     unsigned char data[6];
 343         data[0] = BIT_RESET;
 344         int i;
 345     for ( i = 0; i < 100; i++ )
 346         {
 347 //              if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 1, data) == 0)
 348     if (i2c_writeByte( GYRO_REG_pwr_mgmt_1,  data[0] ) == 0)
 349                 {
 350                         delay(100);
 351
 352                                               // Wake up chip.
 353                         data[0] = 0x00;
 354 //                      if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 1, data)==0)
 355       if (i2c_writeBlock( GYRO_REG_pwr_mgmt_1, 1, data)==0)
 356                         {
 357                                 break;
 358                         }
 359                         else
 360                         {
 361                                 delay(10);
 362                         }
 363                 }
 364                 else
 365                 {
 366                         delay(10);
 367                 }
 368         }
 369 }
 370 */
 371
 372 /**
 373  *  @brief      Initialize hardware.
 374  *  Initial configuration:\n
 375  *  Gyro FSR: +/- 2000DPS\n
 376  *  Accel FSR +/- 2G\n
 377  *  DLPF: 42Hz\n
 378  *  FIFO rate: 50Hz\n
 379  *  Clock source: Gyro PLL\n
 380  *  FIFO: Disabled.\n
 381  *  Data ready interrupt: Disabled, active low, unlatched.
 382  *  @param[in]  int_param   Platform-specific parameters to interrupt API.
 383  *  @return     0 if successful.
 384  */
 385 int mpu_init()
 386 {
 387     unsigned char data[6], rev;
 388     // Reset device.
 389     if (i2c_writeByte(GYRO_REG_pwr_mgmt_1, (uint8_t) BIT_RESET ))  // reset the mpu6050
 390         return -1;
 391     delay(100);                                                    // wait that reset is done
 392
 393     // Wake up chip.                                               // filling 0 is really required (tested) to get good acceleration; this is strange because it is normally cleared automatically when reset is done
 394     if (i2c_writeByte( GYRO_REG_pwr_mgmt_1, (uint8_t) 0x00 ))
 395         return -1;
 396
 397     /* Check product revision. */                     // saving : this code could omitted if accel_half is not used. To check ; My mpu return 0
 398     if (i2c_read( INV6050_addr, GYRO_REG_accel_offs, 6, data))
 399         return -1;
 400     rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) | (data[1] & 0x01);
 401     if (rev) {
 402         /* Congrats, these parts are better. */
 403         if (rev == 1)             chip_cfg.accel_half = 1;
 404         else if ((rev == 2) || (rev ==4))        chip_cfg.accel_half = 0;
 405         else {
 406             log_e("Unsupported software product rev %d.\n", rev);
 407             return -1;
 408         }
 409     } else {
 410         if (i2c_read( INV6050_addr, GYRO_REG_prod_id, 1, data))
 411             return -1;
 412         rev = data[0] & 0x0F;
 413         if (!rev) {
 414             log_e("Product ID read as 0 indicates device is either "
 415                 "incompatible or an MPU3050.\n");
 416             return -1;
 417         } else if (rev == 4) {
 418             log_i("Half sensitivity part found.\n");
 419             chip_cfg.accel_half = 1;
 420         } else
 421             chip_cfg.accel_half = 0;
 422     }
 423
 424     /* Set to invalid values to ensure no I2C writes are skipped. */
 425 ///    chip_cfg.sensors = 0xFF;
 426 //    chip_cfg.gyro_fsr = 0xFF;            // can be omitted because filled by mpu_set_gyro_fsr()
 427 //    chip_cfg.accel_fsr = 0xFF;           // can be omitted because filled by mpu_set_accel_fsr()
 428 //    chip_cfg.lpf = 0xFF;                  // can be omitted because filled by mpu_set_lpf()
 429 //    chip_cfg.sample_rate = 0xFFFF;        // can be omitted because filled by mpu_set_sample_rate()
 430 //    chip_cfg.fifo_enable = 0xFF;
 431 //    chip_cfg.bypass_mode = 0xFF;
 432     /* mpu_set_sensors always preserves this setting. */
 433 //    chip_cfg.clk_src = INV_CLK_PLL;
 434     /* Handled in next call to mpu_set_bypass. */
 435 //    chip_cfg.active_low_int = 1;
 436 //    chip_cfg.latched_int = 0;
 437 //    chip_cfg.int_motion_only = 0;
 438 //    chip_cfg.lp_accel_mode = 0;
 439 //    memset(&chip_cfg.cache, 0, sizeof(chip_cfg.cache));
 440 //    chip_cfg.dmp_on = 0;
 441 //    chip_cfg.dmp_loaded = 0;
 442 //    chip_cfg.dmp_sample_rate = 0;
 443
 444  //   if (mpu_set_gyro_fsr(2000))
 445  //       return -1;
 446     if (i2c_writeByte(  GYRO_REG_gyro_cfg, (INV_FSR_2000DPS << 3 ))) return -1;  // set gyro_fsr to 2000 degree/sec
 447
 448 //    if (mpu_set_accel_fsr(2))  // 2 = 2g at full scale
 449 //        return -1;
 450     if (i2c_writeByte( GYRO_REG_accel_cfg, (INV_FSR_2G << 3) )) return -1;   // set accel_fsr to 2g
 451
 452 //    if (mpu_set_lpf(42))                    // it is fixed automatically to 200/2 in another function (hardcode)
 453 //        return -1;
 454 //    if (mpu_set_sample_rate(50))            // to be checked if it can be ommited ; it is overwritten when DMP is initialized
 455 //        return -1;
 456 //    if (mpu_configure_fifo(0))            // not needed ; it seems that fifo is configured by DMP feature
 457 //        return -1;
 458
 459
 460 //        reg_int_cb(int_param);               // set the call back function and the interrupt number (0) used by 6050
 461
 462     /* Already disabled by setup_compass. */
 463 //    if (mpu_set_bypass(0))               // to be checked if it can be ommited
 464 //        return -1;
 465
 466 //    mpu_set_sensors(0); // set mpu in standby ; could be replaced by next lines if really requested.
 467       if( i2c_writeByte( GYRO_REG_pwr_mgmt_1, BIT_SLEEP )) return -1 ; // set mpu in sleep mode
 468       if( i2c_writeByte( GYRO_REG_pwr_mgmt_2, 0 ) ) return -1 ;
 469     return 0;
 470 }
 471
 472 /*
 473 int getDataFromMpu(){
 474   return 0 ;
 475 }
 476 */
 477
 478 /**
 479  *  @brief      Enter low-power accel-only mode.
 480  *  In low-power accel mode, the chip goes to sleep and only wakes up to sample
 481  *  the accelerometer at one of the following frequencies:
 482  *  \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
 483   *  \n If the requested rate is not one listed above, the device will be set to
 484  *  the next highest rate. Requesting a rate above the maximum supported
 485  *  frequency will result in an error.
 486  *  \n To select a fractional wake-up frequency, round down the value passed to
 487  *  @e rate.
 488  *  @param[in]  rate        Minimum sampling rate, or zero to disable LP
 489  *                          accel mode.
 490  *  @return     0 if successful.
 491  */
 492 /*
 493 int mpu_lp_accel_mode(unsigned char rate)
 494 {
 495     unsigned char tmp[2];
 496
 497     if (rate > 40)
 498         return -1;
 499
 500     if (!rate) {
 501         mpu_set_int_latched(0);
 502         tmp[0] = 0;
 503         tmp[1] = BIT_STBY_XYZG;
 504 //        if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 2, tmp))
 505         if (i2c_writeBlock(  GYRO_REG_pwr_mgmt_1, 2, tmp))
 506             return -1;
 507         chip_cfg.lp_accel_mode = 0;
 508         return 0;
 509     }
 510     // For LP accel, we automatically configure the hardware to produce latched
 511     // interrupts. In LP accel mode, the hardware cycles into sleep mode before
 512     // it gets a chance to deassert the interrupt pin; therefore, we shift this
 513     //  responsibility over to the MCU.
 514     //
 515     // Any register read will clear the interrupt.
 516
 517     mpu_set_int_latched(1);
 518     tmp[0] = BIT_LPA_CYCLE;
 519     if (rate == 1) {
 520         tmp[1] = INV_LPA_1_25HZ;
 521         mpu_set_lpf(5);
 522     } else if (rate <= 5) {
 523         tmp[1] = INV_LPA_5HZ;
 524         mpu_set_lpf(5);
 525     } else if (rate <= 20) {
 526         tmp[1] = INV_LPA_20HZ;
 527         mpu_set_lpf(10);
 528     } else {
 529         tmp[1] = INV_LPA_40HZ;
 530         mpu_set_lpf(20);
 531     }
 532     tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
 533 //    if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 2, tmp))
 534     if (i2c_writeBlock(  GYRO_REG_pwr_mgmt_1, 2, tmp))
 535         return -1;
 536     chip_cfg.sensors = INV_XYZ_ACCEL;
 537     chip_cfg.clk_src = 0;
 538     chip_cfg.lp_accel_mode = 1;
 539 //    mpu_configure_fifo(0);    // not neaded; it seems that fifo set up is done when dmp feature are set up
 540
 541     return 0;
 542 }
 543 */
 544
 545 /**
 546  *  @brief      Read raw gyro data directly from the registers.                           // not used by oXs
 547  *  @param[out] data        Raw data in hardware units.
 548  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
 549  *  @return     0 if successful.
 550  */
 551  /*
 552 int mpu_get_gyro_reg(short *data)
 553 {
 554     unsigned char tmp[6];
 555
 556     if (!(chip_cfg.sensors & INV_XYZ_GYRO))
 557         return -1;
 558
 559     if (i2c_read( INV6050_addr, GYRO_REG_raw_gyro, 6, tmp))
 560         return -1;
 561     data[0] = (tmp[0] << 8) | tmp[1];
 562     data[1] = (tmp[2] << 8) | tmp[3];
 563     data[2] = (tmp[4] << 8) | tmp[5];
 564 //    if (timestamp)             // set as coment to save flash memory
 565 //        get_ms(timestamp);
 566     return 0;
 567 }
 568 */
 569
 570 /**
 571  *  @brief      Read raw accel data directly from the registers.                         // not used by oXs
 572  *  @param[out] data        Raw data in hardware units.
 573  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
 574  *  @return     0 if successful.
 575  */
 576 /*
 577 int mpu_get_accel_reg(short *data)
 578 {
 579     unsigned char tmp[6];
 580
 581     if (!(chip_cfg.sensors & INV_XYZ_ACCEL))
 582         return -1;
 583
 584     if (i2c_read( INV6050_addr, GYRO_REG_raw_accel, 6, tmp))
 585         return -1;
 586     data[0] = (tmp[0] << 8) | tmp[1];
 587     data[1] = (tmp[2] << 8) | tmp[3];
 588     data[2] = (tmp[4] << 8) | tmp[5];
 589 //    if (timestamp)                   // set as comment to save flash memory
 590 //        get_ms(timestamp);
 591     return 0;
 592 }
 593 */
 594
 595 /**                                                                                      // Not used by oXs
 596  *  @brief      Read temperature data directly from the registers.
 597  *  @param[out] data        Data in q16 format.
 598  *  @param[out] timestamp   Timestamp in milliseconds. Null if not needed.
 599  *  @return     0 if successful.
 600  */
 601 /*
 602 int mpu_get_temperature(long *data)
 603 {
 604     unsigned char tmp[2];
 605     short raw;
 606
 607     if (!(chip_cfg.sensors))
 608         return -1;
 609
 610     if (i2c_read( INV6050_addr, GYRO_REG_temp, 2, tmp))
 611         return -1;
 612     raw = (tmp[0] << 8) | tmp[1];
 613 //    if (timestamp)               // set as comment to save flash memory
 614 //        get_ms(timestamp);
 615
 616     data[0] = (long)((35 + ((raw - (float)INV6050_temp_offset) / INV6050_temp_sens)) * 65536L);
 617     return 0;
 618 }
 619 */
 620
 621 /**
 622  *  @brief      Push biases to the accel bias registers.                                          // not used by oXs but we should look if it can be used or not.
 623  *  This function expects biases relative to the current sensor output, and
 624  *  these biases will be added to the factory-supplied values.
 625  *  @param[in]  accel_bias  New biases.
 626  *  @return     0 if successful.
 627  */
 628
 629 int mpu_set_accel_bias(const long *accel_bias)
 630 {
 631     unsigned char data[6];
 632     short accel_hw[3];
 633     short got_accel[3];
 634     short fg[3];
 635
 636     if (!accel_bias)
 637         return -1;
 638     if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
 639         return 0;
 640
 641     if (i2c_read( INV6050_addr, 3, 3, data))
 642         return -1;
 643     fg[0] = ((data[0] >> 4) + 8) & 0xf;
 644     fg[1] = ((data[1] >> 4) + 8) & 0xf;
 645     fg[2] = ((data[2] >> 4) + 8) & 0xf;
 646
 647     accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
 648     accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
 649     accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));
 650
 651     if (i2c_read( INV6050_addr, 0x06, 6, data))
 652         return -1;
 653
 654     got_accel[0] = ((short)data[0] << 8) | data[1];
 655     got_accel[1] = ((short)data[2] << 8) | data[3];
 656     got_accel[2] = ((short)data[4] << 8) | data[5];
 657
 658     accel_hw[0] += got_accel[0];
 659     accel_hw[1] += got_accel[1];
 660     accel_hw[2] += got_accel[2];
 661
 662     data[0] = (accel_hw[0] >> 8) & 0xff;
 663     data[1] = (accel_hw[0]) & 0xff;
 664     data[2] = (accel_hw[1] >> 8) & 0xff;
 665     data[3] = (accel_hw[1]) & 0xff;
 666     data[4] = (accel_hw[2] >> 8) & 0xff;
 667     data[5] = (accel_hw[2]) & 0xff;
 668
 669 //    if (i2c_write( INV6050_addr, 0x06, 6, data))
 670     if (i2c_writeBlock(  0x06, 6, data))
 671         return -1;
 672     return 0;
 673 }
 674
 675
 676 /**
 677  *  @brief  Reset FIFO read/write pointers.
 678  *  @return 0 if successful.
 679  */
 680 /*
 681 int mpu_reset_fifo(void)
 682 {
 683     unsigned char data;
 684
 685 //    if (!(chip_cfg.sensors))         return -1;                // can be ommitted if whe assume that sensors is always filled
 686
 687 //    data = 0;
 688 //    if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, &data))
 689     if (i2c_writeByte(  GYRO_REG_int_enable, 0))                // disable interrupt
 690         return -1;
 691 //    if (i2c_write( INV6050_addr, GYRO_REG_fifo_en, 1, &data))
 692     if (i2c_writeByte( GYRO_REG_fifo_en, 0))                    // disable all types of data in fifo (avoid that data are loaded in fifo)
 693         return -1;
 694 //    if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &data))
 695     if (i2c_writeByte( GYRO_REG_user_ctrl, 0))                  // disable mpu fifo (bit 0x08) and fifo DMP (bit 0x80)
 696         return -1;
 697
 698     if (chip_cfg.dmp_on) {                                                     // this doe snot seem ok because there is no reset if dmp is off
 699 //        data = BIT_FIFO_RST | BIT_DMP_RST;
 700 //      if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &data))
 701         if (i2c_writeByte(  GYRO_REG_user_ctrl, BIT_FIFO_RST | BIT_DMP_RST ))   // reset the fifo (= clear the data in fifo); There are 2 bits
 702             return -1;
 703         delay(50);                                                              // wait that the reset is performed
 704 //        data = BIT_DMP_EN | BIT_FIFO_EN;
 705 //        if (chip_cfg.sensors & INV_XYZ_COMPASS)
 706 //            data |= BIT_AUX_IF_EN;
 707 //        if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &data))
 708         if (i2c_writeByte(  GYRO_REG_user_ctrl, BIT_DMP_EN | BIT_FIFO_EN))      // enable the fifo ( the 2 types)
 709             return -1;
 710         if (chip_cfg.int_enable)
 711             data = BIT_DMP_INT_EN;
 712         else
 713             data = 0;
 714 //        if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, &data))
 715         if (i2c_writeByte( GYRO_REG_int_enable, data))                          // enable interrupt
 716             return -1;
 717         data = 0;
 718 //        if (i2c_write( INV6050_addr, GYRO_REG_fifo_en, 1, &data)) // strange but this requires less flash
 719         if (i2c_writeBlock( GYRO_REG_fifo_en, 1, &data)) // strange but this requires less flash
 720 //        if (i2c_writeByte(  GYRO_REG_fifo_en, 0) )                            // this is normally not needed because it is already done some steps before
 721             return -1;
 722     } else {
 723 //        data = BIT_FIFO_RST;
 724 //        if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &data))
 725         if (i2c_writeByte(  GYRO_REG_user_ctrl, BIT_FIFO_RST))                  // this force a reset of fifo : from datasheet :This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0.
 726             return -1;                                                          // it is strange that there is no delay for this reset
 727 //        if (chip_cfg.bypass_mode || !(chip_cfg.sensors & INV_XYZ_COMPASS))
 728 //            data = BIT_FIFO_EN;
 729 //        else
 730 //            data = BIT_FIFO_EN | BIT_AUX_IF_EN;
 731 //        if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &data))
 732         if (i2c_writeByte(GYRO_REG_user_ctrl, BIT_FIFO_EN | BIT_AUX_IF_EN))     // this enable fifo but it is strange that Aux is enable and DMP is not enable. This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0
 733             return -1;                                                          //  This is not correct: in fact BIT_AUX_IF_EN = 0x20 = I2CMaster reset
 734         delay(50);                                                              // wait here ; This is buggy
 735         if (chip_cfg.int_enable)
 736             data = BIT_DATA_RDY_EN;
 737         else
 738             data = 0;
 739 //        if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, &data))
 740         if (i2c_writeByte( GYRO_REG_int_enable, data))                         // reactive interrupt if they where active
 741             return -1;
 742 //        if (i2c_write( INV6050_addr, GYRO_REG_fifo_en, 1, &chip_cfg.fifo_enable))    // strange but this line take less flash memory
 743         if (i2c_writeBlock(  GYRO_REG_fifo_en, 1, &chip_cfg.fifo_enable))    // strange but this line take less flash memory
 744 //        if (i2c_writeByte( GYRO_REG_fifo_en, chip_cfg.fifo_enable))        //  enable type of fifo that where enable
 745             return -1;
 746     } // end of handling when dmp is off
 747     return 0;
 748 }
 749 */
 750
 751 int mpu_reset_fifo(void)
 752 {
 753     i2c_writeByte(  GYRO_REG_int_enable, 0) ;               // reg 0x38 ; disable interrupt
 754     i2c_writeByte( GYRO_REG_fifo_en, 0)     ;               // reg 0x23 ; disable all types of data in fifo (avoid that data are loaded in fifo)
 755     i2c_writeByte( GYRO_REG_user_ctrl, 0)   ;               // disable mpu fifo (bit 0x08) and fifo DMP (bit 0x80)
 756     i2c_writeByte(  GYRO_REG_user_ctrl, BIT_FIFO_RST | BIT_DMP_RST ) ;      // reset the fifo (= clear the data in fifo); There are 2 bits
 757     delay(50);                                                              // wait that the reset is performed
 758     i2c_writeByte( GYRO_REG_user_ctrl, BIT_DMP_EN | BIT_FIFO_EN);           // enable the fifo ( the 2 types)
 759     i2c_writeByte( GYRO_REG_int_enable, BIT_DMP_INT_EN ) ;                  // reg 0x38 ; byte = 0X02 ; enable interrupt on DMP ; this bit is not documented
 760     i2c_writeByte( GYRO_REG_fifo_en, 0)     ;               // reg 0x23 ; disable all types of data in fifo (avoid that data are loaded in fifo) // not sure it is required but it was so in original code
 761     return 0;
 762 }
 763
 764
 765 /**
 766  *  @brief      Get the gyro full-scale range.
 767  *  @param[out] fsr Current full-scale range.
 768  *  @return     0 if successful.
 769  */
 770 /*
 771 int mpu_get_gyro_fsr(unsigned short *fsr)
 772 {
 773     switch (chip_cfg.gyro_fsr) {
 774     case INV_FSR_250DPS:
 775         fsr[0] = 250;
 776         break;
 777     case INV_FSR_500DPS:
 778         fsr[0] = 500;
 779         break;
 780     case INV_FSR_1000DPS:
 781         fsr[0] = 1000;
 782         break;
 783     case INV_FSR_2000DPS:
 784         fsr[0] = 2000;
 785         break;
 786     default:
 787         fsr[0] = 0;
 788         break;
 789     }
 790     return 0;
 791 }
 792 */
 793
 794 /**
 795  *  @brief      Set the gyro full-scale range.
 796  *  @param[in]  fsr Desired full-scale range.
 797  *  @return     0 if successful.
 798  */
 799 /*
 800 int mpu_set_gyro_fsr(unsigned short fsr)
 801 {
 802     unsigned char data;
 803
 804     if (!(chip_cfg.sensors))
 805         return -1;
 806
 807     switch (fsr) {
 808     case 250:
 809         data = INV_FSR_250DPS << 3;
 810         break;
 811     case 500:
 812         data = INV_FSR_500DPS << 3;
 813         break;
 814     case 1000:
 815         data = INV_FSR_1000DPS << 3;
 816         break;
 817     case 2000:
 818         data = INV_FSR_2000DPS << 3;
 819         break;
 820     default:
 821         return -1;
 822     }
 823
 824     if (chip_cfg.gyro_fsr == (data >> 3))
 825         return 0;
 826 //    if (i2c_write( INV6050_addr, GYRO_REG_gyro_cfg, 1, &data))
 827     if (i2c_writeByte(  GYRO_REG_gyro_cfg, data))
 828         return -1;
 829     chip_cfg.gyro_fsr = data >> 3;
 830     return 0;
 831 }
 832 */
 833
 834 /**
 835  *  @brief      Get the accel full-scale range.
 836  *  @param[out] fsr Current full-scale range.
 837  *  @return     0 if successful.
 838  */
 839 /*
 840 int mpu_get_accel_fsr(unsigned char *fsr)
 841 {
 842     switch (chip_cfg.accel_fsr) {
 843     case INV_FSR_2G:
 844         fsr[0] = 2;
 845         break;
 846     case INV_FSR_4G:
 847         fsr[0] = 4;
 848         break;
 849     case INV_FSR_8G:
 850         fsr[0] = 8;
 851         break;
 852     case INV_FSR_16G:
 853         fsr[0] = 16;
 854         break;
 855     default:
 856         return -1;
 857     }
 858     if (chip_cfg.accel_half)
 859         fsr[0] <<= 1;
 860     return 0;
 861 }
 862 */
 863
 864 /**
 865  *  @brief      Set the accel full-scale range.
 866  *  @param[in]  fsr Desired full-scale range.
 867  *  @return     0 if successful.
 868  */
 869  /*
 870 int mpu_set_accel_fsr(unsigned char fsr)
 871 {
 872     unsigned char data;
 873
 874 //    if (!(chip_cfg.sensors)) return -1;   // can be ommitted is we assume that sensor is always filled
 875
 876     switch (fsr) {
 877     case 2:
 878         data = INV_FSR_2G << 3;
 879         break;
 880     case 4:
 881         data = INV_FSR_4G << 3;
 882         break;
 883     case 8:
 884         data = INV_FSR_8G << 3;
 885         break;
 886     case 16:
 887         data = INV_FSR_16G << 3;
 888         break;
 889     default:
 890         return -1;
 891     }
 892
 893     if (chip_cfg.accel_fsr == (data >> 3))
 894         return 0;
 895 //    if (i2c_write( INV6050_addr, GYRO_REG_accel_cfg, 1, &data))
 896     if (i2c_writeByte( GYRO_REG_accel_cfg, data))
 897         return -1;
 898     chip_cfg.accel_fsr = data >> 3;
 899     return 0;
 900 }
 901 */
 902
 903
 904 /**
 905  *  @brief      Get the current DLPF setting.
 906  *  @param[out] lpf Current LPF setting.
 907  *  0 if successful.
 908  */
 909  /*
 910 int mpu_get_lpf(unsigned short *lpf)
 911 {
 912     switch (chip_cfg.lpf) {
 913     case INV_FILTER_188HZ:
 914         lpf[0] = 188;
 915         break;
 916     case INV_FILTER_98HZ:
 917         lpf[0] = 98;
 918         break;
 919     case INV_FILTER_42HZ:
 920         lpf[0] = 42;
 921         break;
 922     case INV_FILTER_20HZ:
 923         lpf[0] = 20;
 924         break;
 925     case INV_FILTER_10HZ:
 926         lpf[0] = 10;
 927         break;
 928     case INV_FILTER_5HZ:
 929         lpf[0] = 5;
 930         break;
 931     case INV_FILTER_256HZ_NOLPF2:
 932     case INV_FILTER_2100HZ_NOLPF:
 933     default:
 934         lpf[0] = 0;
 935         break;
 936     }
 937     return 0;
 938 }
 939 */
 940
 941 /**
 942  *  @brief      Set digital low pass filter.
 943  *  The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
 944  *  @param[in]  lpf Desired LPF setting.
 945  *  @return     0 if successful.
 946  */
 947  /*
 948 int mpu_set_lpf(unsigned short lpf)
 949 {
 950     unsigned char data;
 951
 952 //    if (!(chip_cfg.sensors)) return -1;      // can be ommitted if we assume that sensors is always filled
 953
 954     if (lpf >= 188)
 955         data = INV_FILTER_188HZ;
 956     else if (lpf >= 98)
 957         data = INV_FILTER_98HZ;
 958     else if (lpf >= 42)
 959         data = INV_FILTER_42HZ;
 960     else if (lpf >= 20)
 961         data = INV_FILTER_20HZ;
 962     else if (lpf >= 10)
 963         data = INV_FILTER_10HZ;
 964     else
 965         data = INV_FILTER_5HZ;
 966
 967     if (chip_cfg.lpf == data)
 968         return 0;
 969 //    if (i2c_write( INV6050_addr, GYRO_REG_lpf, 1, &data))
 970     if (i2c_writeByte( GYRO_REG_lpf, data))
 971         return -1;
 972     chip_cfg.lpf = data;
 973     return 0;
 974 }
 975 */
 976
 977
 978 /**
 979  *  @brief      Get sampling rate.
 980  *  @param[out] rate    Current sampling rate (Hz).
 981  *  @return     0 if successful.
 982  */
 983 /*
 984 int mpu_get_sample_rate(unsigned short *rate)
 985 {
 986     if (chip_cfg.dmp_on)
 987         return -1;
 988     else
 989         rate[0] = chip_cfg.sample_rate;
 990     return 0;
 991 }
 992 */
 993
 994 /**
 995  *  @brief      Set sampling rate.
 996  *  Sampling rate must be between 4Hz and 1kHz.
 997  *  @param[in]  rate    Desired sampling rate (Hz).
 998  *  @return     0 if successful.
 999  */
1000 /*
1001 int mpu_set_sample_rate(unsigned short rate)
1002 {
1003     unsigned char data;
1004
1005 //    if (!(chip_cfg.sensors))  return -1;    // can be omitted if we assume that sensor is always filled
1006
1007     if (chip_cfg.dmp_on)
1008         return -1;
1009     else {
1010         if (chip_cfg.lp_accel_mode) {
1011             if (rate && (rate <= 40)) {
1012                 // Just stay in low-power accel mode.
1013                 mpu_lp_accel_mode(rate);
1014                 return 0;
1015             }
1016             // Requested rate exceeds the allowed frequencies in LP accel mode,
1017             // switch back to full-power mode.
1018
1019             mpu_lp_accel_mode(0);
1020         }
1021         if (rate < 4)
1022             rate = 4;
1023         else if (rate > 1000)
1024             rate = 1000;
1025
1026         data = 1000 / rate - 1;
1027 //        if (i2c_write( INV6050_addr, GYRO_REG_rate_div, 1, &data))
1028         if (i2c_writeByte( GYRO_REG_rate_div, data))
1029             return -1;
1030
1031         chip_cfg.sample_rate = 1000 / (1 + data);
1032
1033         // Automatically set LPF to 1/2 sampling rate.
1034         mpu_set_lpf(chip_cfg.sample_rate >> 1);
1035         return 0;
1036     }
1037 }
1038 */
1039 int mpu_set_sample_rate200hz()
1040 {
1041 //    unsigned char data;
1042 //        data = 4 ; // 1000 / rate - 1 ; //so 4 when rate = 200 hz
1043 //        if (i2c_write( INV6050_addr, GYRO_REG_rate_div, 1, &data))
1044         if (i2c_writeByte( GYRO_REG_rate_div, 4 ))
1045             return -1;
1046
1047 //        chip_cfg.sample_rate =  200 ; //1000 / (1 + data);
1048
1049         // set LPF to 1/2 sampling rate.
1050 //        mpu_set_lpf(100);
1051         if (i2c_writeByte( GYRO_REG_lpf, INV_FILTER_98HZ ))        return -1;
1052         return 0;
1053 }
1054
1055
1056
1057 /**
1058  *  @brief      Get gyro sensitivity scale factor.
1059  *  @param[out] sens    Conversion from hardware units to dps.
1060  *  @return     0 if successful.
1061  */
1062 /*
1063 int mpu_get_gyro_sens(float *sens)
1064 {
1065     switch (chip_cfg.gyro_fsr) {
1066     case INV_FSR_250DPS:
1067         sens[0] = 131.f;
1068         break;
1069     case INV_FSR_500DPS:
1070         sens[0] = 65.5f;
1071         break;
1072     case INV_FSR_1000DPS:
1073         sens[0] = 32.8f;
1074         break;
1075     case INV_FSR_2000DPS:
1076         sens[0] = 16.4f;
1077         break;
1078     default:
1079         return -1;
1080     }
1081     return 0;
1082 }
1083 */
1084
1085
1086 /**
1087  *  @brief      Get accel sensitivity scale factor.
1088  *  @param[out] sens    Conversion from hardware units to g's.
1089  *  @return     0 if successful.
1090  */
1091 /*
1092 int mpu_get_accel_sens(unsigned short *sens)
1093 {
1094     switch (chip_cfg.accel_fsr) {
1095     case INV_FSR_2G:
1096         sens[0] = 16384;
1097         break;
1098     case INV_FSR_4G:
1099         sens[0] = 8092;
1100         break;
1101     case INV_FSR_8G:
1102         sens[0] = 4096;
1103         break;
1104     case INV_FSR_16G:
1105         sens[0] = 2048;
1106         break;
1107     default:
1108         return -1;
1109     }
1110     if (chip_cfg.accel_half)
1111         sens[0] >>= 1;
1112     return 0;
1113 }
1114 */
1115 /**
1116  *  @brief      Get current FIFO configuration.
1117  *  @e sensors can contain a combination of the following flags:
1118  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
1119  *  \n INV_XYZ_GYRO
1120  *  \n INV_XYZ_ACCEL
1121  *  @param[out] sensors Mask of sensors in FIFO.
1122  *  @return     0 if successful.
1123  */
1124 /*
1125 int mpu_get_fifo_configxx(unsigned char *sensors)
1126 {
1127     sensors[0] = chip_cfg.fifo_enable;
1128     return 0;
1129 }
1130 */
1131
1132 /**
1133  *  @brief      Select which sensors are pushed to FIFO.          // not used by oxs because the fifo is set up by dmp feature
1134  *  @e sensors can contain a combination of the following flags:
1135  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
1136  *  \n INV_XYZ_GYRO
1137  *  \n INV_XYZ_ACCEL
1138  *  @param[in]  sensors Mask of sensors to push to FIFO.
1139  *  @return     0 if successful.
1140  */
1141 /*
1142 int mpu_configure_fifo(unsigned char sensors)
1143 {
1144     unsigned char prev;
1145     int result = 0;
1146
1147     // Compass data isn't going into the FIFO. Stop trying.
1148 //    sensors &= ~INV_XYZ_COMPASS;
1149
1150     if (chip_cfg.dmp_on)
1151         return 0;
1152     else {
1153 ///        if (!(chip_cfg.sensors))
1154 //            return -1;
1155         prev = chip_cfg.fifo_enable;
1156         chip_cfg.fifo_enable = sensors & chip_cfg.sensors;
1157         if (chip_cfg.fifo_enable != sensors)
1158             // You're not getting what you asked for. Some sensors are
1159              // asleep.
1160
1161             result = -1;
1162         else
1163             result = 0;
1164         if (sensors || chip_cfg.lp_accel_mode)
1165             set_int_enable(1);
1166         else
1167             set_int_enable(0);
1168         if (sensors) {
1169             if (mpu_reset_fifo()) {
1170                 chip_cfg.fifo_enable = prev;
1171                 return -1;
1172             }
1173         }
1174     }
1175
1176     return result;
1177 }
1178 */
1179
1180 /**
1181  *  @brief      Get current power state.
1182  *  @param[in]  power_on    1 if turned on, 0 if suspended.
1183  *  @return     0 if successful.
1184  */
1185 /*
1186 int mpu_get_power_state(unsigned char *power_on)
1187 {
1188     if (chip_cfg.sensors)
1189         power_on[0] = 1;
1190     else
1191         power_on[0] = 0;
1192     return 0;
1193 }
1194 */
1195
1196 /**
1197  *  @brief      Turn specific sensors on/off.
1198  *  @e sensors can contain a combination of the following flags:
1199  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
1200  *  \n INV_XYZ_GYRO
1201  *  \n INV_XYZ_ACCEL
1202  *  \n INV_XYZ_COMPASS
1203  *  @param[in]  sensors    Mask of sensors to wake.
1204  *  @return     0 if successful.
1205  */
1206 /*
1207 int mpu_set_sensors(unsigned char sensors)
1208 {
1209     unsigned char data;
1210
1211     if (sensors & INV_XYZ_GYRO)
1212         data = INV_CLK_PLL;
1213     else if (sensors)
1214         data = 0;
1215     else
1216         data = BIT_SLEEP;
1217 //    if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 1, &data)) {
1218     if (i2c_writeByte( GYRO_REG_pwr_mgmt_1, data)) {
1219         chip_cfg.sensors = 0;
1220         return -1;
1221     }
1222 //    chip_cfg.clk_src = data & ~BIT_SLEEP;
1223
1224     data = 0;
1225     if (!(sensors & INV_X_GYRO))
1226         data |= BIT_STBY_XG;
1227     if (!(sensors & INV_Y_GYRO))
1228         data |= BIT_STBY_YG;
1229     if (!(sensors & INV_Z_GYRO))
1230         data |= BIT_STBY_ZG;
1231     if (!(sensors & INV_XYZ_ACCEL))
1232         data |= BIT_STBY_XYZA;
1233 //    if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_2, 1, &data)) {
1234     if (i2c_writeByte( GYRO_REG_pwr_mgmt_2, data)) {
1235         chip_cfg.sensors = 0;
1236         return -1;
1237     }
1238 //    if (sensors && (sensors != INV_XYZ_ACCEL))         // Latched interrupts only used in LP accel mode.
1239 //        mpu_set_int_latched(0);
1240
1241     chip_cfg.sensors = sensors;
1242   //  chip_cfg.lp_accel_mode = 0;
1243     delay(50);
1244     return 0;
1245 }
1246 */
1247
1248 void mpu_enable_pwm_mgnt() {
1249   i2c_writeByte( GYRO_REG_pwr_mgmt_1, INV_CLK_PLL ) ;  // define the clock to be used
1250   i2c_writeByte( GYRO_REG_pwr_mgmt_2, 0 ) ; //
1251   delay(50);
1252   i2c_writeByte( GYRO_REG_int_pin_cfg,BIT_ACTL | BIT_BYPASS_EN ) ;       // in reg 0x37 ,set interrupt active low and allow to let MPU bypass I2C to HMC5883 ( requires also I2C_MST-EN bit = 0 in reg 6A and SLEEP = 0 in PWR_MNGT_1 )
1253 //  chip_cfg.sensors = (INV_XYZ_GYRO | INV_XYZ_ACCEL ) ;
1254 }
1255 /**
1256  *  @brief      Read the MPU interrupt status registers.
1257  *  @param[out] status  Mask of interrupt bits.
1258  *  @return     0 if successful.
1259  */
1260  /*
1261 int mpu_get_int_status(short *status)
1262 {
1263     unsigned char tmp[2];
1264 //    if (!chip_cfg.sensors)
1265 //        return -1;
1266     if (i2c_read( INV6050_addr, GYRO_REG_dmp_int_status, 2, tmp))
1267         return -1;
1268     status[0] = (tmp[0] << 8) | tmp[1];
1269     return 0;
1270 }
1271 */
1272
1273 /**
1274  *  @brief      Get one packet from the FIFO.
1275  *  If @e sensors does not contain a particular sensor, disregard the data
1276  *  returned to that pointer.
1277  *  \n @e sensors can contain a combination of the following flags:
1278  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
1279  *  \n INV_XYZ_GYRO
1280  *  \n INV_XYZ_ACCEL
1281  *  \n If the FIFO has no new data, @e sensors will be zero.
1282  *  \n If the FIFO is disabled, @e sensors will be zero and this function will
1283  *  return a non-zero error code.
1284  *  @param[out] gyro        Gyro data in hardware units.
1285  *  @param[out] accel       Accel data in hardware units.
1286  *  @param[out] timestamp   Timestamp in milliseconds.
1287  *  @param[out] sensors     Mask of sensors read from FIFO.
1288  *  @param[out] more        Number of remaining packets.
1289  *  @return     0 if successful.  Negative if error:  -1:  Misconfigured; -2:  I2C read error; -3:  Fifo Overflow
1290  */
1291 /*
1292 int mpu_read_fifo(short *gyro, short *accel, unsigned char *sensors, unsigned char *more)
1293 {
1294     // Assumes maximum packet size is gyro (6) + accel (6).
1295     unsigned char data[MAX_PACKET_LENGTH];
1296     unsigned char packet_size = 0;
1297     unsigned short fifo_count, index = 0;
1298
1299     if (chip_cfg.dmp_on)
1300         return -1;
1301
1302     sensors[0] = 0;
1303 //    if (!chip_cfg.sensors)
1304 //        return -5;
1305 //    if (!chip_cfg.fifo_enable)
1306 //        return -6;
1307
1308     if (chip_cfg.fifo_enable & INV_X_GYRO)
1309         packet_size += 2;
1310     if (chip_cfg.fifo_enable & INV_Y_GYRO)
1311         packet_size += 2;
1312     if (chip_cfg.fifo_enable & INV_Z_GYRO)
1313         packet_size += 2;
1314     if (chip_cfg.fifo_enable & INV_XYZ_ACCEL)
1315         packet_size += 6;
1316
1317     if (i2c_read( INV6050_addr, GYRO_REG_fifo_count_h, 2, data))
1318         return -2;
1319     fifo_count = (data[0] << 8) | data[1];
1320     if (fifo_count < packet_size)
1321         return 0;
1322 //    log_i("FIFO count: %hd\n", fifo_count);
1323     if (fifo_count > (INV6050_max_fifo >> 1)) {
1324         // FIFO is 50% full, better check overflow bit.
1325         if (i2c_read( INV6050_addr, GYRO_REG_int_status, 1, data))
1326             return -2;
1327         if (data[0] & BIT_FIFO_OVERFLOW) {
1328             mpu_reset_fifo();
1329             return -3;
1330         }
1331     }
1332 //    get_ms((unsigned long*)timestamp); // set as comment to save flash memory
1333
1334     if (i2c_read( INV6050_addr, GYRO_REG_fifo_r_w, packet_size, data))
1335         return -2;
1336     more[0] = fifo_count / packet_size - 1;
1337     sensors[0] = 0;
1338
1339     if ((index != packet_size) && chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
1340         accel[0] = (data[index+0] << 8) | data[index+1];
1341         accel[1] = (data[index+2] << 8) | data[index+3];
1342         accel[2] = (data[index+4] << 8) | data[index+5];
1343         sensors[0] |= INV_XYZ_ACCEL;
1344         index += 6;
1345     }
1346     if ((index != packet_size) && chip_cfg.fifo_enable & INV_X_GYRO) {
1347         gyro[0] = (data[index+0] << 8) | data[index+1];
1348         sensors[0] |= INV_X_GYRO;
1349         index += 2;
1350     }
1351     if ((index != packet_size) && chip_cfg.fifo_enable & INV_Y_GYRO) {
1352         gyro[1] = (data[index+0] << 8) | data[index+1];
1353         sensors[0] |= INV_Y_GYRO;
1354         index += 2;
1355     }
1356     if ((index != packet_size) && chip_cfg.fifo_enable & INV_Z_GYRO) {
1357         gyro[2] = (data[index+0] << 8) | data[index+1];
1358         sensors[0] |= INV_Z_GYRO;
1359         index += 2;
1360     }
1361
1362     return 0;
1363 }
1364 */
1365
1366 /**
1367  *  @brief      Get one unparsed packet from the FIFO.
1368  *  This function should be used if the packet is to be parsed elsewhere.
1369  *  @param[in]  length  Length of one FIFO packet.
1370  *  @param[in]  data    FIFO packet.
1371  *  @param[in]  more    Number of remaining packets.
1372  *  @return     0 if successful.  Negative if error:  -1:  DMP Not On; -2:  I2C read error; -3:  Fifo Overflow -4: No Sensors -5: No more data available
1373 */
1374
1375 #ifdef DEBUG_READ_FIFO
1376     unsigned short fifo_count_debug ;
1377     unsigned short mpu_getfifo_count_debug(){
1378       return fifo_count_debug;
1379     }
1380 #endif
1381
1382 int mpu_read_fifo_stream(unsigned short length, unsigned char *data,  unsigned char *more)
1383 {
1384     unsigned char tmp[2];
1385     unsigned short fifo_count;
1386 //    if (!chip_cfg.dmp_on)    // error if dmp is not ON
1387 //        return -1;
1388 //    if (!chip_cfg.sensors)
1389 //        return -4;              // 2 = there is no sensor defined to be active( or to be read?)
1390
1391     if (i2c_read( INV6050_addr, GYRO_REG_fifo_count_h, 2, tmp))
1392         return -2;             //  I2C error reading the 6050
1393     fifo_count = (tmp[0] << 8) | tmp[1];
1394 #ifdef DEBUG_READ_FIFO
1395     fifo_count_debug = fifo_count;
1396 #endif
1397     if (fifo_count < length) {
1398         more[0] = 0;
1399         return -5;             //  fifo does not contains the expected number of char.
1400     }
1401     if (fifo_count > (INV6050_max_fifo >> 1)) {
1402         /* FIFO is 50% full, better check overflow bit. */
1403         if (i2c_read( INV6050_addr, GYRO_REG_int_status, 1, tmp))
1404             return -2;         // i2c error reading the fifo
1405         if (tmp[0] & BIT_FIFO_OVERFLOW) {
1406             mpu_reset_fifo();
1407             return -3;        // fifo inside 6050 has lost some data.
1408         }
1409     }
1410
1411     if (i2c_read( INV6050_addr, GYRO_REG_fifo_r_w, length, data))
1412         return -2;
1413     more[0] = fifo_count / length - 1;
1414     return 0;
1415 }
1416
1417 /**
1418  *  @brief      Set device to bypass mode.
1419  *  @param[in]  bypass_on   1 to enable bypass mode.
1420  *  @return     0 if successful.
1421  */
1422 /*
1423 int mpu_set_bypass(unsigned char bypass_on)
1424 {
1425     unsigned char tmp;
1426
1427     if (chip_cfg.bypass_mode == bypass_on)
1428         return 0;
1429
1430     if (bypass_on) {
1431         if (i2c_read( INV6050_addr, GYRO_REG_user_ctrl, 1, &tmp))
1432             return -1;
1433         tmp &= ~BIT_AUX_IF_EN;
1434 //        if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &tmp))
1435         if (i2c_writeByte( GYRO_REG_user_ctrl, tmp))
1436             return -1;
1437         delay(3);
1438         tmp = BIT_BYPASS_EN;
1439         if (chip_cfg.active_low_int)
1440             tmp |= BIT_ACTL;
1441         if (chip_cfg.latched_int)
1442             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
1443 //        if (i2c_write( INV6050_addr, GYRO_REG_int_pin_cfg, 1, &tmp))        // strange but this line requires less flash memory
1444         if (i2c_writeBlock( GYRO_REG_int_pin_cfg, 1, &tmp))        // strange but this line requires less flash memory
1445 //        if (i2c_writeByte( GYRO_REG_int_pin_cfg, tmp))
1446             return -1;
1447     } else {
1448         // Enable I2C master mode if compass is being used.
1449         if (i2c_read( INV6050_addr, GYRO_REG_user_ctrl, 1, &tmp))
1450             return -1;
1451         if (chip_cfg.sensors & INV_XYZ_COMPASS)
1452             tmp |= BIT_AUX_IF_EN;
1453         else
1454             tmp &= ~BIT_AUX_IF_EN;
1455 //        if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, &tmp))
1456         if (i2c_writeByte(  GYRO_REG_user_ctrl, tmp))
1457             return -1;
1458         delay(3);
1459         if (chip_cfg.active_low_int)
1460             tmp = BIT_ACTL;
1461         else
1462             tmp = 0;
1463         if (chip_cfg.latched_int)
1464             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
1465 //        if (i2c_write( INV6050_addr, GYRO_REG_int_pin_cfg, 1, &tmp))         // strange but this line requires less flash memory
1466         if (i2c_writeBlock(  GYRO_REG_int_pin_cfg, 1, &tmp))         // strange but this line requires less flash memory
1467 //        if (i2c_writeByte( GYRO_REG_int_pin_cfg, tmp))
1468             return -1;
1469     }
1470     chip_cfg.bypass_mode = bypass_on;
1471     return 0;
1472 }
1473 */
1474
1475 /**
1476  *  @brief      Set interrupt level.
1477  *  @param[in]  active_low  1 for active low, 0 for active high.
1478  *  @return     0 if successful.
1479  */
1480 /*
1481 int mpu_set_int_level(unsigned char active_low)
1482 {
1483     chip_cfg.active_low_int = active_low;
1484     return 0;
1485 }
1486 */
1487 /**
1488  *  @brief      Enable latched interrupts.
1489  *  Any MPU register will clear the interrupt.
1490  *  @param[in]  enable  1 to enable, 0 to disable.
1491  *  @return     0 if successful.
1492  */
1493  /*
1494 int mpu_set_int_latched(unsigned char enable)
1495 {
1496     unsigned char tmp;
1497     if (chip_cfg.latched_int == enable)
1498         return 0;
1499
1500     if (enable)
1501         tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
1502     else
1503         tmp = 0;
1504     if (chip_cfg.bypass_mode)
1505         tmp |= BIT_BYPASS_EN;
1506     if (chip_cfg.active_low_int)
1507         tmp |= BIT_ACTL;
1508 //    if (i2c_write( INV6050_addr, GYRO_REG_int_pin_cfg, 1, &tmp))
1509     if (i2c_writeByte( GYRO_REG_int_pin_cfg,tmp))
1510         return -1;
1511     chip_cfg.latched_int = enable;
1512     return 0;
1513 }
1514 */
1515 /*
1516 static int get_accel_prod_shift(float *st_shift)                          // this function is not used by oXs;
1517 {
1518     unsigned char tmp[4], shift_code[3], ii;
1519
1520     if (i2c_read( INV6050_addr, 0x0D, 4, tmp))
1521         return 0x07;
1522
1523     shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
1524     shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
1525     shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
1526     for (ii = 0; ii < 3; ii++) {
1527         if (!shift_code[ii]) {
1528             st_shift[ii] = 0.f;
1529             continue;
1530         }
1531         // Equivalent to..
1532         // st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
1533         //
1534         st_shift[ii] = 0.34f;
1535         while (--shift_code[ii])
1536             st_shift[ii] *= 1.034f;
1537     }
1538     return 0;
1539 }
1540 */
1541
1542 /*
1543 static int accel_self_test(long *bias_regular, long *bias_st)
1544 {
1545     int jj, result = 0;
1546     float st_shift[3], st_shift_cust, st_shift_var;
1547
1548     get_accel_prod_shift(st_shift);
1549     for(jj = 0; jj < 3; jj++) {
1550         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
1551         if (st_shift[jj]) {
1552             st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
1553             if (fabs(st_shift_var) > TEST_max_accel_var)
1554                 result |= 1 << jj;
1555         } else if ((st_shift_cust < TEST_min_g) ||
1556             (st_shift_cust > TEST_max_g))
1557             result |= 1 << jj;
1558     }
1559
1560     return result;
1561 }
1562 */
1563 /*
1564 static int gyro_self_test(long *bias_regular, long *bias_st)
1565 {
1566     int jj, result = 0;
1567     unsigned char tmp[3];
1568     float st_shift, st_shift_cust, st_shift_var;
1569
1570     if (i2c_read( INV6050_addr, 0x0D, 3, tmp))
1571         return 0x07;
1572
1573     tmp[0] &= 0x1F;
1574     tmp[1] &= 0x1F;
1575     tmp[2] &= 0x1F;
1576
1577     for (jj = 0; jj < 3; jj++) {
1578         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
1579         if (tmp[jj]) {
1580             st_shift = 3275.f / TEST_gyro_sens;
1581             while (--tmp[jj])
1582                 st_shift *= 1.046f;
1583             st_shift_var = st_shift_cust / st_shift - 1.f;
1584             if (fabs(st_shift_var) > TEST_max_gyro_var)
1585                 result |= 1 << jj;
1586         } else if ((st_shift_cust < TEST_min_dps) ||
1587             (st_shift_cust > TEST_max_dps))
1588             result |= 1 << jj;
1589     }
1590     return result;
1591 }
1592 */
1593 /*
1594 //static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)               // this function is not used by oXs
1595 int get_st_biases(long *gyro, long *accel, unsigned char hw_test)                        // modified in non static by MS
1596 {
1597     unsigned char data[MAX_PACKET_LENGTH];
1598     unsigned char packet_count, ii;
1599     unsigned short fifo_count;
1600
1601     data[0] = 0x01;
1602     data[1] = 0;
1603 //    if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 2, data))
1604     if (i2c_writeBlock( GYRO_REG_pwr_mgmt_1, 2, data))
1605         return -1;
1606     delay(200);
1607     data[0] = 0;
1608 //    if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, data))
1609     if (i2c_writeBlock( GYRO_REG_int_enable, 1, data))
1610         return -1;
1611 //    if (i2c_write( INV6050_addr, GYRO_REG_fifo_en, 1, data))
1612     if (i2c_writeBlock( GYRO_REG_fifo_en, 1, data))
1613         return -1;
1614 //    if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 1, data))
1615     if (i2c_writeBlock( GYRO_REG_pwr_mgmt_1, 1, data))
1616         return -1;
1617 //    if (i2c_write( INV6050_addr, GYRO_REG_i2c_mst, 1, data))
1618     if (i2c_writeBlock( GYRO_REG_i2c_mst, 1, data))
1619         return -1;
1620 //    if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, data))
1621     if (i2c_writeBlock( GYRO_REG_user_ctrl, 1, data))
1622         return -1;
1623     data[0] = BIT_FIFO_RST | BIT_DMP_RST;
1624 //    if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, data))
1625     if (i2c_writeBlock(GYRO_REG_user_ctrl, 1, data))
1626         return -1;
1627     delay(15);
1628     data[0] = TEST_reg_lpf;
1629 //    if (i2c_write( INV6050_addr, GYRO_REG_lpf, 1, data))
1630     if (i2c_writeBlock( GYRO_REG_lpf, 1, data))
1631         return -1;
1632     data[0] = TEST_reg_rate_div;
1633 //    if (i2c_write( INV6050_addr, GYRO_REG_rate_div, 1, data))
1634     if (i2c_writeBlock(  GYRO_REG_rate_div, 1, data))
1635         return -1;
1636     if (hw_test)
1637         data[0] = TEST_reg_gyro_fsr | 0xE0;
1638     else
1639         data[0] = TEST_reg_gyro_fsr;
1640 //    if (i2c_write( INV6050_addr, GYRO_REG_gyro_cfg, 1, data))
1641     if (i2c_writeBlock(  GYRO_REG_gyro_cfg, 1, data))
1642         return -1;
1643
1644     if (hw_test)
1645         data[0] = TEST_reg_accel_fsr | 0xE0;
1646     else
1647         data[0] = TEST_reg_accel_fsr;
1648 //    if (i2c_write( INV6050_addr, GYRO_REG_accel_cfg, 1, data))
1649     if (i2c_writeBlock(  GYRO_REG_accel_cfg, 1, data))
1650         return -1;
1651     if (hw_test)
1652         delay(200);
1653
1654     // Fill FIFO for TEST_wait_ms milliseconds.
1655     data[0] = BIT_FIFO_EN;
1656 //    if (i2c_write( INV6050_addr, GYRO_REG_user_ctrl, 1, data))
1657     if (i2c_writeBlock( GYRO_REG_user_ctrl, 1, data))
1658         return -1;
1659
1660     data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
1661 //    if (i2c_write( INV6050_addr, GYRO_REG_fifo_en, 1, data))
1662     if (i2c_writeBlock( GYRO_REG_fifo_en, 1, data))
1663         return -1;
1664     delay(TEST_wait_ms);
1665     data[0] = 0;
1666 //    if (i2c_write( INV6050_addr, GYRO_REG_fifo_en, 1, data))
1667     if (i2c_writeBlock( GYRO_REG_fifo_en, 1, data))
1668         return -1;
1669
1670     if (i2c_read( INV6050_addr, GYRO_REG_fifo_count_h, 2, data))
1671         return -1;
1672
1673     fifo_count = (data[0] << 8) | data[1];
1674     packet_count = fifo_count / MAX_PACKET_LENGTH;
1675     gyro[0] = gyro[1] = gyro[2] = 0;
1676     accel[0] = accel[1] = accel[2] = 0;
1677
1678     for (ii = 0; ii < packet_count; ii++) {
1679         short accel_cur[3], gyro_cur[3];
1680         if (i2c_read( INV6050_addr, GYRO_REG_fifo_r_w, MAX_PACKET_LENGTH, data))
1681             return -1;
1682         accel_cur[0] = ((short)data[0] << 8) | data[1];
1683         accel_cur[1] = ((short)data[2] << 8) | data[3];
1684         accel_cur[2] = ((short)data[4] << 8) | data[5];
1685         accel[0] += (long)accel_cur[0];
1686         accel[1] += (long)accel_cur[1];
1687         accel[2] += (long)accel_cur[2];
1688         gyro_cur[0] = (((short)data[6] << 8) | data[7]);
1689         gyro_cur[1] = (((short)data[8] << 8) | data[9]);
1690         gyro_cur[2] = (((short)data[10] << 8) | data[11]);
1691         gyro[0] += (long)gyro_cur[0];
1692         gyro[1] += (long)gyro_cur[1];
1693         gyro[2] += (long)gyro_cur[2];
1694     }
1695 #ifdef EMPL_NO_64BIT
1696     gyro[0] = (long)(((float)gyro[0]*65536.f) / TEST_gyro_sens / packet_count);
1697     gyro[1] = (long)(((float)gyro[1]*65536.f) / TEST_gyro_sens / packet_count);
1698     gyro[2] = (long)(((float)gyro[2]*65536.f) / TEST_gyro_sens / packet_count);
1699     if (has_accel) {
1700         accel[0] = (long)(((float)accel[0]*65536.f) / TEST_accel_sens /
1701             packet_count);
1702         accel[1] = (long)(((float)accel[1]*65536.f) / TEST_accel_sens /
1703             packet_count);
1704         accel[2] = (long)(((float)accel[2]*65536.f) / TEST_accel_sens /
1705             packet_count);
1706         // Don't remove gravity!
1707         accel[2] -= 65536L;
1708     }
1709 #else
1710     gyro[0] = (long)(((long long)gyro[0]<<16) / TEST_gyro_sens / packet_count);
1711     gyro[1] = (long)(((long long)gyro[1]<<16) / TEST_gyro_sens / packet_count);
1712     gyro[2] = (long)(((long long)gyro[2]<<16) / TEST_gyro_sens / packet_count);
1713     accel[0] = (long)(((long long)accel[0]<<16) / TEST_accel_sens /
1714         packet_count);
1715     accel[1] = (long)(((long long)accel[1]<<16) / TEST_accel_sens /
1716         packet_count);
1717     accel[2] = (long)(((long long)accel[2]<<16) / TEST_accel_sens /
1718         packet_count);
1719     // Don't remove gravity!
1720     if (accel[2] > 0L)
1721         accel[2] -= 65536L;
1722     else
1723         accel[2] += 65536L;
1724 #endif
1725
1726     return 0;
1727 }
1728 */
1729
1730 /**
1731  *  @brief      Trigger gyro/accel/compass self-test.                               // this function is normally not used by oXs (so no flash is used)
1732  *  On success/error, the self-test returns a mask representing the sensor(s)
1733  *  that failed. For each bit, a one (1) represents a "pass" case; conversely,
1734  *  a zero (0) indicates a failure.
1735  *
1736  *  \n The mask is defined as follows:
1737  *  \n Bit 0:   Gyro.
1738  *  \n Bit 1:   Accel.
1739  *  \n Bit 2:   Compass.
1740  *
1741  *  \n Currently, the hardware self-test is unsupported for MPU6500. However,
1742  *  this function can still be used to obtain the accel and gyro biases.
1743  *
1744  *  \n This function must be called with the device either face-up or face-down
1745  *  (z-axis is parallel to gravity).
1746  *  @param[out] gyro        Gyro biases in q16 format.
1747  *  @param[out] accel       Accel biases (if applicable) in q16 format.
1748  *  @return     Result mask (see above).
1749  */
1750 /*
1751 int mpu_run_self_test(long *gyro, long *accel)
1752 {
1753     const unsigned char tries = 2;
1754     long gyro_st[3], accel_st[3];
1755     unsigned char accel_result, gyro_result;
1756     int ii;
1757     int result;
1758     unsigned char accel_fsr, fifo_sensors, sensors_on;
1759     unsigned short gyro_fsr, sample_rate, lpf;
1760     unsigned char dmp_was_on;
1761
1762     if (chip_cfg.dmp_on) {
1763         mpu_set_dmp_state(0);
1764         dmp_was_on = 1;
1765     } else
1766         dmp_was_on = 0;
1767
1768     // Get initial settings.
1769     mpu_get_gyro_fsr(&gyro_fsr);
1770     mpu_get_accel_fsr(&accel_fsr);
1771     mpu_get_lpf(&lpf);
1772     mpu_get_sample_rate(&sample_rate);
1773     sensors_on = chip_cfg.sensors;
1774     mpu_get_fifo_config(&fifo_sensors);
1775
1776     // For older chips, the self-test will be different.
1777     for (ii = 0; ii < tries; ii++)
1778         if (!get_st_biases(gyro, accel, 0))
1779             break;
1780     if (ii == tries) {
1781         // If we reach this point, we most likely encountered an I2C error.
1782         // We'll just report an error for all three sensors.
1783
1784         result = 0;
1785         goto restore;
1786     }
1787     for (ii = 0; ii < tries; ii++)
1788         if (!get_st_biases(gyro_st, accel_st, 1))
1789             break;
1790     if (ii == tries) {
1791         // Again, probably an I2C error.
1792         result = 0;
1793         goto restore;
1794     }
1795     accel_result = accel_self_test(accel, accel_st);
1796     gyro_result = gyro_self_test(gyro, gyro_st);
1797
1798     result = 0;
1799     if (!gyro_result)
1800         result |= 0x01;
1801     if (!accel_result)
1802         result |= 0x02;
1803
1804 restore:
1805     // Set to invalid values to ensure no I2C writes are skipped.
1806     chip_cfg.gyro_fsr = 0xFF;
1807     chip_cfg.accel_fsr = 0xFF;
1808     chip_cfg.lpf = 0xFF;
1809     chip_cfg.sample_rate = 0xFFFF;
1810     chip_cfg.sensors = 0xFF;
1811     chip_cfg.fifo_enable = 0xFF;
1812     chip_cfg.clk_src = INV_CLK_PLL;
1813     mpu_set_gyro_fsr(gyro_fsr);
1814     mpu_set_accel_fsr(accel_fsr);
1815     mpu_set_lpf(lpf);
1816     mpu_set_sample_rate(sample_rate);
1817     mpu_set_sensors(sensors_on);
1818     mpu_configure_fifo(fifo_sensors);
1819
1820     if (dmp_was_on)
1821         mpu_set_dmp_state(1);
1822
1823     return result;
1824 }
1825 */
1826
1827 /**
1828  *  @brief      Write to the DMP memory.
1829  *  This function prevents I2C writes past the bank boundaries. The DMP memory
1830  *  is only accessible when the chip is awake.
1831  *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
1832  *  @param[in]  length      Number of bytes to write.
1833  *  @param[in]  data        Bytes to write to memory.
1834  *  @return     0 if successful.
1835  */
1836 int mpu_write_mem(unsigned short mem_addr, unsigned short length, unsigned char *data)
1837 {
1838     unsigned char tmp[2];
1839 //    if (!data)                                                                           // saving : could be avoided to save flash
1840 //        return -1;
1841 //    if (!chip_cfg.sensors)                                                            // saving : could be avoided to save flash
1842 //        return -1;
1843
1844     tmp[0] = (unsigned char)(mem_addr >> 8);
1845     tmp[1] = (unsigned char)(mem_addr & 0xFF);
1846
1847     /* Check bank boundaries. */
1848     if (tmp[1] + length > INV6050_bank_size)                                             // saving : could be avoided to save flash
1849         return -1;
1850
1851 //    if (i2c_write( INV6050_addr, GYRO_REG_bank_sel, 2, tmp))
1852     if (i2c_writeBlock( GYRO_REG_bank_sel, 2, tmp))
1853         return -1;
1854 //    if (i2c_write( INV6050_addr, GYRO_REG_mem_r_w, length, data))
1855     if (i2c_writeBlock( GYRO_REG_mem_r_w, length, data))
1856         return -1;
1857     return 0;
1858 }
1859
1860 /**
1861  *  @brief      Read from the DMP memory.
1862  *  This function prevents I2C reads past the bank boundaries. The DMP memory
1863  *  is only accessible when the chip is awake.
1864  *  @param[in]  mem_addr    Memory location (bank << 8 | start address)
1865  *  @param[in]  length      Number of bytes to read.
1866  *  @param[out] data        Bytes read from memory.
1867  *  @return     0 if successful.
1868  */
1869 int mpu_read_mem(unsigned short mem_addr, unsigned short length, unsigned char *data)   // saving : this function is used when DMP firmware is uploaded; perhaps it can be avoided if we thrust the I2C
1870 {
1871     unsigned char tmp[2];
1872
1873 //    if (!data)                                                                          // saving : could be avoided to save flash
1874 //        return -1;
1875 //    if (!chip_cfg.sensors)                                                           // saving : could be avoided
1876 //        return -1;
1877
1878     tmp[0] = (unsigned char)(mem_addr >> 8);
1879     tmp[1] = (unsigned char)(mem_addr & 0xFF);
1880
1881     /* Check bank boundaries. */
1882     if (tmp[1] + length > INV6050_bank_size)                                            // saving : could be avoided
1883         return -1;
1884
1885 //    if (i2c_write( INV6050_addr, GYRO_REG_bank_sel, 2, tmp))
1886     if (i2c_writeBlock( GYRO_REG_bank_sel, 2, tmp))
1887         return -1;
1888     if (i2c_read( INV6050_addr, GYRO_REG_mem_r_w, length, data))
1889         return -1;
1890     return 0;
1891 }
1892
1893 /**
1894  *  @brief      Load and verify DMP image.
1895  *  @param[in]  length      Length of DMP image.
1896  *  @param[in]  firmware    DMP code.
1897  *  @param[in]  start_addr  Starting address of DMP code memory.
1898  *  @param[in]  sample_rate Fixed sampling rate used when DMP is enabled.
1899  *  @return     0 if successful.
1900  */
1901 //int mpu_load_firmware(unsigned short length, const unsigned char *firmware, unsigned short start_addr, unsigned short sample_rate)
1902 int mpu_load_firmware(unsigned short length, const unsigned char *firmware, unsigned short start_addr)
1903 {
1904     unsigned short ii;
1905     unsigned short this_write;
1906     /* Must divide evenly into INV6050_bank_size to avoid bank crossings. */
1907 #define LOAD_CHUNK  (16)                                                   // write/read data by group of 16.
1908     unsigned char cur[LOAD_CHUNK], tmp[2];
1909
1910         // For arduino, read firmware out of PROGMEM (Flash)
1911
1912         unsigned char firmware_chunk[LOAD_CHUNK];
1913
1914 //    if (chip_cfg.dmp_loaded)                                            // saving : this check could be avoided
1915         /* DMP should only be loaded once. */
1916 //        return -3;
1917
1918 //    if (!firmware)                                                         // saving : this check could be avoided
1919 //        return -4;
1920     for (ii = 0; ii < length; ii += this_write) {
1921         this_write = min(LOAD_CHUNK, length - ii);                        //upload will be handle by max 16 bytes
1922
1923                 unsigned short x;
1924                 unsigned char *pFirmware = (unsigned char *)&firmware[ii];
1925                 for ( x = 0; x < this_write; x++ ) {
1926                         firmware_chunk[x] = pgm_read_byte_near(pFirmware+x);           // fill a buffer of max 16 byte with the data from flash
1927                 }
1928         if (mpu_write_mem(ii, this_write, firmware_chunk))               // write the bytes at adress ii
1929             return -1;
1930
1931         if (mpu_read_mem(ii, this_write, cur))
1932             return -1;
1933                 if (memcmp(firmware_chunk, cur, this_write))                     // compare if what has been read is the same.
1934                 return -2;
1935     }
1936
1937     /* Set program start address. */
1938     tmp[0] = start_addr >> 8;
1939     tmp[1] = start_addr & 0xFF;
1940 //    if (i2c_write( INV6050_addr, GYRO_REG_prgm_start_h, 2, tmp))
1941     if (i2c_writeBlock( GYRO_REG_prgm_start_h, 2, tmp))
1942         return -5;
1943
1944 //    chip_cfg.dmp_loaded = 1;
1945 //    chip_cfg.dmp_sample_rate = sample_rate;
1946     return 0;
1947 }
1948
1949 /**
1950  *  @brief      Enable/disable DMP support.
1951  *  @param[in]  enable  1 to turn on the DMP.
1952  *  @return     0 if successful.
1953  */
1954  /*
1955 int mpu_set_dmp_state(unsigned char enable)
1956 {
1957     unsigned char tmp;                                        // saving : this test could be ommited
1958     if (chip_cfg.dmp_on == enable)
1959         return 0;
1960
1961     if (enable) {
1962         if (!chip_cfg.dmp_loaded)                          // saving : this test could be ommited if we take care to load first
1963             return -1;
1964         // Disable data ready interrupt.
1965         set_int_enable(0);                                   // probably not requested because this is the default value
1966         // Disable bypass mode.
1967         mpu_set_bypass(0);                                             // further to check if really needed
1968         // Keep constant sample rate, FIFO rate controlled by DMP.
1969         mpu_set_sample_rate(chip_cfg.dmp_sample_rate);                 // further to check if really needed
1970         // Remove FIFO elements.
1971 //        tmp = 0;
1972 //        i2c_write( INV6050_addr, 0x23, 1, &tmp);
1973         i2c_writeByte(  0x23, 0);
1974         chip_cfg.dmp_on = 1;
1975         // Enable DMP interrupt.
1976         set_int_enable(1);
1977         mpu_reset_fifo();
1978     }  else {
1979         // Disable DMP interrupt.
1980         set_int_enable(0);
1981         // Restore FIFO settings.
1982         tmp = chip_cfg.fifo_enable;
1983 //        i2c_write( INV6050_addr, 0x23, 1, &tmp);
1984         i2c_writeByte(  0x23, tmp);
1985         chip_cfg.dmp_on = 0;
1986         mpu_reset_fifo();
1987     }
1988     return 0;
1989 }
1990 */
1991 int mpu_set_dmp_state_on()
1992 {
1993         i2c_writeByte(  GYRO_REG_int_enable, 0) ;   // reg 0x38 ; disable interrupt // normally not required because it was not enabled
1994         mpu_set_sample_rate200hz();                 // not sure it must be done (because it is already done in firmware
1995         i2c_writeByte(  GYRO_REG_fifo_en , 0);      // reg 0x23 ; disable all types of data in fifo (avoid that data are loaded in fifo) // not sure it is required but it was so in original code
1996 //        chip_cfg.dmp_on = 1;
1997         i2c_writeByte( GYRO_REG_int_enable, BIT_DMP_INT_EN ) ;                  // reg 0x38 ; byte = 0X02 ; enable interrupt on DMP ; this bit is not documented
1998         mpu_reset_fifo() ;  // this wil Enable DMP interrupt too (so not sure previous int enable is requested)
1999     return 0;
2000 }
2001
2002 /**
2003  *  @brief      Get DMP state.
2004  *  @param[out] enabled 1 if enabled.
2005  *  @return     0 if successful.
2006  */
2007  /*
2008 int mpu_get_dmp_state(unsigned char *enabled)
2009 {
2010     enabled[0] = chip_cfg.dmp_on;
2011     return 0;
2012 }
2013 */
2014
2015 /**                                                                               // this code is not used by oXs
2016  *  @brief      Enters LP accel motion interrupt mode.
2017  *  The behavior of this feature is very different between the MPU6050 and the
2018  *  MPU6500. Each chip's version of this feature is explained below.
2019  *
2020  *  \n MPU6050:
2021  *  \n When this mode is first enabled, the hardware captures a single accel
2022  *  sample, and subsequent samples are compared with this one to determine if
2023  *  the device is in motion. Therefore, whenever this "locked" sample needs to
2024  *  be changed, this function must be called again.
2025  *
2026  *  \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
2027  *  increments.
2028  *
2029  *  \n Low-power accel mode supports the following frequencies:
2030  *  \n 1.25Hz, 5Hz, 20Hz, 40Hz
2031  *
2032  *  \n MPU6500:
2033  *  \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
2034  *  sample. The hardware monitors the accel data and detects any large change
2035  *  over a short period of time.
2036  *
2037  *  \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
2038  *  increments.
2039  *
2040  *  \n MPU6500 Low-power accel mode supports the following frequencies:
2041  *  \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
2042  *
2043  *  \n\n NOTES:
2044  *  \n The driver will round down @e thresh to the nearest supported value if
2045  *  an unsupported threshold is selected.
2046  *  \n To select a fractional wake-up frequency, round down the value passed to
2047  *  @e lpa_freq.
2048  *  \n The MPU6500 does not support a delay parameter. If this function is used
2049  *  for the MPU6500, the value passed to @e time will be ignored.
2050  *  \n To disable this mode, set @e lpa_freq to zero. The driver will restore
2051  *  the previous configuration.
2052  *
2053  *  @param[in]  thresh      Motion threshold in mg.
2054  *  @param[in]  time        Duration in milliseconds that the accel data must
2055  *                          exceed @e thresh before motion is reported.
2056  *  @param[in]  lpa_freq    Minimum sampling rate, or zero to disable.
2057  *  @return     0 if successful.
2058  */
2059 /*                    // commented because not use by oxs
2060 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
2061     unsigned char lpa_freq)
2062 {
2063     unsigned char data[3];
2064
2065     if (lpa_freq) {
2066         unsigned char thresh_hw;
2067
2068
2069         // TODO: Make these const/#defines.
2070         // 1LSb = 32mg.
2071         if (thresh > 8160)
2072             thresh_hw = 255;
2073         else if (thresh < 32)
2074             thresh_hw = 1;
2075         else
2076             thresh_hw = thresh >> 5;
2077
2078         if (!time)
2079             // Minimum duration must be 1ms.
2080             time = 1;
2081
2082         if (lpa_freq > 40)
2083             // At this point, the chip has not been re-configured, so the
2084             // function can safely exit.
2085
2086             return -1;
2087
2088         if (!chip_cfg.int_motion_only) {
2089             // Store current settings for later.
2090             if (chip_cfg.dmp_on) {
2091                 mpu_set_dmp_state(0);
2092                 chip_cfg.cache.dmp_on = 1;
2093             } else
2094                 chip_cfg.cache.dmp_on = 0;
2095             mpu_get_gyro_fsr(&chip_cfg.cache.gyro_fsr);
2096             mpu_get_accel_fsr(&chip_cfg.cache.accel_fsr);
2097             mpu_get_lpf(&chip_cfg.cache.lpf);
2098             mpu_get_sample_rate(&chip_cfg.cache.sample_rate);
2099             chip_cfg.cache.sensors_on = chip_cfg.sensors;
2100             mpu_get_fifo_config(&chip_cfg.cache.fifo_sensors);
2101         }
2102
2103         // Disable hardware interrupts for now.
2104         set_int_enable(0);
2105
2106         // Enter full-power accel-only mode.
2107         mpu_lp_accel_mode(0);
2108
2109         // Override current LPF (and HPF) settings to obtain a valid accel
2110         //  reading.
2111
2112         data[0] = INV_FILTER_256HZ_NOLPF2;
2113 //        if (i2c_write( INV6050_addr, GYRO_REG_lpf, 1, data))
2114         if (i2c_writeBlock( GYRO_REG_lpf, 1, data))
2115             return -1;
2116
2117         // NOTE: Digital high pass filter should be configured here. Since this
2118         // driver doesn't modify those bits anywhere, they should already be
2119         // cleared by default.
2120
2121
2122         // Configure the device to send motion interrupts.
2123         // Enable motion interrupt.
2124         data[0] = BIT_MOT_INT_EN;
2125 //        if (i2c_write( INV6050_addr, GYRO_REG_int_enable, 1, data))
2126         if (i2c_writeBlock( GYRO_REG_int_enable, 1, data))
2127             goto lp_int_restore;
2128
2129         // Set motion interrupt parameters.
2130         data[0] = thresh_hw;
2131         data[1] = time;
2132 //        if (i2c_write( INV6050_addr, GYRO_REG_motion_thr, 2, data))
2133         if (i2c_writeBlock( GYRO_REG_motion_thr, 2, data))
2134             goto lp_int_restore;
2135
2136         // Force hardware to "lock" current accel sample.
2137         delay(5);
2138         data[0] = (chip_cfg.accel_fsr << 3) | BITS_HPF;
2139 //        if (i2c_write( INV6050_addr, GYRO_REG_accel_cfg, 1, data))
2140         if (i2c_writeBlock( GYRO_REG_accel_cfg, 1, data))
2141             goto lp_int_restore;
2142
2143         // Set up LP accel mode.
2144         data[0] = BIT_LPA_CYCLE;                                             // saving : we could fix the value being used
2145         if (lpa_freq == 1)
2146             data[1] = INV_LPA_1_25HZ;
2147         else if (lpa_freq <= 5)
2148             data[1] = INV_LPA_5HZ;
2149         else if (lpa_freq <= 20)
2150             data[1] = INV_LPA_20HZ;
2151         else
2152             data[1] = INV_LPA_40HZ;
2153         data[1] = (data[1] << 6) | BIT_STBY_XYZG;
2154 //        if (i2c_write( INV6050_addr, GYRO_REG_pwr_mgmt_1, 2, data))
2155         if (i2c_writeBlock( GYRO_REG_pwr_mgmt_1, 2, data))
2156             goto lp_int_restore;
2157
2158         chip_cfg.int_motion_only = 1;
2159         return 0;
2160     } else {
2161         // Don't "restore" the previous state if no state has been saved.
2162         unsigned int ii;
2163         char *cache_ptr = (char*)&chip_cfg.cache;
2164         for (ii = 0; ii < sizeof(chip_cfg.cache); ii++) {
2165             if (cache_ptr[ii] != 0)
2166                 goto lp_int_restore;
2167         }
2168         // If we reach this point, motion interrupt mode hasn't been used yet.
2169         return -1;
2170     }
2171 lp_int_restore:
2172     // Set to invalid values to ensure no I2C writes are skipped.
2173     chip_cfg.gyro_fsr = 0xFF;
2174     chip_cfg.accel_fsr = 0xFF;
2175     chip_cfg.lpf = 0xFF;
2176     chip_cfg.sample_rate = 0xFFFF;
2177     chip_cfg.sensors = 0xFF;
2178     chip_cfg.fifo_enable = 0xFF;
2179     chip_cfg.clk_src = INV_CLK_PLL;
2180     mpu_set_sensors(chip_cfg.cache.sensors_on);
2181     mpu_set_gyro_fsr(chip_cfg.cache.gyro_fsr);
2182     mpu_set_accel_fsr(chip_cfg.cache.accel_fsr);
2183     mpu_set_lpf(chip_cfg.cache.lpf);
2184     mpu_set_sample_rate(chip_cfg.cache.sample_rate);
2185     mpu_configure_fifo(chip_cfg.cache.fifo_sensors);
2186
2187     if (chip_cfg.cache.dmp_on)
2188         mpu_set_dmp_state(1);
2189
2190     chip_cfg.int_motion_only = 0;
2191     return 0;
2192 }
2193 */
2194 /**
2195  *  @}
2196  */