IMU_FIP_nucleo - class IMU nucleo

Robotique FIP » Code » IMU_FIP_nucleo

Robotique FIP / IMU_FIP_nucleo

class IMU nucleo

Dependents: Coupe-Robotique-FIP-Main

Fork of IMU_FIP by Robotique FIP

Components/LIS3MDL/lis3mdl.cpp@5:e2e603447679, 2015-05-21 (annotated)

Committer:: quentin9696
Date:: Thu May 21 11:21:58 2015 +0000
Revision:: 5:e2e603447679
Parent:: 0:528e23a13fb7

Who changed what in which revision?

User	Revision	Line number	New 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>© 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	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	27 Sep 2015
Imports:	2
Forks:	0
Commits:	6
Dependents:	2
Dependencies:	0
Followers:	10

Components/LIS3MDL/lis3mdl.cpp@5:e2e603447679, 2015-05-21 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning