lis3mdl_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    lis3mdl_class.cpp
00004  * @author  AST / EST
00005  * @version V0.0.1
00006  * @date    14-April-2015
00007  * @brief   Implementation file for the LIS3MDL driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00012  *
00013  * Redistribution and use in source and binary forms, with or without modification,
00014  * are permitted provided that the following conditions are met:
00015  *   1. Redistributions of source code must retain the above copyright notice,
00016  *      this list of conditions and the following disclaimer.
00017  *   2. Redistributions in binary form must reproduce the above copyright notice,
00018  *      this list of conditions and the following disclaimer in the documentation
00019  *      and/or other materials provided with the distribution.
00020  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021  *      may be used to endorse or promote products derived from this software
00022  *      without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  ******************************************************************************
00036 */
00037 
00038 /* Includes ------------------------------------------------------------------*/
00039 #include "lis3mdl_class.h"
00040 #include "lis3mdl.h"
00041 
00042 /* Methods -------------------------------------------------------------------*/
00043 /* betzw - based on:
00044            X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lis3mdl/lis3mdl.c: revision #400,
00045            X-CUBE-MEMS1/trunk: revision #416
00046 */
00047 /* my
00048 LIS3MDL::LIS3MDL(SPI *spi, PinName cs_pin, PinName int_pin, SPI_type_t spi_type) : 
00049                 _dev_spi(spi), _cs_pin(cs_pin), _int_pin(int_pin), _spi_type(spi_type)
00050 {
00051     assert (spi);
00052     if (cs_pin == NC) 
00053     {
00054         printf ("ERROR LIS3MDL CS MUST NOT BE NC\n\r");       
00055         _dev_spi = NULL;
00056         _dev_i2c=NULL;
00057         return;
00058     }       
00059 
00060     _cs_pin = 0;    
00061     _dev_i2c=NULL;    
00062     
00063     if (_spi_type == SPI3W) LIS3MDL_Set_SpiInterface ((void *)this, LIS3MDL_SPI_3_WIRE);
00064     else if (_spi_type == SPI4W) LIS3MDL_Set_SpiInterface ((void *)this, LIS3MDL_SPI_4_WIRE);
00065 }  
00066 */
00067 LIS3MDL::LIS3MDL(DevI2C *i2c, uint8_t address, PinName int_pin) :
00068        _dev_i2c(i2c), _address(address), _cs_pin(NC), _int_pin(int_pin)  
00069 {
00070     assert (i2c);
00071     //_dev_spi = NULL;
00072 }  
00073 
00074 /* my
00075 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Set_SpiInterface (void *handle, LIS3MDL_SPIMode_t spimode)
00076 {
00077     uint8_t tmp=0x03;  //deft LIS3MDL_CTRL_REG3 value  
00078 
00079     tmp |= (uint8_t)spimode;
00080     if (LIS3MDL_IO_Write(&tmp, LIS3MDL_M_CTRL_REG3_M, 1) != MAGNETO_OK) return MAGNETO_ERROR;      
00081     return MAGNETO_OK;
00082 }
00083 */
00084 /**
00085  * @brief  Set LIS3MDL Initialization
00086  * @param  LIS3MDL_Init the configuration setting for the LIS3MDL
00087  * @retval MAGNETO_OK in case of success, an error code otherwise
00088  */
00089 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Init(MAGNETO_InitTypeDef *LIS3MDL_Init)
00090 {
00091   uint8_t tmp1 = 0x00;
00092   MAGNETO_InitTypeDef *initStructure = LIS3MDL_Init;
00093   MAGNETO_InitTypeDef tempInit;  
00094   
00095   if (initStructure == NULL) {// default params   
00096     tempInit.M_FullScale = LIS3MDL_M_FS_4;
00097     tempInit.M_OperatingMode = LIS3MDL_M_MD_CONTINUOUS;
00098     tempInit.M_XYOperativeMode = LIS3MDL_M_OM_HP;
00099     tempInit.M_OutputDataRate = LIS3MDL_M_DO_80;
00100     initStructure = &tempInit;
00101   }
00102   
00103   
00104   /* Configure the low level interface ---------------------------------------*/
00105   if(LIS3MDL_IO_Init() != MAGNETO_OK)
00106   {
00107     return MAGNETO_ERROR;
00108   }
00109   
00110   /****** Magnetic sensor *******/
00111   
00112   if(LIS3MDL_IO_Read(&tmp1, LIS3MDL_M_CTRL_REG3_M, 1) != MAGNETO_OK)
00113   {
00114     return MAGNETO_ERROR;
00115   }
00116   
00117   /* Conversion mode selection */
00118   tmp1 &= ~(LIS3MDL_M_MD_MASK);
00119   tmp1 |= initStructure->M_OperatingMode;
00120   
00121   if(LIS3MDL_IO_Write(&tmp1, LIS3MDL_M_CTRL_REG3_M, 1) != MAGNETO_OK)
00122   {
00123     return MAGNETO_ERROR;
00124   }
00125   
00126   if(LIS3MDL_IO_Read(&tmp1, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK)
00127   {
00128     return MAGNETO_ERROR;
00129   }
00130   
00131   /* Output data rate selection */
00132   tmp1 &= ~(LIS3MDL_M_DO_MASK);
00133   tmp1 |= initStructure->M_OutputDataRate;
00134   
00135   /* X and Y axes Operative mode selection */
00136   tmp1 &= ~(LIS3MDL_M_OM_MASK);
00137   tmp1 |= initStructure->M_XYOperativeMode;
00138   
00139   if(LIS3MDL_IO_Write(&tmp1, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK)
00140   {
00141     return MAGNETO_ERROR;
00142   }
00143   
00144   if(LIS3MDL_IO_Read(&tmp1, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK)
00145   {
00146     return MAGNETO_ERROR;
00147   }
00148   
00149   /* Full scale selection */
00150   tmp1 &= ~(LIS3MDL_M_FS_MASK);
00151   tmp1 |= initStructure->M_FullScale;
00152   
00153   if(LIS3MDL_IO_Write(&tmp1, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK)
00154   {
00155     return MAGNETO_ERROR;
00156   }
00157   
00158   /* Configure interrupt lines */
00159   LIS3MDL_IO_ITConfig();
00160   
00161   return MAGNETO_OK;
00162   
00163   /******************************/
00164 }
00165 
00166 
00167 /**
00168  * @brief  Read ID of LIS3MDL Magnetic sensor
00169  * @param  m_id the pointer where the ID of the device is stored
00170  * @retval MAGNETO_OK in case of success, an error code otherwise
00171  */
00172 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Read_M_ID(uint8_t *m_id)
00173 {
00174   if(!m_id)
00175   {
00176     return MAGNETO_ERROR;
00177   }
00178   
00179   return LIS3MDL_IO_Read(m_id, LIS3MDL_M_WHO_AM_I_ADDR, 1);
00180 }
00181 
00182 
00183 /**
00184  * @brief  Read raw data from LIS3MDL Magnetic sensor output register
00185  * @param  pData the pointer where the magnetometer raw data are stored
00186  * @retval MAGNETO_OK in case of success, an error code otherwise
00187  */
00188 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxesRaw(int16_t *pData)
00189 {
00190   uint8_t tempReg[2] = {0, 0};
00191   
00192   if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_X_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00193                      2) != MAGNETO_OK)
00194   {
00195     return MAGNETO_ERROR;
00196   }
00197   
00198   pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00199   
00200   if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Y_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00201                      2) != MAGNETO_OK)
00202   {
00203     return MAGNETO_ERROR;
00204   }
00205   
00206   pData[1] = *((int16_t*)&tempReg[0]);//((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00207   
00208   if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Z_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00209                      2) != MAGNETO_OK)
00210   {
00211     return MAGNETO_ERROR;
00212   }
00213   
00214   pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00215   
00216   return MAGNETO_OK;
00217 }
00218 
00219 
00220 
00221 //my----------------------------------------------------------------------------
00222 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_SetAxeOffset(uint8_t axe,int16_t offset)
00223 {
00224   int16_t tmp;
00225   uint8_t tmp1;
00226   uint8_t tempReg = 0x00;
00227   float sensitivity = 0;
00228   float foffset;
00229   
00230   if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK)
00231   {
00232     return MAGNETO_ERROR;
00233   }
00234   tempReg &= LIS3MDL_M_FS_MASK;
00235   switch(tempReg)
00236   {
00237     case LIS3MDL_M_FS_4:
00238       sensitivity = 0.14;
00239       break;
00240     case LIS3MDL_M_FS_8:
00241       sensitivity = 0.29;
00242       break;
00243     case LIS3MDL_M_FS_12:
00244       sensitivity = 0.43;
00245       break;
00246     case LIS3MDL_M_FS_16:
00247       sensitivity = 0.58;
00248       break;
00249   }
00250      
00251   foffset=(float)offset;   
00252   foffset=foffset/sensitivity;
00253   tmp=floor(foffset);
00254   tmp1=tmp; 
00255   if(LIS3MDL_IO_Write(&tmp1, 5+2*axe, 1)!= MAGNETO_OK)
00256     return MAGNETO_ERROR;
00257   tmp1=tmp>>8;    
00258   if(LIS3MDL_IO_Write(&tmp1, 6+2*axe, 1)!= MAGNETO_OK)
00259     return MAGNETO_ERROR;      
00260   return MAGNETO_OK;
00261 }
00262 
00263 
00264 /**
00265  * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss
00266  * @param pData the pointer where the magnetometer data are stored
00267  * @retval MAGNETO_OK in case of success, an error code otherwise
00268  */
00269 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxes(int32_t *pData)
00270 {
00271   uint8_t tempReg = 0x00;
00272   int16_t pDataRaw[3];
00273   float sensitivity = 0;
00274   
00275   if(LIS3MDL_M_GetAxesRaw(pDataRaw) != MAGNETO_OK)
00276   {
00277     return MAGNETO_ERROR;
00278   }
00279   
00280   if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK)
00281   {
00282     return MAGNETO_ERROR;
00283   }
00284   
00285   tempReg &= LIS3MDL_M_FS_MASK;
00286   
00287   switch(tempReg)
00288   {
00289     case LIS3MDL_M_FS_4:
00290       sensitivity = 0.14;
00291       break;
00292     case LIS3MDL_M_FS_8:
00293       sensitivity = 0.29;
00294       break;
00295     case LIS3MDL_M_FS_12:
00296       sensitivity = 0.43;
00297       break;
00298     case LIS3MDL_M_FS_16:
00299       sensitivity = 0.58;
00300       break;
00301   }
00302   
00303   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00304   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00305   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00306   
00307   return MAGNETO_OK;
00308 }
00309 
00310 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/