Robotique FIP / Mbed 2 deprecated FIP_REV1

ble

Dependencies:   HC_SR04_Ultrasonic_Library Servo mbed

Fork of FIP_REV1 by Robotique FIP

Sensor_Shield/lis3mdl.c@4:69a35a56ac48, 2015-07-09 (annotated)

Committer:
julientiron
Date:
Thu Jul 09 13:33:36 2015 +0000
Revision:
4:69a35a56ac48
Parent:
1:bdbf36f8408d 
BLE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julientiron 1:bdbf36f8408d 1 /**
julientiron 1:bdbf36f8408d 2 ******************************************************************************
julientiron 1:bdbf36f8408d 3 * @file lis3mdl.c
julientiron 1:bdbf36f8408d 4 * @author MEMS Application Team
julientiron 1:bdbf36f8408d 5 * @version V1.0.0
julientiron 1:bdbf36f8408d 6 * @date 30-July-2014
julientiron 1:bdbf36f8408d 7 * @brief This file provides a set of functions needed to manage the lis3mdl.
julientiron 1:bdbf36f8408d 8 ******************************************************************************
julientiron 1:bdbf36f8408d 9 * @attention
julientiron 1:bdbf36f8408d 10 *
julientiron 1:bdbf36f8408d 11 * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
julientiron 1:bdbf36f8408d 12 *
julientiron 1:bdbf36f8408d 13 * Redistribution and use in source and binary forms, with or without modification,
julientiron 1:bdbf36f8408d 14 * are permitted provided that the following conditions are met:
julientiron 1:bdbf36f8408d 15 * 1. Redistributions of source code must retain the above copyright notice,
julientiron 1:bdbf36f8408d 16 * this list of conditions and the following disclaimer.
julientiron 1:bdbf36f8408d 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
julientiron 1:bdbf36f8408d 18 * this list of conditions and the following disclaimer in the documentation
julientiron 1:bdbf36f8408d 19 * and/or other materials provided with the distribution.
julientiron 1:bdbf36f8408d 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
julientiron 1:bdbf36f8408d 21 * may be used to endorse or promote products derived from this software
julientiron 1:bdbf36f8408d 22 * without specific prior written permission.
julientiron 1:bdbf36f8408d 23 *
julientiron 1:bdbf36f8408d 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
julientiron 1:bdbf36f8408d 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
julientiron 1:bdbf36f8408d 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
julientiron 1:bdbf36f8408d 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
julientiron 1:bdbf36f8408d 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
julientiron 1:bdbf36f8408d 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
julientiron 1:bdbf36f8408d 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
julientiron 1:bdbf36f8408d 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
julientiron 1:bdbf36f8408d 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
julientiron 1:bdbf36f8408d 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
julientiron 1:bdbf36f8408d 34 *
julientiron 1:bdbf36f8408d 35 ******************************************************************************
julientiron 1:bdbf36f8408d 36 */
julientiron 1:bdbf36f8408d 37 /* Includes ------------------------------------------------------------------*/
julientiron 1:bdbf36f8408d 38 #include "lis3mdl.h"
julientiron 1:bdbf36f8408d 39 #include <math.h>
julientiron 1:bdbf36f8408d 40
julientiron 1:bdbf36f8408d 41 /** @addtogroup BSP
julientiron 1:bdbf36f8408d 42 * @{
julientiron 1:bdbf36f8408d 43 */
julientiron 1:bdbf36f8408d 44
julientiron 1:bdbf36f8408d 45 /** @addtogroup MEMS_SHIELD
julientiron 1:bdbf36f8408d 46 * @{
julientiron 1:bdbf36f8408d 47 */
julientiron 1:bdbf36f8408d 48
julientiron 1:bdbf36f8408d 49 /** @addtogroup LIS3MDL
julientiron 1:bdbf36f8408d 50 * @{
julientiron 1:bdbf36f8408d 51 */
julientiron 1:bdbf36f8408d 52
julientiron 1:bdbf36f8408d 53 /** @defgroup LIS3MDL_Private_TypesDefinitions
julientiron 1:bdbf36f8408d 54 * @{
julientiron 1:bdbf36f8408d 55 */
julientiron 1:bdbf36f8408d 56
julientiron 1:bdbf36f8408d 57 /**
julientiron 1:bdbf36f8408d 58 * @}
julientiron 1:bdbf36f8408d 59 */
julientiron 1:bdbf36f8408d 60
julientiron 1:bdbf36f8408d 61 /** @defgroup LIS3MDL_Private_Defines
julientiron 1:bdbf36f8408d 62 * @{
julientiron 1:bdbf36f8408d 63 */
julientiron 1:bdbf36f8408d 64
julientiron 1:bdbf36f8408d 65 /**
julientiron 1:bdbf36f8408d 66 * @}
julientiron 1:bdbf36f8408d 67 */
julientiron 1:bdbf36f8408d 68
julientiron 1:bdbf36f8408d 69 /** @defgroup LIS3MDL_Private_Macros
julientiron 1:bdbf36f8408d 70 * @{
julientiron 1:bdbf36f8408d 71 */
julientiron 1:bdbf36f8408d 72
julientiron 1:bdbf36f8408d 73 /**
julientiron 1:bdbf36f8408d 74 * @}
julientiron 1:bdbf36f8408d 75 */
julientiron 1:bdbf36f8408d 76
julientiron 1:bdbf36f8408d 77 /** @defgroup LIS3MDL_Private_Variables
julientiron 1:bdbf36f8408d 78 * @{
julientiron 1:bdbf36f8408d 79 */
julientiron 1:bdbf36f8408d 80
julientiron 1:bdbf36f8408d 81 MAGNETO_DrvTypeDef LIS3MDLDrv =
julientiron 1:bdbf36f8408d 82 {
julientiron 1:bdbf36f8408d 83 LIS3MDL_Init,
julientiron 1:bdbf36f8408d 84 LIS3MDL_Read_M_ID,
julientiron 1:bdbf36f8408d 85 LIS3MDL_M_GetAxes
julientiron 1:bdbf36f8408d 86 };
julientiron 1:bdbf36f8408d 87
julientiron 1:bdbf36f8408d 88 /**
julientiron 1:bdbf36f8408d 89 * @}
julientiron 1:bdbf36f8408d 90 */
julientiron 1:bdbf36f8408d 91
julientiron 1:bdbf36f8408d 92 /** @defgroup LIS3MDL_Private_FunctionPrototypes
julientiron 1:bdbf36f8408d 93 * @{
julientiron 1:bdbf36f8408d 94 */
julientiron 1:bdbf36f8408d 95
julientiron 1:bdbf36f8408d 96 void LIS3MDL_M_GetAxesRaw(int16_t *pData);
julientiron 1:bdbf36f8408d 97
julientiron 1:bdbf36f8408d 98 /**
julientiron 1:bdbf36f8408d 99 * @}
julientiron 1:bdbf36f8408d 100 */
julientiron 1:bdbf36f8408d 101
julientiron 1:bdbf36f8408d 102 /** @defgroup LIS3MDL_Private_Functions
julientiron 1:bdbf36f8408d 103 * @{
julientiron 1:bdbf36f8408d 104 */
julientiron 1:bdbf36f8408d 105
julientiron 1:bdbf36f8408d 106
julientiron 1:bdbf36f8408d 107 /**
julientiron 1:bdbf36f8408d 108 * @brief Set LIS3MDL Initialization.
julientiron 1:bdbf36f8408d 109 * @param InitStruct: it contains the configuration setting for the LIS3MDL.
julientiron 1:bdbf36f8408d 110 * @retval None
julientiron 1:bdbf36f8408d 111 */
julientiron 1:bdbf36f8408d 112 void LIS3MDL_Init(MAGNETO_InitTypeDef *LIS3MDL_Init)
julientiron 1:bdbf36f8408d 113 {
julientiron 1:bdbf36f8408d 114 uint8_t tmp1 = 0x00;
julientiron 1:bdbf36f8408d 115
julientiron 1:bdbf36f8408d 116 /* Configure the low level interface ---------------------------------------*/
julientiron 1:bdbf36f8408d 117 MAGNETO_IO_Init();
julientiron 1:bdbf36f8408d 118
julientiron 1:bdbf36f8408d 119
julientiron 1:bdbf36f8408d 120 /****** Magnetic sensor *******/
julientiron 1:bdbf36f8408d 121
julientiron 1:bdbf36f8408d 122 MAGNETO_IO_Read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1);
julientiron 1:bdbf36f8408d 123
julientiron 1:bdbf36f8408d 124 /* Conversion mode selection */
julientiron 1:bdbf36f8408d 125 tmp1 &= ~(LIS3MDL_M_MD_MASK);
julientiron 1:bdbf36f8408d 126 tmp1 |= LIS3MDL_Init->M_OperatingMode;
julientiron 1:bdbf36f8408d 127
julientiron 1:bdbf36f8408d 128 MAGNETO_IO_Write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1);
julientiron 1:bdbf36f8408d 129
julientiron 1:bdbf36f8408d 130
julientiron 1:bdbf36f8408d 131 MAGNETO_IO_Read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1);
julientiron 1:bdbf36f8408d 132
julientiron 1:bdbf36f8408d 133 /* Output data rate selection */
julientiron 1:bdbf36f8408d 134 tmp1 &= ~(LIS3MDL_M_DO_MASK);
julientiron 1:bdbf36f8408d 135 tmp1 |= LIS3MDL_Init->M_OutputDataRate;
julientiron 1:bdbf36f8408d 136
julientiron 1:bdbf36f8408d 137 /* X and Y axes Operative mode selection */
julientiron 1:bdbf36f8408d 138 tmp1 &= ~(LIS3MDL_M_OM_MASK);
julientiron 1:bdbf36f8408d 139 tmp1 |= LIS3MDL_Init->M_XYOperativeMode;
julientiron 1:bdbf36f8408d 140
julientiron 1:bdbf36f8408d 141 MAGNETO_IO_Write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1);
julientiron 1:bdbf36f8408d 142
julientiron 1:bdbf36f8408d 143
julientiron 1:bdbf36f8408d 144 MAGNETO_IO_Read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
julientiron 1:bdbf36f8408d 145
julientiron 1:bdbf36f8408d 146 /* Full scale selection */
julientiron 1:bdbf36f8408d 147 tmp1 &= ~(LIS3MDL_M_FS_MASK);
julientiron 1:bdbf36f8408d 148 tmp1 |= LIS3MDL_Init->M_FullScale;
julientiron 1:bdbf36f8408d 149
julientiron 1:bdbf36f8408d 150 MAGNETO_IO_Write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
julientiron 1:bdbf36f8408d 151
julientiron 1:bdbf36f8408d 152 /******************************/
julientiron 1:bdbf36f8408d 153 }
julientiron 1:bdbf36f8408d 154
julientiron 1:bdbf36f8408d 155
julientiron 1:bdbf36f8408d 156 /**
julientiron 1:bdbf36f8408d 157 * @brief Read ID of LIS3MDL Magnetic sensor
julientiron 1:bdbf36f8408d 158 * @param Device ID
julientiron 1:bdbf36f8408d 159 * @retval ID name
julientiron 1:bdbf36f8408d 160 */
julientiron 1:bdbf36f8408d 161 uint8_t LIS3MDL_Read_M_ID(void)
julientiron 1:bdbf36f8408d 162 {
julientiron 1:bdbf36f8408d 163 uint8_t tmp = 0x00;
julientiron 1:bdbf36f8408d 164
julientiron 1:bdbf36f8408d 165 /* Read WHO I AM register */
julientiron 1:bdbf36f8408d 166 MAGNETO_IO_Read(&tmp, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_WHO_AM_I_ADDR, 1);
julientiron 1:bdbf36f8408d 167
julientiron 1:bdbf36f8408d 168 /* Return the ID */
julientiron 1:bdbf36f8408d 169 return (uint8_t)tmp;
julientiron 1:bdbf36f8408d 170 }
julientiron 1:bdbf36f8408d 171
julientiron 1:bdbf36f8408d 172
julientiron 1:bdbf36f8408d 173 /**
julientiron 1:bdbf36f8408d 174 * @brief Read raw data from LIS3MDL Magnetic sensor output register.
julientiron 1:bdbf36f8408d 175 * @param float *pfData
julientiron 1:bdbf36f8408d 176 * @retval None.
julientiron 1:bdbf36f8408d 177 */
julientiron 1:bdbf36f8408d 178 void LIS3MDL_M_GetAxesRaw(int16_t *pData)
julientiron 1:bdbf36f8408d 179 {
julientiron 1:bdbf36f8408d 180 uint8_t tempReg[2] = {0,0};
julientiron 1:bdbf36f8408d 181
julientiron 1:bdbf36f8408d 182
julientiron 1:bdbf36f8408d 183 MAGNETO_IO_Read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_X_L_M + 0x80, 2);
julientiron 1:bdbf36f8408d 184
julientiron 1:bdbf36f8408d 185 pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 186
julientiron 1:bdbf36f8408d 187 MAGNETO_IO_Read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Y_L_M + 0x80, 2);
julientiron 1:bdbf36f8408d 188
julientiron 1:bdbf36f8408d 189 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 190
julientiron 1:bdbf36f8408d 191 MAGNETO_IO_Read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Z_L_M + 0x80, 2);
julientiron 1:bdbf36f8408d 192
julientiron 1:bdbf36f8408d 193 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
julientiron 1:bdbf36f8408d 194 }
julientiron 1:bdbf36f8408d 195
julientiron 1:bdbf36f8408d 196
julientiron 1:bdbf36f8408d 197 /**
julientiron 1:bdbf36f8408d 198 * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss.
julientiron 1:bdbf36f8408d 199 * @param float *pfData
julientiron 1:bdbf36f8408d 200 * @retval None.
julientiron 1:bdbf36f8408d 201 */
julientiron 1:bdbf36f8408d 202 void LIS3MDL_M_GetAxes(int32_t *pData)
julientiron 1:bdbf36f8408d 203 {
julientiron 1:bdbf36f8408d 204
julientiron 1:bdbf36f8408d 205 uint8_t tempReg = 0x00;
julientiron 1:bdbf36f8408d 206 int16_t pDataRaw[3];
julientiron 1:bdbf36f8408d 207 float sensitivity = 0;
julientiron 1:bdbf36f8408d 208
julientiron 1:bdbf36f8408d 209 LIS3MDL_M_GetAxesRaw(pDataRaw);
julientiron 1:bdbf36f8408d 210
julientiron 1:bdbf36f8408d 211 MAGNETO_IO_Read(&tempReg, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
julientiron 1:bdbf36f8408d 212
julientiron 1:bdbf36f8408d 213 tempReg &= LIS3MDL_M_FS_MASK;
julientiron 1:bdbf36f8408d 214
julientiron 1:bdbf36f8408d 215 switch(tempReg)
julientiron 1:bdbf36f8408d 216 {
julientiron 1:bdbf36f8408d 217 case LIS3MDL_M_FS_4:
julientiron 1:bdbf36f8408d 218 sensitivity = 0.14;
julientiron 1:bdbf36f8408d 219 break;
julientiron 1:bdbf36f8408d 220 case LIS3MDL_M_FS_8:
julientiron 1:bdbf36f8408d 221 sensitivity = 0.29;
julientiron 1:bdbf36f8408d 222 break;
julientiron 1:bdbf36f8408d 223 case LIS3MDL_M_FS_12:
julientiron 1:bdbf36f8408d 224 sensitivity = 0.43;
julientiron 1:bdbf36f8408d 225 break;
julientiron 1:bdbf36f8408d 226 case LIS3MDL_M_FS_16:
julientiron 1:bdbf36f8408d 227 sensitivity = 0.58;
julientiron 1:bdbf36f8408d 228 break;
julientiron 1:bdbf36f8408d 229 }
julientiron 1:bdbf36f8408d 230
julientiron 1:bdbf36f8408d 231 pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
julientiron 1:bdbf36f8408d 232 pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
julientiron 1:bdbf36f8408d 233 pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
julientiron 1:bdbf36f8408d 234 }
julientiron 1:bdbf36f8408d 235
julientiron 1:bdbf36f8408d 236 /**
julientiron 1:bdbf36f8408d 237 * @}
julientiron 1:bdbf36f8408d 238 */
julientiron 1:bdbf36f8408d 239
julientiron 1:bdbf36f8408d 240 /**
julientiron 1:bdbf36f8408d 241 * @}
julientiron 1:bdbf36f8408d 242 */
julientiron 1:bdbf36f8408d 243
julientiron 1:bdbf36f8408d 244 /**
julientiron 1:bdbf36f8408d 245 * @}
julientiron 1:bdbf36f8408d 246 */
julientiron 1:bdbf36f8408d 247
julientiron 1:bdbf36f8408d 248 /**
julientiron 1:bdbf36f8408d 249 * @}
julientiron 1:bdbf36f8408d 250 */
julientiron 1:bdbf36f8408d 251
julientiron 1:bdbf36f8408d 252
julientiron 1:bdbf36f8408d 253 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
julientiron 1:bdbf36f8408d 254