qiutao zhang
/
X_NUCLEO_IKS01A1
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.
Fork of
X_NUCLEO_IKS01A1
by 
ST
Components/lsm6ds0/lsm6ds0_class.cpp@7:a2bb3d5e12e9, 2015-04-15 (annotated)
- Committer:
- Wolfgang Betz
- Date:
- Wed Apr 15 14:08:20 2015 +0200
- Revision:
- 7:a2bb3d5e12e9
- Child:
- 24:92cc9c6e4b2b
Added component LSM6DS0
Who changed what in which revision?
| 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 "mbed.h" |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 40 | #include "lsm6ds0_class.h" |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 41 | #include "lsm6ds0.h" |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 42 | #include "../../x_nucleo_iks01a1_targets.h" |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 43 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 44 | /* Methods -------------------------------------------------------------------*/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 45 | /* betzw - based on: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 46 | X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds0/lsm6ds0.c: revision #184, |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 47 | X-CUBE-MEMS1/trunk: revision #293 |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 48 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 49 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 50 | * @brief Set LSM6DS0 Initialization |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 51 | * @param LSM6DS0_Init the configuration setting for the LSM6DS0 |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 52 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 53 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 54 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 55 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 56 | uint8_t tmp1 = 0x00; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 57 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 58 | /* Configure the low level interface ---------------------------------------*/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 59 | if(LSM6DS0_IO_Init() != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 60 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 61 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 62 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 63 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 64 | /******* Gyroscope init *******/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 65 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 66 | if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 67 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 68 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 69 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 70 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 71 | /* Output Data Rate selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 72 | tmp1 &= ~(LSM6DS0_G_ODR_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 73 | tmp1 |= LSM6DS0_Init->G_OutputDataRate; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 74 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 75 | /* Full scale selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 76 | tmp1 &= ~(LSM6DS0_G_FS_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 77 | tmp1 |= LSM6DS0_Init->G_FullScale; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 78 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 79 | if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 80 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 81 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 82 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 83 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 84 | if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 85 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 86 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 87 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 88 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 89 | /* Enable X axis selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 90 | tmp1 &= ~(LSM6DS0_G_XEN_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 91 | tmp1 |= LSM6DS0_Init->G_X_Axis; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 92 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 93 | /* Enable Y axis selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 94 | tmp1 &= ~(LSM6DS0_G_YEN_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 95 | tmp1 |= LSM6DS0_Init->G_Y_Axis; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 96 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 97 | /* Enable Z axis selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 98 | tmp1 &= ~(LSM6DS0_G_ZEN_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 99 | tmp1 |= LSM6DS0_Init->G_Z_Axis; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 100 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 101 | if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 102 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 103 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 104 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 105 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 106 | /******************************/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 107 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 108 | /***** Accelerometer init *****/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 109 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 110 | if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 111 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 112 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 113 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 114 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 115 | /* Output Data Rate selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 116 | tmp1 &= ~(LSM6DS0_XL_ODR_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 117 | tmp1 |= LSM6DS0_Init->X_OutputDataRate; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 118 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 119 | /* Full scale selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 120 | tmp1 &= ~(LSM6DS0_XL_FS_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 121 | tmp1 |= LSM6DS0_Init->X_FullScale; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 122 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 123 | if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 124 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 125 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 126 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 127 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 128 | if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 129 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 130 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 131 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 132 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 133 | /* Enable X axis selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 134 | tmp1 &= ~(LSM6DS0_XL_XEN_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 135 | tmp1 |= LSM6DS0_Init->X_X_Axis; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 136 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 137 | /* Enable Y axis selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 138 | tmp1 &= ~(LSM6DS0_XL_YEN_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 139 | tmp1 |= LSM6DS0_Init->X_Y_Axis; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 140 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 141 | /* Enable Z axis selection */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 142 | tmp1 &= ~(LSM6DS0_XL_ZEN_MASK); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 143 | tmp1 |= LSM6DS0_Init->X_Z_Axis; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 144 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 145 | if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 146 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 147 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 148 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 149 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 150 | /* Configure interrupt lines */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 151 | LSM6DS0_IO_ITConfig(); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 152 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 153 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 154 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 155 | /******************************/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 156 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 157 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 158 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 159 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 160 | * @brief Read ID of LSM6DS0 Accelerometer and Gyroscope |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 161 | * @param xg_id the pointer where the ID of the device is stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 162 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 163 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 164 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Read_XG_ID(uint8_t *xg_id) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 165 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 166 | if(!xg_id) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 167 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 168 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 169 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 170 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 171 | return LSM6DS0_IO_Read(xg_id, LSM6DS0_XG_WHO_AM_I_ADDR, 1); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 172 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 173 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 174 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 175 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 176 | * @brief Read raw data from LSM6DS0 Accelerometer output register |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 177 | * @param pData the pointer where the accelerometer raw data are stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 178 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 179 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 180 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxesRaw(int16_t *pData) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 181 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 182 | uint8_t tempReg[2] = {0,0}; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 183 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 184 | if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 185 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 186 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 187 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 188 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 189 | pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 190 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 191 | if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 192 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 193 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 194 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 195 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 196 | pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 197 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 198 | if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 199 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 200 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 201 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 202 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 203 | pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 204 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 205 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 206 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 207 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 208 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 209 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 210 | * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 211 | * @param pData the pointer where the accelerometer data are stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 212 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 213 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 214 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxes(int32_t *pData) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 215 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 216 | uint8_t tempReg = 0x00; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 217 | int16_t pDataRaw[3]; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 218 | float sensitivity = 0; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 219 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 220 | if(LSM6DS0_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 221 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 222 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 223 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 224 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 225 | if(LSM6DS0_IO_Read(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 226 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 227 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 228 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 229 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 230 | tempReg &= LSM6DS0_XL_FS_MASK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 231 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 232 | switch(tempReg) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 233 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 234 | case LSM6DS0_XL_FS_2G: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 235 | sensitivity = 0.061; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 236 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 237 | case LSM6DS0_XL_FS_4G: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 238 | sensitivity = 0.122; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 239 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 240 | case LSM6DS0_XL_FS_8G: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 241 | sensitivity = 0.244; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 242 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 243 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 244 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 245 | pData[0] = (int32_t)(pDataRaw[0] * sensitivity); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 246 | pData[1] = (int32_t)(pDataRaw[1] * sensitivity); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 247 | pData[2] = (int32_t)(pDataRaw[2] * sensitivity); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 248 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 249 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 250 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 251 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 252 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 253 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 254 | * @brief Read raw data from LSM6DS0 Gyroscope output register |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 255 | * @param pData the pointer where the gyroscope raw data are stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 256 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 257 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 258 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxesRaw(int16_t *pData) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 259 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 260 | uint8_t tempReg[2] = {0,0}; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 261 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 262 | if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 263 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 264 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 265 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 266 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 267 | pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 268 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 269 | if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 270 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 271 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 272 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 273 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 274 | pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 275 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 276 | if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD), 2) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 277 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 278 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 279 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 280 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 281 | pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 282 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 283 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 284 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 285 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 286 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 287 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 288 | * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 289 | * @param pData the pointer where the gyroscope data are stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 290 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 291 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 292 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxes(int32_t *pData) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 293 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 294 | uint8_t tempReg = 0x00; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 295 | int16_t pDataRaw[3]; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 296 | float sensitivity = 0; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 297 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 298 | if(LSM6DS0_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 299 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 300 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 301 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 302 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 303 | if(LSM6DS0_IO_Read(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 304 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 305 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 306 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 307 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 308 | tempReg &= LSM6DS0_G_FS_MASK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 309 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 310 | switch(tempReg) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 311 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 312 | case LSM6DS0_G_FS_245: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 313 | sensitivity = 8.75; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 314 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 315 | case LSM6DS0_G_FS_500: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 316 | sensitivity = 17.50; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 317 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 318 | case LSM6DS0_G_FS_2000: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 319 | sensitivity = 70; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 320 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 321 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 322 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 323 | pData[0] = (int32_t)(pDataRaw[0] * sensitivity); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 324 | pData[1] = (int32_t)(pDataRaw[1] * sensitivity); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 325 | pData[2] = (int32_t)(pDataRaw[2] * sensitivity); |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 326 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 327 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 328 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 329 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 330 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 331 | * @brief Read Accelero Sensitivity |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 332 | * @param pfData the pointer where the accelerometer sensitivity is stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 333 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 334 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 335 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetSensitivity( float *pfData ) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 336 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 337 | /*Here we have to add the check if the parameters are valid*/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 338 | uint8_t tempReg = 0x00; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 339 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 340 | if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 341 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 342 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 343 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 344 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 345 | tempReg &= LSM6DS0_XL_FS_MASK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 346 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 347 | switch( tempReg ) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 348 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 349 | case LSM6DS0_XL_FS_2G: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 350 | *pfData = 0.061; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 351 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 352 | case LSM6DS0_XL_FS_4G: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 353 | *pfData = 0.122; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 354 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 355 | case LSM6DS0_XL_FS_8G: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 356 | *pfData = 0.244; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 357 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 358 | default: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 359 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 360 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 361 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 362 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 363 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 364 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 365 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 366 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 367 | /** |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 368 | * @brief Read Gyro Sensitivity |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 369 | * @param pfData the pointer where the gyroscope sensitivity is stored |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 370 | * @retval IMU_6AXES_OK in case of success, an error code otherwise |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 371 | */ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 372 | IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetSensitivity( float *pfData ) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 373 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 374 | /*Here we have to add the check if the parameters are valid*/ |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 375 | uint8_t tempReg = 0x00; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 376 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 377 | if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 378 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 379 | return IMU_6AXES_ERROR; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 380 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 381 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 382 | tempReg &= LSM6DS0_G_FS_MASK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 383 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 384 | switch( tempReg ) |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 385 | { |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 386 | case LSM6DS0_G_FS_245: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 387 | *pfData = 8.75; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 388 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 389 | case LSM6DS0_G_FS_500: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 390 | *pfData = 17.50; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 391 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 392 | case LSM6DS0_G_FS_2000: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 393 | *pfData = 70; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 394 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 395 | default: |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 396 | break; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 397 | } |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 398 | |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 399 | return IMU_6AXES_OK; |
| Wolfgang Betz |
7:a2bb3d5e12e9 | 400 | } |