vl6180x_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    vl6180x_class.cpp
00004  * @author  AST / EST
00005  * @version V0.0.1
00006  * @date    14-April-2015
00007  * @brief   Implementation file for the HTS221 driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2015 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 /* Includes */
00039 #include "vl6180x_class.h"
00040  
00041  
00042 #ifndef  VL6180x_RANGE_STATUS_ERRSTRING
00043 #warning "VL6180x_RANGE_STATUS_ERRSTRING not defined ?"
00044 /* TODO you may remove or comment these #warning and keep the default below to keep compatibility
00045    or update your vl6180x_platform.h file */
00046 /**
00047  * force VL6180x_RANGE_STATUS_ERRSTRING to not supported when not part of any cfg file
00048  */
00049 #define VL6180x_RANGE_STATUS_ERRSTRING  0
00050 #endif
00051  
00052 #ifndef  VL6180X_SAFE_POLLING_ENTER
00053 #warning "VL6180X_SAFE_POLLING_ENTER not defined, likely old vl6180x_cfg.h file ?"
00054 /* TODO you may remove or comment these #warning and keep the default below to keep compatibility
00055    or update your vl6180x_platform.h file */
00056 /**
00057  * force VL6180X_SAFE_POLLING_ENTER to off when not in cfg file
00058  */
00059 #define VL6180X_SAFE_POLLING_ENTER 0 /* off by default as in api 2.0 */
00060 #endif
00061  
00062 #ifndef VL6180X_LOG_ENABLE
00063 /**
00064  * Force VL6180X_LOG_ENABLE to none as default
00065  */
00066 #define VL6180X_LOG_ENABLE  0
00067 #endif
00068 
00069 #ifdef MY_LOG 
00070 Serial pc(USBTX, USBRX);
00071 #endif 
00072  
00073 #if VL6180x_RANGE_STATUS_ERRSTRING
00074 /**@def VL6180x_HAVE_RANGE_STATUS_ERRSTRING
00075  * @brief is defined when @a #VL6180x_RANGE_STATUS_ERRSTRING is enable
00076  */
00077 #define  VL6180x_HAVE_RANGE_STATUS_ERRSTRING
00078 #endif
00079  
00080  
00081 /** @brief Get API version as "hex integer" 0xMMnnss
00082  */
00083 #define VL6180x_ApiRevInt  ((VL6180x_API_REV_MAJOR<<24)+(VL6180x_API_REV_MINOR<<16)+VL6180x_API_REV_SUB)
00084  
00085 /** Get API version as string for exe "2.1.12" "
00086  */
00087 #define VL6180x_ApiRevStr  VL6180X_STR(VL6180x_API_REV_MAJOR) "." VL6180X_STR(VL6180x_API_REV_MINOR) "." VL6180X_STR(VL6180x_API_REV_SUB)
00088  
00089 /** @defgroup api_init Init functions
00090  *  @brief    API init functions
00091  *  @ingroup api_hl
00092  *  @{  
00093  */
00094  
00095  
00096 /****************** define for i2c configuration *******************************/
00097  
00098 #define TEMP_BUF_SIZE   32
00099  
00100 #define IsValidGPIOFunction(x) ((x)==GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x)==GPIOx_SELECT_OFF)
00101 /**
00102  * @brief Clear All interrupt causes (als+range+error)
00103  *
00104  * @param dev    The device
00105  * @return  0    On success
00106  */
00107 #define VL6180x_ClearAllInterrupt(dev) VL6180x_ClearInterrupt(dev, INTERRUPT_CLEAR_ERROR|INTERRUPT_CLEAR_RANGING|INTERRUPT_CLEAR_ALS)
00108 /**
00109  * @brief  Clear ALS interrupt
00110  *
00111  * @param dev    The device
00112  * @return  0    On success
00113  */
00114  #define VL6180x_AlsClearInterrupt(dev) VL6180x_ClearInterrupt(dev, INTERRUPT_CLEAR_ALS)
00115 /**
00116  * @brief Clear range interrupt
00117  *
00118  * @param dev    The device
00119  * @return  0    On success
00120  */
00121 #define VL6180x_RangeClearInterrupt(dev) VL6180x_ClearInterrupt(dev, INTERRUPT_CLEAR_RANGING)
00122  
00123 /******************************************************************************/
00124 /******************************* file api.c ***********************************/
00125  
00126 #define VL6180x_9to7Conv(x) (x)
00127  
00128 /* TODO when set all "cached" value with "default init" are updated after init from register read back */
00129 #define REFRESH_CACHED_DATA_AFTER_INIT  1
00130  
00131  
00132 #define IsValidGPIOFunction(x) ((x)==GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x)==GPIOx_SELECT_OFF)
00133  
00134  
00135 /** default value ECE factor Molecular */
00136 #define DEF_ECE_FACTOR_M    85
00137 /** default value ECE factor Denominator */
00138 #define DEF_ECE_FACTOR_D    100
00139 /** default value ALS integration time */
00140 #define DEF_INT_PEFRIOD     100
00141 /** default value ALS gain */
00142 #define DEF_ALS_GAIN        1
00143 /** default value ALS scaler */
00144 #define DEF_ALS_SCALER      1
00145 /** default value for DMAX Enbale */
00146 #define DEF_DMAX_ENABLE     1
00147 /** default ambient tuning factor %x1000 */
00148 #define DEF_AMBIENT_TUNING  80
00149  
00150 #if VL6180x_SINGLE_DEVICE_DRIVER
00151 extern  struct VL6180xDevData_t SingleVL6180xDevData;
00152 #define VL6180xDevDataGet(dev, field) (SingleVL6180xDevData.field)
00153 #define VL6180xDevDataSet(dev, field, data) SingleVL6180xDevData.field=(data)
00154 #endif
00155  
00156 #define LUXRES_FIX_PREC 8
00157 #define GAIN_FIX_PREC    8  /* ! if not sme as LUX_PREC then :( adjust GetLux */
00158 #define AN_GAIN_MULT    (1<<GAIN_FIX_PREC)
00159  
00160  
00161 //int32_t _GetAveTotalTime(VL6180xDev_t dev);
00162 //int VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev);
00163  
00164 /**
00165  * ScalerLookUP scaling factor-1 to register #RANGE_SCALER lookup
00166  */
00167 static const uint16_t ScalerLookUP[]      ROMABLE_DATA ={ 253, 127,  84}; /* lookup table for scaling->scalar 1x2x 3x */
00168 /**
00169  * scaling factor to Upper limit look up
00170  */
00171 static const uint16_t UpperLimitLookUP[]  ROMABLE_DATA ={ 185, 370,  580}; /* lookup table for scaling->limit  1x2x3x */
00172 /**
00173  * Als Code gain to fix point gain lookup
00174  */
00175 static const uint16_t AlsGainLookUp[8]    ROMABLE_DATA = {
00176         (uint16_t)(20.0f * AN_GAIN_MULT),
00177         (uint16_t)(10.0f * AN_GAIN_MULT),
00178         (uint16_t)(5.0f  * AN_GAIN_MULT),
00179         (uint16_t)(2.5f  * AN_GAIN_MULT),
00180         (uint16_t)(1.67f * AN_GAIN_MULT),
00181         (uint16_t)(1.25f * AN_GAIN_MULT),
00182         (uint16_t)(1.0f  * AN_GAIN_MULT),
00183         (uint16_t)(40.0f * AN_GAIN_MULT),
00184 };
00185  
00186  
00187 #if VL6180x_RANGE_STATUS_ERRSTRING
00188 const char * ROMABLE_DATA VL6180x_RangeStatusErrString[]={
00189        "No Error",
00190        "VCSEL Continuity Test",
00191        "VCSEL Watchdog Test",
00192        "VCSEL Watchdog",
00193        "PLL1 Lock",
00194        "PLL2 Lock",
00195        "Early Convergence Estimate",
00196        "Max Convergence",
00197        "No Target Ignore",
00198        "Not used 9",
00199        "Not used 10",
00200        "Max Signal To Noise Ratio",
00201        "Raw Ranging Algo Underflow",
00202        "Raw Ranging Algo Overflow",
00203        "Ranging Algo Underflow",
00204        "Ranging Algo Overflow",
00205  
00206        "Filtered by post processing"
00207 };
00208  
00209 const char * VL6180x_RangeGetStatusErrString(uint8_t RangeErrCode){
00210     if( RangeErrCode  > sizeof(VL6180x_RangeStatusErrString)/sizeof(VL6180x_RangeStatusErrString[0]) )
00211         return NULL;
00212     return  VL6180x_RangeStatusErrString[RangeErrCode];
00213 }
00214 #endif
00215  
00216 #if VL6180x_UPSCALE_SUPPORT == 1
00217     #define _GetUpscale(dev, ... )  1
00218     #define _SetUpscale(...) -1
00219     #define DEF_UPSCALE 1
00220 #elif VL6180x_UPSCALE_SUPPORT == 2
00221     #define _GetUpscale(dev, ... )  2
00222     #define _SetUpscale(...)
00223     #define DEF_UPSCALE 2
00224 #elif  VL6180x_UPSCALE_SUPPORT == 3
00225     #define _GetUpscale(dev, ... )  3
00226     #define _SetUpscale(...)
00227     #define DEF_UPSCALE 3
00228 #else
00229     #define DEF_UPSCALE (-(VL6180x_UPSCALE_SUPPORT))
00230     #define _GetUpscale(dev, ... ) VL6180xDevDataGet(dev, UpscaleFactor)
00231     #define _SetUpscale(dev, Scaling ) VL6180xDevDataSet(dev, UpscaleFactor, Scaling)
00232 #endif
00233  
00234  
00235 #if VL6180x_SINGLE_DEVICE_DRIVER
00236 /**
00237  * the unique driver data  When single device driver is active
00238  */
00239 struct VL6180xDevData_t VL6180x_DEV_DATA_ATTR  SingleVL6180xDevData={
00240         .EceFactorM         = DEF_ECE_FACTOR_M,
00241         .EceFactorD        = DEF_ECE_FACTOR_D,
00242 #ifdef VL6180x_HAVE_UPSCALE_DATA
00243         .UpscaleFactor     = DEF_UPSCALE,
00244 #endif
00245 #ifdef VL6180x_HAVE_ALS_DATA
00246         .IntegrationPeriod = DEF_INT_PEFRIOD,
00247         .AlsGainCode       = DEF_ALS_GAIN,
00248         .AlsScaler         = DEF_ALS_SCALER,
00249 #endif
00250 #ifdef VL6180x_HAVE_DMAX_RANGING
00251         .DMaxEnable =   DEF_DMAX_ENABLE,
00252 #endif
00253 };
00254 #endif /* VL6180x_SINGLE_DEVICE_DRIVER */
00255  
00256 #define Fix7_2_KCPs(x) ((((uint32_t)(x))*1000)>>7)
00257  
00258 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
00259 static int _filter_Init(VL6180xDev_t dev);
00260     #define _IsWrapArroundActive(dev) VL6180xDevDataGet(dev,WrapAroundFilterActive)
00261 #else
00262     #define _IsWrapArroundActive(dev) 0
00263 #endif
00264  
00265  
00266 #if VL6180x_HAVE_DMAX_RANGING
00267    void _DMax_OneTimeInit(VL6180xDev_t dev);
00268 //   static int _DMax_InitData(VL6180xDev_t dev);
00269    static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange);
00270     #define _IsDMaxActive(dev) VL6180xDevDataGet(dev,DMaxEnable)
00271 #else
00272     #define _DMax_InitData(...) 0 /* success */
00273     #define _DMax_OneTimeInit(...) (void)0
00274     #define _IsDMaxActive(...) 0
00275 #endif
00276  
00277 //static int VL6180x_RangeStaticInit(VL6180xDev_t dev);
00278 //static int  VL6180x_UpscaleStaticInit(VL6180xDev_t dev);
00279  
00280 int VL6180X::VL6180x_WaitDeviceBooted(VL6180xDev_t dev){
00281     uint8_t FreshOutReset;
00282     int status;
00283     LOG_FUNCTION_START("");
00284     do{
00285         status = VL6180x_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
00286     }
00287     while( FreshOutReset!=1 && status==0);
00288     LOG_FUNCTION_END(status);
00289     return status;
00290 }
00291  
00292 int VL6180X::VL6180x_InitData(VL6180xDev_t dev){
00293     int status, dmax_status ;
00294     int8_t offset;
00295     uint8_t FreshOutReset;
00296     uint32_t CalValue;
00297     uint16_t u16;
00298     uint32_t XTalkCompRate_KCps;
00299  
00300     LOG_FUNCTION_START("");
00301  
00302     VL6180xDevDataSet(dev, EceFactorM  , DEF_ECE_FACTOR_M);
00303     VL6180xDevDataSet(dev, EceFactorD  , DEF_ECE_FACTOR_D);
00304  
00305 #ifdef VL6180x_HAVE_UPSCALE_DATA
00306     VL6180xDevDataSet(dev, UpscaleFactor  ,  DEF_UPSCALE);
00307 #endif
00308  
00309 #ifdef VL6180x_HAVE_ALS_DATA
00310     VL6180xDevDataSet(dev, IntegrationPeriod , DEF_INT_PEFRIOD);
00311     VL6180xDevDataSet(dev, AlsGainCode , DEF_ALS_GAIN);
00312     VL6180xDevDataSet(dev, AlsScaler , DEF_ALS_SCALER);
00313 #endif
00314  
00315 #ifdef  VL6180x_HAVE_WRAP_AROUND_DATA
00316     VL6180xDevDataSet(dev, WrapAroundFilterActive , (VL6180x_WRAP_AROUND_FILTER_SUPPORT >0));
00317     VL6180xDevDataSet(dev, DMaxEnable, DEF_DMAX_ENABLE);
00318 #endif
00319  
00320     _DMax_OneTimeInit(dev);
00321     do{
00322  
00323         /* backup offset initial value from nvm these must be done prior any over call that use offset */
00324         status = VL6180X::VL6180x_RdByte(dev,SYSRANGE_PART_TO_PART_RANGE_OFFSET, (uint8_t*)&offset);
00325         if( status ){
00326             VL6180x_ErrLog("SYSRANGE_PART_TO_PART_RANGE_OFFSET rd fail");
00327             break;
00328         }
00329         VL6180xDevDataSet(dev, Part2PartOffsetNVM , offset);
00330  
00331         status=VL6180x_RdDWord( dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
00332         if( status ){
00333             VL6180x_ErrLog("Part2PartAmbNVM rd fail");
00334             break;
00335         }
00336         if( (CalValue&0xFFFF0000) == 0 ){
00337             CalValue=0x00CE03F8;
00338         }
00339         VL6180xDevDataSet(dev, Part2PartAmbNVM , CalValue);
00340  
00341         status = VL6180x_RdWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE ,&u16);
00342         if( status){
00343             VL6180x_ErrLog("SYSRANGE_CROSSTALK_COMPENSATION_RATE rd fail ");
00344             break;
00345         }
00346         XTalkCompRate_KCps = Fix7_2_KCPs(u16);
00347         VL6180xDevDataSet(dev, XTalkCompRate_KCps , XTalkCompRate_KCps );
00348  
00349         dmax_status = _DMax_InitData(dev);
00350         if( dmax_status < 0 ){
00351             VL6180x_ErrLog("DMax init failure");
00352             break;
00353         }
00354  
00355         /* Read or wait for fresh out of reset  */
00356         status = VL6180x_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
00357         if( status )  {
00358             VL6180x_ErrLog("SYSTEM_FRESH_OUT_OF_RESET rd fail");
00359             break;
00360         }
00361         if( FreshOutReset!= 1 || dmax_status )
00362             status = CALIBRATION_WARNING;
00363  
00364     }
00365     while(0);
00366  
00367     LOG_FUNCTION_END(status);
00368     return status;
00369 }
00370  
00371 int8_t VL6180X::VL6180x_GetOffsetCalibrationData(VL6180xDev_t dev)
00372 {
00373     int8_t offset;
00374     LOG_FUNCTION_START("");
00375     offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM );
00376     LOG_FUNCTION_END( offset );
00377     return offset;
00378 }
00379  
00380 void VL6180X::VL6180x_SetOffsetCalibrationData(VL6180xDev_t dev, int8_t offset)
00381 {
00382     LOG_FUNCTION_START("%d", offset);
00383     VL6180xDevDataSet(dev, Part2PartOffsetNVM , offset);
00384     LOG_FUNCTION_END(0);
00385 }
00386  
00387 int VL6180X::VL6180x_SetXTalkCompensationRate(VL6180xDev_t dev, FixPoint97_t Rate)
00388 {
00389     int status;
00390     LOG_FUNCTION_START("%d", Rate);
00391     status = VL6180x_WrWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, Rate);
00392     if( status ==0 ){
00393         uint32_t XTalkCompRate_KCps;
00394         XTalkCompRate_KCps = Fix7_2_KCPs(Rate);
00395         VL6180xDevDataSet(dev, XTalkCompRate_KCps , XTalkCompRate_KCps );
00396         /* update dmax whenever xtalk rate changes */
00397         status = _DMax_InitData(dev);
00398     }
00399     LOG_FUNCTION_END(status);
00400     return status;
00401 }
00402  
00403 int VL6180X::VL6180x_SetI2CAddress(VL6180xDev_t dev, uint8_t NewAddress){
00404     int status;
00405     LOG_FUNCTION_START("");
00406  
00407     status = VL6180x_WrByte(dev, I2C_SLAVE_DEVICE_ADDRESS, NewAddress);
00408     if( status ){
00409         VL6180x_ErrLog("new i2c addr Wr fail");
00410     }
00411     LOG_FUNCTION_END(status);
00412     return status;
00413 }
00414  
00415  
00416 uint16_t VL6180X::VL6180x_GetUpperLimit(VL6180xDev_t dev) {
00417     uint16_t limit;
00418     int scaling;
00419  
00420     LOG_FUNCTION_START("");
00421  
00422     scaling = _GetUpscale(dev);
00423     /* FIXME we do assume here _GetUpscale is valid if  user call us prior to init we may overflow the LUT  mem area */
00424     limit = UpperLimitLookUP[scaling - 1];
00425  
00426     LOG_FUNCTION_END((int )limit);
00427     return limit;
00428 }
00429  
00430  
00431  
00432 int VL6180X::VL6180x_StaticInit(VL6180xDev_t dev){
00433     int status=0, init_status;
00434     LOG_FUNCTION_START("");
00435  
00436     /* TODO doc When using configurable scaling but using 1x as start condition
00437      * load tunning upscale  or not ??? */
00438     if( _GetUpscale(dev) == 1 && !(VL6180x_UPSCALE_SUPPORT<0))
00439         init_status=VL6180x_RangeStaticInit(dev);
00440     else
00441         init_status=VL6180x_UpscaleStaticInit(dev);
00442  
00443     if( init_status <0 ){
00444         VL6180x_ErrLog("StaticInit fail");
00445         goto error;
00446     }
00447     else if(init_status > 0){
00448         VL6180x_ErrLog("StaticInit warning");
00449     }
00450  
00451 #if REFRESH_CACHED_DATA_AFTER_INIT
00452     /* update cached value after tuning applied */
00453     do{
00454 #ifdef  VL6180x_HAVE_ALS_DATA
00455         uint8_t data;
00456         status=  VL6180x_RdByte(dev, FW_ALS_RESULT_SCALER, &data);
00457         if( status ) break;
00458         VL6180xDevDataSet(dev, AlsScaler , data);
00459  
00460         status=  VL6180x_RdByte(dev, SYSALS_ANALOGUE_GAIN, &data);
00461         if( status ) break;
00462         VL6180x_AlsSetAnalogueGain(dev, data);
00463 #endif
00464     }
00465     while(0);
00466 #endif /* REFRESH_CACHED_DATA_AFTER_INIT */
00467     if( status < 0 ){
00468         VL6180x_ErrLog("StaticInit fail");
00469     }
00470     if( !status && init_status){
00471         status = init_status;
00472     }
00473 error:
00474     LOG_FUNCTION_END(status);
00475     return status;
00476 }
00477  
00478  
00479 int VL6180X::VL6180x_SetGroupParamHold(VL6180xDev_t dev, int Hold)
00480 {
00481     int status;
00482     uint8_t value;
00483  
00484     LOG_FUNCTION_START("%d", Hold);
00485     if( Hold )
00486         value = 1;
00487     else
00488         value = 0;
00489     status = VL6180x_WrByte(dev, SYSTEM_GROUPED_PARAMETER_HOLD, value);
00490  
00491     LOG_FUNCTION_END(status);
00492     return status;
00493  
00494 }
00495  
00496 int VL6180X::VL6180x_Prepare(VL6180xDev_t dev)
00497 {
00498     int status;
00499     LOG_FUNCTION_START("");
00500  
00501     do{
00502         status=VL6180x_StaticInit(dev);
00503         if( status<0) break;
00504  
00505         /* set range InterruptMode to new sample */
00506         status=VL6180x_RangeConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_DISABLED );
00507         if( status)
00508             break;
00509  
00510         /* set default threshold */
00511         status=VL6180x_RangeSetRawThresholds(dev, 10, 200);
00512         if( status ){
00513             VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
00514             break;
00515         }
00516 #if VL6180x_ALS_SUPPORT
00517         status =VL6180x_AlsSetIntegrationPeriod(dev, 100);
00518         if( status ) break;
00519         status = VL6180x_AlsSetInterMeasurementPeriod(dev,  200);
00520         if( status ) break;
00521         status = VL6180x_AlsSetAnalogueGain(dev,  0);
00522         if( status ) break;
00523         status = VL6180x_AlsSetThresholds(dev, 0, 0xFFFF);
00524         if( status ) break;
00525         /* set Als InterruptMode to new sample */
00526         status=VL6180x_AlsConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_DISABLED);
00527         if( status ) {
00528             VL6180x_ErrLog("VL6180x_AlsConfigInterrupt fail");
00529             break;
00530         }
00531 #endif
00532 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
00533         _filter_Init(dev);
00534 #endif
00535         /* make sure to reset any left previous condition that can hangs first poll */
00536         status=VL6180x_ClearAllInterrupt(dev);
00537     }
00538     while(0);
00539     LOG_FUNCTION_END(status);
00540  
00541     return status;
00542 }
00543  
00544 #if VL6180x_ALS_SUPPORT
00545 int VL6180X::VL6180x_AlsGetLux(VL6180xDev_t dev, lux_t *pLux)
00546 {
00547     int status;
00548     uint16_t RawAls;
00549     uint32_t luxValue = 0;
00550     uint32_t IntPeriod;
00551     uint32_t AlsAnGain;
00552     uint32_t GainFix;
00553     uint32_t AlsScaler ;
00554  
00555 #if LUXRES_FIX_PREC !=  GAIN_FIX_PREC
00556 #error "LUXRES_FIX_PREC != GAIN_FIX_PREC  review these code to be correct"
00557 #endif
00558     const uint32_t LuxResxIntIme =(uint32_t)(0.56f* DEF_INT_PEFRIOD *(1<<LUXRES_FIX_PREC));
00559  
00560     LOG_FUNCTION_START("%p", pLux);
00561  
00562     status = VL6180x_RdWord( dev, RESULT_ALS_VAL, &RawAls);
00563     if( !status){
00564         /* wer are yet here at no fix point */
00565         IntPeriod=VL6180xDevDataGet(dev, IntegrationPeriod );
00566         AlsScaler=VL6180xDevDataGet(dev, AlsScaler);
00567         IntPeriod++; /* what stored is real time  ms -1 and it can be 0 for or 0 or 1ms */
00568         luxValue = (uint32_t)RawAls * LuxResxIntIme; /* max # 16+8bits + 6bit (0.56*100)  */
00569         luxValue /= IntPeriod;                         /* max # 16+8bits + 6bit 16+8+1 to 9 bit */
00570         /* between  29 - 21 bit */
00571         AlsAnGain = VL6180xDevDataGet(dev, AlsGainCode );
00572         GainFix = AlsGainLookUp[AlsAnGain];
00573         luxValue = luxValue / (AlsScaler * GainFix);
00574         *pLux=luxValue;
00575     }
00576  
00577     LOG_FUNCTION_END_FMT(status, "%x",(int)*pLux);
00578     return status;
00579 }
00580  
00581 int VL6180X::VL6180x_AlsGetMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData)
00582 {
00583     int status;
00584     uint8_t ErrStatus;
00585  
00586     LOG_FUNCTION_START("%p", pAlsData);
00587  
00588     status = VL6180x_AlsGetLux(dev, &pAlsData->lux);
00589     if( !status ){
00590         status = VL6180x_RdByte(dev, RESULT_ALS_STATUS, & ErrStatus);
00591         pAlsData->errorStatus = ErrStatus>>4;
00592     }
00593     LOG_FUNCTION_END_FMT(status,"%d %d", (int)pAlsData->lux,  (int)pAlsData->errorStatus);
00594  
00595     return status;
00596 }
00597  
00598  
00599 int VL6180X::VL6180x_AlsPollMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData) {
00600     int status;
00601     int ClrStatus;
00602     uint8_t IntStatus;
00603  
00604     LOG_FUNCTION_START("%p", pAlsData);
00605 #if VL6180X_SAFE_POLLING_ENTER
00606     /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
00607     status=VL6180x_AlsClearInterrupt(dev);
00608     if(status){
00609         VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
00610         goto over;
00611     }
00612 #endif
00613  
00614     status=VL6180x_AlsSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
00615     if( status){
00616         VL6180x_ErrLog("VL6180x_AlsSetSystemMode fail");
00617         goto over;
00618     }
00619  
00620     /* poll for new sample ready */
00621     while (1 ) {
00622         status = VL6180x_AlsGetInterruptStatus(dev, &IntStatus);
00623         if (status) {
00624             break;
00625         }
00626         if (IntStatus == RES_INT_STAT_GPIO_NEW_SAMPLE_READY) {
00627             break; /* break on new data (status is 0)  */
00628         }
00629         wait_ms(10);
00630     };
00631  
00632     if (!status) {
00633         status = VL6180x_AlsGetMeasurement(dev, pAlsData);
00634     }
00635  
00636     ClrStatus = VL6180x_AlsClearInterrupt(dev);
00637     if (ClrStatus) {
00638         VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
00639         if (!status) {
00640             status = ClrStatus; /* leave previous if already on error */
00641         }
00642     }
00643 over:
00644     LOG_FUNCTION_END(status);
00645  
00646     return status;
00647 }
00648  
00649 int VL6180X::VL6180x_AlsGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
00650     int status;
00651     uint8_t IntStatus;
00652     LOG_FUNCTION_START("%p", pIntStatus);
00653  
00654     status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
00655     *pIntStatus= (IntStatus>>3)&0x07;
00656  
00657     LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
00658     return status;
00659 }
00660  
00661 int VL6180X::VL6180x_AlsWaitDeviceReady(VL6180xDev_t dev, int MaxLoop ){
00662     int status;
00663     int  n;
00664     uint8_t u8;
00665     LOG_FUNCTION_START("%d", (int)MaxLoop);
00666     if( MaxLoop<1){
00667           status=INVALID_PARAMS;
00668      }
00669     else{
00670         for( n=0; n < MaxLoop ; n++){
00671             status=VL6180x_RdByte(dev, RESULT_ALS_STATUS, &u8);
00672             if( status)
00673                 break;
00674             u8 = u8 & ALS_DEVICE_READY_MASK;
00675             if( u8 )
00676                 break;
00677  
00678         }
00679         if( !status && !u8 ){
00680             status = TIME_OUT;
00681         }
00682     }
00683     LOG_FUNCTION_END(status);
00684     return status;
00685 }
00686  
00687 int VL6180X::VL6180x_AlsSetSystemMode(VL6180xDev_t dev, uint8_t mode)
00688 {
00689     int status;
00690     LOG_FUNCTION_START("%d", (int)mode);
00691     /* FIXME if we are called back to back real fast we are not checking
00692      * if previous mode "set" got absorbed => bit 0 must be 0 so that wr 1 work */
00693     if( mode <= 3){
00694         status=VL6180x_WrByte(dev, SYSALS_START, mode);
00695     }
00696     else{
00697         status = INVALID_PARAMS;
00698     }
00699     LOG_FUNCTION_END(status);
00700     return status;
00701 }
00702  
00703 int VL6180X::VL6180x_AlsConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt)
00704 {
00705     int status;
00706  
00707     if( ConfigGpioInt<= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY){
00708         status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO, (uint8_t)(~CONFIG_GPIO_ALS_MASK), (ConfigGpioInt<<3));
00709     }
00710     else{
00711         VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
00712         status = INVALID_PARAMS;
00713     }
00714     LOG_FUNCTION_END(status);
00715     return status;
00716 }
00717  
00718  
00719  
00720 int VL6180X::VL6180x_AlsSetThresholds(VL6180xDev_t dev, uint16_t low, uint16_t high) { 
00721     int status;
00722 
00723     LOG_FUNCTION_START("%d %d", (int )low, (int)high);
00724 
00725     status = VL6180x_WrWord(dev, SYSALS_THRESH_LOW, low);
00726     if(!status ){
00727         status = VL6180x_WrWord(dev, SYSALS_THRESH_HIGH, high);
00728     }
00729 
00730     LOG_FUNCTION_END(status) ;
00731     return status;
00732 }
00733  
00734  
00735 int VL6180X::VL6180x_AlsSetAnalogueGain(VL6180xDev_t dev, uint8_t gain) {
00736     int status;
00737     uint8_t GainTotal;
00738  
00739     LOG_FUNCTION_START("%d", (int )gain);
00740     gain&=~0x40;
00741     if (gain > 7) {
00742         gain = 7;
00743     }
00744     GainTotal = gain|0x40;
00745  
00746     status = VL6180x_WrByte(dev, SYSALS_ANALOGUE_GAIN, GainTotal);
00747     if( !status){
00748         VL6180xDevDataSet(dev, AlsGainCode , gain);
00749     }
00750  
00751     LOG_FUNCTION_END_FMT(status, "%d %d", (int ) gain, (int )GainTotal);
00752     return status;
00753 }
00754  
00755 int VL6180X::VL6180x_AlsSetInterMeasurementPeriod(VL6180xDev_t dev,  uint16_t intermeasurement_period_ms)
00756 {
00757     int status;
00758  
00759     LOG_FUNCTION_START("%d",(int)intermeasurement_period_ms);
00760         /* clipping: range is 0-2550ms */
00761         if (intermeasurement_period_ms >= 255 *10)
00762                 intermeasurement_period_ms = 255 *10;
00763     status=VL6180x_WrByte(dev, SYSALS_INTERMEASUREMENT_PERIOD, (uint8_t)(intermeasurement_period_ms/10));
00764  
00765     LOG_FUNCTION_END_FMT(status, "%d", (int) intermeasurement_period_ms);
00766     return status;
00767 }
00768  
00769  
00770 int VL6180X::VL6180x_AlsSetIntegrationPeriod(VL6180xDev_t dev, uint16_t period_ms)
00771 {
00772     int status;
00773     uint16_t SetIntegrationPeriod;
00774  
00775     LOG_FUNCTION_START("%d", (int)period_ms);
00776  
00777     if( period_ms>=1 )
00778         SetIntegrationPeriod = period_ms - 1;
00779     else
00780         SetIntegrationPeriod = period_ms;
00781  
00782     if (SetIntegrationPeriod > 464) {
00783         SetIntegrationPeriod = 464;
00784     }
00785     else if (SetIntegrationPeriod == 255)   {
00786         SetIntegrationPeriod++; /* can't write 255 since this causes the device to lock out.*/
00787     }
00788  
00789     status =VL6180x_WrWord(dev, SYSALS_INTEGRATION_PERIOD, SetIntegrationPeriod);
00790     if( !status ){
00791         VL6180xDevDataSet(dev, IntegrationPeriod , SetIntegrationPeriod) ;
00792     }
00793     LOG_FUNCTION_END_FMT(status, "%d", (int)SetIntegrationPeriod);
00794     return status;
00795 }
00796  
00797 #endif /* HAVE_ALS_SUPPORT */
00798  
00799  
00800 int VL6180X::VL6180x_RangePollMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
00801 {
00802     int status;
00803     int ClrStatus;
00804     IntrStatus_t IntStatus;
00805  
00806     LOG_FUNCTION_START("");
00807     /* start single range measurement */
00808  
00809  
00810 #if VL6180X_SAFE_POLLING_ENTER
00811     /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
00812     status=VL6180x_RangeClearInterrupt(dev);
00813     if(status){
00814         VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
00815         goto done;
00816     }
00817 #endif
00818     /* //![single_shot_snipet] */
00819     status=VL6180x_RangeSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
00820     if( status ){
00821         VL6180x_ErrLog("VL6180x_RangeSetSystemMode fail");
00822         goto done;
00823     }
00824  
00825     /* poll for new sample ready */
00826     while(1 ){
00827         status=VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val );
00828         if( status ){
00829             break;
00830         }
00831         if( IntStatus.status .Error  !=0 ){
00832           VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val );
00833           status = RANGE_ERROR;
00834           break;
00835         }
00836         else
00837         if( IntStatus.status .Range  == RES_INT_STAT_GPIO_NEW_SAMPLE_READY){
00838             break;
00839         }
00840         wait_ms(10);
00841     }
00842  /* //![single_shot_snipet] */
00843  
00844     if ( !status ){
00845         status = VL6180x_RangeGetMeasurement(dev, pRangeData);
00846     }
00847  
00848     /*  clear range interrupt source */
00849     ClrStatus = VL6180x_RangeClearInterrupt(dev);
00850     if( ClrStatus ){
00851         VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
00852         /*  leave initial status if already in error  */
00853         if( !status ){
00854             status=ClrStatus;
00855         }
00856     }
00857 done:
00858     LOG_FUNCTION_END(status);
00859     return status;
00860 }
00861  
00862  
00863  
00864 int VL6180X::VL6180x_RangeGetMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
00865 {
00866     int status;
00867     uint16_t RawRate;
00868     uint8_t RawStatus;
00869  
00870     LOG_FUNCTION_START("");
00871  
00872     status = VL6180x_RangeGetResult(dev, &pRangeData->range_mm );
00873     if( !status ){
00874         status = VL6180x_RdWord(dev,RESULT_RANGE_SIGNAL_RATE, &RawRate );
00875         if( !status ){
00876             pRangeData->signalRate_mcps  = VL6180x_9to7Conv(RawRate);
00877             status = VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &RawStatus);
00878             if( !status ){
00879                 pRangeData->errorStatus  = RawStatus >>4;
00880             }
00881             else{
00882                 VL6180x_ErrLog("Rd RESULT_RANGE_STATUS fail");
00883             }
00884     #if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
00885             status = _GetRateResult(dev, pRangeData);
00886             if( status )
00887                 goto error;
00888     #endif
00889     #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
00890             /* if enabled run filter */
00891             if( _IsWrapArroundActive(dev) ){
00892                 status=_filter_GetResult(dev, pRangeData);
00893                 if( !status){
00894                     /* patch the range status and measure if it is filtered */
00895                     if( pRangeData->range_mm  != pRangeData->FilteredData .range_mm ) {
00896                         pRangeData->errorStatus =RangingFiltered;
00897                         pRangeData->range_mm  = pRangeData->FilteredData .range_mm ;
00898                     }
00899                 }
00900             }
00901     #endif
00902  
00903 #if VL6180x_HAVE_DMAX_RANGING
00904             if(_IsDMaxActive(dev) ){
00905                 _DMax_Compute(dev, pRangeData);
00906             }
00907 #endif
00908         }
00909         else{
00910             VL6180x_ErrLog("Rd RESULT_RANGE_SIGNAL_RATE fail");
00911         }
00912     }
00913     else{
00914         VL6180x_ErrLog("VL6180x_GetRangeResult fail");
00915     }
00916 error:
00917     LOG_FUNCTION_END_FMT(status, "%d %d %d", (int)pRangeData->range_mm , (int)pRangeData->signalRate_mcps ,  (int)pRangeData->errorStatus ) ;
00918     return status;
00919 }
00920  
00921  
00922 int VL6180X::VL6180x_RangeGetMeasurementIfReady(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
00923 {
00924     int status;
00925     IntrStatus_t IntStatus;
00926  
00927     LOG_FUNCTION_START();
00928  
00929     status = VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val );
00930     if( status ==0 ){
00931         if( IntStatus.status .Error  !=0 ){
00932             VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val );
00933             status = RANGE_ERROR;
00934         }
00935         else
00936         if( IntStatus.status .Range  == RES_INT_STAT_GPIO_NEW_SAMPLE_READY){
00937            status = VL6180x_RangeGetMeasurement(dev,pRangeData );
00938            if( status == 0){
00939                /*  clear range interrupt source */
00940                status = VL6180x_RangeClearInterrupt(dev);
00941                if( status ){
00942                    VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
00943                }
00944            }
00945         }
00946         else{
00947             status = NOT_READY;
00948         }
00949     }
00950     else{
00951         VL6180x_ErrLog("fail to get interrupt status");
00952     }
00953     LOG_FUNCTION_END(status) ;
00954     return status;
00955 }
00956  
00957 int VL6180X::VL6180x_FilterSetState(VL6180xDev_t dev, int state){
00958     int status;
00959     LOG_FUNCTION_START("%d", state);
00960 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
00961     VL6180xDevDataSet(dev,WrapAroundFilterActive , state);
00962     status = 0;
00963 #else
00964     status =  NOT_SUPPORTED;
00965 #endif
00966     LOG_FUNCTION_END(status);
00967     return status;
00968 }
00969  
00970 int VL6180X::VL6180x_FilterGetState(VL6180xDev_t dev){
00971     int status;
00972     LOG_FUNCTION_START("");
00973 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
00974     status = VL6180xDevDataGet(dev,WrapAroundFilterActive );
00975 #else
00976     status = 0;
00977 #endif
00978     LOG_FUNCTION_END(status);
00979     return status;
00980 }
00981  
00982 int VL6180X::VL6180x_RangeGetResult(VL6180xDev_t dev, int32_t *pRange_mm) {
00983     int status;
00984     uint8_t RawRange;
00985     int32_t Upscale;
00986  
00987     LOG_FUNCTION_START("%p",pRange_mm);
00988  
00989     status = VL6180x_RdByte(dev, RESULT_RANGE_VAL, &RawRange);
00990     if( !status ){
00991          Upscale = _GetUpscale(dev);
00992         *pRange_mm= Upscale*(int32_t)RawRange;
00993     }
00994     LOG_FUNCTION_END_FMT(status, "%d", (int)*pRange_mm);
00995     return status;
00996 }
00997  
00998 int VL6180X::VL6180x_RangeSetRawThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high)
00999 {
01000     int status;
01001     LOG_FUNCTION_START("%d %d", (int) low, (int)high);
01002     /* TODO we can optimize here grouping high/low in a word but that's cpu endianness dependent */
01003     status=VL6180x_WrByte(dev, SYSRANGE_THRESH_HIGH,high);
01004     if( !status){
01005         status=VL6180x_WrByte(dev, SYSRANGE_THRESH_LOW, low);
01006     }
01007  
01008     LOG_FUNCTION_END(status);
01009     return status;
01010 }
01011  
01012 int VL6180X::VL6180x_RangeSetThresholds(VL6180xDev_t dev, uint16_t low, uint16_t high, int UseSafeParamHold)
01013 {
01014     int status;
01015     int scale;
01016     LOG_FUNCTION_START("%d %d", (int) low, (int)high);
01017     scale=_GetUpscale(dev,UpscaleFactor );
01018     if( low>scale*255 || high >scale*255){
01019         status = INVALID_PARAMS;
01020     }
01021     else{
01022         do{
01023             if( UseSafeParamHold ){
01024                 status=VL6180x_SetGroupParamHold(dev, 1);
01025                 if( status )
01026                     break;
01027             }
01028             status=VL6180x_RangeSetRawThresholds(dev, (uint8_t)(low/scale), (uint8_t)(high/scale));
01029             if( status ){
01030                 VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
01031             }
01032             if( UseSafeParamHold ){
01033                 int HoldStatus;
01034                 /* tryt to unset param hold vene if previous fail */
01035                 HoldStatus=VL6180x_SetGroupParamHold(dev, 0);
01036                 if( !status)
01037                     status=HoldStatus;
01038             }
01039         }
01040         while(0);
01041     }
01042  
01043     LOG_FUNCTION_END(status);
01044     return status;
01045 }
01046  
01047  
01048 int VL6180X::VL6180x_RangeGetThresholds(VL6180xDev_t dev, uint16_t *low, uint16_t *high)
01049 {
01050     int status;
01051     uint8_t RawLow, RawHigh;
01052     int scale;
01053  
01054     LOG_FUNCTION_START("%p %p", low , high);
01055  
01056     scale=_GetUpscale(dev,UpscaleFactor );
01057     do{
01058         if( high != NULL ){
01059             status=VL6180x_RdByte(dev, SYSRANGE_THRESH_HIGH,&RawHigh);
01060             if( status ){
01061                 VL6180x_ErrLog("rd SYSRANGE_THRESH_HIGH fail");
01062                 break;
01063             }
01064             *high=(uint16_t)RawHigh*scale;
01065         }
01066         if( low != NULL ) {
01067             status=VL6180x_RdByte(dev, SYSRANGE_THRESH_LOW, &RawLow);
01068             if( status ){
01069                 VL6180x_ErrLog("rd SYSRANGE_THRESH_LOW fail");
01070                 break;
01071             }
01072             *low=(uint16_t)RawLow*scale;
01073         }
01074     }
01075     while(0);
01076     LOG_FUNCTION_END_FMT(status, "%d %d",(int)*low ,(int)*high);
01077     return status;
01078 }
01079  
01080  
01081 int VL6180X::VL6180x_RangeGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
01082     int status;
01083     uint8_t IntStatus;
01084     LOG_FUNCTION_START("%p", pIntStatus);
01085     /* FIXME we are grouping "error" with over status the user must check implicitly for it
01086      * not just new sample or over status , that will nevr show up in case of error*/
01087     status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
01088     *pIntStatus= IntStatus&0xC7;
01089  
01090     LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
01091     return status;
01092 }
01093  
01094  
01095 int VL6180X::VL6180x_GetInterruptStatus(VL6180xDev_t dev, uint8_t *IntStatus)
01096 {
01097     int status;
01098     LOG_FUNCTION_START("%p" , IntStatus);
01099     status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, IntStatus);
01100     LOG_FUNCTION_END_FMT(status, "%d", (int)*IntStatus);
01101     return status;
01102 }
01103  
01104 int VL6180X::VL6180x_ClearInterrupt(VL6180xDev_t dev, uint8_t IntClear )
01105 {
01106     int status;
01107     LOG_FUNCTION_START("%d" ,(int)IntClear);
01108     if( IntClear <= 7 ){
01109         status=VL6180x_WrByte( dev, SYSTEM_INTERRUPT_CLEAR, IntClear);
01110     }
01111     else{
01112         status = INVALID_PARAMS;
01113     }
01114     LOG_FUNCTION_END(status);
01115     return status;
01116 }
01117  
01118  
01119 int VL6180X::VL6180x_RangeStaticInit(VL6180xDev_t dev)
01120 {
01121     int status;
01122     LOG_FUNCTION_START("");
01123  
01124     /* REGISTER_TUNING_SR03_270514_CustomerView.txt */
01125     VL6180x_WrByte( dev, 0x0207, 0x01);
01126     VL6180x_WrByte( dev, 0x0208, 0x01);
01127     VL6180x_WrByte( dev, 0x0096, 0x00);
01128     VL6180x_WrByte( dev, 0x0097, 0xfd);
01129     VL6180x_WrByte( dev, 0x00e3, 0x00);
01130     VL6180x_WrByte( dev, 0x00e4, 0x04);
01131     VL6180x_WrByte( dev, 0x00e5, 0x02);
01132     VL6180x_WrByte( dev, 0x00e6, 0x01);
01133     VL6180x_WrByte( dev, 0x00e7, 0x03);
01134     VL6180x_WrByte( dev, 0x00f5, 0x02);
01135     VL6180x_WrByte( dev, 0x00d9, 0x05);
01136     VL6180x_WrByte( dev, 0x00db, 0xce);
01137     VL6180x_WrByte( dev, 0x00dc, 0x03);
01138     VL6180x_WrByte( dev, 0x00dd, 0xf8);
01139     VL6180x_WrByte( dev, 0x009f, 0x00);
01140     VL6180x_WrByte( dev, 0x00a3, 0x3c);
01141     VL6180x_WrByte( dev, 0x00b7, 0x00);
01142     VL6180x_WrByte( dev, 0x00bb, 0x3c);
01143     VL6180x_WrByte( dev, 0x00b2, 0x09);
01144     VL6180x_WrByte( dev, 0x00ca, 0x09);
01145     VL6180x_WrByte( dev, 0x0198, 0x01);
01146     VL6180x_WrByte( dev, 0x01b0, 0x17);
01147     VL6180x_WrByte( dev, 0x01ad, 0x00);
01148     VL6180x_WrByte( dev, 0x00ff, 0x05);
01149     VL6180x_WrByte( dev, 0x0100, 0x05);
01150     VL6180x_WrByte( dev, 0x0199, 0x05);
01151     VL6180x_WrByte( dev, 0x01a6, 0x1b);
01152     VL6180x_WrByte( dev, 0x01ac, 0x3e);
01153     VL6180x_WrByte( dev, 0x01a7, 0x1f);
01154     VL6180x_WrByte( dev, 0x0030, 0x00);
01155  
01156     /* Recommended : Public registers - See data sheet for more detail */
01157     VL6180x_WrByte( dev, SYSTEM_MODE_GPIO1, 0x10); /* Enables polling for New Sample ready when measurement completes */
01158     VL6180x_WrByte( dev, READOUT_AVERAGING_SAMPLE_PERIOD, 0x30); /* Set the averaging sample period (compromise between lower noise and increased execution time) */
01159     VL6180x_WrByte( dev, SYSALS_ANALOGUE_GAIN, 0x46); /* Sets the light and dark gain (upper nibble). Dark gain should not be changed.*/
01160     VL6180x_WrByte( dev, SYSRANGE_VHV_REPEAT_RATE, 0xFF); /* sets the # of range measurements after which auto calibration of system is performed */
01161     VL6180x_WrByte( dev, SYSALS_INTEGRATION_PERIOD, 0x63); /* Set ALS integration time to 100ms */
01162     VL6180x_WrByte( dev, SYSRANGE_VHV_RECALIBRATE, 0x01); /* perform a single temperature calibration of the ranging sensor */
01163  
01164     /* Optional: Public registers - See data sheet for more detail */
01165     VL6180x_WrByte( dev, SYSRANGE_INTERMEASUREMENT_PERIOD, 0x09); /* Set default ranging inter-measurement period to 100ms */
01166     VL6180x_WrByte( dev, SYSALS_INTERMEASUREMENT_PERIOD, 0x31); /* Set default ALS inter-measurement period to 500ms */
01167     VL6180x_WrByte( dev, SYSTEM_INTERRUPT_CONFIG_GPIO, 0x24); /* Configures interrupt on New sample ready */
01168  
01169  
01170     status=VL6180x_RangeSetMaxConvergenceTime(dev, 50); /*  Calculate ece value on initialization (use max conv) */
01171     LOG_FUNCTION_END(status);
01172  
01173     return status;
01174 }
01175  
01176 #if VL6180x_UPSCALE_SUPPORT != 1
01177  
01178 int VL6180X::_UpscaleInitPatch0(VL6180xDev_t dev){
01179     int status;
01180     uint32_t CalValue=0;
01181     CalValue= VL6180xDevDataGet(dev, Part2PartAmbNVM );
01182     status=VL6180x_WrDWord( dev, 0xDA, CalValue);
01183     return status;
01184 }
01185  
01186 /* only include up-scaling register setting when up-scale support is configured in */
01187 int VL6180X::VL6180x_UpscaleRegInit(VL6180xDev_t dev)
01188 {
01189     /*  apply REGISTER_TUNING_ER02_100614_CustomerView.txt */
01190     VL6180x_WrByte( dev, 0x0207, 0x01);
01191     VL6180x_WrByte( dev, 0x0208, 0x01);
01192     VL6180x_WrByte( dev, 0x0096, 0x00);
01193     VL6180x_WrByte( dev, 0x0097, 0x54);
01194     VL6180x_WrByte( dev, 0x00e3, 0x00);
01195     VL6180x_WrByte( dev, 0x00e4, 0x04);
01196     VL6180x_WrByte( dev, 0x00e5, 0x02);
01197     VL6180x_WrByte( dev, 0x00e6, 0x01);
01198     VL6180x_WrByte( dev, 0x00e7, 0x03);
01199     VL6180x_WrByte( dev, 0x00f5, 0x02);
01200     VL6180x_WrByte( dev, 0x00d9, 0x05);
01201  
01202     _UpscaleInitPatch0(dev);
01203  
01204     VL6180x_WrByte( dev, 0x009f, 0x00);
01205     VL6180x_WrByte( dev, 0x00a3, 0x28);
01206     VL6180x_WrByte( dev, 0x00b7, 0x00);
01207     VL6180x_WrByte( dev, 0x00bb, 0x28);
01208     VL6180x_WrByte( dev, 0x00b2, 0x09);
01209     VL6180x_WrByte( dev, 0x00ca, 0x09);
01210     VL6180x_WrByte( dev, 0x0198, 0x01);
01211     VL6180x_WrByte( dev, 0x01b0, 0x17);
01212     VL6180x_WrByte( dev, 0x01ad, 0x00);
01213     VL6180x_WrByte( dev, 0x00ff, 0x05);
01214     VL6180x_WrByte( dev, 0x0100, 0x05);
01215     VL6180x_WrByte( dev, 0x0199, 0x05);
01216     VL6180x_WrByte( dev, 0x01a6, 0x1b);
01217     VL6180x_WrByte( dev, 0x01ac, 0x3e);
01218     VL6180x_WrByte( dev, 0x01a7, 0x1f);
01219     VL6180x_WrByte( dev, 0x0030, 0x00);
01220     VL6180x_WrByte( dev, SYSTEM_MODE_GPIO1, 0x10);
01221     VL6180x_WrByte( dev, READOUT_AVERAGING_SAMPLE_PERIOD, 0x30);
01222     VL6180x_WrByte( dev, SYSALS_ANALOGUE_GAIN, 0x46);
01223     VL6180x_WrByte( dev, SYSRANGE_VHV_REPEAT_RATE, 0xFF);
01224     VL6180x_WrByte( dev, SYSALS_INTEGRATION_PERIOD, 0x63);
01225     VL6180x_WrByte( dev, SYSRANGE_VHV_RECALIBRATE, 0x01);
01226     VL6180x_WrByte( dev, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, 0xff);
01227     VL6180x_WrByte( dev, SYSRANGE_INTERMEASUREMENT_PERIOD, 0x09);
01228     VL6180x_WrByte( dev, SYSALS_INTERMEASUREMENT_PERIOD, 0x31);
01229     VL6180x_WrByte( dev, SYSTEM_INTERRUPT_CONFIG_GPIO, 0x24);
01230 #if VL6180x_EXTENDED_RANGE
01231     VL6180x_RangeSetMaxConvergenceTime(dev, 63);
01232 #else
01233     VL6180x_RangeSetMaxConvergenceTime(dev, 50);
01234 #endif
01235     return 0;
01236 }
01237 #else
01238 #define VL6180x_UpscaleRegInit(...) -1
01239 #endif
01240  
01241 int VL6180X::VL6180x_UpscaleSetScaling(VL6180xDev_t dev, uint8_t scaling)
01242 {
01243     int status;
01244     uint16_t Scaler;
01245     int8_t  Offset;
01246  
01247     LOG_FUNCTION_START("%d",(int) scaling);
01248  
01249 #ifdef VL6180x_HAVE_UPSCALE_DATA
01250     #define min_scaling 1
01251     #define max_scaling sizeof(ScalerLookUP)/sizeof(ScalerLookUP[0])
01252 #else
01253      /* we are in fixed config so only allow configured factor */
01254     #define min_scaling VL6180x_UPSCALE_SUPPORT
01255     #define max_scaling VL6180x_UPSCALE_SUPPORT
01256 #endif
01257  
01258     if( scaling>=min_scaling  && scaling<= max_scaling ){
01259  
01260         Scaler = ScalerLookUP[scaling-1];
01261         status = VL6180x_WrWord(dev, RANGE_SCALER, Scaler);
01262         _SetUpscale(dev, scaling );
01263  
01264         /* Apply scaling on  part-2-part offset */
01265         Offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM )/scaling;
01266         status = VL6180x_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, Offset);
01267 #if ! VL6180x_EXTENDED_RANGE
01268         if( status ==0 ){
01269             status = VL6180x_RangeSetEceState(dev, scaling == 1); /* enable ece only at 1x scaling */
01270         }
01271         if( status == 0 && !VL6180x_EXTENDED_RANGE && scaling!=1 ){
01272             status = NOT_GUARANTEED ;
01273         }
01274 #endif
01275     }
01276     else{
01277         status = INVALID_PARAMS;
01278     }
01279 #undef min_scaling
01280 #undef max_scaling
01281     LOG_FUNCTION_END(status);
01282     return status;
01283 }
01284  
01285  
01286 int VL6180X::VL6180x_UpscaleGetScaling(VL6180xDev_t dev)
01287 {
01288     int status;
01289     LOG_FUNCTION_START("");
01290     status=_GetUpscale(dev );
01291     LOG_FUNCTION_END(status);
01292  
01293     return status;
01294 }
01295  
01296  
01297 int VL6180X::VL6180x_UpscaleStaticInit(VL6180xDev_t dev)
01298 {
01299     /* todo make these a fail macro in case only 1x is suppoted */
01300     int status;
01301  
01302     LOG_FUNCTION_START("");
01303     do{
01304         status=VL6180x_UpscaleRegInit(dev);
01305         if( status){
01306             VL6180x_ErrLog("regInit fail");
01307             break;
01308         }
01309 #if VL6180x_EXTENDED_RANGE
01310         status = VL6180x_RangeSetEceState(dev, 0);
01311         if( status){
01312             VL6180x_ErrLog("VL6180x_RangeSetEceState fail");
01313             break;
01314         }
01315 #endif
01316     } while(0);
01317     if( !status){
01318         /*  must write the scaler at least once to the device to ensure the scaler is in a known state. */
01319         status=VL6180x_UpscaleSetScaling(dev, _GetUpscale(dev));
01320         VL6180x_WrByte( dev, SYSTEM_FRESH_OUT_OF_RESET, 0x00); /* change fresh out of set status to 0 */
01321     }
01322     LOG_FUNCTION_END(status);
01323     return status;
01324 }
01325  
01326  
01327 int VL6180X::VL6180x_SetGPIOxPolarity(VL6180xDev_t dev, int pin, int active_high)
01328 {
01329     int status;
01330     LOG_FUNCTION_START("%d %d",(int) pin, (int)active_high);
01331  
01332     if( pin ==0  || pin ==1  ){
01333        uint16_t RegIndex;
01334        uint8_t  DataSet;
01335        if( pin==0 )
01336            RegIndex= SYSTEM_MODE_GPIO0;
01337        else
01338            RegIndex= SYSTEM_MODE_GPIO1;
01339  
01340        if (active_high )
01341            DataSet = GPIOx_POLARITY_SELECT_MASK;
01342        else
01343            DataSet = 0;
01344  
01345        status = VL6180x_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_POLARITY_SELECT_MASK, DataSet);
01346     }
01347     else{
01348         VL6180x_ErrLog("Invalid pin param %d", (int)pin);
01349         status = INVALID_PARAMS;
01350     }
01351  
01352     LOG_FUNCTION_END(status);
01353  
01354     return status;
01355 }
01356  
01357 int VL6180X::VL6180x_SetGPIOxFunctionality(VL6180xDev_t dev, int pin, uint8_t functionality)
01358 {
01359     int status;
01360  
01361     LOG_FUNCTION_START("%d %d",(int) pin, (int)functionality);
01362  
01363     if( ((pin ==0)  || (pin ==1))  && IsValidGPIOFunction(functionality)  ){
01364        uint16_t RegIndex;
01365  
01366        if( pin==0 )
01367            RegIndex= SYSTEM_MODE_GPIO0;
01368        else
01369            RegIndex= SYSTEM_MODE_GPIO1;
01370  
01371        status = VL6180x_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_FUNCTIONALITY_SELECT_MASK,  functionality<<GPIOx_FUNCTIONALITY_SELECT_SHIFT);
01372        if( status){
01373            VL6180x_ErrLog("Update SYSTEM_MODE_GPIO%d fail", (int)pin);
01374        }
01375     }
01376     else{
01377         VL6180x_ErrLog("Invalid pin %d  or function %d", (int)pin, (int) functionality);
01378         status = INVALID_PARAMS;
01379     }
01380  
01381     LOG_FUNCTION_END(status);
01382     return status;
01383 }
01384  
01385  
01386 int VL6180X::VL6180x_SetupGPIOx(VL6180xDev_t dev, int pin,  uint8_t IntFunction, int  ActiveHigh)
01387 {
01388     int status;
01389  
01390     LOG_FUNCTION_START("%d %d",(int) pin, (int)IntFunction);
01391  
01392     if( ((pin ==0)  || (pin ==1))  && IsValidGPIOFunction(IntFunction)  ){
01393        uint16_t RegIndex;
01394        uint8_t value=0;
01395  
01396        if( pin==0 )
01397            RegIndex= SYSTEM_MODE_GPIO0;
01398        else
01399            RegIndex= SYSTEM_MODE_GPIO1;
01400  
01401        if( ActiveHigh  )
01402            value|=GPIOx_POLARITY_SELECT_MASK;
01403  
01404        value |=  IntFunction<<GPIOx_FUNCTIONALITY_SELECT_SHIFT;
01405        status = VL6180x_WrByte(dev, RegIndex, value);
01406        if( status ){
01407            VL6180x_ErrLog("SYSTEM_MODE_GPIO%d wr fail", (int)pin-SYSTEM_MODE_GPIO0);
01408        }
01409     }
01410     else{
01411         VL6180x_ErrLog("Invalid pin %d or function %d", (int)pin, (int) IntFunction);
01412         status = INVALID_PARAMS;
01413     }
01414  
01415     LOG_FUNCTION_END(status);
01416     return status;
01417 }
01418  
01419  
01420 int VL6180X::VL6180x_DisableGPIOxOut(VL6180xDev_t dev, int pin) {
01421     int status;
01422  
01423     LOG_FUNCTION_START("%d",(int)pin);
01424  
01425     status=VL6180x_SetGPIOxFunctionality(dev, pin, GPIOx_SELECT_OFF);
01426  
01427     LOG_FUNCTION_END(status);
01428     return status;
01429 }
01430  
01431  
01432 int VL6180X::VL6180x_SetupGPIO1(VL6180xDev_t dev, uint8_t IntFunction, int ActiveHigh)
01433 {
01434     int status;
01435     LOG_FUNCTION_START("%d %d",(int)IntFunction, (int)ActiveHigh  );
01436     status=VL6180x_SetupGPIOx(dev, 1 , IntFunction, ActiveHigh);
01437     LOG_FUNCTION_END(status);
01438     return status;
01439 }
01440  
01441 int VL6180X::VL6180x_RangeConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt)
01442 {
01443     int status;
01444  
01445     if( ConfigGpioInt<= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY){
01446         status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO, (uint8_t)(~CONFIG_GPIO_RANGE_MASK), ConfigGpioInt);
01447     }
01448     else{
01449         VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
01450         status = INVALID_PARAMS;
01451     }
01452     LOG_FUNCTION_END(status);
01453     return status;
01454 }
01455  
01456  
01457 int VL6180X::VL6180x_RangeSetEceFactor(VL6180xDev_t dev, uint16_t  FactorM, uint16_t FactorD){
01458     int status;
01459     uint8_t u8;
01460  
01461     LOG_FUNCTION_START("%d %d", (int)FactorM, (int)FactorD );
01462     do{
01463         /* D cannot be 0 M must be <=D and >= 0 */
01464         if( FactorM <= FactorD  && FactorD> 0){
01465             VL6180xDevDataSet(dev, EceFactorM , FactorM);
01466             VL6180xDevDataSet(dev, EceFactorD , FactorD);
01467             /* read and re-apply max conv time to get new ece factor set */
01468             status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
01469             if( status){
01470                VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
01471                break;
01472             }
01473             status = VL6180x_RangeSetMaxConvergenceTime(dev, u8);
01474             if( status <0 ){
01475                 VL6180x_ErrLog("fail to apply time after ece m/d change");
01476                 break;
01477             }
01478         }
01479         else{
01480             VL6180x_ErrLog("invalid factor %d/%d", (int)FactorM, (int)FactorD );
01481             status = INVALID_PARAMS;
01482         }
01483     }
01484     while(0);
01485     LOG_FUNCTION_END(status);
01486     return status;
01487 }
01488  
01489 int VL6180X::VL6180x_RangeSetEceState(VL6180xDev_t dev, int enable ){
01490     int status;
01491     uint8_t or_mask;
01492  
01493     LOG_FUNCTION_START("%d", (int)enable);
01494     if( enable )
01495         or_mask = RANGE_CHECK_ECE_ENABLE_MASK;
01496     else
01497         or_mask = 0;
01498  
01499     status =VL6180x_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, ~RANGE_CHECK_ECE_ENABLE_MASK, or_mask);
01500     LOG_FUNCTION_END(status);
01501     return status;
01502 }
01503  
01504  
01505 int VL6180X::VL6180x_RangeSetMaxConvergenceTime(VL6180xDev_t dev, uint8_t  MaxConTime_msec)
01506 {
01507     int status = 0;
01508     LOG_FUNCTION_START("%d",(int)MaxConTime_msec);
01509     do{
01510         status=VL6180x_WrByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, MaxConTime_msec);
01511         if( status ){
01512             break;
01513         }
01514         status=VL6180x_RangeSetEarlyConvergenceEestimateThreshold(dev);
01515         if( status){
01516             break;
01517         }
01518         status = _DMax_InitData(dev);
01519     }
01520     while(0);
01521     LOG_FUNCTION_END(status);
01522     return status;
01523 }
01524  
01525 int VL6180X::VL6180x_RangeSetInterMeasPeriod(VL6180xDev_t dev, uint32_t  InterMeasTime_msec){
01526     uint8_t SetTime;
01527     int status;
01528  
01529     LOG_FUNCTION_START("%d",(int)InterMeasTime_msec);
01530     do {
01531         if( InterMeasTime_msec > 2550 ){
01532             status = INVALID_PARAMS;
01533             break;
01534         }
01535         /* doc in not 100% clear and confusing about the limit practically all value are OK but 0
01536          * that can hang device in continuous mode */
01537         if( InterMeasTime_msec < 10 ) {
01538             InterMeasTime_msec=10;
01539         }
01540         SetTime=(uint8_t)(InterMeasTime_msec/10);
01541         status=VL6180x_WrByte(dev, SYSRANGE_INTERMEASUREMENT_PERIOD, SetTime);
01542         if( status ){
01543             VL6180x_ErrLog("SYSRANGE_INTERMEASUREMENT_PERIOD wr fail");
01544         }
01545         else
01546         if( SetTime != InterMeasTime_msec /10 ) {
01547             status = MIN_CLIPED;  /* on success change status to clip if it did */
01548         }
01549     }while(0);
01550     LOG_FUNCTION_END(status);
01551     return status;
01552 }
01553  
01554  
01555 int VL6180X::VL6180x_RangeGetDeviceReady(VL6180xDev_t dev, int * Ready){
01556     int status;
01557     uint8_t u8;
01558     LOG_FUNCTION_START("%p", (int)Ready);
01559     status=VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
01560     if( !status)
01561         *Ready = u8&RANGE_DEVICE_READY_MASK;
01562     LOG_FUNCTION_END_FMT(status,"%d", *Ready);
01563     return status;
01564 }
01565  
01566  
01567 int VL6180X::VL6180x_RangeWaitDeviceReady(VL6180xDev_t dev, int MaxLoop ){
01568     int status; /* if user specify an invalid <=0 loop count we'll return error */
01569     int  n;
01570     uint8_t u8;
01571     LOG_FUNCTION_START("%d", (int)MaxLoop);
01572     if( MaxLoop<1){
01573         status=INVALID_PARAMS;
01574     }
01575     else{
01576         for( n=0; n < MaxLoop ; n++){
01577             status=VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
01578             if( status)
01579                 break;
01580             u8 = u8 & RANGE_DEVICE_READY_MASK;
01581             if( u8 )
01582                 break;
01583  
01584         }
01585         if( !status && !u8 ){
01586             status = TIME_OUT;
01587         }
01588     }
01589     LOG_FUNCTION_END(status);
01590     return status;
01591 }
01592  
01593 int VL6180X::VL6180x_RangeSetSystemMode(VL6180xDev_t dev, uint8_t  mode)
01594 {
01595     int status;
01596     LOG_FUNCTION_START("%d", (int)mode);
01597     /* FIXME we are not checking device is ready via @a VL6180x_RangeWaitDeviceReady
01598      * so if called back to back real fast we are not checking
01599      * if previous mode "set" got absorbed => bit 0 must be 0 so that it work
01600      */
01601     if( mode <= 3){
01602         status=VL6180x_WrByte(dev, SYSRANGE_START, mode);
01603         if( status ){
01604             VL6180x_ErrLog("SYSRANGE_START wr fail");
01605         }
01606     }
01607     else{
01608         status = INVALID_PARAMS;
01609     }
01610     LOG_FUNCTION_END(status);
01611     return status;
01612 }
01613  
01614  
01615 int VL6180X::VL6180x_RangeStartContinuousMode(VL6180xDev_t dev)
01616 {
01617     int status;
01618     LOG_FUNCTION_START("");
01619     status= VL6180x_RangeSetSystemMode(dev, MODE_START_STOP | MODE_CONTINUOUS);
01620     LOG_FUNCTION_END(status);
01621     return status;
01622 }
01623  
01624 int VL6180X::VL6180x_RangeStartSingleShot(VL6180xDev_t dev) {
01625     int status;
01626     LOG_FUNCTION_START("");
01627     status = VL6180x_RangeSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
01628     LOG_FUNCTION_END(status);
01629     return status;
01630 }
01631  
01632  
01633 int VL6180X::VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev)
01634 {
01635     int status;
01636  
01637     const uint32_t cMicroSecPerMilliSec  = 1000;
01638     const uint32_t cEceSampleTime_us     = 500;
01639     uint32_t ece_factor_m          = VL6180xDevDataGet(dev, EceFactorM );
01640     uint32_t ece_factor_d          = VL6180xDevDataGet(dev, EceFactorD );
01641     uint32_t convergTime_us;
01642     uint32_t fineThresh;
01643     uint32_t eceThresh;
01644     uint8_t  u8;
01645     uint32_t maxConv_ms;
01646     int32_t AveTime;
01647  
01648     LOG_FUNCTION_START("");
01649  
01650     do{
01651         status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
01652         if( status ){
01653             VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail");
01654             break;
01655         }
01656         maxConv_ms = u8;
01657         AveTime = _GetAveTotalTime(dev);
01658         if( AveTime <0 ){
01659             status=-1;
01660             break;
01661         }
01662  
01663         convergTime_us = maxConv_ms * cMicroSecPerMilliSec - AveTime;
01664         status = VL6180x_RdDWord(dev, 0xB8, &fineThresh);
01665         if( status ) {
01666             VL6180x_ErrLog("reg 0xB8 rd fail");
01667             break;
01668         }
01669         fineThresh*=256;
01670         eceThresh      = ece_factor_m * cEceSampleTime_us * fineThresh/(convergTime_us * ece_factor_d);
01671  
01672         status=VL6180x_WrWord(dev, SYSRANGE_EARLY_CONVERGENCE_ESTIMATE, (uint16_t)eceThresh);
01673     }
01674     while(0);
01675  
01676     LOG_FUNCTION_END(status);
01677     return status;
01678 }
01679  
01680 /*
01681  * Return >0 = time
01682  *       <0 1 if fail to get read data from device to compute time
01683  */
01684 int32_t VL6180X::_GetAveTotalTime(VL6180xDev_t dev) {
01685     uint32_t cFwOverhead_us = 24;
01686     uint32_t cVcpSetupTime_us = 70;
01687     uint32_t cPLL2_StartupDelay_us = 200;
01688     uint8_t cMeasMask = 0x07;
01689     uint32_t Samples;
01690     uint32_t SamplePeriod;
01691     uint32_t SingleTime_us;
01692     int32_t TotalAveTime_us;
01693     uint8_t u8;
01694     int status;
01695  
01696     LOG_FUNCTION_START("");
01697  
01698     status = VL6180x_RdByte(dev, 0x109, &u8);
01699     if (status) {
01700         VL6180x_ErrLog("rd 0x109 fail");
01701         return -1;
01702     }
01703     Samples = u8 & cMeasMask;
01704     status = VL6180x_RdByte(dev, READOUT_AVERAGING_SAMPLE_PERIOD, &u8);
01705     if (status) {
01706         VL6180x_ErrLog("i2c READOUT_AVERAGING_SAMPLE_PERIOD fail");
01707         return -1;
01708     }
01709     SamplePeriod = u8;
01710     SingleTime_us = cFwOverhead_us + cVcpSetupTime_us + (SamplePeriod * 10);
01711     TotalAveTime_us = (Samples + 1) * SingleTime_us + cPLL2_StartupDelay_us;
01712  
01713     LOG_FUNCTION_END(TotalAveTime_us);
01714     return TotalAveTime_us;
01715 }
01716  
01717 #if VL6180x_HAVE_DMAX_RANGING
01718 #define _GetDMaxDataRetSignalAt400mm(dev) VL6180xDevDataGet(dev, DMaxData.retSignalAt400mm)
01719 #else
01720 #define _GetDMaxDataRetSignalAt400mm(dev) 375 // Use a default high value
01721 #endif
01722  
01723  
01724 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
01725  
01726 #define FILTER_STDDEV_SAMPLES           6
01727 #define MIN_FILTER_STDDEV_SAMPLES       3
01728 #define MIN_FILTER_VALID_STDDEV_SAMPLES 3
01729 #define FILTER_INVALID_DISTANCE     65535
01730  
01731 #define _FilterData(field) VL6180xDevDataGet(dev, FilterData.field)
01732 /*
01733  * One time init
01734  */
01735 static int _filter_Init( VL6180xDev_t dev) {
01736     int i;
01737     _FilterData(MeasurementIndex) = 0;
01738  
01739     _FilterData(Default_ZeroVal) = 0;
01740     _FilterData(Default_VAVGVal) = 0;
01741     _FilterData(NoDelay_ZeroVal) = 0;
01742     _FilterData(NoDelay_VAVGVal) = 0;
01743     _FilterData(Previous_VAVGDiff) = 0;
01744  
01745     _FilterData(StdFilteredReads) = 0;
01746  
01747     for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
01748         _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
01749         _FilterData(LastReturnRates)[i] = 0;
01750     }
01751     return 0;
01752 }
01753  
01754  
01755 static uint32_t _filter_StdDevDamper(uint32_t AmbientRate, uint32_t SignalRate, const uint32_t StdDevLimitLowLight, const uint32_t StdDevLimitLowLightSNR, const uint32_t StdDevLimitHighLight, const uint32_t StdDevLimitHighLightSNR) {
01756     uint32_t newStdDev;
01757     uint16_t SNR;
01758  
01759     if (AmbientRate > 0)
01760         SNR = (uint16_t) ((100 * SignalRate) / AmbientRate);
01761     else
01762         SNR = 9999;
01763  
01764     if (SNR >= StdDevLimitLowLightSNR) {
01765         newStdDev = StdDevLimitLowLight;
01766     } else {
01767         if (SNR <= StdDevLimitHighLightSNR)
01768             newStdDev = StdDevLimitHighLight;
01769         else {
01770             newStdDev = (uint32_t) (StdDevLimitHighLight + (SNR - StdDevLimitHighLightSNR) * (int) (StdDevLimitLowLight - StdDevLimitHighLight) / (StdDevLimitLowLightSNR - StdDevLimitHighLightSNR));
01771         }
01772     }
01773  
01774     return newStdDev;
01775 }
01776  
01777  
01778 /*
01779  * Return <0 on error
01780  */
01781 int32_t VL6180X::_filter_Start(VL6180xDev_t dev, uint16_t m_trueRange_mm, uint16_t m_rawRange_mm, uint32_t m_rtnSignalRate, uint32_t m_rtnAmbientRate, uint16_t errorCode) {
01782     int status;
01783     uint16_t m_newTrueRange_mm = 0;
01784  
01785     uint16_t i;
01786     uint16_t bypassFilter = 0;
01787  
01788     uint16_t registerValue;
01789  
01790     uint32_t register32BitsValue1;
01791     uint32_t register32BitsValue2;
01792  
01793     uint16_t ValidDistance = 0;
01794  
01795     uint16_t WrapAroundFlag = 0;
01796     uint16_t NoWrapAroundFlag = 0;
01797     uint16_t NoWrapAroundHighConfidenceFlag = 0;
01798  
01799     uint16_t FlushFilter = 0;
01800     uint32_t RateChange = 0;
01801  
01802     uint16_t StdDevSamples = 0;
01803     uint32_t StdDevDistanceSum = 0;
01804     uint32_t StdDevDistanceMean = 0;
01805     uint32_t StdDevDistance = 0;
01806     uint32_t StdDevRateSum = 0;
01807     uint32_t StdDevRateMean = 0;
01808     uint32_t StdDevRate = 0;
01809     uint32_t StdDevLimitWithTargetMove = 0;
01810  
01811     uint32_t VAVGDiff;
01812     uint32_t IdealVAVGDiff;
01813     uint32_t MinVAVGDiff;
01814     uint32_t MaxVAVGDiff;
01815  
01816     /* Filter Parameters */
01817     static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit = 60;
01818     static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit_ROM = 800; // Shall be adapted depending on crossTalk
01819     static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit2 = 165;
01820     static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit2_ROM = 180; // Shall be adapted depending on crossTalk and device sensitivity
01821  
01822     static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateFilterLimit_ROM = 850; // Shall be adapted depending on crossTalk and device sensitivity
01823     static const uint16_t ROMABLE_DATA WrapAroundHighRawRangeFilterLimit = 350;
01824     static const uint32_t ROMABLE_DATA WrapAroundHighReturnRateFilterLimit_ROM = 1400; // Shall be adapted depending on crossTalk and device sensitivity
01825  
01826     static const uint32_t ROMABLE_DATA WrapAroundMaximumAmbientRateFilterLimit = 7500;
01827  
01828     /*  Temporal filter data and flush values */
01829     static const uint32_t ROMABLE_DATA MinReturnRateFilterFlush = 75;
01830     static const uint32_t ROMABLE_DATA MaxReturnRateChangeFilterFlush = 50;
01831  
01832     /* STDDEV values and damper values */
01833  
01834     static const uint32_t ROMABLE_DATA StdDevLimitLowLight = 300;
01835     static const uint32_t ROMABLE_DATA StdDevLimitLowLightSNR = 30; /* 0.3 */
01836     static const uint32_t ROMABLE_DATA StdDevLimitHighLight = 2500;
01837     static const uint32_t ROMABLE_DATA StdDevLimitHighLightSNR = 5; /* 0.05 */
01838  
01839     static const uint32_t ROMABLE_DATA StdDevHighConfidenceSNRLimit = 8;
01840  
01841     static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevLimit = 90000;
01842  
01843     static const uint32_t ROMABLE_DATA StdDevMovingTargetReturnRateLimit = 3500;
01844     static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevForReturnRateLimit = 5000;
01845  
01846     static const uint32_t ROMABLE_DATA MAX_VAVGDiff = 1800;
01847  
01848     /* WrapAroundDetection variables */
01849     static const uint16_t ROMABLE_DATA WrapAroundNoDelayCheckPeriod = 2;
01850     static const uint16_t ROMABLE_DATA StdFilteredReadsIncrement = 2;
01851     static const uint16_t ROMABLE_DATA StdMaxFilteredReads = 4;
01852     
01853     uint32_t SignalRateDMax;
01854     uint32_t WrapAroundLowReturnRateLimit; 
01855     uint32_t WrapAroundLowReturnRateLimit2;
01856     uint32_t WrapAroundLowReturnRateFilterLimit;
01857     uint32_t WrapAroundHighReturnRateFilterLimit; 
01858  
01859     uint8_t u8, u8_2;
01860     uint32_t XTalkCompRate_KCps;
01861     uint32_t StdDevLimit = 300;
01862     uint32_t MaxOrInvalidDistance =   255*_GetUpscale(dev);
01863     /* #define MaxOrInvalidDistance  (uint16_t) (255 * 3) */
01864  
01865     /* Check if distance is Valid or not */
01866     switch (errorCode) {
01867     case 0x0C:
01868         m_trueRange_mm = MaxOrInvalidDistance;
01869         ValidDistance = 0;
01870         break;
01871     case 0x0D:
01872         m_trueRange_mm = MaxOrInvalidDistance;
01873         ValidDistance = 1;
01874         break;
01875     case 0x0F:
01876         m_trueRange_mm = MaxOrInvalidDistance;
01877         ValidDistance = 1;
01878         break;
01879     default:
01880         if (m_rawRange_mm >= MaxOrInvalidDistance) {
01881             ValidDistance = 0;
01882         } else {
01883             ValidDistance = 1;
01884         }
01885         break;
01886     }
01887     m_newTrueRange_mm = m_trueRange_mm;
01888     
01889     XTalkCompRate_KCps = VL6180xDevDataGet(dev, XTalkCompRate_KCps );
01890  
01891     
01892     //Update signal rate limits depending on crosstalk
01893     SignalRateDMax = (uint32_t)_GetDMaxDataRetSignalAt400mm(dev) + XTalkCompRate_KCps;
01894     WrapAroundLowReturnRateLimit = WrapAroundLowReturnRateLimit_ROM  + XTalkCompRate_KCps; 
01895     WrapAroundLowReturnRateLimit2 = ((WrapAroundLowReturnRateLimit2_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
01896     WrapAroundLowReturnRateFilterLimit = ((WrapAroundLowReturnRateFilterLimit_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
01897     WrapAroundHighReturnRateFilterLimit = ((WrapAroundHighReturnRateFilterLimit_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps; 
01898  
01899  
01900     /* Checks on low range data */
01901     if ((m_rawRange_mm < WrapAroundLowRawRangeLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit)) {
01902         m_newTrueRange_mm = MaxOrInvalidDistance;
01903         bypassFilter = 1;
01904     }
01905     if ((m_rawRange_mm < WrapAroundLowRawRangeLimit2) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit2)) {
01906         m_newTrueRange_mm = MaxOrInvalidDistance;
01907         bypassFilter = 1;
01908     }
01909  
01910     /* Checks on Ambient rate level */
01911     if (m_rtnAmbientRate > WrapAroundMaximumAmbientRateFilterLimit) {
01912         /* Too high ambient rate */
01913         FlushFilter = 1;
01914         bypassFilter = 1;
01915     }
01916     /*  Checks on Filter flush */
01917     if (m_rtnSignalRate < MinReturnRateFilterFlush) {
01918         /* Completely lost target, so flush the filter */
01919         FlushFilter = 1;
01920         bypassFilter = 1;
01921     }
01922     if (_FilterData(LastReturnRates)[0] != 0) {
01923         if (m_rtnSignalRate > _FilterData(LastReturnRates)[0])
01924             RateChange = (100 * (m_rtnSignalRate - _FilterData(LastReturnRates)[0])) / _FilterData(LastReturnRates)[0];
01925         else
01926             RateChange = (100 * (_FilterData(LastReturnRates)[0] - m_rtnSignalRate)) / _FilterData(LastReturnRates)[0];
01927     } else
01928         RateChange = 0;
01929     if (RateChange > MaxReturnRateChangeFilterFlush) {
01930         FlushFilter = 1;
01931     }
01932 /* TODO optimize filter  using circular buffer */
01933     if (FlushFilter == 1) {
01934         _FilterData(MeasurementIndex) = 0;
01935         for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
01936             _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
01937             _FilterData(LastReturnRates)[i] = 0;
01938         }
01939     } else {
01940         for (i = (uint16_t) (FILTER_NBOF_SAMPLES - 1); i > 0; i--) {
01941             _FilterData(LastTrueRange)[i] = _FilterData(LastTrueRange)[i - 1];
01942             _FilterData(LastReturnRates)[i] = _FilterData(LastReturnRates)[i - 1];
01943         }
01944     }
01945     if (ValidDistance == 1)
01946         _FilterData(LastTrueRange)[0] = m_trueRange_mm;
01947     else
01948         _FilterData(LastTrueRange)[0] = FILTER_INVALID_DISTANCE;
01949     _FilterData(LastReturnRates)[0] = m_rtnSignalRate;
01950  
01951     /* Check if we need to go through the filter or not */
01952     if (!(((m_rawRange_mm < WrapAroundHighRawRangeFilterLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateFilterLimit)) || ((m_rawRange_mm >= WrapAroundHighRawRangeFilterLimit) && (m_rtnSignalRate < WrapAroundHighReturnRateFilterLimit))))
01953         bypassFilter = 1;
01954  
01955     /* Check which kind of measurement has been made */
01956     status = VL6180x_RdByte(dev, 0x01AC, &u8 );
01957     if( status ){
01958         VL6180x_ErrLog("0x01AC rd fail");
01959         goto done_err;
01960     }
01961     registerValue =u8;
01962  
01963     /* Read data for filtering */
01964     status = VL6180x_RdByte(dev, 0x10C, &u8 ); /* read only 8 lsb bits */
01965     if( status ){
01966         VL6180x_ErrLog("0x010C rd fail");
01967         goto done_err;
01968     }
01969     register32BitsValue1=u8;
01970     status = VL6180x_RdByte(dev, 0x0110, &u8); /* read only 8 lsb bits */
01971     if( status ){
01972         VL6180x_ErrLog("0x0110 rd fail");
01973         goto done_err;
01974     }
01975     register32BitsValue2 = u8;
01976  
01977     if (registerValue == 0x3E) {
01978         _FilterData(Default_ZeroVal) = register32BitsValue1;
01979         _FilterData(Default_VAVGVal) = register32BitsValue2;
01980     } else {
01981         _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
01982         _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
01983     }
01984  
01985     if (bypassFilter == 1) {
01986         /* Do not go through the filter */
01987         if (registerValue != 0x3E) {
01988             status = VL6180x_WrByte(dev, 0x1AC, 0x3E);
01989             if( status ){
01990                 VL6180x_ErrLog("0x01AC bypass wr fail");
01991                 goto done_err;
01992             }
01993             status = VL6180x_WrByte(dev, 0x0F2, 0x01);
01994             if( status ){
01995                 VL6180x_ErrLog("0x0F2 bypass wr fail");
01996                 goto done_err;
01997             }
01998         }
01999         /* Set both Default and NoDelay To same value */
02000         _FilterData(Default_ZeroVal) = register32BitsValue1;
02001         _FilterData(Default_VAVGVal) = register32BitsValue2;
02002         _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
02003         _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
02004         _FilterData(MeasurementIndex) = 0;
02005  
02006         return m_newTrueRange_mm;
02007     }
02008  
02009     if (_FilterData(MeasurementIndex) % WrapAroundNoDelayCheckPeriod == 0) {
02010         u8=0x3C;
02011         u8_2 = 0x05;
02012     } else {
02013         u8=0x3E;
02014         u8_2 = 0x01;
02015     }
02016     status = VL6180x_WrByte(dev, 0x01AC, u8);
02017     if( status ){
02018         VL6180x_ErrLog("0x01AC wr fail");
02019         goto done_err;
02020     }
02021     status = VL6180x_WrByte(dev, 0x0F2, u8_2);
02022     if( status ){
02023         VL6180x_ErrLog("0x0F2  wr fail");
02024         goto done_err;
02025     }
02026  
02027  
02028     _FilterData(MeasurementIndex)++;
02029  
02030     /* Computes current VAVGDiff */
02031     if (_FilterData(Default_VAVGVal) > _FilterData(NoDelay_VAVGVal))
02032         VAVGDiff = _FilterData(Default_VAVGVal) - _FilterData(NoDelay_VAVGVal);
02033     else
02034         VAVGDiff = 0;
02035     _FilterData(Previous_VAVGDiff) = VAVGDiff;
02036  
02037     /* Check the VAVGDiff */
02038     if (_FilterData(Default_ZeroVal) > _FilterData(NoDelay_ZeroVal))
02039         IdealVAVGDiff = _FilterData(Default_ZeroVal) - _FilterData(NoDelay_ZeroVal);
02040     else
02041         IdealVAVGDiff = _FilterData(NoDelay_ZeroVal) - _FilterData(Default_ZeroVal);
02042     if (IdealVAVGDiff > MAX_VAVGDiff)
02043         MinVAVGDiff = IdealVAVGDiff - MAX_VAVGDiff;
02044     else
02045         MinVAVGDiff = 0;
02046     MaxVAVGDiff = IdealVAVGDiff + MAX_VAVGDiff;
02047     if (VAVGDiff < MinVAVGDiff || VAVGDiff > MaxVAVGDiff) {
02048         WrapAroundFlag = 1;
02049     } else {
02050         /* Go through filtering check */
02051  
02052         /* StdDevLimit Damper on SNR */
02053         StdDevLimit = _filter_StdDevDamper(m_rtnAmbientRate, m_rtnSignalRate, StdDevLimitLowLight, StdDevLimitLowLightSNR, StdDevLimitHighLight, StdDevLimitHighLightSNR);
02054  
02055         /* Standard deviations computations */
02056         StdDevSamples = 0;
02057         StdDevDistanceSum = 0;
02058         StdDevDistanceMean = 0;
02059         StdDevDistance = 0;
02060         StdDevRateSum = 0;
02061         StdDevRateMean = 0;
02062         StdDevRate = 0;
02063         for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
02064             if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
02065                 StdDevSamples = (uint16_t) (StdDevSamples + 1);
02066                 StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + _FilterData(LastTrueRange)[i]);
02067                 StdDevRateSum = (uint32_t) (StdDevRateSum + _FilterData(LastReturnRates)[i]);
02068             }
02069         }
02070         if (StdDevSamples > 0) {
02071             StdDevDistanceMean = (uint32_t) (StdDevDistanceSum / StdDevSamples);
02072             StdDevRateMean = (uint32_t) (StdDevRateSum / StdDevSamples);
02073         }
02074         /* TODO optimize shorten Std dev in aisngle loop computation using sum of x2 - (sum of x)2 */
02075         StdDevSamples = 0;
02076         StdDevDistanceSum = 0;
02077         StdDevRateSum = 0;
02078         for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
02079             if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
02080                 StdDevSamples = (uint16_t) (StdDevSamples + 1);
02081                 StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + (int) (_FilterData(LastTrueRange)[i] - StdDevDistanceMean) * (int) (_FilterData(LastTrueRange)[i] - StdDevDistanceMean));
02082                 StdDevRateSum = (uint32_t) (StdDevRateSum + (int) (_FilterData(LastReturnRates)[i] - StdDevRateMean) * (int) (_FilterData(LastReturnRates)[i] - StdDevRateMean));
02083             }
02084         }
02085         if (StdDevSamples >= MIN_FILTER_STDDEV_SAMPLES) {
02086             StdDevDistance = (uint16_t) (StdDevDistanceSum / StdDevSamples);
02087             StdDevRate = (uint16_t) (StdDevRateSum / StdDevSamples);
02088         } else {
02089             StdDevDistance = 0;
02090             StdDevRate = 0;
02091         }
02092  
02093         /* Check Return rate standard deviation */
02094         if (StdDevRate < StdDevMovingTargetStdDevLimit) {
02095             if (StdDevSamples < MIN_FILTER_VALID_STDDEV_SAMPLES) {
02096                 m_newTrueRange_mm = MaxOrInvalidDistance;
02097             } else {
02098                 /* Check distance standard deviation */
02099                 if (StdDevRate < StdDevMovingTargetReturnRateLimit)
02100                     StdDevLimitWithTargetMove = StdDevLimit + (((StdDevMovingTargetStdDevForReturnRateLimit - StdDevLimit) * StdDevRate) / StdDevMovingTargetReturnRateLimit);
02101                 else
02102                     StdDevLimitWithTargetMove = StdDevMovingTargetStdDevForReturnRateLimit;
02103  
02104                 if ((StdDevDistance * StdDevHighConfidenceSNRLimit) < StdDevLimitWithTargetMove) {
02105                     NoWrapAroundHighConfidenceFlag = 1;
02106                 } else {
02107                     if (StdDevDistance < StdDevLimitWithTargetMove) {
02108                         if (StdDevSamples >= MIN_FILTER_VALID_STDDEV_SAMPLES) {
02109                             NoWrapAroundFlag = 1;
02110                         } else {
02111                             m_newTrueRange_mm = MaxOrInvalidDistance;
02112                         }
02113                     } else {
02114                         WrapAroundFlag = 1;
02115                     }
02116                 }
02117             }
02118         } else {
02119             WrapAroundFlag = 1;
02120         }
02121     }
02122  
02123     if (m_newTrueRange_mm == MaxOrInvalidDistance) {
02124         if (_FilterData(StdFilteredReads) > 0)
02125             _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - 1);
02126     } else {
02127         if (WrapAroundFlag == 1) {
02128             m_newTrueRange_mm = MaxOrInvalidDistance;
02129             _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) + StdFilteredReadsIncrement);
02130             if (_FilterData(StdFilteredReads) > StdMaxFilteredReads)
02131                 _FilterData(StdFilteredReads) = StdMaxFilteredReads;
02132         } else {
02133             if (NoWrapAroundFlag == 1) {
02134                 if (_FilterData(StdFilteredReads) > 0) {
02135                     m_newTrueRange_mm = MaxOrInvalidDistance;
02136                     if (_FilterData(StdFilteredReads) > StdFilteredReadsIncrement)
02137                         _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - StdFilteredReadsIncrement);
02138                     else
02139                         _FilterData(StdFilteredReads) = 0;
02140                 }
02141             } else {
02142                 if (NoWrapAroundHighConfidenceFlag == 1) {
02143                     _FilterData(StdFilteredReads) = 0;
02144                 }
02145             }
02146         }
02147     }
02148  
02149     return m_newTrueRange_mm;
02150     done_err:
02151     return -1;
02152  
02153     #undef MaxOrInvalidDistance
02154 }
02155  
02156  
02157 int VL6180X::_filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData) {
02158     uint32_t m_rawRange_mm = 0;
02159     int32_t  FilteredRange;
02160     const uint8_t scaler = _GetUpscale(dev);
02161     uint8_t u8;
02162     int status;
02163  
02164     do {
02165         status = VL6180x_RdByte(dev, RESULT_RANGE_RAW, &u8);
02166         if (status) {
02167             VL6180x_ErrLog("RESULT_RANGE_RAW rd fail");
02168             break;
02169         }
02170         m_rawRange_mm = u8;
02171  
02172         FilteredRange = _filter_Start(dev, pRangeData->range_mm , (m_rawRange_mm * scaler), pRangeData->rtnRate , pRangeData->rtnAmbRate , pRangeData->errorStatus );
02173         if( FilteredRange<0 ){
02174             status = -1;
02175             break;
02176         }
02177         pRangeData->FilteredData .range_mm = FilteredRange;
02178         pRangeData->FilteredData .rawRange_mm  = m_rawRange_mm * scaler;
02179     } while (0);
02180     return status;
02181 }
02182  
02183 #undef _FilterData
02184 #undef FILTER_STDDEV_SAMPLES
02185 #undef MIN_FILTER_STDDEV_SAMPLES
02186 #undef MIN_FILTER_VALID_STDDEV_SAMPLES
02187 #undef FILTER_INVALID_DISTANCE
02188  
02189 #endif /* VL6180x_WRAP_AROUND_FILTER_SUPPORT */
02190  
02191 #ifdef VL6180x_HAVE_RATE_DATA
02192  
02193 int VL6180X::_GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData) {
02194     uint32_t m_rtnConvTime = 0;
02195     uint32_t m_rtnSignalRate = 0;
02196     uint32_t m_rtnAmbientRate = 0;
02197     uint32_t m_rtnSignalCount = 0;
02198     uint32_t m_rtnAmbientCount = 0;
02199     uint32_t m_refConvTime = 0;
02200     uint32_t cRtnSignalCountMax = 0x7FFFFFFF;
02201     uint32_t cDllPeriods = 6;
02202     uint32_t calcConvTime = 0;
02203  
02204     int status;
02205  
02206     do {
02207  
02208         status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_SIGNAL_COUNT, &m_rtnSignalCount);
02209         if (status) {
02210             VL6180x_ErrLog("RESULT_RANGE_RETURN_SIGNAL_COUNT rd fail");
02211             break;
02212         }
02213         if (m_rtnSignalCount > cRtnSignalCountMax) {
02214             m_rtnSignalCount = 0;
02215         }
02216  
02217         status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_AMB_COUNT, &m_rtnAmbientCount);
02218         if (status) {
02219             VL6180x_ErrLog("RESULT_RANGE_RETURN_AMB_COUNTrd fail");
02220             break;
02221         }
02222  
02223  
02224         status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_CONV_TIME, &m_rtnConvTime);
02225         if (status) {
02226             VL6180x_ErrLog("RESULT_RANGE_RETURN_CONV_TIME rd fail");
02227             break;
02228         }
02229  
02230         status = VL6180x_RdDWord(dev, RESULT_RANGE_REFERENCE_CONV_TIME, &m_refConvTime);
02231         if (status) {
02232             VL6180x_ErrLog("RESULT_RANGE_REFERENCE_CONV_TIME rd fail");
02233             break;
02234         }
02235  
02236         pRangeData->rtnConvTime  = m_rtnConvTime;
02237         pRangeData->refConvTime  = m_refConvTime;
02238  
02239         calcConvTime = m_refConvTime;
02240         if (m_rtnConvTime > m_refConvTime) {
02241             calcConvTime = m_rtnConvTime;
02242         }
02243         if (calcConvTime == 0)
02244             calcConvTime = 63000;
02245  
02246         m_rtnSignalRate = (m_rtnSignalCount * 1000) / calcConvTime;
02247         m_rtnAmbientRate = (m_rtnAmbientCount * cDllPeriods * 1000) / calcConvTime;
02248  
02249         pRangeData->rtnRate  = m_rtnSignalRate;
02250         pRangeData->rtnAmbRate  = m_rtnAmbientRate;
02251  
02252  
02253     } while (0);
02254     return status;
02255 }
02256 #endif /* VL6180x_HAVE_RATE_DATA */
02257  
02258  
02259 int VL6180X::VL6180x_DMaxSetState(VL6180xDev_t dev, int state){
02260     int status;
02261     LOG_FUNCTION_START("%d", state);
02262 #if VL6180x_HAVE_DMAX_RANGING
02263     VL6180xDevDataSet(dev,DMaxEnable, state);
02264     if( state ){
02265         status = _DMax_InitData(dev);
02266     }
02267     else {
02268         status = 0;
02269     }
02270 #else
02271     status =  NOT_SUPPORTED;
02272 #endif
02273     LOG_FUNCTION_END(status);
02274     return status;
02275 }
02276  
02277 int VL6180X::VL6180x_DMaxGetState(VL6180xDev_t dev){
02278     int status;
02279     LOG_FUNCTION_START("");
02280 #if VL6180x_HAVE_DMAX_RANGING
02281     status = VL6180xDevDataGet(dev,DMaxEnable);
02282 #else
02283     status = 0;
02284 #endif
02285     LOG_FUNCTION_END(status);
02286     return status;
02287 }
02288  
02289  
02290 #if VL6180x_HAVE_DMAX_RANGING
02291  
02292 #define _DMaxData(field) VL6180xDevDataGet(dev, DMaxData.field)
02293 /*
02294  * Convert fix point  x.7 to KCpount per sec
02295  */
02296  
02297 #ifndef VL6180x_PLATFORM_PROVIDE_SQRT
02298  
02299 /*
02300  * 32 bit integer square root with not so bad precision (integer result) and is quite fast
02301  * see http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
02302  */
02303 uint32_t VL6180x_SqrtUint32(uint32_t num) {
02304     uint32_t res = 0;
02305     uint32_t bit = 1 << 30; /* The second-to-top bit is set: 1 << 30 for 32 bits */
02306  
02307     /* "bit" starts at the highest power of four <= the argument. */
02308     while (bit > num)
02309         bit >>= 2;
02310  
02311     while (bit != 0) {
02312         if (num >= res + bit) {
02313             num -= res + bit;
02314             res = (res >> 1) + bit;
02315         }
02316         else
02317             res >>= 1;
02318         bit >>= 2;
02319     }
02320     return res;
02321 }
02322 #endif
02323  
02324  
02325 /* DMax one time init */
02326 void _DMax_OneTimeInit(VL6180xDev_t dev){
02327     _DMaxData(ambTuningWindowFactor_K)=DEF_AMBIENT_TUNING;
02328 }
02329  
02330  
02331 static uint32_t _DMax_RawValueAtRateKCps(VL6180xDev_t dev, int32_t rate){
02332     uint32_t snrLimit_K;
02333     int32_t DMaxSq;
02334     uint32_t RawDMax;
02335     DMaxFix_t retSignalAt400mm;
02336     uint32_t ambTuningWindowFactor_K;
02337  
02338  
02339     ambTuningWindowFactor_K = _DMaxData(ambTuningWindowFactor_K);
02340     snrLimit_K              = _DMaxData(snrLimit_K);
02341     retSignalAt400mm        = _DMaxData(retSignalAt400mm); /* 12 to 18 bits Kcps */
02342     if( rate > 0 ){
02343         DMaxSq = 400*400*1000 / rate -(400*400/330); /* K of (1/RtnAmb -1/330 )=> 30bit- (12-18)bit  => 12-18 bits*/
02344         if( DMaxSq<= 0){
02345             RawDMax = 0;
02346         }
02347         else{
02348             /* value can be more 32 bit so base on raneg apply *retSignalAt400mm before or after division to presevr accuracy */
02349             if( DMaxSq< (2<<12)  ){
02350                 DMaxSq = DMaxSq*retSignalAt400mm/(snrLimit_K+ambTuningWindowFactor_K);       /* max 12 + 12 to 18 -10 => 12-26 bit */
02351             }else{
02352                 DMaxSq = DMaxSq/(snrLimit_K+ambTuningWindowFactor_K)*retSignalAt400mm;       /* 12 to 18 -10 + 12 to 18 *=> 12-26 bit */
02353             }
02354             RawDMax=VL6180x_SqrtUint32(DMaxSq);
02355         }
02356     }
02357     else{
02358         RawDMax = 0x7FFFFFFF; /* bigest possibmle 32bit signed value */
02359     }
02360     return RawDMax;
02361 }
02362  
02363 /*
02364  * fetch static data from register to avoid re-read
02365  * precompute all intermediate constant and cliipings
02366  *
02367  * to be re-used/call on  changes of :
02368  *  0x2A
02369  *  SYSRANGE_MAX_AMBIENT_LEVEL_MULT
02370  *  Dev Data XtalkComRate_KCPs
02371  *  SYSRANGE_MAX_CONVERGENCE_TIME
02372  *  SYSRANGE_RANGE_CHECK_ENABLES    mask RANGE_CHECK_RANGE_ENABLE_MASK
02373  *  range 0xb8-0xbb (0xbb)
02374  */
02375 int VL6180X::_DMax_InitData(VL6180xDev_t dev){
02376     int status, warning;
02377     uint8_t u8;
02378     uint16_t u16;
02379     uint32_t u32;
02380     uint32_t Reg2A_KCps;
02381     uint32_t RegB8;
02382     uint8_t  MaxConvTime;
02383     uint32_t XTalkCompRate_KCps;
02384     uint32_t RangeIgnoreThreshold;
02385     int32_t minSignalNeeded;
02386     uint8_t SysRangeCheckEn;
02387     uint8_t snrLimit;
02388     warning=0;
02389  
02390     static const int ROMABLE_DATA MaxConvTimeAdjust=-4;
02391  
02392     LOG_FUNCTION_START("");
02393     do{
02394         status = VL6180x_RdByte(dev, 0x02A ,&u8);
02395         if( status ){
02396             VL6180x_ErrLog("Reg 0x02A rd fail");
02397             break;
02398         }
02399  
02400         if( u8 == 0 ) {
02401             warning = CALIBRATION_WARNING;
02402             u8 = 40; /* use a default average value */
02403         }
02404         Reg2A_KCps = Fix7_2_KCPs(u8); /* convert to KCPs */
02405  
02406         status = VL6180x_RdByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, &SysRangeCheckEn);
02407         if (status) {
02408             VL6180x_ErrLog("SYSRANGE_RANGE_CHECK_ENABLES rd fail ");
02409             break;
02410         }
02411  
02412         status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &MaxConvTime);
02413         if( status){
02414             VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
02415             break;
02416         }
02417  
02418         status = VL6180x_RdDWord(dev, 0x0B8, &RegB8);
02419         if( status ){
02420             VL6180x_ErrLog("reg 0x0B8 rd fail ");
02421             break;
02422         }
02423  
02424         status = VL6180x_RdByte(dev, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, &snrLimit);
02425         if( status){
02426             VL6180x_ErrLog("SYSRANGE_MAX_AMBIENT_LEVEL_MULT rd fail ");
02427             break;
02428         }
02429         _DMaxData(snrLimit_K) = (int32_t)16*1000/snrLimit;
02430         XTalkCompRate_KCps =   VL6180xDevDataGet(dev, XTalkCompRate_KCps );
02431  
02432         if( Reg2A_KCps >= XTalkCompRate_KCps){
02433             _DMaxData(retSignalAt400mm)=( Reg2A_KCps - XTalkCompRate_KCps);
02434         }
02435         else{
02436             _DMaxData(retSignalAt400mm)=0;             /* Reg2A_K - XTalkCompRate_KCp <0 is invalid */
02437         }
02438  
02439         /* if xtalk range check is off omit it in snr clipping */
02440         if( SysRangeCheckEn&RANGE_CHECK_RANGE_ENABLE_MASK ){
02441             status = VL6180x_RdWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &u16);
02442             if( status){
02443                 VL6180x_ErrLog("SYSRANGE_RANGE_IGNORE_THRESHOLD rd fail ");
02444                 break;
02445             }
02446             RangeIgnoreThreshold = Fix7_2_KCPs(u16);
02447         }
02448         else{
02449             RangeIgnoreThreshold  = 0;
02450         }
02451  
02452         minSignalNeeded = (RegB8*256)/((int32_t)MaxConvTime+(int32_t)MaxConvTimeAdjust); /* KCps 8+8 bit -(1 to 6 bit) => 15-10 bit */
02453         /* minSignalNeeded = max ( minSignalNeeded,  RangeIgnoreThreshold - XTalkCompRate_KCps) */
02454         if( minSignalNeeded  <= RangeIgnoreThreshold - XTalkCompRate_KCps )
02455             minSignalNeeded  =  RangeIgnoreThreshold - XTalkCompRate_KCps;
02456  
02457         u32 = (minSignalNeeded*(uint32_t)snrLimit)/16;
02458         _DMaxData(ClipSnrLimit ) = _DMax_RawValueAtRateKCps(dev, u32 ); /* clip to dmax to min signal snr limit rate*/
02459     }
02460     while(0);
02461     if( !status )
02462         status = warning;
02463     LOG_FUNCTION_END(status);
02464     return status;
02465 }
02466  
02467 static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange){
02468     uint32_t rtnAmbRate;
02469     int32_t DMax;
02470     int scaling;
02471     uint16_t HwLimitAtScale;
02472     static const int ROMABLE_DATA rtnAmbLowLimit_KCps=330*1000;
02473  
02474     rtnAmbRate = pRange->rtnAmbRate ;
02475     if( rtnAmbRate  < rtnAmbLowLimit_KCps ){
02476         DMax = _DMax_RawValueAtRateKCps( dev, rtnAmbRate);
02477         scaling = _GetUpscale(dev);
02478         HwLimitAtScale=UpperLimitLookUP[scaling - 1];
02479  
02480         if( DMax > _DMaxData(ClipSnrLimit) ){
02481             DMax=_DMaxData(ClipSnrLimit);
02482         }
02483         if( DMax > HwLimitAtScale ){
02484             DMax=HwLimitAtScale;
02485         }
02486         pRange->DMax =DMax;
02487     }
02488     else{
02489         pRange->DMax  = 0;
02490     }
02491     return 0;
02492 }
02493  
02494 #undef _DMaxData
02495 #undef Fix7_2_KCPs
02496  
02497 #endif /* VL6180x_HAVE_DMAX_RANGING */
02498  
02499  
02500 /******************************************************************************/
02501 /******************************************************************************/
02502  
02503  
02504  
02505 /****************** Write and read functions from I2C *************************/
02506  
02507 int VL6180X::VL6180x_WrByte(VL6180xDev_t dev, uint16_t index, uint8_t data)
02508 {
02509    int  status;
02510  
02511    status=VL6180x_I2CWrite(Device->I2cAddr, index, &data,(uint8_t)1);
02512    return status;
02513 }
02514  
02515 int VL6180X::VL6180x_WrWord(VL6180xDev_t dev, uint16_t index, uint16_t data)
02516 {
02517    int  status;
02518    
02519    status=VL6180x_I2CWrite(Device->I2cAddr, index, (uint8_t *)&data,(uint8_t)2);
02520    return status;
02521 }
02522  
02523 int VL6180X::VL6180x_WrDWord(VL6180xDev_t dev, uint16_t index, uint32_t data)
02524 {
02525    int  status;
02526    
02527    status=VL6180x_I2CWrite(Device->I2cAddr, index, (uint8_t *)&data,(uint8_t)4);
02528    return status;
02529 }
02530  
02531 int VL6180X::VL6180x_RdByte(VL6180xDev_t dev, uint16_t index, uint8_t *data)
02532 {
02533    int  status;
02534     
02535    uint8_t buffer;
02536    status=VL6180x_I2CRead(Device->I2cAddr, index, &buffer,1);
02537    if(!status)
02538    {
02539       *data=buffer;
02540    }
02541    return status;
02542 }
02543  
02544 int VL6180X::VL6180x_RdWord(VL6180xDev_t dev, uint16_t index, uint16_t *data)
02545 {
02546    int  status;
02547     
02548    uint8_t buffer[2];
02549    status=VL6180x_I2CRead(Device->I2cAddr, index, buffer, 2);
02550    if(!status)
02551    {
02552       memcpy(data, buffer, 2);
02553    }
02554    return status;
02555 }
02556  
02557 int VL6180X::VL6180x_RdDWord(VL6180xDev_t dev, uint16_t index, uint32_t *data)
02558 {
02559    int status;
02560    uint8_t buffer[4];
02561    status=VL6180x_I2CRead(Device->I2cAddr, index, buffer,4);
02562    if(!status)
02563    {
02564       memcpy(data, buffer, 4);
02565    }
02566    return status;
02567 }
02568  
02569 int VL6180X::VL6180x_UpdateByte(VL6180xDev_t dev, uint16_t index, uint8_t AndData, uint8_t OrData)
02570 {
02571    int  status;
02572    uint8_t buffer;
02573  
02574    status=VL6180x_I2CWrite(Device->I2cAddr, index, (uint8_t *)buffer,(uint8_t)0);
02575    if(!status)
02576    {
02577       /* read data direct onto buffer */
02578       status=VL6180x_I2CRead(Device->I2cAddr, index, &buffer,1);
02579       if(!status)
02580       {
02581          buffer=(buffer & AndData)|OrData;
02582          status=VL6180x_I2CWrite(Device->I2cAddr, index, &buffer, (uint8_t)1);
02583       }
02584    }
02585    return status;
02586 }
02587  
02588 int VL6180X::VL6180x_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
02589 {
02590    int ret;
02591    int i;
02592    uint8_t tmp[TEMP_BUF_SIZE];
02593    uint16_t myRegisterAddr = RegisterAddr;
02594    uint16_t WriteDeviceAddr=0;
02595    
02596    /* First, prepare 8 bits device address in 7bits i2ci format */
02597    WriteDeviceAddr=DeviceAddr*2;
02598    if(NumByteToWrite >= TEMP_BUF_SIZE) return -2;
02599         
02600    /* then prepare 16 bits register address in BE format. Then, send data and STOP condition */
02601    tmp[0] = *(((uint8_t*)&myRegisterAddr)+1);  
02602    tmp[1] = (uint8_t)RegisterAddr;
02603     
02604    if(NumByteToWrite>1)  /* swap data endianess */
02605    {
02606       for(i=0;i<NumByteToWrite;i++)
02607       {
02608          tmp[NumByteToWrite+sizeof(RegisterAddr)-1-i]=pBuffer[i];
02609       }
02610    }
02611    else
02612    {    
02613       memcpy(tmp+sizeof(RegisterAddr), pBuffer, NumByteToWrite);
02614    }
02615    ret = dev_i2c.write(WriteDeviceAddr, (const char*)tmp, NumByteToWrite+sizeof(RegisterAddr), false);
02616  
02617    if(ret) 
02618       return -1;
02619    return 0;
02620 }
02621  
02622 int VL6180X::VL6180x_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
02623 {
02624    int ret,i;
02625    uint8_t tmp[TEMP_BUF_SIZE]; 
02626    uint16_t myRegisterAddr = RegisterAddr;
02627    uint16_t myRegisterAddrBE;
02628    uint16_t ReadDeviceAddr=DeviceAddr;
02629      
02630    ReadDeviceAddr=DeviceAddr*2;    
02631    myRegisterAddrBE = *(((uint8_t*)&myRegisterAddr)+1);
02632    *(((uint8_t*)&myRegisterAddrBE)+1) = (uint8_t)myRegisterAddr;
02633     
02634    /* Send 8 bits device address and 16 bits register address in BE format, with no STOP condition */
02635    ret = dev_i2c.write(ReadDeviceAddr, (const char*)&myRegisterAddrBE, sizeof(RegisterAddr), true);
02636    if(!ret) 
02637    {
02638       ReadDeviceAddr|=0x001;
02639       /* Read data, with STOP condition  */
02640       ret = dev_i2c.read(ReadDeviceAddr, (char*)tmp, NumByteToRead, false);
02641    }    
02642    if(ret) 
02643       return -1;
02644         
02645    if(NumByteToRead>1)  /* swap data endianess */
02646    {
02647       for(i=0;i<NumByteToRead;i++)
02648       {      
02649          pBuffer[i] = tmp[NumByteToRead-1-i];
02650       }
02651    }
02652    else
02653    {      
02654       memcpy(pBuffer, tmp, NumByteToRead);
02655    }       
02656    return 0;
02657 } 
02658 
02659 /******************************************************************************/
02660 
02661 int VL6180X::AlsSetThresholds(uint16_t lux_threshold_low, uint16_t lux_threshold_high)
02662 {
02663    uint32_t AlsAnGain, IntPeriod, AlsScaler , GainFix, RawAlsHigh, RawAlsLow;
02664    uint16_t RawThreshLow, RawThreshHigh;
02665    const uint32_t LuxResxIntIme =(uint32_t)(0.56f* DEF_INT_PEFRIOD *(1<<LUXRES_FIX_PREC));
02666    void *p_low;
02667    void *p_high;
02668  
02669    AlsAnGain=VL6180xDevDataGet(Device, AlsGainCode );
02670    IntPeriod=VL6180xDevDataGet(Device, IntegrationPeriod );
02671    AlsScaler=VL6180xDevDataGet(Device, AlsScaler);
02672    GainFix=AlsGainLookUp[AlsAnGain];
02673    IntPeriod++;
02674    RawAlsLow=lux_threshold_low*AlsScaler*GainFix;
02675    RawAlsLow=RawAlsLow*IntPeriod;
02676    RawAlsLow=RawAlsLow/LuxResxIntIme;  
02677    RawAlsHigh=lux_threshold_high*(AlsScaler*GainFix);
02678    RawAlsHigh=RawAlsHigh*IntPeriod;
02679    RawAlsHigh=RawAlsHigh/LuxResxIntIme;
02680    p_low=&RawAlsLow;
02681    RawThreshLow=*(uint16_t*)p_low;
02682    p_high=&RawAlsHigh;
02683    RawThreshHigh=*(uint16_t*)p_high;
02684    return VL6180x_AlsSetThresholds(Device, RawThreshLow, RawThreshHigh);
02685 }
02686 
02687 
02688 int VL6180X::ReadID()
02689 {
02690    int status;
02691    uint8_t rl_id=0;
02692     
02693    status=VL6180x_RdByte(Device, IDENTIFICATION_MODEL_ID, &rl_id);
02694    if((status==0)&&(rl_id==0xB4))
02695      return status;
02696    else
02697      return INVALID_PARAMS;
02698 }
02699  
02700  
02701 int VL6180X::InitSensor(uint8_t NewAddr)
02702 {
02703    int status;
02704  
02705    VL6180x_Off();
02706    VL6180x_On();
02707    status=VL6180x_WaitDeviceBooted(Device);
02708    if(status)
02709       VL6180x_ErrLog("WaitDeviceBooted fail\n\r");
02710    status=IsPresent();
02711    if(!status)
02712    {
02713       Device->Present=1;
02714       status=Init();
02715       if(status)
02716       {
02717          printf("Failed to init VL6180X sensor!\n\r");
02718          return status;
02719       }
02720       status=Prepare();
02721       if(status)
02722       {
02723          printf("Failed to prepare VL6180X!\n\r");
02724          return status;
02725       }
02726       if(NewAddr!=DEFAULT_DEVICE_ADDRESS)
02727       {
02728          status=SetI2CAddress(NewAddr);
02729          if(status)
02730          {
02731             printf("Failed to change I2C address!\n\r");
02732             return status;
02733          }
02734       }
02735       else
02736       {
02737          printf("Invalid new address!\n\r");
02738          return INVALID_PARAMS;
02739       }
02740       Device->Ready=1;
02741    }
02742    return status; 
02743 }
02744  
02745  
02746 int VL6180X::StartMeasurement(OperatingMode operating_mode, void (*fptr)(void), uint16_t low, uint16_t high)
02747 {
02748    int status, r_status, l_status;
02749     
02750    switch(operating_mode)
02751    {
02752       case(range_single_shot_polling):
02753         r_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02754         l_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02755         if((!r_status)&&(!l_status))
02756            return RangeMeasPollSingleShot();
02757         else
02758            return (r_status|l_status);
02759         
02760       case(als_single_shot_polling):
02761         r_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02762         l_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02763         if((!r_status)&&(!l_status))
02764            return AlsMeasPollSingleShot();
02765         else
02766            return (r_status|l_status);
02767             
02768       case(range_continuous_polling):
02769         r_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02770         l_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02771         if((!r_status)&&(!l_status))
02772            return RangeMeasPollContinuousMode();
02773         else
02774            return (r_status|l_status);
02775                  
02776       case(als_continuous_polling):
02777         r_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02778         l_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02779         if((!r_status)&&(!l_status))
02780            return AlsMeasPollContinuousMode();
02781         else
02782            return (r_status|l_status);
02783                  
02784       case(range_continuous_interrupt):
02785         r_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02786         l_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02787         if((!r_status)&&(!l_status))
02788            return RangeMeasIntContinuousMode(fptr);
02789         else
02790            return (r_status|l_status);
02791                      
02792       case(als_continuous_interrupt):
02793         l_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02794         r_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02795         if((!r_status)&&(!l_status))
02796            return AlsMeasIntContinuousMode(fptr);
02797         else
02798            return (r_status|l_status);
02799  
02800       case(interleaved_mode_interrupt):
02801         l_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
02802         r_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);            
02803         if((!r_status)&&(!l_status))
02804            return InterleavedMode(fptr);
02805         else
02806            return (r_status|l_status);
02807  
02808       case(range_continuous_polling_low_threshold):
02809         r_status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_LOW);
02810         l_status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02811         if((!r_status)&&(!l_status))
02812         {
02813            status=RangeSetLowThreshold(low);
02814            if(!status)
02815               return RangeMeasPollContinuousMode();
02816            else
02817               return status;
02818         }
02819         else
02820            return (r_status|l_status);
02821        
02822       case(range_continuous_polling_high_threshold):
02823         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_HIGH);
02824         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02825         if((!r_status)&&(!l_status))
02826         {
02827            status=RangeSetHighThreshold(high);
02828            if(!status)
02829               return RangeMeasPollContinuousMode();
02830            else
02831               return status;
02832         }
02833         else
02834            return (r_status|l_status);
02835                 
02836       case(range_continuous_polling_out_of_window):
02837         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_OUT_OF_WINDOW);
02838         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02839         if((!r_status)&&(!l_status))
02840         {
02841            status=VL6180x_RangeSetThresholds(Device,low,high,1);
02842            if(!status)
02843               return RangeMeasPollContinuousMode();
02844            else
02845               return status;
02846         }
02847         else
02848            return (r_status|l_status);
02849                 
02850       case(als_continuous_polling_low_threshold):
02851         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_LOW);
02852         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02853         if((!r_status)&&(!l_status))
02854         {
02855            status=AlsSetLowThreshold(low);
02856            if(!status)
02857               return AlsMeasPollContinuousMode();
02858            else
02859               return status;
02860         }
02861         else
02862            return (r_status|l_status);
02863             
02864       case(als_continuous_polling_high_threshold):
02865         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_HIGH);
02866         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02867         if((!r_status)&&(!l_status))
02868         {
02869            status=AlsSetHighThreshold(high);
02870            if(!status)
02871               return AlsMeasPollContinuousMode();
02872            else
02873               return status;
02874         }
02875         else
02876            return (r_status|l_status);
02877                 
02878       case(als_continuous_polling_out_of_window):
02879         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_OUT_OF_WINDOW);
02880         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02881         if((!r_status)&&(!l_status))
02882         {
02883            status=VL6180x_AlsSetThresholds(Device,low,high);
02884            if(!status)
02885               return AlsMeasPollContinuousMode();
02886            else
02887               return status;
02888         }
02889         else
02890            return (r_status|l_status);
02891                 
02892       case(range_continuous_interrupt_low_threshold):
02893         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_LOW);
02894         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02895         if((!r_status)&&(!l_status))
02896         {
02897            status=RangeSetLowThreshold(low);
02898            if(!status)
02899               return RangeMeasIntContinuousMode(fptr);
02900            else
02901               return status;
02902         }
02903         else
02904            return (r_status|l_status);                  
02905             
02906       case(range_continuous_interrupt_high_threshold):   
02907         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_HIGH);
02908         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02909         if((!r_status)&&(!l_status))
02910         {
02911            status=RangeSetHighThreshold(high);
02912            if(!status)
02913               return RangeMeasIntContinuousMode(fptr);
02914            else
02915               return status;
02916         }
02917         else
02918            return (r_status|l_status);
02919                 
02920        case(range_continuous_interrupt_out_of_window):
02921         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_OUT_OF_WINDOW);
02922         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02923         if((!r_status)&&(!l_status))
02924         {
02925            status=VL6180x_RangeSetThresholds(Device,low,high,1);
02926            if(!status)
02927               return RangeMeasIntContinuousMode(fptr);
02928            else
02929               return status;
02930         }
02931         else
02932            return (r_status|l_status);
02933             
02934       case(als_continuous_interrupt_low_threshold):
02935         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_LOW);
02936         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02937         if((!r_status)&&(!l_status))
02938         {
02939            status=AlsSetLowThreshold(low);
02940            if(!status)
02941               return AlsMeasIntContinuousMode(fptr);
02942            else
02943               return status;
02944         }
02945         else
02946            return (r_status|l_status);  
02947                 
02948       case(als_continuous_interrupt_high_threshold):
02949         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_LEVEL_HIGH);
02950         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02951         if((!r_status)&&(!l_status))
02952         {
02953            status=AlsSetHighThreshold(high);
02954            if(!status)
02955               return AlsMeasIntContinuousMode(fptr);
02956            else
02957               return status;
02958         }
02959         else
02960            return (r_status|l_status);
02961                 
02962       case(als_continuous_interrupt_out_of_window):
02963         status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_OUT_OF_WINDOW);
02964         status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
02965         if((!r_status)&&(!l_status))
02966         {
02967            status=VL6180x_AlsSetThresholds(Device,low,high);
02968            if(!status)
02969               return AlsMeasIntContinuousMode(fptr);
02970            else
02971               return status;
02972         }
02973         else
02974            return (r_status|l_status);
02975                 
02976       default:
02977         return INVALID_PARAMS;
02978    }
02979 }
02980  
02981  
02982 int VL6180X::GetRangeError(MeasureData_t *Data, VL6180x_RangeData_t RangeData)
02983 {
02984    Data->range_error=RangeData.errorStatus ;
02985    if(Data->range_error!=0)
02986    {
02987       VL6180x_ErrLog("Range error %d",Data->range_error);
02988       return RANGE_ERROR;
02989    }
02990    return NoError_;
02991 }
02992  
02993  
02994 int VL6180X::GetAlsError(MeasureData_t *Data, VL6180x_AlsData_t AlsData)
02995 {
02996    Data->als_error=AlsData.errorStatus;
02997    if(Data->als_error!=0)
02998    {
02999       VL6180x_ErrLog("Light error %d",Data->light_error);
03000       return API_ERROR;
03001    }
03002    return NoError_;
03003 }
03004  
03005  
03006 int VL6180X::RangeMeasPollSingleShot()
03007 {
03008    int status;
03009             
03010    status=VL6180x_RangeClearInterrupt(Device);
03011    if(status)
03012    {
03013       VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
03014       return status;
03015    }
03016    status=VL6180x_ClearErrorInterrupt(Device);
03017    if(status)
03018    {
03019       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03020       return status;
03021    }
03022    return RangeStartSingleShot(); 
03023 }   
03024  
03025  
03026 int VL6180X::AlsMeasPollSingleShot()
03027 {
03028    int status;            
03029      
03030    status=VL6180x_AlsClearInterrupt(Device);
03031    if(status)
03032    {
03033       VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
03034       return status;
03035    }
03036    status=VL6180x_ClearErrorInterrupt(Device);
03037    if(status)
03038    {
03039       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03040       return status;
03041    }
03042    return AlsStartSingleShot();
03043 }
03044  
03045  
03046 int VL6180X::RangeMeasPollContinuousMode()
03047 {
03048    int status;
03049             
03050    status=VL6180x_RangeClearInterrupt(Device);
03051    if(status)
03052    {
03053       VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
03054       return status;
03055    }
03056    status=VL6180x_ClearErrorInterrupt(Device);
03057    if(status)
03058    {
03059       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03060       return status;
03061    }
03062    return RangeStartContinuousMode();
03063 }
03064  
03065  
03066 int VL6180X::AlsMeasPollContinuousMode() 
03067 {
03068    int status;            
03069      
03070    status=VL6180x_AlsClearInterrupt(Device);
03071    if(status)
03072    {
03073       VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
03074       return status;
03075    }
03076    status=VL6180x_ClearErrorInterrupt(Device);
03077    if(status)
03078    {
03079       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03080       return status;
03081    }
03082    return AlsStartContinuousMode();
03083 }
03084  
03085  
03086 int VL6180X::AlsGetMeasurementIfReady(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData)
03087 {
03088    int status;
03089    uint8_t IntStatus;
03090      
03091    status=VL6180x_AlsGetInterruptStatus(dev, &IntStatus);
03092    if(!status)
03093    {
03094       if(IntStatus==RES_INT_STAT_GPIO_NEW_SAMPLE_READY)
03095       {
03096          status = VL6180x_AlsGetMeasurement(dev, pAlsData);
03097          if(!status)
03098          {
03099             status=VL6180x_AlsClearInterrupt(Device);
03100             if(status)
03101                VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
03102          }
03103       }
03104       else
03105          status=NOT_READY;
03106    }
03107    else
03108       VL6180x_ErrLog("Failed to get interrupt status");
03109    return status;
03110 }
03111  
03112  
03113 int VL6180X::RangeMeasIntContinuousMode(void (*fptr)(void))
03114 {
03115    int status, ClrStatus;
03116      
03117    EnableInterruptMeasureDetectionIRQ();
03118    AttachInterruptMeasureDetectionIRQ(fptr);
03119    status=SetupGPIO1(GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT, 1);
03120    ClrStatus=VL6180x_ClearAllInterrupt(Device);
03121    if(ClrStatus)
03122       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03123    if(!status)
03124       status=RangeStartContinuousMode();
03125    return status;
03126 }
03127  
03128  
03129 int VL6180X::AlsMeasIntContinuousMode(void (*fptr)(void))
03130 {
03131    int status, ClrStatus;
03132     
03133    EnableInterruptMeasureDetectionIRQ();
03134    AttachInterruptMeasureDetectionIRQ(fptr);
03135    status=SetupGPIO1(GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT, 1);
03136    ClrStatus=VL6180x_ClearAllInterrupt(Device);
03137    if(ClrStatus)
03138       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03139    if(!status)
03140       status=AlsStartContinuousMode();
03141    return status;
03142 }   
03143  
03144  
03145 int VL6180X::StartInterleavedMode()
03146 {
03147    int status;
03148    uint16_t integration_period, intermeasurement_period_ms;
03149    uint8_t max_convergence_time;
03150    uint8_t buf;
03151     
03152    status=VL6180x_WrByte(Device, 0x2A3, 1);
03153    if(status)
03154    {
03155       VL6180x_ErrLog("Failed to write INTERLEAVED_MODE_ENABLE!\n\r");
03156       return status;
03157    }
03158    status=VL6180x_RdByte(Device, SYSRANGE_MAX_CONVERGENCE_TIME, &max_convergence_time);
03159    if(status)
03160    {
03161       VL6180x_ErrLog("Failed to read SYSRANGE_MAX_CONVERGENCE_TIME!\n\r");
03162       return status;
03163    }
03164    status=VL6180x_RdWord(Device, SYSALS_INTEGRATION_PERIOD, &integration_period);
03165    if(status)
03166    {
03167       VL6180x_ErrLog("Failed to read SYSALS_INTEGRATION_PERIOD!\n\r");
03168       return status;
03169    }
03170    max_convergence_time&=0x3F;
03171    integration_period&=0x01FF;
03172    intermeasurement_period_ms=((max_convergence_time+5)+(integration_period*1.1));
03173    intermeasurement_period_ms=(intermeasurement_period_ms/0.9)+10;
03174    intermeasurement_period_ms=200;
03175    status=VL6180x_AlsSetInterMeasurementPeriod(Device, intermeasurement_period_ms);
03176    VL6180x_RdByte(Device, 0x03E, &buf);
03177    if(status)
03178    {
03179       VL6180x_ErrLog("Failed to write SYSALS_INTERMEASUREMENT_PERIOD!\n\r");
03180       return status;
03181    }
03182    return AlsStartContinuousMode();
03183 }
03184  
03185  
03186 int VL6180X::InterleavedMode(void (*fptr)(void))
03187 {
03188    int status, ClrStatus;
03189     
03190    EnableInterruptMeasureDetectionIRQ();
03191    AttachInterruptMeasureDetectionIRQ(fptr);
03192    status=SetupGPIO1(GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT, 1);
03193    ClrStatus=VL6180x_ClearAllInterrupt(Device);
03194    if(ClrStatus)
03195       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03196    if(!status)
03197       status=StartInterleavedMode();
03198    return status;
03199 } 
03200  
03201  
03202 int VL6180X::HandleIRQ(OperatingMode operating_mode, MeasureData_t *Data)
03203 {
03204    int status;
03205     
03206    EnableInterruptMeasureDetectionIRQ();
03207    status=GetMeasurement(operating_mode, Data);
03208    return status;
03209 }
03210      
03211  
03212 int VL6180X::RangeSetLowThreshold(uint16_t threshold)
03213 {
03214    int status;
03215    uint16_t low, high;
03216      
03217    status=VL6180x_RangeGetThresholds(Device, &low, &high);
03218    if(!status)
03219       status=VL6180x_RangeSetThresholds(Device, threshold, high, 1);
03220    return status;
03221 }
03222  
03223  
03224 int VL6180X::RangeSetHighThreshold(uint16_t threshold)
03225 {
03226    int status;
03227    uint16_t low, high;
03228      
03229    status=VL6180x_RangeGetThresholds(Device, &low, &high);
03230    if(!status)
03231       status=VL6180x_RangeSetThresholds(Device, low, threshold, 1);
03232    return status;
03233 }
03234     
03235  
03236 int VL6180X::AlsSetLowThreshold(uint16_t threshold)
03237 {
03238    int status;
03239    lux_t low, high;
03240      
03241    status=AlsGetThresholds(Device, &low, &high);
03242    if(!status)
03243       status=VL6180x_AlsSetThresholds(Device, threshold, high);
03244    return status;
03245 }
03246     
03247  
03248 int VL6180X::AlsSetHighThreshold(uint16_t threshold)
03249 {
03250    int status;
03251    lux_t low, high;
03252      
03253    status=AlsGetThresholds(Device, &low, &high);
03254    if(!status)
03255       status=VL6180x_AlsSetThresholds(Device, low, threshold);
03256    return status;
03257 }
03258  
03259     
03260 int VL6180X::AlsGetThresholds(VL6180xDev_t dev, lux_t *low, lux_t *high)  
03261 {
03262    int status;
03263    uint16_t RawAlsLow, RawAlsHigh;
03264    uint32_t luxLowValue, luxHighValue, IntPeriod, AlsAnGain, GainFix, AlsScaler ;
03265    const uint32_t LuxResxIntIme =(uint32_t)(0.56f* DEF_INT_PEFRIOD *(1<<LUXRES_FIX_PREC));
03266      
03267    status=VL6180x_RdWord(dev, SYSALS_THRESH_LOW, &RawAlsLow);
03268    if(status)
03269    {
03270       VL6180x_ErrLog("rd SYSALS_THRESH_LOW fail");
03271       return status;
03272    }
03273    status=VL6180x_RdWord(dev, SYSALS_THRESH_HIGH, &RawAlsHigh);
03274    if(status)
03275    {
03276       VL6180x_ErrLog("rd SYSALS_THRESH_HIGH fail");
03277       return status;
03278    }
03279    AlsAnGain=VL6180xDevDataGet(dev, AlsGainCode );
03280    IntPeriod=VL6180xDevDataGet(dev, IntegrationPeriod );
03281    AlsScaler=VL6180xDevDataGet(dev, AlsScaler);
03282    GainFix=AlsGainLookUp[AlsAnGain];
03283    IntPeriod++;
03284    luxLowValue=(uint32_t)RawAlsLow*LuxResxIntIme;
03285    luxLowValue=luxLowValue/IntPeriod;
03286    luxLowValue=luxLowValue/(AlsScaler*GainFix);
03287    luxHighValue=(uint32_t)RawAlsHigh*LuxResxIntIme;
03288    luxHighValue=luxHighValue/IntPeriod;
03289    luxHighValue=luxHighValue/(AlsScaler*GainFix);
03290    *low=luxLowValue;
03291    *high=luxHighValue;
03292    return status;
03293 }
03294     
03295  
03296 int VL6180X::GetMeasurement(OperatingMode operating_mode, MeasureData_t *Data)
03297 {
03298    switch(operating_mode)
03299    {
03300       case(range_single_shot_polling):
03301       case(range_continuous_polling):
03302       case(range_continuous_interrupt):
03303       case(range_continuous_polling_low_threshold):
03304       case(range_continuous_polling_high_threshold):
03305       case(range_continuous_polling_out_of_window):
03306       case(range_continuous_interrupt_low_threshold):
03307       case(range_continuous_interrupt_high_threshold):
03308       case(range_continuous_interrupt_out_of_window):
03309         return GetRangeMeas(operating_mode, Data);
03310          
03311       case(als_single_shot_polling):
03312       case(als_continuous_polling):
03313       case(als_continuous_interrupt):
03314       case(als_continuous_polling_low_threshold):
03315       case(als_continuous_polling_high_threshold):
03316       case(als_continuous_polling_out_of_window):
03317       case(als_continuous_interrupt_low_threshold):
03318       case(als_continuous_interrupt_high_threshold):
03319       case(als_continuous_interrupt_out_of_window):
03320         return GetAlsMeas(operating_mode, Data);
03321              
03322       case(interleaved_mode_interrupt):
03323         return GetRangeAlsMeas(Data);
03324          
03325       default:
03326         return INVALID_PARAMS;
03327    }
03328 }
03329      
03330  
03331 int VL6180X::GetRangeMeas(OperatingMode operating_mode, MeasureData_t *Data)
03332 {
03333    VL6180x_RangeData_t RangeData;
03334    int status, ClrStatus;
03335    IntrStatus_t IntStatus;
03336     
03337    status=VL6180x_RangeGetInterruptStatus(Device, &IntStatus.val );
03338    if(!status)
03339    {
03340       Data->int_error=IntStatus.status .Error ;
03341       if(IntStatus.status .Error !=0)
03342       {
03343          VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val );
03344          status=RANGE_ERROR;
03345       } 
03346    }
03347    else
03348    {
03349       VL6180x_ErrLog("Failed to read RESULT_INTERRUPT_STATUS_GPIO");
03350    }  
03351    ClrStatus=VL6180x_RangeClearInterrupt(Device);
03352    if(ClrStatus)
03353    {
03354       VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
03355    }
03356    ClrStatus=VL6180x_ClearErrorInterrupt(Device);
03357    if(ClrStatus)
03358    {
03359       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03360    }
03361    if(status)
03362       return status;
03363    if((operating_mode==range_single_shot_polling)||(operating_mode==range_continuous_polling)||(operating_mode==range_continuous_interrupt))
03364    {
03365       if(IntStatus.status .Range ==RES_INT_STAT_GPIO_NEW_SAMPLE_READY)
03366          status=VL6180x_RangeGetMeasurement(Device, &RangeData);
03367    else
03368          return NOT_READY;
03369    }
03370    else if((operating_mode==range_continuous_polling_low_threshold)||(operating_mode==range_continuous_interrupt_low_threshold))
03371    {
03372       if(IntStatus.status .Range ==RES_INT_STAT_GPIO_LOW_LEVEL_THRESHOLD)
03373          status=VL6180x_RangeGetMeasurement(Device, &RangeData);
03374       else
03375          return NOT_READY;
03376    }
03377    else if((operating_mode==range_continuous_polling_high_threshold)||(operating_mode==range_continuous_interrupt_high_threshold))
03378    {
03379       if(IntStatus.status .Range ==RES_INT_STAT_GPIO_HIGH_LEVEL_THRESHOLD)
03380          status=VL6180x_RangeGetMeasurement(Device, &RangeData);
03381       else
03382          return NOT_READY;
03383    }
03384    else if((operating_mode==range_continuous_polling_out_of_window)||(operating_mode==range_continuous_interrupt_out_of_window))
03385    {
03386       if(IntStatus.status .Range ==RES_INT_STAT_GPIO_OUT_OF_WINDOW)
03387          status=VL6180x_RangeGetMeasurement(Device, &RangeData);
03388       else
03389          return NOT_READY;
03390    }
03391    if(!status)
03392    {
03393       status=GetRangeError(Data, RangeData);
03394       if(!status)
03395          Data->range_mm=RangeData.range_mm ;
03396       else
03397          Data->range_mm=0xFFFFFFFF;
03398    }
03399    return status;
03400 }
03401  
03402  
03403 int VL6180X::GetAlsMeas(OperatingMode operating_mode, MeasureData_t *Data)
03404 {
03405    VL6180x_AlsData_t AlsData;
03406    int status, ClrStatus;
03407    uint8_t IntStatus;
03408     
03409    status=VL6180x_AlsGetInterruptStatus(Device, &IntStatus);
03410    if(status)
03411    {
03412       VL6180x_ErrLog("Failed to read RESULT_INTERRUPT_STATUS_GPIO");
03413    }
03414    ClrStatus=VL6180x_AlsClearInterrupt(Device);
03415    if(ClrStatus)
03416    {
03417       VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
03418    }
03419    ClrStatus=VL6180x_ClearErrorInterrupt(Device);
03420    if(ClrStatus)
03421    {
03422       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03423    }
03424    if(status)
03425       return status;
03426    if((operating_mode==als_single_shot_polling)||(operating_mode==als_continuous_polling)||(operating_mode==als_continuous_interrupt))
03427    {
03428       if(IntStatus==RES_INT_STAT_GPIO_NEW_SAMPLE_READY)
03429          status=VL6180x_AlsGetMeasurement(Device, &AlsData);
03430       else
03431          return NOT_READY;
03432    }
03433    else if((operating_mode==als_continuous_polling_low_threshold)||(operating_mode==als_continuous_interrupt_low_threshold))
03434    {
03435       if(IntStatus==RES_INT_STAT_GPIO_LOW_LEVEL_THRESHOLD)
03436          status=VL6180x_AlsGetMeasurement(Device, &AlsData);
03437       else
03438          return NOT_READY;
03439    }
03440    else if((operating_mode==als_continuous_polling_high_threshold)||(operating_mode==als_continuous_interrupt_high_threshold))
03441    {
03442       if(IntStatus==RES_INT_STAT_GPIO_HIGH_LEVEL_THRESHOLD)
03443          status=VL6180x_AlsGetMeasurement(Device, &AlsData);
03444       else
03445          return NOT_READY;
03446    }
03447    else if((operating_mode==als_continuous_polling_out_of_window)||(operating_mode==als_continuous_interrupt_out_of_window))
03448    {
03449       if(IntStatus==RES_INT_STAT_GPIO_OUT_OF_WINDOW)
03450          status=VL6180x_AlsGetMeasurement(Device, &AlsData);
03451       else
03452          return NOT_READY;
03453    }
03454    if(!status)
03455    {
03456       status=GetAlsError(Data, AlsData);
03457       if(!status)
03458          Data->lux=AlsData.lux;
03459       else
03460          Data->lux=0xFFFFFFFF;
03461    }
03462    return status; 
03463 }
03464  
03465  
03466 int VL6180X::GetRangeAlsMeas(MeasureData_t *Data)
03467 {
03468    int status, ClrStatus, r_status, l_status;
03469    IntrStatus_t IntStatus;
03470    VL6180x_RangeData_t RangeData;
03471    VL6180x_AlsData_t AlsData;
03472     
03473    status=VL6180x_RdByte(Device, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus.val );
03474    if(!status)
03475    {
03476       Data->int_error=IntStatus.status .Error ;
03477       if(IntStatus.status .Error !=0)
03478       {
03479          VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val );
03480          status=RANGE_ERROR;
03481       } 
03482    }
03483    else
03484    {
03485       VL6180x_ErrLog("Failed to read RESULT_INTERRUPT_STATUS_GPIO"); 
03486    }
03487    ClrStatus=VL6180x_ClearAllInterrupt(Device);
03488    if(ClrStatus)
03489       VL6180x_ErrLog("VL6180x_ClearAllInterrupt fail");
03490    if(status)
03491       return status;
03492      
03493    if(IntStatus.status .Als ==RES_INT_STAT_GPIO_NEW_SAMPLE_READY)
03494    {
03495       r_status=VL6180x_RangeGetMeasurement(Device, &RangeData);
03496       l_status=VL6180x_AlsGetMeasurement(Device, &AlsData);
03497       if((!r_status)&&(!l_status))
03498       {
03499          r_status=GetRangeError(Data, RangeData);
03500          l_status=GetAlsError(Data, AlsData);
03501          if(!r_status) 
03502             Data->range_mm=RangeData.range_mm ;
03503          else
03504             Data->range_mm=0xFFFFFFFF;
03505          if(!l_status)
03506             Data->lux=AlsData.lux;
03507          else
03508             Data->lux=0xFFFFFFFF;
03509          status=r_status|l_status; 
03510       }
03511       else
03512       {
03513          status=r_status|l_status;
03514       }
03515    }
03516    else
03517       return NOT_READY;
03518    return status; 
03519 }
03520  
03521  
03522 int VL6180X::StopMeasurement(OperatingMode operating_mode)
03523 {
03524    int status;
03525      
03526    switch(operating_mode)
03527    {
03528       case(range_single_shot_polling):
03529       case(range_continuous_polling):
03530       case(range_continuous_interrupt):
03531       case(range_continuous_polling_low_threshold):
03532       case(range_continuous_polling_high_threshold):
03533       case(range_continuous_polling_out_of_window):
03534       case(range_continuous_interrupt_low_threshold):
03535       case(range_continuous_interrupt_high_threshold):
03536       case(range_continuous_interrupt_out_of_window):
03537         return StopRangeMeasurement(operating_mode);
03538          
03539       case(als_single_shot_polling):
03540       case(als_continuous_polling):
03541       case(als_continuous_interrupt):
03542       case(als_continuous_polling_low_threshold):
03543       case(als_continuous_polling_high_threshold):
03544       case(als_continuous_polling_out_of_window):
03545       case(als_continuous_interrupt_low_threshold):
03546       case(als_continuous_interrupt_high_threshold):
03547       case(als_continuous_interrupt_out_of_window):
03548         return StopAlsMeasurement(operating_mode);
03549              
03550       case(interleaved_mode_interrupt):
03551         status=StopRangeMeasurement(range_continuous_interrupt);
03552         if(!status)
03553            return StopAlsMeasurement(als_continuous_interrupt);
03554         else return status;
03555          
03556       default:
03557         return INVALID_PARAMS;
03558    }
03559 }
03560  
03561  
03562 int VL6180X::StopRangeMeasurement(OperatingMode operating_mode)
03563 {
03564    int status;
03565     
03566    if(operating_mode==range_single_shot_polling)
03567       status=VL6180x_RangeSetSystemMode(Device, MODE_SINGLESHOT);
03568    else
03569       status=VL6180x_RangeSetSystemMode(Device, MODE_START_STOP|MODE_SINGLESHOT);
03570    if(status)
03571       return status;
03572    status=VL6180x_RangeConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
03573    if(status)
03574    {
03575       VL6180x_ErrLog("VL6180x_RangeConfigInterrupt fail"\n\r);
03576       return status;
03577    }
03578    status=VL6180x_RangeClearInterrupt(Device);
03579    if(status)
03580    {
03581       VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
03582       return status;
03583    }
03584    status=VL6180x_ClearErrorInterrupt(Device);
03585    if(status)
03586    {
03587       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03588       return status;
03589    }
03590    status=VL6180x_RangeSetRawThresholds(Device, 10, 200);
03591    if(status)
03592       VL6180x_ErrLog("VL6180x_RangeSetThresholds fail");
03593    return status;
03594 }
03595  
03596  
03597 int VL6180X::StopAlsMeasurement(OperatingMode operating_mode)
03598 {
03599    int status;
03600     
03601    if(operating_mode==als_single_shot_polling)
03602       status=VL6180x_AlsSetSystemMode(Device, MODE_SINGLESHOT);
03603    else
03604       status=VL6180x_AlsSetSystemMode(Device, MODE_START_STOP|MODE_SINGLESHOT);
03605    if(status)
03606       return status;
03607    status=VL6180x_AlsConfigInterrupt(Device, CONFIG_GPIO_INTERRUPT_DISABLED);
03608    if(status)
03609    {
03610       VL6180x_ErrLog("VL6180x_AlsConfigInterrupt fail"\n\r);
03611       return status;
03612    }
03613    status=VL6180x_AlsClearInterrupt(Device);
03614    if(status)
03615    {
03616       VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
03617       return status;
03618    }
03619    status=VL6180x_ClearErrorInterrupt(Device);
03620    if(status)
03621    {
03622       VL6180x_ErrLog("VL6180x_ClearErrorInterrupt fail");
03623       return status;
03624    }
03625    status=VL6180x_AlsSetThresholds(Device, 0x0, 1800);
03626    if(status)
03627       VL6180x_ErrLog("VL6180x_AlsSetThresholds fail");
03628    return status;
03629 }
03630  
03631  
03632  
03633  
03634 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/