This fork is necessary for the Starship code.
Fork of BSP_DISCO_F429ZI by
l3gd20.c
00001 /** 00002 ****************************************************************************** 00003 * @file l3gd20.c 00004 * @author MCD Application Team 00005 * @version V2.0.0 00006 * @date 26-June-2015 00007 * @brief This file provides a set of functions needed to manage the L3GD20, 00008 * ST MEMS motion sensor, 3-axis digital output gyroscope. 00009 ****************************************************************************** 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2> 00013 * 00014 * Redistribution and use in source and binary forms, with or without modification, 00015 * are permitted provided that the following conditions are met: 00016 * 1. Redistributions of source code must retain the above copyright notice, 00017 * this list of conditions and the following disclaimer. 00018 * 2. Redistributions in binary form must reproduce the above copyright notice, 00019 * this list of conditions and the following disclaimer in the documentation 00020 * and/or other materials provided with the distribution. 00021 * 3. Neither the name of STMicroelectronics nor the names of its contributors 00022 * may be used to endorse or promote products derived from this software 00023 * without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00026 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00027 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00028 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00029 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00030 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00031 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00033 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00034 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 ****************************************************************************** 00037 */ 00038 /* Includes ------------------------------------------------------------------*/ 00039 #include "l3gd20.h" 00040 00041 /** @addtogroup BSP 00042 * @{ 00043 */ 00044 00045 /** @addtogroup Components 00046 * @{ 00047 */ 00048 00049 /** @addtogroup L3GD20 00050 * @{ 00051 */ 00052 00053 /** @defgroup L3GD20_Private_TypesDefinitions 00054 * @{ 00055 */ 00056 00057 /** 00058 * @} 00059 */ 00060 00061 /** @defgroup L3GD20_Private_Defines 00062 * @{ 00063 */ 00064 00065 /** 00066 * @} 00067 */ 00068 00069 /** @defgroup L3GD20_Private_Macros 00070 * @{ 00071 */ 00072 00073 /** 00074 * @} 00075 */ 00076 00077 /** @defgroup L3GD20_Private_Variables 00078 * @{ 00079 */ 00080 GYRO_DrvTypeDef L3gd20Drv = 00081 { 00082 L3GD20_Init, 00083 L3GD20_DeInit, 00084 L3GD20_ReadID, 00085 L3GD20_RebootCmd, 00086 L3GD20_LowPower, 00087 L3GD20_INT1InterruptConfig, 00088 L3GD20_EnableIT, 00089 L3GD20_DisableIT, 00090 0, 00091 0, 00092 L3GD20_FilterConfig, 00093 L3GD20_FilterCmd, 00094 L3GD20_ReadXYZAngRate 00095 }; 00096 00097 /** 00098 * @} 00099 */ 00100 00101 /** @defgroup L3GD20_Private_FunctionPrototypes 00102 * @{ 00103 */ 00104 00105 /** 00106 * @} 00107 */ 00108 00109 /** @defgroup L3GD20_Private_Functions 00110 * @{ 00111 */ 00112 00113 /** 00114 * @brief Set L3GD20 Initialization. 00115 * @param L3GD20_InitStruct: pointer to a L3GD20_InitTypeDef structure 00116 * that contains the configuration setting for the L3GD20. 00117 * @retval None 00118 */ 00119 void L3GD20_Init(uint16_t InitStruct) 00120 { 00121 uint8_t ctrl = 0x00; 00122 00123 /* Configure the low level interface */ 00124 GYRO_IO_Init(); 00125 00126 /* Write value to MEMS CTRL_REG1 register */ 00127 ctrl = (uint8_t) InitStruct; 00128 GYRO_IO_Write(&ctrl, L3GD20_CTRL_REG1_ADDR, 1); 00129 00130 /* Write value to MEMS CTRL_REG4 register */ 00131 ctrl = (uint8_t) (InitStruct >> 8); 00132 GYRO_IO_Write(&ctrl, L3GD20_CTRL_REG4_ADDR, 1); 00133 } 00134 00135 00136 00137 /** 00138 * @brief L3GD20 De-initialization 00139 * @param None 00140 * @retval None 00141 */ 00142 void L3GD20_DeInit(void) 00143 { 00144 } 00145 00146 /** 00147 * @brief Read ID address of L3GD20 00148 * @param None 00149 * @retval ID name 00150 */ 00151 uint8_t L3GD20_ReadID(void) 00152 { 00153 uint8_t tmp; 00154 00155 /* Configure the low level interface */ 00156 GYRO_IO_Init(); 00157 00158 /* Read WHO I AM register */ 00159 GYRO_IO_Read(&tmp, L3GD20_WHO_AM_I_ADDR, 1); 00160 00161 /* Return the ID */ 00162 return (uint8_t)tmp; 00163 } 00164 00165 /** 00166 * @brief Reboot memory content of L3GD20 00167 * @param None 00168 * @retval None 00169 */ 00170 void L3GD20_RebootCmd(void) 00171 { 00172 uint8_t tmpreg; 00173 00174 /* Read CTRL_REG5 register */ 00175 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); 00176 00177 /* Enable or Disable the reboot memory */ 00178 tmpreg |= L3GD20_BOOT_REBOOTMEMORY; 00179 00180 /* Write value to MEMS CTRL_REG5 register */ 00181 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); 00182 } 00183 00184 /** 00185 * @brief Set L3GD20 in low-power mode 00186 * @param 00187 * @retval None 00188 */ 00189 void L3GD20_LowPower(uint16_t InitStruct) 00190 { 00191 uint8_t ctrl = 0x00; 00192 00193 /* Write value to MEMS CTRL_REG1 register */ 00194 ctrl = (uint8_t) InitStruct; 00195 GYRO_IO_Write(&ctrl, L3GD20_CTRL_REG1_ADDR, 1); 00196 } 00197 00198 /** 00199 * @brief Set L3GD20 Interrupt INT1 configuration 00200 * @param Int1Config: the configuration setting for the L3GD20 Interrupt. 00201 * @retval None 00202 */ 00203 void L3GD20_INT1InterruptConfig(uint16_t Int1Config) 00204 { 00205 uint8_t ctrl_cfr = 0x00, ctrl3 = 0x00; 00206 00207 /* Read INT1_CFG register */ 00208 GYRO_IO_Read(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1); 00209 00210 /* Read CTRL_REG3 register */ 00211 GYRO_IO_Read(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1); 00212 00213 ctrl_cfr &= 0x80; 00214 ctrl_cfr |= ((uint8_t) Int1Config >> 8); 00215 00216 ctrl3 &= 0xDF; 00217 ctrl3 |= ((uint8_t) Int1Config); 00218 00219 /* Write value to MEMS INT1_CFG register */ 00220 GYRO_IO_Write(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1); 00221 00222 /* Write value to MEMS CTRL_REG3 register */ 00223 GYRO_IO_Write(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1); 00224 } 00225 00226 /** 00227 * @brief Enable INT1 or INT2 interrupt 00228 * @param IntSel: choice of INT1 or INT2 00229 * This parameter can be: 00230 * @arg L3GD20_INT1 00231 * @arg L3GD20_INT2 00232 * @retval None 00233 */ 00234 void L3GD20_EnableIT(uint8_t IntSel) 00235 { 00236 uint8_t tmpreg; 00237 00238 /* Read CTRL_REG3 register */ 00239 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1); 00240 00241 if(IntSel == L3GD20_INT1) 00242 { 00243 tmpreg &= 0x7F; 00244 tmpreg |= L3GD20_INT1INTERRUPT_ENABLE; 00245 } 00246 else if(IntSel == L3GD20_INT2) 00247 { 00248 tmpreg &= 0xF7; 00249 tmpreg |= L3GD20_INT2INTERRUPT_ENABLE; 00250 } 00251 00252 /* Write value to MEMS CTRL_REG3 register */ 00253 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1); 00254 } 00255 00256 /** 00257 * @brief Disable INT1 or INT2 interrupt 00258 * @param IntSel: choice of INT1 or INT2 00259 * This parameter can be: 00260 * @arg L3GD20_INT1 00261 * @arg L3GD20_INT2 00262 * @retval None 00263 */ 00264 void L3GD20_DisableIT(uint8_t IntSel) 00265 { 00266 uint8_t tmpreg; 00267 00268 /* Read CTRL_REG3 register */ 00269 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1); 00270 00271 if(IntSel == L3GD20_INT1) 00272 { 00273 tmpreg &= 0x7F; 00274 tmpreg |= L3GD20_INT1INTERRUPT_DISABLE; 00275 } 00276 else if(IntSel == L3GD20_INT2) 00277 { 00278 tmpreg &= 0xF7; 00279 tmpreg |= L3GD20_INT2INTERRUPT_DISABLE; 00280 } 00281 00282 /* Write value to MEMS CTRL_REG3 register */ 00283 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1); 00284 } 00285 00286 /** 00287 * @brief Set High Pass Filter Modality 00288 * @param FilterStruct: contains the configuration setting for the L3GD20. 00289 * @retval None 00290 */ 00291 void L3GD20_FilterConfig(uint8_t FilterStruct) 00292 { 00293 uint8_t tmpreg; 00294 00295 /* Read CTRL_REG2 register */ 00296 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1); 00297 00298 tmpreg &= 0xC0; 00299 00300 /* Configure MEMS: mode and cutoff frequency */ 00301 tmpreg |= FilterStruct; 00302 00303 /* Write value to MEMS CTRL_REG2 register */ 00304 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1); 00305 } 00306 00307 /** 00308 * @brief Enable or Disable High Pass Filter 00309 * @param HighPassFilterState: new state of the High Pass Filter feature. 00310 * This parameter can be: 00311 * @arg: L3GD20_HIGHPASSFILTER_DISABLE 00312 * @arg: L3GD20_HIGHPASSFILTER_ENABLE 00313 * @retval None 00314 */ 00315 void L3GD20_FilterCmd(uint8_t HighPassFilterState) 00316 { 00317 uint8_t tmpreg; 00318 00319 /* Read CTRL_REG5 register */ 00320 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); 00321 00322 tmpreg &= 0xEF; 00323 00324 tmpreg |= HighPassFilterState; 00325 00326 /* Write value to MEMS CTRL_REG5 register */ 00327 GYRO_IO_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); 00328 } 00329 00330 /** 00331 * @brief Get status for L3GD20 data 00332 * @param None 00333 * @retval Data status in a L3GD20 Data 00334 */ 00335 uint8_t L3GD20_GetDataStatus(void) 00336 { 00337 uint8_t tmpreg; 00338 00339 /* Read STATUS_REG register */ 00340 GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1); 00341 00342 return tmpreg; 00343 } 00344 00345 /** 00346 * @brief Calculate the L3GD20 angular data. 00347 * @param pfData: Data out pointer 00348 * @retval None 00349 */ 00350 void L3GD20_ReadXYZAngRate(float *pfData) 00351 { 00352 uint8_t tmpbuffer[6] ={0}; 00353 int16_t RawData[3] = {0}; 00354 uint8_t tmpreg = 0; 00355 float sensitivity = 0; 00356 int i =0; 00357 00358 GYRO_IO_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1); 00359 00360 GYRO_IO_Read(tmpbuffer,L3GD20_OUT_X_L_ADDR,6); 00361 00362 /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/ 00363 if(!(tmpreg & L3GD20_BLE_MSB)) 00364 { 00365 for(i=0; i<3; i++) 00366 { 00367 RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]); 00368 } 00369 } 00370 else 00371 { 00372 for(i=0; i<3; i++) 00373 { 00374 RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]); 00375 } 00376 } 00377 00378 /* Switch the sensitivity value set in the CRTL4 */ 00379 switch(tmpreg & L3GD20_FULLSCALE_SELECTION) 00380 { 00381 case L3GD20_FULLSCALE_250: 00382 sensitivity=L3GD20_SENSITIVITY_250DPS; 00383 break; 00384 00385 case L3GD20_FULLSCALE_500: 00386 sensitivity=L3GD20_SENSITIVITY_500DPS; 00387 break; 00388 00389 case L3GD20_FULLSCALE_2000: 00390 sensitivity=L3GD20_SENSITIVITY_2000DPS; 00391 break; 00392 } 00393 /* Divide by sensitivity */ 00394 for(i=0; i<3; i++) 00395 { 00396 pfData[i]=(float)(RawData[i] * sensitivity); 00397 } 00398 } 00399 00400 /** 00401 * @} 00402 */ 00403 00404 /** 00405 * @} 00406 */ 00407 00408 /** 00409 * @} 00410 */ 00411 00412 /** 00413 * @} 00414 */ 00415 00416 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Generated on Tue Jul 12 2022 19:08:44 by 1.7.2