liweiyan/shoe_mcu2.1.1/drivers/drv_lsm6ds3tr_c.c

/*Includes ----------------------------------------------*/
#include "bsp_spi.h"
#include "bsp_time.h"
#include "nrf_delay.h"
#include "drv_lsm6ds3tr_c.h"
#include "exception.h"
#include "system.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												//挂载的地磁设备地址	

#define 	 LSM_IIC_QMC_ADDR								(0x1c<<1)

#define    LSM_IIC_SA0										PIN_FRONT_SPI_MISO

#define    LSM_IIC_NCS										PIN_FRONT_SPI_nCS

#define		 LSM_IIC_SCL										PIN_FRONT_SPI_SCLK

#define    LSM_IIC_SDA										PIN_FRONT_SPI_MOSI

#define		 LSM_IIC_SCL_SET								nrf_gpio_pin_write(LSM_IIC_SCL,1)

#define		 LSM_IIC_SCL_CLR								nrf_gpio_pin_write(LSM_IIC_SCL,0)

#define		 LSM_IIC_SDA_SET								nrf_gpio_pin_write(LSM_IIC_SDA,1)

#define		 LSM_IIC_SDA_CLR								nrf_gpio_pin_write(LSM_IIC_SDA,0)

#define		 LSM_IIC_SDA_READ								nrf_gpio_pin_read(LSM_IIC_SDA)

/*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				temp_data_raw[4];														//临时存储原始数据
static uint32_t 					spi_write_error_counter = 0;								//spi写错误上报
static uint32_t 					spi_read_error_counter = 0;									//spi读错误上报
static uint8_t						whoamI, rst;
static int16_t 						self_check_data_raw[3];
static float 							val_st_off[3];
static float 							val_st_on[3];
static float 							test_val[3];
static uint8_t 						st_result;
static uint8_t 						drdy;
static uint8_t 						i;
static uint8_t 						j;




/*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;
		spi_write_error_counter++;
	}

	return ierror;
}
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
	int32_t ierror = 0;
	
	uint32_t errcode = SPI0_OnlyReadReg(BOARD_SPI0_CS0_IO, reg, bufp, len);
	if(errcode)
	{
		ierror = -1;
		spi_read_error_counter++;
	}

	
	return ierror;
}

static void drv_lsm_spi_error_report_process(void)
{
	char buff[30]={0};
	
	if(spi_write_error_counter > 0)
	{
		sprintf(buff,"spi_write_err,%d\r\n",spi_write_error_counter);
		Except_TxError(EXCEPT_SPI_RW,(const char *)buff);
	}
	
	if(spi_read_error_counter > 0)
	{
		sprintf(buff,"spi_read_err,%d\r\n",spi_read_error_counter);
		Except_TxError(EXCEPT_SPI_RW,(const char *)buff);
	}

	spi_write_error_counter = 0;
	spi_read_error_counter = 0;
}

static void LSM_IIC_Start(void)
{
	LSM_IIC_SDA_SET;
	LSM_IIC_SCL_SET;
	nrf_delay_us(5);
	LSM_IIC_SDA_CLR;
	nrf_delay_us(5);
	LSM_IIC_SCL_CLR;
}

static void LSM_IIC_Stop(void)
{
	LSM_IIC_SCL_CLR;
	LSM_IIC_SDA_CLR;
	nrf_delay_us(5);
	LSM_IIC_SCL_SET;
	LSM_IIC_SDA_SET;
	nrf_delay_us(5);
}

static void LSM_IIC_ACK(void)
{
	LSM_IIC_SCL_CLR;
	nrf_delay_us(1);
	LSM_IIC_SDA_CLR;
	nrf_delay_us(1);
	LSM_IIC_SCL_SET;
	nrf_delay_us(1);
	LSM_IIC_SCL_CLR;
}

static void LSM_IIC_NoACK(void)
{
	LSM_IIC_SCL_CLR;
	LSM_IIC_SDA_SET;
	nrf_delay_us(1);
	LSM_IIC_SCL_SET;
	nrf_delay_us(1);
	LSM_IIC_SCL_CLR;    
}

static bool LSM_IIC_WaitACK(void)
{
	uint8_t tim = 0;
//	IIC_BACK_SCL_CLR;
	LSM_IIC_SDA_SET;
	nrf_delay_us(1);
	LSM_IIC_SCL_SET;
//	nrf_delay_us(1);
	while(LSM_IIC_SDA_READ){
		if(++tim>=50){
			LSM_IIC_Stop();
			return false;
		}
		nrf_delay_us(1);
	}
	LSM_IIC_SCL_CLR;
	return true;
}

static void LSM_IIC_SendByte(uint8_t _byte)
{
	uint8_t i = 0;
	for (i=0; i<8; i++){
		LSM_IIC_SCL_CLR;
		nrf_delay_us(5);
		if (_byte&0x80)	LSM_IIC_SDA_SET;
		else LSM_IIC_SDA_CLR;
		_byte <<= 1;
		LSM_IIC_SCL_SET;
		nrf_delay_us(5);
	}
	LSM_IIC_SCL_CLR;
}

static uint8_t LSM_IIC_RecByte(void)
{
	uint8_t i = 0;
	uint8_t rec_byte;
   
	LSM_IIC_SDA_SET;
	for (i=0; i<8; i++){
		rec_byte <<= 1;
		LSM_IIC_SCL_CLR;
		nrf_delay_us(5);
		LSM_IIC_SCL_SET;
		nrf_delay_us(5);
		if (LSM_IIC_SDA_READ) rec_byte |= 0x01;
	} 
	LSM_IIC_SCL_CLR;
	return rec_byte;
}

/********************************************/
static bool LSM_IIC_WriteBytes(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len)
{
	uint8_t i = 0;
	LSM_IIC_Start();
	LSM_IIC_SendByte(add);
	if(!LSM_IIC_WaitACK()) return false;
	LSM_IIC_SendByte(reg);
	if(!LSM_IIC_WaitACK()) return false;
	for(i=0;i<len;i++){
		LSM_IIC_SendByte(p[i]);
		if(!LSM_IIC_WaitACK()) return false;
	}
	LSM_IIC_Stop();
	return true;
}

static bool LSM_IIC_ReadBytes(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len)
{
	uint8_t i = 0;
	LSM_IIC_Start();
	LSM_IIC_SendByte(add);
	if(!LSM_IIC_WaitACK()) return false;
	LSM_IIC_SendByte(reg);
	if(!LSM_IIC_WaitACK()) return false;
	LSM_IIC_Start();
	LSM_IIC_SendByte(add+1);
	if(!LSM_IIC_WaitACK()) return false;
	for(i=0;i<len-1;i++){
		p[i] = LSM_IIC_RecByte(); LSM_IIC_ACK();
	}
	p[i] = LSM_IIC_RecByte(); LSM_IIC_NoACK();
	LSM_IIC_Stop();
	return true;
}


static void LSM_IIC_Init(void)
{
	nrf_gpio_cfg(
        LSM_IIC_SCL,
        NRF_GPIO_PIN_DIR_OUTPUT,
        NRF_GPIO_PIN_INPUT_DISCONNECT,
        NRF_GPIO_PIN_NOPULL,
        NRF_GPIO_PIN_S0S1,
        NRF_GPIO_PIN_NOSENSE);
	
	nrf_gpio_cfg_watcher(LSM_IIC_SCL);
	nrf_gpio_pin_set(LSM_IIC_SCL);
	
	nrf_gpio_cfg(
        LSM_IIC_SDA,
        NRF_GPIO_PIN_DIR_OUTPUT,
        NRF_GPIO_PIN_INPUT_DISCONNECT,
        NRF_GPIO_PIN_NOPULL,
        NRF_GPIO_PIN_S0D1,
        NRF_GPIO_PIN_NOSENSE);
	
	nrf_gpio_cfg_watcher(LSM_IIC_SDA);
	nrf_gpio_pin_set(LSM_IIC_SDA);
	
	nrf_gpio_cfg_output(LSM_IIC_NCS);nrf_gpio_pin_write(LSM_IIC_NCS,1);
	
	nrf_gpio_cfg_output(LSM_IIC_SA0);
}
static void LSM_IIC_UnInit(void)
{
	nrf_gpio_cfg_default(LSM_IIC_SCL);
	nrf_gpio_cfg_default(LSM_IIC_SDA);
	nrf_gpio_cfg_default(LSM_IIC_NCS);
	nrf_gpio_cfg_default(LSM_IIC_SA0);
}

static int read_hub(uint8_t device_addr, uint8_t reg, uint8_t* data,uint8_t len)
{
  int32_t 									ret;
  uint8_t 									val;
	lsm6ds3tr_c_odr_fifo_t 		r_fifo_odr;
	lsm6ds3tr_c_odr_xl_t 			acc_odr;
	lsm6ds3tr_c_odr_g_t				gry_odr;
	lsm6ds3tr_c_ctrl10_c_t 		ctrl10_c;
	lsm6ds3tr_c_pull_up_en_t  up_en;
	
	//关闭ACC
	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,&acc_odr);

	if(acc_odr != LSM6DS3TR_C_XL_ODR_OFF)return -1;

	ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
	
	//关闭GRY
	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,&gry_odr);

	if(gry_odr != LSM6DS3TR_C_GY_ODR_OFF)return -1;

	ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
	
	//关闭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;
	
	//设置hub不使用iic
	
	lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);
	
	val = 0xFF;
	lsm6ds3tr_c_sh_master_get(&ob_lsm6ds3tr_c.ctx, &val);
	
	if(val != PROPERTY_DISABLE)return -1;
	
	//设置hub总开关为关闭
	
	lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
	
	lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_CTRL10_C,(uint8_t *)&ctrl10_c, 1);
	
	if(ctrl10_c.func_en != PROPERTY_DISABLE)return -1;

	//开启直通模式
	
	lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
	lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
	
	if(val != 1)return -1;
	
	//开启内部上拉
	
	lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
	
	lsm6ds3tr_c_sh_pin_mode_get(&ob_lsm6ds3tr_c.ctx,&up_en);
	
	if(up_en != LSM6DS3TR_C_INTERNAL_PULL_UP)return -1;
	
	//SPI切换为IIC
	
	SPI0_Disable();
	
	LSM_IIC_Init();
	
	if(LSM_IIC_ReadBytes(device_addr,reg,data,len))
	{
		ret = 0;
	}
	else
	{
		ret = -1;
		SEGGER_RTT_printf(0,"-------->LSM_IIC_ReadBytes err\n");
	}
	
	//IIC切换为SPI
	
	LSM_IIC_UnInit();
	
	SPI0_Init();
	
	//关闭直通模式
	
	lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,0);
	
	lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
	
	if(val != 0)return -1;
	
	//关闭内部上拉
	
	lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_EXT_PULL_UP);
	
	up_en = LSM6DS3TR_C_SH_PIN_MODE;
	lsm6ds3tr_c_sh_pin_mode_get(&ob_lsm6ds3tr_c.ctx,&up_en);
	
	if(up_en != LSM6DS3TR_C_EXT_PULL_UP)return -1;

  return ret;
}
static int write_hub(uint8_t device_addr, uint8_t reg, uint8_t* data,uint8_t len)
{
  int32_t 									ret;
  uint8_t 									val;
	lsm6ds3tr_c_odr_fifo_t 		r_fifo_odr;
	lsm6ds3tr_c_odr_xl_t 			acc_odr;
	lsm6ds3tr_c_odr_g_t				gry_odr;
	lsm6ds3tr_c_ctrl10_c_t 		ctrl10_c;
	lsm6ds3tr_c_pull_up_en_t  up_en;
	
	//关闭ACC
	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,&acc_odr);

	if(acc_odr != LSM6DS3TR_C_XL_ODR_OFF)return -1;

	ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
	
	//关闭GRY
	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,&gry_odr);

	if(gry_odr != LSM6DS3TR_C_GY_ODR_OFF)return -1;

	ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
	
	//关闭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;
	
	//设置hub不使用iic
	
	lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);
	
	val = 0xFF;
	lsm6ds3tr_c_sh_master_get(&ob_lsm6ds3tr_c.ctx, &val);
	
	if(val != PROPERTY_DISABLE)return -1;
	
	//设置hub总开关为关闭
	
	lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
	
	lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_CTRL10_C,(uint8_t *)&ctrl10_c, 1);
	
	if(ctrl10_c.func_en != PROPERTY_DISABLE)return -1;

	//开启直通模式
	
	lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
	lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
	
	if(val != 1)return -1;
	
	//开启内部上拉
	
	lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
	
	lsm6ds3tr_c_sh_pin_mode_get(&ob_lsm6ds3tr_c.ctx,&up_en);
	
	if(up_en != LSM6DS3TR_C_INTERNAL_PULL_UP)return -1;
	
	//SPI切换为IIC
	
	SPI0_Disable();
	
	LSM_IIC_Init();	
	
	if(LSM_IIC_WriteBytes(device_addr,reg,data,len))
	{
		ret = 0;
	}
	else
	{
		ret = -1;
		SEGGER_RTT_printf(0,"-------->LSM_IIC_WriteBytes err\n");
	}
	
	//IIC切换为SPI
	
	LSM_IIC_UnInit();
	
	SPI0_Init();
	
	//关闭直通模式
	
	lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,0);
	
	lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
	
	if(val != 0)return -1;
	
	//关闭内部上拉
	
	lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_EXT_PULL_UP);
	
	up_en = LSM6DS3TR_C_SH_PIN_MODE;
	lsm6ds3tr_c_sh_pin_mode_get(&ob_lsm6ds3tr_c.ctx,&up_en);
	
	if(up_en != LSM6DS3TR_C_EXT_PULL_UP)return -1;

  return ret;
}
static int self_check(void)
{
	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)
	{
		DEBUG_LOG("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, self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_off[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_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, self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_on[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_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, self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_off[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
                         self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_on[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
                        self_check_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) {
    DEBUG_LOG("LSM Self Test - PASS\r\n");
		
		lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
		rst = 0Xff;
		lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
		
		if(rst != 1)return -1;
		
  }else{
    DEBUG_LOG("Self Test - FAIL\r\n");
		
		lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
		rst = 0Xff;
		lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
		
		if(rst != 1)return -1;
		
		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;
	ob_lsm6ds3tr_c.op_timeout												=	0;
	//供电
	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();
	//初始化SPI错误上报
	Process_Start(1000,"drv_lsm_spi_error_report_process",drv_lsm_spi_error_report_process);
	//开始自检
	return self_check();	
}
/**
	@brief 		LSM断电
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_power_off(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_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);
	
	return 0;
}
/**
	@brief 		LSM上电（默认配置挂起）
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_power_on(void)
{
	nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
	
	nrf_gpio_cfg_input(BOARD_SPI0_MISO_IO, (nrf_gpio_pin_pull_t)NRFX_SPIM_MISO_PULL_CFG);
	
	//初始化结构体
	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.op_timeout												=	0;
	
	//关闭SPI
	SPI0_Disable();				//注意：若SPI线短路，该函数内部实现有个while循环导致卡死。已修改源文件规避。
	//重新初始化SPI
	SPI0_Init();
	
//	lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
	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, LSM_ACC_ODR_e acc_odr, LSM_GRY_ODR_e gry_odr, LSM_MAG_ODR_e mag_odr, LSM_TIMESTAMP_SWITCH_e timestamp_switch)
{
	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(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(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(mag_odr != LSM_MAG_ODR_OFF)
				{
					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(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量程需要的步骤数量
	@return 	错误代码							- [out]	配置MAG量程需要的步骤数量
*/
int	drv_lsm_get_mag_fs_flow(void)
{
	return 1;
}
/**
	@brief 		设置MAG量程
	@param 		mag_fs 								- [in]	MAG量程
	@param 		flow 									- [in]	当前需要处理的步骤
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_mag_fs(LSM_MAG_FS_e mag_fs, int flow)
{
	int 		ret;
	uint8_t data;
	
	if(flow <= 0 || flow >= 2)return -1;
	
	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(LSM_IIC_QMC_ADDR,0x0b,&data,1);
				if(ret == -1)return -1;
				data = 0xFF;
				ret = read_hub(LSM_IIC_QMC_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采样频率需要的步骤数量
	@return 	错误代码							- [out]	配置MAG采样频率需要的步骤数量
*/
int	drv_lsm_get_mag_odr_flow(void)
{
	return 5;
}
/**
	@brief 		设置MAG采样频率
	@param 		mag_odr 							- [in]	MAG采样频率
	@param 		set_hub 							- [in]	是否配置hub
	@param 		flow 									- [in]	当前需要处理的步骤
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_lsm_set_mag_odr(LSM_MAG_ODR_e mag_odr, bool set_hub, int flow)
{
	int 		ret;
	uint8_t data;
	
	if(flow <= 0 || flow >= 6)return -1;
	
	if(ob_lsm6ds3tr_c.cur_param.mag_odr != mag_odr)
	{
		switch(flow)
		{
			case 1:
				//define the sign for x y and z axis
				if(mag_odr != LSM_MAG_ODR_OFF)
				{
					data = 0x06;
					ret = write_hub(LSM_IIC_QMC_ADDR,0x29,&data,1);
				}
				else
				{
					ret = 0;
				}
				break;
			
			case 2:
				//define the sign for x y and z axis
				if(mag_odr != LSM_MAG_ODR_OFF)
				{
					data = 0xFF;
					ret = read_hub(LSM_IIC_QMC_ADDR,0x29,&data,1);
					if(ret == -1 || data != 0x06)ret = -1;
					else ret = 0;
				}
				else
				{
					ret = 0;
				}
				break;
			
			case 3:
				//set odr
				data = mag_odr;
				ret = write_hub(LSM_IIC_QMC_ADDR,0x0a,&data,1);
				break;
			
			case 4:
				data = 0xFF;
				ret = read_hub(LSM_IIC_QMC_ADDR,0x0a,&data,1);
				if(ret == -1 || data != mag_odr)ret = -1;	
				else{
					ret = 0;
				}
				break;
			
			case 5:
				//是否配置hub
				if(set_hub)
				{
					lsm6ds3tr_c_sh_pin_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_INTERNAL_PULL_UP);
					
					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;
					ob_lsm6ds3tr_c.cur_param.mag_odr = mag_odr;
				}
				else{
					ret = 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)
{	
	int ret;
	int	read_len 	= 0;
	int read_num 	= 0;
	
	//获取原始数据的长度
	if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
		/* Read gyro samples len */
		read_len += GRY_OUT_XYZ_WORD_NUM * sizeof(int16_t);
	}
	if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
		/* Read XL samples len*/
		read_len += ACC_OUT_XYZ_WORD_NUM * sizeof(int16_t);
	}
	if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
		/* Read mag samples len*/
		read_len += MAG_OUT_XYZ_WORD_NUM * sizeof(int16_t);
	}
	if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
		/* Read timestamp samples len*/
		read_len += TIMESTAMP_OUT_WORD_NUM * sizeof(int16_t);
	}
	
	//清空缓存
	memset(temp_data_raw,0,sizeof(temp_data_raw));
	
	//获取原始数据
	ret = lsm6ds3tr_c_fifo_raw_data_get(&ob_lsm6ds3tr_c.ctx, temp_data_raw[0].u8bit,read_len);
	if(ret == -1)return -1;

	
	//返回数据
	if(p_data != NULL)
	{
		if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
			/* Read gyro samples */
			p_data->gry[0] = temp_data_raw[read_num].i16bit[0];
			p_data->gry[1] = temp_data_raw[read_num].i16bit[1];
			p_data->gry[2] = temp_data_raw[read_num].i16bit[2];
			
			read_num++;
		}
		if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
			/* Read XL samples */
			p_data->acc[0] = temp_data_raw[read_num].i16bit[0];
			p_data->acc[1] = temp_data_raw[read_num].i16bit[1];
			p_data->acc[2] = temp_data_raw[read_num].i16bit[2];
			
			read_num++;
		}
		if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
			/* Read mag samples */
			p_data->mag[0] = temp_data_raw[read_num].i16bit[0];
			p_data->mag[1] = temp_data_raw[read_num].i16bit[1];
			p_data->mag[2] = temp_data_raw[read_num].i16bit[2];
			
			read_num++;
		}
		if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
			/*														时间戳和计步数数据
			
						字节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 = ((temp_data_raw[read_num].u8bit[1] << 16) | (temp_data_raw[read_num].u8bit[0] << 8) | (temp_data_raw[read_num].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(temp_data_raw[0].i16bit, 0x00, 3 * sizeof(int16_t));
      lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx,temp_data_raw[0].i16bit);
			
			p_data->acc[0] = temp_data_raw[0].i16bit[0];
			p_data->acc[1] = temp_data_raw[0].i16bit[1];
			p_data->acc[2] = temp_data_raw[0].i16bit[2];
		}
		else return -1;
	}
	else
	{
		return -1;
	}
	
	return 0;
}
/**
	@brief 		LSM自检步骤拆分——1，执行玩需等待100ms稳定输出才能执行步骤2
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_self_check_1(void)
{
 /* Check device ID */
  lsm6ds3tr_c_device_id_get(&ob_lsm6ds3tr_c.ctx, &whoamI);
	if(whoamI != LSM6DS3TR_C_ID)
	{
		DEBUG_LOG("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_833Hz);
  /* Set full scale */
  lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_4g);
	
	return 0;
}
/**
	@brief 		LSM自检步骤拆分——2，执行玩需等待100ms稳定输出才能执行步骤3
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_self_check_2(void)
{
/* 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, self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_off[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_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);	
	
	return 0;
}
/**
	@brief 		LSM自检步骤拆分——3，执行玩需等待150ms稳定输出才能执行步骤4
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_self_check_3(void)
{
 /* 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, self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_on[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_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_833Hz);
  /* Set full scale */
  lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2000dps);
	
	return 0;
}
/**
	@brief 		LSM自检步骤拆分——4，执行玩需等待50ms稳定输出才能执行步骤5
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_self_check_4(void)
{
/* 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, self_check_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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_off[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
                         self_check_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);
	
	return 0;
}
/**
	@brief 		LSM自检步骤拆分——5，获取自检结果。
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_lsm_self_check_5(void)
{
/* 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, self_check_data_raw);

    for (j = 0; j < 3; j++) {
      val_st_on[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
                        self_check_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) {
    DEBUG_LOG("LSM Self Test - PASS\r\n");
		
		lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
		rst = 0Xff;
		lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
		
		if(rst != 1)return -1;
		
  }else{
    DEBUG_LOG("Self Test - FAIL\r\n");
		
		lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
	
		rst = 0Xff;
		lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
		
		if(rst != 1)return -1;
		
		return -1;
  }	
	
	return 0;
}