Robotique FIP / IMU_FIP_nucleo

class IMU nucleo

Dependents:   Coupe-Robotique-FIP-Main

Fork of IMU_FIP by Robotique FIP

Components/LIS3MDL/lis3mdl.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_lis3mdl.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 Implementation file for component LIS3MDL
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 "lis3mdl.h"
quentin9696 0:528e23a13fb7 41 #include "lis3mdl_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 LIS3MDL Magnetic sensor and calculate Magnetic in mgauss.
quentin9696 0:528e23a13fb7 48 * @param float *pfData
quentin9696 0:528e23a13fb7 49 * @retval None.
quentin9696 0:528e23a13fb7 50 */
quentin9696 0:528e23a13fb7 51 void LIS3MDL::GetAxes(AxesRaw_TypeDef *pData)
quentin9696 0:528e23a13fb7 52 {
quentin9696 0:528e23a13fb7 53 uint8_t tempReg = 0x00;
quentin9696 0:528e23a13fb7 54 int16_t pDataRaw[3];
quentin9696 0:528e23a13fb7 55 float sensitivity = 0;
quentin9696 0:528e23a13fb7 56 int ret;
quentin9696 0:528e23a13fb7 57
quentin9696 0:528e23a13fb7 58 GetAxesRaw(pDataRaw);
quentin9696 0:528e23a13fb7 59
quentin9696 0:528e23a13fb7 60 ret = dev_i2c.i2c_read(&tempReg, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
quentin9696 0:528e23a13fb7 61
quentin9696 0:528e23a13fb7 62 if (ret == 0)
quentin9696 0:528e23a13fb7 63 {
quentin9696 0:528e23a13fb7 64 tempReg &= LIS3MDL_M_FS_MASK;
quentin9696 0:528e23a13fb7 65
quentin9696 0:528e23a13fb7 66 switch(tempReg)
quentin9696 0:528e23a13fb7 67 {
quentin9696 0:528e23a13fb7 68 case LIS3MDL_M_FS_4:
quentin9696 0:528e23a13fb7 69 sensitivity = 0.14;
quentin9696 0:528e23a13fb7 70 break;
quentin9696 0:528e23a13fb7 71 case LIS3MDL_M_FS_8:
quentin9696 0:528e23a13fb7 72 sensitivity = 0.29;
quentin9696 0:528e23a13fb7 73 break;
quentin9696 0:528e23a13fb7 74 case LIS3MDL_M_FS_12:
quentin9696 0:528e23a13fb7 75 sensitivity = 0.43;
quentin9696 0:528e23a13fb7 76 break;
quentin9696 0:528e23a13fb7 77 case LIS3MDL_M_FS_16:
quentin9696 0:528e23a13fb7 78 sensitivity = 0.58;
quentin9696 0:528e23a13fb7 79 break;
quentin9696 0:528e23a13fb7 80 }
quentin9696 0:528e23a13fb7 81 }
quentin9696 0:528e23a13fb7 82
quentin9696 0:528e23a13fb7 83 pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
quentin9696 0:528e23a13fb7 84 pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity);
quentin9696 0:528e23a13fb7 85 pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity);
quentin9696 0:528e23a13fb7 86
quentin9696 0:528e23a13fb7 87 }
quentin9696 0:528e23a13fb7 88
quentin9696 0:528e23a13fb7 89 /**
quentin9696 0:528e23a13fb7 90 * @brief Read raw data from LIS3MDL Magnetic sensor output register.
quentin9696 0:528e23a13fb7 91 * @param float *pfData
quentin9696 0:528e23a13fb7 92 * @retval None.
quentin9696 0:528e23a13fb7 93 */
quentin9696 0:528e23a13fb7 94 void LIS3MDL::GetAxesRaw(int16_t *pData)
quentin9696 0:528e23a13fb7 95 {
quentin9696 0:528e23a13fb7 96 uint8_t tempReg[2] = {0,0};
quentin9696 0:528e23a13fb7 97 int ret;
quentin9696 0:528e23a13fb7 98
quentin9696 0:528e23a13fb7 99 pData[0] = pData[1] = pData[2] = 0;
quentin9696 0:528e23a13fb7 100
quentin9696 0:528e23a13fb7 101 ret = dev_i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_X_L_M + 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 ret = dev_i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Y_L_M + 0x80, 2);
quentin9696 0:528e23a13fb7 107 }
quentin9696 0:528e23a13fb7 108
quentin9696 0:528e23a13fb7 109 if (ret == 0)
quentin9696 0:528e23a13fb7 110 {
quentin9696 0:528e23a13fb7 111 pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 112 ret = dev_i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Z_L_M + 0x80, 2);
quentin9696 0:528e23a13fb7 113 }
quentin9696 0:528e23a13fb7 114
quentin9696 0:528e23a13fb7 115 if (ret == 0)
quentin9696 0:528e23a13fb7 116 {
quentin9696 0:528e23a13fb7 117 pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
quentin9696 0:528e23a13fb7 118 }
quentin9696 0:528e23a13fb7 119 }
quentin9696 0:528e23a13fb7 120
quentin9696 0:528e23a13fb7 121 /**
quentin9696 0:528e23a13fb7 122 * @brief Read ID address of HTS221
quentin9696 0:528e23a13fb7 123 * @param Device ID address
quentin9696 0:528e23a13fb7 124 * @retval ID name
quentin9696 0:528e23a13fb7 125 */
quentin9696 0:528e23a13fb7 126 uint8_t LIS3MDL::ReadID(void)
quentin9696 0:528e23a13fb7 127 {
quentin9696 0:528e23a13fb7 128 uint8_t tmp=0x00;
quentin9696 0:528e23a13fb7 129 int ret;
quentin9696 0:528e23a13fb7 130
quentin9696 0:528e23a13fb7 131 /* Read WHO I AM register */
quentin9696 0:528e23a13fb7 132 ret = dev_i2c.i2c_read(&tmp, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_WHO_AM_I_ADDR, 1);
quentin9696 0:528e23a13fb7 133
quentin9696 0:528e23a13fb7 134 /* Return the ID */
quentin9696 0:528e23a13fb7 135 return ((ret == 0) ? (uint8_t)tmp : 0);
quentin9696 0:528e23a13fb7 136 }
quentin9696 0:528e23a13fb7 137
quentin9696 0:528e23a13fb7 138 /**
quentin9696 0:528e23a13fb7 139 * @brief Set HTS221 Initialization.
quentin9696 0:528e23a13fb7 140 * @param InitStruct: it contains the configuration setting for the HTS221.
quentin9696 0:528e23a13fb7 141 * @retval None
quentin9696 0:528e23a13fb7 142 */
quentin9696 0:528e23a13fb7 143 void LIS3MDL::Init() {
quentin9696 0:528e23a13fb7 144
quentin9696 0:528e23a13fb7 145 uint8_t tmp1 = 0x00;
quentin9696 0:528e23a13fb7 146 int ret;
quentin9696 0:528e23a13fb7 147
quentin9696 0:528e23a13fb7 148 /****** Magnetic sensor *******/
quentin9696 0:528e23a13fb7 149
quentin9696 0:528e23a13fb7 150 ret = dev_i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1);
quentin9696 0:528e23a13fb7 151
quentin9696 0:528e23a13fb7 152 /* Conversion mode selection */
quentin9696 0:528e23a13fb7 153 if (ret == 0)
quentin9696 0:528e23a13fb7 154 {
quentin9696 0:528e23a13fb7 155 tmp1 &= ~(LIS3MDL_M_MD_MASK);
quentin9696 0:528e23a13fb7 156 tmp1 |= LIS3MDL_M_MD_CONTINUOUS;
quentin9696 0:528e23a13fb7 157 ret = dev_i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1);
quentin9696 0:528e23a13fb7 158 }
quentin9696 0:528e23a13fb7 159
quentin9696 0:528e23a13fb7 160 if (ret == 0)
quentin9696 0:528e23a13fb7 161 {
quentin9696 0:528e23a13fb7 162 ret = dev_i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1);
quentin9696 0:528e23a13fb7 163 }
quentin9696 0:528e23a13fb7 164
quentin9696 0:528e23a13fb7 165 if (ret == 0)
quentin9696 0:528e23a13fb7 166 {
quentin9696 0:528e23a13fb7 167 /* Output data rate selection */
quentin9696 0:528e23a13fb7 168 tmp1 &= ~(LIS3MDL_M_DO_MASK);
quentin9696 0:528e23a13fb7 169 tmp1 |= LIS3MDL_M_DO_80;
quentin9696 0:528e23a13fb7 170
quentin9696 0:528e23a13fb7 171 /* X and Y axes Operative mode selection */
quentin9696 0:528e23a13fb7 172 tmp1 &= ~(LIS3MDL_M_OM_MASK);
quentin9696 0:528e23a13fb7 173 tmp1 |= LIS3MDL_M_OM_HP;
quentin9696 0:528e23a13fb7 174
quentin9696 0:528e23a13fb7 175 ret = dev_i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1);
quentin9696 0:528e23a13fb7 176 }
quentin9696 0:528e23a13fb7 177
quentin9696 0:528e23a13fb7 178 if (ret == 0)
quentin9696 0:528e23a13fb7 179 {
quentin9696 0:528e23a13fb7 180 ret = dev_i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
quentin9696 0:528e23a13fb7 181 }
quentin9696 0:528e23a13fb7 182
quentin9696 0:528e23a13fb7 183 if (ret == 0)
quentin9696 0:528e23a13fb7 184 {
quentin9696 0:528e23a13fb7 185 /* Full scale selection */
quentin9696 0:528e23a13fb7 186 tmp1 &= ~(LIS3MDL_M_FS_MASK);
quentin9696 0:528e23a13fb7 187 tmp1 |= LIS3MDL_M_FS_4;
quentin9696 0:528e23a13fb7 188
quentin9696 0:528e23a13fb7 189 ret = dev_i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
quentin9696 0:528e23a13fb7 190 }
quentin9696 0:528e23a13fb7 191
quentin9696 0:528e23a13fb7 192 /******************************/
quentin9696 0:528e23a13fb7 193
quentin9696 0:528e23a13fb7 194 if (ret == 0)
quentin9696 0:528e23a13fb7 195 {
quentin9696 0:528e23a13fb7 196 if(ReadID() == I_AM_LIS3MDL_M)
quentin9696 0:528e23a13fb7 197 {
quentin9696 0:528e23a13fb7 198 LIS3MDLInitialized = 1;
quentin9696 0:528e23a13fb7 199 }
quentin9696 0:528e23a13fb7 200 }
quentin9696 0:528e23a13fb7 201
quentin9696 0:528e23a13fb7 202 return;
quentin9696 0:528e23a13fb7 203 }