Jamie Satchell / Mbed OS JSatchell_SOFT253ReferredCoursework

Finished project.

Dependencies:   X_NUCLEO_COMMON

Fork of ReferredCoursework2016 by Stage-1 Students SoCEM

Components/lsm6ds3/lsm6ds3_class.cpp@91:cd9fcd45ecf6, 2017-08-17 (annotated)

Committer:
J_Satchell
Date:
Thu Aug 17 06:58:29 2017 +0000
Revision:
91:cd9fcd45ecf6
Parent:
57:04563dd74269 
Added mutex

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