LSM303C_MAG_Sensor.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    LSM303C_MAG_Sensor.cpp
00004  * @author  CLab
00005  * @version V1.0.0
00006  * @date    5 August 2016
00007  * @brief   Implementation an LSM303C magnetometer sensor.
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2016 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 
00039 /* Includes ------------------------------------------------------------------*/
00040 
00041 #include "mbed.h"
00042 #include "DevI2C.h"
00043 #include "LSM303C_MAG_Sensor.h"
00044 #include "LSM303C_MAG_driver.h"
00045 
00046 
00047 /* Class Implementation ------------------------------------------------------*/
00048 
00049 /** Constructor
00050  * @param i2c object of an helper class which handles the I2C peripheral
00051  * @param address the address of the component's instance
00052  */
00053 LSM303C_MAG_Sensor::LSM303C_MAG_Sensor(DevI2C &i2c) : dev_i2c(i2c)
00054 {
00055   address = LSM303C_MAG_I2C_ADDRESS;
00056 };
00057 
00058 /** Constructor
00059  * @param i2c object of an helper class which handles the I2C peripheral
00060  * @param address the address of the component's instance
00061  */
00062 LSM303C_MAG_Sensor::LSM303C_MAG_Sensor(DevI2C &i2c, uint8_t address) : dev_i2c(i2c), address(address)
00063 {
00064 
00065 };
00066 
00067 /**
00068  * @brief     Initializing the component.
00069  * @param[in] init pointer to device specific initalization structure.
00070  * @retval    "0" in case of success, an error code otherwise.
00071  */
00072 int LSM303C_MAG_Sensor::Init(void *init)
00073 {
00074   /* Operating mode selection - power down */
00075   if ( LSM303C_MAG_W_MD( (void *)this, LSM303C_MAG_MD_IDLE1_MODE ) == MEMS_ERROR )
00076   {
00077     return 1;
00078   }
00079   
00080   /* Enable BDU */
00081   if ( LSM303C_MAG_W_BDU( (void *)this, LSM303C_MAG_BDU_ENABLED ) == MEMS_ERROR )
00082   {
00083     return 1;
00084   }
00085   
00086   if ( Set_M_ODR( 80.0f ) == 1 )
00087   {
00088     return 1;
00089   }
00090   
00091   if ( Set_M_FS( 16.0f ) == 1 )
00092   {
00093     return 1;
00094   }
00095 
00096   if ( LSM303C_MAG_W_ST( (void *)this, LSM303C_MAG_ST_DISABLED ) == MEMS_ERROR )
00097   {
00098     return 1;
00099   }
00100   
00101   return 0;
00102 }
00103 
00104 /**
00105  * @brief  Enable LSM303C magnetometer
00106  * @retval 0 in case of success, an error code otherwise
00107  */
00108 int LSM303C_MAG_Sensor::Enable(void)
00109 {
00110   /* Operating mode selection */
00111   if ( LSM303C_MAG_W_MD( (void *)this, LSM303C_MAG_MD_CONTINUOS_MODE ) == MEMS_ERROR )
00112   {
00113     return 1;
00114   }
00115   
00116   return 0;
00117 }
00118 
00119 /**
00120  * @brief  Disable LSM303C magnetometer
00121  * @retval 0 in case of success, an error code otherwise
00122  */
00123 int LSM303C_MAG_Sensor::Disable(void)
00124 {
00125   /* Operating mode selection - power down */
00126   if ( LSM303C_MAG_W_MD( (void *)this, LSM303C_MAG_MD_IDLE1_MODE ) == MEMS_ERROR )
00127   {
00128     return 1;
00129   }
00130   
00131   return 0;
00132 }
00133 
00134 /**
00135  * @brief  Read ID of LSM303C Magnetometer
00136  * @param  p_id the pointer where the ID of the device is stored
00137  * @retval 0 in case of success, an error code otherwise
00138  */
00139 int LSM303C_MAG_Sensor::ReadID(uint8_t *id)
00140 {
00141   if(!id)
00142   { 
00143     return 1; 
00144   }
00145  
00146   /* Read WHO AM I register */
00147   if ( LSM303C_MAG_R_WHO_AM_I( (void *)this, id ) == MEMS_ERROR )
00148   {
00149     return 1;
00150   }
00151   
00152   return 0;
00153 }
00154 
00155 /**
00156  * @brief  Read data from LSM303C Magnetometer
00157  * @param  pData the pointer where the magnetometer data are stored
00158  * @retval 0 in case of success, an error code otherwise
00159  */
00160 int LSM303C_MAG_Sensor::Get_M_Axes(int32_t *pData)
00161 {
00162   int16_t pDataRaw[3];
00163   float sensitivity = 0;
00164   
00165   /* Read raw data from LSM303C output register. */
00166   if ( Get_M_AxesRaw( pDataRaw ) == 1 )
00167   {
00168     return 1;
00169   }
00170   
00171   /* Get LSM303C actual sensitivity. */
00172   if ( Get_M_Sensitivity( &sensitivity ) == 1 )
00173   {
00174     return 1;
00175   }
00176   
00177   /* Calculate the data. */
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 0;
00183 }
00184 
00185 /**
00186  * @brief  Read Magnetometer Sensitivity
00187  * @param  pfData the pointer where the magnetometer sensitivity is stored
00188  * @retval 0 in case of success, an error code otherwise
00189  */
00190 int LSM303C_MAG_Sensor::Get_M_Sensitivity(float *pfData)
00191 {
00192   *pfData = (float)1000.0/2048.0;
00193   
00194   return 0;
00195 }
00196 
00197 /**
00198  * @brief  Read raw data from LSM303C Magnetometer
00199  * @param  pData the pointer where the magnetomer raw data are stored
00200  * @retval 0 in case of success, an error code otherwise
00201  */
00202 int LSM303C_MAG_Sensor::Get_M_AxesRaw(int16_t *pData)
00203 {
00204   uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};
00205   int16_t *regValueInt16;
00206   
00207   /* Read output registers from LSM303C_MAG_OUTX_L to LSM303C_MAG_OUTZ_H. */
00208   if ( LSM303C_MAG_Get_Raw_Magnetic( (void *)this, regValue ) == MEMS_ERROR )
00209   {
00210     return 1;
00211   }
00212   
00213   regValueInt16 = (int16_t *)regValue;
00214   
00215   /* Format the data. */
00216   pData[0] = regValueInt16[0];
00217   pData[1] = regValueInt16[1];
00218   pData[2] = regValueInt16[2];
00219   
00220   return 0;
00221 }
00222 
00223 /**
00224  * @brief  Read LSM303C Magnetometer output data rate
00225  * @param  odr the pointer to the output data rate
00226  * @retval 0 in case of success, an error code otherwise
00227  */
00228 int LSM303C_MAG_Sensor::Get_M_ODR(float* odr)
00229 {
00230   LSM303C_MAG_ODR_t odr_low_level;
00231   
00232   if ( LSM303C_MAG_R_ODR( (void *)this, &odr_low_level ) == MEMS_ERROR )
00233   {
00234     return 1;
00235   }
00236   
00237   switch( odr_low_level )
00238   {
00239     case LSM303C_MAG_ODR_0_625Hz:
00240       *odr = 0.625f;
00241       break;
00242     case LSM303C_MAG_ODR_1_25Hz:
00243       *odr = 1.250f;
00244       break;
00245     case LSM303C_MAG_ODR_2_5Hz:
00246       *odr = 2.500f;
00247       break;
00248     case LSM303C_MAG_ODR_5Hz:
00249       *odr = 5.000f;
00250       break;
00251     case LSM303C_MAG_ODR_10Hz:
00252       *odr = 10.000f;
00253       break;
00254     case LSM303C_MAG_ODR_20Hz:
00255       *odr = 20.000f;
00256       break;
00257     case LSM303C_MAG_ODR_40Hz:
00258       *odr = 40.000f;
00259       break;
00260     case LSM303C_MAG_ODR_80Hz:
00261       *odr = 80.000f;
00262       break;
00263 
00264     default:
00265       *odr = -1.000f;
00266       return 1;
00267   }  
00268   return 0;
00269 }
00270 
00271 /**
00272  * @brief  Set ODR
00273  * @param  odr the output data rate to be set
00274  * @retval 0 in case of success, an error code otherwise
00275  */
00276 int LSM303C_MAG_Sensor::Set_M_ODR(float odr)
00277 {
00278   LSM303C_MAG_ODR_t new_odr;
00279   
00280   new_odr = ( odr <= 0.625f ) ? LSM303C_MAG_ODR_0_625Hz
00281           : ( odr <= 1.250f ) ? LSM303C_MAG_ODR_1_25Hz
00282           : ( odr <= 2.500f ) ? LSM303C_MAG_ODR_2_5Hz
00283           : ( odr <= 5.000f ) ? LSM303C_MAG_ODR_5Hz
00284           : ( odr <= 10.000f ) ? LSM303C_MAG_ODR_10Hz
00285           : ( odr <= 20.000f ) ? LSM303C_MAG_ODR_20Hz
00286           : ( odr <= 40.000f ) ? LSM303C_MAG_ODR_40Hz
00287           :                      LSM303C_MAG_ODR_80Hz;
00288             
00289   if ( LSM303C_MAG_W_ODR( (void *)this, new_odr ) == MEMS_ERROR )
00290   {
00291     return 1;
00292   }
00293   
00294   return 0;
00295 }
00296 
00297 
00298 /**
00299  * @brief  Read LSM303C Magnetometer full scale
00300  * @param  fullScale the pointer to the output data rate
00301  * @retval 0 in case of success, an error code otherwise
00302  */
00303 int LSM303C_MAG_Sensor::Get_M_FS(float* fullScale)
00304 {
00305   *fullScale = 16.0f;
00306   
00307   return 0;
00308 }
00309 
00310 /**
00311  * @brief  Set full scale
00312  * @param  fullScale the full scale to be set
00313  * @retval 0 in case of success, an error code otherwise
00314  */
00315 int LSM303C_MAG_Sensor::Set_M_FS(float fullScale)
00316 {
00317   return 0;
00318 }
00319 
00320 
00321 /**
00322  * @brief Read magnetometer data from register
00323  * @param reg register address
00324  * @param data register data
00325  * @retval 0 in case of success
00326  * @retval 1 in case of failure
00327  */
00328 int LSM303C_MAG_Sensor::ReadReg( uint8_t reg, uint8_t *data )
00329 {
00330   if ( LSM303C_MAG_ReadReg( (void *)this, reg, data ) == MEMS_ERROR )
00331   {
00332     return 1;
00333   }
00334 
00335   return 0;
00336 }
00337 
00338 
00339 /**
00340  * @brief Write magnetometer data to register
00341  * @param reg register address
00342  * @param data register data
00343  * @retval 0 in case of success
00344  * @retval 1 in case of failure
00345  */
00346 int LSM303C_MAG_Sensor::WriteReg( uint8_t reg, uint8_t data )
00347 {
00348   if ( LSM303C_MAG_WriteReg( (void *)this, reg, data ) == MEMS_ERROR )
00349   {
00350     return 1;
00351   }
00352 
00353   return 0;
00354 }
00355 
00356 uint8_t LSM303C_MAG_IO_Write( void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite )
00357 {
00358   return ((LSM303C_MAG_Sensor *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite);
00359 }
00360 
00361 uint8_t LSM303C_MAG_IO_Read( void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead )
00362 {
00363   return ((LSM303C_MAG_Sensor *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead);
00364 }