Robotique FIP / IMU_FIP_nucleo

class IMU nucleo

Dependents:   Coupe-Robotique-FIP-Main

Fork of IMU_FIP by Robotique FIP

Components/LSM6DS0/lsm6ds0.cpp@0:528e23a13fb7, 2015-04-09 (annotated)

Committer:
quentin9696
Date:
Thu Apr 09 17:08:14 2015 +0000
Revision:
0:528e23a13fb7
bug pointeur :(

Who changed what in which revision?

UserRevisionLine numberNew contents of line
quentin9696 0:528e23a13fb7 1 /**
quentin9696 0:528e23a13fb7 2 ******************************************************************************
quentin9696 0:528e23a13fb7 3 * @file x_cube_mems_lsm6ds0.h
quentin9696 0:528e23a13fb7 4 * @author AST / EST
quentin9696 0:528e23a13fb7 5 * @version V0.0.1
quentin9696 0:528e23a13fb7 6 * @date 9-December-2014
quentin9696 0:528e23a13fb7 7 * @brief Header file for component LSM6DS0
quentin9696 0:528e23a13fb7 8 ******************************************************************************
quentin9696 0:528e23a13fb7 9 * @attention
quentin9696 0:528e23a13fb7 10 *
quentin9696 0:528e23a13fb7 11 * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
quentin9696 0:528e23a13fb7 12 *
quentin9696 0:528e23a13fb7 13 * Redistribution and use in source and binary forms, with or without modification,
quentin9696 0:528e23a13fb7 14 * are permitted provided that the following conditions are met:
quentin9696 0:528e23a13fb7 15 * 1. Redistributions of source code must retain the above copyright notice,
quentin9696 0:528e23a13fb7 16 * this list of conditions and the following disclaimer.
quentin9696 0:528e23a13fb7 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
quentin9696 0:528e23a13fb7 18 * this list of conditions and the following disclaimer in the documentation
quentin9696 0:528e23a13fb7 19 * and/or other materials provided with the distribution.
quentin9696 0:528e23a13fb7 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
quentin9696 0:528e23a13fb7 21 * may be used to endorse or promote products derived from this software
quentin9696 0:528e23a13fb7 22 * without specific prior written permission.
quentin9696 0:528e23a13fb7 23 *
quentin9696 0:528e23a13fb7 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
quentin9696 0:528e23a13fb7 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
quentin9696 0:528e23a13fb7 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
quentin9696 0:528e23a13fb7 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
quentin9696 0:528e23a13fb7 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
quentin9696 0:528e23a13fb7 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
quentin9696 0:528e23a13fb7 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
quentin9696 0:528e23a13fb7 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
quentin9696 0:528e23a13fb7 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
quentin9696 0:528e23a13fb7 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
quentin9696 0:528e23a13fb7 34 *
quentin9696 0:528e23a13fb7 35 ******************************************************************************
quentin9696 0:528e23a13fb7 36 */
quentin9696 0:528e23a13fb7 37
quentin9696 0:528e23a13fb7 38 /* Includes ------------------------------------------------------------------*/
quentin9696 0:528e23a13fb7 39 #include "mbed.h"
quentin9696 0:528e23a13fb7 40 #include "lsm6ds0.h"
quentin9696 0:528e23a13fb7 41 #include "lsm6ds0_platform.h"
quentin9696 0:528e23a13fb7 42 #include <math.h>
quentin9696 0:528e23a13fb7 43
quentin9696 0:528e23a13fb7 44 /* Methods -------------------------------------------------------------------*/
quentin9696 0:528e23a13fb7 45
quentin9696 0:528e23a13fb7 46 /**
quentin9696 0:528e23a13fb7 47 * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps.
quentin9696 0:528e23a13fb7 48 * @param float *pfData
quentin9696 0:528e23a13fb7 49 * @retval None.
quentin9696 0:528e23a13fb7 50 */
quentin9696 0:528e23a13fb7 51 void LSM6DS0::Gyro_GetAxes(AxesRaw_TypeDef *pData)
quentin9696 0:528e23a13fb7 52 {
quentin9696 0:528e23a13fb7 53
quentin9696 0:528e23a13fb7 54 uint8_t tempReg = 0x00;
quentin9696 0:528e23a13fb7 55 int16_t pDataRaw[3];
quentin9696 0:528e23a13fb7 56 float sensitivity = 0;
quentin9696 0:528e23a13fb7 57 int ret;
quentin9696 0:528e23a13fb7 58
quentin9696 0:528e23a13fb7 59 LSM6DS0::Gyro_GetAxesRaw(pDataRaw);
quentin9696 0:528e23a13fb7 60
quentin9696 0:528e23a13fb7 61 //IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
quentin9696 0:528e23a13fb7 62 ret = dev_i2c.i2c_read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
quentin9696 0:528e23a13fb7 63
quentin9696 0:528e23a13fb7 64 if (ret == 0)
quentin9696 0:528e23a13fb7 65 {
quentin9696 0:528e23a13fb7 66 tempReg &= LSM6DS0_G_FS_MASK;
quentin9696 0:528e23a13fb7 67
quentin9696 0:528e23a13fb7 68 switch(tempReg)
quentin9696 0:528e23a13fb7 69 {
quentin9696 0:528e23a13fb7 70 case LSM6DS0_G_FS_245:
quentin9696 0:528e23a13fb7 71 sensitivity = 8.75;
quentin9696 0:528e23a13fb7 72 break;
quentin9696 0:528e23a13fb7 73 case LSM6DS0_G_FS_500:
quentin9696 0:528e23a13fb7 74 sensitivity = 17.50;
quentin9696 0:528e23a13fb7 75 break;
quentin9696 0:528e23a13fb7 76 case LSM6DS0_G_FS_2000:
quentin9696 0:528e23a13fb7 77 sensitivity = 70;
quentin9696 0:528e23a13fb7 78 break;
quentin9696 0:528e23a13fb7 79 }
quentin9696 0:528e23a13fb7 80 }
quentin9696 0:528e23a13fb7 81
quentin9696 0:528e23a13fb7 82 pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
quentin9696 0:528e23a13fb7 83 pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity);
quentin9696 0:528e23a13fb7 84 pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity);
quentin9696 0:528e23a13fb7 85 }
quentin9696 0:528e23a13fb7 86
quentin9696 0:528e23a13fb7 87
quentin9696 0:528e23a13fb7 88 /**
quentin9696 0:528e23a13fb7 89 * @brief Read raw data from LSM6DS0 Gyroscope output register.
quentin9696 0:528e23a13fb7 90 * @param float *pfData
quentin9696 0:528e23a13fb7 91 * @retval None.
quentin9696 0:528e23a13fb7 92 */
quentin9696 0:528e23a13fb7 93 void LSM6DS0::Gyro_GetAxesRaw(int16_t *pData)
quentin9696 0:528e23a13fb7 94 {
quentin9696 0:528e23a13fb7 95 uint8_t tempReg[2] = {0,0};
quentin9696 0:528e23a13fb7 96 int ret;
quentin9696 0:528e23a13fb7 97
quentin9696 0:528e23a13fb7 98 pData[0] = pData[1] = pData[2] = 0;
quentin9696 0:528e23a13fb7 99
quentin9696 0:528e23a13fb7 100 //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
quentin9696 0:528e23a13fb7 101 ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
quentin9696 0:528e23a13fb7 102
quentin9696 0:528e23a13fb7 103 if (ret == 0)
quentin9696 0:528e23a13fb7 104 {
quentin9696 0:528e23a13fb7 105 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 106
quentin9696 0:528e23a13fb7 107 //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
quentin9696 0:528e23a13fb7 108 ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
quentin9696 0:528e23a13fb7 109 }
quentin9696 0:528e23a13fb7 110
quentin9696 0:528e23a13fb7 111 if (ret == 0)
quentin9696 0:528e23a13fb7 112 {
quentin9696 0:528e23a13fb7 113 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 114
quentin9696 0:528e23a13fb7 115 //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
quentin9696 0:528e23a13fb7 116 ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
quentin9696 0:528e23a13fb7 117 }
quentin9696 0:528e23a13fb7 118
quentin9696 0:528e23a13fb7 119 if (ret == 0)
quentin9696 0:528e23a13fb7 120 {
quentin9696 0:528e23a13fb7 121 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 122 }
quentin9696 0:528e23a13fb7 123 }
quentin9696 0:528e23a13fb7 124
quentin9696 0:528e23a13fb7 125
quentin9696 0:528e23a13fb7 126 /**
quentin9696 0:528e23a13fb7 127 * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg.
quentin9696 0:528e23a13fb7 128 * @param float *pfData
quentin9696 0:528e23a13fb7 129 * @retval None.
quentin9696 0:528e23a13fb7 130 */
quentin9696 0:528e23a13fb7 131 void LSM6DS0::Acc_GetAxes(AxesRaw_TypeDef *pData)
quentin9696 0:528e23a13fb7 132 {
quentin9696 0:528e23a13fb7 133 uint8_t tempReg = 0x00;
quentin9696 0:528e23a13fb7 134 int16_t pDataRaw[3];
quentin9696 0:528e23a13fb7 135 float sensitivity = 0;
quentin9696 0:528e23a13fb7 136 int ret;
quentin9696 0:528e23a13fb7 137
quentin9696 0:528e23a13fb7 138 Acc_GetAxesRaw(pDataRaw);
quentin9696 0:528e23a13fb7 139
quentin9696 0:528e23a13fb7 140 //IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
quentin9696 0:528e23a13fb7 141 ret = dev_i2c.i2c_read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
quentin9696 0:528e23a13fb7 142
quentin9696 0:528e23a13fb7 143 if (ret == 0)
quentin9696 0:528e23a13fb7 144 {
quentin9696 0:528e23a13fb7 145 tempReg &= LSM6DS0_XL_FS_MASK;
quentin9696 0:528e23a13fb7 146
quentin9696 0:528e23a13fb7 147 switch(tempReg)
quentin9696 0:528e23a13fb7 148 {
quentin9696 0:528e23a13fb7 149 case LSM6DS0_XL_FS_2G:
quentin9696 0:528e23a13fb7 150 sensitivity = 0.061;
quentin9696 0:528e23a13fb7 151 break;
quentin9696 0:528e23a13fb7 152 case LSM6DS0_XL_FS_4G:
quentin9696 0:528e23a13fb7 153 sensitivity = 0.122;
quentin9696 0:528e23a13fb7 154 break;
quentin9696 0:528e23a13fb7 155 case LSM6DS0_XL_FS_8G:
quentin9696 0:528e23a13fb7 156 sensitivity = 0.244;
quentin9696 0:528e23a13fb7 157 break;
quentin9696 0:528e23a13fb7 158 }
quentin9696 0:528e23a13fb7 159 }
quentin9696 0:528e23a13fb7 160
quentin9696 0:528e23a13fb7 161 pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
quentin9696 0:528e23a13fb7 162 pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity);
quentin9696 0:528e23a13fb7 163 pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity);
quentin9696 0:528e23a13fb7 164
quentin9696 0:528e23a13fb7 165 }
quentin9696 0:528e23a13fb7 166
quentin9696 0:528e23a13fb7 167 /**
quentin9696 0:528e23a13fb7 168 * @brief Read raw data from LSM6DS0 Accelerometer output register.
quentin9696 0:528e23a13fb7 169 * @param float *pfData
quentin9696 0:528e23a13fb7 170 * @retval None.
quentin9696 0:528e23a13fb7 171 */
quentin9696 0:528e23a13fb7 172 void LSM6DS0::Acc_GetAxesRaw(int16_t *pData)
quentin9696 0:528e23a13fb7 173 {
quentin9696 0:528e23a13fb7 174 uint8_t tempReg[2] = {0,0};
quentin9696 0:528e23a13fb7 175 int ret;
quentin9696 0:528e23a13fb7 176
quentin9696 0:528e23a13fb7 177 pData[0] = pData[1] = pData[2] = 0;
quentin9696 0:528e23a13fb7 178
quentin9696 0:528e23a13fb7 179 //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
quentin9696 0:528e23a13fb7 180 ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
quentin9696 0:528e23a13fb7 181
quentin9696 0:528e23a13fb7 182 if (ret == 0)
quentin9696 0:528e23a13fb7 183 {
quentin9696 0:528e23a13fb7 184 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 185
quentin9696 0:528e23a13fb7 186 //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
quentin9696 0:528e23a13fb7 187 ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
quentin9696 0:528e23a13fb7 188 }
quentin9696 0:528e23a13fb7 189
quentin9696 0:528e23a13fb7 190 if (ret == 0)
quentin9696 0:528e23a13fb7 191 {
quentin9696 0:528e23a13fb7 192 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 193
quentin9696 0:528e23a13fb7 194 //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
quentin9696 0:528e23a13fb7 195 ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
quentin9696 0:528e23a13fb7 196 }
quentin9696 0:528e23a13fb7 197
quentin9696 0:528e23a13fb7 198 if (ret == 0)
quentin9696 0:528e23a13fb7 199 {
quentin9696 0:528e23a13fb7 200 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 201 }
quentin9696 0:528e23a13fb7 202 }
quentin9696 0:528e23a13fb7 203
quentin9696 0:528e23a13fb7 204 /**
quentin9696 0:528e23a13fb7 205 * @brief Read ID address of HTS221
quentin9696 0:528e23a13fb7 206 * @param Device ID address
quentin9696 0:528e23a13fb7 207 * @retval ID name
quentin9696 0:528e23a13fb7 208 */
quentin9696 0:528e23a13fb7 209 uint8_t LSM6DS0::ReadID(void)
quentin9696 0:528e23a13fb7 210 {
quentin9696 0:528e23a13fb7 211 uint8_t tmp=0x00;
quentin9696 0:528e23a13fb7 212 int ret;
quentin9696 0:528e23a13fb7 213
quentin9696 0:528e23a13fb7 214 /* Read WHO I AM register */
quentin9696 0:528e23a13fb7 215 //IMU_6AXES_IO_Read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
quentin9696 0:528e23a13fb7 216 ret = dev_i2c.i2c_read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
quentin9696 0:528e23a13fb7 217
quentin9696 0:528e23a13fb7 218 /* Return the ID */
quentin9696 0:528e23a13fb7 219 return ((ret == 0) ? (uint8_t)tmp : 0);
quentin9696 0:528e23a13fb7 220 }
quentin9696 0:528e23a13fb7 221
quentin9696 0:528e23a13fb7 222 /**
quentin9696 0:528e23a13fb7 223 * @brief Set LSM6DS0 Initialization.
quentin9696 0:528e23a13fb7 224 * @param InitStruct: it contains the configuration setting for the LSM6DS0.
quentin9696 0:528e23a13fb7 225 * @retval None
quentin9696 0:528e23a13fb7 226 */
quentin9696 0:528e23a13fb7 227 void LSM6DS0::Init() {
quentin9696 0:528e23a13fb7 228
quentin9696 0:528e23a13fb7 229 uint8_t tmp1 = 0x00;
quentin9696 0:528e23a13fb7 230 int ret;
quentin9696 0:528e23a13fb7 231
quentin9696 0:528e23a13fb7 232 /******* Gyroscope init *******/
quentin9696 0:528e23a13fb7 233
quentin9696 0:528e23a13fb7 234 //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
quentin9696 0:528e23a13fb7 235 ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
quentin9696 0:528e23a13fb7 236
quentin9696 0:528e23a13fb7 237 if (ret == 0)
quentin9696 0:528e23a13fb7 238 {
quentin9696 0:528e23a13fb7 239 /* Output Data Rate selection */
quentin9696 0:528e23a13fb7 240 tmp1 &= ~(LSM6DS0_G_ODR_MASK);
quentin9696 0:528e23a13fb7 241 tmp1 |= LSM6DS0_G_ODR_119HZ;
quentin9696 0:528e23a13fb7 242
quentin9696 0:528e23a13fb7 243 /* Full scale selection */
quentin9696 0:528e23a13fb7 244 tmp1 &= ~(LSM6DS0_G_FS_MASK);
quentin9696 0:528e23a13fb7 245 tmp1 |= LSM6DS0_G_FS_2000;
quentin9696 0:528e23a13fb7 246
quentin9696 0:528e23a13fb7 247 //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
quentin9696 0:528e23a13fb7 248 ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
quentin9696 0:528e23a13fb7 249 }
quentin9696 0:528e23a13fb7 250
quentin9696 0:528e23a13fb7 251 if (ret == 0)
quentin9696 0:528e23a13fb7 252 {
quentin9696 0:528e23a13fb7 253 //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
quentin9696 0:528e23a13fb7 254 ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
quentin9696 0:528e23a13fb7 255 }
quentin9696 0:528e23a13fb7 256
quentin9696 0:528e23a13fb7 257 if (ret == 0)
quentin9696 0:528e23a13fb7 258 {
quentin9696 0:528e23a13fb7 259 /* Enable X axis selection */
quentin9696 0:528e23a13fb7 260 tmp1 &= ~(LSM6DS0_G_XEN_MASK);
quentin9696 0:528e23a13fb7 261 tmp1 |= LSM6DS0_G_XEN_ENABLE;
quentin9696 0:528e23a13fb7 262
quentin9696 0:528e23a13fb7 263 /* Enable Y axis selection */
quentin9696 0:528e23a13fb7 264 tmp1 &= ~(LSM6DS0_G_YEN_MASK);
quentin9696 0:528e23a13fb7 265 tmp1 |= LSM6DS0_G_YEN_ENABLE;
quentin9696 0:528e23a13fb7 266
quentin9696 0:528e23a13fb7 267 /* Enable Z axis selection */
quentin9696 0:528e23a13fb7 268 tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
quentin9696 0:528e23a13fb7 269 tmp1 |= LSM6DS0_G_ZEN_ENABLE;
quentin9696 0:528e23a13fb7 270
quentin9696 0:528e23a13fb7 271 //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
quentin9696 0:528e23a13fb7 272 ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
quentin9696 0:528e23a13fb7 273 }
quentin9696 0:528e23a13fb7 274
quentin9696 0:528e23a13fb7 275 /******************************/
quentin9696 0:528e23a13fb7 276 /***** Accelerometer init *****/
quentin9696 0:528e23a13fb7 277 if (ret == 0)
quentin9696 0:528e23a13fb7 278 {
quentin9696 0:528e23a13fb7 279 //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
quentin9696 0:528e23a13fb7 280 ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
quentin9696 0:528e23a13fb7 281 }
quentin9696 0:528e23a13fb7 282
quentin9696 0:528e23a13fb7 283 if (ret == 0)
quentin9696 0:528e23a13fb7 284 {
quentin9696 0:528e23a13fb7 285 /* Output Data Rate selection */
quentin9696 0:528e23a13fb7 286 tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
quentin9696 0:528e23a13fb7 287 tmp1 |= LSM6DS0_XL_ODR_119HZ;
quentin9696 0:528e23a13fb7 288
quentin9696 0:528e23a13fb7 289 /* Full scale selection */
quentin9696 0:528e23a13fb7 290 tmp1 &= ~(LSM6DS0_XL_FS_MASK);
quentin9696 0:528e23a13fb7 291 tmp1 |= LSM6DS0_XL_FS_2G;
quentin9696 0:528e23a13fb7 292
quentin9696 0:528e23a13fb7 293 //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
quentin9696 0:528e23a13fb7 294 ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
quentin9696 0:528e23a13fb7 295 }
quentin9696 0:528e23a13fb7 296
quentin9696 0:528e23a13fb7 297 if (ret == 0)
quentin9696 0:528e23a13fb7 298 {
quentin9696 0:528e23a13fb7 299 //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
quentin9696 0:528e23a13fb7 300 ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
quentin9696 0:528e23a13fb7 301 }
quentin9696 0:528e23a13fb7 302
quentin9696 0:528e23a13fb7 303 if (ret == 0)
quentin9696 0:528e23a13fb7 304 {
quentin9696 0:528e23a13fb7 305 /* Enable X axis selection */
quentin9696 0:528e23a13fb7 306 tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
quentin9696 0:528e23a13fb7 307 tmp1 |= LSM6DS0_XL_XEN_ENABLE;
quentin9696 0:528e23a13fb7 308
quentin9696 0:528e23a13fb7 309 /* Enable Y axis selection */
quentin9696 0:528e23a13fb7 310 tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
quentin9696 0:528e23a13fb7 311 tmp1 |= LSM6DS0_XL_YEN_ENABLE;
quentin9696 0:528e23a13fb7 312
quentin9696 0:528e23a13fb7 313 /* Enable Z axis selection */
quentin9696 0:528e23a13fb7 314 tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
quentin9696 0:528e23a13fb7 315 tmp1 |= LSM6DS0_XL_ZEN_ENABLE;
quentin9696 0:528e23a13fb7 316
quentin9696 0:528e23a13fb7 317 //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
quentin9696 0:528e23a13fb7 318 ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
quentin9696 0:528e23a13fb7 319 }
quentin9696 0:528e23a13fb7 320 /******************************/
quentin9696 0:528e23a13fb7 321
quentin9696 0:528e23a13fb7 322 if (ret == 0)
quentin9696 0:528e23a13fb7 323 {
quentin9696 0:528e23a13fb7 324 if(ReadID() == I_AM_LSM6DS0_XG)
quentin9696 0:528e23a13fb7 325 {
quentin9696 0:528e23a13fb7 326 LSM6DS0Initialized = 1;
quentin9696 0:528e23a13fb7 327 //ret = HUM_TEMP_OK;
quentin9696 0:528e23a13fb7 328 }
quentin9696 0:528e23a13fb7 329 }
quentin9696 0:528e23a13fb7 330
quentin9696 0:528e23a13fb7 331 return;
quentin9696 0:528e23a13fb7 332 }