/*reset sensors*/
imu_status_t imu_sensor_reset(void)
{
if(LSM6DS3_IO_Init() != imu_status_ok)
{
printf("lsm6ds3 io init error\n");
return imu_status_fail;
}
/* Configure interrupt lines */
LSM6DS3_IO_ITConfig();
/*Config LSM303AGR interrupt lines*/
LSM303AGR_IO_ITConfig();
printf("IT IO Config\n");
if(lsm6ds3_fifo_sensor_enable() != imu_status_ok)
{
printf("sensor fifoenable error\n");
return imu_status_fail;
}
printf("sensor enable\n");
if(imu_sensor_magneto_interrupt_enable() != imu_status_ok)
{
printf("imu_sensor_magneto_interrupt_enable\n");
return imu_status_fail;
}
return imu_status_ok;
}
/**
* @brief Set LSM6DS3 Initialization
* @param LSM6DS3_Init the configuration setting for the LSM6DS3
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_Init( IMU_6AXES_InitTypeDef *LSM6DS3_Init )
{
/*Here we have to add the check if the parameters are valid*/
/* Configure the low level interface -------------------------------------*/
if(LSM6DS3_IO_Init() != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/******** Common init *********/
if(LSM6DS3_Common_Sensor_Enable() != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/******* Gyroscope init *******/
if(LSM6DS3_G_Set_ODR( LSM6DS3_Init->G_OutputDataRate ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_G_Set_FS( LSM6DS3_Init->G_FullScale ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_G_Set_Axes_Status(LSM6DS3_Init->G_X_Axis, LSM6DS3_Init->G_Y_Axis, LSM6DS3_Init->G_Z_Axis) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/***** Accelerometer init *****/
if(LSM6DS3_X_Set_ODR( LSM6DS3_Init->X_OutputDataRate ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_X_Set_FS( LSM6DS3_Init->X_FullScale ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_X_Set_Axes_Status(LSM6DS3_Init->X_X_Axis, LSM6DS3_Init->X_Y_Axis, LSM6DS3_Init->X_Z_Axis) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* Configure interrupt lines */
LSM6DS3_IO_ITConfig();
return IMU_6AXES_OK;
}
/*reset sensors*/
imu_status_t imu_sensor_reset(void)
{
uint8_t tempReg[2] = {0,0};
if(LSM6DS3_IO_Init() != imu_status_ok)
{
printf("lsm6ds3 io init error\n");
return imu_status_fail;
}
/* Configure interrupt lines */
LSM6DS3_IO_ITConfig();
printf("IT IO Config\n");
/*soft reset*/
if(imu_sensor_lsm6ds3_soft_reset() != imu_status_ok)
{
printf("lsm6ds3 reset error\n");
return imu_status_fail;
}
printf("sensor reset\n");
imu_sensor_read_fifo_status();
/*clear fifo data*/
imu_sensor_clear_fifo();
imu_sensor_read_fifo_status();
/*set fifo water mark level*/
if(imu_sensor_fifo_threshold_level(2046) != imu_status_ok) { //1.2kBytes
printf("sensor fifo water mark setting error\n");
return imu_status_fail;
}
printf("fifo water mark level set over\n");
/*enable fifo interrupt*/
if(imu_sensor_fifo_threshold_interrupt() != imu_status_ok ) {
printf("sensor fifo interrupt setting error\n");
return imu_status_fail;
}
printf("fifo threshold interrupt set over\n");
if(lsm6ds3_fifo_sensor_enable() != imu_status_ok)
{
printf("sensor fifoenable error\n");
return imu_status_fail;
}
printf("sensor enable\n");
return imu_status_ok;
}
