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 
00048 /**
00049  * @brief  Set LIS3MDL Initialization
00050  * @param  LIS3MDL_Init the configuration setting for the LIS3MDL
00051  * @retval MAGNETO_OK in case of success, an error code otherwise
00052  */
00053 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Init(MAGNETO_InitTypeDef *LIS3MDL_Init)
00054 {
00055     uint8_t tmp1 = 0x00;
00056 
00057     /* Configure the low level interface ---------------------------------------*/
00058     if(LIS3MDL_IO_Init() != MAGNETO_OK) {
00059         return MAGNETO_ERROR;
00060     }
00061 
00062     /****** Magnetic sensor *******/
00063 
00064     if(LIS3MDL_IO_Read(&tmp1, LIS3MDL_M_CTRL_REG3_M, 1) != MAGNETO_OK) {
00065         return MAGNETO_ERROR;
00066     }
00067 
00068     /* Conversion mode selection */
00069     tmp1 &= ~(LIS3MDL_M_MD_MASK);
00070     tmp1 |= LIS3MDL_Init->M_OperatingMode;
00071 
00072     if(LIS3MDL_IO_Write(&tmp1, LIS3MDL_M_CTRL_REG3_M, 1) != MAGNETO_OK) {
00073         return MAGNETO_ERROR;
00074     }
00075     
00076     if(LIS3MDL_M_Set_ODR( LIS3MDL_Init->M_OutputDataRate ) != MAGNETO_OK) {
00077         return MAGNETO_ERROR;
00078     }
00079 
00080     if(LIS3MDL_M_Set_FS( LIS3MDL_Init->M_FullScale ) != MAGNETO_OK) {
00081         return MAGNETO_ERROR;
00082     }
00083 
00084     /* Configure interrupt lines */
00085     LIS3MDL_IO_ITConfig();
00086 
00087     return MAGNETO_OK;
00088 
00089     /******************************/
00090 }
00091 
00092 
00093 /**
00094  * @brief  Read ID of LIS3MDL Magnetic sensor
00095  * @param  m_id the pointer where the ID of the device is stored
00096  * @retval MAGNETO_OK in case of success, an error code otherwise
00097  */
00098 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Read_M_ID(uint8_t *m_id)
00099 {
00100     if(!m_id) {
00101         return MAGNETO_ERROR;
00102     }
00103 
00104     return LIS3MDL_IO_Read(m_id, LIS3MDL_M_WHO_AM_I_ADDR, 1);
00105 }
00106 
00107 
00108 /**
00109  * @brief  Read raw data from LIS3MDL Magnetic sensor output register
00110  * @param  pData the pointer where the magnetometer raw data are stored
00111  * @retval MAGNETO_OK in case of success, an error code otherwise
00112  */
00113 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxesRaw(int16_t *pData)
00114 {
00115     uint8_t tempReg[6] = {0, 0, 0, 0, 0, 0};
00116 
00117     if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_X_L_M), 6) != MAGNETO_OK) {
00118         return MAGNETO_ERROR;
00119     }
00120 
00121     pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00122 /*
00123     if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Y_L_M), 2) != MAGNETO_OK) {
00124         return MAGNETO_ERROR;
00125     }
00126 */
00127     pData[1] = ((((int16_t)tempReg[3]) << 8) + (int16_t)tempReg[2]);
00128 /*
00129     if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Z_L_M), 2) != MAGNETO_OK) {
00130         return MAGNETO_ERROR;
00131     }
00132 */
00133     pData[2] = ((((int16_t)tempReg[5]) << 8) + (int16_t)tempReg[4]);
00134 
00135     return MAGNETO_OK;
00136 }
00137 
00138 
00139 /**
00140  * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss
00141  * @param pData the pointer where the magnetometer data are stored
00142  * @retval MAGNETO_OK in case of success, an error code otherwise
00143  */
00144 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxes(int32_t *pData)
00145 {
00146     uint8_t tempReg = 0x00;
00147     int16_t pDataRaw[3];
00148     float sensitivity = 0;
00149 
00150     if(LIS3MDL_M_GetAxesRaw(pDataRaw) != MAGNETO_OK) {
00151         return MAGNETO_ERROR;
00152     }
00153 
00154     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00155         return MAGNETO_ERROR;
00156     }
00157 
00158     tempReg &= LIS3MDL_M_FS_MASK;
00159 
00160     switch(tempReg) {
00161         case LIS3MDL_M_FS_4:
00162             sensitivity = 0.14;
00163             break;
00164         case LIS3MDL_M_FS_8:
00165             sensitivity = 0.29;
00166             break;
00167         case LIS3MDL_M_FS_12:
00168             sensitivity = 0.43;
00169             break;
00170         case LIS3MDL_M_FS_16:
00171             sensitivity = 0.58;
00172             break;
00173     }
00174 
00175     pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00176     pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00177     pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00178 
00179     return MAGNETO_OK;
00180 }
00181 
00182 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Get_ODR( float *odr )
00183 {
00184     /*Here we have to add the check if the parameters are valid*/
00185     uint8_t tempReg = 0x00;
00186 
00187     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00188         return MAGNETO_ERROR;
00189     }
00190 
00191     tempReg &= LIS3MDL_M_FAST_ODR_MASK;
00192     if(tempReg == LIS3MDL_M_FAST_ODR_ENABLE) {
00193 
00194         if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00195             return MAGNETO_ERROR;
00196         }
00197         tempReg &= LIS3MDL_M_OM_MASK;
00198         switch(tempReg) {
00199             case LIS3MDL_M_OM_LP:
00200                 *odr = 1000.0f;
00201                 break;
00202             case LIS3MDL_M_OM_MP:
00203                 *odr = 560.0f;
00204                 break;
00205             case LIS3MDL_M_OM_HP:
00206                 *odr = 300.0f;
00207                 break;
00208             case LIS3MDL_M_OM_UHP:
00209                 *odr = 155.0f;
00210                 break;
00211         }
00212     } else {
00213         if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00214             return MAGNETO_ERROR;
00215         }
00216         tempReg &= LIS3MDL_M_DO_MASK;
00217         switch(tempReg) {
00218             case LIS3MDL_M_DO_0_625:
00219                 *odr = 0.625f;
00220                 break;
00221             case LIS3MDL_M_DO_1_25:
00222                 *odr = 1.25f;
00223                 break;
00224             case LIS3MDL_M_DO_2_5:
00225                 *odr = 2.5f;
00226                 break;
00227             case LIS3MDL_M_DO_5:
00228                 *odr = 5.0f;
00229                 break;
00230             case LIS3MDL_M_DO_10:
00231                 *odr = 10.0f;
00232                 break;
00233             case LIS3MDL_M_DO_20:
00234                 *odr = 20.0f;
00235                 break;
00236             case LIS3MDL_M_DO_40:
00237                 *odr = 40.0f;
00238                 break;
00239             case LIS3MDL_M_DO_80:
00240                 *odr = 80.0f;
00241                 break;
00242         }
00243     }
00244     return MAGNETO_OK;
00245 }
00246 
00247 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Set_ODR( float odr )
00248 {
00249     uint8_t new_odr = 0x00;
00250     uint8_t tempReg = 0x00;
00251 
00252 if(odr>80.0f){
00253     new_odr = ( odr <= 155.0f   ) ? LIS3MDL_M_OM_UHP
00254               : ( odr <= 300.0f  ) ? LIS3MDL_M_OM_HP
00255               : ( odr <= 560.0f  ) ? LIS3MDL_M_OM_MP
00256               :                      LIS3MDL_M_OM_LP;
00257               
00258     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00259         return MAGNETO_ERROR;
00260     }
00261 
00262     tempReg &= ~(LIS3MDL_M_OM_MASK);
00263     tempReg |= (new_odr|LIS3MDL_M_FAST_ODR_ENABLE);
00264     
00265     if(LIS3MDL_IO_Write(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00266         return MAGNETO_ERROR;
00267     }
00268     }
00269     else{
00270         
00271     new_odr = ( odr <= 0.625f   ) ? LIS3MDL_M_DO_0_625
00272               : ( odr <= 1.25f  ) ? LIS3MDL_M_DO_1_25
00273               : ( odr <= 2.5f  ) ? LIS3MDL_M_DO_2_5
00274               : ( odr <= 5.0f  ) ? LIS3MDL_M_DO_5
00275               : ( odr <= 10.0f ) ? LIS3MDL_M_DO_10
00276               : ( odr <= 20.0f ) ? LIS3MDL_M_DO_20
00277               : ( odr <= 40.0f ) ? LIS3MDL_M_DO_40
00278               :                    LIS3MDL_M_DO_80;
00279 
00280     if(LIS3MDL_IO_Read( &tempReg, LIS3MDL_M_CTRL_REG1_M, 1 ) != MAGNETO_OK) {
00281         return MAGNETO_ERROR;
00282     }
00283 
00284     tempReg &= ~(LIS3MDL_M_DO_MASK);
00285     tempReg |= new_odr;
00286 
00287     if(LIS3MDL_IO_Write(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00288         return MAGNETO_ERROR;
00289     }
00290 }
00291     return MAGNETO_OK;
00292 }
00293 
00294 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Get_FS( float *fullScale )
00295 {
00296     /*Here we have to add the check if the parameters are valid*/
00297     uint8_t tempReg = 0x00;
00298 
00299     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00300         return MAGNETO_ERROR;
00301     }
00302 
00303     tempReg &= LIS3MDL_M_FS_MASK;
00304 
00305     switch(tempReg) {
00306         case LIS3MDL_M_FS_4:
00307             *fullScale = 4.0f;
00308             break;
00309         case LIS3MDL_M_FS_8:
00310             *fullScale = 8.0f;
00311             break;
00312         case LIS3MDL_M_FS_12:
00313             *fullScale = 12.0f;
00314             break;
00315         case LIS3MDL_M_FS_16:
00316             *fullScale = 16.0f;
00317             break;
00318     }
00319 
00320     return MAGNETO_OK;
00321 }
00322 
00323 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Set_FS( float fullScale )
00324 {
00325     uint8_t new_fs = 0x00;
00326     uint8_t tempReg = 0x00;
00327 
00328     new_fs = ( fullScale <= 4.0f ) ? LIS3MDL_M_FS_4
00329              : ( fullScale <= 8.0f ) ? LIS3MDL_M_FS_8
00330              : ( fullScale <= 12.0f ) ? LIS3MDL_M_FS_12
00331              :                         LIS3MDL_M_FS_16;
00332 
00333     if(LIS3MDL_IO_Read( &tempReg, LIS3MDL_M_CTRL_REG2_M, 1 ) != MAGNETO_OK) {
00334         return MAGNETO_ERROR;
00335     }
00336 
00337     tempReg &= ~(LIS3MDL_M_FS_MASK);
00338     tempReg |= new_fs;
00339 
00340     if(LIS3MDL_IO_Write(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00341         return MAGNETO_ERROR;
00342     }
00343 
00344     return MAGNETO_OK;
00345 }
00346 
00347 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetSensitivity( float *pfData )
00348 {
00349     /*Here we have to add the check if the parameters are valid*/
00350 
00351     uint8_t tempReg = 0x00;
00352     if(LIS3MDL_IO_Read( &tempReg, LIS3MDL_M_CTRL_REG2_M, 1 ) != MAGNETO_OK) {
00353         return MAGNETO_ERROR;
00354     }
00355 
00356     tempReg &= LIS3MDL_M_FS_MASK;
00357 
00358     switch(tempReg) {
00359         case LIS3MDL_M_FS_4:
00360             *pfData = 0.14;
00361             break;
00362         case LIS3MDL_M_FS_8:
00363             *pfData = 0.29;
00364             break;
00365         case LIS3MDL_M_FS_12:
00366             *pfData = 0.43;
00367             break;
00368         case LIS3MDL_M_FS_16:
00369             *pfData = 0.58;
00370             break;
00371     }
00372     
00373     return MAGNETO_OK;
00374 }
00375 
00376 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_Reset_Regs()
00377 {
00378     uint8_t tmp = 0;
00379     
00380     if(LIS3MDL_IO_Read( &tmp, LIS3MDL_M_CTRL_REG2_M, 1 ) != MAGNETO_OK) {
00381         return MAGNETO_ERROR;
00382     }
00383     
00384     tmp &= LIS3MDL_M_SOFT_RST_MASK;
00385     tmp |= LIS3MDL_M_SOFT_RST_RESET;
00386     
00387     if(LIS3MDL_IO_Write( &tmp, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00388         return MAGNETO_ERROR;
00389     }
00390     
00391     return MAGNETO_OK;
00392 }
00393 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
00394