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 #include "LSM303AGR_acc_driver.h"
00046 #include <assert.h>
00047 
00048 /* Class Implementation ------------------------------------------------------*/
00049 
00050 LSM303AGRMagSensor::LSM303AGRMagSensor(SPI3W *spi3w, PinName cs_pin, PinName intmag_pin) : 
00051                                        _dev_spi3w(spi3w), _cs_pin(cs_pin), _intmag_pin(intmag_pin)
00052 {
00053     assert (spi3w);
00054     if (cs_pin == NC) 
00055     {
00056         printf ("ERROR LSM303AGRMagSensor CS MUST NOT BE NC\n\r");      
00057         _dev_spi3w = NULL;
00058         _dev_i2c=NULL;
00059         return;
00060     }       
00061     _cs_pin = 0;     // enable SPI3W disable I2C
00062   _dev_i2c=NULL;    
00063 
00064     LSM303AGR_ACC_W_SPI_mode((void *)this, LSM303AGR_ACC_SIM_3_WIRES);
00065 };
00066 
00067 
00068 /** Constructor
00069  * @param i2c object of an helper class which handles the I2C peripheral
00070  * @param address the address of the component's instance
00071  */
00072 LSM303AGRMagSensor::LSM303AGRMagSensor(DevI2C *i2c, uint8_t address, PinName intmag_pin) : 
00073                                        _dev_i2c(i2c), _address(address), _cs_pin(NC), _intmag_pin(intmag_pin)
00074 {
00075     assert (i2c);
00076     _dev_spi3w = NULL;  
00077 };
00078 
00079 /**
00080  * @brief     Initializing the component.
00081  * @param[in] init pointer to device specific initalization structure.
00082  * @retval    "0" in case of success, an error code otherwise.
00083  */
00084 int LSM303AGRMagSensor::init(void *init)
00085 {
00086   /* Operating mode selection - power down */
00087   if ( LSM303AGR_MAG_W_MD( (void *)this, LSM303AGR_MAG_MD_IDLE1_MODE ) == MEMS_ERROR )
00088   {
00089     return 1;
00090   }
00091   
00092   /* Enable BDU */
00093   if ( LSM303AGR_MAG_W_BDU( (void *)this, LSM303AGR_MAG_BDU_ENABLED ) == MEMS_ERROR )
00094   {
00095     return 1;
00096   }
00097   
00098   if ( set_m_odr( 100.0f ) == 1 )
00099   {
00100     return 1;
00101   }
00102   
00103   if ( set_m_fs( 50.0f ) == 1 )
00104   {
00105     return 1;
00106   }
00107 
00108   if ( LSM303AGR_MAG_W_ST( (void *)this, LSM303AGR_MAG_ST_DISABLED ) == MEMS_ERROR )
00109   {
00110     return 1;
00111   }
00112   
00113   return 0;
00114 }
00115 
00116 /**
00117  * @brief  Enable LSM303AGR magnetometer
00118  * @retval 0 in case of success, an error code otherwise
00119  */
00120 int LSM303AGRMagSensor::enable(void)
00121 {
00122   /* Operating mode selection */
00123   if ( LSM303AGR_MAG_W_MD( (void *)this, LSM303AGR_MAG_MD_CONTINUOS_MODE ) == MEMS_ERROR )
00124   {
00125     return 1;
00126   }
00127   
00128   return 0;
00129 }
00130 
00131 /**
00132  * @brief  Disable LSM303AGR magnetometer
00133  * @retval 0 in case of success, an error code otherwise
00134  */
00135 int LSM303AGRMagSensor::disable(void)
00136 {
00137   /* Operating mode selection - power down */
00138   if ( LSM303AGR_MAG_W_MD( (void *)this, LSM303AGR_MAG_MD_IDLE1_MODE ) == MEMS_ERROR )
00139   {
00140     return 1;
00141   }
00142   
00143   return 0;
00144 }
00145 
00146 /**
00147  * @brief  Read ID of LSM303AGR Magnetometer
00148  * @param  p_id the pointer where the ID of the device is stored
00149  * @retval 0 in case of success, an error code otherwise
00150  */
00151 int LSM303AGRMagSensor::read_id(uint8_t *id)
00152 {
00153   if(!id)
00154   { 
00155     return 1; 
00156   }
00157  
00158   /* Read WHO AM I register */
00159   if ( LSM303AGR_MAG_R_WHO_AM_I( (void *)this, id ) == MEMS_ERROR )
00160   {
00161     return 1;
00162   }
00163   
00164   return 0;
00165 }
00166 
00167 /**
00168  * @brief  Read data from LSM303AGR Magnetometer
00169  * @param  pData the pointer where the magnetometer data are stored
00170  * @retval 0 in case of success, an error code otherwise
00171  */
00172 int LSM303AGRMagSensor::get_m_axes(int32_t *pData)
00173 {
00174   int16_t pDataRaw[3];
00175   float sensitivity = 0;
00176   
00177   /* Read raw data from LSM303AGR output register. */
00178   if ( get_m_axes_raw( pDataRaw ) == 1 )
00179   {
00180     return 1;
00181   }
00182   
00183   /* Get LSM303AGR actual sensitivity. */
00184   if ( get_m_sensitivity( &sensitivity ) == 1 )
00185   {
00186     return 1;
00187   }
00188   
00189   /* Calculate the data. */
00190   pData[0] = ( int32_t )( pDataRaw[0] * sensitivity );
00191   pData[1] = ( int32_t )( pDataRaw[1] * sensitivity );
00192   pData[2] = ( int32_t )( pDataRaw[2] * sensitivity );
00193   
00194   return 0;
00195 }
00196 
00197 /**
00198  * @brief  Read Magnetometer Sensitivity
00199  * @param  pfData the pointer where the magnetometer sensitivity is stored
00200  * @retval 0 in case of success, an error code otherwise
00201  */
00202 int LSM303AGRMagSensor::get_m_sensitivity(float *pfData)
00203 {
00204   *pfData = 1.5f;
00205   
00206   return 0;
00207 }
00208 
00209 /**
00210  * @brief  Read raw data from LSM303AGR Magnetometer
00211  * @param  pData the pointer where the magnetomer raw data are stored
00212  * @retval 0 in case of success, an error code otherwise
00213  */
00214 int LSM303AGRMagSensor::get_m_axes_raw(int16_t *pData)
00215 {
00216   uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};
00217   int16_t *regValueInt16;
00218   
00219   /* Read output registers from LSM303AGR_MAG_OUTX_L to LSM303AGR_MAG_OUTZ_H. */
00220   if ( LSM303AGR_MAG_Get_Raw_Magnetic( (void *)this, regValue ) == MEMS_ERROR )
00221   {
00222     return 1;
00223   }
00224   
00225   regValueInt16 = (int16_t *)regValue;
00226   
00227   /* Format the data. */
00228   pData[0] = regValueInt16[0];
00229   pData[1] = regValueInt16[1];
00230   pData[2] = regValueInt16[2];
00231   
00232   return 0;
00233 }
00234 
00235 /**
00236  * @brief  Read LSM303AGR Magnetometer output data rate
00237  * @param  odr the pointer to the output data rate
00238  * @retval 0 in case of success, an error code otherwise
00239  */
00240 int LSM303AGRMagSensor::get_m_odr(float* odr)
00241 {
00242   LSM303AGR_MAG_ODR_t odr_low_level;
00243   
00244   if ( LSM303AGR_MAG_R_ODR( (void *)this, &odr_low_level ) == MEMS_ERROR )
00245   {
00246     return 1;
00247   }
00248   
00249   switch( odr_low_level )
00250   {
00251     case LSM303AGR_MAG_ODR_10Hz:
00252       *odr = 10.000f;
00253       break;
00254     case LSM303AGR_MAG_ODR_20Hz:
00255       *odr = 20.000f;
00256       break;
00257     case LSM303AGR_MAG_ODR_50Hz:
00258       *odr = 50.000f;
00259       break;
00260     case LSM303AGR_MAG_ODR_100Hz:
00261       *odr = 100.000f;
00262       break;
00263     default:
00264       *odr = -1.000f;
00265       return 1;
00266   }  
00267   return 0;
00268 }
00269 
00270 /**
00271  * @brief  Set ODR
00272  * @param  odr the output data rate to be set
00273  * @retval 0 in case of success, an error code otherwise
00274  */
00275 int LSM303AGRMagSensor::set_m_odr(float odr)
00276 {
00277   LSM303AGR_MAG_ODR_t new_odr;
00278   
00279   new_odr = ( odr <= 10.000f ) ? LSM303AGR_MAG_ODR_10Hz
00280           : ( odr <= 20.000f ) ? LSM303AGR_MAG_ODR_20Hz
00281           : ( odr <= 50.000f ) ? LSM303AGR_MAG_ODR_50Hz
00282           :                      LSM303AGR_MAG_ODR_100Hz;
00283             
00284   if ( LSM303AGR_MAG_W_ODR( (void *)this, new_odr ) == MEMS_ERROR )
00285   {
00286     return 1;
00287   }
00288   
00289   return 0;
00290 }
00291 
00292 
00293 /**
00294  * @brief  Read LSM303AGR Magnetometer full scale
00295  * @param  fullScale the pointer to the output data rate
00296  * @retval 0 in case of success, an error code otherwise
00297  */
00298 int LSM303AGRMagSensor::get_m_fs(float* fullScale)
00299 {
00300   *fullScale = 50.0f;
00301   
00302   return 0;
00303 }
00304 
00305 /**
00306  * @brief  Set full scale
00307  * @param  fullScale the full scale to be set
00308  * @retval 0 in case of success, an error code otherwise
00309  */
00310 int LSM303AGRMagSensor::set_m_fs(float fullScale)
00311 {
00312   return 0;
00313 }
00314 
00315 
00316 /**
00317  * @brief Read magnetometer data from register
00318  * @param reg register address
00319  * @param data register data
00320  * @retval 0 in case of success
00321  * @retval 1 in case of failure
00322  */
00323 int LSM303AGRMagSensor::read_reg( uint8_t reg, uint8_t *data )
00324 {
00325   if ( LSM303AGR_MAG_read_reg( (void *)this, reg, data ) == MEMS_ERROR )
00326   {
00327     return 1;
00328   }
00329 
00330   return 0;
00331 }
00332 
00333 
00334 /**
00335  * @brief Write magnetometer data to register
00336  * @param reg register address
00337  * @param data register data
00338  * @retval 0 in case of success
00339  * @retval 1 in case of failure
00340  */
00341 int LSM303AGRMagSensor::write_reg( uint8_t reg, uint8_t data )
00342 {
00343   if ( LSM303AGR_MAG_write_reg( (void *)this, reg, data ) == MEMS_ERROR )
00344   {
00345     return 1;
00346   }
00347 
00348   return 0;
00349 }
00350 
00351 uint8_t LSM303AGR_MAG_io_write( void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite )
00352 {
00353   return ((LSM303AGRMagSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
00354 }
00355 
00356 uint8_t LSM303AGR_MAG_io_read( void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead )
00357 {
00358   return ((LSM303AGRMagSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
00359 }