/**
* @brief Read data from LSM6DS3 Gyroscope and calculate angular rate in mdps
* @param pData the pointer where the gyroscope data are stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
static IMU_6AXES_StatusTypeDef LSM6DS3_G_GetAxes( int32_t *pData )
{
/*Here we have to add the check if the parameters are valid*/
int16_t pDataRaw[3];
float sensitivity = 0.0f;
if(LSM6DS3_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
if(LSM6DS3_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
{
return IMU_6AXES_ERROR;
}
pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
return IMU_6AXES_OK;
}
void LSM6DS3_G_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_G_GetAxesRaw(pDataRaw);
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:
sensitivity = 4.375;
break;
case LSM6DS3_G_FS_245:
sensitivity = 8.75;
break;
case LSM6DS3_G_FS_500:
sensitivity = 17.50;
break;
case LSM6DS3_G_FS_1000:
sensitivity = 35;
break;
case LSM6DS3_G_FS_2000:
sensitivity = 70;
break;
}
pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
}
