/*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;
}
ReturnStatus_t LSM6DS3_Init( IMU_6AXES_InitTypeDef *LSM6DS3_InitStruct )
{
/*Here we could add the check if the parameters are valid*/
uint8_t tmp1 = 0x00;
/* Configure the low level interface -------------------------------------*/
if (LSM6DS3_IO_Init() == NOK )
{
return NOK; // if issue at initialization, exit here
}
/******** Common init *********/
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1);
/* Enable register address automatically incremented during a multiple byte
access with a serial interface (I2C or SPI) */
tmp1 &= ~(LSM6DS3_XG_IF_INC_MASK);
tmp1 |= LSM6DS3_XG_IF_INC;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1);
/* FIFO ODR selection */
tmp1 &= ~(LSM6DS3_XG_FIFO_ODR_MASK);
tmp1 |= LSM6DS3_XG_FIFO_ODR_NA;
/* FIFO mode selection */
tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK);
tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1);
/******* Gyroscope init *******/
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) ;
/* Output Data Rate selection */
tmp1 &= ~(LSM6DS3_G_ODR_MASK);
tmp1 |= LSM6DS3_InitStruct->G_OutputDataRate;
/* Full scale selection */
tmp1 &= ~(LSM6DS3_G_FS_MASK);
tmp1 |= LSM6DS3_InitStruct->G_FullScale;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1);
/* Enable X axis selection */
tmp1 &= ~(LSM6DS3_G_XEN_MASK);
tmp1 |= LSM6DS3_InitStruct->G_X_Axis;
/* Enable Y axis selection */
tmp1 &= ~(LSM6DS3_G_YEN_MASK);
tmp1 |= LSM6DS3_InitStruct->G_Y_Axis;
/* Enable Z axis selection */
tmp1 &= ~(LSM6DS3_G_ZEN_MASK);
tmp1 |= LSM6DS3_InitStruct->G_Z_Axis;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1);
/***** Accelerometer init *****/
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1);
/* Output Data Rate selection */
tmp1 &= ~(LSM6DS3_XL_ODR_MASK);
tmp1 |= LSM6DS3_InitStruct->X_OutputDataRate;
/* Full scale selection */
tmp1 &= ~(LSM6DS3_XL_FS_MASK);
tmp1 |= LSM6DS3_InitStruct->X_FullScale;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL9_XL, 1);
/* Enable X axis selection */
tmp1 &= ~(LSM6DS3_XL_XEN_MASK);
tmp1 |= LSM6DS3_InitStruct->X_X_Axis;
/* Enable Y axis selection */
tmp1 &= ~(LSM6DS3_XL_YEN_MASK);
tmp1 |= LSM6DS3_InitStruct->X_Y_Axis;
/* Enable Z axis selection */
tmp1 &= ~(LSM6DS3_XL_ZEN_MASK);
tmp1 |= LSM6DS3_InitStruct->X_Z_Axis;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL9_XL, 1);
/* Configure interrupt lines */
// LSM6DS3_IO_ITConfig();
//TODO
return OK;
}