Peter Ferland
Multitech mDot/UDK support.
Dependencies: ST_INTERFACES X_NUCLEO_COMMON
Fork of
X_NUCLEO_IKS01A1
by 
ST
Diff: Components/lsm6ds0/lsm6ds0_class.cpp
- Revision:
- 24:92cc9c6e4b2b
- Parent:
- 7:a2bb3d5e12e9
- Child:
- 57:04563dd74269
diff -r 9da7be2a27c1 -r 92cc9c6e4b2b Components/lsm6ds0/lsm6ds0_class.cpp
--- a/Components/lsm6ds0/lsm6ds0_class.cpp Fri May 29 16:50:56 2015 +0200
+++ b/Components/lsm6ds0/lsm6ds0_class.cpp Wed Jun 03 14:57:57 2015 +0200
@@ -43,8 +43,8 @@
/* Methods -------------------------------------------------------------------*/
/* betzw - based on:
- X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds0/lsm6ds0.c: revision #184,
- X-CUBE-MEMS1/trunk: revision #293
+ X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds0/lsm6ds0.c: revision #400,
+ X-CUBE-MEMS1/trunk: revision #402
*/
/**
* @brief Set LSM6DS0 Initialization
@@ -53,106 +53,54 @@
*/
IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init)
{
- uint8_t tmp1 = 0x00;
-
- /* Configure the low level interface ---------------------------------------*/
- if(LSM6DS0_IO_Init() != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /******* Gyroscope init *******/
-
- if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /* Output Data Rate selection */
- tmp1 &= ~(LSM6DS0_G_ODR_MASK);
- tmp1 |= LSM6DS0_Init->G_OutputDataRate;
-
- /* Full scale selection */
- tmp1 &= ~(LSM6DS0_G_FS_MASK);
- tmp1 |= LSM6DS0_Init->G_FullScale;
-
- if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /* Enable X axis selection */
- tmp1 &= ~(LSM6DS0_G_XEN_MASK);
- tmp1 |= LSM6DS0_Init->G_X_Axis;
-
- /* Enable Y axis selection */
- tmp1 &= ~(LSM6DS0_G_YEN_MASK);
- tmp1 |= LSM6DS0_Init->G_Y_Axis;
-
- /* Enable Z axis selection */
- tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
- tmp1 |= LSM6DS0_Init->G_Z_Axis;
-
- if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /******************************/
-
- /***** Accelerometer init *****/
-
- if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /* Output Data Rate selection */
- tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
- tmp1 |= LSM6DS0_Init->X_OutputDataRate;
-
- /* Full scale selection */
- tmp1 &= ~(LSM6DS0_XL_FS_MASK);
- tmp1 |= LSM6DS0_Init->X_FullScale;
-
- if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /* Enable X axis selection */
- tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
- tmp1 |= LSM6DS0_Init->X_X_Axis;
-
- /* Enable Y axis selection */
- tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
- tmp1 |= LSM6DS0_Init->X_Y_Axis;
-
- /* Enable Z axis selection */
- tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
- tmp1 |= LSM6DS0_Init->X_Z_Axis;
-
- if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- /* Configure interrupt lines */
- LSM6DS0_IO_ITConfig();
-
- return IMU_6AXES_OK;
-
- /******************************/
+ /* Configure the low level interface ---------------------------------------*/
+ if(LSM6DS0_IO_Init() != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ /******* Gyroscope init *******/
+
+ if(LSM6DS0_G_Set_ODR( LSM6DS0_Init->G_OutputDataRate ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ if(LSM6DS0_G_Set_FS( LSM6DS0_Init->G_FullScale ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ if(LSM6DS0_G_Set_Axes_Status(LSM6DS0_Init->G_X_Axis, LSM6DS0_Init->G_Y_Axis, LSM6DS0_Init->G_Z_Axis) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ /******************************/
+
+ /***** Accelerometer init *****/
+
+ if(LSM6DS0_X_Set_ODR( LSM6DS0_Init->X_OutputDataRate ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ if(LSM6DS0_X_Set_FS( LSM6DS0_Init->X_FullScale ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ if(LSM6DS0_X_Set_Axes_Status(LSM6DS0_Init->X_X_Axis, LSM6DS0_Init->X_Y_Axis, LSM6DS0_Init->X_Z_Axis) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ /* Configure interrupt lines */
+ LSM6DS0_IO_ITConfig();
+
+ return IMU_6AXES_OK;
+
+ /******************************/
}
@@ -163,12 +111,12 @@
*/
IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Read_XG_ID(uint8_t *xg_id)
{
- if(!xg_id)
- {
- return IMU_6AXES_ERROR;
- }
-
- return LSM6DS0_IO_Read(xg_id, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
+ if(!xg_id)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return LSM6DS0_IO_Read(xg_id, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
}
@@ -179,30 +127,33 @@
*/
IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxesRaw(int16_t *pData)
{
- uint8_t tempReg[2] = {0,0};
-
- if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- return IMU_6AXES_OK;
+ uint8_t tempReg[2] = {0, 0};
+
+ if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
+ 2) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
+
+ if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
+ 2) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
+
+ if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
+ 2) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
+
+ return IMU_6AXES_OK;
}
@@ -213,40 +164,24 @@
*/
IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxes(int32_t *pData)
{
- uint8_t tempReg = 0x00;
- int16_t pDataRaw[3];
- float sensitivity = 0;
-
- if(LSM6DS0_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- if(LSM6DS0_IO_Read(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- tempReg &= LSM6DS0_XL_FS_MASK;
-
- switch(tempReg)
- {
- case LSM6DS0_XL_FS_2G:
- sensitivity = 0.061;
- break;
- case LSM6DS0_XL_FS_4G:
- sensitivity = 0.122;
- break;
- case LSM6DS0_XL_FS_8G:
- sensitivity = 0.244;
- break;
- }
-
- 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;
+ int16_t pDataRaw[3];
+ float sensitivity = 0;
+
+ if(LSM6DS0_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ if(LSM6DS0_X_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;
}
@@ -257,30 +192,119 @@
*/
IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxesRaw(int16_t *pData)
{
- uint8_t tempReg[2] = {0,0};
-
- if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+ uint8_t tempReg[2] = {0, 0};
+
+ if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
+ 2) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
+
+ if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
+ 2) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
+
+ if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
+ 2) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
+
+ return IMU_6AXES_OK;
+}
- if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
+/**
+ * @brief Set the status of the axes for accelerometer
+ * @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
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_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 ) ? LSM6DS0_XL_XEN_DISABLE : LSM6DS0_XL_XEN_ENABLE;
+ eY = ( enableY == 0 ) ? LSM6DS0_XL_YEN_DISABLE : LSM6DS0_XL_YEN_ENABLE;
+ eZ = ( enableZ == 0 ) ? LSM6DS0_XL_ZEN_DISABLE : LSM6DS0_XL_ZEN_ENABLE;
+
+ if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ /* Enable X axis selection */
+ tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
+ tmp1 |= eX;
+
+ /* Enable Y axis selection */
+ tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
+ tmp1 |= eY;
+
+ /* Enable Z axis selection */
+ tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
+ tmp1 |= eZ;
+
+ if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return IMU_6AXES_OK;
+}
- if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-
- 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
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_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 ) ? LSM6DS0_G_XEN_DISABLE : LSM6DS0_G_XEN_ENABLE;
+ eY = ( enableY == 0 ) ? LSM6DS0_G_YEN_DISABLE : LSM6DS0_G_YEN_ENABLE;
+ eZ = ( enableZ == 0 ) ? LSM6DS0_G_ZEN_DISABLE : LSM6DS0_G_ZEN_ENABLE;
+
+ if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ /* Enable X axis selection */
+ tmp1 &= ~(LSM6DS0_G_XEN_MASK);
+ tmp1 |= eX;
+
+ /* Enable Y axis selection */
+ tmp1 &= ~(LSM6DS0_G_YEN_MASK);
+ tmp1 |= eY;
+
+ /* Enable Z axis selection */
+ tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
+ tmp1 |= eZ;
+
+ if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return IMU_6AXES_OK;
}
@@ -291,40 +315,105 @@
*/
IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxes(int32_t *pData)
{
- uint8_t tempReg = 0x00;
- int16_t pDataRaw[3];
- float sensitivity = 0;
-
- if(LSM6DS0_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- if(LSM6DS0_IO_Read(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- tempReg &= LSM6DS0_G_FS_MASK;
+ int16_t pDataRaw[3];
+ float sensitivity = 0;
+
+ if(LSM6DS0_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ if(LSM6DS0_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;
+}
- switch(tempReg)
- {
- case LSM6DS0_G_FS_245:
- sensitivity = 8.75;
- break;
- case LSM6DS0_G_FS_500:
- sensitivity = 17.50;
- break;
- case LSM6DS0_G_FS_2000:
- sensitivity = 70;
- break;
- }
+/**
+ * @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
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Get_ODR( float *odr )
+{
+ /*Here we have to add the check if the parameters are valid*/
+ uint8_t tempReg = 0x00;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= LSM6DS0_XL_ODR_MASK;
+
+ switch( tempReg )
+ {
+ case LSM6DS0_XL_ODR_PD:
+ *odr = 0.0f;
+ break;
+ case LSM6DS0_XL_ODR_10HZ:
+ *odr = 10.0f;
+ break;
+ case LSM6DS0_XL_ODR_50HZ:
+ *odr = 50.0f;
+ break;
+ case LSM6DS0_XL_ODR_119HZ:
+ *odr = 119.0f;
+ break;
+ case LSM6DS0_XL_ODR_238HZ:
+ *odr = 238.0f;
+ break;
+ case LSM6DS0_XL_ODR_476HZ:
+ *odr = 476.0f;
+ break;
+ case LSM6DS0_XL_ODR_952HZ:
+ *odr = 952.0f;
+ break;
+ default:
+ break;
+ }
+
+ return IMU_6AXES_OK;
+}
- 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;
+/**
+ * @brief Write Accelero Output Data Rate
+ * @param odr the accelerometer output data rate to be set
+ * @retval IMU_6AXES_OK in case of success, an error code otherwise
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Set_ODR( float odr )
+{
+ uint8_t new_odr = 0x00;
+ uint8_t tempReg = 0x00;
+
+ new_odr = ( odr <= 0.0f ) ? LSM6DS0_XL_ODR_PD /* Power Down */
+ : ( odr <= 10.0f ) ? LSM6DS0_XL_ODR_10HZ
+ : ( odr <= 50.0f ) ? LSM6DS0_XL_ODR_50HZ
+ : ( odr <= 119.0f ) ? LSM6DS0_XL_ODR_119HZ
+ : ( odr <= 238.0f ) ? LSM6DS0_XL_ODR_238HZ
+ : ( odr <= 476.0f ) ? LSM6DS0_XL_ODR_476HZ
+ : LSM6DS0_XL_ODR_952HZ;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= ~(LSM6DS0_XL_ODR_MASK);
+ tempReg |= new_odr;
+
+ if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return IMU_6AXES_OK;
}
/**
@@ -332,69 +421,287 @@
* @param pfData the pointer where the accelerometer sensitivity is stored
* @retval IMU_6AXES_OK in case of success, an error code otherwise
*/
-IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetSensitivity( float *pfData )
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetSensitivity( float *pfData )
{
- /*Here we have to add the check if the parameters are valid*/
- uint8_t tempReg = 0x00;
-
- if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- tempReg &= LSM6DS0_XL_FS_MASK;
-
- switch( tempReg )
- {
- case LSM6DS0_XL_FS_2G:
- *pfData = 0.061;
- break;
- case LSM6DS0_XL_FS_4G:
- *pfData = 0.122;
- break;
- case LSM6DS0_XL_FS_8G:
- *pfData = 0.244;
- break;
- default:
- break;
- }
-
- return IMU_6AXES_OK;
+ /*Here we have to add the check if the parameters are valid*/
+ uint8_t tempReg = 0x00;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= LSM6DS0_XL_FS_MASK;
+
+ switch( tempReg )
+ {
+ case LSM6DS0_XL_FS_2G:
+ *pfData = 0.061f;
+ break;
+ case LSM6DS0_XL_FS_4G:
+ *pfData = 0.122f;
+ break;
+ case LSM6DS0_XL_FS_8G:
+ *pfData = 0.244f;
+ break;
+ case LSM6DS0_XL_FS_16G:
+ *pfData = 0.732f;
+ break;
+ default:
+ break;
+ }
+
+ 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
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Get_FS( float *fullScale )
+{
+ /*Here we have to add the check if the parameters are valid*/
+ uint8_t tempReg = 0x00;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= LSM6DS0_XL_FS_MASK;
+
+ switch( tempReg )
+ {
+ case LSM6DS0_XL_FS_2G:
+ *fullScale = 2.0f;
+ break;
+ case LSM6DS0_XL_FS_4G:
+ *fullScale = 4.0f;
+ break;
+ case LSM6DS0_XL_FS_8G:
+ *fullScale = 8.0f;
+ break;
+ case LSM6DS0_XL_FS_16G:
+ *fullScale = 16.0f;
+ break;
+ default:
+ break;
+ }
+
+ 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
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Set_FS( float fullScale )
+{
+ uint8_t new_fs = 0x00;
+ uint8_t tempReg = 0x00;
+
+ new_fs = ( fullScale <= 2.0f ) ? LSM6DS0_XL_FS_2G
+ : ( fullScale <= 4.0f ) ? LSM6DS0_XL_FS_4G
+ : ( fullScale <= 8.0f ) ? LSM6DS0_XL_FS_8G
+ : LSM6DS0_XL_FS_16G;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= ~(LSM6DS0_XL_FS_MASK);
+ tempReg |= new_fs;
+
+ if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return IMU_6AXES_OK;
+}
+/**
+ * @brief Read Gyro Output Data Rate
+ * @param odr the pointer where the gyroscope output data rate is stored
+ * @retval IMU_6AXES_OK in case of success, an error code otherwise
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Get_ODR( float *odr )
+{
+ /*Here we have to add the check if the parameters are valid*/
+ uint8_t tempReg = 0x00;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= LSM6DS0_G_ODR_MASK;
+
+ switch( tempReg )
+ {
+ case LSM6DS0_G_ODR_PD:
+ *odr = 0.0f;
+ break;
+ case LSM6DS0_G_ODR_14_9HZ:
+ *odr = 14.9f;
+ break;
+ case LSM6DS0_G_ODR_59_5HZ:
+ *odr = 59.5f;
+ break;
+ case LSM6DS0_G_ODR_119HZ:
+ *odr = 119.0f;
+ break;
+ case LSM6DS0_G_ODR_238HZ:
+ *odr = 238.0f;
+ break;
+ case LSM6DS0_G_ODR_476HZ:
+ *odr = 476.0f;
+ break;
+ case LSM6DS0_G_ODR_952HZ:
+ *odr = 952.0f;
+ break;
+ default:
+ break;
+ }
+
+ 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
+ */
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Set_ODR( float odr )
+{
+ uint8_t new_odr = 0x00;
+ uint8_t tempReg = 0x00;
+
+ new_odr = ( odr <= 0.0f ) ? LSM6DS0_G_ODR_PD /* Power Down */
+ : ( odr <= 14.9f ) ? LSM6DS0_G_ODR_14_9HZ
+ : ( odr <= 59.5f ) ? LSM6DS0_G_ODR_59_5HZ
+ : ( odr <= 119.0f ) ? LSM6DS0_G_ODR_119HZ
+ : ( odr <= 238.0f ) ? LSM6DS0_G_ODR_238HZ
+ : ( odr <= 476.0f ) ? LSM6DS0_G_ODR_476HZ
+ : LSM6DS0_G_ODR_952HZ;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= ~(LSM6DS0_G_ODR_MASK);
+ tempReg |= new_odr;
+
+ if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return IMU_6AXES_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
*/
-IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetSensitivity( float *pfData )
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetSensitivity( float *pfData )
+{
+ /*Here we have to add the check if the parameters are valid*/
+ uint8_t tempReg = 0x00;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= LSM6DS0_G_FS_MASK;
+
+ switch( tempReg )
+ {
+ case LSM6DS0_G_FS_245:
+ *pfData = 8.75f;
+ break;
+ case LSM6DS0_G_FS_500:
+ *pfData = 17.50f;
+ break;
+ case LSM6DS0_G_FS_2000:
+ *pfData = 70.0f;
+ break;
+ default:
+ break;
+ }
+
+ return IMU_6AXES_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
+*/
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Get_FS( float *fullScale )
{
- /*Here we have to add the check if the parameters are valid*/
- uint8_t tempReg = 0x00;
-
- if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
- {
- return IMU_6AXES_ERROR;
- }
-
- tempReg &= LSM6DS0_G_FS_MASK;
-
- switch( tempReg )
- {
- case LSM6DS0_G_FS_245:
- *pfData = 8.75;
- break;
- case LSM6DS0_G_FS_500:
- *pfData = 17.50;
- break;
- case LSM6DS0_G_FS_2000:
- *pfData = 70;
- break;
- default:
- break;
- }
-
- return IMU_6AXES_OK;
+ /*Here we have to add the check if the parameters are valid*/
+ uint8_t tempReg = 0x00;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= LSM6DS0_G_FS_MASK;
+
+ switch( tempReg )
+ {
+ case LSM6DS0_G_FS_245:
+ *fullScale = 245.0f;
+ break;
+ case LSM6DS0_G_FS_500:
+ *fullScale = 500.0f;
+ break;
+ case LSM6DS0_G_FS_2000:
+ *fullScale = 2000.0f;
+ break;
+ default:
+ break;
+ }
+
+ return IMU_6AXES_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
+*/
+IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Set_FS( float fullScale )
+{
+ uint8_t new_fs = 0x00;
+ uint8_t tempReg = 0x00;
+
+ new_fs = ( fullScale <= 245.0f ) ? LSM6DS0_G_FS_245
+ : ( fullScale <= 500.0f ) ? LSM6DS0_G_FS_500
+ : LSM6DS0_G_FS_2000;
+
+ if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ tempReg &= ~(LSM6DS0_G_FS_MASK);
+ tempReg |= new_fs;
+
+ if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
+ {
+ return IMU_6AXES_ERROR;
+ }
+
+ return IMU_6AXES_OK;
+}
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/