Shih-Ho Hsieh / XYZ_sensor_Platform_SPI

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of XYZ_sensor_Platform by Shih-Ho Hsieh

AltIMU_10_v5/components/lsm6ds33/lsm6ds33_class.cpp@2:856f03e64695, 2017-11-09 (annotated)

Committer:
hober
Date:
Thu Nov 09 03:40:43 2017 +0000
Revision:
2:856f03e64695
20171109

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hober 2:856f03e64695 1 /**
hober 2:856f03e64695 2 ******************************************************************************
hober 2:856f03e64695 3 * @file lsm6ds33_class.cpp
hober 2:856f03e64695 4 * @author AST / EST
hober 2:856f03e64695 5 * @version V0.0.1
hober 2:856f03e64695 6 * @date 14-April-2015
hober 2:856f03e64695 7 * @brief Implementation file for the LSM6DS33 driver class
hober 2:856f03e64695 8 ******************************************************************************
hober 2:856f03e64695 9 * @attention
hober 2:856f03e64695 10 *
hober 2:856f03e64695 11 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
hober 2:856f03e64695 12 *
hober 2:856f03e64695 13 * Redistribution and use in source and binary forms, with or without modification,
hober 2:856f03e64695 14 * are permitted provided that the following conditions are met:
hober 2:856f03e64695 15 * 1. Redistributions of source code must retain the above copyright notice,
hober 2:856f03e64695 16 * this list of conditions and the following disclaimer.
hober 2:856f03e64695 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
hober 2:856f03e64695 18 * this list of conditions and the following disclaimer in the documentation
hober 2:856f03e64695 19 * and/or other materials provided with the distribution.
hober 2:856f03e64695 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
hober 2:856f03e64695 21 * may be used to endorse or promote products derived from this software
hober 2:856f03e64695 22 * without specific prior written permission.
hober 2:856f03e64695 23 *
hober 2:856f03e64695 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
hober 2:856f03e64695 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
hober 2:856f03e64695 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
hober 2:856f03e64695 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
hober 2:856f03e64695 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
hober 2:856f03e64695 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
hober 2:856f03e64695 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
hober 2:856f03e64695 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
hober 2:856f03e64695 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
hober 2:856f03e64695 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
hober 2:856f03e64695 34 *
hober 2:856f03e64695 35 ******************************************************************************
hober 2:856f03e64695 36 */
hober 2:856f03e64695 37
hober 2:856f03e64695 38 /* Includes ------------------------------------------------------------------*/
hober 2:856f03e64695 39 #include "lsm6ds33_class.h"
hober 2:856f03e64695 40 #include "lsm6ds33.h"
hober 2:856f03e64695 41
hober 2:856f03e64695 42 /* Methods -------------------------------------------------------------------*/
hober 2:856f03e64695 43 /* betzw - based on:
hober 2:856f03e64695 44 X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds33/lsm6ds33.c: revision #400,
hober 2:856f03e64695 45 X-CUBE-MEMS1/trunk: revision #416
hober 2:856f03e64695 46 */
hober 2:856f03e64695 47
hober 2:856f03e64695 48 /**
hober 2:856f03e64695 49 * @brief Set LSM6DS33 Initialization
hober 2:856f03e64695 50 * @param LSM6DS33_Init the configuration setting for the LSM6DS3
hober 2:856f03e64695 51 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 52 */
hober 2:856f03e64695 53 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_Init( IMU_6AXES_InitTypeDef *LSM6DS33_Init )
hober 2:856f03e64695 54 {
hober 2:856f03e64695 55 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 56
hober 2:856f03e64695 57 /* Configure the low level interface -------------------------------------*/
hober 2:856f03e64695 58 if(LSM6DS33_IO_Init() != IMU_6AXES_OK)
hober 2:856f03e64695 59 {
hober 2:856f03e64695 60 return IMU_6AXES_ERROR;
hober 2:856f03e64695 61 }
hober 2:856f03e64695 62
hober 2:856f03e64695 63
hober 2:856f03e64695 64 /******** Common init *********/
hober 2:856f03e64695 65
hober 2:856f03e64695 66 if(LSM6DS33_Common_Sensor_Enable() != IMU_6AXES_OK)
hober 2:856f03e64695 67 {
hober 2:856f03e64695 68 return IMU_6AXES_ERROR;
hober 2:856f03e64695 69 }
hober 2:856f03e64695 70
hober 2:856f03e64695 71
hober 2:856f03e64695 72 /******* Gyroscope init *******/
hober 2:856f03e64695 73
hober 2:856f03e64695 74 if(LSM6DS33_G_Set_ODR( LSM6DS33_Init->G_OutputDataRate ) != IMU_6AXES_OK)
hober 2:856f03e64695 75 {
hober 2:856f03e64695 76 return IMU_6AXES_ERROR;
hober 2:856f03e64695 77 }
hober 2:856f03e64695 78
hober 2:856f03e64695 79 if(LSM6DS33_G_Set_FS( LSM6DS33_Init->G_FullScale ) != IMU_6AXES_OK)
hober 2:856f03e64695 80 {
hober 2:856f03e64695 81 return IMU_6AXES_ERROR;
hober 2:856f03e64695 82 }
hober 2:856f03e64695 83
hober 2:856f03e64695 84 if(LSM6DS33_G_Set_Axes_Status(LSM6DS33_Init->G_X_Axis, LSM6DS33_Init->G_Y_Axis, LSM6DS33_Init->G_Z_Axis) != IMU_6AXES_OK)
hober 2:856f03e64695 85 {
hober 2:856f03e64695 86 return IMU_6AXES_ERROR;
hober 2:856f03e64695 87 }
hober 2:856f03e64695 88
hober 2:856f03e64695 89
hober 2:856f03e64695 90 /***** Accelerometer init *****/
hober 2:856f03e64695 91
hober 2:856f03e64695 92 if(LSM6DS33_X_Set_ODR( LSM6DS33_Init->X_OutputDataRate ) != IMU_6AXES_OK)
hober 2:856f03e64695 93 {
hober 2:856f03e64695 94 return IMU_6AXES_ERROR;
hober 2:856f03e64695 95 }
hober 2:856f03e64695 96
hober 2:856f03e64695 97 if(LSM6DS33_X_Set_FS( LSM6DS33_Init->X_FullScale ) != IMU_6AXES_OK)
hober 2:856f03e64695 98 {
hober 2:856f03e64695 99 return IMU_6AXES_ERROR;
hober 2:856f03e64695 100 }
hober 2:856f03e64695 101
hober 2:856f03e64695 102 if(LSM6DS33_X_Set_Axes_Status(LSM6DS33_Init->X_X_Axis, LSM6DS33_Init->X_Y_Axis, LSM6DS33_Init->X_Z_Axis) != IMU_6AXES_OK)
hober 2:856f03e64695 103 {
hober 2:856f03e64695 104 return IMU_6AXES_ERROR;
hober 2:856f03e64695 105 }
hober 2:856f03e64695 106
hober 2:856f03e64695 107 /* Configure interrupt lines */
hober 2:856f03e64695 108 LSM6DS33_IO_ITConfig();
hober 2:856f03e64695 109
hober 2:856f03e64695 110 return IMU_6AXES_OK;
hober 2:856f03e64695 111 }
hober 2:856f03e64695 112
hober 2:856f03e64695 113 /**
hober 2:856f03e64695 114 * @brief Read ID of LSM6DS33 Accelerometer and Gyroscope
hober 2:856f03e64695 115 * @param xg_id the pointer where the ID of the device is stored
hober 2:856f03e64695 116 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 117 */
hober 2:856f03e64695 118 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_Read_XG_ID( uint8_t *xg_id)
hober 2:856f03e64695 119 {
hober 2:856f03e64695 120 if(!xg_id)
hober 2:856f03e64695 121 {
hober 2:856f03e64695 122 return IMU_6AXES_ERROR;
hober 2:856f03e64695 123 }
hober 2:856f03e64695 124
hober 2:856f03e64695 125 return LSM6DS33_IO_Read(xg_id, LSM6DS33_XG_WHO_AM_I_ADDR, 1);
hober 2:856f03e64695 126 }
hober 2:856f03e64695 127
hober 2:856f03e64695 128 /**
hober 2:856f03e64695 129 * @brief Set LSM6DS33 common initialization
hober 2:856f03e64695 130 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 131 */
hober 2:856f03e64695 132 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_Common_Sensor_Enable(void)
hober 2:856f03e64695 133 {
hober 2:856f03e64695 134 uint8_t tmp1 = 0x00;
hober 2:856f03e64695 135
hober 2:856f03e64695 136 if(LSM6DS33_IO_Read(&tmp1, LSM6DS33_XG_CTRL3_C, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 137 {
hober 2:856f03e64695 138 return IMU_6AXES_ERROR;
hober 2:856f03e64695 139 }
hober 2:856f03e64695 140
hober 2:856f03e64695 141 /* Enable register address automatically incremented during a multiple byte
hober 2:856f03e64695 142 access with a serial interface (I2C or SPI) */
hober 2:856f03e64695 143 tmp1 &= ~(LSM6DS33_XG_IF_INC_MASK);
hober 2:856f03e64695 144 tmp1 |= LSM6DS33_XG_IF_INC;
hober 2:856f03e64695 145
hober 2:856f03e64695 146 if(LSM6DS33_IO_Write(&tmp1, LSM6DS33_XG_CTRL3_C, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 147 {
hober 2:856f03e64695 148 return IMU_6AXES_ERROR;
hober 2:856f03e64695 149 }
hober 2:856f03e64695 150
hober 2:856f03e64695 151
hober 2:856f03e64695 152 if(LSM6DS33_IO_Read(&tmp1, LSM6DS33_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 153 {
hober 2:856f03e64695 154 return IMU_6AXES_ERROR;
hober 2:856f03e64695 155 }
hober 2:856f03e64695 156
hober 2:856f03e64695 157 /* FIFO ODR selection */
hober 2:856f03e64695 158 tmp1 &= ~(LSM6DS33_XG_FIFO_ODR_MASK);
hober 2:856f03e64695 159 tmp1 |= LSM6DS33_XG_FIFO_ODR_NA;
hober 2:856f03e64695 160
hober 2:856f03e64695 161 /* FIFO mode selection */
hober 2:856f03e64695 162 tmp1 &= ~(LSM6DS33_XG_FIFO_MODE_MASK);
hober 2:856f03e64695 163 tmp1 |= LSM6DS33_XG_FIFO_MODE_BYPASS;
hober 2:856f03e64695 164
hober 2:856f03e64695 165 if(LSM6DS33_IO_Write(&tmp1, LSM6DS33_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 166 {
hober 2:856f03e64695 167 return IMU_6AXES_ERROR;
hober 2:856f03e64695 168 }
hober 2:856f03e64695 169
hober 2:856f03e64695 170 return IMU_6AXES_OK;
hober 2:856f03e64695 171 }
hober 2:856f03e64695 172
hober 2:856f03e64695 173 /**
hober 2:856f03e64695 174 * @brief Read raw data from LSM6DS33 Accelerometer output register
hober 2:856f03e64695 175 * @param pData the pointer where the accelerometer raw data are stored
hober 2:856f03e64695 176 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 177 */
hober 2:856f03e64695 178 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_GetAxesRaw( int16_t *pData )
hober 2:856f03e64695 179 {
hober 2:856f03e64695 180 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 181
hober 2:856f03e64695 182 uint8_t tempReg[2] = {0, 0};
hober 2:856f03e64695 183
hober 2:856f03e64695 184
hober 2:856f03e64695 185 if(LSM6DS33_IO_Read(&tempReg[0], LSM6DS33_XG_OUT_X_L_XL, 2) != IMU_6AXES_OK)
hober 2:856f03e64695 186 {
hober 2:856f03e64695 187 return IMU_6AXES_ERROR;
hober 2:856f03e64695 188 }
hober 2:856f03e64695 189
hober 2:856f03e64695 190 pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
hober 2:856f03e64695 191
hober 2:856f03e64695 192 if(LSM6DS33_IO_Read(&tempReg[0], LSM6DS33_XG_OUT_Y_L_XL, 2) != IMU_6AXES_OK)
hober 2:856f03e64695 193 {
hober 2:856f03e64695 194 return IMU_6AXES_ERROR;
hober 2:856f03e64695 195 }
hober 2:856f03e64695 196
hober 2:856f03e64695 197 pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
hober 2:856f03e64695 198
hober 2:856f03e64695 199 if(LSM6DS33_IO_Read(&tempReg[0], LSM6DS33_XG_OUT_Z_L_XL, 2) != IMU_6AXES_OK)
hober 2:856f03e64695 200 {
hober 2:856f03e64695 201 return IMU_6AXES_ERROR;
hober 2:856f03e64695 202 }
hober 2:856f03e64695 203
hober 2:856f03e64695 204 pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
hober 2:856f03e64695 205
hober 2:856f03e64695 206 return IMU_6AXES_OK;
hober 2:856f03e64695 207 }
hober 2:856f03e64695 208
hober 2:856f03e64695 209
hober 2:856f03e64695 210
hober 2:856f03e64695 211 /**
hober 2:856f03e64695 212 * @brief Read data from LSM6DS33 Accelerometer and calculate linear acceleration in mg
hober 2:856f03e64695 213 * @param pData the pointer where the accelerometer data are stored
hober 2:856f03e64695 214 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 215 */
hober 2:856f03e64695 216 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_GetAxes( int32_t *pData )
hober 2:856f03e64695 217 {
hober 2:856f03e64695 218 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 219 int16_t pDataRaw[3];
hober 2:856f03e64695 220 float sensitivity = 0.0f;
hober 2:856f03e64695 221
hober 2:856f03e64695 222 if(LSM6DS33_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
hober 2:856f03e64695 223 {
hober 2:856f03e64695 224 return IMU_6AXES_ERROR;
hober 2:856f03e64695 225 }
hober 2:856f03e64695 226
hober 2:856f03e64695 227 if(LSM6DS33_X_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
hober 2:856f03e64695 228 {
hober 2:856f03e64695 229 return IMU_6AXES_ERROR;
hober 2:856f03e64695 230 }
hober 2:856f03e64695 231
hober 2:856f03e64695 232 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
hober 2:856f03e64695 233 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
hober 2:856f03e64695 234 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
hober 2:856f03e64695 235
hober 2:856f03e64695 236 return IMU_6AXES_OK;
hober 2:856f03e64695 237 }
hober 2:856f03e64695 238
hober 2:856f03e64695 239
hober 2:856f03e64695 240
hober 2:856f03e64695 241 /**
hober 2:856f03e64695 242 * @brief Read raw data from LSM6DS33 Gyroscope output register
hober 2:856f03e64695 243 * @param pData the pointer where the gyroscope raw data are stored
hober 2:856f03e64695 244 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 245 */
hober 2:856f03e64695 246 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_GetAxesRaw( int16_t *pData )
hober 2:856f03e64695 247 {
hober 2:856f03e64695 248 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 249
hober 2:856f03e64695 250 uint8_t tempReg[2] = {0, 0};
hober 2:856f03e64695 251
hober 2:856f03e64695 252
hober 2:856f03e64695 253 if(LSM6DS33_IO_Read(&tempReg[0], LSM6DS33_XG_OUT_X_L_G, 2) != IMU_6AXES_OK)
hober 2:856f03e64695 254 {
hober 2:856f03e64695 255 return IMU_6AXES_ERROR;
hober 2:856f03e64695 256 }
hober 2:856f03e64695 257
hober 2:856f03e64695 258 pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
hober 2:856f03e64695 259
hober 2:856f03e64695 260 if(LSM6DS33_IO_Read(&tempReg[0], LSM6DS33_XG_OUT_Y_L_G, 2) != IMU_6AXES_OK)
hober 2:856f03e64695 261 {
hober 2:856f03e64695 262 return IMU_6AXES_ERROR;
hober 2:856f03e64695 263 }
hober 2:856f03e64695 264
hober 2:856f03e64695 265 pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
hober 2:856f03e64695 266
hober 2:856f03e64695 267 if(LSM6DS33_IO_Read(&tempReg[0], LSM6DS33_XG_OUT_Z_L_G, 2) != IMU_6AXES_OK)
hober 2:856f03e64695 268 {
hober 2:856f03e64695 269 return IMU_6AXES_ERROR;
hober 2:856f03e64695 270 }
hober 2:856f03e64695 271
hober 2:856f03e64695 272 pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
hober 2:856f03e64695 273
hober 2:856f03e64695 274 return IMU_6AXES_OK;
hober 2:856f03e64695 275 }
hober 2:856f03e64695 276
hober 2:856f03e64695 277 /**
hober 2:856f03e64695 278 * @brief Set the status of the axes for accelerometer
hober 2:856f03e64695 279 * @param enableX the status of the x axis to be set
hober 2:856f03e64695 280 * @param enableY the status of the y axis to be set
hober 2:856f03e64695 281 * @param enableZ the status of the z axis to be set
hober 2:856f03e64695 282 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 283 */
hober 2:856f03e64695 284 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
hober 2:856f03e64695 285 {
hober 2:856f03e64695 286 uint8_t tmp1 = 0x00;
hober 2:856f03e64695 287 uint8_t eX = 0x00;
hober 2:856f03e64695 288 uint8_t eY = 0x00;
hober 2:856f03e64695 289 uint8_t eZ = 0x00;
hober 2:856f03e64695 290
hober 2:856f03e64695 291 eX = ( enableX == 0 ) ? LSM6DS33_XL_XEN_DISABLE : LSM6DS33_XL_XEN_ENABLE;
hober 2:856f03e64695 292 eY = ( enableY == 0 ) ? LSM6DS33_XL_YEN_DISABLE : LSM6DS33_XL_YEN_ENABLE;
hober 2:856f03e64695 293 eZ = ( enableZ == 0 ) ? LSM6DS33_XL_ZEN_DISABLE : LSM6DS33_XL_ZEN_ENABLE;
hober 2:856f03e64695 294
hober 2:856f03e64695 295 if(LSM6DS33_IO_Read(&tmp1, LSM6DS33_XG_CTRL9_XL, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 296 {
hober 2:856f03e64695 297 return IMU_6AXES_ERROR;
hober 2:856f03e64695 298 }
hober 2:856f03e64695 299
hober 2:856f03e64695 300 /* Enable X axis selection */
hober 2:856f03e64695 301 tmp1 &= ~(LSM6DS33_XL_XEN_MASK);
hober 2:856f03e64695 302 tmp1 |= eX;
hober 2:856f03e64695 303
hober 2:856f03e64695 304 /* Enable Y axis selection */
hober 2:856f03e64695 305 tmp1 &= ~(LSM6DS33_XL_YEN_MASK);
hober 2:856f03e64695 306 tmp1 |= eY;
hober 2:856f03e64695 307
hober 2:856f03e64695 308 /* Enable Z axis selection */
hober 2:856f03e64695 309 tmp1 &= ~(LSM6DS33_XL_ZEN_MASK);
hober 2:856f03e64695 310 tmp1 |= eZ;
hober 2:856f03e64695 311
hober 2:856f03e64695 312 if(LSM6DS33_IO_Write(&tmp1, LSM6DS33_XG_CTRL9_XL, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 313 {
hober 2:856f03e64695 314 return IMU_6AXES_ERROR;
hober 2:856f03e64695 315 }
hober 2:856f03e64695 316
hober 2:856f03e64695 317 return IMU_6AXES_OK;
hober 2:856f03e64695 318 }
hober 2:856f03e64695 319
hober 2:856f03e64695 320 /**
hober 2:856f03e64695 321 * @brief Set the status of the axes for gyroscope
hober 2:856f03e64695 322 * @param enableX the status of the x axis to be set
hober 2:856f03e64695 323 * @param enableY the status of the y axis to be set
hober 2:856f03e64695 324 * @param enableZ the status of the z axis to be set
hober 2:856f03e64695 325 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 326 */
hober 2:856f03e64695 327 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
hober 2:856f03e64695 328 {
hober 2:856f03e64695 329 uint8_t tmp1 = 0x00;
hober 2:856f03e64695 330 uint8_t eX = 0x00;
hober 2:856f03e64695 331 uint8_t eY = 0x00;
hober 2:856f03e64695 332 uint8_t eZ = 0x00;
hober 2:856f03e64695 333
hober 2:856f03e64695 334 eX = ( enableX == 0 ) ? LSM6DS33_G_XEN_DISABLE : LSM6DS33_G_XEN_ENABLE;
hober 2:856f03e64695 335 eY = ( enableY == 0 ) ? LSM6DS33_G_YEN_DISABLE : LSM6DS33_G_YEN_ENABLE;
hober 2:856f03e64695 336 eZ = ( enableZ == 0 ) ? LSM6DS33_G_ZEN_DISABLE : LSM6DS33_G_ZEN_ENABLE;
hober 2:856f03e64695 337
hober 2:856f03e64695 338 if(LSM6DS33_IO_Read(&tmp1, LSM6DS33_XG_CTRL10_C, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 339 {
hober 2:856f03e64695 340 return IMU_6AXES_ERROR;
hober 2:856f03e64695 341 }
hober 2:856f03e64695 342
hober 2:856f03e64695 343 /* Enable X axis selection */
hober 2:856f03e64695 344 tmp1 &= ~(LSM6DS33_G_XEN_MASK);
hober 2:856f03e64695 345 tmp1 |= eX;
hober 2:856f03e64695 346
hober 2:856f03e64695 347 /* Enable Y axis selection */
hober 2:856f03e64695 348 tmp1 &= ~(LSM6DS33_G_YEN_MASK);
hober 2:856f03e64695 349 tmp1 |= eY;
hober 2:856f03e64695 350
hober 2:856f03e64695 351 /* Enable Z axis selection */
hober 2:856f03e64695 352 tmp1 &= ~(LSM6DS33_G_ZEN_MASK);
hober 2:856f03e64695 353 tmp1 |= eZ;
hober 2:856f03e64695 354
hober 2:856f03e64695 355 if(LSM6DS33_IO_Write(&tmp1, LSM6DS33_XG_CTRL10_C, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 356 {
hober 2:856f03e64695 357 return IMU_6AXES_ERROR;
hober 2:856f03e64695 358 }
hober 2:856f03e64695 359
hober 2:856f03e64695 360 return IMU_6AXES_OK;
hober 2:856f03e64695 361 }
hober 2:856f03e64695 362
hober 2:856f03e64695 363 /**
hober 2:856f03e64695 364 * @brief Read data from LSM6DS33 Gyroscope and calculate angular rate in mdps
hober 2:856f03e64695 365 * @param pData the pointer where the gyroscope data are stored
hober 2:856f03e64695 366 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 367 */
hober 2:856f03e64695 368 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_GetAxes( int32_t *pData )
hober 2:856f03e64695 369 {
hober 2:856f03e64695 370 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 371 int16_t pDataRaw[3];
hober 2:856f03e64695 372 float sensitivity = 0.0f;
hober 2:856f03e64695 373
hober 2:856f03e64695 374 if(LSM6DS33_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
hober 2:856f03e64695 375 {
hober 2:856f03e64695 376 return IMU_6AXES_ERROR;
hober 2:856f03e64695 377 }
hober 2:856f03e64695 378
hober 2:856f03e64695 379 if(LSM6DS33_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
hober 2:856f03e64695 380 {
hober 2:856f03e64695 381 return IMU_6AXES_ERROR;
hober 2:856f03e64695 382 }
hober 2:856f03e64695 383
hober 2:856f03e64695 384 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
hober 2:856f03e64695 385 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
hober 2:856f03e64695 386 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
hober 2:856f03e64695 387
hober 2:856f03e64695 388 return IMU_6AXES_OK;
hober 2:856f03e64695 389 }
hober 2:856f03e64695 390
hober 2:856f03e64695 391 /**
hober 2:856f03e64695 392 * @brief Read Accelero Output Data Rate
hober 2:856f03e64695 393 * @param odr the pointer where the accelerometer output data rate is stored
hober 2:856f03e64695 394 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 395 */
hober 2:856f03e64695 396 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_Get_ODR( float *odr )
hober 2:856f03e64695 397 {
hober 2:856f03e64695 398 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 399 uint8_t tempReg = 0x00;
hober 2:856f03e64695 400
hober 2:856f03e64695 401 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 402 {
hober 2:856f03e64695 403 return IMU_6AXES_ERROR;
hober 2:856f03e64695 404 }
hober 2:856f03e64695 405
hober 2:856f03e64695 406 tempReg &= LSM6DS33_XL_ODR_MASK;
hober 2:856f03e64695 407
hober 2:856f03e64695 408 switch( tempReg )
hober 2:856f03e64695 409 {
hober 2:856f03e64695 410 case LSM6DS33_XL_ODR_PD:
hober 2:856f03e64695 411 *odr = 0.0f;
hober 2:856f03e64695 412 break;
hober 2:856f03e64695 413 case LSM6DS33_XL_ODR_13HZ:
hober 2:856f03e64695 414 *odr = 13.0f;
hober 2:856f03e64695 415 break;
hober 2:856f03e64695 416 case LSM6DS33_XL_ODR_26HZ:
hober 2:856f03e64695 417 *odr = 26.0f;
hober 2:856f03e64695 418 break;
hober 2:856f03e64695 419 case LSM6DS33_XL_ODR_52HZ:
hober 2:856f03e64695 420 *odr = 52.0f;
hober 2:856f03e64695 421 break;
hober 2:856f03e64695 422 case LSM6DS33_XL_ODR_104HZ:
hober 2:856f03e64695 423 *odr = 104.0f;
hober 2:856f03e64695 424 break;
hober 2:856f03e64695 425 case LSM6DS33_XL_ODR_208HZ:
hober 2:856f03e64695 426 *odr = 208.0f;
hober 2:856f03e64695 427 break;
hober 2:856f03e64695 428 case LSM6DS33_XL_ODR_416HZ:
hober 2:856f03e64695 429 *odr = 416.0f;
hober 2:856f03e64695 430 break;
hober 2:856f03e64695 431 case LSM6DS33_XL_ODR_833HZ:
hober 2:856f03e64695 432 *odr = 833.0f;
hober 2:856f03e64695 433 break;
hober 2:856f03e64695 434 case LSM6DS33_XL_ODR_1660HZ:
hober 2:856f03e64695 435 *odr = 1660.0f;
hober 2:856f03e64695 436 break;
hober 2:856f03e64695 437 case LSM6DS33_XL_ODR_3330HZ:
hober 2:856f03e64695 438 *odr = 3330.0f;
hober 2:856f03e64695 439 break;
hober 2:856f03e64695 440 case LSM6DS33_XL_ODR_6660HZ:
hober 2:856f03e64695 441 *odr = 6660.0f;
hober 2:856f03e64695 442 break;
hober 2:856f03e64695 443 default:
hober 2:856f03e64695 444 break;
hober 2:856f03e64695 445 }
hober 2:856f03e64695 446
hober 2:856f03e64695 447 return IMU_6AXES_OK;
hober 2:856f03e64695 448 }
hober 2:856f03e64695 449
hober 2:856f03e64695 450 /**
hober 2:856f03e64695 451 * @brief Write Accelero Output Data Rate
hober 2:856f03e64695 452 * @param odr the accelerometer output data rate to be set
hober 2:856f03e64695 453 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 454 */
hober 2:856f03e64695 455 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_Set_ODR( float odr )
hober 2:856f03e64695 456 {
hober 2:856f03e64695 457 uint8_t new_odr = 0x00;
hober 2:856f03e64695 458 uint8_t tempReg = 0x00;
hober 2:856f03e64695 459
hober 2:856f03e64695 460 new_odr = ( odr <= 0.0f ) ? LSM6DS33_XL_ODR_PD /* Power Down */
hober 2:856f03e64695 461 : ( odr <= 13.0f ) ? LSM6DS33_XL_ODR_13HZ
hober 2:856f03e64695 462 : ( odr <= 26.0f ) ? LSM6DS33_XL_ODR_26HZ
hober 2:856f03e64695 463 : ( odr <= 52.0f ) ? LSM6DS33_XL_ODR_52HZ
hober 2:856f03e64695 464 : ( odr <= 104.0f ) ? LSM6DS33_XL_ODR_104HZ
hober 2:856f03e64695 465 : ( odr <= 208.0f ) ? LSM6DS33_XL_ODR_208HZ
hober 2:856f03e64695 466 : ( odr <= 416.0f ) ? LSM6DS33_XL_ODR_416HZ
hober 2:856f03e64695 467 : ( odr <= 833.0f ) ? LSM6DS33_XL_ODR_833HZ
hober 2:856f03e64695 468 : ( odr <= 1660.0f ) ? LSM6DS33_XL_ODR_1660HZ
hober 2:856f03e64695 469 : ( odr <= 3330.0f ) ? LSM6DS33_XL_ODR_3330HZ
hober 2:856f03e64695 470 : LSM6DS33_XL_ODR_6660HZ;
hober 2:856f03e64695 471
hober 2:856f03e64695 472 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 473 {
hober 2:856f03e64695 474 return IMU_6AXES_ERROR;
hober 2:856f03e64695 475 }
hober 2:856f03e64695 476
hober 2:856f03e64695 477 tempReg &= ~(LSM6DS33_XL_ODR_MASK);
hober 2:856f03e64695 478 tempReg |= new_odr;
hober 2:856f03e64695 479
hober 2:856f03e64695 480 if(LSM6DS33_IO_Write(&tempReg, LSM6DS33_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 481 {
hober 2:856f03e64695 482 return IMU_6AXES_ERROR;
hober 2:856f03e64695 483 }
hober 2:856f03e64695 484
hober 2:856f03e64695 485 return IMU_6AXES_OK;
hober 2:856f03e64695 486 }
hober 2:856f03e64695 487
hober 2:856f03e64695 488 /**
hober 2:856f03e64695 489 * @brief Read Accelero Sensitivity
hober 2:856f03e64695 490 * @param pfData the pointer where the accelerometer sensitivity is stored
hober 2:856f03e64695 491 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 492 */
hober 2:856f03e64695 493 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_GetSensitivity( float *pfData )
hober 2:856f03e64695 494 {
hober 2:856f03e64695 495 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 496
hober 2:856f03e64695 497 uint8_t tempReg = 0x00;
hober 2:856f03e64695 498
hober 2:856f03e64695 499
hober 2:856f03e64695 500 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 501 {
hober 2:856f03e64695 502 return IMU_6AXES_ERROR;
hober 2:856f03e64695 503 }
hober 2:856f03e64695 504
hober 2:856f03e64695 505 tempReg &= LSM6DS33_XL_FS_MASK;
hober 2:856f03e64695 506
hober 2:856f03e64695 507 switch( tempReg )
hober 2:856f03e64695 508 {
hober 2:856f03e64695 509 case LSM6DS33_XL_FS_2G:
hober 2:856f03e64695 510 *pfData = 0.061f;
hober 2:856f03e64695 511 break;
hober 2:856f03e64695 512 case LSM6DS33_XL_FS_4G:
hober 2:856f03e64695 513 *pfData = 0.122f;
hober 2:856f03e64695 514 break;
hober 2:856f03e64695 515 case LSM6DS33_XL_FS_8G:
hober 2:856f03e64695 516 *pfData = 0.244f;
hober 2:856f03e64695 517 break;
hober 2:856f03e64695 518 case LSM6DS33_XL_FS_16G:
hober 2:856f03e64695 519 *pfData = 0.488f;
hober 2:856f03e64695 520 break;
hober 2:856f03e64695 521 default:
hober 2:856f03e64695 522 break;
hober 2:856f03e64695 523 }
hober 2:856f03e64695 524
hober 2:856f03e64695 525 return IMU_6AXES_OK;
hober 2:856f03e64695 526 }
hober 2:856f03e64695 527
hober 2:856f03e64695 528 /**
hober 2:856f03e64695 529 * @brief Read Accelero Full Scale
hober 2:856f03e64695 530 * @param fullScale the pointer where the accelerometer full scale is stored
hober 2:856f03e64695 531 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 532 */
hober 2:856f03e64695 533 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_Get_FS( float *fullScale )
hober 2:856f03e64695 534 {
hober 2:856f03e64695 535 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 536
hober 2:856f03e64695 537 uint8_t tempReg = 0x00;
hober 2:856f03e64695 538
hober 2:856f03e64695 539
hober 2:856f03e64695 540 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 541 {
hober 2:856f03e64695 542 return IMU_6AXES_ERROR;
hober 2:856f03e64695 543 }
hober 2:856f03e64695 544
hober 2:856f03e64695 545 tempReg &= LSM6DS33_XL_FS_MASK;
hober 2:856f03e64695 546
hober 2:856f03e64695 547 switch( tempReg )
hober 2:856f03e64695 548 {
hober 2:856f03e64695 549 case LSM6DS33_XL_FS_2G:
hober 2:856f03e64695 550 *fullScale = 2.0f;
hober 2:856f03e64695 551 break;
hober 2:856f03e64695 552 case LSM6DS33_XL_FS_4G:
hober 2:856f03e64695 553 *fullScale = 4.0f;
hober 2:856f03e64695 554 break;
hober 2:856f03e64695 555 case LSM6DS33_XL_FS_8G:
hober 2:856f03e64695 556 *fullScale = 8.0f;
hober 2:856f03e64695 557 break;
hober 2:856f03e64695 558 case LSM6DS33_XL_FS_16G:
hober 2:856f03e64695 559 *fullScale = 16.0f;
hober 2:856f03e64695 560 break;
hober 2:856f03e64695 561 default:
hober 2:856f03e64695 562 break;
hober 2:856f03e64695 563 }
hober 2:856f03e64695 564
hober 2:856f03e64695 565 return IMU_6AXES_OK;
hober 2:856f03e64695 566 }
hober 2:856f03e64695 567
hober 2:856f03e64695 568 /**
hober 2:856f03e64695 569 * @brief Write Accelero Full Scale
hober 2:856f03e64695 570 * @param fullScale the accelerometer full scale to be set
hober 2:856f03e64695 571 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 572 */
hober 2:856f03e64695 573 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_X_Set_FS( float fullScale )
hober 2:856f03e64695 574 {
hober 2:856f03e64695 575 uint8_t new_fs = 0x00;
hober 2:856f03e64695 576 uint8_t tempReg = 0x00;
hober 2:856f03e64695 577
hober 2:856f03e64695 578 new_fs = ( fullScale <= 2.0f ) ? LSM6DS33_XL_FS_2G
hober 2:856f03e64695 579 : ( fullScale <= 4.0f ) ? LSM6DS33_XL_FS_4G
hober 2:856f03e64695 580 : ( fullScale <= 8.0f ) ? LSM6DS33_XL_FS_8G
hober 2:856f03e64695 581 : LSM6DS33_XL_FS_16G;
hober 2:856f03e64695 582
hober 2:856f03e64695 583 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 584 {
hober 2:856f03e64695 585 return IMU_6AXES_ERROR;
hober 2:856f03e64695 586 }
hober 2:856f03e64695 587
hober 2:856f03e64695 588 tempReg &= ~(LSM6DS33_XL_FS_MASK);
hober 2:856f03e64695 589 tempReg |= new_fs;
hober 2:856f03e64695 590
hober 2:856f03e64695 591 if(LSM6DS33_IO_Write(&tempReg, LSM6DS33_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 592 {
hober 2:856f03e64695 593 return IMU_6AXES_ERROR;
hober 2:856f03e64695 594 }
hober 2:856f03e64695 595
hober 2:856f03e64695 596 return IMU_6AXES_OK;
hober 2:856f03e64695 597 }
hober 2:856f03e64695 598
hober 2:856f03e64695 599 /**
hober 2:856f03e64695 600 * @brief Read Gyro Output Data Rate
hober 2:856f03e64695 601 * @param odr the pointer where the gyroscope output data rate is stored
hober 2:856f03e64695 602 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 603 */
hober 2:856f03e64695 604 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_Get_ODR( float *odr )
hober 2:856f03e64695 605 {
hober 2:856f03e64695 606 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 607 uint8_t tempReg = 0x00;
hober 2:856f03e64695 608
hober 2:856f03e64695 609 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 610 {
hober 2:856f03e64695 611 return IMU_6AXES_ERROR;
hober 2:856f03e64695 612 }
hober 2:856f03e64695 613
hober 2:856f03e64695 614 tempReg &= LSM6DS33_G_ODR_MASK;
hober 2:856f03e64695 615
hober 2:856f03e64695 616 switch( tempReg )
hober 2:856f03e64695 617 {
hober 2:856f03e64695 618 case LSM6DS33_G_ODR_PD:
hober 2:856f03e64695 619 *odr = 0.0f;
hober 2:856f03e64695 620 break;
hober 2:856f03e64695 621 case LSM6DS33_G_ODR_13HZ:
hober 2:856f03e64695 622 *odr = 13.0f;
hober 2:856f03e64695 623 break;
hober 2:856f03e64695 624 case LSM6DS33_G_ODR_26HZ:
hober 2:856f03e64695 625 *odr = 26.0f;
hober 2:856f03e64695 626 break;
hober 2:856f03e64695 627 case LSM6DS33_G_ODR_52HZ:
hober 2:856f03e64695 628 *odr = 52.0f;
hober 2:856f03e64695 629 break;
hober 2:856f03e64695 630 case LSM6DS33_G_ODR_104HZ:
hober 2:856f03e64695 631 *odr = 104.0f;
hober 2:856f03e64695 632 break;
hober 2:856f03e64695 633 case LSM6DS33_G_ODR_208HZ:
hober 2:856f03e64695 634 *odr = 208.0f;
hober 2:856f03e64695 635 break;
hober 2:856f03e64695 636 case LSM6DS33_G_ODR_416HZ:
hober 2:856f03e64695 637 *odr = 416.0f;
hober 2:856f03e64695 638 break;
hober 2:856f03e64695 639 case LSM6DS33_G_ODR_833HZ:
hober 2:856f03e64695 640 *odr = 833.0f;
hober 2:856f03e64695 641 break;
hober 2:856f03e64695 642 case LSM6DS33_G_ODR_1660HZ:
hober 2:856f03e64695 643 *odr = 1660.0f;
hober 2:856f03e64695 644 break;
hober 2:856f03e64695 645 default:
hober 2:856f03e64695 646 break;
hober 2:856f03e64695 647 }
hober 2:856f03e64695 648
hober 2:856f03e64695 649 return IMU_6AXES_OK;
hober 2:856f03e64695 650 }
hober 2:856f03e64695 651
hober 2:856f03e64695 652 /**
hober 2:856f03e64695 653 * @brief Write Gyro Output Data Rate
hober 2:856f03e64695 654 * @param odr the gyroscope output data rate to be set
hober 2:856f03e64695 655 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 656 */
hober 2:856f03e64695 657 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_Set_ODR( float odr )
hober 2:856f03e64695 658 {
hober 2:856f03e64695 659 uint8_t new_odr = 0x00;
hober 2:856f03e64695 660 uint8_t tempReg = 0x00;
hober 2:856f03e64695 661
hober 2:856f03e64695 662 new_odr = ( odr <= 0.0f ) ? LSM6DS33_G_ODR_PD /* Power Down */
hober 2:856f03e64695 663 : ( odr <= 13.0f ) ? LSM6DS33_G_ODR_13HZ
hober 2:856f03e64695 664 : ( odr <= 26.0f ) ? LSM6DS33_G_ODR_26HZ
hober 2:856f03e64695 665 : ( odr <= 52.0f ) ? LSM6DS33_G_ODR_52HZ
hober 2:856f03e64695 666 : ( odr <= 104.0f ) ? LSM6DS33_G_ODR_104HZ
hober 2:856f03e64695 667 : ( odr <= 208.0f ) ? LSM6DS33_G_ODR_208HZ
hober 2:856f03e64695 668 : ( odr <= 416.0f ) ? LSM6DS33_G_ODR_416HZ
hober 2:856f03e64695 669 : ( odr <= 833.0f ) ? LSM6DS33_G_ODR_833HZ
hober 2:856f03e64695 670 : LSM6DS33_G_ODR_1660HZ;
hober 2:856f03e64695 671
hober 2:856f03e64695 672 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 673 {
hober 2:856f03e64695 674 return IMU_6AXES_ERROR;
hober 2:856f03e64695 675 }
hober 2:856f03e64695 676
hober 2:856f03e64695 677 tempReg &= ~(LSM6DS33_G_ODR_MASK);
hober 2:856f03e64695 678 tempReg |= new_odr;
hober 2:856f03e64695 679
hober 2:856f03e64695 680 if(LSM6DS33_IO_Write(&tempReg, LSM6DS33_XG_CTRL2_G, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 681 {
hober 2:856f03e64695 682 return IMU_6AXES_ERROR;
hober 2:856f03e64695 683 }
hober 2:856f03e64695 684
hober 2:856f03e64695 685 return IMU_6AXES_OK;
hober 2:856f03e64695 686 }
hober 2:856f03e64695 687
hober 2:856f03e64695 688 /**
hober 2:856f03e64695 689 * @brief Read Gyro Sensitivity
hober 2:856f03e64695 690 * @param pfData the pointer where the gyroscope sensitivity is stored
hober 2:856f03e64695 691 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 692 */
hober 2:856f03e64695 693 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_GetSensitivity( float *pfData )
hober 2:856f03e64695 694 {
hober 2:856f03e64695 695 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 696
hober 2:856f03e64695 697 uint8_t tempReg = 0x00;
hober 2:856f03e64695 698
hober 2:856f03e64695 699 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 700 {
hober 2:856f03e64695 701 return IMU_6AXES_ERROR;
hober 2:856f03e64695 702 }
hober 2:856f03e64695 703
hober 2:856f03e64695 704 tempReg &= LSM6DS33_G_FS_125_MASK;
hober 2:856f03e64695 705
hober 2:856f03e64695 706 if(tempReg == LSM6DS33_G_FS_125_ENABLE)
hober 2:856f03e64695 707 {
hober 2:856f03e64695 708 *pfData = 4.375f;
hober 2:856f03e64695 709 }
hober 2:856f03e64695 710 else
hober 2:856f03e64695 711 {
hober 2:856f03e64695 712 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 713 {
hober 2:856f03e64695 714 return IMU_6AXES_ERROR;
hober 2:856f03e64695 715 }
hober 2:856f03e64695 716
hober 2:856f03e64695 717 tempReg &= LSM6DS33_G_FS_MASK;
hober 2:856f03e64695 718
hober 2:856f03e64695 719 switch( tempReg )
hober 2:856f03e64695 720 {
hober 2:856f03e64695 721 case LSM6DS33_G_FS_245:
hober 2:856f03e64695 722 *pfData = 8.75f;
hober 2:856f03e64695 723 break;
hober 2:856f03e64695 724 case LSM6DS33_G_FS_500:
hober 2:856f03e64695 725 *pfData = 17.50f;
hober 2:856f03e64695 726 break;
hober 2:856f03e64695 727 case LSM6DS33_G_FS_1000:
hober 2:856f03e64695 728 *pfData = 35.0f;
hober 2:856f03e64695 729 break;
hober 2:856f03e64695 730 case LSM6DS33_G_FS_2000:
hober 2:856f03e64695 731 *pfData = 70.0f;
hober 2:856f03e64695 732 break;
hober 2:856f03e64695 733 default:
hober 2:856f03e64695 734 break;
hober 2:856f03e64695 735 }
hober 2:856f03e64695 736 }
hober 2:856f03e64695 737
hober 2:856f03e64695 738 return IMU_6AXES_OK;
hober 2:856f03e64695 739 }
hober 2:856f03e64695 740
hober 2:856f03e64695 741 /**
hober 2:856f03e64695 742 * @brief Read Gyro Full Scale
hober 2:856f03e64695 743 * @param fullScale the pointer where the gyroscope full scale is stored
hober 2:856f03e64695 744 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 745 */
hober 2:856f03e64695 746 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_Get_FS( float *fullScale )
hober 2:856f03e64695 747 {
hober 2:856f03e64695 748 /*Here we have to add the check if the parameters are valid*/
hober 2:856f03e64695 749
hober 2:856f03e64695 750 uint8_t tempReg = 0x00;
hober 2:856f03e64695 751
hober 2:856f03e64695 752 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 753 {
hober 2:856f03e64695 754 return IMU_6AXES_ERROR;
hober 2:856f03e64695 755 }
hober 2:856f03e64695 756
hober 2:856f03e64695 757 tempReg &= LSM6DS33_G_FS_125_MASK;
hober 2:856f03e64695 758
hober 2:856f03e64695 759 if(tempReg == LSM6DS33_G_FS_125_ENABLE)
hober 2:856f03e64695 760 {
hober 2:856f03e64695 761 *fullScale = 125.0f;
hober 2:856f03e64695 762 }
hober 2:856f03e64695 763 else
hober 2:856f03e64695 764 {
hober 2:856f03e64695 765 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 766 {
hober 2:856f03e64695 767 return IMU_6AXES_ERROR;
hober 2:856f03e64695 768 }
hober 2:856f03e64695 769
hober 2:856f03e64695 770 tempReg &= LSM6DS33_G_FS_MASK;
hober 2:856f03e64695 771
hober 2:856f03e64695 772 switch( tempReg )
hober 2:856f03e64695 773 {
hober 2:856f03e64695 774 case LSM6DS33_G_FS_245:
hober 2:856f03e64695 775 *fullScale = 245.0f;
hober 2:856f03e64695 776 break;
hober 2:856f03e64695 777 case LSM6DS33_G_FS_500:
hober 2:856f03e64695 778 *fullScale = 500.0f;
hober 2:856f03e64695 779 break;
hober 2:856f03e64695 780 case LSM6DS33_G_FS_1000:
hober 2:856f03e64695 781 *fullScale = 1000.0f;
hober 2:856f03e64695 782 break;
hober 2:856f03e64695 783 case LSM6DS33_G_FS_2000:
hober 2:856f03e64695 784 *fullScale = 2000.0f;
hober 2:856f03e64695 785 break;
hober 2:856f03e64695 786 default:
hober 2:856f03e64695 787 break;
hober 2:856f03e64695 788 }
hober 2:856f03e64695 789 }
hober 2:856f03e64695 790
hober 2:856f03e64695 791 return IMU_6AXES_OK;
hober 2:856f03e64695 792 }
hober 2:856f03e64695 793
hober 2:856f03e64695 794 /**
hober 2:856f03e64695 795 * @brief Write Gyro Full Scale
hober 2:856f03e64695 796 * @param fullScale the gyroscope full scale to be set
hober 2:856f03e64695 797 * @retval IMU_6AXES_OK in case of success, an error code otherwise
hober 2:856f03e64695 798 */
hober 2:856f03e64695 799 IMU_6AXES_StatusTypeDef LSM6DS33::LSM6DS33_G_Set_FS( float fullScale )
hober 2:856f03e64695 800 {
hober 2:856f03e64695 801 uint8_t new_fs = 0x00;
hober 2:856f03e64695 802 uint8_t tempReg = 0x00;
hober 2:856f03e64695 803
hober 2:856f03e64695 804 if(fullScale <= 125.0f)
hober 2:856f03e64695 805 {
hober 2:856f03e64695 806 new_fs = LSM6DS33_G_FS_125_ENABLE;
hober 2:856f03e64695 807
hober 2:856f03e64695 808 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 809 {
hober 2:856f03e64695 810 return IMU_6AXES_ERROR;
hober 2:856f03e64695 811 }
hober 2:856f03e64695 812
hober 2:856f03e64695 813 tempReg &= ~(LSM6DS33_G_FS_125_MASK);
hober 2:856f03e64695 814 tempReg |= new_fs;
hober 2:856f03e64695 815
hober 2:856f03e64695 816 if(LSM6DS33_IO_Write(&tempReg, LSM6DS33_XG_CTRL2_G, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 817 {
hober 2:856f03e64695 818 return IMU_6AXES_ERROR;
hober 2:856f03e64695 819 }
hober 2:856f03e64695 820 }
hober 2:856f03e64695 821 else
hober 2:856f03e64695 822 {
hober 2:856f03e64695 823 /* Disable G FS 125dpp */
hober 2:856f03e64695 824 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 825 {
hober 2:856f03e64695 826 return IMU_6AXES_ERROR;
hober 2:856f03e64695 827 }
hober 2:856f03e64695 828
hober 2:856f03e64695 829 tempReg &= ~(LSM6DS33_G_FS_125_MASK);
hober 2:856f03e64695 830 tempReg |= LSM6DS33_G_FS_125_DISABLE;
hober 2:856f03e64695 831
hober 2:856f03e64695 832 if(LSM6DS33_IO_Write(&tempReg, LSM6DS33_XG_CTRL2_G, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 833 {
hober 2:856f03e64695 834 return IMU_6AXES_ERROR;
hober 2:856f03e64695 835 }
hober 2:856f03e64695 836
hober 2:856f03e64695 837 new_fs = ( fullScale <= 245.0f ) ? LSM6DS33_G_FS_245
hober 2:856f03e64695 838 : ( fullScale <= 500.0f ) ? LSM6DS33_G_FS_500
hober 2:856f03e64695 839 : ( fullScale <= 1000.0f ) ? LSM6DS33_G_FS_1000
hober 2:856f03e64695 840 : LSM6DS33_G_FS_2000;
hober 2:856f03e64695 841
hober 2:856f03e64695 842 if(LSM6DS33_IO_Read( &tempReg, LSM6DS33_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
hober 2:856f03e64695 843 {
hober 2:856f03e64695 844 return IMU_6AXES_ERROR;
hober 2:856f03e64695 845 }
hober 2:856f03e64695 846
hober 2:856f03e64695 847 tempReg &= ~(LSM6DS33_G_FS_MASK);
hober 2:856f03e64695 848 tempReg |= new_fs;
hober 2:856f03e64695 849
hober 2:856f03e64695 850 if(LSM6DS33_IO_Write(&tempReg, LSM6DS33_XG_CTRL2_G, 1) != IMU_6AXES_OK)
hober 2:856f03e64695 851 {
hober 2:856f03e64695 852 return IMU_6AXES_ERROR;
hober 2:856f03e64695 853 }
hober 2:856f03e64695 854 }
hober 2:856f03e64695 855
hober 2:856f03e64695 856 return IMU_6AXES_OK;
hober 2:856f03e64695 857 }
hober 2:856f03e64695 858
hober 2:856f03e64695 859 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/