LSM303C_ACC_Sensor.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    LSM303C_ACC_Sensor.cpp
00004  * @author  CLab
00005  * @version V1.0.0
00006  * @date    5 August 2016
00007  * @brief   Implementation an LSM303C accelerometer 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 "DevI2C.h"
00042 #include "LSM303C_ACC_Sensor.h"
00043 #include "LSM303C_ACC_driver.h"
00044 
00045 
00046 /* Class Implementation ------------------------------------------------------*/
00047 
00048 /** Constructor
00049  * @param i2c object of an helper class which handles the I2C peripheral
00050  * @param address the address of the component's instance
00051  */
00052 LSM303C_ACC_Sensor::LSM303C_ACC_Sensor(DevI2C &i2c) : dev_i2c(i2c)
00053 {
00054   address = LSM303C_ACC_I2C_ADDRESS;
00055 };
00056 
00057 /** Constructor
00058  * @param i2c object of an helper class which handles the I2C peripheral
00059  * @param address the address of the component's instance
00060  */
00061 LSM303C_ACC_Sensor::LSM303C_ACC_Sensor(DevI2C &i2c, uint8_t address) : dev_i2c(i2c), address(address)
00062 {
00063 
00064 };
00065 
00066 /**
00067  * @brief     Initializing the component.
00068  * @param[in] init pointer to device specific initalization structure.
00069  * @retval    "0" in case of success, an error code otherwise.
00070  */
00071 int LSM303C_ACC_Sensor::Init(void *init)
00072 {
00073   /* Enable BDU */
00074   if ( LSM303C_ACC_W_BlockDataUpdate( (void *)this, LSM303C_ACC_BDU_ENABLED ) == MEMS_ERROR )
00075   {
00076     return 1;
00077   }
00078   
00079   /* Output data rate selection - power down. */
00080   if ( LSM303C_ACC_W_ODR( (void *)this, LSM303C_ACC_ODR_DO_PWR_DOWN ) == MEMS_ERROR )
00081   {
00082     return 1;
00083   }
00084   
00085   /* Full scale selection. */
00086   if ( Set_X_FS( 2.0f ) == 1 )
00087   {
00088     return 1;
00089   }
00090   
00091   /* Enable axes. */
00092   if ( LSM303C_ACC_W_XEN( (void *)this, LSM303C_ACC_XEN_ENABLED ) == MEMS_ERROR )
00093   {
00094     return 1;
00095   }
00096   
00097   if ( LSM303C_ACC_W_YEN ( (void *)this, LSM303C_ACC_YEN_ENABLED ) == MEMS_ERROR )
00098   {
00099     return 1;
00100   }
00101   
00102   if ( LSM303C_ACC_W_ZEN ( (void *)this, LSM303C_ACC_ZEN_ENABLED ) == MEMS_ERROR )
00103   {
00104     return 1;
00105   }
00106   
00107   /* Select default output data rate. */
00108   Last_ODR = 100.0f;
00109   
00110   isEnabled = 0;
00111   
00112   return 0;
00113 }
00114 
00115 /**
00116  * @brief  Enable LSM303C Accelerator
00117  * @retval 0 in case of success, an error code otherwise
00118  */
00119 int LSM303C_ACC_Sensor::Enable(void)
00120 { 
00121   /* Check if the component is already enabled */
00122   if ( isEnabled == 1 )
00123   {
00124     return 0;
00125   }
00126   
00127   /* Output data rate selection. */
00128   if ( Set_X_ODR_When_Enabled( Last_ODR ) == 1 )
00129   {
00130     return 1;
00131   }
00132   
00133   isEnabled = 1;
00134   
00135   return 0;
00136 }
00137 
00138 /**
00139  * @brief  Disable LSM303C Accelerator
00140  * @retval 0 in case of success, an error code otherwise
00141  */
00142 int LSM303C_ACC_Sensor::Disable(void)
00143 { 
00144   /* Check if the component is already disabled */
00145   if ( isEnabled == 0 )
00146   {
00147     return 0;
00148   }
00149   
00150   /* Store actual output data rate. */
00151   if ( Get_X_ODR( &Last_ODR ) == 1 )
00152   {
00153     return 1;
00154   }
00155   
00156   /* Output data rate selection - power down. */
00157   if ( LSM303C_ACC_W_ODR( (void *)this, LSM303C_ACC_ODR_DO_PWR_DOWN ) == MEMS_ERROR )
00158   {
00159     return 1;
00160   }
00161   
00162   isEnabled = 0;
00163   
00164   return 0;
00165 }
00166 
00167 /**
00168  * @brief  Read ID of LSM303C Accelerometer
00169  * @param  p_id the pointer where the ID of the device is stored
00170  * @retval 0 in case of success, an error code otherwise
00171  */
00172 int LSM303C_ACC_Sensor::ReadID(uint8_t *id)
00173 {
00174   if(!id)
00175   { 
00176     return 1; 
00177   }
00178  
00179   /* Read WHO AM I register */
00180   if ( LSM303C_ACC_R_WHO_AM_I( (void *)this, id ) == MEMS_ERROR )
00181   {
00182     return 1;
00183   }
00184   
00185   return 0;
00186 }
00187 
00188 /**
00189  * @brief  Read data from LSM303C Accelerometer
00190  * @param  pData the pointer where the accelerometer data are stored
00191  * @retval 0 in case of success, an error code otherwise
00192  */
00193 int LSM303C_ACC_Sensor::Get_X_Axes(int32_t *pData)
00194 {
00195   int data[3];
00196   int shift = 0;
00197   
00198   /* Read data from LSM303C. */
00199   if ( !LSM303C_ACC_Get_Acceleration((void *)this, data) )
00200   {
00201     return 1;
00202   }
00203   
00204   /* Calculate the data. */
00205   pData[0] = (int32_t)(data[0]/16); // Divide by 16 to convert to mg
00206   pData[1] = (int32_t)(data[1]/16);
00207   pData[2] = (int32_t)(data[2]/16);
00208   
00209   return 0;
00210 }
00211 
00212 /**
00213  * @brief  Read Accelerometer Sensitivity
00214  * @param  pfData the pointer where the accelerometer sensitivity is stored
00215  * @retval 0 in case of success, an error code otherwise
00216  */
00217 int LSM303C_ACC_Sensor::Get_X_Sensitivity(float *pfData)
00218 {
00219     return Get_X_Sensitivity_Normal_Mode( pfData );
00220 }
00221 
00222 /**
00223  * @brief  Read Accelerometer Sensitivity in Normal Mode
00224  * @param  sensitivity the pointer where the accelerometer sensitivity is stored
00225  * @retval 0 in case of success, an error code otherwise
00226  */
00227 int LSM303C_ACC_Sensor::Get_X_Sensitivity_Normal_Mode( float *sensitivity )
00228 {
00229   LSM303C_ACC_FS_t fullScale;
00230   
00231   /* Read actual full scale selection from sensor. */
00232   if ( LSM303C_ACC_R_FullScale( (void *)this, &fullScale ) == MEMS_ERROR )
00233   {
00234     return 1;
00235   }
00236   
00237   /* Store the sensitivity based on actual full scale. */
00238   switch( fullScale )
00239   {
00240     case LSM303C_ACC_FS_2G:
00241       *sensitivity = ( float )LSM303C_ACC_SENSITIVITY_FOR_FS_2G_NORMAL_MODE;
00242       break;
00243     case LSM303C_ACC_FS_4G:
00244       *sensitivity = ( float )LSM303C_ACC_SENSITIVITY_FOR_FS_4G_NORMAL_MODE;
00245       break;
00246     case LSM303C_ACC_FS_8G:
00247       *sensitivity = ( float )LSM303C_ACC_SENSITIVITY_FOR_FS_8G_NORMAL_MODE;
00248       break;
00249     default:
00250       *sensitivity = -1.0f;
00251       return 1;
00252   }
00253   
00254   return 0;
00255 }
00256 
00257 /**
00258  * @brief  Read raw data from LSM303C Accelerometer
00259  * @param  pData the pointer where the accelerometer raw data are stored
00260  * @retval 0 in case of success, an error code otherwise
00261  */
00262 int LSM303C_ACC_Sensor::Get_X_AxesRaw(int16_t *pData)
00263 {
00264   uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};
00265   u8_t shift = 0;
00266   
00267   /* Read output registers from LSM303C_ACC_GYRO_OUTX_L_XL to LSM303C_ACC_GYRO_OUTZ_H_XL. */
00268   if (!LSM303C_ACC_Get_Raw_Acceleration( (void *)this, ( uint8_t* )regValue ))
00269   {
00270     return 1;
00271   }
00272   
00273   /* Format the data. */
00274   pData[0] = ( ( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] ) >> shift );
00275   pData[1] = ( ( ( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] ) >> shift );
00276   pData[2] = ( ( ( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] ) >> shift );
00277   
00278   return 0;
00279 }
00280 
00281 /**
00282  * @brief  Read LSM303C Accelerometer output data rate
00283  * @param  odr the pointer to the output data rate
00284  * @retval 0 in case of success, an error code otherwise
00285  */
00286 int LSM303C_ACC_Sensor::Get_X_ODR(float* odr)
00287 {
00288   LSM303C_ACC_ODR_t odr_low_level;
00289   
00290   if ( LSM303C_ACC_R_ODR( (void *)this, &odr_low_level ) == MEMS_ERROR )
00291   {
00292     return 1;
00293   }
00294   
00295   switch( odr_low_level )
00296   {
00297     case LSM303C_ACC_ODR_DO_PWR_DOWN:
00298       *odr = 0.0f;
00299       break;
00300     case LSM303C_ACC_ODR_DO_10Hz:
00301       *odr = 10.0f;
00302       break;
00303     case LSM303C_ACC_ODR_DO_50Hz:
00304       *odr = 50.0f;
00305       break;
00306     case LSM303C_ACC_ODR_DO_100Hz:
00307       *odr = 100.0f;
00308       break;
00309     case LSM303C_ACC_ODR_DO_200Hz:
00310       *odr = 200.0f;
00311       break;
00312     case LSM303C_ACC_ODR_DO_400Hz:
00313       *odr = 400.0f;
00314       break;
00315     case LSM303C_ACC_ODR_DO_800Hz:
00316       *odr = 400.0f;
00317       break;
00318     default:
00319       *odr = -1.0f;
00320       return 1;
00321   }
00322   
00323   return 0;
00324 }
00325 
00326 /**
00327  * @brief  Set ODR
00328  * @param  odr the output data rate to be set
00329  * @retval 0 in case of success, an error code otherwise
00330  */
00331 int LSM303C_ACC_Sensor::Set_X_ODR(float odr)
00332 {
00333   if(isEnabled == 1)
00334   {
00335     if(Set_X_ODR_When_Enabled(odr) == 1)
00336     {
00337       return 1;
00338     }
00339   }
00340   else
00341   {
00342     if(Set_X_ODR_When_Disabled(odr) == 1)
00343     {
00344       return 1;
00345     }
00346   }
00347   
00348   return 0;
00349 }
00350 
00351 /**
00352  * @brief  Set ODR when enabled
00353  * @param  odr the output data rate to be set
00354  * @retval 0 in case of success, an error code otherwise
00355  */
00356 int LSM303C_ACC_Sensor::Set_X_ODR_When_Enabled(float odr)
00357 {
00358   LSM303C_ACC_ODR_t new_odr;
00359   
00360   new_odr = ( odr <=   10.0f ) ? LSM303C_ACC_ODR_DO_10Hz
00361           : ( odr <=   50.0f ) ? LSM303C_ACC_ODR_DO_50Hz
00362           : ( odr <=  100.0f ) ? LSM303C_ACC_ODR_DO_100Hz
00363           : ( odr <=  200.0f ) ? LSM303C_ACC_ODR_DO_200Hz
00364           : ( odr <=  400.0f ) ? LSM303C_ACC_ODR_DO_400Hz
00365           :                      LSM303C_ACC_ODR_DO_800Hz;
00366             
00367   if ( LSM303C_ACC_W_ODR( (void *)this, new_odr ) == MEMS_ERROR )
00368   {
00369     return 1;
00370   }
00371   
00372   return 0;
00373 }
00374 
00375 /**
00376  * @brief  Set ODR when disabled
00377  * @param  odr the output data rate to be set
00378  * @retval 0 in case of success, an error code otherwise
00379  */
00380 int LSM303C_ACC_Sensor::Set_X_ODR_When_Disabled(float odr)
00381 { 
00382   Last_ODR = ( odr <=   10.0f ) ? 10.0f
00383            : ( odr <=   50.0f ) ? 50.0f
00384            : ( odr <=  100.0f ) ? 100.0f
00385            : ( odr <=  200.0f ) ? 200.0f
00386            : ( odr <=  400.0f ) ? 400.0f
00387            :                      800.0f;
00388                                  
00389   return 0;
00390 }
00391 
00392 
00393 /**
00394  * @brief  Read LSM303C Accelerometer full scale
00395  * @param  fullScale the pointer to the full scale
00396  * @retval 0 in case of success, an error code otherwise
00397  */
00398 int LSM303C_ACC_Sensor::Get_X_FS(float* fullScale)
00399 {
00400   LSM303C_ACC_FS_t fs_low_level;
00401   
00402   if ( LSM303C_ACC_R_FullScale( (void *)this, &fs_low_level ) == MEMS_ERROR )
00403   {
00404     return 1;
00405   }
00406   
00407   switch( fs_low_level )
00408   {
00409     case LSM303C_ACC_FS_2G:
00410       *fullScale =  2.0f;
00411       break;
00412     case LSM303C_ACC_FS_4G:
00413       *fullScale =  4.0f;
00414       break;
00415     case LSM303C_ACC_FS_8G:
00416       *fullScale =  8.0f;
00417       break;
00418     default:
00419       *fullScale = -1.0f;
00420       return 1;
00421   }
00422   
00423   return 0;
00424 }
00425 
00426 /**
00427  * @brief  Set full scale
00428  * @param  fullScale the full scale to be set
00429  * @retval 0 in case of success, an error code otherwise
00430  */
00431 int LSM303C_ACC_Sensor::Set_X_FS(float fullScale)
00432 {
00433   LSM303C_ACC_FS_t new_fs;
00434   
00435   new_fs = ( fullScale <= 2.0f ) ? LSM303C_ACC_FS_2G
00436          : ( fullScale <= 4.0f ) ? LSM303C_ACC_FS_4G
00437          :                         LSM303C_ACC_FS_8G;
00438            
00439   if ( LSM303C_ACC_W_FullScale( (void *)this, new_fs ) == MEMS_ERROR )
00440   {
00441     return 1;
00442   }
00443   
00444   return 0;
00445 }
00446 
00447 /**
00448  * @brief Read accelerometer data from register
00449  * @param reg register address
00450  * @param data register data
00451  * @retval 0 in case of success
00452  * @retval 1 in case of failure
00453  */
00454 int LSM303C_ACC_Sensor::ReadReg( uint8_t reg, uint8_t *data )
00455 {
00456 
00457   if ( LSM303C_ACC_ReadReg( (void *)this, reg, data ) == MEMS_ERROR )
00458   {
00459     return 1;
00460   }
00461 
00462   return 0;
00463 }
00464 
00465 /**
00466  * @brief Write accelerometer data to register
00467  * @param reg register address
00468  * @param data register data
00469  * @retval 0 in case of success
00470  * @retval 1 in case of failure
00471  */
00472 int LSM303C_ACC_Sensor::WriteReg( uint8_t reg, uint8_t data )
00473 {
00474 
00475   if ( LSM303C_ACC_WriteReg( (void *)this, reg, data ) == MEMS_ERROR )
00476   {
00477     return 1;
00478   }
00479 
00480   return 0;
00481 }
00482 
00483 uint8_t LSM303C_ACC_IO_Write( void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite )
00484 {
00485   return ((LSM303C_ACC_Sensor *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite);
00486 }
00487 
00488 uint8_t LSM303C_ACC_IO_Read( void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead )
00489 {
00490   return ((LSM303C_ACC_Sensor *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead);
00491 }