static imu_status_t imu_sensor_bypass_clear_fifo(void* arg)
{
uint8_t tmp1;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok)
{
return imu_status_fail;
}
/* FIFO mode selection */
tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK);
/*bypass mode*/
tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS;
/*BYPASS MODE*/
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok)
{
return imu_status_fail;
}
/* FIFO mode selection */
tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK);
/*bypass mode*/
tmp1 |= LSM6DS3_XG_FIFO_MODE_CONTINUOUS_OVERWRITE;
/*CONTINUES MODE*/
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok)
{
return imu_status_fail;
}
return imu_status_ok;
}
/**
* @brief Disable free fall detection
* @param None
* @retval none
*/
void LSM6DS3_Disable_Free_Fall_Detection( void )
{
uint8_t tmp1 = 0x00;
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1);
/* INT1_FF setting */
tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1);
/* FF_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT;
/* FF_THS setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1);
}
/**
* @brief Write Gyro Full Scale
* @param fullScale the gyroscope full scale to be set
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_Set_FS( float fullScale )
{
uint8_t new_fs = 0x00;
uint8_t tempReg = 0x00;
if(fullScale <= 125.0f)
{
new_fs = LSM6DS3_G_FS_125_ENABLE;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
tempReg &= ~(LSM6DS3_G_FS_125_MASK);
tempReg |= new_fs;
if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
}
else
{
/* Disable G FS 125dpp */
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
tempReg &= ~(LSM6DS3_G_FS_125_MASK);
tempReg |= LSM6DS3_G_FS_125_DISABLE;
if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
new_fs = ( fullScale <= 245.0f ) ? LSM6DS3_G_FS_245
: ( fullScale <= 500.0f ) ? LSM6DS3_G_FS_500
: ( fullScale <= 1000.0f ) ? LSM6DS3_G_FS_1000
: LSM6DS3_G_FS_2000;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
tempReg &= ~(LSM6DS3_G_FS_MASK);
tempReg |= new_fs;
if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
}
return IMU_6AXES_OK;
}
/**
* @brief Enable free fall detection
* @param None
* @retval none
*/
void LSM6DS3_Enable_Free_Fall_Detection( void )
{
uint8_t tmp1 = 0x00;
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1);
/* Output Data Rate selection */
tmp1 &= ~(LSM6DS3_XL_ODR_MASK);
tmp1 |= LSM6DS3_XL_ODR_416HZ;
/* Full scale selection */
tmp1 &= ~(LSM6DS3_XL_FS_MASK);
tmp1 |= LSM6DS3_XL_FS_2G;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_UP_DUR, 1);
/* FF_DUR5 setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_DEFAULT;
/* WAKE_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_DEFAULT;
/* TIMER_HR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_DEFAULT;
/* SLEEP_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_DEFAULT;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_UP_DUR, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1);
/* FF_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_TYPICAL;
/* FF_THS setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_312MG;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1);
LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1);
/* INT1_FF setting */
tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_ENABLE;
LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1);
}
/*fifo threshold level setting*/
static imu_status_t imu_sensor_fifo_threshold_level(uint16_t fifo_level)
{
uint8_t tmp1;
{
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL1, 1) != imu_status_ok)
{
return imu_status_fail;
}
/*set water mark low 8bit*/
tmp1 &= ~(0xFF);
tmp1 |= (fifo_level & 0xFF);
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL1, 1) != imu_status_ok)
{
return imu_status_fail;
}
}
{
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL2, 1) != imu_status_ok)
{
return imu_status_fail;
}
/*set water mark high 4bit*/
tmp1 &= ~(LSM6DS3_XG_FIFO_THRESHOLD_MASK);
tmp1 |= (fifo_level >> 8) & 0x0F;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL2, 1) != imu_status_ok)
{
return imu_status_fail;
}
}
{
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL3, 1) != imu_status_ok)
{
return imu_status_fail;
}
/*set */
tmp1 &= ~(0x3F);
tmp1 |= 0x09;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL3, 1) != imu_status_ok)
{
return imu_status_fail;
}
}
return imu_status_ok;
}
/*active gyro*/
static imu_status_t imu_sensor_config_gyro(uint8_t data_rate, uint8_t scale)
{
uint8_t tempReg = 0x00;
/*gyro rate and scale*/
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != imu_status_ok)
{
return imu_status_fail;
}
tempReg &= ~(LSM6DS3_G_ODR_MASK);
tempReg |= data_rate;
/* Full scale selection */
tempReg &= ~(LSM6DS3_G_FS_125_MASK);
tempReg |= scale;
if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != imu_status_ok)
{
return imu_status_fail;
}
/*get sensitivity*/
if(imu_sensor_gyro_get_sensitivity( &sensor_data_sensitivity.gyro_sensitivity ) != imu_status_ok)
{
return imu_status_fail;
}
printf("gyro_sensitivity : %f\n", sensor_data_sensitivity.gyro_sensitivity);
return imu_status_ok;
}
/*fifo config*/
static imu_status_t lsm6ds3_fifo_sensor_enable(void)
{
uint8_t tmp1 = 0x00;
/*lsm6ds3*/
{
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
/* 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;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
}
return imu_status_ok;
}
/**
* @brief Write Gyro Output Data Rate
* @param odr the gyroscope output data rate to be set
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_Set_ODR( float odr )
{
uint8_t new_odr = 0x00;
uint8_t tempReg = 0x00;
new_odr = ( odr <= 0.0f ) ? LSM6DS3_G_ODR_PD /* Power Down */
: ( odr <= 13.0f ) ? LSM6DS3_G_ODR_13HZ
: ( odr <= 26.0f ) ? LSM6DS3_G_ODR_26HZ
: ( odr <= 52.0f ) ? LSM6DS3_G_ODR_52HZ
: ( odr <= 104.0f ) ? LSM6DS3_G_ODR_104HZ
: ( odr <= 208.0f ) ? LSM6DS3_G_ODR_208HZ
: ( odr <= 416.0f ) ? LSM6DS3_G_ODR_416HZ
: ( odr <= 833.0f ) ? LSM6DS3_G_ODR_833HZ
: LSM6DS3_G_ODR_1660HZ;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
tempReg &= ~(LSM6DS3_G_ODR_MASK);
tempReg |= new_odr;
if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
return IMU_6AXES_OK;
}
/**
* @brief Write Accelero Full Scale
* @param fullScale the accelerometer full scale to be set
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_X_Set_FS( float fullScale )
{
uint8_t new_fs = 0x00;
uint8_t tempReg = 0x00;
new_fs = ( fullScale <= 2.0f ) ? LSM6DS3_XL_FS_2G
: ( fullScale <= 4.0f ) ? LSM6DS3_XL_FS_4G
: ( fullScale <= 8.0f ) ? LSM6DS3_XL_FS_8G
: LSM6DS3_XL_FS_16G;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
tempReg &= ~(LSM6DS3_XL_FS_MASK);
tempReg |= new_fs;
if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
return IMU_6AXES_OK;
}
/**
* @brief Set the status of the axes for gyroscope
* @param enableX the status of the x axis to be set
* @param enableY the status of the y axis to be set
* @param enableZ the status of the z axis to be set
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
{
uint8_t tmp1 = 0x00;
uint8_t eX = 0x00;
uint8_t eY = 0x00;
uint8_t eZ = 0x00;
eX = ( enableX == 0 ) ? LSM6DS3_G_XEN_DISABLE : LSM6DS3_G_XEN_ENABLE;
eY = ( enableY == 0 ) ? LSM6DS3_G_YEN_DISABLE : LSM6DS3_G_YEN_ENABLE;
eZ = ( enableZ == 0 ) ? LSM6DS3_G_ZEN_DISABLE : LSM6DS3_G_ZEN_ENABLE;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* Enable X axis selection */
tmp1 &= ~(LSM6DS3_G_XEN_MASK);
tmp1 |= eX;
/* Enable Y axis selection */
tmp1 &= ~(LSM6DS3_G_YEN_MASK);
tmp1 |= eY;
/* Enable Z axis selection */
tmp1 &= ~(LSM6DS3_G_ZEN_MASK);
tmp1 |= eZ;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
return IMU_6AXES_OK;
}
/**
* @brief Set LSM6DS3 common initialization
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_Common_Sensor_Enable(void)
{
uint8_t tmp1 = 0x00;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* 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;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* 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;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
return IMU_6AXES_OK;
}
/**
* @brief Disable free fall detection
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_Disable_Free_Fall_Detection( void )
{
uint8_t tmp1 = 0x00;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* INT1_FF setting */
tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* FF_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT;
/* FF_THS setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
return IMU_6AXES_OK;
}
/*fifo block data update*/
static imu_status_t imu_sensor_fifo_block_data_update(void)
{
uint8_t tmp1 = 0x00;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
tmp1 &= 0xbf;
tmp1 |= 0x40;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
return imu_status_ok;
}
/*fifo threshold interrupt*/
static imu_status_t imu_sensor_fifo_threshold_interrupt(void)
{
uint8_t tmp1;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_INT1_CTRL, 1) != imu_status_ok)
{
return imu_status_fail;
}
/*enable fifo threshold interrupt*/
tmp1 &= ~(LSM6DS3_XG_FIFO_INT_THRESHOLD_MASK);
tmp1 |= LSM6DS3_XG_FIFO_INT_THRESHOLD_MASK;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_INT1_CTRL, 1) != imu_status_ok)
{
return imu_status_fail;
}
return imu_status_ok;
}
/*lsm6ds3 soft reset*/
static imu_status_t imu_sensor_lsm6ds3_soft_reset(void)
{
uint8_t tmp1;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
/*soft reset*/
tmp1 &= ~(0x01);
tmp1 |= 0x01;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
HAL_Delay(1);
return imu_status_ok;
}
static uint8_t imu_sensor_rounding_pattern(void)
{
uint8_t tmp1 = 0x00;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL5_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
tmp1 &= ~(0x30);
tmp1 |= (0x30);
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL5_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
return imu_status_ok;
}
/*set gyro output state*/
static imu_status_t imu_sensor_gyro_output_status_config(uint8_t status)
{
uint8_t tmp1 = 0x00;
uint8_t eX = 0x00;
uint8_t eY = 0x00;
uint8_t eZ = 0x00;
if(status == OUTPUT_ENABLE) {
eX = LSM6DS3_G_XEN_ENABLE;
eY = LSM6DS3_G_YEN_ENABLE;
eZ = LSM6DS3_G_ZEN_ENABLE;
}
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
/* Enable X axis selection */
tmp1 &= ~(LSM6DS3_G_XEN_MASK);
tmp1 |= eX;
/* Enable Y axis selection */
tmp1 &= ~(LSM6DS3_G_YEN_MASK);
tmp1 |= eY;
/* Enable Z axis selection */
tmp1 &= ~(LSM6DS3_G_ZEN_MASK);
tmp1 |= eZ;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1) != imu_status_ok)
{
return imu_status_fail;
}
return imu_status_ok;
}
/*init lsm303agr*/
static void lsm303agr_init(void)
{
int8_t ret;
#ifdef CANNON_V1
uint8_t byte_read;
//set hub read
byte_read = 0x80;
LSM6DS3_IO_Write(&byte_read, NULL, 0x01, 1); //enable embeded register
byte_read = 0x3d;
LSM6DS3_IO_Write(&byte_read, NULL, 0x02, 1); //set lis3mdl i2c address and read mode
byte_read = 0x68;
LSM6DS3_IO_Write(&byte_read, NULL, 0x03, 1); //set register address as 0x68
byte_read = 0x06;
LSM6DS3_IO_Write(&byte_read, NULL, 0x04, 1); //6 register length
byte_read = 0x00;
LSM6DS3_IO_Write(&byte_read, NULL, 0x01, 1); //disable embeded register
//byte_read = 0x04;
byte_read = 0x3c;
LSM6DS3_IO_Write(&byte_read, NULL, 0x19, 1); //enable embeded funciton
byte_read = 0x09;
LSM6DS3_IO_Write(&byte_read, NULL, 0x1a, 1); //enable pull_up and master
#endif
#ifdef CANNON_V2
ret = init_LSM303AGR_mag(LSM303AGR_MAG_ODR_100Hz);
while(ret == -1)
{
}
#endif
}
/**
* @brief Enable free fall detection
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_Enable_Free_Fall_Detection( void )
{
uint8_t tmp1 = 0x00;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* Output Data Rate selection */
tmp1 &= ~(LSM6DS3_XL_ODR_MASK);
tmp1 |= LSM6DS3_XL_ODR_416HZ;
/* Full scale selection */
tmp1 &= ~(LSM6DS3_XL_FS_MASK);
tmp1 |= LSM6DS3_XL_FS_2G;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* FF_DUR5 setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_DEFAULT;
/* WAKE_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_DEFAULT;
/* TIMER_HR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_DEFAULT;
/* SLEEP_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_DEFAULT;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* FF_DUR setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_TYPICAL;
/* FF_THS setting */
tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_312MG;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
/* INT1_FF setting */
tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_ENABLE;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
return IMU_6AXES_OK;
}
/*set data rate*/
imu_status_t imu_sensor_set_data_rate(uint32_t* p_data_rate, uint8_t mode)
{
uint8_t tmp1 = 0x00;
uint8_t new_odr = 0x00;
/*lsm6ds3*/
printf("fifo odr:%x\n",* p_data_rate);
{
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok)
{
return imu_status_fail;
}
/* FIFO ODR selection */
switch(* p_data_rate) {
case 0:
tmp1 |= LSM6DS3_XG_FIFO_ODR_NA;
case 10:
tmp1 |= LSM6DS3_XG_FIFO_ODR_10HZ;
break;
case 25:
tmp1 |= LSM6DS3_XG_FIFO_ODR_25HZ;
break;
case 50:
tmp1 |= LSM6DS3_XG_FIFO_ODR_50HZ;
break;
case 100:
tmp1 |= LSM6DS3_XG_FIFO_ODR_100HZ;
break;
case 200:
tmp1 |= LSM6DS3_XG_FIFO_ODR_200HZ;
break;
case 400:
tmp1 |= LSM6DS3_XG_FIFO_ODR_400HZ;
break;
case 800:
tmp1 |= LSM6DS3_XG_FIFO_ODR_800HZ;
break;
case 1600:
tmp1 |= LSM6DS3_XG_FIFO_ODR_1600HZ;
break;
default:
break;
}
/* FIFO mode selection */
tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK);
//tmp1 |= LSM6DS3_XG_FIFO_MODE_FIFO;
/*continus mode*/
tmp1 |= LSM6DS3_XG_FIFO_MODE_CONTINUOUS_OVERWRITE;
if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok)
{
return imu_status_fail;
}
}
/*acc and gyro odr */
{
new_odr = ( * p_data_rate <= 0 ) ? LSM6DS3_XL_ODR_PD /* Power Down */
: ( * p_data_rate <= 13 ) ? LSM6DS3_XL_ODR_13HZ
: ( * p_data_rate <= 26 ) ? LSM6DS3_XL_ODR_26HZ
: ( * p_data_rate <= 52 ) ? LSM6DS3_XL_ODR_52HZ
: ( * p_data_rate <= 104 ) ? LSM6DS3_XL_ODR_104HZ
: ( * p_data_rate <= 208 ) ? LSM6DS3_XL_ODR_208HZ
: ( * p_data_rate <= 416 ) ? LSM6DS3_XL_ODR_416HZ
: ( * p_data_rate <= 833 ) ? LSM6DS3_XL_ODR_833HZ
: LSM6DS3_XL_ODR_1660HZ;
if(sensor_selection & ACC_ENABLE) {
if(imu_sensor_config_acc(new_odr, LSM6DS3_XL_FS_2G) != imu_status_ok) {
return imu_status_fail;
}
}
if(sensor_selection & GYRO_ENABLE) {
if(imu_sensor_config_gyro(new_odr, LSM6DS3_G_FS_125_ENABLE) != imu_status_ok) {
return imu_status_fail;
}
}
printf("acc and gyro odr set over:%x\n",new_odr);
}
/*lsm303agr*/
{
if(LSM303AGR_MAG_W_ODR(LSM303AGR_MAG_ODR_100Hz) != imu_status_ok)
{
return imu_status_fail;
}
printf("mag odr set over\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;
}
