LSM303AGRAccSensor.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    LSM303AGRAccSensor.cpp
00004  * @author  CLab
00005  * @version V1.0.0
00006  * @date    5 August 2016
00007  * @brief   Implementation an LSM303AGR 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 "LSM303AGRAccSensor.h"
00043 #include "LSM303AGR_acc_driver.h"
00044 #include <assert.h>
00045 
00046 
00047 /* Class Implementation ------------------------------------------------------*/
00048 LSM303AGRAccSensor::LSM303AGRAccSensor(SPI3W *spi3w, PinName cs_pin, PinName int1_pin, PinName int2_pin) :
00049                                        _dev_spi3w(spi3w), _cs_pin(cs_pin), _int1_pin(int1_pin), _int2_pin(int2_pin)
00050 {
00051     assert (spi3w);
00052     if (cs_pin == NC) 
00053     {
00054             printf ("ERROR LSM303AGRAccSensor CS MUST NOT BE NC\n\r");      
00055           _dev_spi3w = NULL;
00056         _dev_i2c=NULL;
00057         return;
00058     }       
00059     _cs_pin = 0;     // enable SPI3W disable I2C
00060   _dev_i2c=NULL;    
00061 
00062     LSM303AGR_ACC_W_SPI_mode((void *)this, LSM303AGR_ACC_SIM_3_WIRES);  
00063 }
00064 
00065 
00066 /** Constructor
00067  * @param i2c object of an helper class which handles the I2C peripheral
00068  * @param address the address of the component's instance
00069  */
00070 LSM303AGRAccSensor::LSM303AGRAccSensor(DevI2C *i2c, uint8_t address, PinName int1_pin, PinName int2_pin) : 
00071                                        _dev_i2c(i2c), _address(address), _cs_pin(NC), _int1_pin(int1_pin), _int2_pin(int2_pin)
00072 {
00073     assert (i2c);
00074     _dev_spi3w = NULL; 
00075 };
00076 
00077 /**
00078  * @brief     Initializing the component.
00079  * @param[in] init pointer to device specific initalization structure.
00080  * @retval    "0" in case of success, an error code otherwise.
00081  */
00082 int LSM303AGRAccSensor::init(void *init)
00083 {
00084   /* Enable BDU */
00085   if ( LSM303AGR_ACC_W_BlockDataUpdate( (void *)this, LSM303AGR_ACC_BDU_ENABLED ) == MEMS_ERROR )
00086   {
00087     return 1;
00088   }
00089   
00090   /* FIFO mode selection */
00091   if ( LSM303AGR_ACC_W_FifoMode( (void *)this, LSM303AGR_ACC_FM_BYPASS ) == MEMS_ERROR )
00092   {
00093     return 1;
00094   }
00095   
00096   /* Output data rate selection - power down. */
00097   if ( LSM303AGR_ACC_W_ODR( (void *)this, LSM303AGR_ACC_ODR_DO_PWR_DOWN ) == MEMS_ERROR )
00098   {
00099     return 1;
00100   }
00101   
00102   /* Full scale selection. */
00103   if ( set_x_fs( 2.0f ) == 1 )
00104   {
00105     return 1;
00106   }
00107   
00108   /* Enable axes. */
00109   if ( LSM303AGR_ACC_W_XEN( (void *)this, LSM303AGR_ACC_XEN_ENABLED ) == MEMS_ERROR )
00110   {
00111     return 1;
00112   }
00113   
00114   if ( LSM303AGR_ACC_W_YEN ( (void *)this, LSM303AGR_ACC_YEN_ENABLED ) == MEMS_ERROR )
00115   {
00116     return 1;
00117   }
00118   
00119   if ( LSM303AGR_ACC_W_ZEN ( (void *)this, LSM303AGR_ACC_ZEN_ENABLED ) == MEMS_ERROR )
00120   {
00121     return 1;
00122   }
00123   
00124   /* Select default output data rate. */
00125   _last_odr = 100.0f;
00126   
00127   _is_enabled = 0;
00128   
00129   return 0;
00130 }
00131 
00132 /**
00133  * @brief  Enable LSM303AGR Accelerator
00134  * @retval 0 in case of success, an error code otherwise
00135  */
00136 int LSM303AGRAccSensor::enable(void)
00137 { 
00138   /* Check if the component is already enabled */
00139   if ( _is_enabled == 1 )
00140   {
00141     return 0;
00142   }
00143   
00144   /* Output data rate selection. */
00145   if ( set_x_odr_when_enabled( _last_odr ) == 1 )
00146   {
00147     return 1;
00148   }
00149   
00150   _is_enabled = 1;
00151   
00152   return 0;
00153 }
00154 
00155 /**
00156  * @brief  Disable LSM303AGR Accelerator
00157  * @retval 0 in case of success, an error code otherwise
00158  */
00159 int LSM303AGRAccSensor::disable(void)
00160 { 
00161   /* Check if the component is already disabled */
00162   if ( _is_enabled == 0 )
00163   {
00164     return 0;
00165   }
00166   
00167   /* Store actual output data rate. */
00168   if ( get_x_odr( &_last_odr ) == 1 )
00169   {
00170     return 1;
00171   }
00172   
00173   /* Output data rate selection - power down. */
00174   if ( LSM303AGR_ACC_W_ODR( (void *)this, LSM303AGR_ACC_ODR_DO_PWR_DOWN ) == MEMS_ERROR )
00175   {
00176     return 1;
00177   }
00178   
00179   _is_enabled = 0;
00180   
00181   return 0;
00182 }
00183 
00184 /**
00185  * @brief  Read ID of LSM303AGR Accelerometer
00186  * @param  p_id the pointer where the ID of the device is stored
00187  * @retval 0 in case of success, an error code otherwise
00188  */
00189 int LSM303AGRAccSensor::read_id(uint8_t *id)
00190 {
00191   if(!id)
00192   { 
00193     return 1; 
00194   }
00195  
00196   /* Read WHO AM I register */
00197   if ( LSM303AGR_ACC_R_WHO_AM_I( (void *)this, id ) == MEMS_ERROR )
00198   {
00199     return 1;
00200   }
00201   
00202   return 0;
00203 }
00204 
00205 /**
00206  * @brief  Read data from LSM303AGR Accelerometer
00207  * @param  pData the pointer where the accelerometer data are stored
00208  * @retval 0 in case of success, an error code otherwise
00209  */
00210 int LSM303AGRAccSensor::get_x_axes(int32_t *pData)
00211 {
00212   int data[3];
00213   
00214   /* Read data from LSM303AGR. */
00215   if ( !LSM303AGR_ACC_Get_Acceleration((void *)this, data) )
00216   {
00217     return 1;
00218   }
00219   
00220   /* Calculate the data. */
00221   pData[0] = (int32_t)data[0];
00222   pData[1] = (int32_t)data[1];
00223   pData[2] = (int32_t)data[2];
00224   
00225   return 0;
00226 }
00227 
00228 /**
00229  * @brief  Read Accelerometer Sensitivity
00230  * @param  pfData the pointer where the accelerometer sensitivity is stored
00231  * @retval 0 in case of success, an error code otherwise
00232  */
00233 int LSM303AGRAccSensor::get_x_sensitivity(float *pfData)
00234 {
00235   LSM303AGR_ACC_LPEN_t lp_value;
00236   LSM303AGR_ACC_HR_t hr_value;
00237   
00238   /* Read low power flag */
00239   if( LSM303AGR_ACC_R_LOWPWR_EN( (void *)this, &lp_value ) == MEMS_ERROR )
00240   {
00241     return 1;
00242   }
00243   
00244   /* Read high performance flag */
00245   if( LSM303AGR_ACC_R_HiRes( (void *)this, &hr_value ) == MEMS_ERROR )
00246   {
00247     return 1;
00248   }
00249   
00250   if( lp_value == LSM303AGR_ACC_LPEN_DISABLED && hr_value == LSM303AGR_ACC_HR_DISABLED )
00251   {
00252     /* Normal Mode */
00253     return get_x_sensitivity_normal_mode( pfData );
00254   } else if ( lp_value == LSM303AGR_ACC_LPEN_ENABLED && hr_value == LSM303AGR_ACC_HR_DISABLED )
00255   {
00256     /* Low Power Mode */
00257     return get_x_sensitivity_lp_mode( pfData );
00258   } else if ( lp_value == LSM303AGR_ACC_LPEN_DISABLED && hr_value == LSM303AGR_ACC_HR_ENABLED )
00259   {
00260     /* High Resolution Mode */
00261     return get_x_sensitivity_hr_mode( pfData );
00262   } else
00263   {
00264     /* Not allowed */
00265     return 1;
00266   }
00267 }
00268 
00269 /**
00270  * @brief  Read Accelerometer Sensitivity in Normal Mode
00271  * @param  sensitivity the pointer where the accelerometer sensitivity is stored
00272  * @retval 0 in case of success, an error code otherwise
00273  */
00274 int LSM303AGRAccSensor::get_x_sensitivity_normal_mode( float *sensitivity )
00275 {
00276   LSM303AGR_ACC_FS_t fullScale;
00277   
00278   /* Read actual full scale selection from sensor. */
00279   if ( LSM303AGR_ACC_R_FullScale( (void *)this, &fullScale ) == MEMS_ERROR )
00280   {
00281     return 1;
00282   }
00283   
00284   /* Store the sensitivity based on actual full scale. */
00285   switch( fullScale )
00286   {
00287     case LSM303AGR_ACC_FS_2G:
00288       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_2G_NORMAL_MODE;
00289       break;
00290     case LSM303AGR_ACC_FS_4G:
00291       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_4G_NORMAL_MODE;
00292       break;
00293     case LSM303AGR_ACC_FS_8G:
00294       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_8G_NORMAL_MODE;
00295       break;
00296     case LSM303AGR_ACC_FS_16G:
00297       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_16G_NORMAL_MODE;
00298       break;
00299     default:
00300       *sensitivity = -1.0f;
00301       return 1;
00302   }
00303   
00304   return 0;
00305 }
00306 
00307 /**
00308  * @brief  Read Accelerometer Sensitivity in LP Mode
00309  * @param  sensitivity the pointer where the accelerometer sensitivity is stored
00310  * @retval 0 in case of success, an error code otherwise
00311  */
00312 int LSM303AGRAccSensor::get_x_sensitivity_lp_mode( float *sensitivity )
00313 {
00314   LSM303AGR_ACC_FS_t fullScale;
00315   
00316   /* Read actual full scale selection from sensor. */
00317   if ( LSM303AGR_ACC_R_FullScale( (void *)this, &fullScale ) == MEMS_ERROR )
00318   {
00319     return 1;
00320   }
00321   
00322   /* Store the sensitivity based on actual full scale. */
00323   switch( fullScale )
00324   {
00325     case LSM303AGR_ACC_FS_2G:
00326       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_2G_LOW_POWER_MODE;
00327       break;
00328     case LSM303AGR_ACC_FS_4G:
00329       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_4G_LOW_POWER_MODE;
00330       break;
00331     case LSM303AGR_ACC_FS_8G:
00332       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_8G_LOW_POWER_MODE;
00333       break;
00334     case LSM303AGR_ACC_FS_16G:
00335       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_16G_LOW_POWER_MODE;
00336       break;
00337     default:
00338       *sensitivity = -1.0f;
00339       return 1;
00340   }
00341   
00342   return 0;
00343 }
00344 
00345 /**
00346  * @brief  Read Accelerometer Sensitivity in HR Mode
00347  * @param  sensitivity the pointer where the accelerometer sensitivity is stored
00348  * @retval 0 in case of success, an error code otherwise
00349  */
00350 int LSM303AGRAccSensor::get_x_sensitivity_hr_mode( float *sensitivity )
00351 {
00352   LSM303AGR_ACC_FS_t fullScale;
00353   
00354   /* Read actual full scale selection from sensor. */
00355   if ( LSM303AGR_ACC_R_FullScale( (void *)this, &fullScale ) == MEMS_ERROR )
00356   {
00357     return 1;
00358   }
00359   
00360   /* Store the sensitivity based on actual full scale. */
00361   switch( fullScale )
00362   {
00363     case LSM303AGR_ACC_FS_2G:
00364       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_2G_HIGH_RESOLUTION_MODE;
00365       break;
00366     case LSM303AGR_ACC_FS_4G:
00367       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_4G_HIGH_RESOLUTION_MODE;
00368       break;
00369     case LSM303AGR_ACC_FS_8G:
00370       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_8G_HIGH_RESOLUTION_MODE;
00371       break;
00372     case LSM303AGR_ACC_FS_16G:
00373       *sensitivity = ( float )LSM303AGR_ACC_SENSITIVITY_FOR_FS_16G_HIGH_RESOLUTION_MODE;
00374       break;
00375     default:
00376       *sensitivity = -1.0f;
00377       return 1;
00378   }
00379   
00380   return 0;
00381 }
00382 
00383 /**
00384  * @brief  Read raw data from LSM303AGR Accelerometer
00385  * @param  pData the pointer where the accelerometer raw data are stored
00386  * @retval 0 in case of success, an error code otherwise
00387  */
00388 int LSM303AGRAccSensor::get_x_axes_raw(int16_t *pData)
00389 {
00390   uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};
00391   u8_t shift = 0;
00392   LSM303AGR_ACC_LPEN_t lp;
00393   LSM303AGR_ACC_HR_t hr;
00394   
00395   /* Determine which operational mode the acc is set */
00396   if(!LSM303AGR_ACC_R_HiRes( (void *)this, &hr )) {
00397     return 1;
00398   }
00399 
00400   if(!LSM303AGR_ACC_R_LOWPWR_EN( (void *)this, &lp )) {
00401     return 1;
00402   }
00403   
00404   if (lp == LSM303AGR_ACC_LPEN_ENABLED && hr == LSM303AGR_ACC_HR_DISABLED) {
00405     /* op mode is LP 8-bit */
00406     shift = 8;
00407   } else if (lp == LSM303AGR_ACC_LPEN_DISABLED && hr == LSM303AGR_ACC_HR_DISABLED) {
00408     /* op mode is Normal 10-bit */
00409     shift = 6;
00410   } else if (lp == LSM303AGR_ACC_LPEN_DISABLED && hr == LSM303AGR_ACC_HR_ENABLED) {
00411     /* op mode is HR 12-bit */
00412     shift = 4;
00413   } else {
00414     return 1;
00415   }
00416   
00417   /* Read output registers from LSM303AGR_ACC_GYRO_OUTX_L_XL to LSM303AGR_ACC_GYRO_OUTZ_H_XL. */
00418   if (!LSM303AGR_ACC_Get_Raw_Acceleration( (void *)this, ( uint8_t* )regValue ))
00419   {
00420     return 1;
00421   }
00422   
00423   /* Format the data. */
00424   pData[0] = ( ( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] ) >> shift );
00425   pData[1] = ( ( ( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] ) >> shift );
00426   pData[2] = ( ( ( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] ) >> shift );
00427   
00428   return 0;
00429 }
00430 
00431 /**
00432  * @brief  Read LSM303AGR Accelerometer output data rate
00433  * @param  odr the pointer to the output data rate
00434  * @retval 0 in case of success, an error code otherwise
00435  */
00436 int LSM303AGRAccSensor::get_x_odr(float* odr)
00437 {
00438   LSM303AGR_ACC_ODR_t odr_low_level;
00439   
00440   if ( LSM303AGR_ACC_R_ODR( (void *)this, &odr_low_level ) == MEMS_ERROR )
00441   {
00442     return 1;
00443   }
00444   
00445   switch( odr_low_level )
00446   {
00447     case LSM303AGR_ACC_ODR_DO_PWR_DOWN:
00448       *odr = 0.0f;
00449       break;
00450     case LSM303AGR_ACC_ODR_DO_1Hz:
00451       *odr = 1.0f;
00452       break;
00453     case LSM303AGR_ACC_ODR_DO_10Hz:
00454       *odr = 10.0f;
00455       break;
00456     case LSM303AGR_ACC_ODR_DO_25Hz:
00457       *odr = 25.0f;
00458       break;
00459     case LSM303AGR_ACC_ODR_DO_50Hz:
00460       *odr = 50.0f;
00461       break;
00462     case LSM303AGR_ACC_ODR_DO_100Hz:
00463       *odr = 100.0f;
00464       break;
00465     case LSM303AGR_ACC_ODR_DO_200Hz:
00466       *odr = 200.0f;
00467       break;
00468     case LSM303AGR_ACC_ODR_DO_400Hz:
00469       *odr = 400.0f;
00470       break;
00471     default:
00472       *odr = -1.0f;
00473       return 1;
00474   }
00475   
00476   return 0;
00477 }
00478 
00479 /**
00480  * @brief  Set ODR
00481  * @param  odr the output data rate to be set
00482  * @retval 0 in case of success, an error code otherwise
00483  */
00484 int LSM303AGRAccSensor::set_x_odr(float odr)
00485 {
00486   if(_is_enabled == 1)
00487   {
00488     if(set_x_odr_when_enabled(odr) == 1)
00489     {
00490       return 1;
00491     }
00492   }
00493   else
00494   {
00495     if(set_x_odr_when_disabled(odr) == 1)
00496     {
00497       return 1;
00498     }
00499   }
00500   
00501   return 0;
00502 }
00503 
00504 /**
00505  * @brief  Set ODR when enabled
00506  * @param  odr the output data rate to be set
00507  * @retval 0 in case of success, an error code otherwise
00508  */
00509 int LSM303AGRAccSensor::set_x_odr_when_enabled(float odr)
00510 {
00511   LSM303AGR_ACC_ODR_t new_odr;
00512   
00513   new_odr = ( odr <=    1.0f ) ? LSM303AGR_ACC_ODR_DO_1Hz
00514           : ( odr <=   10.0f ) ? LSM303AGR_ACC_ODR_DO_10Hz
00515           : ( odr <=   25.0f ) ? LSM303AGR_ACC_ODR_DO_25Hz
00516           : ( odr <=   50.0f ) ? LSM303AGR_ACC_ODR_DO_50Hz
00517           : ( odr <=  100.0f ) ? LSM303AGR_ACC_ODR_DO_100Hz
00518           : ( odr <=  200.0f ) ? LSM303AGR_ACC_ODR_DO_200Hz
00519           :                      LSM303AGR_ACC_ODR_DO_400Hz;
00520             
00521   if ( LSM303AGR_ACC_W_ODR( (void *)this, new_odr ) == MEMS_ERROR )
00522   {
00523     return 1;
00524   }
00525   
00526   return 0;
00527 }
00528 
00529 /**
00530  * @brief  Set ODR when disabled
00531  * @param  odr the output data rate to be set
00532  * @retval 0 in case of success, an error code otherwise
00533  */
00534 int LSM303AGRAccSensor::set_x_odr_when_disabled(float odr)
00535 { 
00536   _last_odr = ( odr <=    1.0f ) ?  1.0f
00537            : ( odr <=   10.0f ) ? 10.0f
00538            : ( odr <=   25.0f ) ? 25.0f
00539            : ( odr <=   50.0f ) ? 50.0f
00540            : ( odr <=  100.0f ) ? 100.0f
00541            : ( odr <=  200.0f ) ? 200.0f
00542            :                      400.0f;
00543                                  
00544   return 0;
00545 }
00546 
00547 
00548 /**
00549  * @brief  Read LSM303AGR Accelerometer full scale
00550  * @param  fullScale the pointer to the full scale
00551  * @retval 0 in case of success, an error code otherwise
00552  */
00553 int LSM303AGRAccSensor::get_x_fs(float* fullScale)
00554 {
00555   LSM303AGR_ACC_FS_t fs_low_level;
00556   
00557   if ( LSM303AGR_ACC_R_FullScale( (void *)this, &fs_low_level ) == MEMS_ERROR )
00558   {
00559     return 1;
00560   }
00561   
00562   switch( fs_low_level )
00563   {
00564     case LSM303AGR_ACC_FS_2G:
00565       *fullScale =  2.0f;
00566       break;
00567     case LSM303AGR_ACC_FS_4G:
00568       *fullScale =  4.0f;
00569       break;
00570     case LSM303AGR_ACC_FS_8G:
00571       *fullScale =  8.0f;
00572       break;
00573     case LSM303AGR_ACC_FS_16G:
00574       *fullScale = 16.0f;
00575       break;
00576     default:
00577       *fullScale = -1.0f;
00578       return 1;
00579   }
00580   
00581   return 0;
00582 }
00583 
00584 /**
00585  * @brief  Set full scale
00586  * @param  fullScale the full scale to be set
00587  * @retval 0 in case of success, an error code otherwise
00588  */
00589 int LSM303AGRAccSensor::set_x_fs(float fullScale)
00590 {
00591   LSM303AGR_ACC_FS_t new_fs;
00592   
00593   new_fs = ( fullScale <= 2.0f ) ? LSM303AGR_ACC_FS_2G
00594          : ( fullScale <= 4.0f ) ? LSM303AGR_ACC_FS_4G
00595          : ( fullScale <= 8.0f ) ? LSM303AGR_ACC_FS_8G
00596          :                         LSM303AGR_ACC_FS_16G;
00597            
00598   if ( LSM303AGR_ACC_W_FullScale( (void *)this, new_fs ) == MEMS_ERROR )
00599   {
00600     return 1;
00601   }
00602   
00603   return 0;
00604 }
00605 
00606 /**
00607  * @brief Read accelerometer data from register
00608  * @param reg register address
00609  * @param data register data
00610  * @retval 0 in case of success
00611  * @retval 1 in case of failure
00612  */
00613 int LSM303AGRAccSensor::read_reg( uint8_t reg, uint8_t *data )
00614 {
00615 
00616   if ( LSM303AGR_ACC_read_reg( (void *)this, reg, data ) == MEMS_ERROR )
00617   {
00618     return 1;
00619   }
00620 
00621   return 0;
00622 }
00623 
00624 /**
00625  * @brief Write accelerometer data to register
00626  * @param reg register address
00627  * @param data register data
00628  * @retval 0 in case of success
00629  * @retval 1 in case of failure
00630  */
00631 int LSM303AGRAccSensor::write_reg( uint8_t reg, uint8_t data )
00632 {
00633 
00634   if ( LSM303AGR_ACC_write_reg( (void *)this, reg, data ) == MEMS_ERROR )
00635   {
00636     return 1;
00637   }
00638 
00639   return 0;
00640 }
00641 
00642 uint8_t LSM303AGR_ACC_io_write( void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite )
00643 {
00644   return ((LSM303AGRAccSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
00645 }
00646 
00647 uint8_t LSM303AGR_ACC_io_read( void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead )
00648 {
00649   return ((LSM303AGRAccSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
00650 }