LSM303AGRMagSensor.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    LSM303AGRMagSensor.cpp
00004  * @author  CLab
00005  * @version V1.0.0
00006  * @date    5 August 2016
00007  * @brief   Implementation an LSM303AGR 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 "LSM303AGRMagSensor.h"
00044 #include "LSM303AGR_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 LSM303AGRMagSensor::LSM303AGRMagSensor(DevI2C &i2c) : _dev_i2c(i2c)
00054 {
00055   _address = LSM303AGR_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 LSM303AGRMagSensor::LSM303AGRMagSensor(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 LSM303AGRMagSensor::init(void *init)
00073 {
00074   /* Operating mode selection - power down */
00075   if ( LSM303AGR_MAG_W_MD( (void *)this, LSM303AGR_MAG_MD_IDLE1_MODE ) == MEMS_ERROR )
00076   {
00077     return 1;
00078   }
00079   
00080   /* Enable BDU */
00081   if ( LSM303AGR_MAG_W_BDU( (void *)this, LSM303AGR_MAG_BDU_ENABLED ) == MEMS_ERROR )
00082   {
00083     return 1;
00084   }
00085   
00086   if ( set_m_odr( 100.0f ) == 1 )
00087   {
00088     return 1;
00089   }
00090   
00091   if ( set_m_fs( 50.0f ) == 1 )
00092   {
00093     return 1;
00094   }
00095 
00096   if ( LSM303AGR_MAG_W_ST( (void *)this, LSM303AGR_MAG_ST_DISABLED ) == MEMS_ERROR )
00097   {
00098     return 1;
00099   }
00100   
00101   return 0;
00102 }
00103 
00104 /**
00105  * @brief  Enable LSM303AGR magnetometer
00106  * @retval 0 in case of success, an error code otherwise
00107  */
00108 int LSM303AGRMagSensor::enable(void)
00109 {
00110   /* Operating mode selection */
00111   if ( LSM303AGR_MAG_W_MD( (void *)this, LSM303AGR_MAG_MD_CONTINUOS_MODE ) == MEMS_ERROR )
00112   {
00113     return 1;
00114   }
00115   
00116   return 0;
00117 }
00118 
00119 /**
00120  * @brief  Disable LSM303AGR magnetometer
00121  * @retval 0 in case of success, an error code otherwise
00122  */
00123 int LSM303AGRMagSensor::disable(void)
00124 {
00125   /* Operating mode selection - power down */
00126   if ( LSM303AGR_MAG_W_MD( (void *)this, LSM303AGR_MAG_MD_IDLE1_MODE ) == MEMS_ERROR )
00127   {
00128     return 1;
00129   }
00130   
00131   return 0;
00132 }
00133 
00134 /**
00135  * @brief  Read ID of LSM303AGR 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 LSM303AGRMagSensor::read_id(uint8_t *id)
00140 {
00141   if(!id)
00142   { 
00143     return 1; 
00144   }
00145  
00146   /* Read WHO AM I register */
00147   if ( LSM303AGR_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 LSM303AGR 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 LSM303AGRMagSensor::get_m_axes(int32_t *pData)
00161 {
00162   int16_t pDataRaw[3];
00163   float sensitivity = 0;
00164   
00165   /* Read raw data from LSM303AGR output register. */
00166   if ( get_m_axes_raw( pDataRaw ) == 1 )
00167   {
00168     return 1;
00169   }
00170   
00171   /* Get LSM303AGR 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 LSM303AGRMagSensor::get_m_sensitivity(float *pfData)
00191 {
00192   *pfData = 1.5f;
00193   
00194   return 0;
00195 }
00196 
00197 /**
00198  * @brief  Read raw data from LSM303AGR 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 LSM303AGRMagSensor::get_m_axes_raw(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 LSM303AGR_MAG_OUTX_L to LSM303AGR_MAG_OUTZ_H. */
00208   if ( LSM303AGR_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 LSM303AGR 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 LSM303AGRMagSensor::get_m_odr(float* odr)
00229 {
00230   LSM303AGR_MAG_ODR_t odr_low_level;
00231   
00232   if ( LSM303AGR_MAG_R_ODR( (void *)this, &odr_low_level ) == MEMS_ERROR )
00233   {
00234     return 1;
00235   }
00236   
00237   switch( odr_low_level )
00238   {
00239     case LSM303AGR_MAG_ODR_10Hz:
00240       *odr = 10.000f;
00241       break;
00242     case LSM303AGR_MAG_ODR_20Hz:
00243       *odr = 20.000f;
00244       break;
00245     case LSM303AGR_MAG_ODR_50Hz:
00246       *odr = 50.000f;
00247       break;
00248     case LSM303AGR_MAG_ODR_100Hz:
00249       *odr = 100.000f;
00250       break;
00251     default:
00252       *odr = -1.000f;
00253       return 1;
00254   }  
00255   return 0;
00256 }
00257 
00258 /**
00259  * @brief  Set ODR
00260  * @param  odr the output data rate to be set
00261  * @retval 0 in case of success, an error code otherwise
00262  */
00263 int LSM303AGRMagSensor::set_m_odr(float odr)
00264 {
00265   LSM303AGR_MAG_ODR_t new_odr;
00266   
00267   new_odr = ( odr <= 10.000f ) ? LSM303AGR_MAG_ODR_10Hz
00268           : ( odr <= 20.000f ) ? LSM303AGR_MAG_ODR_20Hz
00269           : ( odr <= 50.000f ) ? LSM303AGR_MAG_ODR_50Hz
00270           :                      LSM303AGR_MAG_ODR_100Hz;
00271             
00272   if ( LSM303AGR_MAG_W_ODR( (void *)this, new_odr ) == MEMS_ERROR )
00273   {
00274     return 1;
00275   }
00276   
00277   return 0;
00278 }
00279 
00280 
00281 /**
00282  * @brief  Read LSM303AGR Magnetometer full scale
00283  * @param  fullScale the pointer to the output data rate
00284  * @retval 0 in case of success, an error code otherwise
00285  */
00286 int LSM303AGRMagSensor::get_m_fs(float* fullScale)
00287 {
00288   *fullScale = 50.0f;
00289   
00290   return 0;
00291 }
00292 
00293 /**
00294  * @brief  Set full scale
00295  * @param  fullScale the full scale to be set
00296  * @retval 0 in case of success, an error code otherwise
00297  */
00298 int LSM303AGRMagSensor::set_m_fs(float fullScale)
00299 {
00300   return 0;
00301 }
00302 
00303 
00304 /**
00305  * @brief Read magnetometer data from register
00306  * @param reg register address
00307  * @param data register data
00308  * @retval 0 in case of success
00309  * @retval 1 in case of failure
00310  */
00311 int LSM303AGRMagSensor::read_reg( uint8_t reg, uint8_t *data )
00312 {
00313   if ( LSM303AGR_MAG_read_reg( (void *)this, reg, data ) == MEMS_ERROR )
00314   {
00315     return 1;
00316   }
00317 
00318   return 0;
00319 }
00320 
00321 
00322 /**
00323  * @brief Write magnetometer data to register
00324  * @param reg register address
00325  * @param data register data
00326  * @retval 0 in case of success
00327  * @retval 1 in case of failure
00328  */
00329 int LSM303AGRMagSensor::write_reg( uint8_t reg, uint8_t data )
00330 {
00331   if ( LSM303AGR_MAG_write_reg( (void *)this, reg, data ) == MEMS_ERROR )
00332   {
00333     return 1;
00334   }
00335 
00336   return 0;
00337 }
00338 
00339 uint8_t LSM303AGR_MAG_io_write( void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite )
00340 {
00341   return ((LSM303AGRMagSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
00342 }
00343 
00344 uint8_t LSM303AGR_MAG_io_read( void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead )
00345 {
00346   return ((LSM303AGRMagSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
00347 }