/* Includes ------------------------------------------------------------------*/
#include "lsm6ds3tr_c.h"
#include "drv_qmc6310.h"
#include "bsp_spi.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)

#define		 ACC_OUT_XYZ_WORD_SIZE    			3

#define		 GRY_OUT_XYZ_WORD_SIZE    			3

#define		 MAG_OUT_XYZ_WORD_SIZE    			3

#define		 TIMESTAMP_OUT_WORD_SIZE    		3

#define		 USED_ACC_FIFO_104_HZ						LSM6DS3TR_C_XL_ODR_104Hz		
#define    USED_GRY_FIFO_104_HZ						LSM6DS3TR_C_GY_ODR_104Hz
#define 	 USED_FIFO_104_HZ								LSM6DS3TR_C_FIFO_104Hz						//同时也是timestamp的频率

#define		 TIMEOUT												500

#define		 SENSOR_HUB_HZ									LSM6DS3TR_C_XL_ODR_1k66Hz

/* Private type -------------------------------------------------------------*/
typedef union{
  int16_t i16bit[3];
  uint8_t u8bit[6];
} axis3bit16_t;

typedef struct {
  uint8_t enable;
  lsm6ds3tr_c_odr_xl_t odr;
  uint16_t odr_hz_val;
  lsm6ds3tr_c_fs_xl_t fs;
  uint8_t decimation;
  uint8_t samples_num_in_pattern;
} sensor_lsm6ds3_xl;

typedef struct {
  uint8_t enable;
  lsm6ds3tr_c_odr_g_t odr;
  uint16_t odr_hz_val;
  lsm6ds3tr_c_fs_g_t fs;
  uint8_t decimation;
  uint8_t samples_num_in_pattern;
} sensor_lsm6ds3_gy;

/* static variable ------------------------------------------------------------------*/
static uint8_t 											whoamI, rst;
static stmdev_ctx_t									dev_ctx;
static axis3bit16_t 								data_raw_acceleration;
static axis3bit16_t 								data_raw_angular_rate;
static axis3bit16_t 								data_raw_magnetic;
static axis3bit16_t 								data_raw_timestamp;
static lsm6ds3tr_c_emb_sh_read_t 		raw_data;

static uint16_t  										acc_mag_timestamp_pattern_len = ACC_OUT_XYZ_WORD_SIZE + MAG_OUT_XYZ_WORD_SIZE + TIMESTAMP_OUT_WORD_SIZE;
static uint16_t  										gry_acc_mag_timestamp_pattern_len = GRY_OUT_XYZ_WORD_SIZE + ACC_OUT_XYZ_WORD_SIZE + MAG_OUT_XYZ_WORD_SIZE + TIMESTAMP_OUT_WORD_SIZE;

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

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

lsm6ds3tr_c_sh_cfg_read_t qmc6310 = {
	.slv_add = 0x1C,										
	.slv_subadd = QMC6310_DATA_OUT_X_LSB_REG,					//预备读取qmc6310地磁数据的寄存器
	.slv_len = MAG_OUT_XYZ_WORD_SIZE * 2,							//转为字节长度
};

/* static fuction ------------------------------------------------------------------*/

/** Please note that is MANDATORY: return 0 -> no Error.**/
static int32_t platform_front_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	if(SPI0_OnlyWriteReg(BOARD_SPI0_CS0_IO, reg, (uint8_t *)bufp, len))
	{
		ierror = -1;
	}
	
	return ierror;
}
static int32_t platform_front_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	if(SPI0_OnlyReadReg(BOARD_SPI0_CS0_IO, reg, bufp, len))
	{
		ierror = -1;
	}
	
	return ierror;
}
static int32_t platform_center_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	if(SPI1_OnlyWriteReg(BOARD_SPI1_CS0_IO, reg, (uint8_t *)bufp, len))
	{
		ierror = -1;
	}
	
	return ierror;
}
static int32_t platform_center_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	if(SPI1_OnlyReadReg(BOARD_SPI1_CS0_IO, reg, bufp, len))
	{
		ierror = -1;
	}
	
	return ierror;
}
static void platform_delay(uint32_t ms)
{
	nrf_delay_ms(ms);
}

static void switch_dev(lsm_device_t dex_num)
{
	/* Initialize mems driver interface */
	switch(dex_num)
	{
		case LSM_DEV_FRONT:
		  dev_ctx.write_reg = platform_front_write;
			dev_ctx.read_reg = platform_front_read;
			dev_ctx.handle = NULL;
			break;
		case LSM_DEV_CENTER:
			dev_ctx.write_reg = platform_center_write;
			dev_ctx.read_reg = platform_center_read;
			dev_ctx.handle = NULL;
			break;
	}
}

/* Read data byte from internal register of a slave device connected
 * to master I2C interface
 * 注意：因为该函数会配置加速度的采样率
 */
static int32_t lsm6ds3tr_c_read_sensor_hub_cx(stmdev_ctx_t *dev_ctx, uint8_t device_addr, uint8_t reg, uint8_t* data,uint16_t len)
{
  axis3bit16_t data_raw_acceleration;
  int32_t mm_error;
  uint8_t drdy;
	lsm6ds3tr_c_all_sources_t endop;
	
  lsm6ds3tr_c_sh_cfg_read_t val = {
    .slv_add = device_addr,										
    .slv_subadd = reg,
    .slv_len = len,
  };
	
	/* Disable accelerometer */
  lsm6ds3tr_c_xl_data_rate_set(dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);
	
  /* Configure Sensor Hub to read QMC6310 */
	/* Prepare sensor hub to read data from external Slave0 */
  mm_error = lsm6ds3tr_c_sh_slv0_cfg_read(dev_ctx, &val);
	
	/* Configure Sensor Hub to read two slaves */
  lsm6ds3tr_c_sh_num_of_dev_connected_set(dev_ctx, LSM6DS3TR_C_SLV_0_1_2_3);		//这里注意，IIC要比你外设多一个，原因未知，不这么做容易读不到数据。
	
	lsm6ds3tr_c_func_en_set(dev_ctx,PROPERTY_ENABLE);

  /* Enable I2C Master and I2C master Pull Up */
	/* Enable master and XL trigger */
  lsm6ds3tr_c_sh_master_set(dev_ctx, PROPERTY_ENABLE);

  /* Enable accelerometer to trigger Sensor Hub operation */
  lsm6ds3tr_c_xl_data_rate_set(dev_ctx, SENSOR_HUB_HZ);
	

  /* Wait Sensor Hub operation flag set */
  lsm6ds3tr_c_acceleration_raw_get(dev_ctx, data_raw_acceleration.i16bit);
	
	uint16_t timeout = TIMEOUT;
	
  do {
    lsm6ds3tr_c_xl_flag_data_ready_get(dev_ctx, &drdy);
		timeout--;
  } while (!drdy && timeout!=0);
	
//	SEGGER_RTT_printf(0,"read sensor hub timeout_1:%d\n",timeout);
	
	if(timeout == 0)return -1;
	
	timeout = TIMEOUT;

  do
  {
		lsm6ds3tr_c_all_sources_get(dev_ctx,&endop);
		timeout--;
  } while (!endop.func_src1.sensorhub_end_op && timeout!=0);
	
//	SEGGER_RTT_printf(0,"read sensor hub timeout_2:%d\n",timeout);
	
	if(timeout == 0)return -1;

  lsm6ds3tr_c_xl_data_rate_set(dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);
  lsm6ds3tr_c_sh_read_data_raw_get(dev_ctx, &raw_data);
	
	memcpy(data,&raw_data,len);
										
  lsm6ds3tr_c_sh_master_set(dev_ctx, PROPERTY_DISABLE);
	
	lsm6ds3tr_c_func_en_set(dev_ctx,PROPERTY_DISABLE);

  return mm_error;
}
/* Write data byte to internal register of a slave device connected
 * to master I2C interface
 * 必须先配置外挂的地磁先，之后才能配置加速度或陀螺仪。因为该函数会配置加速度的采样率
 */
static int32_t lsm6ds3_write_sensor_hub_cx(stmdev_ctx_t *dev_ctx, uint8_t device_addr, uint8_t reg, uint8_t* data)
{
	axis3bit16_t data_raw_acceleration;
  int32_t mm_error;
  uint8_t drdy;
  lsm6ds3tr_c_all_sources_t endop;
	
	
  lsm6ds3tr_c_sh_cfg_write_t val = {
    .slv0_add = device_addr,
    .slv0_subadd = reg,
    .slv0_data = *data,
  };
	
	/* Disable accelerometer */
  lsm6ds3tr_c_xl_data_rate_set(dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);
	
	/* Configure Sensor Hub to write */
  mm_error = lsm6ds3tr_c_sh_cfg_write(dev_ctx, &val);
	
	lsm6ds3tr_c_func_en_set(dev_ctx,PROPERTY_ENABLE);

  /* Enable I2C Master and I2C master Pull Up */
  lsm6ds3tr_c_sh_master_set(dev_ctx, PROPERTY_ENABLE);

  /* Enable accelerometer to trigger Sensor Hub operation */
  lsm6ds3tr_c_xl_data_rate_set(dev_ctx, SENSOR_HUB_HZ);

	
  /* Wait Sensor Hub operation flag set */
  lsm6ds3tr_c_acceleration_raw_get(dev_ctx, data_raw_acceleration.i16bit);
	
	uint16_t timeout = TIMEOUT;
	
  do {
    lsm6ds3tr_c_xl_flag_data_ready_get(dev_ctx, &drdy);
		timeout--;
  } while (!drdy && timeout !=0);
	
//	SEGGER_RTT_printf(0,"write sensor hub timeout_1:%d\n",timeout);
	
	if(timeout == 0)return -1;
	
	timeout = TIMEOUT;

  do
  {
    lsm6ds3tr_c_all_sources_get(dev_ctx,&endop);
		timeout--;
  } while (!endop.func_src1.sensorhub_end_op && timeout!=0);
	
//	SEGGER_RTT_printf(0,"write sensor hub timeout_2:%d\n",timeout);
	
	if(timeout == 0)return -1;

  lsm6ds3tr_c_xl_data_rate_set(dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);

  lsm6ds3tr_c_sh_master_set(dev_ctx, PROPERTY_DISABLE);
	
	lsm6ds3tr_c_func_en_set(dev_ctx,PROPERTY_DISABLE);

  return mm_error;
}
/* API ------------------------------------------------------------------*/
int32_t lsm6ds3tr_c_switch_read_sensor_hub_cx(lsm_device_t dex_num, uint8_t device_addr, uint8_t reg, uint8_t* data,uint16_t len)
{
	switch_dev(dex_num);
	return lsm6ds3tr_c_read_sensor_hub_cx(&dev_ctx, device_addr, reg, data,len);
}
int32_t lsm6ds3_c_switch_write_sensor_hub_cx(lsm_device_t dex_num, uint8_t device_addr, uint8_t reg, uint8_t* data)
{
	switch_dev(dex_num);
	return lsm6ds3_write_sensor_hub_cx(&dev_ctx, device_addr, reg, data);
}

int lsm6ds3tr_c_init(lsm_device_t dex_num)
{
  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;
	
	switch(dex_num)
	{
		case LSM_DEV_FRONT:
		  nrf_gpio_cfg(
			PIN_FRONT_SENSE_POWER,
			NRF_GPIO_PIN_DIR_OUTPUT,
			NRF_GPIO_PIN_INPUT_DISCONNECT,
			NRF_GPIO_PIN_NOPULL,
			NRF_GPIO_PIN_H0H1,
			NRF_GPIO_PIN_NOSENSE);
			//供电
			nrf_gpio_cfg_output(BOARD_SPI0_MISO_IO);
			nrf_gpio_cfg_output(BOARD_SPI0_MOSI_IO);
			nrf_gpio_cfg_output(BOARD_SPI0_CLK_IO);
			nrf_gpio_cfg_output(PIN_FRONT_SPI_nCS);

			nrf_gpio_pin_write(BOARD_SPI0_MISO_IO,0);
			nrf_gpio_pin_write(BOARD_SPI0_MOSI_IO,0);
			nrf_gpio_pin_write(BOARD_SPI0_CLK_IO,0);
			nrf_gpio_pin_write(PIN_FRONT_SPI_nCS,0);
		
			nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
			platform_delay(100);
			nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
			platform_delay(100);
			SPI0_Init();
			break;
		case LSM_DEV_CENTER:
			SPI1_Init();
			break;
	}
	
	switch_dev(dex_num);
	
	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);

	/* 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:%d!!!\r\n",dex_num);
		return -1;
	}
	/* 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:%d - PASS\r\n",dex_num);
  }
  else {
    SEGGER_RTT_printf(0,"Self Test:%d - FAIL\r\n",dex_num);
		return -1;
  }
	return 0;
}

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


//	/* 6ds3 Gyroscope test parameters */
//	config_6ds3_gyro.enable = PROPERTY_ENABLE;
//	config_6ds3_gyro.odr = LSM6DS3TR_C_GY_ODR_104Hz;
//	config_6ds3_gyro.odr_hz_val = 0;
//	config_6ds3_gyro.fs = LSM6DS3TR_C_2000dps;
//	config_6ds3_gyro.decimation = 0;
//	config_6ds3_gyro.samples_num_in_pattern = 0;
//	
//	switch_dev(dex_num);

//	/* 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:%d!!!\r\n",dex_num);
//		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);
//	
//	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL);
//	
//	lsm6ds3tr_c_gy_power_mode_set(&dev_ctx,LSM6DS3TR_C_GY_NORMAL);
//	
//  /* Set Output Data Rate */
//  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, config_6ds3_xl.odr);
//  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, config_6ds3_gyro.odr);
//  /* Set full scale */
//  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, config_6ds3_xl.fs);
//  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, config_6ds3_gyro.fs);
//	
//	platform_delay(20);
//	
//  /* 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(lsm_device_t dex_num, int16_t *acc, int16_t *gry, int16_t *temp)
{
//		switch_dev(dex_num);
//	
//	 /* 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 acc field data */
//      memset( data_raw_acceleration.i16bit, 0x00, 3 * sizeof(int16_t));
//      lsm6ds3tr_c_acceleration_raw_get(&dev_ctx,
//                                        data_raw_acceleration.i16bit);

//			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 gry 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);
//			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);
//			*temp = data_raw_temperature;
//    }
//	
}
void lsm6ds3tr_c_standby_power_acc_mode_config(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
	platform_delay(10);
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
	platform_delay(10);

	/* 6ds3 Accelerometer test parameters */
	config_6ds3_xl.enable = PROPERTY_ENABLE;
	config_6ds3_xl.odr = USED_ACC_FIFO_104_HZ;
	config_6ds3_xl.odr_hz_val = 0;
	config_6ds3_xl.fs = LSM6DS3TR_C_16g;
	config_6ds3_xl.decimation = 0;
	config_6ds3_xl.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_low_power_acc_mode_config error:%d!!!\r\n",dex_num);
		return;
	}
	
	/* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);

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

	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL);
	
	lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, config_6ds3_xl.fs);
	
	lsm6ds3tr_c_gy_sleep_mode_set(&dev_ctx, 1);

}
void lsm6ds3tr_c_standby_power_acc_mode_start(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	/* Set XL Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, config_6ds3_xl.odr);
}
void lsm6ds3tr_c_standby_power_acc_read(lsm_device_t dex_num, int16_t *acc)
{
	switch_dev(dex_num);
	/* 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);
			acc[0] = data_raw_acceleration.i16bit[0];
			acc[1] = data_raw_acceleration.i16bit[1];
			acc[2] = data_raw_acceleration.i16bit[2];
			#if DEBUG_IMU
			SEGGER_RTT_printf(0,"dev:%d h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",dex_num,acc[0],acc[1],acc[2]);
			#endif
		}
}

int16_t lsm6ds3tr_c_fifo_mode_read_group_num(lsm_device_t dex_num, lsm_mode_t fifo_mode)
{
	uint8_t wt;
	uint16_t num = 0;
	uint16_t num_pattern = 0;
	
	switch_dev(dex_num);
	
	/* 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);
//		SEGGER_RTT_printf(0,"---------->dex_num:%d num:%d!!!\r\n",dex_num,num);
		switch(fifo_mode)
		{
			case LSM_STATE_ACC_MODE:
				break;
			case LSM_STATE_ACC_QMC_FIFO_MODE:
				num_pattern = num / acc_mag_timestamp_pattern_len;
				break;
			case LSM_STATE_GRY_ACC_QMC_FIFO_MODE:
				num_pattern = num / gry_acc_mag_timestamp_pattern_len;
				break;
			case LSM_STATE_ILLEGAL_MODE:
				num_pattern = 0;
				break;
		}
	}

	return num_pattern;
}

void lsm6ds3tr_c_acc_sensor_hub_qmc6310_fifo_with_timestamp_mode_config(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
	platform_delay(10);
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
	platform_delay(10);
	
	/* 6ds3 Accelerometer test parameters */
	config_6ds3_xl.enable = PROPERTY_ENABLE;
	config_6ds3_xl.odr = USED_ACC_FIFO_104_HZ;
	config_6ds3_xl.odr_hz_val = 0;
	config_6ds3_xl.fs = LSM6DS3TR_C_16g;
	config_6ds3_xl.decimation = 0;
	config_6ds3_xl.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_acc_sensor_hub_qmc6310_fifo_with_timestamp_mode error:%d!!!\r\n",dex_num);
		return;
	}
	
	/* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
  } while (rst);
	
	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);
	
	/*qmc6310-------------------------------------------------------------*/
	/* Some hardware require to enable pull up on master I2C interface */
	lsm6ds3tr_c_sh_pin_mode_set(&dev_ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
	platform_delay(10);
	
	
	//qmc6310初始化
	switch(dex_num)
	{
		case LSM_DEV_FRONT:
		  if(qmc6310_front_Init(QMC6310_100HZ))SEGGER_RTT_printf(0,"qmc6310_front_Init sucess!!\n");
			else {lsm6ds3tr_c_suspend_mode(dex_num);SEGGER_RTT_printf(0,"qmc6310_front_Init fail!!\n");return;}
			break;
		case LSM_DEV_CENTER:
//			if(qmc6310_center_Init(QMC6310_100HZ))SEGGER_RTT_printf(0,"qmc6310_center_Init sucess!!\n");
//			else {lsm6ds3tr_c_suspend_mode(dex_num);SEGGER_RTT_printf(0,"qmc6310_center_Init fail!!\n");return;}
			break;
	}

	uint8_t data = 0xFF,timeout = 3;
	do{
		lsm6ds3tr_c_sh_slv0_cfg_read(&dev_ctx, &qmc6310);						//这里只有一个地磁，但不能用slv1，只能用slv0，不知道是不是要先配置slv0后，才能配置slv1。
		lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_BANK_A);
		lsm6ds3tr_c_read_reg(&dev_ctx, LSM6DS3TR_C_SLV0_SUBADD,&data, 1);
//		SEGGER_RTT_printf(0,"lsm6ds3tr_c_read_reg LSM6DS3TR_C_SLV0_SUBADD:%x!!!\r\n",data);	
		platform_delay(100);
		lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);
		timeout--;
	}while(data != QMC6310_DATA_OUT_X_LSB_REG && timeout != 0);


	/* Configure Sensor Hub to read two slaves */
  lsm6ds3tr_c_sh_num_of_dev_connected_set(&dev_ctx, LSM6DS3TR_C_SLV_0_1_2_3);

	lsm6ds3tr_c_func_en_set(&dev_ctx,PROPERTY_ENABLE);
	
  lsm6ds3tr_c_sh_master_set(&dev_ctx, PROPERTY_ENABLE);

	/*acc-------------------------------------------------------------*/
	
	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL);
	
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, config_6ds3_xl.fs);
	
	lsm6ds3tr_c_gy_sleep_mode_set(&dev_ctx, 1);
	
	lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&dev_ctx,PROPERTY_ENABLE);	//使能开关
	
	  /* Set High Resolution Timestamp (25 us tick) */
  lsm6ds3tr_c_timestamp_res_set(&dev_ctx, LSM6DS3TR_C_LSB_25us);
	
	  /* Enable timestamp in HW */
  lsm6ds3tr_c_timestamp_set(&dev_ctx, PROPERTY_ENABLE);

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

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

  /* Set ODR FIFO */
  lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, USED_FIFO_104_HZ);//该参数关联到timestamp

  /* Set FIFO sensor decimator */
  lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);//因为acc和fifo都是104HZ，所以不用抽取因子。
	
	lsm6ds3tr_c_fifo_dataset_3_batch_set(&dev_ctx,LSM6DS3TR_C_FIFO_DS3_NO_DEC);//因为mag和fifo都是104HZ，所以不用抽取因子。
	
	lsm6ds3tr_c_fifo_dataset_4_batch_set(&dev_ctx,LSM6DS3TR_C_FIFO_DS4_NO_DEC);//该参数关联到fifo_odr,timestamp和fifo的HZ一致
}

void lsm6ds3tr_c_acc_sensor_hub_qmc6310_fifo_with_timestamp_mode_start(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	/* Set XL Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, config_6ds3_xl.odr);
}

int lsm6ds3tr_c_acc_sensor_hub_qmc6310_fifo_with_timestamp_read(lsm_device_t dex_num, int16_t *acc, int16_t *mag, int32_t *timestamp, int16_t group_num)
{
  uint16_t num_pattern = 0;
	int16_t cur_count = 0;
	
	switch_dev(dex_num);
	
	if(group_num <= 0)return 0;
	
	num_pattern = group_num;

	if (num_pattern)
	{
//		SEGGER_RTT_printf(0,"acc_mag_timestamp_pattern_len:%d num_pattern:%d\r\n",acc_mag_timestamp_pattern_len,num_pattern);

		while (num_pattern-- > 0){
//			SEGGER_RTT_printf(0,"num:%d pattern_len:%d num_pattern:%d mag_num:%d xl_num:%d\r\n",num,pattern_len,num_pattern,mag_num,xl_num);
			
			/* FIFO pattern is composed by gy_num gyroscope triplets and
			 * xl_num accelerometer triplets. The sequence has always following order:
			 * gyro first, accelerometer second , mag third
			 */
				/* Read XL samples */
				if (config_6ds3_xl.enable)
				{
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit,
																		ACC_OUT_XYZ_WORD_SIZE * sizeof(int16_t));
					
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_magnetic.u8bit,
																		MAG_OUT_XYZ_WORD_SIZE * sizeof(int16_t));
					
					/* Read timestamp samples */
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_timestamp.u8bit,TIMESTAMP_OUT_WORD_SIZE * sizeof(int16_t));
					
					if(cur_count != group_num )
					{
						acc[cur_count*3 + 0] = data_raw_acceleration.i16bit[0];
						acc[cur_count*3 + 1] = data_raw_acceleration.i16bit[1];
						acc[cur_count*3 + 2] = data_raw_acceleration.i16bit[2];
						
						
						QMC6310_map c_map;
						switch(dex_num)
						{
							case LSM_DEV_FRONT:
								c_map = qmc6310_front_GetMap();
								break;
							case LSM_DEV_CENTER:
//								c_map = qmc6310_center_GetMap();
								break;
						}

						mag[cur_count*3 + 0] = (int16_t)(c_map.sign[0]*data_raw_magnetic.i16bit[c_map.map[0]]);
						mag[cur_count*3 + 1] = (int16_t)(c_map.sign[0]*data_raw_magnetic.i16bit[c_map.map[1]]);
						mag[cur_count*3 + 2] = (int16_t)(c_map.sign[0]*data_raw_magnetic.i16bit[c_map.map[2]]);
						
						
						/*														时间戳和计步数数据
						
									字节1					字节2				字节3				字节4				字节5				字节6
								TIMESTAMP			TIMESTAMP			-----			TIMESTAMP			STEPS				STEPS
								 [15:8]				 [23:16]									[7:0]				[7:0]				[15:8]
						*/
						
						timestamp[cur_count] = ((data_raw_timestamp.u8bit[1] << 16) | (data_raw_timestamp.u8bit[0] << 8) | (data_raw_timestamp.u8bit[3]))*25;//counter == 25us
						
						cur_count++;
					}
					
				}
		}
	}
	
	return cur_count;
}

void lsm6ds3tr_c_gry_acc_sensor_hub_qmc6310_fifo_with_timestamp_mode_config(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
	platform_delay(10);
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
	platform_delay(10);
	
	/* 6ds3 Accelerometer test parameters */
	config_6ds3_xl.enable = PROPERTY_ENABLE;
	config_6ds3_xl.odr = USED_ACC_FIFO_104_HZ;
	config_6ds3_xl.odr_hz_val = 0;
	config_6ds3_xl.fs = LSM6DS3TR_C_16g;
	config_6ds3_xl.decimation = 0;
	config_6ds3_xl.samples_num_in_pattern = 0;
	
	/* 6ds3 Gyroscope test parameters */
	config_6ds3_gyro.enable = PROPERTY_ENABLE;
	config_6ds3_gyro.odr = USED_GRY_FIFO_104_HZ;
	config_6ds3_gyro.odr_hz_val = 0;
	config_6ds3_gyro.fs = LSM6DS3TR_C_2000dps;
	config_6ds3_gyro.decimation = 0;
	config_6ds3_gyro.samples_num_in_pattern = 0;
	
//	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);

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

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

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

	
	/*qmc6310-------------------------------------------------------------*/
	
	/* Some hardware require to enable pull up on master I2C interface */
	lsm6ds3tr_c_sh_pin_mode_set(&dev_ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
	platform_delay(10);
	
	//qmc6310初始化
	switch(dex_num)
	{
		case LSM_DEV_FRONT:
		  if(qmc6310_front_Init(QMC6310_100HZ))SEGGER_RTT_printf(0,"qmc6310_front_Init sucess!!\n");
			else {lsm6ds3tr_c_suspend_mode(dex_num);SEGGER_RTT_printf(0,"qmc6310_front_Init fail!!\n");return;}
			break;
		case LSM_DEV_CENTER:
//			if(qmc6310_center_Init(QMC6310_100HZ))SEGGER_RTT_printf(0,"qmc6310_center_Init sucess!!\n");
//			else {lsm6ds3tr_c_suspend_mode(dex_num);SEGGER_RTT_printf(0,"qmc6310_center_Init fail!!\n");return;}
			break;
	}
	
	uint8_t data = 0xFF,timeout = 3;
	do{
		lsm6ds3tr_c_sh_slv0_cfg_read(&dev_ctx, &qmc6310);						//这里只有一个地磁，但不能用slv1，只能用slv0，不知道是不是要先配置slv0后，才能配置slv1。
		lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_BANK_A);
		lsm6ds3tr_c_read_reg(&dev_ctx, LSM6DS3TR_C_SLV0_SUBADD,&data, 1);
//		SEGGER_RTT_printf(0,"lsm6ds3tr_c_read_reg LSM6DS3TR_C_SLV0_SUBADD:%x!!!\r\n",data);	
		platform_delay(100);
		lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);
		timeout--;
	}while(data != QMC6310_DATA_OUT_X_LSB_REG && timeout != 0);

	/* Configure Sensor Hub to read two slaves */
  lsm6ds3tr_c_sh_num_of_dev_connected_set(&dev_ctx, LSM6DS3TR_C_SLV_0_1_2_3);

	lsm6ds3tr_c_func_en_set(&dev_ctx,PROPERTY_ENABLE);
	
  lsm6ds3tr_c_sh_master_set(&dev_ctx, PROPERTY_ENABLE);

	/*acc-------------------------------------------------------------*/
	
	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL);
	
	lsm6ds3tr_c_gy_power_mode_set(&dev_ctx,LSM6DS3TR_C_GY_NORMAL);
	
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, config_6ds3_xl.fs);
	
	lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, config_6ds3_gyro.fs);
	
	lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&dev_ctx,PROPERTY_ENABLE);	//使能开关
	
	  /* Set High Resolution Timestamp (25 us tick) */
  lsm6ds3tr_c_timestamp_res_set(&dev_ctx, LSM6DS3TR_C_LSB_25us);
	
	  /* Enable timestamp in HW */
  lsm6ds3tr_c_timestamp_set(&dev_ctx, PROPERTY_ENABLE);
	
//	lsm6ds3tr_c_fifo_write_trigger_set(&dev_ctx,LSM6DS3TR_C_TRG_XL_GY_DRDY);

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

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

  /* Set ODR FIFO */
  lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, USED_FIFO_104_HZ);//该参数关联到timestamp

  /* Set FIFO sensor decimator */
  lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);//acc和fifo的HZ一致
	
	lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_GY_NO_DEC);//gry和fifo的HZ一致
	
	lsm6ds3tr_c_fifo_dataset_3_batch_set(&dev_ctx,LSM6DS3TR_C_FIFO_DS3_NO_DEC);//mag和fifo的HZ一致
	
	lsm6ds3tr_c_fifo_dataset_4_batch_set(&dev_ctx,LSM6DS3TR_C_FIFO_DS4_NO_DEC);//该参数关联到fifo_odr,timestamp和fifo的HZ一致

}

void lsm6ds3tr_c_gry_acc_sensor_hub_qmc6310_fifo_with_timestamp_mode_start(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	/* Set XL Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, config_6ds3_xl.odr);
	lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, config_6ds3_gyro.odr);
}

int lsm6ds3tr_c_gry_acc_sensor_hub_qmc6310_fifo_with_timestamp_read(lsm_device_t dex_num, int16_t *gry, int16_t *acc, int16_t *mag, int32_t *timestamp, int16_t group_num)
{
  uint16_t num_pattern = 0;
	int16_t cur_count = 0;
	
	switch_dev(dex_num);
	
	if(group_num <= 0)return 0;
	
	num_pattern = group_num;

	if (num_pattern)
	{
//		SEGGER_RTT_printf(0,"gry_acc_mag_timestamp_pattern_len:%d num_pattern:%d\r\n",gry_acc_mag_timestamp_pattern_len,num_pattern);

		while (num_pattern-- > 0){
//			SEGGER_RTT_printf(0,"num:%d pattern_len:%d num_pattern:%d mag_num:%d xl_num:%d\r\n",num,pattern_len,num_pattern,mag_num,xl_num);
			
			/* FIFO pattern is composed by gy_num gyroscope triplets and
			 * xl_num accelerometer triplets. The sequence has always following order:
			 * gyro first, accelerometer second , mag third , timestamp fourth
			 */
				
				if (config_6ds3_gyro.enable && config_6ds3_xl.enable)
				{
					/* Read gyro samples */
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_angular_rate.u8bit,
																		GRY_OUT_XYZ_WORD_SIZE * sizeof(int16_t));
					/* Read XL samples */
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit,
																		ACC_OUT_XYZ_WORD_SIZE * sizeof(int16_t));
					/* Read mag samples */
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_magnetic.u8bit,
																		MAG_OUT_XYZ_WORD_SIZE * sizeof(int16_t));
					/* Read timestamp samples */
					lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_timestamp.u8bit,TIMESTAMP_OUT_WORD_SIZE * sizeof(int16_t));
					
					
					if(cur_count != group_num)
					{
						gry[cur_count*3 + 0] = data_raw_angular_rate.i16bit[0];
						gry[cur_count*3 + 1] = data_raw_angular_rate.i16bit[1];
						gry[cur_count*3 + 2] = data_raw_angular_rate.i16bit[2];
					
						acc[cur_count*3 + 0] = data_raw_acceleration.i16bit[0];
						acc[cur_count*3 + 1] = data_raw_acceleration.i16bit[1];
						acc[cur_count*3 + 2] = data_raw_acceleration.i16bit[2];
						
						QMC6310_map c_map;
						switch(dex_num)
						{
							case LSM_DEV_FRONT:
								c_map = qmc6310_front_GetMap();
								break;
							case LSM_DEV_CENTER:
//								c_map = qmc6310_center_GetMap();
								break;
						}

						mag[cur_count*3 + 0] = (int16_t)(c_map.sign[0]*data_raw_magnetic.i16bit[c_map.map[0]]);
						mag[cur_count*3 + 1] = (int16_t)(c_map.sign[0]*data_raw_magnetic.i16bit[c_map.map[1]]);
						mag[cur_count*3 + 2] = (int16_t)(c_map.sign[0]*data_raw_magnetic.i16bit[c_map.map[2]]);
						
						/*														时间戳和计步数数据
						
									字节1					字节2				字节3				字节4				字节5				字节6
								TIMESTAMP			TIMESTAMP			-----			TIMESTAMP			STEPS				STEPS
								 [15:8]				 [23:16]									[7:0]				[7:0]				[15:8]
						*/
						
						timestamp[cur_count] = ((data_raw_timestamp.u8bit[1] << 16) | (data_raw_timestamp.u8bit[0] << 8) | (data_raw_timestamp.u8bit[3]))*25;//counter == 25us
						
						cur_count++;
					}
				}
		}
	}
	
	return cur_count;
}
void lsm6ds3tr_c_get_mode_stat(lsm_device_t dex_num, uint8_t *stat)
{
	switch_dev(dex_num);
	
	int16_t	lsm_acc[3]={0,0,0}, lsm_gry[3] = {0,0,0}, mag[3] = {0,0,0};
	lsm6ds3tr_c_odr_xl_t 	acc_odr;
	lsm6ds3tr_c_odr_g_t 	gry_odr;
	uint8_t reg_val = 0;
	int32_t ret,timestamp;
	uint16_t timeout = TIMEOUT;
	int16_t fifo_num = 0;
	
	*stat = LSM_STATE_ILLEGAL_MODE;

  ret = lsm6ds3tr_c_read_reg(&dev_ctx, LSM6DS3TR_C_FIFO_CTRL5,
                             &reg_val, 1);
	
	if(ret == 0 && (reg_val & 0x78)!=0)		//fifo 开启
	{
		lsm6ds3tr_c_gy_sleep_mode_get(&dev_ctx, &reg_val);
		
		switch(reg_val)
		{
			case 1:						// gry关闭
				lsm6ds3tr_c_xl_data_rate_get(&dev_ctx, &acc_odr);
				if(acc_odr != USED_ACC_FIFO_104_HZ){
					*stat = LSM_STATE_ILLEGAL_MODE;
				}else{
					do{
						fifo_num = lsm6ds3tr_c_fifo_mode_read_group_num(dex_num,LSM_STATE_ACC_QMC_FIFO_MODE);
						timeout--;
					}while(fifo_num == 0 && timeout != 0);
					while(fifo_num--)lsm6ds3tr_c_acc_sensor_hub_qmc6310_fifo_with_timestamp_read(dex_num, lsm_acc, mag, &timestamp, 1);
					if((lsm_acc[0] == 0 && lsm_acc[1] == 0 && lsm_acc[2] == 0) || (mag[0] == 0 && mag[1] == 0 && mag[2] == 0) || timestamp == 0){
						*stat = LSM_STATE_ILLEGAL_MODE;
					}else{
						*stat = LSM_STATE_ACC_QMC_FIFO_MODE;
//						SEGGER_RTT_printf(0,"LSM_STATE_ACC_QMC_FIFO_MODE dex_num:%d fifo_num:%d timeout:%d\n",dex_num,fifo_num,timeout);
					}
				}
			break;
				
			case 0:						// gry 开启
				lsm6ds3tr_c_xl_data_rate_get(&dev_ctx, &acc_odr);
				lsm6ds3tr_c_gy_data_rate_get(&dev_ctx,&gry_odr);
				if(acc_odr != USED_ACC_FIFO_104_HZ || gry_odr != USED_GRY_FIFO_104_HZ){
					*stat = LSM_STATE_ILLEGAL_MODE;
				}else{
					do{
						fifo_num = lsm6ds3tr_c_fifo_mode_read_group_num(dex_num,LSM_STATE_GRY_ACC_QMC_FIFO_MODE);
						timeout--;
					}while(fifo_num == 0 && timeout != 0);
					while(fifo_num--)lsm6ds3tr_c_gry_acc_sensor_hub_qmc6310_fifo_with_timestamp_read(dex_num, lsm_gry, lsm_acc, mag, &timestamp, 1);
					if((lsm_acc[0] == 0 && lsm_acc[1] == 0 && lsm_acc[2] == 0) || (mag[0] == 0 && mag[1] == 0 && mag[2] == 0) || (lsm_gry[0] == 0 && lsm_gry[1] == 0 && lsm_gry[2] == 0) || timestamp == 0){
						*stat = LSM_STATE_ILLEGAL_MODE;
					}else{
						*stat = LSM_STATE_GRY_ACC_QMC_FIFO_MODE;
//						SEGGER_RTT_printf(0,"LSM_STATE_GRY_ACC_QMC_FIFO_MODE dex_num:%d fifo_num:%d timeout:%d\n",dex_num,fifo_num,timeout);
					}
				}
			break;
		}
	}
	else	//fifo 关闭
	{
		lsm6ds3tr_c_gy_sleep_mode_get(&dev_ctx, &reg_val);
		if(reg_val){				// gry关闭
			lsm6ds3tr_c_xl_data_rate_get(&dev_ctx, &acc_odr);
			if(acc_odr != USED_ACC_FIFO_104_HZ){
					*stat = LSM_STATE_ILLEGAL_MODE;
			}else{
				lsm6ds3tr_c_standby_power_acc_read(dex_num, lsm_acc);
				if(lsm_acc[0] == 0 && lsm_acc[1] == 0 && lsm_acc[2] == 0){
					*stat = LSM_STATE_ILLEGAL_MODE;
				}else{
					*stat = LSM_STATE_ACC_MODE;
				}
			}
		}else{
			*stat = LSM_STATE_ILLEGAL_MODE;
		}
	}

}
void lsm6ds3tr_c_powerdown_mode(lsm_device_t dex_num)
{
	switch_dev(dex_num);
	
	//关闭qmc6310
	switch(dex_num)
	{
		case LSM_DEV_FRONT:
		  if(qmc6310_front_Suspend())SEGGER_RTT_printf(0,"qmc6310_front_Suspend sucess!!\n");
			break;
		case LSM_DEV_CENTER:
//			if(qmc6310_center_Suspend())SEGGER_RTT_printf(0,"qmc6310_center_Suspend sucess!!\n");
			break;
	}
	
	lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);
	lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_OFF);
	
	switch(dex_num)
	{
		case LSM_DEV_FRONT:
			SPI0_Disable();
			nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
			platform_delay(100);
			break;
		case LSM_DEV_CENTER:
			SPI1_Disable();
			break;
	}
}

void lsm6ds3tr_c_suspend_mode(lsm_device_t dex_num)
{
	uint16_t 										timeout = TIMEOUT;
	lsm6ds3tr_c_fifo_mode_t 		fifo_mode;
	lsm6ds3tr_c_odr_xl_t 				acc_odr;
	lsm6ds3tr_c_odr_g_t 				gry_odr;
	
	switch_dev(dex_num);

//	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);
//	
//	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_BANK_A);
//	
//	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_BANK_B);
	
//	/* 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);

				
				
	
	do{
		lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_OFF);
		lsm6ds3tr_c_xl_data_rate_get(&dev_ctx, &acc_odr);
		timeout--;
	}while(acc_odr != LSM6DS3TR_C_XL_ODR_OFF && timeout != 0);
//	if(acc_odr == LSM6DS3TR_C_XL_ODR_OFF && timeout != 0)SEGGER_RTT_printf(0,"acc_odr == LSM6DS3TR_C_XL_ODR_OFF %d sucess!!\n",dex_num);

	timeout = TIMEOUT;
	
	do{
		lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_OFF);
		lsm6ds3tr_c_gy_data_rate_get(&dev_ctx,&gry_odr);
		timeout--;
	}while(gry_odr != LSM6DS3TR_C_GY_ODR_OFF && timeout != 0);
//	if(gry_odr == LSM6DS3TR_C_GY_ODR_OFF && timeout != 0)SEGGER_RTT_printf(0,"gry_odr == LSM6DS3TR_C_GY_ODR_OFF %d sucess!!\n",dex_num);
	
//	lsm6ds3tr_c_mem_bank_set(&dev_ctx, LSM6DS3TR_C_USER_BANK);
//	
//	lsm6ds3tr_c_sh_master_set(&dev_ctx, PROPERTY_DISABLE);
//	
//	lsm6ds3tr_c_func_en_set(&dev_ctx,PROPERTY_DISABLE);
	
	timeout = TIMEOUT;

	//设置旁路模式用于清空FIFO
	do{
		lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE);
		lsm6ds3tr_c_fifo_mode_get(&dev_ctx,&fifo_mode);
		timeout--;
	}while(fifo_mode != LSM6DS3TR_C_BYPASS_MODE && timeout != 0);
//	if(fifo_mode == LSM6DS3TR_C_BYPASS_MODE && timeout != 0)SEGGER_RTT_printf(0,"lsm6ds3tr_c_suspend_mode %d sucess!!\n",dex_num);
	//设置为旁路模式后，在设置为不同的FIFO工作模式之前，必须要等待至少30us
	platform_delay(10);
}