/*Includes ----------------------------------------------*/
#include "bsp_spi.h"
#include "nrf_delay.h"
#include "drv_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		 OP_TIMEOUT											200													//最大操作次数

#define		 ACC_OUT_XYZ_WORD_NUM    				3														//加速度三轴数据的字个数

#define		 GRY_OUT_XYZ_WORD_NUM    				3														//陀螺仪三轴数据的字个数

#define		 MAG_OUT_XYZ_WORD_NUM    				3														//地磁计三轴数据的字个数

#define		 TIMESTAMP_OUT_WORD_NUM    			3														//FIFO时间戳数据的字个数

#define 	 QMC6310_DATA_OUT_X_LSB_REG			0x01												//qmc6310地磁数据的寄存器

#define 	 HUB_MAG_DEV_ADDR								0x1C												//挂载的地磁设备地址	

/*STRUCTION -----------------------------------------------------*/
typedef union
{
  int16_t i16bit[3];
  uint8_t u8bit[6];	
	
} axis3bit16_u;
typedef struct drv_lsm6ds3tr_c
{
	stmdev_ctx_t							ctx;																			//抽象层接口（如I2C/SPI，外部不可操作，属于私有成员）
	
	lsm6ds3tr_c_emb_sh_read_t raw_data;																	//用于hub读取数据（外部不可操作，属于私有成员）	
	
	lsm6ds3tr_c_all_sources_t endop;																		//用于hub判断是否操作完成
	
	uint32_t									op_timeout;																//最大操作次数
	
	lsm_data_t								cur_data;																	//当前LSM数据
	
	drv_lsm_config_param_t 		cur_param;																//当前LSM配置
	
} Drv_Lsm6ds3tr_c_t;

/*Local Variable ----------------------------------------------*/
static lsm6ds3tr_c_sh_cfg_read_t sub_qmc6310 = {
	.slv_add 		= 		HUB_MAG_DEV_ADDR,																	//挂载的qmc6310设备地址										
	.slv_subadd = 		QMC6310_DATA_OUT_X_LSB_REG,												//预备读取qmc6310地磁数据的寄存器
	.slv_len 		= 		MAG_OUT_XYZ_WORD_NUM * 2,													//转为字节长度
};

static Drv_Lsm6ds3tr_c_t 	ob_lsm6ds3tr_c;
static axis3bit16_u 			data_raw_acceleration;											//临时存储加速度原始数据
static axis3bit16_u 			data_raw_angular_rate;											//临时存储陀螺仪原始数据
static axis3bit16_u 			data_raw_magnetic;													//临时存储地磁计原始数据
static axis3bit16_u 			data_raw_timestamp;													//临时存储时间戳原始数据
/*Local Functions ----------------------------------------------*/
static void platform_delay_ms(uint32_t ms)
{
	nrf_delay_ms(ms);
}
static int32_t platform_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_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 int read_hub(uint8_t device_addr, uint8_t reg, uint8_t* data,uint16_t len)
{
  int32_t 		ret = -1;
  uint8_t 		drdy;
	static int 	flow_ctl = 0;		//流程控制
	
	
  lsm6ds3tr_c_sh_cfg_read_t val = {
    .slv_add = device_addr,										
    .slv_subadd = reg,
    .slv_len = len,
  };

	switch(flow_ctl)
	{
		case 0:
			/* Disable accelerometer */
			lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
			
			lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
			
			/* Configure Sensor Hub to read QMC6310 */
			/* Prepare sensor hub to read data from external Slave0 */
			lsm6ds3tr_c_sh_slv0_cfg_read(&ob_lsm6ds3tr_c.ctx, &val);
			
			/* Configure Sensor Hub to read two slaves */
			lsm6ds3tr_c_sh_num_of_dev_connected_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_SLV_0);		
			
			lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_ENABLE);

			/* Enable I2C Master and I2C master Pull Up */
			/* Enable master and XL trigger */
			lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);	
			
			/* Wait Sensor Hub operation flag set */
			lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, data_raw_acceleration.i16bit);	

			/* Enable accelerometer to trigger Sensor Hub operation */
			lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_104Hz);
			
			/* Wait Sensor Hub operation flag set */
			lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, data_raw_acceleration.i16bit);
			
			flow_ctl = 1;
			break;
		
		case 1:
			lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
			if(drdy == 0)return -1;
			
			flow_ctl = 2;
			break;
		
		case 2:
			lsm6ds3tr_c_all_sources_get(&ob_lsm6ds3tr_c.ctx,&ob_lsm6ds3tr_c.endop);
		
			if(ob_lsm6ds3tr_c.endop.func_src1.sensorhub_end_op == 0)return -1;
		
			flow_ctl = 3;
			break;
		
		case 3:
			lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
			
			lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);

			lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
			
			lsm6ds3tr_c_sh_read_data_raw_get(&ob_lsm6ds3tr_c.ctx, &ob_lsm6ds3tr_c.raw_data);
			
			memcpy(data,&ob_lsm6ds3tr_c.raw_data,len);
			
			flow_ctl = 0;
			ret = 0;
			break;
	}

  return ret;
}
static int write_hub(uint8_t device_addr, uint8_t reg, uint8_t* data)
{
  int32_t 		ret = -1;
  uint8_t 		drdy;
	static int 	flow_ctl = 0;		//流程控制
	
	
  lsm6ds3tr_c_sh_cfg_write_t val = {
    .slv0_add = device_addr,
    .slv0_subadd = reg,
    .slv0_data = *data,
  };
	
	switch(flow_ctl)
	{
		case 0:
			/* Disable accelerometer */
			lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
			
			lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
			
			/* Configure Sensor Hub to write */
			lsm6ds3tr_c_sh_cfg_write(&ob_lsm6ds3tr_c.ctx, &val);
			
			lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_ENABLE);

			/* Enable I2C Master and I2C master Pull Up */
			lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
			
				lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, data_raw_acceleration.i16bit);

			/* Enable accelerometer to trigger Sensor Hub operation */
			lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_208Hz);

			
			/* Wait Sensor Hub operation flag set */
			lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, data_raw_acceleration.i16bit);
			
			flow_ctl = 1;
			break;
		
		case 1:
			lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
			if(drdy == 0)return -1;
			
			flow_ctl = 2;
			break;
		
		case 2:
			lsm6ds3tr_c_all_sources_get(&ob_lsm6ds3tr_c.ctx,&ob_lsm6ds3tr_c.endop);
		
			if(ob_lsm6ds3tr_c.endop.func_src1.sensorhub_end_op == 0)return -1;
		
			flow_ctl = 3;
			break;
		
		case 3:
			lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
			
			lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);

			lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
		
			flow_ctl = 0;
			ret = 0;		
			break;
	}

  return ret;
}
static int self_check(void)
{
	uint8_t	whoamI, rst;
	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;
	
	lsm6ds3tr_c_mem_bank_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_USER_BANK);
	/* Wait sensor boot time */
  platform_delay_ms(BOOT_TIME);	
  /* Check device ID */
  lsm6ds3tr_c_device_id_get(&ob_lsm6ds3tr_c.ctx, &whoamI);
	if(whoamI != LSM6DS3TR_C_ID)
	{
		SEGGER_RTT_printf(0,"LSM self_check error!!!\r\n");
		return -1;
	}
	/* Restore default configuration */
  lsm6ds3tr_c_reset_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
	ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
	do {
    lsm6ds3tr_c_reset_get(&ob_lsm6ds3tr_c.ctx, &rst);
		ob_lsm6ds3tr_c.op_timeout--;
  } while(rst && ob_lsm6ds3tr_c.op_timeout!=0);
	if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;	
	/* Accelerometer Self Test */
	/* Set Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_52Hz);
  /* Set full scale */
  lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_4g);
  /* Wait stable output */
  platform_delay_ms(WAIT_TIME_A);
	/* Check if new value available */
	ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
  do {
    lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
		ob_lsm6ds3tr_c.op_timeout--;
  } while(!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
	if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
  /* Read dummy data and discard it */
  lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.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 */
		ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
    do {
      lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
			ob_lsm6ds3tr_c.op_timeout--;
    } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
		if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.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(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ST_NEGATIVE);	
  /* Wait stable output */
  platform_delay_ms(WAIT_TIME_A);
  /* Check if new value available */
	ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
  do {
    lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
		ob_lsm6ds3tr_c.op_timeout--;
  } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
	if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;	
  /* Read dummy data and discard it */
  lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.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 */
		ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
    do {
      lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
			ob_lsm6ds3tr_c.op_timeout--;
    } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
		if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.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(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ST_DISABLE);
  /* Disable sensor. */
  lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
	/* Gyroscope Self Test. */
  /* Set Output Data Rate */
  lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_208Hz);
  /* Set full scale */
  lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2000dps);
  /* Wait stable output */
  platform_delay_ms(WAIT_TIME_G_01);
  /* Check if new value available */
	ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
  do {
    lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
		ob_lsm6ds3tr_c.op_timeout--;
  } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
	if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
  /* Read dummy data and discard it */
  lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.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 */
		ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
    do {
      lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
			ob_lsm6ds3tr_c.op_timeout--;
    } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
		if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.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(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ST_POSITIVE);
  /* Wait stable output */
  platform_delay_ms(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 */
		ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
    do {
      lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
			ob_lsm6ds3tr_c.op_timeout--;
    } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
		if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;

    /* Read data and accumulate the mg value */
    lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.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(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ST_DISABLE);	
  /* Disable sensor. */
  lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);	
  if (st_result == ST_PASS) {
    SEGGER_RTT_printf(0,"LSM Self Test - PASS\r\n");
  }else{
    SEGGER_RTT_printf(0,"Self Test - FAIL\r\n");
		return -1;
  }	
	return 0;
}
/*API ----------------------------------------------*/
/**
	@brief 		初始化LSM驱动
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_Init(void)
{
	//初始化结构体
	memset(&ob_lsm6ds3tr_c.cur_data,0,sizeof(ob_lsm6ds3tr_c.cur_data));
	ob_lsm6ds3tr_c.cur_param.acc_fs 								= LSM_ACC_FS_2G;
	ob_lsm6ds3tr_c.cur_param.acc_odr 								= LSM_ACC_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.gry_fs									=	LSM_GRY_FS_250DPS;
	ob_lsm6ds3tr_c.cur_param.gry_odr								= LSM_GRY_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.mag_fs									=	LSM_MAG_FS_30GS;
	ob_lsm6ds3tr_c.cur_param.mag_odr								=	LSM_MAG_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.fifo_odr								=	LSM_FIFO_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.timestamp_switch 			= LSM_TIMESTAMP_OFF;
	ob_lsm6ds3tr_c.cur_param.timestamp_resolution		=	LSM_TIMESTAMP_6MS4;
	ob_lsm6ds3tr_c.cur_param.acc_power_mode					=	LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
	ob_lsm6ds3tr_c.cur_param.gry_power_mode					=	LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
	ob_lsm6ds3tr_c.ctx.read_reg 										=	platform_read;
	ob_lsm6ds3tr_c.ctx.write_reg										=	platform_write;
	//供电
	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_ms(3);
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
	platform_delay_ms(3);	
	//初始化SPI接口
	SPI0_Init();
	//开始自检
	return self_check();	
}
/**
	@brief 		LSM挂起
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_suspend(void)
{
	//关闭SPI
	SPI0_Disable();
	//断电，清空lsm配置
	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_ms(5);
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
	platform_delay_ms(5);
	//初始化结构体
	memset(&ob_lsm6ds3tr_c.cur_data,0,sizeof(ob_lsm6ds3tr_c.cur_data));
	ob_lsm6ds3tr_c.cur_param.acc_fs 								= LSM_ACC_FS_2G;
	ob_lsm6ds3tr_c.cur_param.acc_odr 								= LSM_ACC_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.gry_fs									=	LSM_GRY_FS_250DPS;
	ob_lsm6ds3tr_c.cur_param.gry_odr								= LSM_GRY_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.mag_fs									=	LSM_MAG_FS_30GS;
	ob_lsm6ds3tr_c.cur_param.mag_odr								=	LSM_MAG_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.fifo_odr								=	LSM_FIFO_ODR_OFF;
	ob_lsm6ds3tr_c.cur_param.timestamp_switch 			= LSM_TIMESTAMP_OFF;
	ob_lsm6ds3tr_c.cur_param.timestamp_resolution		=	LSM_TIMESTAMP_6MS4;
	ob_lsm6ds3tr_c.cur_param.acc_power_mode					=	LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
	ob_lsm6ds3tr_c.cur_param.gry_power_mode					=	LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;	
	//重新初始化SPI
	SPI0_Init();
	
	return 0;
}
/**
	@brief 		设置加速度工作模式
	@param 		acc_power_mode 				- [in]	加速度工作模式
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_acc_power_mode(LSM_ACC_POWER_MODE_e acc_power_mode)
{
	lsm6ds3tr_c_xl_hm_mode_t val;
	
	if(ob_lsm6ds3tr_c.cur_param.acc_power_mode != acc_power_mode)
	{
		switch(acc_power_mode)
		{
			case LSM_ACC_POWER_MODE_HIGH_PERFORMANCE:
				lsm6ds3tr_c_xl_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_XL_HIGH_PERFORMANCE);
			
				lsm6ds3tr_c_xl_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);

				if(val != LSM6DS3TR_C_XL_HIGH_PERFORMANCE)return -1;
				
				ob_lsm6ds3tr_c.cur_param.acc_power_mode	=	LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
				break;
			case LSM_ACC_POWER_MODE_NORMAL:
				lsm6ds3tr_c_xl_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_XL_NORMAL);
			
				lsm6ds3tr_c_xl_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_XL_NORMAL)return -1;
			
				ob_lsm6ds3tr_c.cur_param.acc_power_mode	=	LSM_ACC_POWER_MODE_NORMAL;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置陀螺仪工作模式
	@param 		gry_power_mode 				- [in]	陀螺仪工作模式
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_gry_power_mode(LSM_GRY_POWER_MODE_e gry_power_mode)
{
	lsm6ds3tr_c_g_hm_mode_t val;
	
	if(ob_lsm6ds3tr_c.cur_param.gry_power_mode != gry_power_mode)
	{
		switch(gry_power_mode)
		{
			case LSM_GRY_POWER_MODE_HIGH_PERFORMANCE:
				lsm6ds3tr_c_gy_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_GY_HIGH_PERFORMANCE);
			
				lsm6ds3tr_c_gy_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM_GRY_POWER_MODE_HIGH_PERFORMANCE)return -1;
			
				ob_lsm6ds3tr_c.cur_param.gry_power_mode	=	LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
				break;
			case LSM_GRY_POWER_MODE_NORMAL:
				lsm6ds3tr_c_gy_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_GY_NORMAL);
			
				lsm6ds3tr_c_gy_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM_GRY_POWER_MODE_NORMAL)return -1;
			
				ob_lsm6ds3tr_c.cur_param.gry_power_mode	=	LSM_GRY_POWER_MODE_NORMAL;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置时间戳精度
	@param 		timestamp_resolution 	- [in]	时间戳精度
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_timestamp_resolution(LSM_TIMESTAMP_RESOLUTION_e timestamp_resolution)
{
	lsm6ds3tr_c_timer_hr_t val;
	
	if(ob_lsm6ds3tr_c.cur_param.timestamp_resolution != timestamp_resolution)
	{
		switch(timestamp_resolution)
		{
			case LSM_TIMESTAMP_6MS4:
				/* Set High Resolution Timestamp (6.4ms) */
				lsm6ds3tr_c_timestamp_res_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_LSB_6ms4);
			
				lsm6ds3tr_c_timestamp_res_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_LSB_6ms4)return -1;
			
				ob_lsm6ds3tr_c.cur_param.timestamp_resolution = LSM_TIMESTAMP_6MS4;
				break;
			case LSM_TIMESTAMP_25US:
				/* Set High Resolution Timestamp (25 us tick) */
				lsm6ds3tr_c_timestamp_res_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_LSB_25us);
			
				lsm6ds3tr_c_timestamp_res_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_LSB_25us)return -1;
			
				ob_lsm6ds3tr_c.cur_param.timestamp_resolution = LSM_TIMESTAMP_25US;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置时间戳开关
	@param 		timestamp_switch 			- [in]	时间戳开关
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_timestamp_switch(LSM_TIMESTAMP_SWITCH_e timestamp_switch)
{
	uint8_t val;
	
	if(ob_lsm6ds3tr_c.cur_param.timestamp_switch != timestamp_switch)
	{
		switch(timestamp_switch)
		{
			case LSM_TIMESTAMP_ON:
				/* Enable timestamp in HW */
				lsm6ds3tr_c_timestamp_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
			
				lsm6ds3tr_c_timestamp_get(&ob_lsm6ds3tr_c.ctx, &val);
			
				if(val != PROPERTY_ENABLE)return -1;
			
				ob_lsm6ds3tr_c.cur_param.timestamp_switch = LSM_TIMESTAMP_ON;
				break;
			case LSM_TIMESTAMP_OFF:
				/*Disable timestamp in HW */
				lsm6ds3tr_c_timestamp_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);
			
				lsm6ds3tr_c_timestamp_get(&ob_lsm6ds3tr_c.ctx, &val);
			
				if(val != PROPERTY_DISABLE)return -1;			
			
				ob_lsm6ds3tr_c.cur_param.timestamp_switch = LSM_TIMESTAMP_OFF;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置FIFO采样频率
	@param 		fifo_odr 							- [in]	FIFO采样频率
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_fifo_odr(LSM_FIFO_ODR_e fifo_odr)
{
	uint8_t 											val;
	lsm6ds3tr_c_odr_fifo_t 				r_fifo_odr;
	lsm6ds3tr_c_fifo_mode_t 			r_fifo_mode;
	lsm6ds3tr_c_dec_fifo_xl_t 		r_fifo_xl;
	lsm6ds3tr_c_dec_fifo_gyro_t 	r_fifo_gy;
	lsm6ds3tr_c_dec_ds3_fifo_t 		r_fifo_ds3;
	lsm6ds3tr_c_dec_ds4_fifo_t 		r_fifo_ds4;
	uint16_t 											fifo_watermark;
	uint16_t 											pattern_len = 0;
	
	if(ob_lsm6ds3tr_c.cur_param.fifo_odr != fifo_odr)
	{
		switch(fifo_odr)
		{
			case LSM_FIFO_ODR_OFF:
				//禁用步进计数器和时间戳数据作为第4步FIFO数据集
				lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
			
				lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != PROPERTY_DISABLE)return -1;
			
				/* Set ODR FIFO */
				lsm6ds3tr_c_fifo_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_DISABLE);
			
				lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr);
			
				if(r_fifo_odr != LSM6DS3TR_C_FIFO_DISABLE) return -1;
			
				ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_OFF;
				break;
			case LSM_FIFO_ODR_104HZ:
				//启用步进计数器和时间戳数据作为第4步FIFO数据集
				lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_ENABLE);	
			
				lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != PROPERTY_ENABLE)return -1;
			
				/* Set FIFO mode to Stream mode */
				lsm6ds3tr_c_fifo_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_STREAM_MODE);
			
				lsm6ds3tr_c_fifo_mode_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_mode);
			
				if(r_fifo_mode != LSM6DS3TR_C_STREAM_MODE)return -1;
			
				/* Set FIFO sensor decimator */
				if(ob_lsm6ds3tr_c.cur_param.acc_odr == LSM_ACC_ODR_104HZ)
				{
					lsm6ds3tr_c_fifo_xl_batch_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);//acc和fifo的HZ一致
					
					lsm6ds3tr_c_fifo_xl_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_xl);
					
					if(r_fifo_xl != LSM6DS3TR_C_FIFO_XL_NO_DEC)return -1;
					
					pattern_len += ACC_OUT_XYZ_WORD_NUM;
				}
				
				if(ob_lsm6ds3tr_c.cur_param.gry_odr == LSM_GRY_ODR_104HZ)
				{
					lsm6ds3tr_c_fifo_gy_batch_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_GY_NO_DEC);//gry和fifo的HZ一致
					
					lsm6ds3tr_c_fifo_gy_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_gy);	
					
					if(r_fifo_gy != LSM6DS3TR_C_FIFO_GY_NO_DEC)return -1;
					
					pattern_len += GRY_OUT_XYZ_WORD_NUM;
				}
				
				if(ob_lsm6ds3tr_c.cur_param.mag_odr == LSM_MAG_ODR_100HZ)
				{
					lsm6ds3tr_c_fifo_dataset_3_batch_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_FIFO_DS3_NO_DEC);//mag和fifo的HZ一致
					
					lsm6ds3tr_c_fifo_dataset_3_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_ds3);	
					
					if(r_fifo_ds3 != LSM6DS3TR_C_FIFO_DS3_NO_DEC)return -1;
					
					pattern_len += MAG_OUT_XYZ_WORD_NUM;
				}
				
				if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON)
				{
					lsm6ds3tr_c_fifo_dataset_4_batch_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_FIFO_DS4_NO_DEC);//该参数关联到fifo_odr,timestamp和fifo的HZ一致
					
					lsm6ds3tr_c_fifo_dataset_4_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_ds4);	
					
					if(r_fifo_ds4 != LSM6DS3TR_C_FIFO_DS4_NO_DEC)return -1;
					
					pattern_len += TIMESTAMP_OUT_WORD_NUM;
				}
				
				/* Set FIFO watermark to a multiple of a pattern */
				lsm6ds3tr_c_fifo_watermark_set(&ob_lsm6ds3tr_c.ctx, pattern_len);
				
				lsm6ds3tr_c_fifo_watermark_get(&ob_lsm6ds3tr_c.ctx,&fifo_watermark);
				
				if(fifo_watermark != pattern_len)return -1;
				
				
				/* Set ODR FIFO */
				lsm6ds3tr_c_fifo_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_104Hz);//该参数关联到timestamp		
				
				lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr);
				
				if(r_fifo_odr != LSM6DS3TR_C_FIFO_104Hz) return -1;
				
				ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_104HZ;
				break;
		}	
	}
	
	return 0;
}
/**
	@brief 		设置ACC量程
	@param 		acc_fs 								- [in]	ACC量程
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_acc_fs(LSM_ACC_FS_e acc_fs)
{
	lsm6ds3tr_c_fs_xl_t	val;
	
	if(ob_lsm6ds3tr_c.cur_param.acc_fs != acc_fs)
	{
		switch(acc_fs)
		{
			case LSM_ACC_FS_2G:
				lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2g);
			
				lsm6ds3tr_c_xl_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_2g)return -1;
			
				ob_lsm6ds3tr_c.cur_param.acc_fs = LSM_ACC_FS_2G;
				break;
			case LSM_ACC_FS_16G:
				lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_16g);
			
				lsm6ds3tr_c_xl_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_16g)return -1;
			
				ob_lsm6ds3tr_c.cur_param.acc_fs = LSM_ACC_FS_16G;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置ACC采样频率
	@param 		acc_odr 							- [in]	ACC采样频率
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_acc_odr(LSM_ACC_ODR_e acc_odr)
{
	lsm6ds3tr_c_odr_xl_t	val;
	
	if(ob_lsm6ds3tr_c.cur_param.acc_odr != acc_odr)
	{
		switch(acc_odr)
		{
			case LSM_ACC_ODR_OFF:
				lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
			
				lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_XL_ODR_OFF)return -1;
			
				ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
				break;
			case LSM_ACC_ODR_104HZ:
				lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_104Hz);
			
				lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_XL_ODR_104Hz)return -1;
			
				ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_104HZ;
				break;
			case LSM_ACC_ODR_12HZ5:
				lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_12Hz5);
			
				lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_XL_ODR_12Hz5)return -1;	
			
				ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_12HZ5;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置GRY量程
	@param 		gry_fs 								- [in]	GRY量程
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_gry_fs(LSM_GRY_FS_e gry_fs)
{
	lsm6ds3tr_c_fs_g_t	val;
	
	if(ob_lsm6ds3tr_c.cur_param.gry_fs != gry_fs)
	{
		switch(gry_fs)
		{
			case LSM_GRY_FS_250DPS:
				lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_250dps);
			
				lsm6ds3tr_c_gy_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_250dps)return -1;
			
				ob_lsm6ds3tr_c.cur_param.gry_fs = LSM_GRY_FS_250DPS;
				break;
			case LSM_GRY_FS_2000DPS:
				lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2000dps);
			
				lsm6ds3tr_c_gy_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_2000dps)return -1;
			
				ob_lsm6ds3tr_c.cur_param.gry_fs = LSM_GRY_FS_2000DPS;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置GRY采样频率
	@param 		gry_odr 							- [in]	GRY采样频率
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_gry_odr(LSM_GRY_ODR_e gry_odr)
{
	lsm6ds3tr_c_odr_g_t	val;
	if(ob_lsm6ds3tr_c.cur_param.gry_odr != gry_odr)
	{
		switch(gry_odr)
		{
			case LSM_GRY_ODR_OFF:
				lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);
			
				lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_GY_ODR_OFF)return -1;
			
				ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
				break;
			case LSM_GRY_ODR_104HZ:
				lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_104Hz);
			
				lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_GY_ODR_104Hz)return -1;
			
				ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_104HZ;
				break;
			case LSM_GRY_ODR_12HZ5:
				lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_12Hz5);
		
				lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
			
				if(val != LSM6DS3TR_C_GY_ODR_12Hz5)return -1;			
			
				ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_12HZ5;
				break;
		}
	}
	
	return 0;	
}
/**
	@brief 		设置MAG量程
	@param 		mag_fs 								- [in]	MAG量程
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_mag_fs(LSM_MAG_FS_e mag_fs)
{
	int 		ret;
	uint8_t data;
	
	if(ob_lsm6ds3tr_c.cur_param.mag_fs != mag_fs)
	{
		switch(mag_fs)
		{
			case LSM_MAG_FS_30GS:
				data = LSM_MAG_FS_30GS;
				ret = write_hub(HUB_MAG_DEV_ADDR, 0x0b, &data);
				if(ret == -1)return -1;
				data = 0xFF;
				ret = read_hub(HUB_MAG_DEV_ADDR, 0x0b, &data,1);
				if(ret == -1 || data != LSM_MAG_FS_30GS)return -1;			
				ob_lsm6ds3tr_c.cur_param.mag_fs = LSM_MAG_FS_30GS;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		设置MAG采样频率
	@param 		mag_odr 							- [in]	MAG采样频率
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_mag_odr(LSM_MAG_ODR_e mag_odr, bool set_hub)
{
	int 				ret = -1;
	int					err;
	uint8_t 		data;
	static int 	flow_ctl = 0;		//流程控制
	
	if(ob_lsm6ds3tr_c.cur_param.mag_odr != mag_odr)
	{
		switch(flow_ctl)
		{
			case 0:
				//define the sign for x y and z axis
				if(mag_odr != LSM_MAG_ODR_OFF)
				{
					data = 0x06;
					err = write_hub(HUB_MAG_DEV_ADDR, 0x29, &data);
					if(err == -1)return -1;
					
					flow_ctl = 1;
				}
				else flow_ctl = 1;
				break;
			
			case 1:
				//define the sign for x y and z axis
				if(mag_odr != LSM_MAG_ODR_OFF)
				{
					data = 0xFF;
					err = read_hub(HUB_MAG_DEV_ADDR, 0x29, &data,1);
					if(err == -1 || data != 0x06)return -1;
					
					flow_ctl = 2;
				}
				else flow_ctl = 2;
				break;
			
			case 2:
				//set odr
				data = mag_odr;
				err = write_hub(HUB_MAG_DEV_ADDR, 0x0a, &data);
				if(err == -1)return -1;
			
				flow_ctl = 3;
				break;
			
			case 3:
//				data = 0xFF;
//				err = read_hub(HUB_MAG_DEV_ADDR, 0x0a, &data,1);
//				if(err == -1 || data != mag_odr)
//				{
//					flow_ctl = 2;
//					return -1;	
//				}

				flow_ctl = 4;
				break;
			
			case 4:
				//是否配置hub
				if(set_hub)
				{
					lsm6ds3tr_c_sh_slv0_cfg_read(&ob_lsm6ds3tr_c.ctx, &sub_qmc6310);
					
					lsm6ds3tr_c_mem_bank_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_BANK_A);
					
					data = 0xFF;
					lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_SLV0_SUBADD,&data, 1);
					
					lsm6ds3tr_c_mem_bank_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_USER_BANK);
					
					if(data != QMC6310_DATA_OUT_X_LSB_REG)return -1;
					
					/* Configure Sensor Hub to read one slaves */
					lsm6ds3tr_c_sh_num_of_dev_connected_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_SLV_0);

					lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_ENABLE);
					
					lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
					
					ret = 0;
					flow_ctl = 0;
					ob_lsm6ds3tr_c.cur_param.mag_odr = mag_odr;
				}
				else{
					ret = 0;
					flow_ctl = 0;
					ob_lsm6ds3tr_c.cur_param.mag_odr = mag_odr;
				}
				break;
		}
	}
	else
	{
		return 0;
	}
	
	return ret;
}
/**
	@brief 		获取LSM配置参数
	@param 		param 								- [in]	LSM配置参数
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_get_config_param(drv_lsm_config_param_t	*p_param)
{
	*p_param = ob_lsm6ds3tr_c.cur_param;
	
	return 0;
}
/**
	@brief 		当前LSM的FIFO有几组数据
	@return 	错误代码							- [out]	FIFO当前有几组数据
*/
int	drv_lsm_get_fifo_group_num(void)
{
	uint8_t  wt;
	uint16_t num = 0;
	uint16_t num_pattern = 0;	
	uint16_t pattern_len = 0;
	
	/* Read FIFO watermark flag in polling mode */
	lsm6ds3tr_c_fifo_wtm_flag_get(&ob_lsm6ds3tr_c.ctx, &wt);
	
	if(wt)
	{
		/* Read number of word in FIFO */
		lsm6ds3tr_c_fifo_data_level_get(&ob_lsm6ds3tr_c.ctx, &num);
		
		if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
			pattern_len += ACC_OUT_XYZ_WORD_NUM;
		}
		if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
			pattern_len += GRY_OUT_XYZ_WORD_NUM;
		}
		if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
			pattern_len += MAG_OUT_XYZ_WORD_NUM;
		}
		if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
			pattern_len += TIMESTAMP_OUT_WORD_NUM;
		}

		num_pattern = num / pattern_len;
	}
	
	return num_pattern;
}
/**
	@brief 		获取LSM的FIFO数据
	@param 		p_data 								- [out]	返回的FIFO数据
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_get_fifo_data(lsm_data_t	*p_data)
{	
	//获取原始数据
	if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
		/* Read XL samples */
		lsm6ds3tr_c_fifo_raw_data_get(&ob_lsm6ds3tr_c.ctx, data_raw_acceleration.u8bit,ACC_OUT_XYZ_WORD_NUM * sizeof(int16_t));
	}
	if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
		/* Read gyro samples */
		lsm6ds3tr_c_fifo_raw_data_get(&ob_lsm6ds3tr_c.ctx, data_raw_angular_rate.u8bit,GRY_OUT_XYZ_WORD_NUM * sizeof(int16_t));
	}
	if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
		/* Read mag samples */
		lsm6ds3tr_c_fifo_raw_data_get(&ob_lsm6ds3tr_c.ctx, data_raw_magnetic.u8bit,MAG_OUT_XYZ_WORD_NUM * sizeof(int16_t));
	}
	if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
		/* Read timestamp samples */
		lsm6ds3tr_c_fifo_raw_data_get(&ob_lsm6ds3tr_c.ctx, data_raw_timestamp.u8bit,TIMESTAMP_OUT_WORD_NUM * sizeof(int16_t));
	}
	//返回数据
	if(p_data != NULL)
	{
		p_data->gry[0] = data_raw_angular_rate.i16bit[0];
		p_data->gry[1] = data_raw_angular_rate.i16bit[1];
		p_data->gry[2] = data_raw_angular_rate.i16bit[2];

		p_data->acc[0] = data_raw_acceleration.i16bit[0];
		p_data->acc[1] = data_raw_acceleration.i16bit[1];
		p_data->acc[2] = data_raw_acceleration.i16bit[2];
		
		p_data->mag[0] = data_raw_magnetic.i16bit[0];
		p_data->mag[1] = data_raw_magnetic.i16bit[1];
		p_data->mag[2] = data_raw_magnetic.i16bit[2];
		
		/*														时间戳和计步数数据
		
					字节1					字节2				字节3				字节4				字节5				字节6
				TIMESTAMP			TIMESTAMP			-----			TIMESTAMP			STEPS				STEPS
				 [15:8]				 [23:16]									[7:0]				[7:0]				[15:8]
		*/	
		
		p_data->fifo_timestamp = ((data_raw_timestamp.u8bit[1] << 16) | (data_raw_timestamp.u8bit[0] << 8) | (data_raw_timestamp.u8bit[3]))*25;//counter == 25us
	}
	
	return 0;
}
/**
	@brief 		获取LSM的ACC数据
	@param 		p_data 								- [out]	返回的ACC三轴数据
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_get_acc_data(lsm_data_t	*p_data)
{
	lsm6ds3tr_c_reg_t reg;
	
	if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF)
	{
		/* Read samples in polling mode (no int) */
		/* Read output only if new value is available */
		lsm6ds3tr_c_status_reg_get(&ob_lsm6ds3tr_c.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(&ob_lsm6ds3tr_c.ctx,data_raw_acceleration.i16bit);
			
			p_data->acc[0] = data_raw_acceleration.i16bit[0];
			p_data->acc[1] = data_raw_acceleration.i16bit[1];
			p_data->acc[2] = data_raw_acceleration.i16bit[2];
		}
	}
	else
	{
		return -1;
	}
	
	return 0;
}