/ X_NUCLEO_IKS01A1_Local

local changes to increase sample rate

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON_Local

Fork of X_NUCLEO_IKS01A1 by ST

Components/lsm6ds0/lsm6ds0_class.cpp@24:92cc9c6e4b2b, 2015-06-03 (annotated)

Committer:
Wolfgang Betz
Date:
Wed Jun 03 14:57:57 2015 +0200
Revision:
24:92cc9c6e4b2b
Parent:
7:a2bb3d5e12e9
Child:
57:04563dd74269
Upgrade to match X-CUBE-MEMS1/trunk, revision #402

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 24:92cc9c6e4b2b 46 X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds0/lsm6ds0.c: revision #400,
Wolfgang Betz 24:92cc9c6e4b2b 47 X-CUBE-MEMS1/trunk: revision #402
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 24:92cc9c6e4b2b 56 /* Configure the low level interface ---------------------------------------*/
Wolfgang Betz 24:92cc9c6e4b2b 57 if(LSM6DS0_IO_Init() != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 58 {
Wolfgang Betz 24:92cc9c6e4b2b 59 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 60 }
Wolfgang Betz 24:92cc9c6e4b2b 61
Wolfgang Betz 24:92cc9c6e4b2b 62 /******* Gyroscope init *******/
Wolfgang Betz 24:92cc9c6e4b2b 63
Wolfgang Betz 24:92cc9c6e4b2b 64 if(LSM6DS0_G_Set_ODR( LSM6DS0_Init->G_OutputDataRate ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 65 {
Wolfgang Betz 24:92cc9c6e4b2b 66 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 67 }
Wolfgang Betz 24:92cc9c6e4b2b 68
Wolfgang Betz 24:92cc9c6e4b2b 69 if(LSM6DS0_G_Set_FS( LSM6DS0_Init->G_FullScale ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 70 {
Wolfgang Betz 24:92cc9c6e4b2b 71 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 72 }
Wolfgang Betz 24:92cc9c6e4b2b 73
Wolfgang Betz 24:92cc9c6e4b2b 74 if(LSM6DS0_G_Set_Axes_Status(LSM6DS0_Init->G_X_Axis, LSM6DS0_Init->G_Y_Axis, LSM6DS0_Init->G_Z_Axis) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 75 {
Wolfgang Betz 24:92cc9c6e4b2b 76 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 77 }
Wolfgang Betz 24:92cc9c6e4b2b 78
Wolfgang Betz 24:92cc9c6e4b2b 79 /******************************/
Wolfgang Betz 24:92cc9c6e4b2b 80
Wolfgang Betz 24:92cc9c6e4b2b 81 /***** Accelerometer init *****/
Wolfgang Betz 24:92cc9c6e4b2b 82
Wolfgang Betz 24:92cc9c6e4b2b 83 if(LSM6DS0_X_Set_ODR( LSM6DS0_Init->X_OutputDataRate ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 84 {
Wolfgang Betz 24:92cc9c6e4b2b 85 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 86 }
Wolfgang Betz 24:92cc9c6e4b2b 87
Wolfgang Betz 24:92cc9c6e4b2b 88 if(LSM6DS0_X_Set_FS( LSM6DS0_Init->X_FullScale ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 89 {
Wolfgang Betz 24:92cc9c6e4b2b 90 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 91 }
Wolfgang Betz 24:92cc9c6e4b2b 92
Wolfgang Betz 24:92cc9c6e4b2b 93 if(LSM6DS0_X_Set_Axes_Status(LSM6DS0_Init->X_X_Axis, LSM6DS0_Init->X_Y_Axis, LSM6DS0_Init->X_Z_Axis) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 94 {
Wolfgang Betz 24:92cc9c6e4b2b 95 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 96 }
Wolfgang Betz 24:92cc9c6e4b2b 97
Wolfgang Betz 24:92cc9c6e4b2b 98 /* Configure interrupt lines */
Wolfgang Betz 24:92cc9c6e4b2b 99 LSM6DS0_IO_ITConfig();
Wolfgang Betz 24:92cc9c6e4b2b 100
Wolfgang Betz 24:92cc9c6e4b2b 101 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 102
Wolfgang Betz 24:92cc9c6e4b2b 103 /******************************/
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 * @brief Read ID of LSM6DS0 Accelerometer and Gyroscope
Wolfgang Betz 7:a2bb3d5e12e9 109 * @param xg_id the pointer where the ID of the device is stored
Wolfgang Betz 7:a2bb3d5e12e9 110 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 111 */
Wolfgang Betz 7:a2bb3d5e12e9 112 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Read_XG_ID(uint8_t *xg_id)
Wolfgang Betz 7:a2bb3d5e12e9 113 {
Wolfgang Betz 24:92cc9c6e4b2b 114 if(!xg_id)
Wolfgang Betz 24:92cc9c6e4b2b 115 {
Wolfgang Betz 24:92cc9c6e4b2b 116 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 117 }
Wolfgang Betz 24:92cc9c6e4b2b 118
Wolfgang Betz 24:92cc9c6e4b2b 119 return LSM6DS0_IO_Read(xg_id, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
Wolfgang Betz 7:a2bb3d5e12e9 120 }
Wolfgang Betz 7:a2bb3d5e12e9 121
Wolfgang Betz 7:a2bb3d5e12e9 122
Wolfgang Betz 7:a2bb3d5e12e9 123 /**
Wolfgang Betz 7:a2bb3d5e12e9 124 * @brief Read raw data from LSM6DS0 Accelerometer output register
Wolfgang Betz 7:a2bb3d5e12e9 125 * @param pData the pointer where the accelerometer raw data are stored
Wolfgang Betz 7:a2bb3d5e12e9 126 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 127 */
Wolfgang Betz 7:a2bb3d5e12e9 128 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxesRaw(int16_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 129 {
Wolfgang Betz 24:92cc9c6e4b2b 130 uint8_t tempReg[2] = {0, 0};
Wolfgang Betz 24:92cc9c6e4b2b 131
Wolfgang Betz 24:92cc9c6e4b2b 132 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
Wolfgang Betz 24:92cc9c6e4b2b 133 2) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 134 {
Wolfgang Betz 24:92cc9c6e4b2b 135 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 136 }
Wolfgang Betz 24:92cc9c6e4b2b 137
Wolfgang Betz 24:92cc9c6e4b2b 138 pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 24:92cc9c6e4b2b 139
Wolfgang Betz 24:92cc9c6e4b2b 140 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
Wolfgang Betz 24:92cc9c6e4b2b 141 2) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 142 {
Wolfgang Betz 24:92cc9c6e4b2b 143 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 144 }
Wolfgang Betz 24:92cc9c6e4b2b 145
Wolfgang Betz 24:92cc9c6e4b2b 146 pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 24:92cc9c6e4b2b 147
Wolfgang Betz 24:92cc9c6e4b2b 148 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
Wolfgang Betz 24:92cc9c6e4b2b 149 2) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 150 {
Wolfgang Betz 24:92cc9c6e4b2b 151 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 152 }
Wolfgang Betz 24:92cc9c6e4b2b 153
Wolfgang Betz 24:92cc9c6e4b2b 154 pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 24:92cc9c6e4b2b 155
Wolfgang Betz 24:92cc9c6e4b2b 156 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 157 }
Wolfgang Betz 7:a2bb3d5e12e9 158
Wolfgang Betz 7:a2bb3d5e12e9 159
Wolfgang Betz 7:a2bb3d5e12e9 160 /**
Wolfgang Betz 7:a2bb3d5e12e9 161 * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg
Wolfgang Betz 7:a2bb3d5e12e9 162 * @param pData the pointer where the accelerometer data are stored
Wolfgang Betz 7:a2bb3d5e12e9 163 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 164 */
Wolfgang Betz 7:a2bb3d5e12e9 165 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxes(int32_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 166 {
Wolfgang Betz 24:92cc9c6e4b2b 167 int16_t pDataRaw[3];
Wolfgang Betz 24:92cc9c6e4b2b 168 float sensitivity = 0;
Wolfgang Betz 24:92cc9c6e4b2b 169
Wolfgang Betz 24:92cc9c6e4b2b 170 if(LSM6DS0_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 171 {
Wolfgang Betz 24:92cc9c6e4b2b 172 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 173 }
Wolfgang Betz 24:92cc9c6e4b2b 174
Wolfgang Betz 24:92cc9c6e4b2b 175 if(LSM6DS0_X_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 176 {
Wolfgang Betz 24:92cc9c6e4b2b 177 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 178 }
Wolfgang Betz 24:92cc9c6e4b2b 179
Wolfgang Betz 24:92cc9c6e4b2b 180 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
Wolfgang Betz 24:92cc9c6e4b2b 181 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
Wolfgang Betz 24:92cc9c6e4b2b 182 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
Wolfgang Betz 24:92cc9c6e4b2b 183
Wolfgang Betz 24:92cc9c6e4b2b 184 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 185 }
Wolfgang Betz 7:a2bb3d5e12e9 186
Wolfgang Betz 7:a2bb3d5e12e9 187
Wolfgang Betz 7:a2bb3d5e12e9 188 /**
Wolfgang Betz 7:a2bb3d5e12e9 189 * @brief Read raw data from LSM6DS0 Gyroscope output register
Wolfgang Betz 7:a2bb3d5e12e9 190 * @param pData the pointer where the gyroscope raw data are stored
Wolfgang Betz 7:a2bb3d5e12e9 191 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 192 */
Wolfgang Betz 7:a2bb3d5e12e9 193 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxesRaw(int16_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 194 {
Wolfgang Betz 24:92cc9c6e4b2b 195 uint8_t tempReg[2] = {0, 0};
Wolfgang Betz 24:92cc9c6e4b2b 196
Wolfgang Betz 24:92cc9c6e4b2b 197 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
Wolfgang Betz 24:92cc9c6e4b2b 198 2) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 199 {
Wolfgang Betz 24:92cc9c6e4b2b 200 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 201 }
Wolfgang Betz 24:92cc9c6e4b2b 202
Wolfgang Betz 24:92cc9c6e4b2b 203 pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 24:92cc9c6e4b2b 204
Wolfgang Betz 24:92cc9c6e4b2b 205 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
Wolfgang Betz 24:92cc9c6e4b2b 206 2) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 207 {
Wolfgang Betz 24:92cc9c6e4b2b 208 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 209 }
Wolfgang Betz 24:92cc9c6e4b2b 210
Wolfgang Betz 24:92cc9c6e4b2b 211 pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 24:92cc9c6e4b2b 212
Wolfgang Betz 24:92cc9c6e4b2b 213 if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
Wolfgang Betz 24:92cc9c6e4b2b 214 2) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 215 {
Wolfgang Betz 24:92cc9c6e4b2b 216 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 217 }
Wolfgang Betz 24:92cc9c6e4b2b 218
Wolfgang Betz 24:92cc9c6e4b2b 219 pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
Wolfgang Betz 24:92cc9c6e4b2b 220
Wolfgang Betz 24:92cc9c6e4b2b 221 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 222 }
Wolfgang Betz 7:a2bb3d5e12e9 223
Wolfgang Betz 24:92cc9c6e4b2b 224 /**
Wolfgang Betz 24:92cc9c6e4b2b 225 * @brief Set the status of the axes for accelerometer
Wolfgang Betz 24:92cc9c6e4b2b 226 * @param enableX the status of the x axis to be set
Wolfgang Betz 24:92cc9c6e4b2b 227 * @param enableY the status of the y axis to be set
Wolfgang Betz 24:92cc9c6e4b2b 228 * @param enableZ the status of the z axis to be set
Wolfgang Betz 24:92cc9c6e4b2b 229 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 230 */
Wolfgang Betz 24:92cc9c6e4b2b 231 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
Wolfgang Betz 24:92cc9c6e4b2b 232 {
Wolfgang Betz 24:92cc9c6e4b2b 233 uint8_t tmp1 = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 234 uint8_t eX = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 235 uint8_t eY = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 236 uint8_t eZ = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 237
Wolfgang Betz 24:92cc9c6e4b2b 238 eX = ( enableX == 0 ) ? LSM6DS0_XL_XEN_DISABLE : LSM6DS0_XL_XEN_ENABLE;
Wolfgang Betz 24:92cc9c6e4b2b 239 eY = ( enableY == 0 ) ? LSM6DS0_XL_YEN_DISABLE : LSM6DS0_XL_YEN_ENABLE;
Wolfgang Betz 24:92cc9c6e4b2b 240 eZ = ( enableZ == 0 ) ? LSM6DS0_XL_ZEN_DISABLE : LSM6DS0_XL_ZEN_ENABLE;
Wolfgang Betz 24:92cc9c6e4b2b 241
Wolfgang Betz 24:92cc9c6e4b2b 242 if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 243 {
Wolfgang Betz 24:92cc9c6e4b2b 244 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 245 }
Wolfgang Betz 24:92cc9c6e4b2b 246
Wolfgang Betz 24:92cc9c6e4b2b 247 /* Enable X axis selection */
Wolfgang Betz 24:92cc9c6e4b2b 248 tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 249 tmp1 |= eX;
Wolfgang Betz 24:92cc9c6e4b2b 250
Wolfgang Betz 24:92cc9c6e4b2b 251 /* Enable Y axis selection */
Wolfgang Betz 24:92cc9c6e4b2b 252 tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 253 tmp1 |= eY;
Wolfgang Betz 24:92cc9c6e4b2b 254
Wolfgang Betz 24:92cc9c6e4b2b 255 /* Enable Z axis selection */
Wolfgang Betz 24:92cc9c6e4b2b 256 tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 257 tmp1 |= eZ;
Wolfgang Betz 24:92cc9c6e4b2b 258
Wolfgang Betz 24:92cc9c6e4b2b 259 if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 260 {
Wolfgang Betz 24:92cc9c6e4b2b 261 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 262 }
Wolfgang Betz 24:92cc9c6e4b2b 263
Wolfgang Betz 24:92cc9c6e4b2b 264 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 265 }
Wolfgang Betz 7:a2bb3d5e12e9 266
Wolfgang Betz 24:92cc9c6e4b2b 267 /**
Wolfgang Betz 24:92cc9c6e4b2b 268 * @brief Set the status of the axes for gyroscope
Wolfgang Betz 24:92cc9c6e4b2b 269 * @param enableX the status of the x axis to be set
Wolfgang Betz 24:92cc9c6e4b2b 270 * @param enableY the status of the y axis to be set
Wolfgang Betz 24:92cc9c6e4b2b 271 * @param enableZ the status of the z axis to be set
Wolfgang Betz 24:92cc9c6e4b2b 272 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 273 */
Wolfgang Betz 24:92cc9c6e4b2b 274 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
Wolfgang Betz 24:92cc9c6e4b2b 275 {
Wolfgang Betz 24:92cc9c6e4b2b 276 uint8_t tmp1 = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 277 uint8_t eX = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 278 uint8_t eY = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 279 uint8_t eZ = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 280
Wolfgang Betz 24:92cc9c6e4b2b 281 eX = ( enableX == 0 ) ? LSM6DS0_G_XEN_DISABLE : LSM6DS0_G_XEN_ENABLE;
Wolfgang Betz 24:92cc9c6e4b2b 282 eY = ( enableY == 0 ) ? LSM6DS0_G_YEN_DISABLE : LSM6DS0_G_YEN_ENABLE;
Wolfgang Betz 24:92cc9c6e4b2b 283 eZ = ( enableZ == 0 ) ? LSM6DS0_G_ZEN_DISABLE : LSM6DS0_G_ZEN_ENABLE;
Wolfgang Betz 24:92cc9c6e4b2b 284
Wolfgang Betz 24:92cc9c6e4b2b 285 if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 286 {
Wolfgang Betz 24:92cc9c6e4b2b 287 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 288 }
Wolfgang Betz 24:92cc9c6e4b2b 289
Wolfgang Betz 24:92cc9c6e4b2b 290 /* Enable X axis selection */
Wolfgang Betz 24:92cc9c6e4b2b 291 tmp1 &= ~(LSM6DS0_G_XEN_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 292 tmp1 |= eX;
Wolfgang Betz 24:92cc9c6e4b2b 293
Wolfgang Betz 24:92cc9c6e4b2b 294 /* Enable Y axis selection */
Wolfgang Betz 24:92cc9c6e4b2b 295 tmp1 &= ~(LSM6DS0_G_YEN_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 296 tmp1 |= eY;
Wolfgang Betz 24:92cc9c6e4b2b 297
Wolfgang Betz 24:92cc9c6e4b2b 298 /* Enable Z axis selection */
Wolfgang Betz 24:92cc9c6e4b2b 299 tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 300 tmp1 |= eZ;
Wolfgang Betz 24:92cc9c6e4b2b 301
Wolfgang Betz 24:92cc9c6e4b2b 302 if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 303 {
Wolfgang Betz 24:92cc9c6e4b2b 304 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 305 }
Wolfgang Betz 24:92cc9c6e4b2b 306
Wolfgang Betz 24:92cc9c6e4b2b 307 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 308 }
Wolfgang Betz 7:a2bb3d5e12e9 309
Wolfgang Betz 7:a2bb3d5e12e9 310
Wolfgang Betz 7:a2bb3d5e12e9 311 /**
Wolfgang Betz 7:a2bb3d5e12e9 312 * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps
Wolfgang Betz 7:a2bb3d5e12e9 313 * @param pData the pointer where the gyroscope data are stored
Wolfgang Betz 7:a2bb3d5e12e9 314 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 315 */
Wolfgang Betz 7:a2bb3d5e12e9 316 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxes(int32_t *pData)
Wolfgang Betz 7:a2bb3d5e12e9 317 {
Wolfgang Betz 24:92cc9c6e4b2b 318 int16_t pDataRaw[3];
Wolfgang Betz 24:92cc9c6e4b2b 319 float sensitivity = 0;
Wolfgang Betz 24:92cc9c6e4b2b 320
Wolfgang Betz 24:92cc9c6e4b2b 321 if(LSM6DS0_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 322 {
Wolfgang Betz 24:92cc9c6e4b2b 323 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 324 }
Wolfgang Betz 24:92cc9c6e4b2b 325
Wolfgang Betz 24:92cc9c6e4b2b 326 if(LSM6DS0_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 327 {
Wolfgang Betz 24:92cc9c6e4b2b 328 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 329 }
Wolfgang Betz 24:92cc9c6e4b2b 330
Wolfgang Betz 24:92cc9c6e4b2b 331 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
Wolfgang Betz 24:92cc9c6e4b2b 332 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
Wolfgang Betz 24:92cc9c6e4b2b 333 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
Wolfgang Betz 24:92cc9c6e4b2b 334
Wolfgang Betz 24:92cc9c6e4b2b 335 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 336 }
Wolfgang Betz 7:a2bb3d5e12e9 337
Wolfgang Betz 24:92cc9c6e4b2b 338 /**
Wolfgang Betz 24:92cc9c6e4b2b 339 * @brief Read Accelero Output Data Rate
Wolfgang Betz 24:92cc9c6e4b2b 340 * @param odr the pointer where the accelerometer output data rate is stored
Wolfgang Betz 24:92cc9c6e4b2b 341 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 342 */
Wolfgang Betz 24:92cc9c6e4b2b 343 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Get_ODR( float *odr )
Wolfgang Betz 24:92cc9c6e4b2b 344 {
Wolfgang Betz 24:92cc9c6e4b2b 345 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 24:92cc9c6e4b2b 346 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 347
Wolfgang Betz 24:92cc9c6e4b2b 348 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 349 {
Wolfgang Betz 24:92cc9c6e4b2b 350 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 351 }
Wolfgang Betz 24:92cc9c6e4b2b 352
Wolfgang Betz 24:92cc9c6e4b2b 353 tempReg &= LSM6DS0_XL_ODR_MASK;
Wolfgang Betz 24:92cc9c6e4b2b 354
Wolfgang Betz 24:92cc9c6e4b2b 355 switch( tempReg )
Wolfgang Betz 24:92cc9c6e4b2b 356 {
Wolfgang Betz 24:92cc9c6e4b2b 357 case LSM6DS0_XL_ODR_PD:
Wolfgang Betz 24:92cc9c6e4b2b 358 *odr = 0.0f;
Wolfgang Betz 24:92cc9c6e4b2b 359 break;
Wolfgang Betz 24:92cc9c6e4b2b 360 case LSM6DS0_XL_ODR_10HZ:
Wolfgang Betz 24:92cc9c6e4b2b 361 *odr = 10.0f;
Wolfgang Betz 24:92cc9c6e4b2b 362 break;
Wolfgang Betz 24:92cc9c6e4b2b 363 case LSM6DS0_XL_ODR_50HZ:
Wolfgang Betz 24:92cc9c6e4b2b 364 *odr = 50.0f;
Wolfgang Betz 24:92cc9c6e4b2b 365 break;
Wolfgang Betz 24:92cc9c6e4b2b 366 case LSM6DS0_XL_ODR_119HZ:
Wolfgang Betz 24:92cc9c6e4b2b 367 *odr = 119.0f;
Wolfgang Betz 24:92cc9c6e4b2b 368 break;
Wolfgang Betz 24:92cc9c6e4b2b 369 case LSM6DS0_XL_ODR_238HZ:
Wolfgang Betz 24:92cc9c6e4b2b 370 *odr = 238.0f;
Wolfgang Betz 24:92cc9c6e4b2b 371 break;
Wolfgang Betz 24:92cc9c6e4b2b 372 case LSM6DS0_XL_ODR_476HZ:
Wolfgang Betz 24:92cc9c6e4b2b 373 *odr = 476.0f;
Wolfgang Betz 24:92cc9c6e4b2b 374 break;
Wolfgang Betz 24:92cc9c6e4b2b 375 case LSM6DS0_XL_ODR_952HZ:
Wolfgang Betz 24:92cc9c6e4b2b 376 *odr = 952.0f;
Wolfgang Betz 24:92cc9c6e4b2b 377 break;
Wolfgang Betz 24:92cc9c6e4b2b 378 default:
Wolfgang Betz 24:92cc9c6e4b2b 379 break;
Wolfgang Betz 24:92cc9c6e4b2b 380 }
Wolfgang Betz 24:92cc9c6e4b2b 381
Wolfgang Betz 24:92cc9c6e4b2b 382 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 383 }
Wolfgang Betz 7:a2bb3d5e12e9 384
Wolfgang Betz 24:92cc9c6e4b2b 385 /**
Wolfgang Betz 24:92cc9c6e4b2b 386 * @brief Write Accelero Output Data Rate
Wolfgang Betz 24:92cc9c6e4b2b 387 * @param odr the accelerometer output data rate to be set
Wolfgang Betz 24:92cc9c6e4b2b 388 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 389 */
Wolfgang Betz 24:92cc9c6e4b2b 390 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Set_ODR( float odr )
Wolfgang Betz 24:92cc9c6e4b2b 391 {
Wolfgang Betz 24:92cc9c6e4b2b 392 uint8_t new_odr = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 393 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 394
Wolfgang Betz 24:92cc9c6e4b2b 395 new_odr = ( odr <= 0.0f ) ? LSM6DS0_XL_ODR_PD /* Power Down */
Wolfgang Betz 24:92cc9c6e4b2b 396 : ( odr <= 10.0f ) ? LSM6DS0_XL_ODR_10HZ
Wolfgang Betz 24:92cc9c6e4b2b 397 : ( odr <= 50.0f ) ? LSM6DS0_XL_ODR_50HZ
Wolfgang Betz 24:92cc9c6e4b2b 398 : ( odr <= 119.0f ) ? LSM6DS0_XL_ODR_119HZ
Wolfgang Betz 24:92cc9c6e4b2b 399 : ( odr <= 238.0f ) ? LSM6DS0_XL_ODR_238HZ
Wolfgang Betz 24:92cc9c6e4b2b 400 : ( odr <= 476.0f ) ? LSM6DS0_XL_ODR_476HZ
Wolfgang Betz 24:92cc9c6e4b2b 401 : LSM6DS0_XL_ODR_952HZ;
Wolfgang Betz 24:92cc9c6e4b2b 402
Wolfgang Betz 24:92cc9c6e4b2b 403 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 404 {
Wolfgang Betz 24:92cc9c6e4b2b 405 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 406 }
Wolfgang Betz 24:92cc9c6e4b2b 407
Wolfgang Betz 24:92cc9c6e4b2b 408 tempReg &= ~(LSM6DS0_XL_ODR_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 409 tempReg |= new_odr;
Wolfgang Betz 24:92cc9c6e4b2b 410
Wolfgang Betz 24:92cc9c6e4b2b 411 if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 412 {
Wolfgang Betz 24:92cc9c6e4b2b 413 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 414 }
Wolfgang Betz 24:92cc9c6e4b2b 415
Wolfgang Betz 24:92cc9c6e4b2b 416 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 417 }
Wolfgang Betz 7:a2bb3d5e12e9 418
Wolfgang Betz 7:a2bb3d5e12e9 419 /**
Wolfgang Betz 7:a2bb3d5e12e9 420 * @brief Read Accelero Sensitivity
Wolfgang Betz 7:a2bb3d5e12e9 421 * @param pfData the pointer where the accelerometer sensitivity is stored
Wolfgang Betz 7:a2bb3d5e12e9 422 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 423 */
Wolfgang Betz 24:92cc9c6e4b2b 424 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetSensitivity( float *pfData )
Wolfgang Betz 7:a2bb3d5e12e9 425 {
Wolfgang Betz 24:92cc9c6e4b2b 426 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 24:92cc9c6e4b2b 427 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 428
Wolfgang Betz 24:92cc9c6e4b2b 429 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 430 {
Wolfgang Betz 24:92cc9c6e4b2b 431 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 432 }
Wolfgang Betz 24:92cc9c6e4b2b 433
Wolfgang Betz 24:92cc9c6e4b2b 434 tempReg &= LSM6DS0_XL_FS_MASK;
Wolfgang Betz 24:92cc9c6e4b2b 435
Wolfgang Betz 24:92cc9c6e4b2b 436 switch( tempReg )
Wolfgang Betz 24:92cc9c6e4b2b 437 {
Wolfgang Betz 24:92cc9c6e4b2b 438 case LSM6DS0_XL_FS_2G:
Wolfgang Betz 24:92cc9c6e4b2b 439 *pfData = 0.061f;
Wolfgang Betz 24:92cc9c6e4b2b 440 break;
Wolfgang Betz 24:92cc9c6e4b2b 441 case LSM6DS0_XL_FS_4G:
Wolfgang Betz 24:92cc9c6e4b2b 442 *pfData = 0.122f;
Wolfgang Betz 24:92cc9c6e4b2b 443 break;
Wolfgang Betz 24:92cc9c6e4b2b 444 case LSM6DS0_XL_FS_8G:
Wolfgang Betz 24:92cc9c6e4b2b 445 *pfData = 0.244f;
Wolfgang Betz 24:92cc9c6e4b2b 446 break;
Wolfgang Betz 24:92cc9c6e4b2b 447 case LSM6DS0_XL_FS_16G:
Wolfgang Betz 24:92cc9c6e4b2b 448 *pfData = 0.732f;
Wolfgang Betz 24:92cc9c6e4b2b 449 break;
Wolfgang Betz 24:92cc9c6e4b2b 450 default:
Wolfgang Betz 24:92cc9c6e4b2b 451 break;
Wolfgang Betz 24:92cc9c6e4b2b 452 }
Wolfgang Betz 24:92cc9c6e4b2b 453
Wolfgang Betz 24:92cc9c6e4b2b 454 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 455 }
Wolfgang Betz 24:92cc9c6e4b2b 456
Wolfgang Betz 24:92cc9c6e4b2b 457 /**
Wolfgang Betz 24:92cc9c6e4b2b 458 * @brief Read Accelero Full Scale
Wolfgang Betz 24:92cc9c6e4b2b 459 * @param fullScale the pointer where the accelerometer full scale is stored
Wolfgang Betz 24:92cc9c6e4b2b 460 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 461 */
Wolfgang Betz 24:92cc9c6e4b2b 462 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Get_FS( float *fullScale )
Wolfgang Betz 24:92cc9c6e4b2b 463 {
Wolfgang Betz 24:92cc9c6e4b2b 464 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 24:92cc9c6e4b2b 465 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 466
Wolfgang Betz 24:92cc9c6e4b2b 467 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 468 {
Wolfgang Betz 24:92cc9c6e4b2b 469 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 470 }
Wolfgang Betz 24:92cc9c6e4b2b 471
Wolfgang Betz 24:92cc9c6e4b2b 472 tempReg &= LSM6DS0_XL_FS_MASK;
Wolfgang Betz 24:92cc9c6e4b2b 473
Wolfgang Betz 24:92cc9c6e4b2b 474 switch( tempReg )
Wolfgang Betz 24:92cc9c6e4b2b 475 {
Wolfgang Betz 24:92cc9c6e4b2b 476 case LSM6DS0_XL_FS_2G:
Wolfgang Betz 24:92cc9c6e4b2b 477 *fullScale = 2.0f;
Wolfgang Betz 24:92cc9c6e4b2b 478 break;
Wolfgang Betz 24:92cc9c6e4b2b 479 case LSM6DS0_XL_FS_4G:
Wolfgang Betz 24:92cc9c6e4b2b 480 *fullScale = 4.0f;
Wolfgang Betz 24:92cc9c6e4b2b 481 break;
Wolfgang Betz 24:92cc9c6e4b2b 482 case LSM6DS0_XL_FS_8G:
Wolfgang Betz 24:92cc9c6e4b2b 483 *fullScale = 8.0f;
Wolfgang Betz 24:92cc9c6e4b2b 484 break;
Wolfgang Betz 24:92cc9c6e4b2b 485 case LSM6DS0_XL_FS_16G:
Wolfgang Betz 24:92cc9c6e4b2b 486 *fullScale = 16.0f;
Wolfgang Betz 24:92cc9c6e4b2b 487 break;
Wolfgang Betz 24:92cc9c6e4b2b 488 default:
Wolfgang Betz 24:92cc9c6e4b2b 489 break;
Wolfgang Betz 24:92cc9c6e4b2b 490 }
Wolfgang Betz 24:92cc9c6e4b2b 491
Wolfgang Betz 24:92cc9c6e4b2b 492 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 493 }
Wolfgang Betz 7:a2bb3d5e12e9 494
Wolfgang Betz 24:92cc9c6e4b2b 495 /**
Wolfgang Betz 24:92cc9c6e4b2b 496 * @brief Write Accelero Full Scale
Wolfgang Betz 24:92cc9c6e4b2b 497 * @param fullScale the accelerometer full scale to be set
Wolfgang Betz 24:92cc9c6e4b2b 498 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 499 */
Wolfgang Betz 24:92cc9c6e4b2b 500 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Set_FS( float fullScale )
Wolfgang Betz 24:92cc9c6e4b2b 501 {
Wolfgang Betz 24:92cc9c6e4b2b 502 uint8_t new_fs = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 503 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 504
Wolfgang Betz 24:92cc9c6e4b2b 505 new_fs = ( fullScale <= 2.0f ) ? LSM6DS0_XL_FS_2G
Wolfgang Betz 24:92cc9c6e4b2b 506 : ( fullScale <= 4.0f ) ? LSM6DS0_XL_FS_4G
Wolfgang Betz 24:92cc9c6e4b2b 507 : ( fullScale <= 8.0f ) ? LSM6DS0_XL_FS_8G
Wolfgang Betz 24:92cc9c6e4b2b 508 : LSM6DS0_XL_FS_16G;
Wolfgang Betz 24:92cc9c6e4b2b 509
Wolfgang Betz 24:92cc9c6e4b2b 510 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 511 {
Wolfgang Betz 24:92cc9c6e4b2b 512 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 513 }
Wolfgang Betz 24:92cc9c6e4b2b 514
Wolfgang Betz 24:92cc9c6e4b2b 515 tempReg &= ~(LSM6DS0_XL_FS_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 516 tempReg |= new_fs;
Wolfgang Betz 24:92cc9c6e4b2b 517
Wolfgang Betz 24:92cc9c6e4b2b 518 if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 519 {
Wolfgang Betz 24:92cc9c6e4b2b 520 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 521 }
Wolfgang Betz 24:92cc9c6e4b2b 522
Wolfgang Betz 24:92cc9c6e4b2b 523 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 524 }
Wolfgang Betz 7:a2bb3d5e12e9 525
Wolfgang Betz 24:92cc9c6e4b2b 526 /**
Wolfgang Betz 24:92cc9c6e4b2b 527 * @brief Read Gyro Output Data Rate
Wolfgang Betz 24:92cc9c6e4b2b 528 * @param odr the pointer where the gyroscope output data rate is stored
Wolfgang Betz 24:92cc9c6e4b2b 529 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 530 */
Wolfgang Betz 24:92cc9c6e4b2b 531 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Get_ODR( float *odr )
Wolfgang Betz 24:92cc9c6e4b2b 532 {
Wolfgang Betz 24:92cc9c6e4b2b 533 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 24:92cc9c6e4b2b 534 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 535
Wolfgang Betz 24:92cc9c6e4b2b 536 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 537 {
Wolfgang Betz 24:92cc9c6e4b2b 538 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 539 }
Wolfgang Betz 24:92cc9c6e4b2b 540
Wolfgang Betz 24:92cc9c6e4b2b 541 tempReg &= LSM6DS0_G_ODR_MASK;
Wolfgang Betz 24:92cc9c6e4b2b 542
Wolfgang Betz 24:92cc9c6e4b2b 543 switch( tempReg )
Wolfgang Betz 24:92cc9c6e4b2b 544 {
Wolfgang Betz 24:92cc9c6e4b2b 545 case LSM6DS0_G_ODR_PD:
Wolfgang Betz 24:92cc9c6e4b2b 546 *odr = 0.0f;
Wolfgang Betz 24:92cc9c6e4b2b 547 break;
Wolfgang Betz 24:92cc9c6e4b2b 548 case LSM6DS0_G_ODR_14_9HZ:
Wolfgang Betz 24:92cc9c6e4b2b 549 *odr = 14.9f;
Wolfgang Betz 24:92cc9c6e4b2b 550 break;
Wolfgang Betz 24:92cc9c6e4b2b 551 case LSM6DS0_G_ODR_59_5HZ:
Wolfgang Betz 24:92cc9c6e4b2b 552 *odr = 59.5f;
Wolfgang Betz 24:92cc9c6e4b2b 553 break;
Wolfgang Betz 24:92cc9c6e4b2b 554 case LSM6DS0_G_ODR_119HZ:
Wolfgang Betz 24:92cc9c6e4b2b 555 *odr = 119.0f;
Wolfgang Betz 24:92cc9c6e4b2b 556 break;
Wolfgang Betz 24:92cc9c6e4b2b 557 case LSM6DS0_G_ODR_238HZ:
Wolfgang Betz 24:92cc9c6e4b2b 558 *odr = 238.0f;
Wolfgang Betz 24:92cc9c6e4b2b 559 break;
Wolfgang Betz 24:92cc9c6e4b2b 560 case LSM6DS0_G_ODR_476HZ:
Wolfgang Betz 24:92cc9c6e4b2b 561 *odr = 476.0f;
Wolfgang Betz 24:92cc9c6e4b2b 562 break;
Wolfgang Betz 24:92cc9c6e4b2b 563 case LSM6DS0_G_ODR_952HZ:
Wolfgang Betz 24:92cc9c6e4b2b 564 *odr = 952.0f;
Wolfgang Betz 24:92cc9c6e4b2b 565 break;
Wolfgang Betz 24:92cc9c6e4b2b 566 default:
Wolfgang Betz 24:92cc9c6e4b2b 567 break;
Wolfgang Betz 24:92cc9c6e4b2b 568 }
Wolfgang Betz 24:92cc9c6e4b2b 569
Wolfgang Betz 24:92cc9c6e4b2b 570 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 571 }
Wolfgang Betz 24:92cc9c6e4b2b 572
Wolfgang Betz 24:92cc9c6e4b2b 573 /**
Wolfgang Betz 24:92cc9c6e4b2b 574 * @brief Write Gyro Output Data Rate
Wolfgang Betz 24:92cc9c6e4b2b 575 * @param odr the gyroscope output data rate to be set
Wolfgang Betz 24:92cc9c6e4b2b 576 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 577 */
Wolfgang Betz 24:92cc9c6e4b2b 578 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Set_ODR( float odr )
Wolfgang Betz 24:92cc9c6e4b2b 579 {
Wolfgang Betz 24:92cc9c6e4b2b 580 uint8_t new_odr = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 581 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 582
Wolfgang Betz 24:92cc9c6e4b2b 583 new_odr = ( odr <= 0.0f ) ? LSM6DS0_G_ODR_PD /* Power Down */
Wolfgang Betz 24:92cc9c6e4b2b 584 : ( odr <= 14.9f ) ? LSM6DS0_G_ODR_14_9HZ
Wolfgang Betz 24:92cc9c6e4b2b 585 : ( odr <= 59.5f ) ? LSM6DS0_G_ODR_59_5HZ
Wolfgang Betz 24:92cc9c6e4b2b 586 : ( odr <= 119.0f ) ? LSM6DS0_G_ODR_119HZ
Wolfgang Betz 24:92cc9c6e4b2b 587 : ( odr <= 238.0f ) ? LSM6DS0_G_ODR_238HZ
Wolfgang Betz 24:92cc9c6e4b2b 588 : ( odr <= 476.0f ) ? LSM6DS0_G_ODR_476HZ
Wolfgang Betz 24:92cc9c6e4b2b 589 : LSM6DS0_G_ODR_952HZ;
Wolfgang Betz 24:92cc9c6e4b2b 590
Wolfgang Betz 24:92cc9c6e4b2b 591 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 592 {
Wolfgang Betz 24:92cc9c6e4b2b 593 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 594 }
Wolfgang Betz 24:92cc9c6e4b2b 595
Wolfgang Betz 24:92cc9c6e4b2b 596 tempReg &= ~(LSM6DS0_G_ODR_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 597 tempReg |= new_odr;
Wolfgang Betz 24:92cc9c6e4b2b 598
Wolfgang Betz 24:92cc9c6e4b2b 599 if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 600 {
Wolfgang Betz 24:92cc9c6e4b2b 601 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 602 }
Wolfgang Betz 24:92cc9c6e4b2b 603
Wolfgang Betz 24:92cc9c6e4b2b 604 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 605 }
Wolfgang Betz 7:a2bb3d5e12e9 606
Wolfgang Betz 7:a2bb3d5e12e9 607 /**
Wolfgang Betz 7:a2bb3d5e12e9 608 * @brief Read Gyro Sensitivity
Wolfgang Betz 7:a2bb3d5e12e9 609 * @param pfData the pointer where the gyroscope sensitivity is stored
Wolfgang Betz 7:a2bb3d5e12e9 610 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 7:a2bb3d5e12e9 611 */
Wolfgang Betz 24:92cc9c6e4b2b 612 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetSensitivity( float *pfData )
Wolfgang Betz 24:92cc9c6e4b2b 613 {
Wolfgang Betz 24:92cc9c6e4b2b 614 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 24:92cc9c6e4b2b 615 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 616
Wolfgang Betz 24:92cc9c6e4b2b 617 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 618 {
Wolfgang Betz 24:92cc9c6e4b2b 619 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 620 }
Wolfgang Betz 24:92cc9c6e4b2b 621
Wolfgang Betz 24:92cc9c6e4b2b 622 tempReg &= LSM6DS0_G_FS_MASK;
Wolfgang Betz 24:92cc9c6e4b2b 623
Wolfgang Betz 24:92cc9c6e4b2b 624 switch( tempReg )
Wolfgang Betz 24:92cc9c6e4b2b 625 {
Wolfgang Betz 24:92cc9c6e4b2b 626 case LSM6DS0_G_FS_245:
Wolfgang Betz 24:92cc9c6e4b2b 627 *pfData = 8.75f;
Wolfgang Betz 24:92cc9c6e4b2b 628 break;
Wolfgang Betz 24:92cc9c6e4b2b 629 case LSM6DS0_G_FS_500:
Wolfgang Betz 24:92cc9c6e4b2b 630 *pfData = 17.50f;
Wolfgang Betz 24:92cc9c6e4b2b 631 break;
Wolfgang Betz 24:92cc9c6e4b2b 632 case LSM6DS0_G_FS_2000:
Wolfgang Betz 24:92cc9c6e4b2b 633 *pfData = 70.0f;
Wolfgang Betz 24:92cc9c6e4b2b 634 break;
Wolfgang Betz 24:92cc9c6e4b2b 635 default:
Wolfgang Betz 24:92cc9c6e4b2b 636 break;
Wolfgang Betz 24:92cc9c6e4b2b 637 }
Wolfgang Betz 24:92cc9c6e4b2b 638
Wolfgang Betz 24:92cc9c6e4b2b 639 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 640 }
Wolfgang Betz 24:92cc9c6e4b2b 641
Wolfgang Betz 24:92cc9c6e4b2b 642 /**
Wolfgang Betz 24:92cc9c6e4b2b 643 * @brief Read Gyro Full Scale
Wolfgang Betz 24:92cc9c6e4b2b 644 * @param fullScale the pointer where the gyroscope full scale is stored
Wolfgang Betz 24:92cc9c6e4b2b 645 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 646 */
Wolfgang Betz 24:92cc9c6e4b2b 647 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Get_FS( float *fullScale )
Wolfgang Betz 7:a2bb3d5e12e9 648 {
Wolfgang Betz 24:92cc9c6e4b2b 649 /*Here we have to add the check if the parameters are valid*/
Wolfgang Betz 24:92cc9c6e4b2b 650 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 651
Wolfgang Betz 24:92cc9c6e4b2b 652 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 653 {
Wolfgang Betz 24:92cc9c6e4b2b 654 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 655 }
Wolfgang Betz 24:92cc9c6e4b2b 656
Wolfgang Betz 24:92cc9c6e4b2b 657 tempReg &= LSM6DS0_G_FS_MASK;
Wolfgang Betz 24:92cc9c6e4b2b 658
Wolfgang Betz 24:92cc9c6e4b2b 659 switch( tempReg )
Wolfgang Betz 24:92cc9c6e4b2b 660 {
Wolfgang Betz 24:92cc9c6e4b2b 661 case LSM6DS0_G_FS_245:
Wolfgang Betz 24:92cc9c6e4b2b 662 *fullScale = 245.0f;
Wolfgang Betz 24:92cc9c6e4b2b 663 break;
Wolfgang Betz 24:92cc9c6e4b2b 664 case LSM6DS0_G_FS_500:
Wolfgang Betz 24:92cc9c6e4b2b 665 *fullScale = 500.0f;
Wolfgang Betz 24:92cc9c6e4b2b 666 break;
Wolfgang Betz 24:92cc9c6e4b2b 667 case LSM6DS0_G_FS_2000:
Wolfgang Betz 24:92cc9c6e4b2b 668 *fullScale = 2000.0f;
Wolfgang Betz 24:92cc9c6e4b2b 669 break;
Wolfgang Betz 24:92cc9c6e4b2b 670 default:
Wolfgang Betz 24:92cc9c6e4b2b 671 break;
Wolfgang Betz 24:92cc9c6e4b2b 672 }
Wolfgang Betz 24:92cc9c6e4b2b 673
Wolfgang Betz 24:92cc9c6e4b2b 674 return IMU_6AXES_OK;
Wolfgang Betz 7:a2bb3d5e12e9 675 }
Wolfgang Betz 24:92cc9c6e4b2b 676
Wolfgang Betz 24:92cc9c6e4b2b 677 /**
Wolfgang Betz 24:92cc9c6e4b2b 678 * @brief Write Gyro Full Scale
Wolfgang Betz 24:92cc9c6e4b2b 679 * @param fullScale the gyroscope full scale to be set
Wolfgang Betz 24:92cc9c6e4b2b 680 * @retval IMU_6AXES_OK in case of success, an error code otherwise
Wolfgang Betz 24:92cc9c6e4b2b 681 */
Wolfgang Betz 24:92cc9c6e4b2b 682 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Set_FS( float fullScale )
Wolfgang Betz 24:92cc9c6e4b2b 683 {
Wolfgang Betz 24:92cc9c6e4b2b 684 uint8_t new_fs = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 685 uint8_t tempReg = 0x00;
Wolfgang Betz 24:92cc9c6e4b2b 686
Wolfgang Betz 24:92cc9c6e4b2b 687 new_fs = ( fullScale <= 245.0f ) ? LSM6DS0_G_FS_245
Wolfgang Betz 24:92cc9c6e4b2b 688 : ( fullScale <= 500.0f ) ? LSM6DS0_G_FS_500
Wolfgang Betz 24:92cc9c6e4b2b 689 : LSM6DS0_G_FS_2000;
Wolfgang Betz 24:92cc9c6e4b2b 690
Wolfgang Betz 24:92cc9c6e4b2b 691 if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 692 {
Wolfgang Betz 24:92cc9c6e4b2b 693 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 694 }
Wolfgang Betz 24:92cc9c6e4b2b 695
Wolfgang Betz 24:92cc9c6e4b2b 696 tempReg &= ~(LSM6DS0_G_FS_MASK);
Wolfgang Betz 24:92cc9c6e4b2b 697 tempReg |= new_fs;
Wolfgang Betz 24:92cc9c6e4b2b 698
Wolfgang Betz 24:92cc9c6e4b2b 699 if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
Wolfgang Betz 24:92cc9c6e4b2b 700 {
Wolfgang Betz 24:92cc9c6e4b2b 701 return IMU_6AXES_ERROR;
Wolfgang Betz 24:92cc9c6e4b2b 702 }
Wolfgang Betz 24:92cc9c6e4b2b 703
Wolfgang Betz 24:92cc9c6e4b2b 704 return IMU_6AXES_OK;
Wolfgang Betz 24:92cc9c6e4b2b 705 }
Wolfgang Betz 24:92cc9c6e4b2b 706
Wolfgang Betz 24:92cc9c6e4b2b 707 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/