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