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   //set_BDU();//my 
00159   
00160   /* Configure interrupt lines */
00161   LIS3MDL_IO_ITConfig();
00162   
00163   return MAGNETO_OK;
00164   
00165   /******************************/
00166 }
00167 
00168 
00169 /**
00170  * @brief  Read ID of LIS3MDL Magnetic sensor
00171  * @param  m_id the pointer where the ID of the device is stored
00172  * @retval MAGNETO_OK in case of success, an error code otherwise
00173  */
00174 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Read_M_ID(uint8_t *m_id)
00175 {
00176   if(!m_id)
00177   {
00178     return MAGNETO_ERROR;
00179   }
00180   
00181   return LIS3MDL_IO_Read(m_id, LIS3MDL_M_WHO_AM_I_ADDR, 1);
00182 }
00183 
00184 
00185 /**
00186  * @brief  Read raw data from LIS3MDL Magnetic sensor output register
00187  * @param  pData the pointer where the magnetometer raw data are stored
00188  * @retval MAGNETO_OK in case of success, an error code otherwise
00189  */
00190 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxesRaw(int16_t *pData)
00191 {
00192   uint8_t tempReg[2] = {0, 0};
00193   
00194   if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_X_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00195                      2) != MAGNETO_OK)
00196   {
00197     return MAGNETO_ERROR;
00198   }
00199   
00200   pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00201   
00202   if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Y_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00203                      2) != MAGNETO_OK)
00204   {
00205     return MAGNETO_ERROR;
00206   }
00207   
00208   pData[1] = *((int16_t*)&tempReg[0]);//((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00209   
00210   if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Z_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00211                      2) != MAGNETO_OK)
00212   {
00213     return MAGNETO_ERROR;
00214   }
00215   
00216   pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00217   
00218   return MAGNETO_OK;
00219 }
00220 
00221 //my----------------------------------------------------------------------------
00222 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_SetAxeOffset(uint8_t axe,int16_t offset)
00223 {
00224   uint8_t tmp[2];
00225   tmp[0]=offset;
00226   tmp[1]=offset>>8;
00227   if(LIS3MDL_IO_Write(tmp, 5+2*axe, 2)!= MAGNETO_OK)
00228     return MAGNETO_ERROR;
00229   else  
00230     return MAGNETO_OK;   
00231 /*    
00232   tmp=offset>>8;   
00233   if(LIS3MDL_IO_Write(&tmp, 6+2*axe, 1)!= MAGNETO_OK)
00234     return MAGNETO_ERROR;      
00235   return MAGNETO_OK;
00236 */  
00237 }
00238 
00239 
00240 /**
00241  * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss
00242  * @param pData the pointer where the magnetometer data are stored
00243  * @retval MAGNETO_OK in case of success, an error code otherwise
00244  */
00245 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxes(int32_t *pData)
00246 {
00247   uint8_t tempReg = 0x00;
00248   int16_t pDataRaw[3];
00249   float sensitivity = 0;
00250   
00251   if(LIS3MDL_M_GetAxesRaw(pDataRaw) != MAGNETO_OK)
00252   {
00253     return MAGNETO_ERROR;
00254   }
00255 
00256   if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK)
00257   {
00258     return MAGNETO_ERROR;
00259   }
00260   
00261   tempReg &= LIS3MDL_M_FS_MASK;
00262   
00263   switch(tempReg)
00264   {
00265     case LIS3MDL_M_FS_4:
00266       sensitivity = 0.14;
00267       break;
00268     case LIS3MDL_M_FS_8:
00269       sensitivity = 0.29;
00270       break;
00271     case LIS3MDL_M_FS_12:
00272       sensitivity = 0.43;
00273       break;
00274     case LIS3MDL_M_FS_16:
00275       sensitivity = 0.58;
00276       break;
00277   }
00278   
00279   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00280   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00281   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00282 
00283   return MAGNETO_OK;
00284 }
00285 
00286 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/