Mike Fiore / X_NUCLEO_IKS01A1

removed X-NUCLEO-COMMON and ST-INTERFACES

Dependents:   unh-hackathon-example unh-hackathon-example-raw

Fork of X_NUCLEO_IKS01A1 by ST

Components/lsm6ds0/lsm6ds0_class.cpp@7:a2bb3d5e12e9, 2015-04-15 (annotated)

Committer:
Wolfgang Betz
Date:
Wed Apr 15 14:08:20 2015 +0200
Revision:
7:a2bb3d5e12e9
Child:
24:92cc9c6e4b2b
Added component LSM6DS0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wolfgang Betz 7:a2bb3d5e12e9 1 /**
Wolfgang Betz 7:a2bb3d5e12e9 2 ******************************************************************************
Wolfgang Betz 7:a2bb3d5e12e9 3 * @file lsm6ds0_class.cpp
Wolfgang Betz 7:a2bb3d5e12e9 4 * @author AST / EST
Wolfgang Betz 7:a2bb3d5e12e9 5 * @version V0.0.1
Wolfgang Betz 7:a2bb3d5e12e9 6 * @date 14-April-2015
Wolfgang Betz 7:a2bb3d5e12e9 7 * @brief Implementation file for the LSM6DS0 driver class
Wolfgang Betz 7:a2bb3d5e12e9 8 ******************************************************************************
Wolfgang Betz 7:a2bb3d5e12e9 9 * @attention
Wolfgang Betz 7:a2bb3d5e12e9 10 *
Wolfgang Betz 7:a2bb3d5e12e9 11 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Wolfgang Betz 7:a2bb3d5e12e9 12 *
Wolfgang Betz 7:a2bb3d5e12e9 13 * Redistribution and use in source and binary forms, with or without modification,
Wolfgang Betz 7:a2bb3d5e12e9 14 * are permitted provided that the following conditions are met:
Wolfgang Betz 7:a2bb3d5e12e9 15 * 1. Redistributions of source code must retain the above copyright notice,
Wolfgang Betz 7:a2bb3d5e12e9 16 * this list of conditions and the following disclaimer.
Wolfgang Betz 7:a2bb3d5e12e9 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
Wolfgang Betz 7:a2bb3d5e12e9 18 * this list of conditions and the following disclaimer in the documentation
Wolfgang Betz 7:a2bb3d5e12e9 19 * and/or other materials provided with the distribution.
Wolfgang Betz 7:a2bb3d5e12e9 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Wolfgang Betz 7:a2bb3d5e12e9 21 * may be used to endorse or promote products derived from this software
Wolfgang Betz 7:a2bb3d5e12e9 22 * without specific prior written permission.
Wolfgang Betz 7:a2bb3d5e12e9 23 *
Wolfgang Betz 7:a2bb3d5e12e9 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Wolfgang Betz 7:a2bb3d5e12e9 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Wolfgang Betz 7:a2bb3d5e12e9 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Wolfgang Betz 7:a2bb3d5e12e9 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Wolfgang Betz 7:a2bb3d5e12e9 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Wolfgang Betz 7:a2bb3d5e12e9 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Wolfgang Betz 7:a2bb3d5e12e9 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Wolfgang Betz 7:a2bb3d5e12e9 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Wolfgang Betz 7:a2bb3d5e12e9 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Wolfgang Betz 7:a2bb3d5e12e9 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Wolfgang Betz 7:a2bb3d5e12e9 34 *
Wolfgang Betz 7:a2bb3d5e12e9 35 ******************************************************************************
Wolfgang Betz 7:a2bb3d5e12e9 36 */
Wolfgang Betz 7:a2bb3d5e12e9 37
Wolfgang Betz 7:a2bb3d5e12e9 38 /* Includes ------------------------------------------------------------------*/
Wolfgang Betz 7:a2bb3d5e12e9 39 #include "mbed.h"
Wolfgang Betz 7:a2bb3d5e12e9 40 #include "lsm6ds0_class.h"
Wolfgang Betz 7:a2bb3d5e12e9 41 #include "lsm6ds0.h"
Wolfgang Betz 7:a2bb3d5e12e9 42 #include "../../x_nucleo_iks01a1_targets.h"
Wolfgang Betz 7:a2bb3d5e12e9 43
Wolfgang Betz 7:a2bb3d5e12e9 44 /* Methods -------------------------------------------------------------------*/
Wolfgang Betz 7:a2bb3d5e12e9 45 /* betzw - based on:
Wolfgang Betz 7:a2bb3d5e12e9 46 X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds0/lsm6ds0.c: revision #184,
Wolfgang Betz 7:a2bb3d5e12e9 47 X-CUBE-MEMS1/trunk: revision #293
Wolfgang Betz 7:a2bb3d5e12e9 48 */
Wolfgang Betz 7:a2bb3d5e12e9 49 /**
Wolfgang Betz 7:a2bb3d5e12e9 50 * @brief Set LSM6DS0 Initialization
Wolfgang Betz 7:a2bb3d5e12e9 51 * @param LSM6DS0_Init the configuration setting for the LSM6DS0
Wolfgang Betz 7:a2bb3d5e12e9 52 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 53 */
Wolfgang Betz 7:a2bb3d5e12e9 54 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init)
Wolfgang Betz 7:a2bb3d5e12e9 55 {
Wolfgang Betz 7:a2bb3d5e12e9 56 uint8_t tmp1 = 0x00;
Wolfgang Betz 7:a2bb3d5e12e9 57
Wolfgang Betz 7:a2bb3d5e12e9 58 /* Configure the low level interface ---------------------------------------*/
Wolfgang Betz 7:a2bb3d5e12e9 59 if(LSM6DS0_IO_Init() != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 60 {
Wolfgang Betz 7:a2bb3d5e12e9 61 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 62 }
Wolfgang Betz 7:a2bb3d5e12e9 63
Wolfgang Betz 7:a2bb3d5e12e9 64 /******* Gyroscope init *******/
Wolfgang Betz 7:a2bb3d5e12e9 65
Wolfgang Betz 7:a2bb3d5e12e9 66 if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 67 {
Wolfgang Betz 7:a2bb3d5e12e9 68 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 69 }
Wolfgang Betz 7:a2bb3d5e12e9 70
Wolfgang Betz 7:a2bb3d5e12e9 71 /* Output Data Rate selection */
Wolfgang Betz 7:a2bb3d5e12e9 72 tmp1 &= ~(LSM6DS0_G_ODR_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 73 tmp1 |= LSM6DS0_Init->G_OutputDataRate;
Wolfgang Betz 7:a2bb3d5e12e9 74
Wolfgang Betz 7:a2bb3d5e12e9 75 /* Full scale selection */
Wolfgang Betz 7:a2bb3d5e12e9 76 tmp1 &= ~(LSM6DS0_G_FS_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 77 tmp1 |= LSM6DS0_Init->G_FullScale;
Wolfgang Betz 7:a2bb3d5e12e9 78
Wolfgang Betz 7:a2bb3d5e12e9 79 if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 80 {
Wolfgang Betz 7:a2bb3d5e12e9 81 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 82 }
Wolfgang Betz 7:a2bb3d5e12e9 83
Wolfgang Betz 7:a2bb3d5e12e9 84 if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 85 {
Wolfgang Betz 7:a2bb3d5e12e9 86 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 87 }
Wolfgang Betz 7:a2bb3d5e12e9 88
Wolfgang Betz 7:a2bb3d5e12e9 89 /* Enable X axis selection */
Wolfgang Betz 7:a2bb3d5e12e9 90 tmp1 &= ~(LSM6DS0_G_XEN_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 91 tmp1 |= LSM6DS0_Init->G_X_Axis;
Wolfgang Betz 7:a2bb3d5e12e9 92
Wolfgang Betz 7:a2bb3d5e12e9 93 /* Enable Y axis selection */
Wolfgang Betz 7:a2bb3d5e12e9 94 tmp1 &= ~(LSM6DS0_G_YEN_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 95 tmp1 |= LSM6DS0_Init->G_Y_Axis;
Wolfgang Betz 7:a2bb3d5e12e9 96
Wolfgang Betz 7:a2bb3d5e12e9 97 /* Enable Z axis selection */
Wolfgang Betz 7:a2bb3d5e12e9 98 tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 99 tmp1 |= LSM6DS0_Init->G_Z_Axis;
Wolfgang Betz 7:a2bb3d5e12e9 100
Wolfgang Betz 7:a2bb3d5e12e9 101 if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 102 {
Wolfgang Betz 7:a2bb3d5e12e9 103 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 104 }
Wolfgang Betz 7:a2bb3d5e12e9 105
Wolfgang Betz 7:a2bb3d5e12e9 106 /******************************/
Wolfgang Betz 7:a2bb3d5e12e9 107
Wolfgang Betz 7:a2bb3d5e12e9 108 /***** Accelerometer init *****/
Wolfgang Betz 7:a2bb3d5e12e9 109
Wolfgang Betz 7:a2bb3d5e12e9 110 if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 111 {
Wolfgang Betz 7:a2bb3d5e12e9 112 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 113 }
Wolfgang Betz 7:a2bb3d5e12e9 114
Wolfgang Betz 7:a2bb3d5e12e9 115 /* Output Data Rate selection */
Wolfgang Betz 7:a2bb3d5e12e9 116 tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 117 tmp1 |= LSM6DS0_Init->X_OutputDataRate;
Wolfgang Betz 7:a2bb3d5e12e9 118
Wolfgang Betz 7:a2bb3d5e12e9 119 /* Full scale selection */
Wolfgang Betz 7:a2bb3d5e12e9 120 tmp1 &= ~(LSM6DS0_XL_FS_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 121 tmp1 |= LSM6DS0_Init->X_FullScale;
Wolfgang Betz 7:a2bb3d5e12e9 122
Wolfgang Betz 7:a2bb3d5e12e9 123 if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 124 {
Wolfgang Betz 7:a2bb3d5e12e9 125 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 126 }
Wolfgang Betz 7:a2bb3d5e12e9 127
Wolfgang Betz 7:a2bb3d5e12e9 128 if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 129 {
Wolfgang Betz 7:a2bb3d5e12e9 130 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 131 }
Wolfgang Betz 7:a2bb3d5e12e9 132
Wolfgang Betz 7:a2bb3d5e12e9 133 /* Enable X axis selection */
Wolfgang Betz 7:a2bb3d5e12e9 134 tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 135 tmp1 |= LSM6DS0_Init->X_X_Axis;
Wolfgang Betz 7:a2bb3d5e12e9 136
Wolfgang Betz 7:a2bb3d5e12e9 137 /* Enable Y axis selection */
Wolfgang Betz 7:a2bb3d5e12e9 138 tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 139 tmp1 |= LSM6DS0_Init->X_Y_Axis;
Wolfgang Betz 7:a2bb3d5e12e9 140
Wolfgang Betz 7:a2bb3d5e12e9 141 /* Enable Z axis selection */
Wolfgang Betz 7:a2bb3d5e12e9 142 tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
Wolfgang Betz 7:a2bb3d5e12e9 143 tmp1 |= LSM6DS0_Init->X_Z_Axis;
Wolfgang Betz 7:a2bb3d5e12e9 144
Wolfgang Betz 7:a2bb3d5e12e9 145 if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 146 {
Wolfgang Betz 7:a2bb3d5e12e9 147 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 148 }
Wolfgang Betz 7:a2bb3d5e12e9 149
Wolfgang Betz 7:a2bb3d5e12e9 150 /* Configure interrupt lines */
Wolfgang Betz 7:a2bb3d5e12e9 151 LSM6DS0_IO_ITConfig();
Wolfgang Betz 7:a2bb3d5e12e9 152
Wolfgang Betz 7:a2bb3d5e12e9 153 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 154
Wolfgang Betz 7:a2bb3d5e12e9 155 /******************************/
Wolfgang Betz 7:a2bb3d5e12e9 156 }
Wolfgang Betz 7:a2bb3d5e12e9 157
Wolfgang Betz 7:a2bb3d5e12e9 158
Wolfgang Betz 7:a2bb3d5e12e9 159 /**
Wolfgang Betz 7:a2bb3d5e12e9 160 * @brief Read ID of LSM6DS0 Accelerometer and Gyroscope
Wolfgang Betz 7:a2bb3d5e12e9 161 * @param xg_id the pointer where the ID of the device is stored
Wolfgang Betz 7:a2bb3d5e12e9 162 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 163 */
Wolfgang Betz 7:a2bb3d5e12e9 164 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Read_XG_ID(uint8_t *xg_id)
Wolfgang Betz 7:a2bb3d5e12e9 165 {
Wolfgang Betz 7:a2bb3d5e12e9 166 if(!xg_id)
Wolfgang Betz 7:a2bb3d5e12e9 167 {
Wolfgang Betz 7:a2bb3d5e12e9 168 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 169 }
Wolfgang Betz 7:a2bb3d5e12e9 170
Wolfgang Betz 7:a2bb3d5e12e9 171 return LSM6DS0_IO_Read(xg_id, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
Wolfgang Betz 7:a2bb3d5e12e9 172 }
Wolfgang Betz 7:a2bb3d5e12e9 173
Wolfgang Betz 7:a2bb3d5e12e9 174
Wolfgang Betz 7:a2bb3d5e12e9 175 /**
Wolfgang Betz 7:a2bb3d5e12e9 176 * @brief Read raw data from LSM6DS0 Accelerometer output register
Wolfgang Betz 7:a2bb3d5e12e9 177 * @param pData the pointer where the accelerometer raw data are stored
Wolfgang Betz 7:a2bb3d5e12e9 178 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 179 */
Wolfgang Betz 7:a2bb3d5e12e9 180 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxesRaw(int16_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 181 {
Wolfgang Betz 7:a2bb3d5e12e9 182 uint8_t tempReg[2] = {0,0};
Wolfgang Betz 7:a2bb3d5e12e9 183
Wolfgang Betz 7:a2bb3d5e12e9 184 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 185 {
Wolfgang Betz 7:a2bb3d5e12e9 186 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 187 }
Wolfgang Betz 7:a2bb3d5e12e9 188
Wolfgang Betz 7:a2bb3d5e12e9 189 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
Wolfgang Betz 7:a2bb3d5e12e9 190
Wolfgang Betz 7:a2bb3d5e12e9 191 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 192 {
Wolfgang Betz 7:a2bb3d5e12e9 193 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 194 }
Wolfgang Betz 7:a2bb3d5e12e9 195
Wolfgang Betz 7:a2bb3d5e12e9 196 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
Wolfgang Betz 7:a2bb3d5e12e9 197
Wolfgang Betz 7:a2bb3d5e12e9 198 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 199 {
Wolfgang Betz 7:a2bb3d5e12e9 200 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 201 }
Wolfgang Betz 7:a2bb3d5e12e9 202
Wolfgang Betz 7:a2bb3d5e12e9 203 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
Wolfgang Betz 7:a2bb3d5e12e9 204
Wolfgang Betz 7:a2bb3d5e12e9 205 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 206 }
Wolfgang Betz 7:a2bb3d5e12e9 207
Wolfgang Betz 7:a2bb3d5e12e9 208
Wolfgang Betz 7:a2bb3d5e12e9 209 /**
Wolfgang Betz 7:a2bb3d5e12e9 210 * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg
Wolfgang Betz 7:a2bb3d5e12e9 211 * @param pData the pointer where the accelerometer data are stored
Wolfgang Betz 7:a2bb3d5e12e9 212 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 213 */
Wolfgang Betz 7:a2bb3d5e12e9 214 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxes(int32_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 215 {
Wolfgang Betz 7:a2bb3d5e12e9 216 uint8_t tempReg = 0x00;
Wolfgang Betz 7:a2bb3d5e12e9 217 int16_t pDataRaw[3];
Wolfgang Betz 7:a2bb3d5e12e9 218 float sensitivity = 0;
Wolfgang Betz 7:a2bb3d5e12e9 219
Wolfgang Betz 7:a2bb3d5e12e9 220 if(LSM6DS0_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 221 {
Wolfgang Betz 7:a2bb3d5e12e9 222 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 223 }
Wolfgang Betz 7:a2bb3d5e12e9 224
Wolfgang Betz 7:a2bb3d5e12e9 225 if(LSM6DS0_IO_Read(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 226 {
Wolfgang Betz 7:a2bb3d5e12e9 227 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 228 }
Wolfgang Betz 7:a2bb3d5e12e9 229
Wolfgang Betz 7:a2bb3d5e12e9 230 tempReg &= LSM6DS0_XL_FS_MASK;
Wolfgang Betz 7:a2bb3d5e12e9 231
Wolfgang Betz 7:a2bb3d5e12e9 232 switch(tempReg)
Wolfgang Betz 7:a2bb3d5e12e9 233 {
Wolfgang Betz 7:a2bb3d5e12e9 234 case LSM6DS0_XL_FS_2G:
Wolfgang Betz 7:a2bb3d5e12e9 235 sensitivity = 0.061;
Wolfgang Betz 7:a2bb3d5e12e9 236 break;
Wolfgang Betz 7:a2bb3d5e12e9 237 case LSM6DS0_XL_FS_4G:
Wolfgang Betz 7:a2bb3d5e12e9 238 sensitivity = 0.122;
Wolfgang Betz 7:a2bb3d5e12e9 239 break;
Wolfgang Betz 7:a2bb3d5e12e9 240 case LSM6DS0_XL_FS_8G:
Wolfgang Betz 7:a2bb3d5e12e9 241 sensitivity = 0.244;
Wolfgang Betz 7:a2bb3d5e12e9 242 break;
Wolfgang Betz 7:a2bb3d5e12e9 243 }
Wolfgang Betz 7:a2bb3d5e12e9 244
Wolfgang Betz 7:a2bb3d5e12e9 245 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
Wolfgang Betz 7:a2bb3d5e12e9 246 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
Wolfgang Betz 7:a2bb3d5e12e9 247 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
Wolfgang Betz 7:a2bb3d5e12e9 248
Wolfgang Betz 7:a2bb3d5e12e9 249 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 250 }
Wolfgang Betz 7:a2bb3d5e12e9 251
Wolfgang Betz 7:a2bb3d5e12e9 252
Wolfgang Betz 7:a2bb3d5e12e9 253 /**
Wolfgang Betz 7:a2bb3d5e12e9 254 * @brief Read raw data from LSM6DS0 Gyroscope output register
Wolfgang Betz 7:a2bb3d5e12e9 255 * @param pData the pointer where the gyroscope raw data are stored
Wolfgang Betz 7:a2bb3d5e12e9 256 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 257 */
Wolfgang Betz 7:a2bb3d5e12e9 258 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxesRaw(int16_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 259 {
Wolfgang Betz 7:a2bb3d5e12e9 260 uint8_t tempReg[2] = {0,0};
Wolfgang Betz 7:a2bb3d5e12e9 261
Wolfgang Betz 7:a2bb3d5e12e9 262 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 263 {
Wolfgang Betz 7:a2bb3d5e12e9 264 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 265 }
Wolfgang Betz 7:a2bb3d5e12e9 266
Wolfgang Betz 7:a2bb3d5e12e9 267 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
Wolfgang Betz 7:a2bb3d5e12e9 268
Wolfgang Betz 7:a2bb3d5e12e9 269 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 270 {
Wolfgang Betz 7:a2bb3d5e12e9 271 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 272 }
Wolfgang Betz 7:a2bb3d5e12e9 273
Wolfgang Betz 7:a2bb3d5e12e9 274 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
Wolfgang Betz 7:a2bb3d5e12e9 275
Wolfgang Betz 7:a2bb3d5e12e9 276 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 277 {
Wolfgang Betz 7:a2bb3d5e12e9 278 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 279 }
Wolfgang Betz 7:a2bb3d5e12e9 280
Wolfgang Betz 7:a2bb3d5e12e9 281 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
Wolfgang Betz 7:a2bb3d5e12e9 282
Wolfgang Betz 7:a2bb3d5e12e9 283 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 284 }
Wolfgang Betz 7:a2bb3d5e12e9 285
Wolfgang Betz 7:a2bb3d5e12e9 286
Wolfgang Betz 7:a2bb3d5e12e9 287 /**
Wolfgang Betz 7:a2bb3d5e12e9 288 * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps
Wolfgang Betz 7:a2bb3d5e12e9 289 * @param pData the pointer where the gyroscope data are stored
Wolfgang Betz 7:a2bb3d5e12e9 290 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 291 */
Wolfgang Betz 7:a2bb3d5e12e9 292 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxes(int32_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 293 {
Wolfgang Betz 7:a2bb3d5e12e9 294 uint8_t tempReg = 0x00;
Wolfgang Betz 7:a2bb3d5e12e9 295 int16_t pDataRaw[3];
Wolfgang Betz 7:a2bb3d5e12e9 296 float sensitivity = 0;
Wolfgang Betz 7:a2bb3d5e12e9 297
Wolfgang Betz 7:a2bb3d5e12e9 298 if(LSM6DS0_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 299 {
Wolfgang Betz 7:a2bb3d5e12e9 300 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 301 }
Wolfgang Betz 7:a2bb3d5e12e9 302
Wolfgang Betz 7:a2bb3d5e12e9 303 if(LSM6DS0_IO_Read(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 304 {
Wolfgang Betz 7:a2bb3d5e12e9 305 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 306 }
Wolfgang Betz 7:a2bb3d5e12e9 307
Wolfgang Betz 7:a2bb3d5e12e9 308 tempReg &= LSM6DS0_G_FS_MASK;
Wolfgang Betz 7:a2bb3d5e12e9 309
Wolfgang Betz 7:a2bb3d5e12e9 310 switch(tempReg)
Wolfgang Betz 7:a2bb3d5e12e9 311 {
Wolfgang Betz 7:a2bb3d5e12e9 312 case LSM6DS0_G_FS_245:
Wolfgang Betz 7:a2bb3d5e12e9 313 sensitivity = 8.75;
Wolfgang Betz 7:a2bb3d5e12e9 314 break;
Wolfgang Betz 7:a2bb3d5e12e9 315 case LSM6DS0_G_FS_500:
Wolfgang Betz 7:a2bb3d5e12e9 316 sensitivity = 17.50;
Wolfgang Betz 7:a2bb3d5e12e9 317 break;
Wolfgang Betz 7:a2bb3d5e12e9 318 case LSM6DS0_G_FS_2000:
Wolfgang Betz 7:a2bb3d5e12e9 319 sensitivity = 70;
Wolfgang Betz 7:a2bb3d5e12e9 320 break;
Wolfgang Betz 7:a2bb3d5e12e9 321 }
Wolfgang Betz 7:a2bb3d5e12e9 322
Wolfgang Betz 7:a2bb3d5e12e9 323 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
Wolfgang Betz 7:a2bb3d5e12e9 324 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
Wolfgang Betz 7:a2bb3d5e12e9 325 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
Wolfgang Betz 7:a2bb3d5e12e9 326
Wolfgang Betz 7:a2bb3d5e12e9 327 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 328 }
Wolfgang Betz 7:a2bb3d5e12e9 329
Wolfgang Betz 7:a2bb3d5e12e9 330 /**
Wolfgang Betz 7:a2bb3d5e12e9 331 * @brief Read Accelero Sensitivity
Wolfgang Betz 7:a2bb3d5e12e9 332 * @param pfData the pointer where the accelerometer sensitivity is stored
Wolfgang Betz 7:a2bb3d5e12e9 333 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 334 */
Wolfgang Betz 7:a2bb3d5e12e9 335 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetSensitivity( float *pfData )
Wolfgang Betz 7:a2bb3d5e12e9 336 {
Wolfgang Betz 7:a2bb3d5e12e9 337 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 7:a2bb3d5e12e9 338 uint8_t tempReg = 0x00;
Wolfgang Betz 7:a2bb3d5e12e9 339
Wolfgang Betz 7:a2bb3d5e12e9 340 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 341 {
Wolfgang Betz 7:a2bb3d5e12e9 342 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 343 }
Wolfgang Betz 7:a2bb3d5e12e9 344
Wolfgang Betz 7:a2bb3d5e12e9 345 tempReg &= LSM6DS0_XL_FS_MASK;
Wolfgang Betz 7:a2bb3d5e12e9 346
Wolfgang Betz 7:a2bb3d5e12e9 347 switch( tempReg )
Wolfgang Betz 7:a2bb3d5e12e9 348 {
Wolfgang Betz 7:a2bb3d5e12e9 349 case LSM6DS0_XL_FS_2G:
Wolfgang Betz 7:a2bb3d5e12e9 350 *pfData = 0.061;
Wolfgang Betz 7:a2bb3d5e12e9 351 break;
Wolfgang Betz 7:a2bb3d5e12e9 352 case LSM6DS0_XL_FS_4G:
Wolfgang Betz 7:a2bb3d5e12e9 353 *pfData = 0.122;
Wolfgang Betz 7:a2bb3d5e12e9 354 break;
Wolfgang Betz 7:a2bb3d5e12e9 355 case LSM6DS0_XL_FS_8G:
Wolfgang Betz 7:a2bb3d5e12e9 356 *pfData = 0.244;
Wolfgang Betz 7:a2bb3d5e12e9 357 break;
Wolfgang Betz 7:a2bb3d5e12e9 358 default:
Wolfgang Betz 7:a2bb3d5e12e9 359 break;
Wolfgang Betz 7:a2bb3d5e12e9 360 }
Wolfgang Betz 7:a2bb3d5e12e9 361
Wolfgang Betz 7:a2bb3d5e12e9 362 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 363 }
Wolfgang Betz 7:a2bb3d5e12e9 364
Wolfgang Betz 7:a2bb3d5e12e9 365
Wolfgang Betz 7:a2bb3d5e12e9 366
Wolfgang Betz 7:a2bb3d5e12e9 367 /**
Wolfgang Betz 7:a2bb3d5e12e9 368 * @brief Read Gyro Sensitivity
Wolfgang Betz 7:a2bb3d5e12e9 369 * @param pfData the pointer where the gyroscope sensitivity is stored
Wolfgang Betz 7:a2bb3d5e12e9 370 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 371 */
Wolfgang Betz 7:a2bb3d5e12e9 372 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetSensitivity( float *pfData )
Wolfgang Betz 7:a2bb3d5e12e9 373 {
Wolfgang Betz 7:a2bb3d5e12e9 374 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 7:a2bb3d5e12e9 375 uint8_t tempReg = 0x00;
Wolfgang Betz 7:a2bb3d5e12e9 376
Wolfgang Betz 7:a2bb3d5e12e9 377 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 7:a2bb3d5e12e9 378 {
Wolfgang Betz 7:a2bb3d5e12e9 379 return IMU_6AXES_ERROR;
Wolfgang Betz 7:a2bb3d5e12e9 380 }
Wolfgang Betz 7:a2bb3d5e12e9 381
Wolfgang Betz 7:a2bb3d5e12e9 382 tempReg &= LSM6DS0_G_FS_MASK;
Wolfgang Betz 7:a2bb3d5e12e9 383
Wolfgang Betz 7:a2bb3d5e12e9 384 switch( tempReg )
Wolfgang Betz 7:a2bb3d5e12e9 385 {
Wolfgang Betz 7:a2bb3d5e12e9 386 case LSM6DS0_G_FS_245:
Wolfgang Betz 7:a2bb3d5e12e9 387 *pfData = 8.75;
Wolfgang Betz 7:a2bb3d5e12e9 388 break;
Wolfgang Betz 7:a2bb3d5e12e9 389 case LSM6DS0_G_FS_500:
Wolfgang Betz 7:a2bb3d5e12e9 390 *pfData = 17.50;
Wolfgang Betz 7:a2bb3d5e12e9 391 break;
Wolfgang Betz 7:a2bb3d5e12e9 392 case LSM6DS0_G_FS_2000:
Wolfgang Betz 7:a2bb3d5e12e9 393 *pfData = 70;
Wolfgang Betz 7:a2bb3d5e12e9 394 break;
Wolfgang Betz 7:a2bb3d5e12e9 395 default:
Wolfgang Betz 7:a2bb3d5e12e9 396 break;
Wolfgang Betz 7:a2bb3d5e12e9 397 }
Wolfgang Betz 7:a2bb3d5e12e9 398
Wolfgang Betz 7:a2bb3d5e12e9 399 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 400 }