Jamie Satchell / Mbed OS JSatchell_SOFT253ReferredCoursework

Finished project.

Dependencies:   X_NUCLEO_COMMON

Fork of ReferredCoursework2016 by Stage-1 Students SoCEM

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