removed X-NUCLEO-COMMON and ST-INTERFACES
Dependents: unh-hackathon-example unh-hackathon-example-raw
Fork of X_NUCLEO_IKS01A1 by
Components/lsm6ds0/lsm6ds0_class.cpp@93:c7bd4f2e378c, 2017-04-08 (annotated)
- Committer:
- mfiore
- Date:
- Sat Apr 08 03:55:37 2017 +0000
- Revision:
- 93:c7bd4f2e378c
- Parent:
- 57:04563dd74269
remove sub libraries
Who changed what in which revision?
User | Revision | Line number | New 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>© 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****/ |