/*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_us(uint32_t us) { nrf_delay_us(us); } 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;iLSM_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; DEBUG_LOG("-------->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) { /* 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",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_12HZ5: //启用步进计数器和时间戳数据作为第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_12HZ5) { 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_12HZ5) { 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_12Hz5);//该参数关联到timestamp lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr); if(r_fifo_odr != LSM6DS3TR_C_FIFO_12Hz5) return -1; ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_12HZ5; 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; case LSM_FIFO_ODR_208HZ: //启用步进计数器和时间戳数据作为第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_208HZ) { 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_208HZ) { 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_208Hz);//该参数关联到timestamp lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr); if(r_fifo_odr != LSM6DS3TR_C_FIFO_208Hz) return -1; ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_208HZ; break; case LSM_FIFO_ODR_416HZ: //启用步进计数器和时间戳数据作为第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_416HZ) { 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_416HZ) { 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_416Hz);//该参数关联到timestamp lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr); if(r_fifo_odr != LSM6DS3TR_C_FIFO_416Hz) return -1; ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_416HZ; break; case LSM_FIFO_ODR_833HZ: //启用步进计数器和时间戳数据作为第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_833HZ) { 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_833HZ) { 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_833Hz);//该参数关联到timestamp lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr); if(r_fifo_odr != LSM6DS3TR_C_FIFO_833Hz) return -1; ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_833HZ; 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_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; 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_208HZ: lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_208Hz); lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val); if(val != LSM6DS3TR_C_XL_ODR_208Hz)return -1; ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_208HZ; break; case LSM_ACC_ODR_416HZ: lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_416Hz); lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val); if(val != LSM6DS3TR_C_XL_ODR_416Hz)return -1; ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_416HZ; break; case LSM_ACC_ODR_833HZ: lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_833Hz); lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val); if(val != LSM6DS3TR_C_XL_ODR_833Hz)return -1; ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_833HZ; 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_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; 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_208HZ: lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_208Hz); lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val); if(val != LSM6DS3TR_C_GY_ODR_208Hz)return -1; ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_208HZ; break; case LSM_GRY_ODR_416HZ: lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_416Hz); lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val); if(val != LSM6DS3TR_C_GY_ODR_416Hz)return -1; ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_416HZ; break; case LSM_GRY_ODR_833HZ: lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_833Hz); lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val); if(val != LSM6DS3TR_C_GY_ODR_833Hz)return -1; ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_833HZ; 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; uint8_t fifo_status2; /* Read FIFO watermark flag in polling mode */ lsm6ds3tr_c_fifo_wtm_flag_get(&ob_lsm6ds3tr_c.ctx, &wt); lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_STATUS2,(uint8_t *)&fifo_status2, 1); //溢出了，就清空整个FIFO，因为这个时候已经读不出来FIFO数据了。 if((fifo_status2 & 0xE0) == 0xE0) { /* Set FIFO mode to Stream mode */ lsm6ds3tr_c_fifo_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_BYPASS_MODE); platform_delay_us(30); lsm6ds3tr_c_fifo_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_STREAM_MODE); return -1; } 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, ®.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; }