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[2] = {0, 0};
00116 
00117     if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_X_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00118                        2) != MAGNETO_OK) {
00119         return MAGNETO_ERROR;
00120     }
00121 
00122     pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00123 
00124     if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Y_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00125                        2) != MAGNETO_OK) {
00126         return MAGNETO_ERROR;
00127     }
00128 
00129     pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00130 
00131     if(LIS3MDL_IO_Read(&tempReg[0], (LIS3MDL_M_OUT_Z_L_M | LIS3MDL_I2C_MULTIPLEBYTE_CMD),
00132                        2) != MAGNETO_OK) {
00133         return MAGNETO_ERROR;
00134     }
00135 
00136     pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00137 
00138     return MAGNETO_OK;
00139 }
00140 
00141 
00142 /**
00143  * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss
00144  * @param pData the pointer where the magnetometer data are stored
00145  * @retval MAGNETO_OK in case of success, an error code otherwise
00146  */
00147 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetAxes(int32_t *pData)
00148 {
00149     uint8_t tempReg = 0x00;
00150     int16_t pDataRaw[3];
00151     float sensitivity = 0;
00152 
00153     if(LIS3MDL_M_GetAxesRaw(pDataRaw) != MAGNETO_OK) {
00154         return MAGNETO_ERROR;
00155     }
00156 
00157     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00158         return MAGNETO_ERROR;
00159     }
00160 
00161     tempReg &= LIS3MDL_M_FS_MASK;
00162 
00163     switch(tempReg) {
00164         case LIS3MDL_M_FS_4:
00165             sensitivity = 0.14;
00166             break;
00167         case LIS3MDL_M_FS_8:
00168             sensitivity = 0.29;
00169             break;
00170         case LIS3MDL_M_FS_12:
00171             sensitivity = 0.43;
00172             break;
00173         case LIS3MDL_M_FS_16:
00174             sensitivity = 0.58;
00175             break;
00176     }
00177 
00178     pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00179     pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00180     pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00181 
00182     return MAGNETO_OK;
00183 }
00184 
00185 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Get_ODR( float *odr )
00186 {
00187     /*Here we have to add the check if the parameters are valid*/
00188     uint8_t tempReg = 0x00;
00189 
00190     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00191         return MAGNETO_ERROR;
00192     }
00193 
00194     tempReg &= LIS3MDL_M_FAST_ODR_MASK;
00195     if(tempReg == LIS3MDL_M_FAST_ODR_ENABLE) {
00196 
00197         if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00198             return MAGNETO_ERROR;
00199         }
00200         tempReg &= LIS3MDL_M_OM_MASK;
00201         switch(tempReg) {
00202             case LIS3MDL_M_OM_LP:
00203                 *odr = 1000.0f;
00204                 break;
00205             case LIS3MDL_M_OM_MP:
00206                 *odr = 560.0f;
00207                 break;
00208             case LIS3MDL_M_OM_HP:
00209                 *odr = 300.0f;
00210                 break;
00211             case LIS3MDL_M_OM_UHP:
00212                 *odr = 155.0f;
00213                 break;
00214         }
00215     } else {
00216         if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00217             return MAGNETO_ERROR;
00218         }
00219         tempReg &= LIS3MDL_M_DO_MASK;
00220         switch(tempReg) {
00221             case LIS3MDL_M_DO_0_625:
00222                 *odr = 0.625f;
00223                 break;
00224             case LIS3MDL_M_DO_1_25:
00225                 *odr = 1.25f;
00226                 break;
00227             case LIS3MDL_M_DO_2_5:
00228                 *odr = 2.5f;
00229                 break;
00230             case LIS3MDL_M_DO_5:
00231                 *odr = 5.0f;
00232                 break;
00233             case LIS3MDL_M_DO_10:
00234                 *odr = 10.0f;
00235                 break;
00236             case LIS3MDL_M_DO_20:
00237                 *odr = 20.0f;
00238                 break;
00239             case LIS3MDL_M_DO_40:
00240                 *odr = 40.0f;
00241                 break;
00242             case LIS3MDL_M_DO_80:
00243                 *odr = 80.0f;
00244                 break;
00245         }
00246     }
00247     return MAGNETO_OK;
00248 }
00249 
00250 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Set_ODR( float odr )
00251 {
00252     uint8_t new_odr = 0x00;
00253     uint8_t tempReg = 0x00;
00254 
00255 if(odr>80.0f){
00256     new_odr = ( odr <= 155.0f   ) ? LIS3MDL_M_OM_UHP
00257               : ( odr <= 300.0f  ) ? LIS3MDL_M_OM_HP
00258               : ( odr <= 560.0f  ) ? LIS3MDL_M_OM_MP
00259               :                      LIS3MDL_M_OM_LP;
00260               
00261     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00262         return MAGNETO_ERROR;
00263     }
00264 
00265     tempReg &= ~(LIS3MDL_M_OM_MASK);
00266     tempReg |= (new_odr|LIS3MDL_M_FAST_ODR_ENABLE);
00267     
00268     if(LIS3MDL_IO_Write(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00269         return MAGNETO_ERROR;
00270     }
00271     }
00272     else{
00273         
00274     new_odr = ( odr <= 0.625f   ) ? LIS3MDL_M_DO_0_625
00275               : ( odr <= 1.25f  ) ? LIS3MDL_M_DO_1_25
00276               : ( odr <= 2.5f  ) ? LIS3MDL_M_DO_2_5
00277               : ( odr <= 5.0f  ) ? LIS3MDL_M_DO_5
00278               : ( odr <= 10.0f ) ? LIS3MDL_M_DO_10
00279               : ( odr <= 20.0f ) ? LIS3MDL_M_DO_20
00280               : ( odr <= 40.0f ) ? LIS3MDL_M_DO_40
00281               :                    LIS3MDL_M_DO_80;
00282 
00283     if(LIS3MDL_IO_Read( &tempReg, LIS3MDL_M_CTRL_REG1_M, 1 ) != MAGNETO_OK) {
00284         return MAGNETO_ERROR;
00285     }
00286 
00287     tempReg &= ~(LIS3MDL_M_DO_MASK);
00288     tempReg |= new_odr;
00289 
00290     if(LIS3MDL_IO_Write(&tempReg, LIS3MDL_M_CTRL_REG1_M, 1) != MAGNETO_OK) {
00291         return MAGNETO_ERROR;
00292     }
00293 }
00294     return MAGNETO_OK;
00295 }
00296 
00297 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Get_FS( float *fullScale )
00298 {
00299     /*Here we have to add the check if the parameters are valid*/
00300     uint8_t tempReg = 0x00;
00301 
00302     if(LIS3MDL_IO_Read(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00303         return MAGNETO_ERROR;
00304     }
00305 
00306     tempReg &= LIS3MDL_M_FS_MASK;
00307 
00308     switch(tempReg) {
00309         case LIS3MDL_M_FS_4:
00310             *fullScale = 4.0f;
00311             break;
00312         case LIS3MDL_M_FS_8:
00313             *fullScale = 8.0f;
00314             break;
00315         case LIS3MDL_M_FS_12:
00316             *fullScale = 12.0f;
00317             break;
00318         case LIS3MDL_M_FS_16:
00319             *fullScale = 16.0f;
00320             break;
00321     }
00322 
00323     return MAGNETO_OK;
00324 }
00325 
00326 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_Set_FS( float fullScale )
00327 {
00328     uint8_t new_fs = 0x00;
00329     uint8_t tempReg = 0x00;
00330 
00331     new_fs = ( fullScale <= 4.0f ) ? LIS3MDL_M_FS_4
00332              : ( fullScale <= 8.0f ) ? LIS3MDL_M_FS_8
00333              : ( fullScale <= 12.0f ) ? LIS3MDL_M_FS_12
00334              :                         LIS3MDL_M_FS_16;
00335 
00336     if(LIS3MDL_IO_Read( &tempReg, LIS3MDL_M_CTRL_REG2_M, 1 ) != MAGNETO_OK) {
00337         return MAGNETO_ERROR;
00338     }
00339 
00340     tempReg &= ~(LIS3MDL_M_FS_MASK);
00341     tempReg |= new_fs;
00342 
00343     if(LIS3MDL_IO_Write(&tempReg, LIS3MDL_M_CTRL_REG2_M, 1) != MAGNETO_OK) {
00344         return MAGNETO_ERROR;
00345     }
00346 
00347     return MAGNETO_OK;
00348 }
00349 
00350 MAGNETO_StatusTypeDef LIS3MDL::LIS3MDL_M_GetSensitivity( float *pfData )
00351 {
00352     /*Here we have to add the check if the parameters are valid*/
00353 
00354     uint8_t tempReg = 0x00;
00355     if(LIS3MDL_IO_Read( &tempReg, LIS3MDL_M_CTRL_REG2_M, 1 ) != MAGNETO_OK) {
00356         return MAGNETO_ERROR;
00357     }
00358 
00359     tempReg &= LIS3MDL_M_FS_MASK;
00360 
00361     switch(tempReg) {
00362         case LIS3MDL_M_FS_4:
00363             *pfData = 0.14;
00364             break;
00365         case LIS3MDL_M_FS_8:
00366             *pfData = 0.29;
00367             break;
00368         case LIS3MDL_M_FS_12:
00369             *pfData = 0.43;
00370             break;
00371         case LIS3MDL_M_FS_16:
00372             *pfData = 0.58;
00373             break;
00374     }
00375     
00376     return MAGNETO_OK;
00377 }
00378 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/