shoes_project/touch_floor_by_imu/drivers/lsm6ds3tr_c.c

/* Includes ------------------------------------------------------------------*/
#include "lsm6ds3tr_c.h"

/* Private macro -------------------------------------------------------------*/

#define    BOOT_TIME        							15 //ms
#define    WAIT_TIME_A     								100 //ms
#define    WAIT_TIME_G_01  								150 //ms
#define    WAIT_TIME_G_02   							50 //ms

/* Self test limits. */
#define    MIN_ST_LIMIT_mg        				90.0f
#define    MAX_ST_LIMIT_mg      					1700.0f
#define    MIN_ST_LIMIT_mdps   						150000.0f
#define    MAX_ST_LIMIT_mdps   						700000.0f

/* Self test results. */
#define    ST_PASS    		 								1U
#define    ST_FAIL     										0U

#define 	 MIN_ODR(x, y)         					(x < y ? x : y)
#define 	 MAX_ODR(x, y)         					(x > y ? x : y)
#define 	 MAX_PATTERN_NUM        				FIFO_THRESHOLD / 6
#define 	 LSM6DS3_ODR_LSB_TO_HZ(_odr)  	(_odr ? (13 << (_odr - 1)) : 0)

/* static variable ------------------------------------------------------------------*/
static uint8_t whoamI, rst;
static uint16_t pattern_len;
static stmdev_ctx_t dev_ctx;
static axis3bit16_t data_raw_acceleration;
static axis3bit16_t data_raw_angular_rate;
static int16_t data_raw_temperature;

/* 6ds3 Accelerometer test parameters */
static sensor_lsm6ds3_xl test_6ds3_xl;

/* 6ds3 Gyroscope test parameters */
static sensor_lsm6ds3_gy test_6ds3_gyro;

//static float acceleration_mg[3];
//static float angular_rate_mdps[3];
//static float temperature_degC;
/* static fuction ------------------------------------------------------------------*/

/** Please note that is MANDATORY: return 0 -> no Error.**/
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	if(SPI_OnlyWriteReg(BOARD_SPI0_CS0_IO, reg, (uint8_t *)bufp, len))
	{
		ierror = -1;
	}
	
	return ierror;
}
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	if(SPI_OnlyReadReg(BOARD_SPI0_CS0_IO, reg, bufp, len))
	{
		ierror = -1;
	}
	
	return ierror;
}
static void platform_delay(uint32_t ms)
{
	nrf_delay_ms(ms);
}
/* Following routine calculate the FIFO pattern composition based
 * on gyro and acc enable state and ODR freq
 */
static uint16_t LSM6DS3TR_C_Calculate_FIFO_Pattern(uint16_t *min_odr, uint16_t *max_odr)
{
  uint16_t fifo_samples_tot_num = 0;

  /* Calculate min_odr and max_odr for current configuration */
  if (test_6ds3_gyro.enable)
  {
    test_6ds3_gyro.odr_hz_val = LSM6DS3_ODR_LSB_TO_HZ(test_6ds3_gyro.odr);
    *max_odr = MAX_ODR(*max_odr, test_6ds3_gyro.odr_hz_val);
    *min_odr = MIN_ODR(*min_odr, test_6ds3_gyro.odr_hz_val);
  }

  if (test_6ds3_xl.enable)
  {
    test_6ds3_xl.odr_hz_val = LSM6DS3_ODR_LSB_TO_HZ(test_6ds3_xl.odr);
    *max_odr = MAX_ODR(*max_odr, test_6ds3_xl.odr_hz_val);
    *min_odr = MIN_ODR(*min_odr, test_6ds3_xl.odr_hz_val);
  }

  /* Calculate how many samples for each sensor are in current FIFO pattern */
  if (test_6ds3_gyro.enable)
  {
    test_6ds3_gyro.samples_num_in_pattern = test_6ds3_gyro.odr_hz_val / *min_odr;
    test_6ds3_gyro.decimation =  *max_odr / test_6ds3_gyro.odr_hz_val;
    fifo_samples_tot_num += test_6ds3_gyro.samples_num_in_pattern;
  }

  if (test_6ds3_xl.enable)
  {
    test_6ds3_xl.samples_num_in_pattern = test_6ds3_xl.odr_hz_val / *min_odr;
    test_6ds3_xl.decimation =  *max_odr / test_6ds3_xl.odr_hz_val;
    fifo_samples_tot_num += test_6ds3_xl.samples_num_in_pattern;
  }

  /* Return the total number of 16-bit samples in the pattern */
  return(6 * fifo_samples_tot_num);
}

/* Following routine read a pattern from FIFO */
static void LSM6DS3TR_C_Read_FIFO_Pattern(axis3bit16_t *acc, uint16_t *acc_num, axis3bit16_t *gry, uint16_t *gry_num)
{
  uint8_t gy_num = test_6ds3_gyro.samples_num_in_pattern;
  uint8_t xl_num = test_6ds3_xl.samples_num_in_pattern;

  /* FIFO pattern is composed by gy_num gyroscope triplets and
   * xl_num accelerometer triplets. The sequence has always following order:
   * gyro first, accelerometer second
   */
  while(gy_num > 0 || xl_num > 0)
  {
    /* Read gyro samples */
    if (test_6ds3_gyro.enable && gy_num > 0)
    {
      lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_angular_rate.u8bit,
                                3 * sizeof(int16_t));
			
			gry[*gry_num].i16bit[0] = data_raw_angular_rate.i16bit[0];
			gry[*gry_num].i16bit[1] = data_raw_angular_rate.i16bit[1];
			gry[*gry_num].i16bit[2] = data_raw_angular_rate.i16bit[2];
			(*gry_num)++;

//      angular_rate_mdps[0] =
//        lsm6ds3_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[0]);
//      angular_rate_mdps[1] =
//        lsm6ds3_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[1]);
//      angular_rate_mdps[2] =
//        lsm6ds3_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[2]);

//      sprintf((char*)tx_buffer, "Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
//              angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
//      tx_com( tx_buffer, strlen( (char const*)tx_buffer ) );
      gy_num--;
    }

    /* Read XL samples */
    if (test_6ds3_xl.enable && xl_num > 0)
    {
      lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit,
                                3 * sizeof(int16_t));
			
			acc[*acc_num].i16bit[0] = data_raw_acceleration.i16bit[0];
			acc[*acc_num].i16bit[1] = data_raw_acceleration.i16bit[1];
			acc[*acc_num].i16bit[2] = data_raw_acceleration.i16bit[2];
			(*acc_num)++;
			
//      acceleration_mg[0] =
//        lsm6ds3_from_fs2g_to_mg(data_raw_acceleration.i16bit[0]);
//      acceleration_mg[1] =
//        lsm6ds3_from_fs2g_to_mg(data_raw_acceleration.i16bit[1]);
//      acceleration_mg[2] =
//        lsm6ds3_from_fs2g_to_mg(data_raw_acceleration.i16bit[2]);

//      sprintf((char*)tx_buffer, "Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
//              acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
//      tx_com( tx_buffer, strlen( (char const*)tx_buffer ) );
      xl_num--;
    }
  }
}

/* API ------------------------------------------------------------------*/

void lsm6ds3tr_c_init(void)
{
  int16_t data_raw[3];
  float val_st_off[3];
  float val_st_on[3];
  float test_val[3];
  uint8_t st_result;
  uint8_t drdy;
  uint8_t i;
  uint8_t j;
	
	SPI_Init();
	
	/* Initialize mems driver interface */
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.handle = NULL;
	/* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI);
	if(whoamI != LSM6DS3TR_C_ID)
	{
		SEGGER_RTT_printf(0,"lsm6ds3tr_c_init error!!!\r\n");
		return;
	}
	/* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);
	do {
    lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
  } while(rst);
	/* Enable Block Data Update */
  lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
	/*
   * Accelerometer Self Test
   */
  /* Set Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_52Hz);
  /* Set full scale */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g);
  /* Wait stable output */
  platform_delay(WAIT_TIME_A);

  /* Check if new value available */
  do {
    lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy);
  } while (!drdy);

  /* Read dummy data and discard it */
  lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw);
  /* Read 5 sample and get the average vale for each axis */
  memset(val_st_off, 0x00, 3 * sizeof(float));

  for (i = 0; i < 5; i++) {
    /* Check if new value available */
    do {
      lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy);
    } while (!drdy);

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw);

    for (j = 0; j < 3; j++) {
      val_st_off[j] += lsm6ds3tr_c_from_fs4g_to_mg(data_raw[j]);
    }
  }

  /* Calculate the mg average values */
  for (i = 0; i < 3; i++) {
    val_st_off[i] /= 5.0f;
  }

  /* Enable Self Test positive (or negative) */
  lsm6ds3tr_c_xl_self_test_set(&dev_ctx, LSM6DS3TR_C_XL_ST_NEGATIVE);
  //lsm6ds3tr_c_xl_self_test_set(&dev_ctx, LSM6DS3TR_C_XL_ST_POSITIVE);
  /* Wait stable output */
  platform_delay(WAIT_TIME_A);

  /* Check if new value available */
  do {
    lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy);
  } while (!drdy);

  /* Read dummy data and discard it */
  lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw);
  /* Read 5 sample and get the average vale for each axis */
  memset(val_st_on, 0x00, 3 * sizeof(float));

  for (i = 0; i < 5; i++) {
    /* Check if new value available */
    do {
      lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy);
    } while (!drdy);

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw);

    for (j = 0; j < 3; j++) {
      val_st_on[j] += lsm6ds3tr_c_from_fs4g_to_mg(data_raw[j]);
    }
  }

  /* Calculate the mg average values */
  for (i = 0; i < 3; i++) {
    val_st_on[i] /= 5.0f;
  }

  /* Calculate the mg values for self test */
  for (i = 0; i < 3; i++) {
    test_val[i] = fabs((val_st_on[i] - val_st_off[i]));
  }

  /* Check self test limit */
  st_result = ST_PASS;

  for (i = 0; i < 3; i++) {
    if (( MIN_ST_LIMIT_mg > test_val[i] ) ||
        ( test_val[i] > MAX_ST_LIMIT_mg)) {
      st_result = ST_FAIL;
    }
  }

  /* Disable Self Test */
  lsm6ds3tr_c_xl_self_test_set(&dev_ctx, LSM6DS3TR_C_XL_ST_DISABLE);
  /* Disable sensor. */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);
  /*
   * Gyroscope Self Test
   */
  /* Set Output Data Rate */
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_208Hz);
  /* Set full scale */
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);
  /* Wait stable output */
  platform_delay(WAIT_TIME_G_01);

  /* Check if new value available */
  do {
    lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, &drdy);
  } while (!drdy);

  /* Read dummy data and discard it */
  lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw);
  /* Read 5 sample and get the average vale for each axis */
  memset(val_st_off, 0x00, 3 * sizeof(float));

  for (i = 0; i < 5; i++) {
    /* Check if new value available */
    do {
      lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, &drdy);
    } while (!drdy);

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw);

    for (j = 0; j < 3; j++) {
      val_st_off[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
                         data_raw[j]);
    }
  }

  /* Calculate the mg average values */
  for (i = 0; i < 3; i++) {
    val_st_off[i] /= 5.0f;
  }

  /* Enable Self Test positive (or negative) */
  lsm6ds3tr_c_gy_self_test_set(&dev_ctx, LSM6DS3TR_C_GY_ST_POSITIVE);
  //lsm6ds3tr_c_gy_self_test_set(&dev_ctx, LIS2DH12_GY_ST_NEGATIVE);
  /* Wait stable output */
  platform_delay(WAIT_TIME_G_02);
  /* Read 5 sample and get the average vale for each axis */
  memset(val_st_on, 0x00, 3 * sizeof(float));

  for (i = 0; i < 5; i++) {
    /* Check if new value available */
    do {
      lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, &drdy);
    } while (!drdy);

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw);

    for (j = 0; j < 3; j++) {
      val_st_on[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
                        data_raw[j]);
    }
  }

  /* Calculate the mg average values */
  for (i = 0; i < 3; i++) {
    val_st_on[i] /= 5.0f;
  }

  /* Calculate the mg values for self test */
  for (i = 0; i < 3; i++) {
    test_val[i] = fabs((val_st_on[i] - val_st_off[i]));
  }

  /* Check self test limit */
  for (i = 0; i < 3; i++) {
    if (( MIN_ST_LIMIT_mdps > test_val[i] ) ||
        ( test_val[i] > MAX_ST_LIMIT_mdps)) {
      st_result = ST_FAIL;
    }
  }

  /* Disable Self Test */
  lsm6ds3tr_c_gy_self_test_set(&dev_ctx, LSM6DS3TR_C_GY_ST_DISABLE);
  /* Disable sensor. */
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_OFF);

  if (st_result == ST_PASS) {
    SEGGER_RTT_printf(0,"Self Test - PASS\r\n");
  }

  else {
    SEGGER_RTT_printf(0,"Self Test - FAIL\r\n");
  }
}

void lsm6ds3tr_c_fifo_mode(void)
{
  uint16_t max_odr = 0, min_odr = 0xffff;
	
	/* 6ds3 Accelerometer test parameters */
	test_6ds3_xl.enable = PROPERTY_ENABLE;
	test_6ds3_xl.odr = LSM6DS3TR_C_XL_ODR_104Hz;
	test_6ds3_xl.odr_hz_val = 0;
	test_6ds3_xl.fs = LSM6DS3TR_C_16g;
	test_6ds3_xl.decimation = 0;
	test_6ds3_xl.samples_num_in_pattern = 0;


//	/* 6ds3 Gyroscope test parameters */
//	test_6ds3_gyro.enable = PROPERTY_ENABLE;
//	test_6ds3_gyro.odr = LSM6DS3TR_C_GY_ODR_104Hz;
//	test_6ds3_gyro.odr_hz_val = 0;
//	test_6ds3_gyro.fs = LSM6DS3TR_C_2000dps;
//	test_6ds3_gyro.decimation = 0;
//	test_6ds3_gyro.samples_num_in_pattern = 0;
	

  /* Interrupt generation on FIFO watermark INT1/INT2 pin */
  //lsm6ds3_int2_route_t int_2_reg;
  //lsm6ds3_int1_route_t int_1_reg;
	
	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL);

  /* Set XL and Gyro Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, test_6ds3_xl.odr);
//  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, test_6ds3_gyro.odr);

  /* Set XL full scale and Gyro full scale */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, test_6ds3_xl.fs);
//  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, test_6ds3_gyro.fs);

	lsm6ds3tr_c_gy_sleep_mode_set(&dev_ctx, 1);

  /* Calculate number of sensors samples in each FIFO pattern */
  pattern_len = LSM6DS3TR_C_Calculate_FIFO_Pattern(&min_odr, &max_odr);

  /* Set FIFO watermark to a multiple of a pattern */
  lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, pattern_len);

  /* Set FIFO mode to Stream mode */
  lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_STREAM_MODE);

  /* Enable FIFO watermark interrupt generation on INT1 pin */
  //lsm6ds3_pin_int1_route_get(&dev_ctx, &int_1_reg);
  //int_1_reg.int1_fth = PROPERTY_ENABLE;
  //lsm6ds3_pin_int1_route_set(&dev_ctx, &int_1_reg);

  /* FIFO watermark interrupt on INT2 pin */
  //lsm6ds3_pin_int2_route_get(&dev_ctx, &int_2_reg);
  //int_2_reg.int2_fth = PROPERTY_ENABLE;
  //lsm6ds3_pin_int2_route_set(&dev_ctx, &int_2_reg);

  /* Set ODR FIFO */
  lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, LSM6DS3TR_C_FIFO_416Hz);

  /* Set FIFO sensor decimator */
  lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_xl_t)test_6ds3_xl.decimation);
//  lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_gyro_t)test_6ds3_gyro.decimation);
}

void lsm6ds3tr_c_fifo_read(axis3bit16_t **acc, uint16_t *acc_num, axis3bit16_t **gry, uint16_t *gry_num)
{
	uint8_t wt;
	uint16_t num = 0;
  uint16_t num_pattern = 0;
	uint16_t acc_buf_num = 0, gry_buf_num = 0;
	static axis3bit16_t acc_buf[100], gry_buf[100];

	/* Read FIFO watermark flag in polling mode */
	lsm6ds3tr_c_fifo_wtm_flag_get(&dev_ctx, &wt);
	if (wt)
	{
		/* Read number of word in FIFO */
		lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &num);
		num_pattern = num / pattern_len;

		while (num_pattern-- > 0)
			LSM6DS3TR_C_Read_FIFO_Pattern(acc_buf, &acc_buf_num, gry_buf, &gry_buf_num);
	}
	
	
	if(acc != NULL)
		*acc = acc_buf;
	
	if(acc_num != NULL)
		*acc_num = acc_buf_num;
	
	if(gry != NULL)
		*gry = gry_buf;
	
	if(gry_num != NULL)
		*gry_num = gry_buf_num;
}

void lsm6ds3tr_c_read_data_polling_mode(void)
{
	/* 6ds3 Accelerometer test parameters */
	test_6ds3_xl.enable = PROPERTY_ENABLE;
	test_6ds3_xl.odr = LSM6DS3TR_C_XL_ODR_104Hz;
	test_6ds3_xl.odr_hz_val = 0;
	test_6ds3_xl.fs = LSM6DS3TR_C_16g;
	test_6ds3_xl.decimation = 0;
	test_6ds3_xl.samples_num_in_pattern = 0;


	/* 6ds3 Gyroscope test parameters */
	test_6ds3_gyro.enable = PROPERTY_ENABLE;
	test_6ds3_gyro.odr = LSM6DS3TR_C_GY_ODR_104Hz;
	test_6ds3_gyro.odr_hz_val = 0;
	test_6ds3_gyro.fs = LSM6DS3TR_C_2000dps;
	test_6ds3_gyro.decimation = 0;
	test_6ds3_gyro.samples_num_in_pattern = 0;

	/* Check device ID */
  whoamI = 0;
  lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI);

	if(whoamI != LSM6DS3TR_C_ID)
	{
		SEGGER_RTT_printf(0,"lsm6ds3tr_c_read_data_polling_config error!!!\r\n");
		return;
	}
	
	/* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
  } while (rst);

  /* Enable Block Data Update */
  lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, test_6ds3_xl.odr);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, test_6ds3_gyro.odr);
  /* Set full scale */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, test_6ds3_xl.fs);
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, test_6ds3_gyro.fs);
  /* Configure filtering chain(No aux interface) */
  /* Accelerometer - analog filter */
//  lsm6ds3tr_c_xl_filter_analog_set(&dev_ctx,
//                                   LSM6DS3TR_C_XL_ANA_BW_400Hz);
//  /* Accelerometer - LPF1 path ( LPF2 not used )*/
//  //lsm6ds3tr_c_xl_lp1_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4);
//  /* Accelerometer - LPF1 + LPF2 path */
//  lsm6ds3tr_c_xl_lp2_bandwidth_set(&dev_ctx,
//                                   LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100);
  /* Accelerometer - High Pass / Slope path */
  //lsm6ds3tr_c_xl_reference_mode_set(&dev_ctx, PROPERTY_DISABLE);
  //lsm6ds3tr_c_xl_hp_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_HP_ODR_DIV_100);
  /* Gyroscope - filtering chain */
//  lsm6ds3tr_c_gy_band_pass_set(&dev_ctx,
//                               LSM6DS3TR_C_HP_260mHz_LP1_STRONG);
}

void lsm6ds3tr_c_read_data_polling(int16_t *acc, int16_t *gry, int16_t *temp)
{
	 /* Read samples in polling mode (no int) */
	 /* Read output only if new value is available */
    lsm6ds3tr_c_reg_t reg;
    lsm6ds3tr_c_status_reg_get(&dev_ctx, &reg.status_reg);

    if (reg.status_reg.xlda) {
      /* Read magnetic field data */
      memset( data_raw_acceleration.i16bit, 0x00, 3 * sizeof(int16_t));
      lsm6ds3tr_c_acceleration_raw_get(&dev_ctx,
                                        data_raw_acceleration.i16bit);
//      acceleration_mg[0] = lsm6ds3tr_c_from_fs2g_to_mg(
//                             data_raw_acceleration[0]);
//      acceleration_mg[1] = lsm6ds3tr_c_from_fs2g_to_mg(
//                             data_raw_acceleration[1]);
//      acceleration_mg[2] = lsm6ds3tr_c_from_fs2g_to_mg(
//                             data_raw_acceleration[2]);
//			sprintf((char *)tx_buffer,
//				"Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
//				acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
			acc[0] = data_raw_acceleration.i16bit[0];
			acc[1] = data_raw_acceleration.i16bit[1];
			acc[2] = data_raw_acceleration.i16bit[2];
		}

    if (reg.status_reg.gda) {
      /* Read magnetic field data */
      memset(data_raw_angular_rate.i16bit, 0x00, 3 * sizeof(int16_t));
      lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx,
                                       data_raw_angular_rate.i16bit);
//      angular_rate_mdps[0] = lsm6ds3tr_c_from_fs2000dps_to_mdps(
//                               data_raw_angular_rate[0]);
//      angular_rate_mdps[1] = lsm6ds3tr_c_from_fs2000dps_to_mdps(
//                               data_raw_angular_rate[1]);
//      angular_rate_mdps[2] = lsm6ds3tr_c_from_fs2000dps_to_mdps(
//                               data_raw_angular_rate[2]);		
//			sprintf((char *)tx_buffer,
//              "Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
//              angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
			gry[0] = data_raw_angular_rate.i16bit[0];
			gry[1] = data_raw_angular_rate.i16bit[1];
			gry[2] = data_raw_angular_rate.i16bit[2];
    }

    if (reg.status_reg.tda) {
      /* Read temperature data */
      memset(&data_raw_temperature, 0x00, sizeof(int16_t));
      lsm6ds3tr_c_temperature_raw_get(&dev_ctx, &data_raw_temperature);
//      temperature_degC = lsm6ds3tr_c_from_lsb_to_celsius(
//                           data_raw_temperature );
//      sprintf((char *)tx_buffer, "Temperature [degC]:%6.2f\r\n",
//              temperature_degC );
			*temp = data_raw_temperature;
    }
	
}

void lsm6ds3tr_c_low_power_acc_mode(void)
{
	/* 6ds3 Accelerometer test parameters */
	test_6ds3_xl.enable = PROPERTY_ENABLE;
	test_6ds3_xl.odr = LSM6DS3TR_C_XL_ODR_12Hz5;
	test_6ds3_xl.odr_hz_val = 0;
	test_6ds3_xl.fs = LSM6DS3TR_C_16g;
	test_6ds3_xl.decimation = 0;
	test_6ds3_xl.samples_num_in_pattern = 0;
	
	/* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
  } while (rst);
	
	/* Enable Block Data Update */
  lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
	
	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL);
	
	lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, test_6ds3_xl.odr);
	
	lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, test_6ds3_xl.fs);
	
	lsm6ds3tr_c_xl_filter_analog_set(&dev_ctx,
                                   LSM6DS3TR_C_XL_ANA_BW_400Hz);
	
	lsm6ds3tr_c_xl_lp2_bandwidth_set(&dev_ctx,
                                   LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100);
	
	lsm6ds3tr_c_gy_sleep_mode_set(&dev_ctx, 1);

}
void lsm6ds3tr_c_low_power_acc(int16_t *acc)
{
	/* Read samples in polling mode (no int) */
	 /* Read output only if new value is available */
    lsm6ds3tr_c_reg_t reg;
    lsm6ds3tr_c_status_reg_get(&dev_ctx, &reg.status_reg);

    if (reg.status_reg.xlda) {
      /* Read magnetic field data */
      memset( data_raw_acceleration.i16bit, 0x00, 3 * sizeof(int16_t));
      lsm6ds3tr_c_acceleration_raw_get(&dev_ctx,
                                        data_raw_acceleration.i16bit);
//      acceleration_mg[0] = lsm6ds3tr_c_from_fs2g_to_mg(
//                             data_raw_acceleration[0]);
//      acceleration_mg[1] = lsm6ds3tr_c_from_fs2g_to_mg(
//                             data_raw_acceleration[1]);
//      acceleration_mg[2] = lsm6ds3tr_c_from_fs2g_to_mg(
//                             data_raw_acceleration[2]);
//			sprintf((char *)tx_buffer,
//				"Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
//				acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
			acc[0] = data_raw_acceleration.i16bit[0];
			acc[1] = data_raw_acceleration.i16bit[1];
			acc[2] = data_raw_acceleration.i16bit[2];
		}
}

void lsm6ds3tr_c_get_accel_power_mode_stat(uint8_t *stat)
{
	uint8_t reg_val;
	int32_t ret;

  ret = lsm6ds3tr_c_read_reg(&dev_ctx, LSM6DS3TR_C_FIFO_CTRL5,
                             &reg_val, 1);
	
	if(ret == 0 && (reg_val & 0x78)!=0)
	{
		*stat = 1;  //fifo mode
		return;
	}
	
	ret = lsm6ds3tr_c_gy_sleep_mode_get(&dev_ctx, &reg_val);
	
	if(ret == 0 && (1 == reg_val))
	{
		*stat = 2;  //low power acc mode
		return;
	}
	
	*stat = 3;	//acc + gy
}

//	//test
//	uint16_t i;
//	uint8_t stat = 0;
//	int16_t acc[3], gry[3], temp;
//	axis3bit16_t *acc_fifo, *gry_fifo;
//	uint16_t acc_num, gry_num;
//	lsm6ds3tr_c_init();
////	lsm6ds3tr_c_read_data_polling_mode();
////	lsm6ds3tr_c_low_power_acc_mode();
////	lsm6ds3tr_c_fifo_mode();
//	while(1)
//	{
////		lsm6ds3tr_c_read_data_polling(acc, gry, &temp);
////		SEGGER_RTT_printf(0,"h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",acc[0],acc[1],acc[2]);
////		SEGGER_RTT_printf(0,"h_gx=%d\r,h_gy=%d\r,h_gz=%d\r\n",gry[0],gry[1],gry[2]);
////		SEGGER_RTT_printf(0,"temp:%d\r\n",temp);
////		nrf_delay_ms(10);
//		
//		
////		lsm6ds3tr_c_read_data_polling_mode();
////		lsm6ds3tr_c_get_accel_power_mode_stat(&stat);
////		SEGGER_RTT_printf(0,"lsm6ds3tr_c_read_data_polling_mode stat:%d\r\n",stat);
////		nrf_delay_ms(1000);
////		
////		lsm6ds3tr_c_low_power_acc_mode();
////		lsm6ds3tr_c_get_accel_power_mode_stat(&stat);
////		SEGGER_RTT_printf(0,"lsm6ds3tr_c_low_power_acc_mode stat:%d\r\n",stat);
////		nrf_delay_ms(1000);
////		
////		lsm6ds3tr_c_fifo_mode();
////		lsm6ds3tr_c_get_accel_power_mode_stat(&stat);
////		SEGGER_RTT_printf(0,"lsm6ds3tr_c_fifo_mode stat:%d\r\n",stat);
////		nrf_delay_ms(1000);
//		
////		lsm6ds3tr_c_low_power_acc(acc);
////		SEGGER_RTT_printf(0,"h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",acc[0],acc[1],acc[2]);
////		nrf_delay_ms(10);
//		
////		lsm6ds3tr_c_fifo_read(&acc_fifo, &acc_num, &gry_fifo, &gry_num);
////		for(i=0;i<acc_num;i++)
////		{
////			SEGGER_RTT_printf(0,"acc_num:%d h_ax=%d,h_ay=%d,h_az=%d\r\n",acc_num,acc_fifo[i].i16bit[0],acc_fifo[i].i16bit[1],acc_fifo[i].i16bit[2]);
////		}
////		for(i=0;i<gry_num;i++)
////		{
////			SEGGER_RTT_printf(0,"gry_num:%d h_gx=%d,h_gy=%d,h_gz=%d\r\n",gry_num,gry_fifo[i].i16bit[0],gry_fifo[i].i16bit[1],gry_fifo[i].i16bit[2]);
////		}
////		nrf_delay_ms(100);
//	}