Andrew Lindsay / BSP_DISCO_F429ZI

Additional font macro

Dependents:   EspSerial

Components/l3gd20/l3gd20.c@0:e1d164542663, 2015-12-17 (annotated)

Committer:
bcostm
Date:
Thu Dec 17 10:27:15 2015 +0000
Revision:
0:e1d164542663
Initial release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:e1d164542663 1 /**
bcostm 0:e1d164542663 2 ******************************************************************************
bcostm 0:e1d164542663 3 * @file l3gd20.c
bcostm 0:e1d164542663 4 * @author MCD Application Team
bcostm 0:e1d164542663 5 * @version V2.0.0
bcostm 0:e1d164542663 6 * @date 26-June-2015
bcostm 0:e1d164542663 7 * @brief This file provides a set of functions needed to manage the L3GD20,
bcostm 0:e1d164542663 8 * ST MEMS motion sensor, 3-axis digital output gyroscope.
bcostm 0:e1d164542663 9 ******************************************************************************
bcostm 0:e1d164542663 10 * @attention
bcostm 0:e1d164542663 11 *
bcostm 0:e1d164542663 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
bcostm 0:e1d164542663 13 *
bcostm 0:e1d164542663 14 * Redistribution and use in source and binary forms, with or without modification,
bcostm 0:e1d164542663 15 * are permitted provided that the following conditions are met:
bcostm 0:e1d164542663 16 * 1. Redistributions of source code must retain the above copyright notice,
bcostm 0:e1d164542663 17 * this list of conditions and the following disclaimer.
bcostm 0:e1d164542663 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
bcostm 0:e1d164542663 19 * this list of conditions and the following disclaimer in the documentation
bcostm 0:e1d164542663 20 * and/or other materials provided with the distribution.
bcostm 0:e1d164542663 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
bcostm 0:e1d164542663 22 * may be used to endorse or promote products derived from this software
bcostm 0:e1d164542663 23 * without specific prior written permission.
bcostm 0:e1d164542663 24 *
bcostm 0:e1d164542663 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
bcostm 0:e1d164542663 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
bcostm 0:e1d164542663 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
bcostm 0:e1d164542663 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
bcostm 0:e1d164542663 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
bcostm 0:e1d164542663 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
bcostm 0:e1d164542663 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
bcostm 0:e1d164542663 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
bcostm 0:e1d164542663 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
bcostm 0:e1d164542663 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
bcostm 0:e1d164542663 35 *
bcostm 0:e1d164542663 36 ******************************************************************************
bcostm 0:e1d164542663 37 */
bcostm 0:e1d164542663 38 /* Includes ------------------------------------------------------------------*/
bcostm 0:e1d164542663 39 #include "l3gd20.h"
bcostm 0:e1d164542663 40
bcostm 0:e1d164542663 41 /** @addtogroup BSP
bcostm 0:e1d164542663 42 * @{
bcostm 0:e1d164542663 43 */
bcostm 0:e1d164542663 44
bcostm 0:e1d164542663 45 /** @addtogroup Components
bcostm 0:e1d164542663 46 * @{
bcostm 0:e1d164542663 47 */
bcostm 0:e1d164542663 48
bcostm 0:e1d164542663 49 /** @addtogroup L3GD20
bcostm 0:e1d164542663 50 * @{
bcostm 0:e1d164542663 51 */
bcostm 0:e1d164542663 52
bcostm 0:e1d164542663 53 /** @defgroup L3GD20_Private_TypesDefinitions
bcostm 0:e1d164542663 54 * @{
bcostm 0:e1d164542663 55 */
bcostm 0:e1d164542663 56
bcostm 0:e1d164542663 57 /**
bcostm 0:e1d164542663 58 * @}
bcostm 0:e1d164542663 59 */
bcostm 0:e1d164542663 60
bcostm 0:e1d164542663 61 /** @defgroup L3GD20_Private_Defines
bcostm 0:e1d164542663 62 * @{
bcostm 0:e1d164542663 63 */
bcostm 0:e1d164542663 64
bcostm 0:e1d164542663 65 /**
bcostm 0:e1d164542663 66 * @}
bcostm 0:e1d164542663 67 */
bcostm 0:e1d164542663 68
bcostm 0:e1d164542663 69 /** @defgroup L3GD20_Private_Macros
bcostm 0:e1d164542663 70 * @{
bcostm 0:e1d164542663 71 */
bcostm 0:e1d164542663 72
bcostm 0:e1d164542663 73 /**
bcostm 0:e1d164542663 74 * @}
bcostm 0:e1d164542663 75 */
bcostm 0:e1d164542663 76
bcostm 0:e1d164542663 77 /** @defgroup L3GD20_Private_Variables
bcostm 0:e1d164542663 78 * @{
bcostm 0:e1d164542663 79 */
bcostm 0:e1d164542663 80 GYRO_DrvTypeDef L3gd20Drv =
bcostm 0:e1d164542663 81 {
bcostm 0:e1d164542663 82 L3GD20_Init,
bcostm 0:e1d164542663 83 L3GD20_DeInit,
bcostm 0:e1d164542663 84 L3GD20_ReadID,
bcostm 0:e1d164542663 85 L3GD20_RebootCmd,
bcostm 0:e1d164542663 86 L3GD20_LowPower,
bcostm 0:e1d164542663 87 L3GD20_INT1InterruptConfig,
bcostm 0:e1d164542663 88 L3GD20_EnableIT,
bcostm 0:e1d164542663 89 L3GD20_DisableIT,
bcostm 0:e1d164542663 90 0,
bcostm 0:e1d164542663 91 0,
bcostm 0:e1d164542663 92 L3GD20_FilterConfig,
bcostm 0:e1d164542663 93 L3GD20_FilterCmd,
bcostm 0:e1d164542663 94 L3GD20_ReadXYZAngRate
bcostm 0:e1d164542663 95 };
bcostm 0:e1d164542663 96
bcostm 0:e1d164542663 97 /**
bcostm 0:e1d164542663 98 * @}
bcostm 0:e1d164542663 99 */
bcostm 0:e1d164542663 100
bcostm 0:e1d164542663 101 /** @defgroup L3GD20_Private_FunctionPrototypes
bcostm 0:e1d164542663 102 * @{
bcostm 0:e1d164542663 103 */
bcostm 0:e1d164542663 104
bcostm 0:e1d164542663 105 /**
bcostm 0:e1d164542663 106 * @}
bcostm 0:e1d164542663 107 */
bcostm 0:e1d164542663 108
bcostm 0:e1d164542663 109 /** @defgroup L3GD20_Private_Functions
bcostm 0:e1d164542663 110 * @{
bcostm 0:e1d164542663 111 */
bcostm 0:e1d164542663 112
bcostm 0:e1d164542663 113 /**
bcostm 0:e1d164542663 114 * @brief Set L3GD20 Initialization.
bcostm 0:e1d164542663 115 * @param L3GD20_InitStruct: pointer to a L3GD20_InitTypeDef structure
bcostm 0:e1d164542663 116 * that contains the configuration setting for the L3GD20.
bcostm 0:e1d164542663 117 * @retval None
bcostm 0:e1d164542663 118 */
bcostm 0:e1d164542663 119 void L3GD20_Init(uint16_t InitStruct)
bcostm 0:e1d164542663 120 {
bcostm 0:e1d164542663 121 uint8_t ctrl = 0x00;
bcostm 0:e1d164542663 122
bcostm 0:e1d164542663 123 /* Configure the low level interface */
bcostm 0:e1d164542663 124 GYRO_IO_Init();
bcostm 0:e1d164542663 125
bcostm 0:e1d164542663 126 /* Write value to MEMS CTRL_REG1 register */
bcostm 0:e1d164542663 127 ctrl = (uint8_t) InitStruct;
bcostm 0:e1d164542663 128 GYRO_IO_Write(&ctrl, L3GD20_CTRL_REG1_ADDR, 1);
bcostm 0:e1d164542663 129
bcostm 0:e1d164542663 130 /* Write value to MEMS CTRL_REG4 register */
bcostm 0:e1d164542663 131 ctrl = (uint8_t) (InitStruct >> 8);
bcostm 0:e1d164542663 132 GYRO_IO_Write(&ctrl, L3GD20_CTRL_REG4_ADDR, 1);
bcostm 0:e1d164542663 133 }
bcostm 0:e1d164542663 134
bcostm 0:e1d164542663 135
bcostm 0:e1d164542663 136
bcostm 0:e1d164542663 137 /**
bcostm 0:e1d164542663 138 * @brief L3GD20 De-initialization
bcostm 0:e1d164542663 139 * @param None
bcostm 0:e1d164542663 140 * @retval None
bcostm 0:e1d164542663 141 */
bcostm 0:e1d164542663 142 void L3GD20_DeInit(void)
bcostm 0:e1d164542663 143 {
bcostm 0:e1d164542663 144 }
bcostm 0:e1d164542663 145
bcostm 0:e1d164542663 146 /**
bcostm 0:e1d164542663 147 * @brief Read ID address of L3GD20
bcostm 0:e1d164542663 148 * @param None
bcostm 0:e1d164542663 149 * @retval ID name
bcostm 0:e1d164542663 150 */
bcostm 0:e1d164542663 151 uint8_t L3GD20_ReadID(void)
bcostm 0:e1d164542663 152 {
bcostm 0:e1d164542663 153 uint8_t tmp;
bcostm 0:e1d164542663 154
bcostm 0:e1d164542663 155 /* Configure the low level interface */
bcostm 0:e1d164542663 156 GYRO_IO_Init();
bcostm 0:e1d164542663 157
bcostm 0:e1d164542663 158 /* Read WHO I AM register */
bcostm 0:e1d164542663 159 GYRO_IO_Read(&tmp, L3GD20_WHO_AM_I_ADDR, 1);
bcostm 0:e1d164542663 160
bcostm 0:e1d164542663 161 /* Return the ID */
bcostm 0:e1d164542663 162 return (uint8_t)tmp;
bcostm 0:e1d164542663 163 }
bcostm 0:e1d164542663 164
bcostm 0:e1d164542663 165 /**
bcostm 0:e1d164542663 166 * @brief Reboot memory content of L3GD20
bcostm 0:e1d164542663 167 * @param None
bcostm 0:e1d164542663 168 * @retval None
bcostm 0:e1d164542663 169 */
bcostm 0:e1d164542663 170 void L3GD20_RebootCmd(void)
bcostm 0:e1d164542663 171 {
bcostm 0:e1d164542663 172 uint8_t tmpreg;
bcostm 0:e1d164542663 173
bcostm 0:e1d164542663 174 /* Read CTRL_REG5 register */
bcostm 0:e1d164542663 175 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
bcostm 0:e1d164542663 176
bcostm 0:e1d164542663 177 /* Enable or Disable the reboot memory */
bcostm 0:e1d164542663 178 tmpreg |= L3GD20_BOOT_REBOOTMEMORY;
bcostm 0:e1d164542663 179
bcostm 0:e1d164542663 180 /* Write value to MEMS CTRL_REG5 register */
bcostm 0:e1d164542663 181 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
bcostm 0:e1d164542663 182 }
bcostm 0:e1d164542663 183
bcostm 0:e1d164542663 184 /**
bcostm 0:e1d164542663 185 * @brief Set L3GD20 in low-power mode
bcostm 0:e1d164542663 186 * @param
bcostm 0:e1d164542663 187 * @retval None
bcostm 0:e1d164542663 188 */
bcostm 0:e1d164542663 189 void L3GD20_LowPower(uint16_t InitStruct)
bcostm 0:e1d164542663 190 {
bcostm 0:e1d164542663 191 uint8_t ctrl = 0x00;
bcostm 0:e1d164542663 192
bcostm 0:e1d164542663 193 /* Write value to MEMS CTRL_REG1 register */
bcostm 0:e1d164542663 194 ctrl = (uint8_t) InitStruct;
bcostm 0:e1d164542663 195 GYRO_IO_Write(&ctrl, L3GD20_CTRL_REG1_ADDR, 1);
bcostm 0:e1d164542663 196 }
bcostm 0:e1d164542663 197
bcostm 0:e1d164542663 198 /**
bcostm 0:e1d164542663 199 * @brief Set L3GD20 Interrupt INT1 configuration
bcostm 0:e1d164542663 200 * @param Int1Config: the configuration setting for the L3GD20 Interrupt.
bcostm 0:e1d164542663 201 * @retval None
bcostm 0:e1d164542663 202 */
bcostm 0:e1d164542663 203 void L3GD20_INT1InterruptConfig(uint16_t Int1Config)
bcostm 0:e1d164542663 204 {
bcostm 0:e1d164542663 205 uint8_t ctrl_cfr = 0x00, ctrl3 = 0x00;
bcostm 0:e1d164542663 206
bcostm 0:e1d164542663 207 /* Read INT1_CFG register */
bcostm 0:e1d164542663 208 GYRO_IO_Read(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1);
bcostm 0:e1d164542663 209
bcostm 0:e1d164542663 210 /* Read CTRL_REG3 register */
bcostm 0:e1d164542663 211 GYRO_IO_Read(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1);
bcostm 0:e1d164542663 212
bcostm 0:e1d164542663 213 ctrl_cfr &= 0x80;
bcostm 0:e1d164542663 214 ctrl_cfr |= ((uint8_t) Int1Config >> 8);
bcostm 0:e1d164542663 215
bcostm 0:e1d164542663 216 ctrl3 &= 0xDF;
bcostm 0:e1d164542663 217 ctrl3 |= ((uint8_t) Int1Config);
bcostm 0:e1d164542663 218
bcostm 0:e1d164542663 219 /* Write value to MEMS INT1_CFG register */
bcostm 0:e1d164542663 220 GYRO_IO_Write(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1);
bcostm 0:e1d164542663 221
bcostm 0:e1d164542663 222 /* Write value to MEMS CTRL_REG3 register */
bcostm 0:e1d164542663 223 GYRO_IO_Write(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1);
bcostm 0:e1d164542663 224 }
bcostm 0:e1d164542663 225
bcostm 0:e1d164542663 226 /**
bcostm 0:e1d164542663 227 * @brief Enable INT1 or INT2 interrupt
bcostm 0:e1d164542663 228 * @param IntSel: choice of INT1 or INT2
bcostm 0:e1d164542663 229 * This parameter can be:
bcostm 0:e1d164542663 230 * @arg L3GD20_INT1
bcostm 0:e1d164542663 231 * @arg L3GD20_INT2
bcostm 0:e1d164542663 232 * @retval None
bcostm 0:e1d164542663 233 */
bcostm 0:e1d164542663 234 void L3GD20_EnableIT(uint8_t IntSel)
bcostm 0:e1d164542663 235 {
bcostm 0:e1d164542663 236 uint8_t tmpreg;
bcostm 0:e1d164542663 237
bcostm 0:e1d164542663 238 /* Read CTRL_REG3 register */
bcostm 0:e1d164542663 239 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1);
bcostm 0:e1d164542663 240
bcostm 0:e1d164542663 241 if(IntSel == L3GD20_INT1)
bcostm 0:e1d164542663 242 {
bcostm 0:e1d164542663 243 tmpreg &= 0x7F;
bcostm 0:e1d164542663 244 tmpreg |= L3GD20_INT1INTERRUPT_ENABLE;
bcostm 0:e1d164542663 245 }
bcostm 0:e1d164542663 246 else if(IntSel == L3GD20_INT2)
bcostm 0:e1d164542663 247 {
bcostm 0:e1d164542663 248 tmpreg &= 0xF7;
bcostm 0:e1d164542663 249 tmpreg |= L3GD20_INT2INTERRUPT_ENABLE;
bcostm 0:e1d164542663 250 }
bcostm 0:e1d164542663 251
bcostm 0:e1d164542663 252 /* Write value to MEMS CTRL_REG3 register */
bcostm 0:e1d164542663 253 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1);
bcostm 0:e1d164542663 254 }
bcostm 0:e1d164542663 255
bcostm 0:e1d164542663 256 /**
bcostm 0:e1d164542663 257 * @brief Disable INT1 or INT2 interrupt
bcostm 0:e1d164542663 258 * @param IntSel: choice of INT1 or INT2
bcostm 0:e1d164542663 259 * This parameter can be:
bcostm 0:e1d164542663 260 * @arg L3GD20_INT1
bcostm 0:e1d164542663 261 * @arg L3GD20_INT2
bcostm 0:e1d164542663 262 * @retval None
bcostm 0:e1d164542663 263 */
bcostm 0:e1d164542663 264 void L3GD20_DisableIT(uint8_t IntSel)
bcostm 0:e1d164542663 265 {
bcostm 0:e1d164542663 266 uint8_t tmpreg;
bcostm 0:e1d164542663 267
bcostm 0:e1d164542663 268 /* Read CTRL_REG3 register */
bcostm 0:e1d164542663 269 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1);
bcostm 0:e1d164542663 270
bcostm 0:e1d164542663 271 if(IntSel == L3GD20_INT1)
bcostm 0:e1d164542663 272 {
bcostm 0:e1d164542663 273 tmpreg &= 0x7F;
bcostm 0:e1d164542663 274 tmpreg |= L3GD20_INT1INTERRUPT_DISABLE;
bcostm 0:e1d164542663 275 }
bcostm 0:e1d164542663 276 else if(IntSel == L3GD20_INT2)
bcostm 0:e1d164542663 277 {
bcostm 0:e1d164542663 278 tmpreg &= 0xF7;
bcostm 0:e1d164542663 279 tmpreg |= L3GD20_INT2INTERRUPT_DISABLE;
bcostm 0:e1d164542663 280 }
bcostm 0:e1d164542663 281
bcostm 0:e1d164542663 282 /* Write value to MEMS CTRL_REG3 register */
bcostm 0:e1d164542663 283 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1);
bcostm 0:e1d164542663 284 }
bcostm 0:e1d164542663 285
bcostm 0:e1d164542663 286 /**
bcostm 0:e1d164542663 287 * @brief Set High Pass Filter Modality
bcostm 0:e1d164542663 288 * @param FilterStruct: contains the configuration setting for the L3GD20.
bcostm 0:e1d164542663 289 * @retval None
bcostm 0:e1d164542663 290 */
bcostm 0:e1d164542663 291 void L3GD20_FilterConfig(uint8_t FilterStruct)
bcostm 0:e1d164542663 292 {
bcostm 0:e1d164542663 293 uint8_t tmpreg;
bcostm 0:e1d164542663 294
bcostm 0:e1d164542663 295 /* Read CTRL_REG2 register */
bcostm 0:e1d164542663 296 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1);
bcostm 0:e1d164542663 297
bcostm 0:e1d164542663 298 tmpreg &= 0xC0;
bcostm 0:e1d164542663 299
bcostm 0:e1d164542663 300 /* Configure MEMS: mode and cutoff frequency */
bcostm 0:e1d164542663 301 tmpreg |= FilterStruct;
bcostm 0:e1d164542663 302
bcostm 0:e1d164542663 303 /* Write value to MEMS CTRL_REG2 register */
bcostm 0:e1d164542663 304 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1);
bcostm 0:e1d164542663 305 }
bcostm 0:e1d164542663 306
bcostm 0:e1d164542663 307 /**
bcostm 0:e1d164542663 308 * @brief Enable or Disable High Pass Filter
bcostm 0:e1d164542663 309 * @param HighPassFilterState: new state of the High Pass Filter feature.
bcostm 0:e1d164542663 310 * This parameter can be:
bcostm 0:e1d164542663 311 * @arg: L3GD20_HIGHPASSFILTER_DISABLE
bcostm 0:e1d164542663 312 * @arg: L3GD20_HIGHPASSFILTER_ENABLE
bcostm 0:e1d164542663 313 * @retval None
bcostm 0:e1d164542663 314 */
bcostm 0:e1d164542663 315 void L3GD20_FilterCmd(uint8_t HighPassFilterState)
bcostm 0:e1d164542663 316 {
bcostm 0:e1d164542663 317 uint8_t tmpreg;
bcostm 0:e1d164542663 318
bcostm 0:e1d164542663 319 /* Read CTRL_REG5 register */
bcostm 0:e1d164542663 320 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
bcostm 0:e1d164542663 321
bcostm 0:e1d164542663 322 tmpreg &= 0xEF;
bcostm 0:e1d164542663 323
bcostm 0:e1d164542663 324 tmpreg |= HighPassFilterState;
bcostm 0:e1d164542663 325
bcostm 0:e1d164542663 326 /* Write value to MEMS CTRL_REG5 register */
bcostm 0:e1d164542663 327 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
bcostm 0:e1d164542663 328 }
bcostm 0:e1d164542663 329
bcostm 0:e1d164542663 330 /**
bcostm 0:e1d164542663 331 * @brief Get status for L3GD20 data
bcostm 0:e1d164542663 332 * @param None
bcostm 0:e1d164542663 333 * @retval Data status in a L3GD20 Data
bcostm 0:e1d164542663 334 */
bcostm 0:e1d164542663 335 uint8_t L3GD20_GetDataStatus(void)
bcostm 0:e1d164542663 336 {
bcostm 0:e1d164542663 337 uint8_t tmpreg;
bcostm 0:e1d164542663 338
bcostm 0:e1d164542663 339 /* Read STATUS_REG register */
bcostm 0:e1d164542663 340 GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1);
bcostm 0:e1d164542663 341
bcostm 0:e1d164542663 342 return tmpreg;
bcostm 0:e1d164542663 343 }
bcostm 0:e1d164542663 344
bcostm 0:e1d164542663 345 /**
bcostm 0:e1d164542663 346 * @brief Calculate the L3GD20 angular data.
bcostm 0:e1d164542663 347 * @param pfData: Data out pointer
bcostm 0:e1d164542663 348 * @retval None
bcostm 0:e1d164542663 349 */
bcostm 0:e1d164542663 350 void L3GD20_ReadXYZAngRate(float *pfData)
bcostm 0:e1d164542663 351 {
bcostm 0:e1d164542663 352 uint8_t tmpbuffer[6] ={0};
bcostm 0:e1d164542663 353 int16_t RawData[3] = {0};
bcostm 0:e1d164542663 354 uint8_t tmpreg = 0;
bcostm 0:e1d164542663 355 float sensitivity = 0;
bcostm 0:e1d164542663 356 int i =0;
bcostm 0:e1d164542663 357
bcostm 0:e1d164542663 358 GYRO_IO_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1);
bcostm 0:e1d164542663 359
bcostm 0:e1d164542663 360 GYRO_IO_Read(tmpbuffer,L3GD20_OUT_X_L_ADDR,6);
bcostm 0:e1d164542663 361
bcostm 0:e1d164542663 362 /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/
bcostm 0:e1d164542663 363 if(!(tmpreg & L3GD20_BLE_MSB))
bcostm 0:e1d164542663 364 {
bcostm 0:e1d164542663 365 for(i=0; i<3; i++)
bcostm 0:e1d164542663 366 {
bcostm 0:e1d164542663 367 RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]);
bcostm 0:e1d164542663 368 }
bcostm 0:e1d164542663 369 }
bcostm 0:e1d164542663 370 else
bcostm 0:e1d164542663 371 {
bcostm 0:e1d164542663 372 for(i=0; i<3; i++)
bcostm 0:e1d164542663 373 {
bcostm 0:e1d164542663 374 RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]);
bcostm 0:e1d164542663 375 }
bcostm 0:e1d164542663 376 }
bcostm 0:e1d164542663 377
bcostm 0:e1d164542663 378 /* Switch the sensitivity value set in the CRTL4 */
bcostm 0:e1d164542663 379 switch(tmpreg & L3GD20_FULLSCALE_SELECTION)
bcostm 0:e1d164542663 380 {
bcostm 0:e1d164542663 381 case L3GD20_FULLSCALE_250:
bcostm 0:e1d164542663 382 sensitivity=L3GD20_SENSITIVITY_250DPS;
bcostm 0:e1d164542663 383 break;
bcostm 0:e1d164542663 384
bcostm 0:e1d164542663 385 case L3GD20_FULLSCALE_500:
bcostm 0:e1d164542663 386 sensitivity=L3GD20_SENSITIVITY_500DPS;
bcostm 0:e1d164542663 387 break;
bcostm 0:e1d164542663 388
bcostm 0:e1d164542663 389 case L3GD20_FULLSCALE_2000:
bcostm 0:e1d164542663 390 sensitivity=L3GD20_SENSITIVITY_2000DPS;
bcostm 0:e1d164542663 391 break;
bcostm 0:e1d164542663 392 }
bcostm 0:e1d164542663 393 /* Divide by sensitivity */
bcostm 0:e1d164542663 394 for(i=0; i<3; i++)
bcostm 0:e1d164542663 395 {
bcostm 0:e1d164542663 396 pfData[i]=(float)(RawData[i] * sensitivity);
bcostm 0:e1d164542663 397 }
bcostm 0:e1d164542663 398 }
bcostm 0:e1d164542663 399
bcostm 0:e1d164542663 400 /**
bcostm 0:e1d164542663 401 * @}
bcostm 0:e1d164542663 402 */
bcostm 0:e1d164542663 403
bcostm 0:e1d164542663 404 /**
bcostm 0:e1d164542663 405 * @}
bcostm 0:e1d164542663 406 */
bcostm 0:e1d164542663 407
bcostm 0:e1d164542663 408 /**
bcostm 0:e1d164542663 409 * @}
bcostm 0:e1d164542663 410 */
bcostm 0:e1d164542663 411
bcostm 0:e1d164542663 412 /**
bcostm 0:e1d164542663 413 * @}
bcostm 0:e1d164542663 414 */
bcostm 0:e1d164542663 415
bcostm 0:e1d164542663 416 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/