Peter Ferland / mDot_X_NUCLEO_IKS01A1

Multitech mDot/UDK support.

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Fork of X_NUCLEO_IKS01A1 by ST

Components/lsm6ds3/lsm6ds3_class.cpp@44:d757094f6229, 2015-06-08 (annotated)

Committer:
Wolfgang Betz
Date:
Mon Jun 08 15:36:36 2015 +0200
Revision:
44:d757094f6229
Child:
57:04563dd74269
Add LSM6DS3 component

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wolfgang Betz 44:d757094f6229 1 /**
Wolfgang Betz 44:d757094f6229 2 ******************************************************************************
Wolfgang Betz 44:d757094f6229 3 * @file lsm6ds3_class.cpp
Wolfgang Betz 44:d757094f6229 4 * @author AST / EST
Wolfgang Betz 44:d757094f6229 5 * @version V0.0.1
Wolfgang Betz 44:d757094f6229 6 * @date 14-April-2015
Wolfgang Betz 44:d757094f6229 7 * @brief Implementation file for the LSM6DS3 driver class
Wolfgang Betz 44:d757094f6229 8 ******************************************************************************
Wolfgang Betz 44:d757094f6229 9 * @attention
Wolfgang Betz 44:d757094f6229 10 *
Wolfgang Betz 44:d757094f6229 11 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Wolfgang Betz 44:d757094f6229 12 *
Wolfgang Betz 44:d757094f6229 13 * Redistribution and use in source and binary forms, with or without modification,
Wolfgang Betz 44:d757094f6229 14 * are permitted provided that the following conditions are met:
Wolfgang Betz 44:d757094f6229 15 * 1. Redistributions of source code must retain the above copyright notice,
Wolfgang Betz 44:d757094f6229 16 * this list of conditions and the following disclaimer.
Wolfgang Betz 44:d757094f6229 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
Wolfgang Betz 44:d757094f6229 18 * this list of conditions and the following disclaimer in the documentation
Wolfgang Betz 44:d757094f6229 19 * and/or other materials provided with the distribution.
Wolfgang Betz 44:d757094f6229 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Wolfgang Betz 44:d757094f6229 21 * may be used to endorse or promote products derived from this software
Wolfgang Betz 44:d757094f6229 22 * without specific prior written permission.
Wolfgang Betz 44:d757094f6229 23 *
Wolfgang Betz 44:d757094f6229 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Wolfgang Betz 44:d757094f6229 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Wolfgang Betz 44:d757094f6229 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Wolfgang Betz 44:d757094f6229 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Wolfgang Betz 44:d757094f6229 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Wolfgang Betz 44:d757094f6229 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Wolfgang Betz 44:d757094f6229 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Wolfgang Betz 44:d757094f6229 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Wolfgang Betz 44:d757094f6229 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Wolfgang Betz 44:d757094f6229 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Wolfgang Betz 44:d757094f6229 34 *
Wolfgang Betz 44:d757094f6229 35 ******************************************************************************
Wolfgang Betz 44:d757094f6229 36 */
Wolfgang Betz 44:d757094f6229 37
Wolfgang Betz 44:d757094f6229 38 /* Includes ------------------------------------------------------------------*/
Wolfgang Betz 44:d757094f6229 39 #include "mbed.h"
Wolfgang Betz 44:d757094f6229 40 #include "lsm6ds3_class.h"
Wolfgang Betz 44:d757094f6229 41 #include "lsm6ds3.h"
Wolfgang Betz 44:d757094f6229 42 #include "../../x_nucleo_iks01a1_targets.h"
Wolfgang Betz 44:d757094f6229 43
Wolfgang Betz 44:d757094f6229 44 /* Methods -------------------------------------------------------------------*/
Wolfgang Betz 44:d757094f6229 45 /* betzw - based on:
Wolfgang Betz 44:d757094f6229 46 X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds3/lsm6ds3.c: revision #400,
Wolfgang Betz 44:d757094f6229 47 X-CUBE-MEMS1/trunk: revision #410
Wolfgang Betz 44:d757094f6229 48 */
Wolfgang Betz 44:d757094f6229 49 /**
Wolfgang Betz 44:d757094f6229 50 * @brief Set LSM6DS3 Initialization
Wolfgang Betz 44:d757094f6229 51 * @param LSM6DS3_Init the configuration setting for the LSM6DS3
Wolfgang Betz 44:d757094f6229 52 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 53 */
Wolfgang Betz 44:d757094f6229 54 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Init( IMU_6AXES_InitTypeDef *LSM6DS3_Init )
Wolfgang Betz 44:d757094f6229 55 {
Wolfgang Betz 44:d757094f6229 56 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 57
Wolfgang Betz 44:d757094f6229 58 /* Configure the low level interface -------------------------------------*/
Wolfgang Betz 44:d757094f6229 59 if(LSM6DS3_IO_Init() != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 60 {
Wolfgang Betz 44:d757094f6229 61 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 62 }
Wolfgang Betz 44:d757094f6229 63
Wolfgang Betz 44:d757094f6229 64
Wolfgang Betz 44:d757094f6229 65 /******** Common init *********/
Wolfgang Betz 44:d757094f6229 66
Wolfgang Betz 44:d757094f6229 67 if(LSM6DS3_Common_Sensor_Enable() != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 68 {
Wolfgang Betz 44:d757094f6229 69 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 70 }
Wolfgang Betz 44:d757094f6229 71
Wolfgang Betz 44:d757094f6229 72
Wolfgang Betz 44:d757094f6229 73 /******* Gyroscope init *******/
Wolfgang Betz 44:d757094f6229 74
Wolfgang Betz 44:d757094f6229 75 if(LSM6DS3_G_Set_ODR( LSM6DS3_Init->G_OutputDataRate ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 76 {
Wolfgang Betz 44:d757094f6229 77 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 78 }
Wolfgang Betz 44:d757094f6229 79
Wolfgang Betz 44:d757094f6229 80 if(LSM6DS3_G_Set_FS( LSM6DS3_Init->G_FullScale ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 81 {
Wolfgang Betz 44:d757094f6229 82 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 83 }
Wolfgang Betz 44:d757094f6229 84
Wolfgang Betz 44:d757094f6229 85 if(LSM6DS3_G_Set_Axes_Status(LSM6DS3_Init->G_X_Axis, LSM6DS3_Init->G_Y_Axis, LSM6DS3_Init->G_Z_Axis) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 86 {
Wolfgang Betz 44:d757094f6229 87 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 88 }
Wolfgang Betz 44:d757094f6229 89
Wolfgang Betz 44:d757094f6229 90
Wolfgang Betz 44:d757094f6229 91 /***** Accelerometer init *****/
Wolfgang Betz 44:d757094f6229 92
Wolfgang Betz 44:d757094f6229 93 if(LSM6DS3_X_Set_ODR( LSM6DS3_Init->X_OutputDataRate ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 94 {
Wolfgang Betz 44:d757094f6229 95 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 96 }
Wolfgang Betz 44:d757094f6229 97
Wolfgang Betz 44:d757094f6229 98 if(LSM6DS3_X_Set_FS( LSM6DS3_Init->X_FullScale ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 99 {
Wolfgang Betz 44:d757094f6229 100 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 101 }
Wolfgang Betz 44:d757094f6229 102
Wolfgang Betz 44:d757094f6229 103 if(LSM6DS3_X_Set_Axes_Status(LSM6DS3_Init->X_X_Axis, LSM6DS3_Init->X_Y_Axis, LSM6DS3_Init->X_Z_Axis) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 104 {
Wolfgang Betz 44:d757094f6229 105 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 106 }
Wolfgang Betz 44:d757094f6229 107
Wolfgang Betz 44:d757094f6229 108 /* Configure interrupt lines */
Wolfgang Betz 44:d757094f6229 109 LSM6DS3_IO_ITConfig();
Wolfgang Betz 44:d757094f6229 110
Wolfgang Betz 44:d757094f6229 111 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 112 }
Wolfgang Betz 44:d757094f6229 113
Wolfgang Betz 44:d757094f6229 114 /**
Wolfgang Betz 44:d757094f6229 115 * @brief Read ID of LSM6DS3 Accelerometer and Gyroscope
Wolfgang Betz 44:d757094f6229 116 * @param xg_id the pointer where the ID of the device is stored
Wolfgang Betz 44:d757094f6229 117 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 118 */
Wolfgang Betz 44:d757094f6229 119 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Read_XG_ID( uint8_t *xg_id)
Wolfgang Betz 44:d757094f6229 120 {
Wolfgang Betz 44:d757094f6229 121 if(!xg_id)
Wolfgang Betz 44:d757094f6229 122 {
Wolfgang Betz 44:d757094f6229 123 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 124 }
Wolfgang Betz 44:d757094f6229 125
Wolfgang Betz 44:d757094f6229 126 return LSM6DS3_IO_Read(xg_id, LSM6DS3_XG_WHO_AM_I_ADDR, 1);
Wolfgang Betz 44:d757094f6229 127 }
Wolfgang Betz 44:d757094f6229 128
Wolfgang Betz 44:d757094f6229 129 /**
Wolfgang Betz 44:d757094f6229 130 * @brief Set LSM6DS3 common initialization
Wolfgang Betz 44:d757094f6229 131 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 132 */
Wolfgang Betz 44:d757094f6229 133 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Common_Sensor_Enable(void)
Wolfgang Betz 44:d757094f6229 134 {
Wolfgang Betz 44:d757094f6229 135 uint8_t tmp1 = 0x00;
Wolfgang Betz 44:d757094f6229 136
Wolfgang Betz 44:d757094f6229 137 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 138 {
Wolfgang Betz 44:d757094f6229 139 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 140 }
Wolfgang Betz 44:d757094f6229 141
Wolfgang Betz 44:d757094f6229 142 /* Enable register address automatically incremented during a multiple byte
Wolfgang Betz 44:d757094f6229 143 access with a serial interface (I2C or SPI) */
Wolfgang Betz 44:d757094f6229 144 tmp1 &= ~(LSM6DS3_XG_IF_INC_MASK);
Wolfgang Betz 44:d757094f6229 145 tmp1 |= LSM6DS3_XG_IF_INC;
Wolfgang Betz 44:d757094f6229 146
Wolfgang Betz 44:d757094f6229 147 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 148 {
Wolfgang Betz 44:d757094f6229 149 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 150 }
Wolfgang Betz 44:d757094f6229 151
Wolfgang Betz 44:d757094f6229 152
Wolfgang Betz 44:d757094f6229 153 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 154 {
Wolfgang Betz 44:d757094f6229 155 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 156 }
Wolfgang Betz 44:d757094f6229 157
Wolfgang Betz 44:d757094f6229 158 /* FIFO ODR selection */
Wolfgang Betz 44:d757094f6229 159 tmp1 &= ~(LSM6DS3_XG_FIFO_ODR_MASK);
Wolfgang Betz 44:d757094f6229 160 tmp1 |= LSM6DS3_XG_FIFO_ODR_NA;
Wolfgang Betz 44:d757094f6229 161
Wolfgang Betz 44:d757094f6229 162 /* FIFO mode selection */
Wolfgang Betz 44:d757094f6229 163 tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK);
Wolfgang Betz 44:d757094f6229 164 tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS;
Wolfgang Betz 44:d757094f6229 165
Wolfgang Betz 44:d757094f6229 166 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 167 {
Wolfgang Betz 44:d757094f6229 168 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 169 }
Wolfgang Betz 44:d757094f6229 170
Wolfgang Betz 44:d757094f6229 171 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 172 }
Wolfgang Betz 44:d757094f6229 173
Wolfgang Betz 44:d757094f6229 174 /**
Wolfgang Betz 44:d757094f6229 175 * @brief Read raw data from LSM6DS3 Accelerometer output register
Wolfgang Betz 44:d757094f6229 176 * @param pData the pointer where the accelerometer raw data are stored
Wolfgang Betz 44:d757094f6229 177 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 178 */
Wolfgang Betz 44:d757094f6229 179 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetAxesRaw( int16_t *pData )
Wolfgang Betz 44:d757094f6229 180 {
Wolfgang Betz 44:d757094f6229 181 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 182
Wolfgang Betz 44:d757094f6229 183 uint8_t tempReg[2] = {0, 0};
Wolfgang Betz 44:d757094f6229 184
Wolfgang Betz 44:d757094f6229 185
Wolfgang Betz 44:d757094f6229 186 if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_X_L_XL, 2) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 187 {
Wolfgang Betz 44:d757094f6229 188 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 189 }
Wolfgang Betz 44:d757094f6229 190
Wolfgang Betz 44:d757094f6229 191 pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 44:d757094f6229 192
Wolfgang Betz 44:d757094f6229 193 if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Y_L_XL, 2) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 194 {
Wolfgang Betz 44:d757094f6229 195 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 196 }
Wolfgang Betz 44:d757094f6229 197
Wolfgang Betz 44:d757094f6229 198 pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 44:d757094f6229 199
Wolfgang Betz 44:d757094f6229 200 if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Z_L_XL, 2) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 201 {
Wolfgang Betz 44:d757094f6229 202 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 203 }
Wolfgang Betz 44:d757094f6229 204
Wolfgang Betz 44:d757094f6229 205 pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 44:d757094f6229 206
Wolfgang Betz 44:d757094f6229 207 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 208 }
Wolfgang Betz 44:d757094f6229 209
Wolfgang Betz 44:d757094f6229 210
Wolfgang Betz 44:d757094f6229 211
Wolfgang Betz 44:d757094f6229 212 /**
Wolfgang Betz 44:d757094f6229 213 * @brief Read data from LSM6DS3 Accelerometer and calculate linear acceleration in mg
Wolfgang Betz 44:d757094f6229 214 * @param pData the pointer where the accelerometer data are stored
Wolfgang Betz 44:d757094f6229 215 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 216 */
Wolfgang Betz 44:d757094f6229 217 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetAxes( int32_t *pData )
Wolfgang Betz 44:d757094f6229 218 {
Wolfgang Betz 44:d757094f6229 219 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 220 int16_t pDataRaw[3];
Wolfgang Betz 44:d757094f6229 221 float sensitivity = 0.0f;
Wolfgang Betz 44:d757094f6229 222
Wolfgang Betz 44:d757094f6229 223 if(LSM6DS3_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 224 {
Wolfgang Betz 44:d757094f6229 225 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 226 }
Wolfgang Betz 44:d757094f6229 227
Wolfgang Betz 44:d757094f6229 228 if(LSM6DS3_X_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 229 {
Wolfgang Betz 44:d757094f6229 230 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 231 }
Wolfgang Betz 44:d757094f6229 232
Wolfgang Betz 44:d757094f6229 233 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
Wolfgang Betz 44:d757094f6229 234 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
Wolfgang Betz 44:d757094f6229 235 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
Wolfgang Betz 44:d757094f6229 236
Wolfgang Betz 44:d757094f6229 237 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 238 }
Wolfgang Betz 44:d757094f6229 239
Wolfgang Betz 44:d757094f6229 240
Wolfgang Betz 44:d757094f6229 241
Wolfgang Betz 44:d757094f6229 242 /**
Wolfgang Betz 44:d757094f6229 243 * @brief Read raw data from LSM6DS3 Gyroscope output register
Wolfgang Betz 44:d757094f6229 244 * @param pData the pointer where the gyroscope raw data are stored
Wolfgang Betz 44:d757094f6229 245 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 246 */
Wolfgang Betz 44:d757094f6229 247 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetAxesRaw( int16_t *pData )
Wolfgang Betz 44:d757094f6229 248 {
Wolfgang Betz 44:d757094f6229 249 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 250
Wolfgang Betz 44:d757094f6229 251 uint8_t tempReg[2] = {0, 0};
Wolfgang Betz 44:d757094f6229 252
Wolfgang Betz 44:d757094f6229 253
Wolfgang Betz 44:d757094f6229 254 if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_X_L_G, 2) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 255 {
Wolfgang Betz 44:d757094f6229 256 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 257 }
Wolfgang Betz 44:d757094f6229 258
Wolfgang Betz 44:d757094f6229 259 pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 44:d757094f6229 260
Wolfgang Betz 44:d757094f6229 261 if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Y_L_G, 2) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 262 {
Wolfgang Betz 44:d757094f6229 263 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 264 }
Wolfgang Betz 44:d757094f6229 265
Wolfgang Betz 44:d757094f6229 266 pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 44:d757094f6229 267
Wolfgang Betz 44:d757094f6229 268 if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Z_L_G, 2) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 269 {
Wolfgang Betz 44:d757094f6229 270 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 271 }
Wolfgang Betz 44:d757094f6229 272
Wolfgang Betz 44:d757094f6229 273 pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 44:d757094f6229 274
Wolfgang Betz 44:d757094f6229 275 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 276 }
Wolfgang Betz 44:d757094f6229 277
Wolfgang Betz 44:d757094f6229 278 /**
Wolfgang Betz 44:d757094f6229 279 * @brief Set the status of the axes for accelerometer
Wolfgang Betz 44:d757094f6229 280 * @param enableX the status of the x axis to be set
Wolfgang Betz 44:d757094f6229 281 * @param enableY the status of the y axis to be set
Wolfgang Betz 44:d757094f6229 282 * @param enableZ the status of the z axis to be set
Wolfgang Betz 44:d757094f6229 283 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 284 */
Wolfgang Betz 44:d757094f6229 285 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
Wolfgang Betz 44:d757094f6229 286 {
Wolfgang Betz 44:d757094f6229 287 uint8_t tmp1 = 0x00;
Wolfgang Betz 44:d757094f6229 288 uint8_t eX = 0x00;
Wolfgang Betz 44:d757094f6229 289 uint8_t eY = 0x00;
Wolfgang Betz 44:d757094f6229 290 uint8_t eZ = 0x00;
Wolfgang Betz 44:d757094f6229 291
Wolfgang Betz 44:d757094f6229 292 eX = ( enableX == 0 ) ? LSM6DS3_XL_XEN_DISABLE : LSM6DS3_XL_XEN_ENABLE;
Wolfgang Betz 44:d757094f6229 293 eY = ( enableY == 0 ) ? LSM6DS3_XL_YEN_DISABLE : LSM6DS3_XL_YEN_ENABLE;
Wolfgang Betz 44:d757094f6229 294 eZ = ( enableZ == 0 ) ? LSM6DS3_XL_ZEN_DISABLE : LSM6DS3_XL_ZEN_ENABLE;
Wolfgang Betz 44:d757094f6229 295
Wolfgang Betz 44:d757094f6229 296 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL9_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 297 {
Wolfgang Betz 44:d757094f6229 298 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 299 }
Wolfgang Betz 44:d757094f6229 300
Wolfgang Betz 44:d757094f6229 301 /* Enable X axis selection */
Wolfgang Betz 44:d757094f6229 302 tmp1 &= ~(LSM6DS3_XL_XEN_MASK);
Wolfgang Betz 44:d757094f6229 303 tmp1 |= eX;
Wolfgang Betz 44:d757094f6229 304
Wolfgang Betz 44:d757094f6229 305 /* Enable Y axis selection */
Wolfgang Betz 44:d757094f6229 306 tmp1 &= ~(LSM6DS3_XL_YEN_MASK);
Wolfgang Betz 44:d757094f6229 307 tmp1 |= eY;
Wolfgang Betz 44:d757094f6229 308
Wolfgang Betz 44:d757094f6229 309 /* Enable Z axis selection */
Wolfgang Betz 44:d757094f6229 310 tmp1 &= ~(LSM6DS3_XL_ZEN_MASK);
Wolfgang Betz 44:d757094f6229 311 tmp1 |= eZ;
Wolfgang Betz 44:d757094f6229 312
Wolfgang Betz 44:d757094f6229 313 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL9_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 314 {
Wolfgang Betz 44:d757094f6229 315 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 316 }
Wolfgang Betz 44:d757094f6229 317
Wolfgang Betz 44:d757094f6229 318 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 319 }
Wolfgang Betz 44:d757094f6229 320
Wolfgang Betz 44:d757094f6229 321 /**
Wolfgang Betz 44:d757094f6229 322 * @brief Set the status of the axes for gyroscope
Wolfgang Betz 44:d757094f6229 323 * @param enableX the status of the x axis to be set
Wolfgang Betz 44:d757094f6229 324 * @param enableY the status of the y axis to be set
Wolfgang Betz 44:d757094f6229 325 * @param enableZ the status of the z axis to be set
Wolfgang Betz 44:d757094f6229 326 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 327 */
Wolfgang Betz 44:d757094f6229 328 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
Wolfgang Betz 44:d757094f6229 329 {
Wolfgang Betz 44:d757094f6229 330 uint8_t tmp1 = 0x00;
Wolfgang Betz 44:d757094f6229 331 uint8_t eX = 0x00;
Wolfgang Betz 44:d757094f6229 332 uint8_t eY = 0x00;
Wolfgang Betz 44:d757094f6229 333 uint8_t eZ = 0x00;
Wolfgang Betz 44:d757094f6229 334
Wolfgang Betz 44:d757094f6229 335 eX = ( enableX == 0 ) ? LSM6DS3_G_XEN_DISABLE : LSM6DS3_G_XEN_ENABLE;
Wolfgang Betz 44:d757094f6229 336 eY = ( enableY == 0 ) ? LSM6DS3_G_YEN_DISABLE : LSM6DS3_G_YEN_ENABLE;
Wolfgang Betz 44:d757094f6229 337 eZ = ( enableZ == 0 ) ? LSM6DS3_G_ZEN_DISABLE : LSM6DS3_G_ZEN_ENABLE;
Wolfgang Betz 44:d757094f6229 338
Wolfgang Betz 44:d757094f6229 339 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 340 {
Wolfgang Betz 44:d757094f6229 341 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 342 }
Wolfgang Betz 44:d757094f6229 343
Wolfgang Betz 44:d757094f6229 344 /* Enable X axis selection */
Wolfgang Betz 44:d757094f6229 345 tmp1 &= ~(LSM6DS3_G_XEN_MASK);
Wolfgang Betz 44:d757094f6229 346 tmp1 |= eX;
Wolfgang Betz 44:d757094f6229 347
Wolfgang Betz 44:d757094f6229 348 /* Enable Y axis selection */
Wolfgang Betz 44:d757094f6229 349 tmp1 &= ~(LSM6DS3_G_YEN_MASK);
Wolfgang Betz 44:d757094f6229 350 tmp1 |= eY;
Wolfgang Betz 44:d757094f6229 351
Wolfgang Betz 44:d757094f6229 352 /* Enable Z axis selection */
Wolfgang Betz 44:d757094f6229 353 tmp1 &= ~(LSM6DS3_G_ZEN_MASK);
Wolfgang Betz 44:d757094f6229 354 tmp1 |= eZ;
Wolfgang Betz 44:d757094f6229 355
Wolfgang Betz 44:d757094f6229 356 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 357 {
Wolfgang Betz 44:d757094f6229 358 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 359 }
Wolfgang Betz 44:d757094f6229 360
Wolfgang Betz 44:d757094f6229 361 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 362 }
Wolfgang Betz 44:d757094f6229 363
Wolfgang Betz 44:d757094f6229 364 /**
Wolfgang Betz 44:d757094f6229 365 * @brief Read data from LSM6DS3 Gyroscope and calculate angular rate in mdps
Wolfgang Betz 44:d757094f6229 366 * @param pData the pointer where the gyroscope data are stored
Wolfgang Betz 44:d757094f6229 367 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 368 */
Wolfgang Betz 44:d757094f6229 369 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetAxes( int32_t *pData )
Wolfgang Betz 44:d757094f6229 370 {
Wolfgang Betz 44:d757094f6229 371 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 372 int16_t pDataRaw[3];
Wolfgang Betz 44:d757094f6229 373 float sensitivity = 0.0f;
Wolfgang Betz 44:d757094f6229 374
Wolfgang Betz 44:d757094f6229 375 if(LSM6DS3_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 376 {
Wolfgang Betz 44:d757094f6229 377 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 378 }
Wolfgang Betz 44:d757094f6229 379
Wolfgang Betz 44:d757094f6229 380 if(LSM6DS3_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 381 {
Wolfgang Betz 44:d757094f6229 382 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 383 }
Wolfgang Betz 44:d757094f6229 384
Wolfgang Betz 44:d757094f6229 385 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
Wolfgang Betz 44:d757094f6229 386 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
Wolfgang Betz 44:d757094f6229 387 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
Wolfgang Betz 44:d757094f6229 388
Wolfgang Betz 44:d757094f6229 389 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 390 }
Wolfgang Betz 44:d757094f6229 391
Wolfgang Betz 44:d757094f6229 392 /**
Wolfgang Betz 44:d757094f6229 393 * @brief Read Accelero Output Data Rate
Wolfgang Betz 44:d757094f6229 394 * @param odr the pointer where the accelerometer output data rate is stored
Wolfgang Betz 44:d757094f6229 395 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 396 */
Wolfgang Betz 44:d757094f6229 397 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Get_ODR( float *odr )
Wolfgang Betz 44:d757094f6229 398 {
Wolfgang Betz 44:d757094f6229 399 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 400 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 401
Wolfgang Betz 44:d757094f6229 402 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 403 {
Wolfgang Betz 44:d757094f6229 404 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 405 }
Wolfgang Betz 44:d757094f6229 406
Wolfgang Betz 44:d757094f6229 407 tempReg &= LSM6DS3_XL_ODR_MASK;
Wolfgang Betz 44:d757094f6229 408
Wolfgang Betz 44:d757094f6229 409 switch( tempReg )
Wolfgang Betz 44:d757094f6229 410 {
Wolfgang Betz 44:d757094f6229 411 case LSM6DS3_XL_ODR_PD:
Wolfgang Betz 44:d757094f6229 412 *odr = 0.0f;
Wolfgang Betz 44:d757094f6229 413 break;
Wolfgang Betz 44:d757094f6229 414 case LSM6DS3_XL_ODR_13HZ:
Wolfgang Betz 44:d757094f6229 415 *odr = 13.0f;
Wolfgang Betz 44:d757094f6229 416 break;
Wolfgang Betz 44:d757094f6229 417 case LSM6DS3_XL_ODR_26HZ:
Wolfgang Betz 44:d757094f6229 418 *odr = 26.0f;
Wolfgang Betz 44:d757094f6229 419 break;
Wolfgang Betz 44:d757094f6229 420 case LSM6DS3_XL_ODR_52HZ:
Wolfgang Betz 44:d757094f6229 421 *odr = 52.0f;
Wolfgang Betz 44:d757094f6229 422 break;
Wolfgang Betz 44:d757094f6229 423 case LSM6DS3_XL_ODR_104HZ:
Wolfgang Betz 44:d757094f6229 424 *odr = 104.0f;
Wolfgang Betz 44:d757094f6229 425 break;
Wolfgang Betz 44:d757094f6229 426 case LSM6DS3_XL_ODR_208HZ:
Wolfgang Betz 44:d757094f6229 427 *odr = 208.0f;
Wolfgang Betz 44:d757094f6229 428 break;
Wolfgang Betz 44:d757094f6229 429 case LSM6DS3_XL_ODR_416HZ:
Wolfgang Betz 44:d757094f6229 430 *odr = 416.0f;
Wolfgang Betz 44:d757094f6229 431 break;
Wolfgang Betz 44:d757094f6229 432 case LSM6DS3_XL_ODR_833HZ:
Wolfgang Betz 44:d757094f6229 433 *odr = 833.0f;
Wolfgang Betz 44:d757094f6229 434 break;
Wolfgang Betz 44:d757094f6229 435 case LSM6DS3_XL_ODR_1660HZ:
Wolfgang Betz 44:d757094f6229 436 *odr = 1660.0f;
Wolfgang Betz 44:d757094f6229 437 break;
Wolfgang Betz 44:d757094f6229 438 case LSM6DS3_XL_ODR_3330HZ:
Wolfgang Betz 44:d757094f6229 439 *odr = 3330.0f;
Wolfgang Betz 44:d757094f6229 440 break;
Wolfgang Betz 44:d757094f6229 441 case LSM6DS3_XL_ODR_6660HZ:
Wolfgang Betz 44:d757094f6229 442 *odr = 6660.0f;
Wolfgang Betz 44:d757094f6229 443 break;
Wolfgang Betz 44:d757094f6229 444 default:
Wolfgang Betz 44:d757094f6229 445 break;
Wolfgang Betz 44:d757094f6229 446 }
Wolfgang Betz 44:d757094f6229 447
Wolfgang Betz 44:d757094f6229 448 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 449 }
Wolfgang Betz 44:d757094f6229 450
Wolfgang Betz 44:d757094f6229 451 /**
Wolfgang Betz 44:d757094f6229 452 * @brief Write Accelero Output Data Rate
Wolfgang Betz 44:d757094f6229 453 * @param odr the accelerometer output data rate to be set
Wolfgang Betz 44:d757094f6229 454 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 455 */
Wolfgang Betz 44:d757094f6229 456 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_ODR( float odr )
Wolfgang Betz 44:d757094f6229 457 {
Wolfgang Betz 44:d757094f6229 458 uint8_t new_odr = 0x00;
Wolfgang Betz 44:d757094f6229 459 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 460
Wolfgang Betz 44:d757094f6229 461 new_odr = ( odr <= 0.0f ) ? LSM6DS3_XL_ODR_PD /* Power Down */
Wolfgang Betz 44:d757094f6229 462 : ( odr <= 13.0f ) ? LSM6DS3_XL_ODR_13HZ
Wolfgang Betz 44:d757094f6229 463 : ( odr <= 26.0f ) ? LSM6DS3_XL_ODR_26HZ
Wolfgang Betz 44:d757094f6229 464 : ( odr <= 52.0f ) ? LSM6DS3_XL_ODR_52HZ
Wolfgang Betz 44:d757094f6229 465 : ( odr <= 104.0f ) ? LSM6DS3_XL_ODR_104HZ
Wolfgang Betz 44:d757094f6229 466 : ( odr <= 208.0f ) ? LSM6DS3_XL_ODR_208HZ
Wolfgang Betz 44:d757094f6229 467 : ( odr <= 416.0f ) ? LSM6DS3_XL_ODR_416HZ
Wolfgang Betz 44:d757094f6229 468 : ( odr <= 833.0f ) ? LSM6DS3_XL_ODR_833HZ
Wolfgang Betz 44:d757094f6229 469 : ( odr <= 1660.0f ) ? LSM6DS3_XL_ODR_1660HZ
Wolfgang Betz 44:d757094f6229 470 : ( odr <= 3330.0f ) ? LSM6DS3_XL_ODR_3330HZ
Wolfgang Betz 44:d757094f6229 471 : LSM6DS3_XL_ODR_6660HZ;
Wolfgang Betz 44:d757094f6229 472
Wolfgang Betz 44:d757094f6229 473 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 474 {
Wolfgang Betz 44:d757094f6229 475 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 476 }
Wolfgang Betz 44:d757094f6229 477
Wolfgang Betz 44:d757094f6229 478 tempReg &= ~(LSM6DS3_XL_ODR_MASK);
Wolfgang Betz 44:d757094f6229 479 tempReg |= new_odr;
Wolfgang Betz 44:d757094f6229 480
Wolfgang Betz 44:d757094f6229 481 if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 482 {
Wolfgang Betz 44:d757094f6229 483 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 484 }
Wolfgang Betz 44:d757094f6229 485
Wolfgang Betz 44:d757094f6229 486 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 487 }
Wolfgang Betz 44:d757094f6229 488
Wolfgang Betz 44:d757094f6229 489 /**
Wolfgang Betz 44:d757094f6229 490 * @brief Read Accelero Sensitivity
Wolfgang Betz 44:d757094f6229 491 * @param pfData the pointer where the accelerometer sensitivity is stored
Wolfgang Betz 44:d757094f6229 492 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 493 */
Wolfgang Betz 44:d757094f6229 494 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetSensitivity( float *pfData )
Wolfgang Betz 44:d757094f6229 495 {
Wolfgang Betz 44:d757094f6229 496 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 497
Wolfgang Betz 44:d757094f6229 498 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 499
Wolfgang Betz 44:d757094f6229 500
Wolfgang Betz 44:d757094f6229 501 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 502 {
Wolfgang Betz 44:d757094f6229 503 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 504 }
Wolfgang Betz 44:d757094f6229 505
Wolfgang Betz 44:d757094f6229 506 tempReg &= LSM6DS3_XL_FS_MASK;
Wolfgang Betz 44:d757094f6229 507
Wolfgang Betz 44:d757094f6229 508 switch( tempReg )
Wolfgang Betz 44:d757094f6229 509 {
Wolfgang Betz 44:d757094f6229 510 case LSM6DS3_XL_FS_2G:
Wolfgang Betz 44:d757094f6229 511 *pfData = 0.061f;
Wolfgang Betz 44:d757094f6229 512 break;
Wolfgang Betz 44:d757094f6229 513 case LSM6DS3_XL_FS_4G:
Wolfgang Betz 44:d757094f6229 514 *pfData = 0.122f;
Wolfgang Betz 44:d757094f6229 515 break;
Wolfgang Betz 44:d757094f6229 516 case LSM6DS3_XL_FS_8G:
Wolfgang Betz 44:d757094f6229 517 *pfData = 0.244f;
Wolfgang Betz 44:d757094f6229 518 break;
Wolfgang Betz 44:d757094f6229 519 case LSM6DS3_XL_FS_16G:
Wolfgang Betz 44:d757094f6229 520 *pfData = 0.488f;
Wolfgang Betz 44:d757094f6229 521 break;
Wolfgang Betz 44:d757094f6229 522 default:
Wolfgang Betz 44:d757094f6229 523 break;
Wolfgang Betz 44:d757094f6229 524 }
Wolfgang Betz 44:d757094f6229 525
Wolfgang Betz 44:d757094f6229 526 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 527 }
Wolfgang Betz 44:d757094f6229 528
Wolfgang Betz 44:d757094f6229 529 /**
Wolfgang Betz 44:d757094f6229 530 * @brief Read Accelero Full Scale
Wolfgang Betz 44:d757094f6229 531 * @param fullScale the pointer where the accelerometer full scale is stored
Wolfgang Betz 44:d757094f6229 532 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 533 */
Wolfgang Betz 44:d757094f6229 534 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Get_FS( float *fullScale )
Wolfgang Betz 44:d757094f6229 535 {
Wolfgang Betz 44:d757094f6229 536 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 537
Wolfgang Betz 44:d757094f6229 538 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 539
Wolfgang Betz 44:d757094f6229 540
Wolfgang Betz 44:d757094f6229 541 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 542 {
Wolfgang Betz 44:d757094f6229 543 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 544 }
Wolfgang Betz 44:d757094f6229 545
Wolfgang Betz 44:d757094f6229 546 tempReg &= LSM6DS3_XL_FS_MASK;
Wolfgang Betz 44:d757094f6229 547
Wolfgang Betz 44:d757094f6229 548 switch( tempReg )
Wolfgang Betz 44:d757094f6229 549 {
Wolfgang Betz 44:d757094f6229 550 case LSM6DS3_XL_FS_2G:
Wolfgang Betz 44:d757094f6229 551 *fullScale = 2.0f;
Wolfgang Betz 44:d757094f6229 552 break;
Wolfgang Betz 44:d757094f6229 553 case LSM6DS3_XL_FS_4G:
Wolfgang Betz 44:d757094f6229 554 *fullScale = 4.0f;
Wolfgang Betz 44:d757094f6229 555 break;
Wolfgang Betz 44:d757094f6229 556 case LSM6DS3_XL_FS_8G:
Wolfgang Betz 44:d757094f6229 557 *fullScale = 8.0f;
Wolfgang Betz 44:d757094f6229 558 break;
Wolfgang Betz 44:d757094f6229 559 case LSM6DS3_XL_FS_16G:
Wolfgang Betz 44:d757094f6229 560 *fullScale = 16.0f;
Wolfgang Betz 44:d757094f6229 561 break;
Wolfgang Betz 44:d757094f6229 562 default:
Wolfgang Betz 44:d757094f6229 563 break;
Wolfgang Betz 44:d757094f6229 564 }
Wolfgang Betz 44:d757094f6229 565
Wolfgang Betz 44:d757094f6229 566 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 567 }
Wolfgang Betz 44:d757094f6229 568
Wolfgang Betz 44:d757094f6229 569 /**
Wolfgang Betz 44:d757094f6229 570 * @brief Write Accelero Full Scale
Wolfgang Betz 44:d757094f6229 571 * @param fullScale the accelerometer full scale to be set
Wolfgang Betz 44:d757094f6229 572 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 573 */
Wolfgang Betz 44:d757094f6229 574 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_FS( float fullScale )
Wolfgang Betz 44:d757094f6229 575 {
Wolfgang Betz 44:d757094f6229 576 uint8_t new_fs = 0x00;
Wolfgang Betz 44:d757094f6229 577 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 578
Wolfgang Betz 44:d757094f6229 579 new_fs = ( fullScale <= 2.0f ) ? LSM6DS3_XL_FS_2G
Wolfgang Betz 44:d757094f6229 580 : ( fullScale <= 4.0f ) ? LSM6DS3_XL_FS_4G
Wolfgang Betz 44:d757094f6229 581 : ( fullScale <= 8.0f ) ? LSM6DS3_XL_FS_8G
Wolfgang Betz 44:d757094f6229 582 : LSM6DS3_XL_FS_16G;
Wolfgang Betz 44:d757094f6229 583
Wolfgang Betz 44:d757094f6229 584 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 585 {
Wolfgang Betz 44:d757094f6229 586 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 587 }
Wolfgang Betz 44:d757094f6229 588
Wolfgang Betz 44:d757094f6229 589 tempReg &= ~(LSM6DS3_XL_FS_MASK);
Wolfgang Betz 44:d757094f6229 590 tempReg |= new_fs;
Wolfgang Betz 44:d757094f6229 591
Wolfgang Betz 44:d757094f6229 592 if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 593 {
Wolfgang Betz 44:d757094f6229 594 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 595 }
Wolfgang Betz 44:d757094f6229 596
Wolfgang Betz 44:d757094f6229 597 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 598 }
Wolfgang Betz 44:d757094f6229 599
Wolfgang Betz 44:d757094f6229 600 /**
Wolfgang Betz 44:d757094f6229 601 * @brief Read Gyro Output Data Rate
Wolfgang Betz 44:d757094f6229 602 * @param odr the pointer where the gyroscope output data rate is stored
Wolfgang Betz 44:d757094f6229 603 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 604 */
Wolfgang Betz 44:d757094f6229 605 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Get_ODR( float *odr )
Wolfgang Betz 44:d757094f6229 606 {
Wolfgang Betz 44:d757094f6229 607 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 608 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 609
Wolfgang Betz 44:d757094f6229 610 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 611 {
Wolfgang Betz 44:d757094f6229 612 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 613 }
Wolfgang Betz 44:d757094f6229 614
Wolfgang Betz 44:d757094f6229 615 tempReg &= LSM6DS3_G_ODR_MASK;
Wolfgang Betz 44:d757094f6229 616
Wolfgang Betz 44:d757094f6229 617 switch( tempReg )
Wolfgang Betz 44:d757094f6229 618 {
Wolfgang Betz 44:d757094f6229 619 case LSM6DS3_G_ODR_PD:
Wolfgang Betz 44:d757094f6229 620 *odr = 0.0f;
Wolfgang Betz 44:d757094f6229 621 break;
Wolfgang Betz 44:d757094f6229 622 case LSM6DS3_G_ODR_13HZ:
Wolfgang Betz 44:d757094f6229 623 *odr = 13.0f;
Wolfgang Betz 44:d757094f6229 624 break;
Wolfgang Betz 44:d757094f6229 625 case LSM6DS3_G_ODR_26HZ:
Wolfgang Betz 44:d757094f6229 626 *odr = 26.0f;
Wolfgang Betz 44:d757094f6229 627 break;
Wolfgang Betz 44:d757094f6229 628 case LSM6DS3_G_ODR_52HZ:
Wolfgang Betz 44:d757094f6229 629 *odr = 52.0f;
Wolfgang Betz 44:d757094f6229 630 break;
Wolfgang Betz 44:d757094f6229 631 case LSM6DS3_G_ODR_104HZ:
Wolfgang Betz 44:d757094f6229 632 *odr = 104.0f;
Wolfgang Betz 44:d757094f6229 633 break;
Wolfgang Betz 44:d757094f6229 634 case LSM6DS3_G_ODR_208HZ:
Wolfgang Betz 44:d757094f6229 635 *odr = 208.0f;
Wolfgang Betz 44:d757094f6229 636 break;
Wolfgang Betz 44:d757094f6229 637 case LSM6DS3_G_ODR_416HZ:
Wolfgang Betz 44:d757094f6229 638 *odr = 416.0f;
Wolfgang Betz 44:d757094f6229 639 break;
Wolfgang Betz 44:d757094f6229 640 case LSM6DS3_G_ODR_833HZ:
Wolfgang Betz 44:d757094f6229 641 *odr = 833.0f;
Wolfgang Betz 44:d757094f6229 642 break;
Wolfgang Betz 44:d757094f6229 643 case LSM6DS3_G_ODR_1660HZ:
Wolfgang Betz 44:d757094f6229 644 *odr = 1660.0f;
Wolfgang Betz 44:d757094f6229 645 break;
Wolfgang Betz 44:d757094f6229 646 default:
Wolfgang Betz 44:d757094f6229 647 break;
Wolfgang Betz 44:d757094f6229 648 }
Wolfgang Betz 44:d757094f6229 649
Wolfgang Betz 44:d757094f6229 650 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 651 }
Wolfgang Betz 44:d757094f6229 652
Wolfgang Betz 44:d757094f6229 653 /**
Wolfgang Betz 44:d757094f6229 654 * @brief Write Gyro Output Data Rate
Wolfgang Betz 44:d757094f6229 655 * @param odr the gyroscope output data rate to be set
Wolfgang Betz 44:d757094f6229 656 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 657 */
Wolfgang Betz 44:d757094f6229 658 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_ODR( float odr )
Wolfgang Betz 44:d757094f6229 659 {
Wolfgang Betz 44:d757094f6229 660 uint8_t new_odr = 0x00;
Wolfgang Betz 44:d757094f6229 661 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 662
Wolfgang Betz 44:d757094f6229 663 new_odr = ( odr <= 0.0f ) ? LSM6DS3_G_ODR_PD /* Power Down */
Wolfgang Betz 44:d757094f6229 664 : ( odr <= 13.0f ) ? LSM6DS3_G_ODR_13HZ
Wolfgang Betz 44:d757094f6229 665 : ( odr <= 26.0f ) ? LSM6DS3_G_ODR_26HZ
Wolfgang Betz 44:d757094f6229 666 : ( odr <= 52.0f ) ? LSM6DS3_G_ODR_52HZ
Wolfgang Betz 44:d757094f6229 667 : ( odr <= 104.0f ) ? LSM6DS3_G_ODR_104HZ
Wolfgang Betz 44:d757094f6229 668 : ( odr <= 208.0f ) ? LSM6DS3_G_ODR_208HZ
Wolfgang Betz 44:d757094f6229 669 : ( odr <= 416.0f ) ? LSM6DS3_G_ODR_416HZ
Wolfgang Betz 44:d757094f6229 670 : ( odr <= 833.0f ) ? LSM6DS3_G_ODR_833HZ
Wolfgang Betz 44:d757094f6229 671 : LSM6DS3_G_ODR_1660HZ;
Wolfgang Betz 44:d757094f6229 672
Wolfgang Betz 44:d757094f6229 673 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 674 {
Wolfgang Betz 44:d757094f6229 675 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 676 }
Wolfgang Betz 44:d757094f6229 677
Wolfgang Betz 44:d757094f6229 678 tempReg &= ~(LSM6DS3_G_ODR_MASK);
Wolfgang Betz 44:d757094f6229 679 tempReg |= new_odr;
Wolfgang Betz 44:d757094f6229 680
Wolfgang Betz 44:d757094f6229 681 if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 682 {
Wolfgang Betz 44:d757094f6229 683 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 684 }
Wolfgang Betz 44:d757094f6229 685
Wolfgang Betz 44:d757094f6229 686 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 687 }
Wolfgang Betz 44:d757094f6229 688
Wolfgang Betz 44:d757094f6229 689 /**
Wolfgang Betz 44:d757094f6229 690 * @brief Read Gyro Sensitivity
Wolfgang Betz 44:d757094f6229 691 * @param pfData the pointer where the gyroscope sensitivity is stored
Wolfgang Betz 44:d757094f6229 692 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 693 */
Wolfgang Betz 44:d757094f6229 694 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetSensitivity( float *pfData )
Wolfgang Betz 44:d757094f6229 695 {
Wolfgang Betz 44:d757094f6229 696 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 697
Wolfgang Betz 44:d757094f6229 698 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 699
Wolfgang Betz 44:d757094f6229 700 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 701 {
Wolfgang Betz 44:d757094f6229 702 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 703 }
Wolfgang Betz 44:d757094f6229 704
Wolfgang Betz 44:d757094f6229 705 tempReg &= LSM6DS3_G_FS_125_MASK;
Wolfgang Betz 44:d757094f6229 706
Wolfgang Betz 44:d757094f6229 707 if(tempReg == LSM6DS3_G_FS_125_ENABLE)
Wolfgang Betz 44:d757094f6229 708 {
Wolfgang Betz 44:d757094f6229 709 *pfData = 4.375f;
Wolfgang Betz 44:d757094f6229 710 }
Wolfgang Betz 44:d757094f6229 711 else
Wolfgang Betz 44:d757094f6229 712 {
Wolfgang Betz 44:d757094f6229 713 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 714 {
Wolfgang Betz 44:d757094f6229 715 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 716 }
Wolfgang Betz 44:d757094f6229 717
Wolfgang Betz 44:d757094f6229 718 tempReg &= LSM6DS3_G_FS_MASK;
Wolfgang Betz 44:d757094f6229 719
Wolfgang Betz 44:d757094f6229 720 switch( tempReg )
Wolfgang Betz 44:d757094f6229 721 {
Wolfgang Betz 44:d757094f6229 722 case LSM6DS3_G_FS_245:
Wolfgang Betz 44:d757094f6229 723 *pfData = 8.75f;
Wolfgang Betz 44:d757094f6229 724 break;
Wolfgang Betz 44:d757094f6229 725 case LSM6DS3_G_FS_500:
Wolfgang Betz 44:d757094f6229 726 *pfData = 17.50f;
Wolfgang Betz 44:d757094f6229 727 break;
Wolfgang Betz 44:d757094f6229 728 case LSM6DS3_G_FS_1000:
Wolfgang Betz 44:d757094f6229 729 *pfData = 35.0f;
Wolfgang Betz 44:d757094f6229 730 break;
Wolfgang Betz 44:d757094f6229 731 case LSM6DS3_G_FS_2000:
Wolfgang Betz 44:d757094f6229 732 *pfData = 70.0f;
Wolfgang Betz 44:d757094f6229 733 break;
Wolfgang Betz 44:d757094f6229 734 default:
Wolfgang Betz 44:d757094f6229 735 break;
Wolfgang Betz 44:d757094f6229 736 }
Wolfgang Betz 44:d757094f6229 737 }
Wolfgang Betz 44:d757094f6229 738
Wolfgang Betz 44:d757094f6229 739 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 740 }
Wolfgang Betz 44:d757094f6229 741
Wolfgang Betz 44:d757094f6229 742 /**
Wolfgang Betz 44:d757094f6229 743 * @brief Read Gyro Full Scale
Wolfgang Betz 44:d757094f6229 744 * @param fullScale the pointer where the gyroscope full scale is stored
Wolfgang Betz 44:d757094f6229 745 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 746 */
Wolfgang Betz 44:d757094f6229 747 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Get_FS( float *fullScale )
Wolfgang Betz 44:d757094f6229 748 {
Wolfgang Betz 44:d757094f6229 749 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 44:d757094f6229 750
Wolfgang Betz 44:d757094f6229 751 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 752
Wolfgang Betz 44:d757094f6229 753 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 754 {
Wolfgang Betz 44:d757094f6229 755 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 756 }
Wolfgang Betz 44:d757094f6229 757
Wolfgang Betz 44:d757094f6229 758 tempReg &= LSM6DS3_G_FS_125_MASK;
Wolfgang Betz 44:d757094f6229 759
Wolfgang Betz 44:d757094f6229 760 if(tempReg == LSM6DS3_G_FS_125_ENABLE)
Wolfgang Betz 44:d757094f6229 761 {
Wolfgang Betz 44:d757094f6229 762 *fullScale = 125.0f;
Wolfgang Betz 44:d757094f6229 763 }
Wolfgang Betz 44:d757094f6229 764 else
Wolfgang Betz 44:d757094f6229 765 {
Wolfgang Betz 44:d757094f6229 766 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 767 {
Wolfgang Betz 44:d757094f6229 768 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 769 }
Wolfgang Betz 44:d757094f6229 770
Wolfgang Betz 44:d757094f6229 771 tempReg &= LSM6DS3_G_FS_MASK;
Wolfgang Betz 44:d757094f6229 772
Wolfgang Betz 44:d757094f6229 773 switch( tempReg )
Wolfgang Betz 44:d757094f6229 774 {
Wolfgang Betz 44:d757094f6229 775 case LSM6DS3_G_FS_245:
Wolfgang Betz 44:d757094f6229 776 *fullScale = 245.0f;
Wolfgang Betz 44:d757094f6229 777 break;
Wolfgang Betz 44:d757094f6229 778 case LSM6DS3_G_FS_500:
Wolfgang Betz 44:d757094f6229 779 *fullScale = 500.0f;
Wolfgang Betz 44:d757094f6229 780 break;
Wolfgang Betz 44:d757094f6229 781 case LSM6DS3_G_FS_1000:
Wolfgang Betz 44:d757094f6229 782 *fullScale = 1000.0f;
Wolfgang Betz 44:d757094f6229 783 break;
Wolfgang Betz 44:d757094f6229 784 case LSM6DS3_G_FS_2000:
Wolfgang Betz 44:d757094f6229 785 *fullScale = 2000.0f;
Wolfgang Betz 44:d757094f6229 786 break;
Wolfgang Betz 44:d757094f6229 787 default:
Wolfgang Betz 44:d757094f6229 788 break;
Wolfgang Betz 44:d757094f6229 789 }
Wolfgang Betz 44:d757094f6229 790 }
Wolfgang Betz 44:d757094f6229 791
Wolfgang Betz 44:d757094f6229 792 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 793 }
Wolfgang Betz 44:d757094f6229 794
Wolfgang Betz 44:d757094f6229 795 /**
Wolfgang Betz 44:d757094f6229 796 * @brief Write Gyro Full Scale
Wolfgang Betz 44:d757094f6229 797 * @param fullScale the gyroscope full scale to be set
Wolfgang Betz 44:d757094f6229 798 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 799 */
Wolfgang Betz 44:d757094f6229 800 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_FS( float fullScale )
Wolfgang Betz 44:d757094f6229 801 {
Wolfgang Betz 44:d757094f6229 802 uint8_t new_fs = 0x00;
Wolfgang Betz 44:d757094f6229 803 uint8_t tempReg = 0x00;
Wolfgang Betz 44:d757094f6229 804
Wolfgang Betz 44:d757094f6229 805 if(fullScale <= 125.0f)
Wolfgang Betz 44:d757094f6229 806 {
Wolfgang Betz 44:d757094f6229 807 new_fs = LSM6DS3_G_FS_125_ENABLE;
Wolfgang Betz 44:d757094f6229 808
Wolfgang Betz 44:d757094f6229 809 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 810 {
Wolfgang Betz 44:d757094f6229 811 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 812 }
Wolfgang Betz 44:d757094f6229 813
Wolfgang Betz 44:d757094f6229 814 tempReg &= ~(LSM6DS3_G_FS_125_MASK);
Wolfgang Betz 44:d757094f6229 815 tempReg |= new_fs;
Wolfgang Betz 44:d757094f6229 816
Wolfgang Betz 44:d757094f6229 817 if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 818 {
Wolfgang Betz 44:d757094f6229 819 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 820 }
Wolfgang Betz 44:d757094f6229 821 }
Wolfgang Betz 44:d757094f6229 822 else
Wolfgang Betz 44:d757094f6229 823 {
Wolfgang Betz 44:d757094f6229 824 /* Disable G FS 125dpp */
Wolfgang Betz 44:d757094f6229 825 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 826 {
Wolfgang Betz 44:d757094f6229 827 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 828 }
Wolfgang Betz 44:d757094f6229 829
Wolfgang Betz 44:d757094f6229 830 tempReg &= ~(LSM6DS3_G_FS_125_MASK);
Wolfgang Betz 44:d757094f6229 831 tempReg |= LSM6DS3_G_FS_125_DISABLE;
Wolfgang Betz 44:d757094f6229 832
Wolfgang Betz 44:d757094f6229 833 if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 834 {
Wolfgang Betz 44:d757094f6229 835 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 836 }
Wolfgang Betz 44:d757094f6229 837
Wolfgang Betz 44:d757094f6229 838 new_fs = ( fullScale <= 245.0f ) ? LSM6DS3_G_FS_245
Wolfgang Betz 44:d757094f6229 839 : ( fullScale <= 500.0f ) ? LSM6DS3_G_FS_500
Wolfgang Betz 44:d757094f6229 840 : ( fullScale <= 1000.0f ) ? LSM6DS3_G_FS_1000
Wolfgang Betz 44:d757094f6229 841 : LSM6DS3_G_FS_2000;
Wolfgang Betz 44:d757094f6229 842
Wolfgang Betz 44:d757094f6229 843 if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 844 {
Wolfgang Betz 44:d757094f6229 845 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 846 }
Wolfgang Betz 44:d757094f6229 847
Wolfgang Betz 44:d757094f6229 848 tempReg &= ~(LSM6DS3_G_FS_MASK);
Wolfgang Betz 44:d757094f6229 849 tempReg |= new_fs;
Wolfgang Betz 44:d757094f6229 850
Wolfgang Betz 44:d757094f6229 851 if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 852 {
Wolfgang Betz 44:d757094f6229 853 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 854 }
Wolfgang Betz 44:d757094f6229 855 }
Wolfgang Betz 44:d757094f6229 856
Wolfgang Betz 44:d757094f6229 857 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 858 }
Wolfgang Betz 44:d757094f6229 859
Wolfgang Betz 44:d757094f6229 860 /**
Wolfgang Betz 44:d757094f6229 861 * @brief Enable free fall detection
Wolfgang Betz 44:d757094f6229 862 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 863 */
Wolfgang Betz 44:d757094f6229 864 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Enable_Free_Fall_Detection( void )
Wolfgang Betz 44:d757094f6229 865 {
Wolfgang Betz 44:d757094f6229 866 uint8_t tmp1 = 0x00;
Wolfgang Betz 44:d757094f6229 867
Wolfgang Betz 44:d757094f6229 868 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 869 {
Wolfgang Betz 44:d757094f6229 870 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 871 }
Wolfgang Betz 44:d757094f6229 872
Wolfgang Betz 44:d757094f6229 873 /* Output Data Rate selection */
Wolfgang Betz 44:d757094f6229 874 tmp1 &= ~(LSM6DS3_XL_ODR_MASK);
Wolfgang Betz 44:d757094f6229 875 tmp1 |= LSM6DS3_XL_ODR_416HZ;
Wolfgang Betz 44:d757094f6229 876
Wolfgang Betz 44:d757094f6229 877 /* Full scale selection */
Wolfgang Betz 44:d757094f6229 878 tmp1 &= ~(LSM6DS3_XL_FS_MASK);
Wolfgang Betz 44:d757094f6229 879 tmp1 |= LSM6DS3_XL_FS_2G;
Wolfgang Betz 44:d757094f6229 880
Wolfgang Betz 44:d757094f6229 881 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 882 {
Wolfgang Betz 44:d757094f6229 883 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 884 }
Wolfgang Betz 44:d757094f6229 885
Wolfgang Betz 44:d757094f6229 886 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 887 {
Wolfgang Betz 44:d757094f6229 888 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 889 }
Wolfgang Betz 44:d757094f6229 890
Wolfgang Betz 44:d757094f6229 891 /* FF_DUR5 setting */
Wolfgang Betz 44:d757094f6229 892 tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_MASK);
Wolfgang Betz 44:d757094f6229 893 tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_DEFAULT;
Wolfgang Betz 44:d757094f6229 894
Wolfgang Betz 44:d757094f6229 895 /* WAKE_DUR setting */
Wolfgang Betz 44:d757094f6229 896 tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_MASK);
Wolfgang Betz 44:d757094f6229 897 tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_DEFAULT;
Wolfgang Betz 44:d757094f6229 898
Wolfgang Betz 44:d757094f6229 899 /* TIMER_HR setting */
Wolfgang Betz 44:d757094f6229 900 tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_MASK);
Wolfgang Betz 44:d757094f6229 901 tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_DEFAULT;
Wolfgang Betz 44:d757094f6229 902
Wolfgang Betz 44:d757094f6229 903 /* SLEEP_DUR setting */
Wolfgang Betz 44:d757094f6229 904 tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_MASK);
Wolfgang Betz 44:d757094f6229 905 tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_DEFAULT;
Wolfgang Betz 44:d757094f6229 906
Wolfgang Betz 44:d757094f6229 907 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 908 {
Wolfgang Betz 44:d757094f6229 909 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 910 }
Wolfgang Betz 44:d757094f6229 911
Wolfgang Betz 44:d757094f6229 912 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 913 {
Wolfgang Betz 44:d757094f6229 914 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 915 }
Wolfgang Betz 44:d757094f6229 916
Wolfgang Betz 44:d757094f6229 917 /* FF_DUR setting */
Wolfgang Betz 44:d757094f6229 918 tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
Wolfgang Betz 44:d757094f6229 919 tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_TYPICAL;
Wolfgang Betz 44:d757094f6229 920
Wolfgang Betz 44:d757094f6229 921 /* FF_THS setting */
Wolfgang Betz 44:d757094f6229 922 tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
Wolfgang Betz 44:d757094f6229 923 tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_312MG;
Wolfgang Betz 44:d757094f6229 924
Wolfgang Betz 44:d757094f6229 925 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 926 {
Wolfgang Betz 44:d757094f6229 927 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 928 }
Wolfgang Betz 44:d757094f6229 929
Wolfgang Betz 44:d757094f6229 930 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 931 {
Wolfgang Betz 44:d757094f6229 932 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 933 }
Wolfgang Betz 44:d757094f6229 934
Wolfgang Betz 44:d757094f6229 935 /* INT1_FF setting */
Wolfgang Betz 44:d757094f6229 936 tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
Wolfgang Betz 44:d757094f6229 937 tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_ENABLE;
Wolfgang Betz 44:d757094f6229 938
Wolfgang Betz 44:d757094f6229 939 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 940 {
Wolfgang Betz 44:d757094f6229 941 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 942 }
Wolfgang Betz 44:d757094f6229 943
Wolfgang Betz 44:d757094f6229 944 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 945 }
Wolfgang Betz 44:d757094f6229 946
Wolfgang Betz 44:d757094f6229 947 /**
Wolfgang Betz 44:d757094f6229 948 * @brief Disable free fall detection
Wolfgang Betz 44:d757094f6229 949 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 950 */
Wolfgang Betz 44:d757094f6229 951 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Disable_Free_Fall_Detection( void )
Wolfgang Betz 44:d757094f6229 952 {
Wolfgang Betz 44:d757094f6229 953 uint8_t tmp1 = 0x00;
Wolfgang Betz 44:d757094f6229 954
Wolfgang Betz 44:d757094f6229 955 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 956 {
Wolfgang Betz 44:d757094f6229 957 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 958 }
Wolfgang Betz 44:d757094f6229 959
Wolfgang Betz 44:d757094f6229 960 /* INT1_FF setting */
Wolfgang Betz 44:d757094f6229 961 tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
Wolfgang Betz 44:d757094f6229 962 tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE;
Wolfgang Betz 44:d757094f6229 963
Wolfgang Betz 44:d757094f6229 964 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 965 {
Wolfgang Betz 44:d757094f6229 966 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 967 }
Wolfgang Betz 44:d757094f6229 968
Wolfgang Betz 44:d757094f6229 969 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 970 {
Wolfgang Betz 44:d757094f6229 971 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 972 }
Wolfgang Betz 44:d757094f6229 973
Wolfgang Betz 44:d757094f6229 974 /* FF_DUR setting */
Wolfgang Betz 44:d757094f6229 975 tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
Wolfgang Betz 44:d757094f6229 976 tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT;
Wolfgang Betz 44:d757094f6229 977
Wolfgang Betz 44:d757094f6229 978 /* FF_THS setting */
Wolfgang Betz 44:d757094f6229 979 tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
Wolfgang Betz 44:d757094f6229 980 tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG;
Wolfgang Betz 44:d757094f6229 981
Wolfgang Betz 44:d757094f6229 982 if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 983 {
Wolfgang Betz 44:d757094f6229 984 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 985 }
Wolfgang Betz 44:d757094f6229 986
Wolfgang Betz 44:d757094f6229 987 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 988 }
Wolfgang Betz 44:d757094f6229 989
Wolfgang Betz 44:d757094f6229 990 /**
Wolfgang Betz 44:d757094f6229 991 * @brief Get status of free fall detection
Wolfgang Betz 44:d757094f6229 992 * @param status the pointer where the status of free fall detection is stored; 0 means no detection, 1 means detection happened
Wolfgang Betz 44:d757094f6229 993 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 44:d757094f6229 994 */
Wolfgang Betz 44:d757094f6229 995 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Get_Status_Free_Fall_Detection( uint8_t *status )
Wolfgang Betz 44:d757094f6229 996 {
Wolfgang Betz 44:d757094f6229 997 uint8_t tmp1 = 0x00;
Wolfgang Betz 44:d757094f6229 998
Wolfgang Betz 44:d757094f6229 999 if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_UP_SRC, 1) != IMU_6AXES_OK)
Wolfgang Betz 44:d757094f6229 1000 {
Wolfgang Betz 44:d757094f6229 1001 return IMU_6AXES_ERROR;
Wolfgang Betz 44:d757094f6229 1002 }
Wolfgang Betz 44:d757094f6229 1003
Wolfgang Betz 44:d757094f6229 1004 tmp1 &= LSM6DS3_XG_WAKE_UP_SRC_FF_IA_MASK;
Wolfgang Betz 44:d757094f6229 1005
Wolfgang Betz 44:d757094f6229 1006 switch( tmp1 )
Wolfgang Betz 44:d757094f6229 1007 {
Wolfgang Betz 44:d757094f6229 1008 case LSM6DS3_XG_WAKE_UP_SRC_FF_IA_ENABLE:
Wolfgang Betz 44:d757094f6229 1009 *status = 1;
Wolfgang Betz 44:d757094f6229 1010 break;
Wolfgang Betz 44:d757094f6229 1011 case LSM6DS3_XG_WAKE_UP_SRC_FF_IA_DISABLE:
Wolfgang Betz 44:d757094f6229 1012 default:
Wolfgang Betz 44:d757094f6229 1013 *status = 0;
Wolfgang Betz 44:d757094f6229 1014 break;
Wolfgang Betz 44:d757094f6229 1015 }
Wolfgang Betz 44:d757094f6229 1016
Wolfgang Betz 44:d757094f6229 1017 return IMU_6AXES_OK;
Wolfgang Betz 44:d757094f6229 1018 }
Wolfgang Betz 44:d757094f6229 1019
Wolfgang Betz 44:d757094f6229 1020 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/