hh
Dependencies: VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed
Fork of 1-DoorCloser_jor by
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>© 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****/
Generated on Tue Jul 12 2022 22:31:09 by 1.7.2