Robotique FIP / Mbed 2 deprecated FIP_REV1

ble

Dependencies:   HC_SR04_Ultrasonic_Library Servo mbed

Fork of FIP_REV1 by Robotique FIP

Sensor_Shield/lsm6ds0.c@4:69a35a56ac48, 2015-07-09 (annotated)

Committer:
julientiron
Date:
Thu Jul 09 13:33:36 2015 +0000
Revision:
4:69a35a56ac48
Parent:
1:bdbf36f8408d 
BLE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julientiron 1:bdbf36f8408d 1 /**
julientiron 1:bdbf36f8408d 2 ******************************************************************************
julientiron 1:bdbf36f8408d 3 * @file lsm6ds0.c
julientiron 1:bdbf36f8408d 4 * @author MEMS Application Team
julientiron 1:bdbf36f8408d 5 * @version V1.0.0
julientiron 1:bdbf36f8408d 6 * @date 30-July-2014
julientiron 1:bdbf36f8408d 7 * @brief This file provides a set of functions needed to manage the lsm6ds0.
julientiron 1:bdbf36f8408d 8 ******************************************************************************
julientiron 1:bdbf36f8408d 9 * @attention
julientiron 1:bdbf36f8408d 10 *
julientiron 1:bdbf36f8408d 11 * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
julientiron 1:bdbf36f8408d 12 *
julientiron 1:bdbf36f8408d 13 * Redistribution and use in source and binary forms, with or without modification,
julientiron 1:bdbf36f8408d 14 * are permitted provided that the following conditions are met:
julientiron 1:bdbf36f8408d 15 * 1. Redistributions of source code must retain the above copyright notice,
julientiron 1:bdbf36f8408d 16 * this list of conditions and the following disclaimer.
julientiron 1:bdbf36f8408d 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
julientiron 1:bdbf36f8408d 18 * this list of conditions and the following disclaimer in the documentation
julientiron 1:bdbf36f8408d 19 * and/or other materials provided with the distribution.
julientiron 1:bdbf36f8408d 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
julientiron 1:bdbf36f8408d 21 * may be used to endorse or promote products derived from this software
julientiron 1:bdbf36f8408d 22 * without specific prior written permission.
julientiron 1:bdbf36f8408d 23 *
julientiron 1:bdbf36f8408d 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
julientiron 1:bdbf36f8408d 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
julientiron 1:bdbf36f8408d 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
julientiron 1:bdbf36f8408d 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
julientiron 1:bdbf36f8408d 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
julientiron 1:bdbf36f8408d 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
julientiron 1:bdbf36f8408d 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
julientiron 1:bdbf36f8408d 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
julientiron 1:bdbf36f8408d 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
julientiron 1:bdbf36f8408d 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
julientiron 1:bdbf36f8408d 34 *
julientiron 1:bdbf36f8408d 35 ******************************************************************************
julientiron 1:bdbf36f8408d 36 */
julientiron 1:bdbf36f8408d 37 /* Includes ------------------------------------------------------------------*/
julientiron 1:bdbf36f8408d 38 #include "lsm6ds0.h"
julientiron 1:bdbf36f8408d 39 #include <math.h>
julientiron 1:bdbf36f8408d 40
julientiron 1:bdbf36f8408d 41 /** @addtogroup BSP
julientiron 1:bdbf36f8408d 42 * @{
julientiron 1:bdbf36f8408d 43 */
julientiron 1:bdbf36f8408d 44
julientiron 1:bdbf36f8408d 45 /** @addtogroup MEMS_SHIELD
julientiron 1:bdbf36f8408d 46 * @{
julientiron 1:bdbf36f8408d 47 */
julientiron 1:bdbf36f8408d 48
julientiron 1:bdbf36f8408d 49 /** @addtogroup LSM6DS0
julientiron 1:bdbf36f8408d 50 * @{
julientiron 1:bdbf36f8408d 51 */
julientiron 1:bdbf36f8408d 52
julientiron 1:bdbf36f8408d 53 /** @defgroup LSM6DS0_Private_TypesDefinitions
julientiron 1:bdbf36f8408d 54 * @{
julientiron 1:bdbf36f8408d 55 */
julientiron 1:bdbf36f8408d 56
julientiron 1:bdbf36f8408d 57 /**
julientiron 1:bdbf36f8408d 58 * @}
julientiron 1:bdbf36f8408d 59 */
julientiron 1:bdbf36f8408d 60
julientiron 1:bdbf36f8408d 61 /** @defgroup LSM6DS0_Private_Defines
julientiron 1:bdbf36f8408d 62 * @{
julientiron 1:bdbf36f8408d 63 */
julientiron 1:bdbf36f8408d 64
julientiron 1:bdbf36f8408d 65 /**
julientiron 1:bdbf36f8408d 66 * @}
julientiron 1:bdbf36f8408d 67 */
julientiron 1:bdbf36f8408d 68
julientiron 1:bdbf36f8408d 69 /** @defgroup LSM6DS0_Private_Macros
julientiron 1:bdbf36f8408d 70 * @{
julientiron 1:bdbf36f8408d 71 */
julientiron 1:bdbf36f8408d 72
julientiron 1:bdbf36f8408d 73 /**
julientiron 1:bdbf36f8408d 74 * @}
julientiron 1:bdbf36f8408d 75 */
julientiron 1:bdbf36f8408d 76
julientiron 1:bdbf36f8408d 77 /** @defgroup LSM6DS0_Private_Variables
julientiron 1:bdbf36f8408d 78 * @{
julientiron 1:bdbf36f8408d 79 */
julientiron 1:bdbf36f8408d 80
julientiron 1:bdbf36f8408d 81 IMU_6AXES_DrvTypeDef LSM6DS0Drv =
julientiron 1:bdbf36f8408d 82 {
julientiron 1:bdbf36f8408d 83 LSM6DS0_Init,
julientiron 1:bdbf36f8408d 84 LSM6DS0_Read_XG_ID,
julientiron 1:bdbf36f8408d 85 LSM6DS0_X_GetAxes,
julientiron 1:bdbf36f8408d 86 LSM6DS0_G_GetAxes
julientiron 1:bdbf36f8408d 87 };
julientiron 1:bdbf36f8408d 88
julientiron 1:bdbf36f8408d 89 /**
julientiron 1:bdbf36f8408d 90 * @}
julientiron 1:bdbf36f8408d 91 */
julientiron 1:bdbf36f8408d 92
julientiron 1:bdbf36f8408d 93 /** @defgroup LSM6DS0_Private_FunctionPrototypes
julientiron 1:bdbf36f8408d 94 * @{
julientiron 1:bdbf36f8408d 95 */
julientiron 1:bdbf36f8408d 96
julientiron 1:bdbf36f8408d 97 void LSM6DS0_X_GetAxesRaw(int16_t *pData);
julientiron 1:bdbf36f8408d 98 void LSM6DS0_G_GetAxesRaw(int16_t *pData);
julientiron 1:bdbf36f8408d 99
julientiron 1:bdbf36f8408d 100 /**
julientiron 1:bdbf36f8408d 101 * @}
julientiron 1:bdbf36f8408d 102 */
julientiron 1:bdbf36f8408d 103
julientiron 1:bdbf36f8408d 104 /** @defgroup LSM6DS0_Private_Functions
julientiron 1:bdbf36f8408d 105 * @{
julientiron 1:bdbf36f8408d 106 */
julientiron 1:bdbf36f8408d 107
julientiron 1:bdbf36f8408d 108
julientiron 1:bdbf36f8408d 109
julientiron 1:bdbf36f8408d 110 /**
julientiron 1:bdbf36f8408d 111 * @brief Set LSM6DS0 Initialization.
julientiron 1:bdbf36f8408d 112 * @param InitStruct: it contains the configuration setting for the LSM6DS0.
julientiron 1:bdbf36f8408d 113 * @retval None
julientiron 1:bdbf36f8408d 114 */
julientiron 1:bdbf36f8408d 115 void LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init)
julientiron 1:bdbf36f8408d 116 {
julientiron 1:bdbf36f8408d 117 uint8_t tmp1 = 0x00;
julientiron 1:bdbf36f8408d 118
julientiron 1:bdbf36f8408d 119 /* Configure the low level interface ---------------------------------------*/
julientiron 1:bdbf36f8408d 120 IMU_6AXES_IO_Init();
julientiron 1:bdbf36f8408d 121
julientiron 1:bdbf36f8408d 122 /******* Gyroscope init *******/
julientiron 1:bdbf36f8408d 123
julientiron 1:bdbf36f8408d 124 IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
julientiron 1:bdbf36f8408d 125
julientiron 1:bdbf36f8408d 126 /* Output Data Rate selection */
julientiron 1:bdbf36f8408d 127 tmp1 &= ~(LSM6DS0_G_ODR_MASK);
julientiron 1:bdbf36f8408d 128 tmp1 |= LSM6DS0_Init->G_OutputDataRate;
julientiron 1:bdbf36f8408d 129
julientiron 1:bdbf36f8408d 130 /* Full scale selection */
julientiron 1:bdbf36f8408d 131 tmp1 &= ~(LSM6DS0_G_FS_MASK);
julientiron 1:bdbf36f8408d 132 tmp1 |= LSM6DS0_Init->G_FullScale;
julientiron 1:bdbf36f8408d 133
julientiron 1:bdbf36f8408d 134 IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
julientiron 1:bdbf36f8408d 135
julientiron 1:bdbf36f8408d 136
julientiron 1:bdbf36f8408d 137 IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
julientiron 1:bdbf36f8408d 138
julientiron 1:bdbf36f8408d 139 /* Enable X axis selection */
julientiron 1:bdbf36f8408d 140 tmp1 &= ~(LSM6DS0_G_XEN_MASK);
julientiron 1:bdbf36f8408d 141 tmp1 |= LSM6DS0_Init->G_X_Axis;
julientiron 1:bdbf36f8408d 142
julientiron 1:bdbf36f8408d 143 /* Enable Y axis selection */
julientiron 1:bdbf36f8408d 144 tmp1 &= ~(LSM6DS0_G_YEN_MASK);
julientiron 1:bdbf36f8408d 145 tmp1 |= LSM6DS0_Init->G_Y_Axis;
julientiron 1:bdbf36f8408d 146
julientiron 1:bdbf36f8408d 147 /* Enable Z axis selection */
julientiron 1:bdbf36f8408d 148 tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
julientiron 1:bdbf36f8408d 149 tmp1 |= LSM6DS0_Init->G_Z_Axis;
julientiron 1:bdbf36f8408d 150
julientiron 1:bdbf36f8408d 151 IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
julientiron 1:bdbf36f8408d 152
julientiron 1:bdbf36f8408d 153 /******************************/
julientiron 1:bdbf36f8408d 154
julientiron 1:bdbf36f8408d 155 /***** Accelerometer init *****/
julientiron 1:bdbf36f8408d 156
julientiron 1:bdbf36f8408d 157 IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
julientiron 1:bdbf36f8408d 158
julientiron 1:bdbf36f8408d 159 /* Output Data Rate selection */
julientiron 1:bdbf36f8408d 160 tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
julientiron 1:bdbf36f8408d 161 tmp1 |= LSM6DS0_Init->X_OutputDataRate;
julientiron 1:bdbf36f8408d 162
julientiron 1:bdbf36f8408d 163 /* Full scale selection */
julientiron 1:bdbf36f8408d 164 tmp1 &= ~(LSM6DS0_XL_FS_MASK);
julientiron 1:bdbf36f8408d 165 tmp1 |= LSM6DS0_Init->X_FullScale;
julientiron 1:bdbf36f8408d 166
julientiron 1:bdbf36f8408d 167 IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
julientiron 1:bdbf36f8408d 168
julientiron 1:bdbf36f8408d 169
julientiron 1:bdbf36f8408d 170 IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
julientiron 1:bdbf36f8408d 171
julientiron 1:bdbf36f8408d 172 /* Enable X axis selection */
julientiron 1:bdbf36f8408d 173 tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
julientiron 1:bdbf36f8408d 174 tmp1 |= LSM6DS0_Init->X_X_Axis;
julientiron 1:bdbf36f8408d 175
julientiron 1:bdbf36f8408d 176 /* Enable Y axis selection */
julientiron 1:bdbf36f8408d 177 tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
julientiron 1:bdbf36f8408d 178 tmp1 |= LSM6DS0_Init->X_Y_Axis;
julientiron 1:bdbf36f8408d 179
julientiron 1:bdbf36f8408d 180 /* Enable Z axis selection */
julientiron 1:bdbf36f8408d 181 tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
julientiron 1:bdbf36f8408d 182 tmp1 |= LSM6DS0_Init->X_Z_Axis;
julientiron 1:bdbf36f8408d 183
julientiron 1:bdbf36f8408d 184 IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
julientiron 1:bdbf36f8408d 185
julientiron 1:bdbf36f8408d 186 /******************************/
julientiron 1:bdbf36f8408d 187 }
julientiron 1:bdbf36f8408d 188
julientiron 1:bdbf36f8408d 189
julientiron 1:bdbf36f8408d 190 /**
julientiron 1:bdbf36f8408d 191 * @brief Read ID of LSM6DS0 Accelerometer and Gyroscope
julientiron 1:bdbf36f8408d 192 * @param Device ID
julientiron 1:bdbf36f8408d 193 * @retval ID name
julientiron 1:bdbf36f8408d 194 */
julientiron 1:bdbf36f8408d 195 uint8_t LSM6DS0_Read_XG_ID(void)
julientiron 1:bdbf36f8408d 196 {
julientiron 1:bdbf36f8408d 197 uint8_t tmp = 0x00;
julientiron 1:bdbf36f8408d 198
julientiron 1:bdbf36f8408d 199 /* Read WHO I AM register */
julientiron 1:bdbf36f8408d 200 IMU_6AXES_IO_Read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
julientiron 1:bdbf36f8408d 201
julientiron 1:bdbf36f8408d 202 /* Return the ID */
julientiron 1:bdbf36f8408d 203 return (uint8_t)tmp;
julientiron 1:bdbf36f8408d 204 }
julientiron 1:bdbf36f8408d 205
julientiron 1:bdbf36f8408d 206
julientiron 1:bdbf36f8408d 207 /**
julientiron 1:bdbf36f8408d 208 * @brief Read raw data from LSM6DS0 Accelerometer output register.
julientiron 1:bdbf36f8408d 209 * @param float *pfData
julientiron 1:bdbf36f8408d 210 * @retval None.
julientiron 1:bdbf36f8408d 211 */
julientiron 1:bdbf36f8408d 212 void LSM6DS0_X_GetAxesRaw(int16_t *pData)
julientiron 1:bdbf36f8408d 213 {
julientiron 1:bdbf36f8408d 214 uint8_t tempReg[2] = {0,0};
julientiron 1:bdbf36f8408d 215
julientiron 1:bdbf36f8408d 216
julientiron 1:bdbf36f8408d 217 IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
julientiron 1:bdbf36f8408d 218
julientiron 1:bdbf36f8408d 219 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 220
julientiron 1:bdbf36f8408d 221 IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
julientiron 1:bdbf36f8408d 222
julientiron 1:bdbf36f8408d 223 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 224
julientiron 1:bdbf36f8408d 225 IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
julientiron 1:bdbf36f8408d 226
julientiron 1:bdbf36f8408d 227 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 228 }
julientiron 1:bdbf36f8408d 229
julientiron 1:bdbf36f8408d 230
julientiron 1:bdbf36f8408d 231 /**
julientiron 1:bdbf36f8408d 232 * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg.
julientiron 1:bdbf36f8408d 233 * @param float *pfData
julientiron 1:bdbf36f8408d 234 * @retval None.
julientiron 1:bdbf36f8408d 235 */
julientiron 1:bdbf36f8408d 236 void LSM6DS0_X_GetAxes(int32_t *pData)
julientiron 1:bdbf36f8408d 237 {
julientiron 1:bdbf36f8408d 238
julientiron 1:bdbf36f8408d 239 uint8_t tempReg = 0x00;
julientiron 1:bdbf36f8408d 240 int16_t pDataRaw[3];
julientiron 1:bdbf36f8408d 241 float sensitivity = 0;
julientiron 1:bdbf36f8408d 242
julientiron 1:bdbf36f8408d 243 LSM6DS0_X_GetAxesRaw(pDataRaw);
julientiron 1:bdbf36f8408d 244
julientiron 1:bdbf36f8408d 245 IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
julientiron 1:bdbf36f8408d 246
julientiron 1:bdbf36f8408d 247 tempReg &= LSM6DS0_XL_FS_MASK;
julientiron 1:bdbf36f8408d 248
julientiron 1:bdbf36f8408d 249 switch(tempReg)
julientiron 1:bdbf36f8408d 250 {
julientiron 1:bdbf36f8408d 251 case LSM6DS0_XL_FS_2G:
julientiron 1:bdbf36f8408d 252 sensitivity = 0.061;
julientiron 1:bdbf36f8408d 253 break;
julientiron 1:bdbf36f8408d 254 case LSM6DS0_XL_FS_4G:
julientiron 1:bdbf36f8408d 255 sensitivity = 0.122;
julientiron 1:bdbf36f8408d 256 break;
julientiron 1:bdbf36f8408d 257 case LSM6DS0_XL_FS_8G:
julientiron 1:bdbf36f8408d 258 sensitivity = 0.244;
julientiron 1:bdbf36f8408d 259 break;
julientiron 1:bdbf36f8408d 260 }
julientiron 1:bdbf36f8408d 261
julientiron 1:bdbf36f8408d 262 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
julientiron 1:bdbf36f8408d 263 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
julientiron 1:bdbf36f8408d 264 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
julientiron 1:bdbf36f8408d 265 }
julientiron 1:bdbf36f8408d 266
julientiron 1:bdbf36f8408d 267
julientiron 1:bdbf36f8408d 268 /**
julientiron 1:bdbf36f8408d 269 * @brief Read raw data from LSM6DS0 Gyroscope output register.
julientiron 1:bdbf36f8408d 270 * @param float *pfData
julientiron 1:bdbf36f8408d 271 * @retval None.
julientiron 1:bdbf36f8408d 272 */
julientiron 1:bdbf36f8408d 273 void LSM6DS0_G_GetAxesRaw(int16_t *pData)
julientiron 1:bdbf36f8408d 274 {
julientiron 1:bdbf36f8408d 275 uint8_t tempReg[2] = {0,0};
julientiron 1:bdbf36f8408d 276
julientiron 1:bdbf36f8408d 277
julientiron 1:bdbf36f8408d 278 IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
julientiron 1:bdbf36f8408d 279
julientiron 1:bdbf36f8408d 280 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 281
julientiron 1:bdbf36f8408d 282 IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
julientiron 1:bdbf36f8408d 283
julientiron 1:bdbf36f8408d 284 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 285
julientiron 1:bdbf36f8408d 286 IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
julientiron 1:bdbf36f8408d 287
julientiron 1:bdbf36f8408d 288 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 289 }
julientiron 1:bdbf36f8408d 290
julientiron 1:bdbf36f8408d 291
julientiron 1:bdbf36f8408d 292 /**
julientiron 1:bdbf36f8408d 293 * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps.
julientiron 1:bdbf36f8408d 294 * @param float *pfData
julientiron 1:bdbf36f8408d 295 * @retval None.
julientiron 1:bdbf36f8408d 296 */
julientiron 1:bdbf36f8408d 297 void LSM6DS0_G_GetAxes(int32_t *pData)
julientiron 1:bdbf36f8408d 298 {
julientiron 1:bdbf36f8408d 299
julientiron 1:bdbf36f8408d 300 uint8_t tempReg = 0x00;
julientiron 1:bdbf36f8408d 301 int16_t pDataRaw[3];
julientiron 1:bdbf36f8408d 302 float sensitivity = 0;
julientiron 1:bdbf36f8408d 303
julientiron 1:bdbf36f8408d 304 LSM6DS0_G_GetAxesRaw(pDataRaw);
julientiron 1:bdbf36f8408d 305
julientiron 1:bdbf36f8408d 306 IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
julientiron 1:bdbf36f8408d 307
julientiron 1:bdbf36f8408d 308 tempReg &= LSM6DS0_G_FS_MASK;
julientiron 1:bdbf36f8408d 309
julientiron 1:bdbf36f8408d 310 switch(tempReg)
julientiron 1:bdbf36f8408d 311 {
julientiron 1:bdbf36f8408d 312 case LSM6DS0_G_FS_245:
julientiron 1:bdbf36f8408d 313 sensitivity = 8.75;
julientiron 1:bdbf36f8408d 314 break;
julientiron 1:bdbf36f8408d 315 case LSM6DS0_G_FS_500:
julientiron 1:bdbf36f8408d 316 sensitivity = 17.50;
julientiron 1:bdbf36f8408d 317 break;
julientiron 1:bdbf36f8408d 318 case LSM6DS0_G_FS_2000:
julientiron 1:bdbf36f8408d 319 sensitivity = 70;
julientiron 1:bdbf36f8408d 320 break;
julientiron 1:bdbf36f8408d 321 }
julientiron 1:bdbf36f8408d 322
julientiron 1:bdbf36f8408d 323 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
julientiron 1:bdbf36f8408d 324 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
julientiron 1:bdbf36f8408d 325 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
julientiron 1:bdbf36f8408d 326 }
julientiron 1:bdbf36f8408d 327
julientiron 1:bdbf36f8408d 328 /**
julientiron 1:bdbf36f8408d 329 * @}
julientiron 1:bdbf36f8408d 330 */
julientiron 1:bdbf36f8408d 331
julientiron 1:bdbf36f8408d 332 /**
julientiron 1:bdbf36f8408d 333 * @}
julientiron 1:bdbf36f8408d 334 */
julientiron 1:bdbf36f8408d 335
julientiron 1:bdbf36f8408d 336 /**
julientiron 1:bdbf36f8408d 337 * @}
julientiron 1:bdbf36f8408d 338 */
julientiron 1:bdbf36f8408d 339
julientiron 1:bdbf36f8408d 340 /**
julientiron 1:bdbf36f8408d 341 * @}
julientiron 1:bdbf36f8408d 342 */
julientiron 1:bdbf36f8408d 343
julientiron 1:bdbf36f8408d 344
julientiron 1:bdbf36f8408d 345 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
julientiron 1:bdbf36f8408d 346