/**
* @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);
}
imu_status_t imu_sensor_read_fifo_status(void)
{
uint8_t fifo_1_number = 0,fifo_2_number = 0;
uint32_t p_data_rate = 0;
uint16_t fifo_data_number = 0;
if(LSM6DS3_IO_Read(&fifo_1_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS1, 1) != imu_status_ok)
{
return imu_status_fail;
}
if(LSM6DS3_IO_Read(&fifo_2_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS2, 1) != imu_status_ok)
{
return imu_status_fail;
}
printf("fifo status: %x\n",fifo_2_number);
fifo_data_number |= (fifo_2_number & 0x0F);
fifo_data_number = (fifo_data_number << 8) | fifo_1_number;
printf("number of unread words: %d\n",fifo_data_number);
return imu_status_ok;
}
/**
* @brief Read raw data from LSM6DS3 Gyroscope output register
* @param pData the pointer where the gyroscope raw data are stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_GetAxesRaw( int16_t *pData )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg[2] = {0, 0};
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_X_L_G, 2) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Y_L_G, 2) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Z_L_G, 2) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
return IMU_6AXES_OK;
}
static imu_status_t imu_sensor_clear_fifo(void)
{
uint8_t tempReg[2] = {0, 0};
uint8_t fifo_1_number = 0,fifo_2_number = 0;
uint32_t p_data_rate = 10;
uint16_t fifo_data_number = 0;
if(LSM6DS3_IO_Read(&fifo_1_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS1, 1) != imu_status_ok)
{
return imu_status_fail;
}
if(LSM6DS3_IO_Read(&fifo_2_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS2, 1) != imu_status_ok)
{
return imu_status_fail;
}
fifo_data_number |= (fifo_2_number & 0x0F);
fifo_data_number = (fifo_data_number << 8) | fifo_1_number;
imu_sensor_set_data_rate(&p_data_rate, LSM6DS3_XG_FIFO_MODE_BYPASS);
printf("set data rate to 0x00\n");
uint16_t i;
for(i = 0; i < fifo_data_number; i++){
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, 2) != imu_status_ok)
{
printf("read sensor error\n");
return imu_status_fail;
}
}
printf("clear fifo over \n");
return imu_status_ok;
}
/**
* @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;
}
/**
* @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 Read Accelero Full Scale
* @param fullScale the pointer where the accelerometer full scale is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_X_Get_FS( float *fullScale )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
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;
switch( tempReg )
{
case LSM6DS3_XL_FS_2G:
*fullScale = 2.0f;
break;
case LSM6DS3_XL_FS_4G:
*fullScale = 4.0f;
break;
case LSM6DS3_XL_FS_8G:
*fullScale = 8.0f;
break;
case LSM6DS3_XL_FS_16G:
*fullScale = 16.0f;
break;
default:
break;
}
return IMU_6AXES_OK;
}
/**
* @brief Read Accelero Sensitivity
* @param pfData the pointer where the accelerometer sensitivity is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_X_GetSensitivity( float *pfData )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
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;
switch( tempReg )
{
case LSM6DS3_XL_FS_2G:
*pfData = 0.061f;
break;
case LSM6DS3_XL_FS_4G:
*pfData = 0.122f;
break;
case LSM6DS3_XL_FS_8G:
*pfData = 0.244f;
break;
case LSM6DS3_XL_FS_16G:
*pfData = 0.488f;
break;
default:
break;
}
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 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;
}
/*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;
}
/*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 read*/
void imu_sensor_read_data_from_fifo(void* arg)
{
uint16_t fifo_sample_number = 0;
/*fifo sample number*/
if(imu_sensor_fifo_data_number(&fifo_sample_number) != imu_status_ok) {
printf("imu_sensor_fifo_data_number error \n");
return ;
}
if(fifo_sample_number < 6)
{
if(imu_sensor_bypass_clear_fifo(NULL) != imu_status_ok)
{
printf("imu_sensor_bypass_clear_fifo error\n");
return;
}
}
/*fifo pattern*/
number_tmp = imu_sensor_read_fifo_pattern();
if(number_tmp != 0){
number_tmp = 12 - (number_tmp*2);
}
/*read from dma*/
if(LSM6DS3_IO_Read(tempReg_sensor, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, DMA_READ_6AXIS_DEPTH) != imu_status_ok)
{
printf("LSM6DS3_IO_Read_DMA error \n");
return;
}
}
/**
* @brief Read Accelero Sensitivity
* @param pfData the pointer where the accelerometer sensitivity is stored
* @retval none
*/
void LSM6DS3_X_GetSensitivity( float *pfData )
{
/*Here we could check if the parameters are valid*/
uint8_t tempReg = 0x00;
LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 );
tempReg &= LSM6DS3_XL_FS_MASK;
switch( tempReg ) {
case LSM6DS3_XL_FS_2G:
*pfData = 0.061;
break;
case LSM6DS3_XL_FS_4G:
*pfData = 0.122;
break;
case LSM6DS3_XL_FS_8G:
*pfData = 0.244;
break;
case LSM6DS3_XL_FS_16G:
*pfData = 0.488;
break;
default:
break;
}
}
/**
* @brief Read Gyro Sensitivity
* @param pfData the pointer where the gyroscope sensitivity is stored
* @retval none
*/
void LSM6DS3_G_GetSensitivity( float *pfData )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 );
tempReg &= LSM6DS3_G_FS_MASK;
switch( tempReg ) {
case LSM6DS3_G_FS_125:
*pfData = 4.375;
break;
case LSM6DS3_G_FS_245:
*pfData = 8.75;
break;
case LSM6DS3_G_FS_500:
*pfData = 17.50;
break;
case LSM6DS3_G_FS_1000:
*pfData = 35;
break;
case LSM6DS3_G_FS_2000:
*pfData = 70;
break;
default:
break;
}
}
void LSM6DS3_G_GetAxesRaw( int16_t *pData )
{
/*Here we could check if the parameters are valid*/
uint8_t tempReg[2] = {0,0};
LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_X_L_G, 2);
pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Y_L_G, 2);
pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Z_L_G, 2);
pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
}
void LSM6DS3_X_GetAxes( int32_t *pData )
{
/*Here we could check if the parameters are valid*/
uint8_t tempReg = 0x00;
int16_t pDataRaw[3];
float sensitivity = 0;
LSM6DS3_X_GetAxesRaw(pDataRaw);
LSM6DS3_IO_Read(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1);
tempReg &= LSM6DS3_XL_FS_MASK;
switch(tempReg)
{
case LSM6DS3_XL_FS_2G:
sensitivity = 0.061;
break;
case LSM6DS3_XL_FS_4G:
sensitivity = 0.122;
break;
case LSM6DS3_XL_FS_8G:
sensitivity = 0.244;
break;
case LSM6DS3_XL_FS_16G:
sensitivity = 0.488;
break;
}
pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
}
static imu_status_t imu_sensor_acc_get_sensitivity( float *pfData )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != imu_status_ok)
{
return imu_status_fail;
}
tempReg &= LSM6DS3_XL_FS_MASK;
switch( tempReg )
{
case LSM6DS3_XL_FS_2G:
*pfData = 0.061f;
break;
case LSM6DS3_XL_FS_4G:
*pfData = 0.122f;
break;
case LSM6DS3_XL_FS_8G:
*pfData = 0.244f;
break;
case LSM6DS3_XL_FS_16G:
*pfData = 0.488f;
break;
default:
break;
}
return imu_status_ok;
}
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 Read Gyro Full Scale
* @param fullScale the pointer where the gyroscope full scale is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_Get_FS( float *fullScale )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
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;
if(tempReg == LSM6DS3_G_FS_125_ENABLE)
{
*fullScale = 125.0f;
}
else
{
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;
switch( tempReg )
{
case LSM6DS3_G_FS_245:
*fullScale = 245.0f;
break;
case LSM6DS3_G_FS_500:
*fullScale = 500.0f;
break;
case LSM6DS3_G_FS_1000:
*fullScale = 1000.0f;
break;
case LSM6DS3_G_FS_2000:
*fullScale = 2000.0f;
break;
default:
break;
}
}
return IMU_6AXES_OK;
}
static imu_status_t imu_sensor_gyro_get_sensitivity( float *pfData )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != imu_status_ok)
{
return imu_status_fail;
}
tempReg &= LSM6DS3_G_FS_125_MASK;
if(tempReg == LSM6DS3_G_FS_125_ENABLE)
{
*pfData = 4.375f;
}
else
{
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != imu_status_ok)
{
return imu_status_fail;
}
tempReg &= LSM6DS3_G_FS_MASK;
switch( tempReg )
{
case LSM6DS3_G_FS_245:
*pfData = 8.75f;
break;
case LSM6DS3_G_FS_500:
*pfData = 17.50f;
break;
case LSM6DS3_G_FS_1000:
*pfData = 35.0f;
break;
case LSM6DS3_G_FS_2000:
*pfData = 70.0f;
break;
default:
break;
}
}
return imu_status_ok;
}
/**
* @brief Read Gyro Sensitivity
* @param pfData the pointer where the gyroscope sensitivity is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_GetSensitivity( float *pfData )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
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;
if(tempReg == LSM6DS3_G_FS_125_ENABLE)
{
*pfData = 4.375f;
}
else
{
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;
switch( tempReg )
{
case LSM6DS3_G_FS_245:
*pfData = 8.75f;
break;
case LSM6DS3_G_FS_500:
*pfData = 17.50f;
break;
case LSM6DS3_G_FS_1000:
*pfData = 35.0f;
break;
case LSM6DS3_G_FS_2000:
*pfData = 70.0f;
break;
default:
break;
}
}
return IMU_6AXES_OK;
}
/**
* @brief Read ID of LSM6DS3 Accelerometer and Gyroscope
* @param xg_id the pointer where the ID of the device is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_Read_XG_ID( uint8_t *xg_id)
{
if(!xg_id)
{
return IMU_6AXES_ERROR;
}
return LSM6DS3_IO_Read(xg_id, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WHO_AM_I_ADDR, 1);
}
static imu_status_t imu_sensor_fifo_data_number(uint16_t* number)
{
uint8_t fifo_1_number = 0,fifo_2_number = 0;
if(LSM6DS3_IO_Read(&fifo_1_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS1, 1) != imu_status_ok)
{
return imu_status_fail;
}
if(LSM6DS3_IO_Read(&fifo_2_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS2, 1) != imu_status_ok)
{
return imu_status_fail;
}
*number |= (fifo_2_number & 0x0F);
*number = (*number << 8) | fifo_1_number;
return imu_status_ok;
}
/*read fifo pattern*/
static uint8_t imu_sensor_read_fifo_pattern(void)
{
uint8_t tmp1 = 0x00;
if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS3, 1) != imu_status_ok)
{
return 0xff;
}
return tmp1;
}
/**
* @brief Read Accelero Output Data Rate
* @param odr the pointer where the accelerometer output data rate is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_X_Get_ODR( float *odr )
{
/*Here we have to add the check if the parameters are valid*/
uint8_t tempReg = 0x00;
if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
tempReg &= LSM6DS3_XL_ODR_MASK;
switch( tempReg )
{
case LSM6DS3_XL_ODR_PD:
*odr = 0.0f;
break;
case LSM6DS3_XL_ODR_13HZ:
*odr = 13.0f;
break;
case LSM6DS3_XL_ODR_26HZ:
*odr = 26.0f;
break;
case LSM6DS3_XL_ODR_52HZ:
*odr = 52.0f;
break;
case LSM6DS3_XL_ODR_104HZ:
*odr = 104.0f;
break;
case LSM6DS3_XL_ODR_208HZ:
*odr = 208.0f;
break;
case LSM6DS3_XL_ODR_416HZ:
*odr = 416.0f;
break;
case LSM6DS3_XL_ODR_833HZ:
*odr = 833.0f;
break;
case LSM6DS3_XL_ODR_1660HZ:
*odr = 1660.0f;
break;
case LSM6DS3_XL_ODR_3330HZ:
*odr = 3330.0f;
break;
case LSM6DS3_XL_ODR_6660HZ:
*odr = 6660.0f;
break;
default:
break;
}
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;
}
