vl53l0x_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    vl53l0x_class.cpp
00004  * @author  IMG
00005  * @version V0.0.1
00006  * @date    28-June-2016
00007  * @brief   Implementation file for the VL53L0X driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
00012  *
00013  * Redistribution and use in source and binary forms, with or without modification,
00014  * are permitted provided that the following conditions are met:
00015  *   1. Redistributions of source code must retain the above copyright notice,
00016  *      this list of conditions and the following disclaimer.
00017  *   2. Redistributions in binary form must reproduce the above copyright notice,
00018  *      this list of conditions and the following disclaimer in the documentation
00019  *      and/or other materials provided with the distribution.
00020  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021  *      may be used to endorse or promote products derived from this software
00022  *      without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  ******************************************************************************
00036 */
00037  
00038 /* Includes */
00039 #include <stdlib.h>
00040 
00041 #include "vl53l0x_class.h"
00042 
00043 //#include "vl53l0x_api_core.h"
00044 //#include "vl53l0x_api_calibration.h"
00045 //#include "vl53l0x_api_strings.h"
00046 #include "vl53l0x_interrupt_threshold_settings.h"
00047 #include "vl53l0x_tuning.h"
00048 #include "vl53l0x_types.h"
00049 
00050 
00051 /****************** define for i2c configuration *******************************/
00052  
00053 #define TEMP_BUF_SIZE   64
00054 
00055 /** Maximum buffer size to be used in i2c */
00056 #define VL53L0X_MAX_I2C_XFER_SIZE   64 /* Maximum buffer size to be used in i2c */
00057 #define VL53L0X_I2C_USER_VAR         /* none but could be for a flag var to get/pass to mutex interruptible  return flags and try again */
00058  
00059 
00060 #define LOG_FUNCTION_START(fmt, ...) \
00061     _LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
00062 #define LOG_FUNCTION_END(status, ...) \
00063     _LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
00064 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00065     _LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
00066 
00067 #ifdef VL53L0X_LOG_ENABLE
00068 #define trace_print(level, ...) trace_print_module_function(TRACE_MODULE_API, \
00069     level, TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00070 #endif
00071 
00072 #define REF_ARRAY_SPAD_0  0
00073 #define REF_ARRAY_SPAD_5  5
00074 #define REF_ARRAY_SPAD_10 10
00075 
00076 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5,
00077         REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 };
00078 
00079 
00080 
00081 
00082 VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(VL53L0X_DEV Dev)
00083 {
00084     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00085     uint8_t strobe;
00086     uint32_t LoopNb;
00087     LOG_FUNCTION_START("");
00088 
00089     Status |= VL53L0X_WrByte(Dev, 0x83, 0x00);
00090 
00091     /* polling
00092      * use timeout to avoid deadlock*/
00093     if (Status == VL53L0X_ERROR_NONE) {
00094         LoopNb = 0;
00095         do {
00096             Status = VL53L0X_RdByte(Dev, 0x83, &strobe);
00097             if ((strobe != 0x00) || Status != VL53L0X_ERROR_NONE)
00098                     break;
00099 
00100             LoopNb = LoopNb + 1;
00101         } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
00102 
00103         if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
00104             Status = VL53L0X_ERROR_TIME_OUT;
00105 
00106     }
00107 
00108     Status |= VL53L0X_WrByte(Dev, 0x83, 0x01);
00109 
00110     LOG_FUNCTION_END(Status);
00111     return Status;
00112 
00113 }
00114 
00115 VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option)
00116 {
00117 
00118     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00119     uint8_t byte;
00120     uint32_t TmpDWord;
00121     uint8_t ModuleId;
00122     uint8_t Revision;
00123     uint8_t ReferenceSpadCount = 0;
00124     uint8_t ReferenceSpadType = 0;
00125     uint32_t PartUIDUpper = 0;
00126     uint32_t PartUIDLower = 0;
00127     uint32_t OffsetFixed1104_mm = 0;
00128     int16_t OffsetMicroMeters = 0;
00129     uint32_t DistMeasTgtFixed1104_mm = 400 << 4;
00130     uint32_t DistMeasFixed1104_400_mm = 0;
00131     uint32_t SignalRateMeasFixed1104_400_mm = 0;
00132     char ProductId[19];
00133     char *ProductId_tmp;
00134     uint8_t ReadDataFromDeviceDone;
00135     FixPoint1616_t SignalRateMeasFixed400mmFix = 0;
00136     uint8_t NvmRefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
00137     int i;
00138 
00139 
00140     LOG_FUNCTION_START("");
00141 
00142     ReadDataFromDeviceDone = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
00143             ReadDataFromDeviceDone);
00144 
00145     /* This access is done only once after that a GetDeviceInfo or
00146      * datainit is done*/
00147     if (ReadDataFromDeviceDone != 7) {
00148 
00149         Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
00150         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
00151         Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
00152 
00153         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
00154         Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
00155         Status |= VL53L0X_WrByte(Dev, 0x83, byte|4);
00156         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x07);
00157         Status |= VL53L0X_WrByte(Dev, 0x81, 0x01);
00158 
00159         Status |= VL53L0X_PollingDelay(Dev);
00160 
00161         Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
00162 
00163         if (((option & 1) == 1) &&
00164             ((ReadDataFromDeviceDone & 1) == 0)) {
00165             Status |= VL53L0X_WrByte(Dev, 0x94, 0x6b);
00166             Status |= VL53L0X_device_read_strobe(Dev);
00167             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00168 
00169             ReferenceSpadCount = (uint8_t)((TmpDWord >> 8) & 0x07f);
00170             ReferenceSpadType  = (uint8_t)((TmpDWord >> 15) & 0x01);
00171 
00172             Status |= VL53L0X_WrByte(Dev, 0x94, 0x24);
00173             Status |= VL53L0X_device_read_strobe(Dev);
00174             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00175 
00176 
00177             NvmRefGoodSpadMap[0] = (uint8_t)((TmpDWord >> 24)
00178                 & 0xff);
00179             NvmRefGoodSpadMap[1] = (uint8_t)((TmpDWord >> 16)
00180                 & 0xff);
00181             NvmRefGoodSpadMap[2] = (uint8_t)((TmpDWord >> 8)
00182                 & 0xff);
00183             NvmRefGoodSpadMap[3] = (uint8_t)(TmpDWord & 0xff);
00184 
00185             Status |= VL53L0X_WrByte(Dev, 0x94, 0x25);
00186             Status |= VL53L0X_device_read_strobe(Dev);
00187             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00188 
00189             NvmRefGoodSpadMap[4] = (uint8_t)((TmpDWord >> 24)
00190                 & 0xff);
00191             NvmRefGoodSpadMap[5] = (uint8_t)((TmpDWord >> 16)
00192                 & 0xff);
00193         }
00194 
00195         if (((option & 2) == 2) &&
00196             ((ReadDataFromDeviceDone & 2) == 0)) {
00197 
00198             Status |= VL53L0X_WrByte(Dev, 0x94, 0x02);
00199             Status |= VL53L0X_device_read_strobe(Dev);
00200             Status |= VL53L0X_RdByte(Dev, 0x90, &ModuleId);
00201 
00202             Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
00203             Status |= VL53L0X_device_read_strobe(Dev);
00204             Status |= VL53L0X_RdByte(Dev, 0x90, &Revision);
00205 
00206             Status |= VL53L0X_WrByte(Dev, 0x94, 0x77);
00207             Status |= VL53L0X_device_read_strobe(Dev);
00208             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00209 
00210             ProductId[0] = (char)((TmpDWord >> 25) & 0x07f);
00211             ProductId[1] = (char)((TmpDWord >> 18) & 0x07f);
00212             ProductId[2] = (char)((TmpDWord >> 11) & 0x07f);
00213             ProductId[3] = (char)((TmpDWord >> 4) & 0x07f);
00214 
00215             byte = (uint8_t)((TmpDWord & 0x00f) << 3);
00216 
00217             Status |= VL53L0X_WrByte(Dev, 0x94, 0x78);
00218             Status |= VL53L0X_device_read_strobe(Dev);
00219             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00220 
00221             ProductId[4] = (char)(byte +
00222                     ((TmpDWord >> 29) & 0x07f));
00223             ProductId[5] = (char)((TmpDWord >> 22) & 0x07f);
00224             ProductId[6] = (char)((TmpDWord >> 15) & 0x07f);
00225             ProductId[7] = (char)((TmpDWord >> 8) & 0x07f);
00226             ProductId[8] = (char)((TmpDWord >> 1) & 0x07f);
00227 
00228             byte = (uint8_t)((TmpDWord & 0x001) << 6);
00229 
00230             Status |= VL53L0X_WrByte(Dev, 0x94, 0x79);
00231 
00232             Status |= VL53L0X_device_read_strobe(Dev);
00233 
00234             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00235 
00236             ProductId[9] = (char)(byte +
00237                     ((TmpDWord >> 26) & 0x07f));
00238             ProductId[10] = (char)((TmpDWord >> 19) & 0x07f);
00239             ProductId[11] = (char)((TmpDWord >> 12) & 0x07f);
00240             ProductId[12] = (char)((TmpDWord >> 5) & 0x07f);
00241 
00242             byte = (uint8_t)((TmpDWord & 0x01f) << 2);
00243 
00244             Status |= VL53L0X_WrByte(Dev, 0x94, 0x7A);
00245 
00246             Status |= VL53L0X_device_read_strobe(Dev);
00247 
00248             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00249 
00250             ProductId[13] = (char)(byte +
00251                     ((TmpDWord >> 30) & 0x07f));
00252             ProductId[14] = (char)((TmpDWord >> 23) & 0x07f);
00253             ProductId[15] = (char)((TmpDWord >> 16) & 0x07f);
00254             ProductId[16] = (char)((TmpDWord >> 9) & 0x07f);
00255             ProductId[17] = (char)((TmpDWord >> 2) & 0x07f);
00256             ProductId[18] = '\0';
00257 
00258         }
00259 
00260         if (((option & 4) == 4) &&
00261             ((ReadDataFromDeviceDone & 4) == 0)) {
00262 
00263             Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
00264             Status |= VL53L0X_device_read_strobe(Dev);
00265             Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDUpper);
00266 
00267             Status |= VL53L0X_WrByte(Dev, 0x94, 0x7C);
00268             Status |= VL53L0X_device_read_strobe(Dev);
00269             Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDLower);
00270 
00271             Status |= VL53L0X_WrByte(Dev, 0x94, 0x73);
00272             Status |= VL53L0X_device_read_strobe(Dev);
00273             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00274 
00275             SignalRateMeasFixed1104_400_mm = (TmpDWord &
00276                 0x0000000ff) << 8;
00277 
00278             Status |= VL53L0X_WrByte(Dev, 0x94, 0x74);
00279             Status |= VL53L0X_device_read_strobe(Dev);
00280             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00281 
00282             SignalRateMeasFixed1104_400_mm |= ((TmpDWord &
00283                 0xff000000) >> 24);
00284 
00285             Status |= VL53L0X_WrByte(Dev, 0x94, 0x75);
00286             Status |= VL53L0X_device_read_strobe(Dev);
00287             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00288 
00289             DistMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff)
00290                             << 8;
00291 
00292             Status |= VL53L0X_WrByte(Dev, 0x94, 0x76);
00293             Status |= VL53L0X_device_read_strobe(Dev);
00294             Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
00295 
00296             DistMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000)
00297                             >> 24);
00298         }
00299 
00300         Status |= VL53L0X_WrByte(Dev, 0x81, 0x00);
00301         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
00302         Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
00303         Status |= VL53L0X_WrByte(Dev, 0x83, byte&0xfb);
00304         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
00305         Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
00306 
00307         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
00308         Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
00309     }
00310 
00311     if ((Status == VL53L0X_ERROR_NONE) &&
00312         (ReadDataFromDeviceDone != 7)) {
00313         /* Assign to variable if status is ok */
00314         if (((option & 1) == 1) &&
00315             ((ReadDataFromDeviceDone & 1) == 0)) {
00316             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00317                 ReferenceSpadCount, ReferenceSpadCount);
00318 
00319             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00320                 ReferenceSpadType, ReferenceSpadType);
00321 
00322             for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
00323                 Dev->Data .SpadData .RefGoodSpadMap [i] =
00324                     NvmRefGoodSpadMap[i];
00325             }
00326         }
00327 
00328         if (((option & 2) == 2) &&
00329             ((ReadDataFromDeviceDone & 2) == 0)) {
00330             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00331                     ModuleId, ModuleId);
00332 
00333             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00334                     Revision, Revision);
00335 
00336             ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
00337                     ProductId);
00338             VL53L0X_COPYSTRING(ProductId_tmp, ProductId);
00339 
00340         }
00341 
00342         if (((option & 4) == 4) &&
00343             ((ReadDataFromDeviceDone & 4) == 0)) {
00344             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00345                         PartUIDUpper, PartUIDUpper);
00346 
00347             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00348                         PartUIDLower, PartUIDLower);
00349 
00350             SignalRateMeasFixed400mmFix =
00351                 VL53L0X_FIXPOINT97TOFIXPOINT1616(
00352                     SignalRateMeasFixed1104_400_mm);
00353 
00354             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00355                 SignalRateMeasFixed400mm,
00356                 SignalRateMeasFixed400mmFix);
00357 
00358             OffsetMicroMeters = 0;
00359             if (DistMeasFixed1104_400_mm != 0) {
00360                     OffsetFixed1104_mm =
00361                         DistMeasFixed1104_400_mm -
00362                         DistMeasTgtFixed1104_mm;
00363                     OffsetMicroMeters = (OffsetFixed1104_mm
00364                         * 1000) >> 4;
00365                     OffsetMicroMeters *= -1;
00366             }
00367 
00368             PALDevDataSet(Dev,
00369                 Part2PartOffsetAdjustmentNVMMicroMeter,
00370                 OffsetMicroMeters);
00371         }
00372         byte = (uint8_t)(ReadDataFromDeviceDone|option);
00373         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone,
00374                 byte);
00375     }
00376 
00377     LOG_FUNCTION_END(Status);
00378     return Status;
00379 }
00380 
00381 VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
00382         int32_t *pOffsetCalibrationDataMicroMeter)
00383 {
00384     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00385     uint16_t RangeOffsetRegister;
00386     int16_t cMaxOffset = 2047;
00387     int16_t cOffsetRange = 4096;
00388 
00389     /* Note that offset has 10.2 format */
00390 
00391     Status = VL53L0X_RdWord(Dev,
00392                 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
00393                 &RangeOffsetRegister);
00394 
00395     if (Status == VL53L0X_ERROR_NONE) {
00396         RangeOffsetRegister = (RangeOffsetRegister & 0x0fff);
00397 
00398         /* Apply 12 bit 2's compliment conversion */
00399         if (RangeOffsetRegister > cMaxOffset)
00400             *pOffsetCalibrationDataMicroMeter =
00401                 (int16_t)(RangeOffsetRegister - cOffsetRange)
00402                     * 250;
00403         else
00404             *pOffsetCalibrationDataMicroMeter =
00405                 (int16_t)RangeOffsetRegister * 250;
00406 
00407     }
00408 
00409     return Status;
00410 }
00411 
00412 VL53L0X_Error VL53L0X::VL53L0X_GetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev,
00413     int32_t *pOffsetCalibrationDataMicroMeter)
00414 {
00415     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00416     LOG_FUNCTION_START("");
00417 
00418     Status = VL53L0X_get_offset_calibration_data_micro_meter(Dev,
00419         pOffsetCalibrationDataMicroMeter);
00420 
00421     LOG_FUNCTION_END(Status);
00422     return Status;
00423 }
00424 
00425 VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
00426         int32_t OffsetCalibrationDataMicroMeter)
00427 {
00428     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00429     int32_t cMaxOffsetMicroMeter = 511000;
00430     int32_t cMinOffsetMicroMeter = -512000;
00431     int16_t cOffsetRange = 4096;
00432     uint32_t encodedOffsetVal;
00433 
00434     LOG_FUNCTION_START("");
00435 
00436     if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter)
00437         OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter;
00438     else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter)
00439         OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter;
00440 
00441     /* The offset register is 10.2 format and units are mm
00442      * therefore conversion is applied by a division of
00443      * 250.
00444      */
00445     if (OffsetCalibrationDataMicroMeter >= 0) {
00446         encodedOffsetVal =
00447             OffsetCalibrationDataMicroMeter/250;
00448     } else {
00449         encodedOffsetVal =
00450             cOffsetRange +
00451             OffsetCalibrationDataMicroMeter/250;
00452     }
00453 
00454     Status = VL53L0X_WrWord(Dev,
00455         VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
00456         encodedOffsetVal);
00457 
00458     LOG_FUNCTION_END(Status);
00459     return Status;
00460 }
00461 
00462 VL53L0X_Error VL53L0X::VL53L0X_SetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev,
00463     int32_t OffsetCalibrationDataMicroMeter)
00464 {
00465     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00466     LOG_FUNCTION_START("");
00467 
00468     Status = VL53L0X_set_offset_calibration_data_micro_meter(Dev,
00469         OffsetCalibrationDataMicroMeter);
00470 
00471     LOG_FUNCTION_END(Status);
00472     return Status;
00473 }
00474 
00475 VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev)
00476 {
00477     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00478     int32_t CorrectedOffsetMicroMeters;
00479     int32_t CurrentOffsetMicroMeters;
00480 
00481     /* if we run on this function we can read all the NVM info
00482      * used by the API */
00483     Status = VL53L0X_get_info_from_device(Dev, 7);
00484 
00485     /* Read back current device offset */
00486     if (Status == VL53L0X_ERROR_NONE) {
00487         Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
00488                     &CurrentOffsetMicroMeters);
00489     }
00490 
00491     /* Apply Offset Adjustment derived from 400mm measurements */
00492     if (Status == VL53L0X_ERROR_NONE) {
00493 
00494         /* Store initial device offset */
00495         PALDevDataSet(Dev, Part2PartOffsetNVMMicroMeter,
00496             CurrentOffsetMicroMeters);
00497 
00498         CorrectedOffsetMicroMeters = CurrentOffsetMicroMeters +
00499             (int32_t)PALDevDataGet(Dev,
00500                 Part2PartOffsetAdjustmentNVMMicroMeter);
00501 
00502         Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
00503                     CorrectedOffsetMicroMeters);
00504 
00505         /* store current, adjusted offset */
00506         if (Status == VL53L0X_ERROR_NONE) {
00507             VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
00508                     CorrectedOffsetMicroMeters);
00509         }
00510     }
00511 
00512     return Status;
00513 }
00514 
00515 VL53L0X_Error VL53L0X::VL53L0X_GetDeviceMode(VL53L0X_DEV Dev,
00516     VL53L0X_DeviceModes *pDeviceMode)
00517 {
00518     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00519     LOG_FUNCTION_START("");
00520 
00521     VL53L0X_GETPARAMETERFIELD(Dev, DeviceMode, *pDeviceMode);
00522 
00523     LOG_FUNCTION_END(Status);
00524     return Status;
00525 }
00526 
00527 VL53L0X_Error VL53L0X::VL53L0X_GetInterMeasurementPeriodMilliSeconds(VL53L0X_DEV Dev,
00528     uint32_t *pInterMeasurementPeriodMilliSeconds)
00529 {
00530     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00531     uint16_t osc_calibrate_val;
00532     uint32_t IMPeriodMilliSeconds;
00533 
00534     LOG_FUNCTION_START("");
00535 
00536     Status = VL53L0X_RdWord(Dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
00537         &osc_calibrate_val);
00538 
00539     if (Status == VL53L0X_ERROR_NONE) {
00540         Status = VL53L0X_RdDWord(Dev,
00541         VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
00542             &IMPeriodMilliSeconds);
00543     }
00544 
00545     if (Status == VL53L0X_ERROR_NONE) {
00546         if (osc_calibrate_val != 0) {
00547             *pInterMeasurementPeriodMilliSeconds =
00548                 IMPeriodMilliSeconds / osc_calibrate_val;
00549         }
00550         VL53L0X_SETPARAMETERFIELD(Dev,
00551             InterMeasurementPeriodMilliSeconds,
00552             *pInterMeasurementPeriodMilliSeconds);
00553     }
00554 
00555     LOG_FUNCTION_END(Status);
00556     return Status;
00557 }
00558 
00559 VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationRateMegaCps(VL53L0X_DEV Dev,
00560     FixPoint1616_t *pXTalkCompensationRateMegaCps)
00561 {
00562     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00563     uint16_t Value;
00564     FixPoint1616_t TempFix1616;
00565 
00566     LOG_FUNCTION_START("");
00567 
00568     Status = VL53L0X_RdWord(Dev,
00569     VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&Value);
00570     if (Status == VL53L0X_ERROR_NONE) {
00571         if (Value == 0) {
00572             /* the Xtalk is disabled return value from memory */
00573             VL53L0X_GETPARAMETERFIELD(Dev,
00574                 XTalkCompensationRateMegaCps, TempFix1616);
00575             *pXTalkCompensationRateMegaCps = TempFix1616;
00576             VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable,
00577                 0);
00578         } else {
00579             TempFix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(Value);
00580             *pXTalkCompensationRateMegaCps = TempFix1616;
00581             VL53L0X_SETPARAMETERFIELD(Dev,
00582                 XTalkCompensationRateMegaCps, TempFix1616);
00583             VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable,
00584                 1);
00585         }
00586     }
00587 
00588     LOG_FUNCTION_END(Status);
00589     return Status;
00590 }
00591 
00592 VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId,
00593     FixPoint1616_t *pLimitCheckValue)
00594 {
00595     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00596     uint8_t EnableZeroValue = 0;
00597     uint16_t Temp16;
00598     FixPoint1616_t TempFix1616;
00599 
00600     LOG_FUNCTION_START("");
00601 
00602     switch (LimitCheckId) {
00603 
00604     case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
00605         /* internal computation: */
00606         VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
00607             VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, TempFix1616);
00608         EnableZeroValue = 0;
00609         break;
00610 
00611     case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
00612         Status = VL53L0X_RdWord(Dev,
00613         VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
00614             &Temp16);
00615         if (Status == VL53L0X_ERROR_NONE)
00616             TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16);
00617 
00618 
00619         EnableZeroValue = 1;
00620         break;
00621 
00622     case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
00623         /* internal computation: */
00624         VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
00625             VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, TempFix1616);
00626         EnableZeroValue = 0;
00627         break;
00628 
00629     case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
00630         /* internal computation: */
00631         VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
00632             VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, TempFix1616);
00633         EnableZeroValue = 0;
00634         break;
00635 
00636     case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
00637     case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
00638         Status = VL53L0X_RdWord(Dev,
00639             VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
00640             &Temp16);
00641         if (Status == VL53L0X_ERROR_NONE)
00642             TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16);
00643 
00644 
00645         EnableZeroValue = 0;
00646         break;
00647 
00648     default:
00649         Status = VL53L0X_ERROR_INVALID_PARAMS;
00650 
00651     }
00652 
00653     if (Status == VL53L0X_ERROR_NONE) {
00654 
00655         if (EnableZeroValue == 1) {
00656 
00657             if (TempFix1616 == 0) {
00658                 /* disabled: return value from memory */
00659                 VL53L0X_GETARRAYPARAMETERFIELD(Dev,
00660                     LimitChecksValue, LimitCheckId,
00661                     TempFix1616);
00662                 *pLimitCheckValue = TempFix1616;
00663                 VL53L0X_SETARRAYPARAMETERFIELD(Dev,
00664                     LimitChecksEnable, LimitCheckId, 0);
00665             } else {
00666                 *pLimitCheckValue = TempFix1616;
00667                 VL53L0X_SETARRAYPARAMETERFIELD(Dev,
00668                     LimitChecksValue, LimitCheckId,
00669                     TempFix1616);
00670                 VL53L0X_SETARRAYPARAMETERFIELD(Dev,
00671                     LimitChecksEnable, LimitCheckId, 1);
00672             }
00673         } else {
00674             *pLimitCheckValue = TempFix1616;
00675         }
00676     }
00677 
00678     LOG_FUNCTION_END(Status);
00679     return Status;
00680 
00681 }
00682 
00683 VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId,
00684     uint8_t *pLimitCheckEnable)
00685 {
00686     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00687     uint8_t Temp8;
00688 
00689     LOG_FUNCTION_START("");
00690 
00691     if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
00692         Status = VL53L0X_ERROR_INVALID_PARAMS;
00693         *pLimitCheckEnable = 0;
00694     } else {
00695         VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
00696             LimitCheckId, Temp8);
00697         *pLimitCheckEnable = Temp8;
00698     }
00699 
00700     LOG_FUNCTION_END(Status);
00701     return Status;
00702 }
00703 
00704 VL53L0X_Error VL53L0X::VL53L0X_GetWrapAroundCheckEnable(VL53L0X_DEV Dev,
00705     uint8_t *pWrapAroundCheckEnable)
00706 {
00707     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00708     uint8_t data;
00709 
00710     LOG_FUNCTION_START("");
00711 
00712     Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data);
00713     if (Status == VL53L0X_ERROR_NONE) {
00714         PALDevDataSet(Dev, SequenceConfig, data);
00715         if (data & (0x01 << 7))
00716             *pWrapAroundCheckEnable = 0x01;
00717         else
00718             *pWrapAroundCheckEnable = 0x00;
00719     }
00720     if (Status == VL53L0X_ERROR_NONE) {
00721         VL53L0X_SETPARAMETERFIELD(Dev, WrapAroundCheckEnable,
00722             *pWrapAroundCheckEnable);
00723     }
00724 
00725     LOG_FUNCTION_END(Status);
00726     return Status;
00727 }
00728 
00729 VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_DEV Dev,
00730     VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceConfig,
00731     uint8_t *pSequenceStepEnabled)
00732 {
00733     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00734     *pSequenceStepEnabled = 0;
00735     LOG_FUNCTION_START("");
00736 
00737     switch (SequenceStepId) {
00738     case VL53L0X_SEQUENCESTEP_TCC:
00739         *pSequenceStepEnabled = (SequenceConfig & 0x10) >> 4;
00740         break;
00741     case VL53L0X_SEQUENCESTEP_DSS:
00742         *pSequenceStepEnabled = (SequenceConfig & 0x08) >> 3;
00743         break;
00744     case VL53L0X_SEQUENCESTEP_MSRC:
00745         *pSequenceStepEnabled = (SequenceConfig & 0x04) >> 2;
00746         break;
00747     case VL53L0X_SEQUENCESTEP_PRE_RANGE:
00748         *pSequenceStepEnabled = (SequenceConfig & 0x40) >> 6;
00749         break;
00750     case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
00751         *pSequenceStepEnabled = (SequenceConfig & 0x80) >> 7;
00752         break;
00753     default:
00754         Status = VL53L0X_ERROR_INVALID_PARAMS;
00755     }
00756 
00757     LOG_FUNCTION_END(Status);
00758     return Status;
00759 }
00760 
00761 VL53L0X_Error VL53L0X::VL53L0X_GetSequenceStepEnables(VL53L0X_DEV Dev,
00762     VL53L0X_SchedulerSequenceSteps_t *pSchedulerSequenceSteps)
00763 {
00764     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00765     uint8_t SequenceConfig = 0;
00766     LOG_FUNCTION_START("");
00767 
00768     Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
00769         &SequenceConfig);
00770 
00771     if (Status == VL53L0X_ERROR_NONE) {
00772         Status = sequence_step_enabled(Dev,
00773         VL53L0X_SEQUENCESTEP_TCC, SequenceConfig,
00774             &pSchedulerSequenceSteps->TccOn);
00775     }
00776     if (Status == VL53L0X_ERROR_NONE) {
00777         Status = sequence_step_enabled(Dev,
00778         VL53L0X_SEQUENCESTEP_DSS, SequenceConfig,
00779             &pSchedulerSequenceSteps->DssOn);
00780     }
00781     if (Status == VL53L0X_ERROR_NONE) {
00782         Status = sequence_step_enabled(Dev,
00783         VL53L0X_SEQUENCESTEP_MSRC, SequenceConfig,
00784             &pSchedulerSequenceSteps->MsrcOn);
00785     }
00786     if (Status == VL53L0X_ERROR_NONE) {
00787         Status = sequence_step_enabled(Dev,
00788         VL53L0X_SEQUENCESTEP_PRE_RANGE, SequenceConfig,
00789             &pSchedulerSequenceSteps->PreRangeOn);
00790     }
00791     if (Status == VL53L0X_ERROR_NONE) {
00792         Status = sequence_step_enabled(Dev,
00793         VL53L0X_SEQUENCESTEP_FINAL_RANGE, SequenceConfig,
00794             &pSchedulerSequenceSteps->FinalRangeOn);
00795     }
00796 
00797     LOG_FUNCTION_END(Status);
00798     return Status;
00799 }
00800 
00801 uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
00802 {
00803     /*!
00804      * Converts the encoded VCSEL period register value into the real
00805      * period in PLL clocks
00806      */
00807 
00808     uint8_t vcsel_period_pclks = 0;
00809 
00810     vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
00811 
00812     return vcsel_period_pclks;
00813 }
00814 
00815 uint8_t VL53L0X::VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks)
00816 {
00817     /*!
00818      * Converts the encoded VCSEL period register value into the real period
00819      * in PLL clocks
00820      */
00821 
00822     uint8_t vcsel_period_reg = 0;
00823 
00824     vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
00825 
00826     return vcsel_period_reg;
00827 }
00828 
00829 
00830 VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
00831     VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
00832 {
00833     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00834     uint8_t vcsel_period_reg;
00835     uint8_t MinPreVcselPeriodPCLK = 12;
00836     uint8_t MaxPreVcselPeriodPCLK = 18;
00837     uint8_t MinFinalVcselPeriodPCLK = 8;
00838     uint8_t MaxFinalVcselPeriodPCLK = 14;
00839     uint32_t MeasurementTimingBudgetMicroSeconds;
00840     uint32_t FinalRangeTimeoutMicroSeconds;
00841     uint32_t PreRangeTimeoutMicroSeconds;
00842     uint32_t MsrcTimeoutMicroSeconds;
00843     uint8_t PhaseCalInt = 0;
00844 
00845     /* Check if valid clock period requested */
00846 
00847     if ((VCSELPulsePeriodPCLK % 2) != 0) {
00848         /* Value must be an even number */
00849         Status = VL53L0X_ERROR_INVALID_PARAMS;
00850     } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
00851         (VCSELPulsePeriodPCLK < MinPreVcselPeriodPCLK ||
00852         VCSELPulsePeriodPCLK > MaxPreVcselPeriodPCLK)) {
00853         Status = VL53L0X_ERROR_INVALID_PARAMS;
00854     } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
00855         (VCSELPulsePeriodPCLK < MinFinalVcselPeriodPCLK ||
00856          VCSELPulsePeriodPCLK > MaxFinalVcselPeriodPCLK)) {
00857 
00858         Status = VL53L0X_ERROR_INVALID_PARAMS;
00859     }
00860 
00861     /* Apply specific settings for the requested clock period */
00862 
00863     if (Status != VL53L0X_ERROR_NONE)
00864         return Status;
00865 
00866 
00867     if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
00868 
00869         /* Set phase check limits */
00870         if (VCSELPulsePeriodPCLK == 12) {
00871 
00872             Status = VL53L0X_WrByte(Dev,
00873                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00874                 0x18);
00875             Status = VL53L0X_WrByte(Dev,
00876                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00877                 0x08);
00878         } else if (VCSELPulsePeriodPCLK == 14) {
00879 
00880             Status = VL53L0X_WrByte(Dev,
00881                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00882                 0x30);
00883             Status = VL53L0X_WrByte(Dev,
00884                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00885                 0x08);
00886         } else if (VCSELPulsePeriodPCLK == 16) {
00887 
00888             Status = VL53L0X_WrByte(Dev,
00889                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00890                 0x40);
00891             Status = VL53L0X_WrByte(Dev,
00892                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00893                 0x08);
00894         } else if (VCSELPulsePeriodPCLK == 18) {
00895 
00896             Status = VL53L0X_WrByte(Dev,
00897                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00898                 0x50);
00899             Status = VL53L0X_WrByte(Dev,
00900                 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00901                 0x08);
00902         }
00903     } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
00904 
00905         if (VCSELPulsePeriodPCLK == 8) {
00906 
00907             Status = VL53L0X_WrByte(Dev,
00908                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00909                 0x10);
00910             Status = VL53L0X_WrByte(Dev,
00911                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00912                 0x08);
00913 
00914             Status |= VL53L0X_WrByte(Dev,
00915                 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
00916             Status |= VL53L0X_WrByte(Dev,
00917                 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
00918 
00919             Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
00920             Status |= VL53L0X_WrByte(Dev,
00921                 VL53L0X_REG_ALGO_PHASECAL_LIM,
00922                 0x30);
00923             Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
00924         } else if (VCSELPulsePeriodPCLK == 10) {
00925 
00926             Status = VL53L0X_WrByte(Dev,
00927                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00928                 0x28);
00929             Status = VL53L0X_WrByte(Dev,
00930                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00931                 0x08);
00932 
00933             Status |= VL53L0X_WrByte(Dev,
00934                 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
00935             Status |= VL53L0X_WrByte(Dev,
00936                 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
00937 
00938             Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
00939             Status |= VL53L0X_WrByte(Dev,
00940                 VL53L0X_REG_ALGO_PHASECAL_LIM,
00941                 0x20);
00942             Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
00943         } else if (VCSELPulsePeriodPCLK == 12) {
00944 
00945             Status = VL53L0X_WrByte(Dev,
00946                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00947                 0x38);
00948             Status = VL53L0X_WrByte(Dev,
00949                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00950                 0x08);
00951 
00952             Status |= VL53L0X_WrByte(Dev,
00953                 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
00954             Status |= VL53L0X_WrByte(Dev,
00955                 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
00956 
00957             Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
00958             Status |= VL53L0X_WrByte(Dev,
00959                 VL53L0X_REG_ALGO_PHASECAL_LIM,
00960                 0x20);
00961             Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
00962         } else if (VCSELPulsePeriodPCLK == 14) {
00963 
00964             Status = VL53L0X_WrByte(Dev,
00965                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00966                 0x048);
00967             Status = VL53L0X_WrByte(Dev,
00968                 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00969                 0x08);
00970 
00971             Status |= VL53L0X_WrByte(Dev,
00972                 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
00973             Status |= VL53L0X_WrByte(Dev,
00974                 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
00975 
00976             Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
00977             Status |= VL53L0X_WrByte(Dev,
00978                 VL53L0X_REG_ALGO_PHASECAL_LIM,
00979                 0x20);
00980             Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
00981         }
00982     }
00983 
00984 
00985     /* Re-calculate and apply timeouts, in macro periods */
00986 
00987     if (Status == VL53L0X_ERROR_NONE) {
00988         vcsel_period_reg = VL53L0X_encode_vcsel_period((uint8_t)
00989             VCSELPulsePeriodPCLK);
00990 
00991         /* When the VCSEL period for the pre or final range is changed,
00992         * the corresponding timeout must be read from the device using
00993         * the current VCSEL period, then the new VCSEL period can be
00994         * applied. The timeout then must be written back to the device
00995         * using the new VCSEL period.
00996         *
00997         * For the MSRC timeout, the same applies - this timeout being
00998         * dependant on the pre-range vcsel period.
00999         */
01000         switch (VcselPeriodType) {
01001         case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
01002             Status = get_sequence_step_timeout(Dev,
01003                 VL53L0X_SEQUENCESTEP_PRE_RANGE,
01004                 &PreRangeTimeoutMicroSeconds);
01005 
01006             if (Status == VL53L0X_ERROR_NONE)
01007                 Status = get_sequence_step_timeout(Dev,
01008                     VL53L0X_SEQUENCESTEP_MSRC,
01009                     &MsrcTimeoutMicroSeconds);
01010 
01011             if (Status == VL53L0X_ERROR_NONE)
01012                 Status = VL53L0X_WrByte(Dev,
01013                 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
01014                     vcsel_period_reg);
01015 
01016 
01017             if (Status == VL53L0X_ERROR_NONE)
01018                 Status = set_sequence_step_timeout(Dev,
01019                     VL53L0X_SEQUENCESTEP_PRE_RANGE,
01020                     PreRangeTimeoutMicroSeconds);
01021 
01022 
01023             if (Status == VL53L0X_ERROR_NONE)
01024                 Status = set_sequence_step_timeout(Dev,
01025                     VL53L0X_SEQUENCESTEP_MSRC,
01026                     MsrcTimeoutMicroSeconds);
01027 
01028             VL53L0X_SETDEVICESPECIFICPARAMETER(
01029                 Dev,
01030                 PreRangeVcselPulsePeriod,
01031                 VCSELPulsePeriodPCLK);
01032             break;
01033         case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
01034             Status = get_sequence_step_timeout(Dev,
01035                 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
01036                 &FinalRangeTimeoutMicroSeconds);
01037 
01038             if (Status == VL53L0X_ERROR_NONE)
01039                 Status = VL53L0X_WrByte(Dev,
01040                 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
01041                     vcsel_period_reg);
01042 
01043 
01044             if (Status == VL53L0X_ERROR_NONE)
01045                 Status = set_sequence_step_timeout(Dev,
01046                     VL53L0X_SEQUENCESTEP_FINAL_RANGE,
01047                     FinalRangeTimeoutMicroSeconds);
01048 
01049             VL53L0X_SETDEVICESPECIFICPARAMETER(
01050                 Dev,
01051                 FinalRangeVcselPulsePeriod,
01052                 VCSELPulsePeriodPCLK);
01053             break;
01054         default:
01055             Status = VL53L0X_ERROR_INVALID_PARAMS;
01056         }
01057     }
01058 
01059     /* Finally, the timing budget must be re-applied */
01060     if (Status == VL53L0X_ERROR_NONE) {
01061         VL53L0X_GETPARAMETERFIELD(Dev,
01062             MeasurementTimingBudgetMicroSeconds,
01063             MeasurementTimingBudgetMicroSeconds);
01064 
01065         Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
01066                 MeasurementTimingBudgetMicroSeconds);
01067     }
01068 
01069     /* Perform the phase calibration. This is needed after changing on
01070      * vcsel period.
01071      * get_data_enable = 0, restore_config = 1 */
01072     if (Status == VL53L0X_ERROR_NONE)
01073         Status = VL53L0X_perform_phase_calibration(
01074             Dev, &PhaseCalInt, 0, 1);
01075 
01076     return Status;
01077 }
01078 
01079 VL53L0X_Error VL53L0X::VL53L0X_SetVcselPulsePeriod(VL53L0X_DEV Dev,
01080     VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
01081 {
01082     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01083     LOG_FUNCTION_START("");
01084 
01085     Status = VL53L0X_set_vcsel_pulse_period(Dev, VcselPeriodType,
01086         VCSELPulsePeriodPCLK);
01087 
01088     LOG_FUNCTION_END(Status);
01089     return Status;
01090 }
01091 
01092 VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
01093     VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
01094 {
01095     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01096     uint8_t vcsel_period_reg;
01097 
01098     switch (VcselPeriodType) {
01099     case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
01100         Status = VL53L0X_RdByte(Dev,
01101             VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
01102             &vcsel_period_reg);
01103     break;
01104     case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
01105         Status = VL53L0X_RdByte(Dev,
01106             VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
01107             &vcsel_period_reg);
01108     break;
01109     default:
01110         Status = VL53L0X_ERROR_INVALID_PARAMS;
01111     }
01112 
01113     if (Status == VL53L0X_ERROR_NONE)
01114         *pVCSELPulsePeriodPCLK =
01115             VL53L0X_decode_vcsel_period(vcsel_period_reg);
01116 
01117     return Status;
01118 }
01119 
01120 VL53L0X_Error VL53L0X::VL53L0X_GetVcselPulsePeriod(VL53L0X_DEV Dev,
01121     VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
01122 {
01123     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01124     LOG_FUNCTION_START("");
01125 
01126     Status = VL53L0X_get_vcsel_pulse_period(Dev, VcselPeriodType,
01127         pVCSELPulsePeriodPCLK);
01128 
01129     LOG_FUNCTION_END(Status);
01130     return Status;
01131 }
01132 
01133 uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout)
01134 {
01135     /*!
01136      * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
01137      */
01138 
01139     uint32_t timeout_macro_clks = 0;
01140 
01141     timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
01142             << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1;
01143 
01144     return timeout_macro_clks;
01145 }
01146 
01147 uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev, uint8_t vcsel_period_pclks)
01148 {
01149     uint64_t PLL_period_ps;
01150     uint32_t macro_period_vclks;
01151     uint32_t macro_period_ps;
01152 
01153     LOG_FUNCTION_START("");
01154 
01155     /* The above calculation will produce rounding errors,
01156        therefore set fixed value
01157     */
01158     PLL_period_ps = 1655;
01159 
01160     macro_period_vclks = 2304;
01161     macro_period_ps = (uint32_t)(macro_period_vclks
01162             * vcsel_period_pclks * PLL_period_ps);
01163 
01164     LOG_FUNCTION_END("");
01165     return macro_period_ps;
01166 }
01167 
01168 /* To convert register value into us */
01169 uint32_t VL53L0X::VL53L0X_calc_timeout_us(VL53L0X_DEV Dev,
01170         uint16_t timeout_period_mclks,
01171         uint8_t vcsel_period_pclks)
01172 {
01173     uint32_t macro_period_ps;
01174     uint32_t macro_period_ns;
01175     uint32_t actual_timeout_period_us = 0;
01176 
01177     macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
01178     macro_period_ns = (macro_period_ps + 500) / 1000;
01179 
01180     actual_timeout_period_us =
01181         ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
01182 
01183     return actual_timeout_period_us;
01184 }
01185 
01186 VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV Dev,
01187                 VL53L0X_SequenceStepId SequenceStepId,
01188                 uint32_t *pTimeOutMicroSecs)
01189 {
01190     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01191     uint8_t CurrentVCSELPulsePeriodPClk;
01192     uint8_t EncodedTimeOutByte = 0;
01193     uint32_t TimeoutMicroSeconds = 0;
01194     uint16_t PreRangeEncodedTimeOut = 0;
01195     uint16_t MsrcTimeOutMClks;
01196     uint16_t PreRangeTimeOutMClks;
01197     uint16_t FinalRangeTimeOutMClks = 0;
01198     uint16_t FinalRangeEncodedTimeOut;
01199     VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
01200 
01201     if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC)     ||
01202         (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS)     ||
01203         (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
01204 
01205         Status = VL53L0X_GetVcselPulsePeriod(Dev,
01206                     VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01207                     &CurrentVCSELPulsePeriodPClk);
01208         if (Status == VL53L0X_ERROR_NONE) {
01209             Status = VL53L0X_RdByte(Dev,
01210                     VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
01211                     &EncodedTimeOutByte);
01212         }
01213         MsrcTimeOutMClks = VL53L0X_decode_timeout(EncodedTimeOutByte);
01214 
01215         TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
01216                         MsrcTimeOutMClks,
01217                         CurrentVCSELPulsePeriodPClk);
01218     } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
01219         /* Retrieve PRE-RANGE VCSEL Period */
01220         Status = VL53L0X_GetVcselPulsePeriod(Dev,
01221                         VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01222                         &CurrentVCSELPulsePeriodPClk);
01223 
01224         /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
01225         if (Status == VL53L0X_ERROR_NONE) {
01226 
01227             /* Retrieve PRE-RANGE VCSEL Period */
01228             Status = VL53L0X_GetVcselPulsePeriod(Dev,
01229                     VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01230                     &CurrentVCSELPulsePeriodPClk);
01231 
01232             if (Status == VL53L0X_ERROR_NONE) {
01233                 Status = VL53L0X_RdWord(Dev,
01234                 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
01235                 &PreRangeEncodedTimeOut);
01236             }
01237 
01238             PreRangeTimeOutMClks = VL53L0X_decode_timeout(
01239                     PreRangeEncodedTimeOut);
01240 
01241             TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
01242                     PreRangeTimeOutMClks,
01243                     CurrentVCSELPulsePeriodPClk);
01244         }
01245     } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
01246 
01247         VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
01248         PreRangeTimeOutMClks = 0;
01249 
01250         if (SchedulerSequenceSteps.PreRangeOn) {
01251             /* Retrieve PRE-RANGE VCSEL Period */
01252             Status = VL53L0X_GetVcselPulsePeriod(Dev,
01253                 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01254                 &CurrentVCSELPulsePeriodPClk);
01255 
01256             /* Retrieve PRE-RANGE Timeout in Macro periods
01257              * (MCLKS) */
01258             if (Status == VL53L0X_ERROR_NONE) {
01259                 Status = VL53L0X_RdWord(Dev,
01260                 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
01261                 &PreRangeEncodedTimeOut);
01262                 PreRangeTimeOutMClks = VL53L0X_decode_timeout(
01263                         PreRangeEncodedTimeOut);
01264             }
01265         }
01266 
01267         if (Status == VL53L0X_ERROR_NONE) {
01268             /* Retrieve FINAL-RANGE VCSEL Period */
01269             Status = VL53L0X_GetVcselPulsePeriod(Dev,
01270                     VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
01271                     &CurrentVCSELPulsePeriodPClk);
01272         }
01273 
01274         /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
01275         if (Status == VL53L0X_ERROR_NONE) {
01276             Status = VL53L0X_RdWord(Dev,
01277                 VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
01278                 &FinalRangeEncodedTimeOut);
01279             FinalRangeTimeOutMClks = VL53L0X_decode_timeout(
01280                     FinalRangeEncodedTimeOut);
01281         }
01282 
01283         FinalRangeTimeOutMClks -= PreRangeTimeOutMClks;
01284         TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
01285                         FinalRangeTimeOutMClks,
01286                         CurrentVCSELPulsePeriodPClk);
01287     }
01288 
01289     *pTimeOutMicroSecs = TimeoutMicroSeconds;
01290 
01291     return Status;
01292 }
01293 
01294 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
01295         uint32_t *pMeasurementTimingBudgetMicroSeconds)
01296 {
01297     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01298     VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
01299     uint32_t FinalRangeTimeoutMicroSeconds;
01300     uint32_t MsrcDccTccTimeoutMicroSeconds  = 2000;
01301     uint32_t StartOverheadMicroSeconds      = 1910;
01302     uint32_t EndOverheadMicroSeconds        = 960;
01303     uint32_t MsrcOverheadMicroSeconds       = 660;
01304     uint32_t TccOverheadMicroSeconds        = 590;
01305     uint32_t DssOverheadMicroSeconds        = 690;
01306     uint32_t PreRangeOverheadMicroSeconds   = 660;
01307     uint32_t FinalRangeOverheadMicroSeconds = 550;
01308     uint32_t PreRangeTimeoutMicroSeconds    = 0;
01309 
01310     LOG_FUNCTION_START("");
01311 
01312     /* Start and end overhead times always present */
01313     *pMeasurementTimingBudgetMicroSeconds
01314         = StartOverheadMicroSeconds + EndOverheadMicroSeconds;
01315 
01316     Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
01317 
01318     if (Status != VL53L0X_ERROR_NONE) {
01319         LOG_FUNCTION_END(Status);
01320         return Status;
01321     }
01322 
01323 
01324     if (SchedulerSequenceSteps.TccOn  ||
01325         SchedulerSequenceSteps.MsrcOn ||
01326         SchedulerSequenceSteps.DssOn) {
01327 
01328         Status = get_sequence_step_timeout(Dev,
01329                 VL53L0X_SEQUENCESTEP_MSRC,
01330                 &MsrcDccTccTimeoutMicroSeconds);
01331 
01332         if (Status == VL53L0X_ERROR_NONE) {
01333             if (SchedulerSequenceSteps.TccOn) {
01334                 *pMeasurementTimingBudgetMicroSeconds +=
01335                     MsrcDccTccTimeoutMicroSeconds +
01336                     TccOverheadMicroSeconds;
01337             }
01338 
01339             if (SchedulerSequenceSteps.DssOn) {
01340                 *pMeasurementTimingBudgetMicroSeconds +=
01341                 2 * (MsrcDccTccTimeoutMicroSeconds +
01342                     DssOverheadMicroSeconds);
01343             } else if (SchedulerSequenceSteps.MsrcOn) {
01344                 *pMeasurementTimingBudgetMicroSeconds +=
01345                     MsrcDccTccTimeoutMicroSeconds +
01346                     MsrcOverheadMicroSeconds;
01347             }
01348         }
01349     }
01350 
01351     if (Status == VL53L0X_ERROR_NONE) {
01352         if (SchedulerSequenceSteps.PreRangeOn) {
01353             Status = get_sequence_step_timeout(Dev,
01354                 VL53L0X_SEQUENCESTEP_PRE_RANGE,
01355                 &PreRangeTimeoutMicroSeconds);
01356             *pMeasurementTimingBudgetMicroSeconds +=
01357                 PreRangeTimeoutMicroSeconds +
01358                 PreRangeOverheadMicroSeconds;
01359         }
01360     }
01361 
01362     if (Status == VL53L0X_ERROR_NONE) {
01363         if (SchedulerSequenceSteps.FinalRangeOn) {
01364             Status = get_sequence_step_timeout(Dev,
01365                     VL53L0X_SEQUENCESTEP_FINAL_RANGE,
01366                     &FinalRangeTimeoutMicroSeconds);
01367             *pMeasurementTimingBudgetMicroSeconds +=
01368                 (FinalRangeTimeoutMicroSeconds +
01369                 FinalRangeOverheadMicroSeconds);
01370         }
01371     }
01372 
01373     if (Status == VL53L0X_ERROR_NONE) {
01374         VL53L0X_SETPARAMETERFIELD(Dev,
01375             MeasurementTimingBudgetMicroSeconds,
01376             *pMeasurementTimingBudgetMicroSeconds);
01377     }
01378 
01379     LOG_FUNCTION_END(Status);
01380     return Status;
01381 }
01382 
01383 VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev,
01384     uint32_t *pMeasurementTimingBudgetMicroSeconds)
01385 {
01386     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01387     LOG_FUNCTION_START("");
01388 
01389     Status = VL53L0X_get_measurement_timing_budget_micro_seconds(Dev,
01390         pMeasurementTimingBudgetMicroSeconds);
01391 
01392     LOG_FUNCTION_END(Status);
01393     return Status;
01394 }
01395 
01396 VL53L0X_Error VL53L0X::VL53L0X_GetDeviceParameters(VL53L0X_DEV Dev,
01397     VL53L0X_DeviceParameters_t *pDeviceParameters)
01398 {
01399     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01400     int i;
01401 
01402     LOG_FUNCTION_START("");
01403 
01404     Status = VL53L0X_GetDeviceMode(Dev, &(pDeviceParameters->DeviceMode ));
01405 
01406     if (Status == VL53L0X_ERROR_NONE)
01407         Status = VL53L0X_GetInterMeasurementPeriodMilliSeconds(Dev,
01408         &(pDeviceParameters->InterMeasurementPeriodMilliSeconds ));
01409 
01410 
01411     if (Status == VL53L0X_ERROR_NONE)
01412         pDeviceParameters->XTalkCompensationEnable  = 0;
01413 
01414     if (Status == VL53L0X_ERROR_NONE)
01415         Status = VL53L0X_GetXTalkCompensationRateMegaCps(Dev,
01416             &(pDeviceParameters->XTalkCompensationRateMegaCps ));
01417 
01418 
01419     if (Status == VL53L0X_ERROR_NONE)
01420         Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
01421             &(pDeviceParameters->RangeOffsetMicroMeters ));
01422 
01423 
01424     if (Status == VL53L0X_ERROR_NONE) {
01425         for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
01426             /* get first the values, then the enables.
01427              * VL53L0X_GetLimitCheckValue will modify the enable
01428              * flags
01429              */
01430             if (Status == VL53L0X_ERROR_NONE) {
01431                 Status |= VL53L0X_GetLimitCheckValue(Dev, i,
01432                 &(pDeviceParameters->LimitChecksValue [i]));
01433             } else {
01434                 break;
01435             }
01436             if (Status == VL53L0X_ERROR_NONE) {
01437                 Status |= VL53L0X_GetLimitCheckEnable(Dev, i,
01438                 &(pDeviceParameters->LimitChecksEnable [i]));
01439             } else {
01440                 break;
01441             }
01442         }
01443     }
01444 
01445     if (Status == VL53L0X_ERROR_NONE) {
01446         Status = VL53L0X_GetWrapAroundCheckEnable(Dev,
01447             &(pDeviceParameters->WrapAroundCheckEnable ));
01448     }
01449 
01450     /* Need to be done at the end as it uses VCSELPulsePeriod */
01451     if (Status == VL53L0X_ERROR_NONE) {
01452         Status = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(Dev,
01453         &(pDeviceParameters->MeasurementTimingBudgetMicroSeconds ));
01454     }
01455 
01456     LOG_FUNCTION_END(Status);
01457     return Status;
01458 }
01459 
01460 VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId,
01461     FixPoint1616_t LimitCheckValue)
01462 {
01463     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01464     uint8_t Temp8;
01465 
01466     LOG_FUNCTION_START("");
01467 
01468     VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId,
01469         Temp8);
01470 
01471     if (Temp8 == 0) { /* disabled write only internal value */
01472         VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
01473             LimitCheckId, LimitCheckValue);
01474     } else {
01475 
01476         switch (LimitCheckId) {
01477 
01478         case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
01479             /* internal computation: */
01480             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
01481                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
01482                 LimitCheckValue);
01483             break;
01484 
01485         case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
01486 
01487             Status = VL53L0X_WrWord(Dev,
01488             VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
01489                 VL53L0X_FIXPOINT1616TOFIXPOINT97(
01490                     LimitCheckValue));
01491 
01492             break;
01493 
01494         case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
01495 
01496             /* internal computation: */
01497             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
01498                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
01499                 LimitCheckValue);
01500 
01501             break;
01502 
01503         case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
01504 
01505             /* internal computation: */
01506             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
01507                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
01508                 LimitCheckValue);
01509 
01510             break;
01511 
01512         case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
01513         case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
01514 
01515             Status = VL53L0X_WrWord(Dev,
01516                 VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
01517                 VL53L0X_FIXPOINT1616TOFIXPOINT97(
01518                     LimitCheckValue));
01519 
01520             break;
01521 
01522         default:
01523             Status = VL53L0X_ERROR_INVALID_PARAMS;
01524 
01525         }
01526 
01527         if (Status == VL53L0X_ERROR_NONE) {
01528             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
01529                 LimitCheckId, LimitCheckValue);
01530         }
01531     }
01532 
01533     LOG_FUNCTION_END(Status);
01534     return Status;
01535 }
01536 
01537 VL53L0X_Error VL53L0X::VL53L0X_DataInit(VL53L0X_DEV Dev)
01538 {
01539     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01540     VL53L0X_DeviceParameters_t CurrentParameters;
01541     int i;
01542     uint8_t StopVariable;
01543 
01544     LOG_FUNCTION_START("");
01545 
01546     /* by default the I2C is running at 1V8 if you want to change it you
01547      * need to include this define at compilation level. */
01548 #ifdef USE_I2C_2V8
01549     Status = VL53L0X_UpdateByte(Dev,
01550         VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
01551         0xFE,
01552         0x01);
01553 #endif
01554 
01555     /* Set I2C standard mode */
01556     if (Status == VL53L0X_ERROR_NONE)
01557         Status = VL53L0X_WrByte(Dev, 0x88, 0x00);
01558 
01559     VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone, 0);
01560 
01561 #ifdef USE_IQC_STATION
01562     if (Status == VL53L0X_ERROR_NONE)
01563         Status = VL53L0X_apply_offset_adjustment(Dev);
01564 #endif
01565 
01566     /* Default value is 1000 for Linearity Corrective Gain */
01567     PALDevDataSet(Dev, LinearityCorrectiveGain, 1000);
01568 
01569     /* Dmax default Parameter */
01570     PALDevDataSet(Dev, DmaxCalRangeMilliMeter, 400);
01571     PALDevDataSet(Dev, DmaxCalSignalRateRtnMegaCps,
01572         (FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/
01573 
01574     /* Set Default static parameters
01575      *set first temporary values 9.44MHz * 65536 = 618660 */
01576     VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, OscFrequencyMHz, 618660);
01577 
01578     /* Set Default XTalkCompensationRateMegaCps to 0  */
01579     VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, 0);
01580 
01581     /* Get default parameters */
01582     Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters);
01583     if (Status == VL53L0X_ERROR_NONE) {
01584         /* initialize PAL values */
01585         CurrentParameters.DeviceMode  = VL53L0X_DEVICEMODE_SINGLE_RANGING;
01586         CurrentParameters.HistogramMode  = VL53L0X_HISTOGRAMMODE_DISABLED;
01587         PALDevDataSet(Dev, CurrentParameters, CurrentParameters);
01588     }
01589 
01590     /* Sigma estimator variable */
01591     PALDevDataSet(Dev, SigmaEstRefArray, 100);
01592     PALDevDataSet(Dev, SigmaEstEffPulseWidth, 900);
01593     PALDevDataSet(Dev, SigmaEstEffAmbWidth, 500);
01594     PALDevDataSet(Dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */
01595 
01596     /* Use internal default settings */
01597     PALDevDataSet(Dev, UseInternalTuningSettings, 1);
01598 
01599     Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
01600     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
01601     Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
01602     Status |= VL53L0X_RdByte(Dev, 0x91, &StopVariable);
01603     PALDevDataSet(Dev, StopVariable, StopVariable);
01604     Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
01605     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
01606     Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
01607 
01608     /* Enable all check */
01609     for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
01610         if (Status == VL53L0X_ERROR_NONE)
01611             Status |= VL53L0X_SetLimitCheckEnable(Dev, i, 1);
01612         else
01613             break;
01614 
01615     }
01616 
01617     /* Disable the following checks */
01618     if (Status == VL53L0X_ERROR_NONE)
01619         Status = VL53L0X_SetLimitCheckEnable(Dev,
01620             VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
01621 
01622     if (Status == VL53L0X_ERROR_NONE)
01623         Status = VL53L0X_SetLimitCheckEnable(Dev,
01624             VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
01625 
01626     if (Status == VL53L0X_ERROR_NONE)
01627         Status = VL53L0X_SetLimitCheckEnable(Dev,
01628             VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
01629 
01630     if (Status == VL53L0X_ERROR_NONE)
01631         Status = VL53L0X_SetLimitCheckEnable(Dev,
01632             VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
01633 
01634     /* Limit default values */
01635     if (Status == VL53L0X_ERROR_NONE) {
01636         Status = VL53L0X_SetLimitCheckValue(Dev,
01637             VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
01638                 (FixPoint1616_t)(18 * 65536));
01639     }
01640     if (Status == VL53L0X_ERROR_NONE) {
01641         Status = VL53L0X_SetLimitCheckValue(Dev,
01642             VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
01643                 (FixPoint1616_t)(25 * 65536 / 100));
01644                 /* 0.25 * 65536 */
01645     }
01646 
01647     if (Status == VL53L0X_ERROR_NONE) {
01648         Status = VL53L0X_SetLimitCheckValue(Dev,
01649             VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
01650                 (FixPoint1616_t)(35 * 65536));
01651     }
01652 
01653     if (Status == VL53L0X_ERROR_NONE) {
01654         Status = VL53L0X_SetLimitCheckValue(Dev,
01655             VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
01656                 (FixPoint1616_t)(0 * 65536));
01657     }
01658 
01659     if (Status == VL53L0X_ERROR_NONE) {
01660 
01661         PALDevDataSet(Dev, SequenceConfig, 0xFF);
01662         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01663             0xFF);
01664 
01665         /* Set PAL state to tell that we are waiting for call to
01666          * VL53L0X_StaticInit */
01667         PALDevDataSet(Dev, PalState, VL53L0X_STATE_WAIT_STATICINIT);
01668     }
01669 
01670     if (Status == VL53L0X_ERROR_NONE)
01671         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 0);
01672 
01673 
01674     LOG_FUNCTION_END(Status);
01675     return Status;
01676 }
01677 
01678 VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV Dev,
01679         uint8_t *Revision,
01680         VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
01681 {
01682     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01683     uint8_t ModuleIdInt;
01684     char *ProductId_tmp;
01685 
01686     LOG_FUNCTION_START("");
01687 
01688     Status = VL53L0X_get_info_from_device(Dev, 2);
01689 
01690     if (Status == VL53L0X_ERROR_NONE) {
01691         ModuleIdInt = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ModuleId);
01692 
01693     if (ModuleIdInt == 0) {
01694         *Revision = 0;
01695         VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId , "");
01696     } else {
01697         *Revision = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Revision);
01698         ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
01699             ProductId);
01700         VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId , ProductId_tmp);
01701     }
01702     }
01703 
01704     LOG_FUNCTION_END(Status);
01705     return Status;
01706 }
01707 
01708 VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV Dev,
01709                 VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
01710 {
01711     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01712     uint8_t revision_id;
01713     uint8_t Revision;
01714 
01715     Status = VL53L0X_check_part_used(Dev, &Revision, pVL53L0X_DeviceInfo);
01716 
01717     if (Status == VL53L0X_ERROR_NONE) {
01718         if (Revision == 0) {
01719             VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name ,
01720                     VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
01721         } else if ((Revision <= 34) && (Revision != 32)) {
01722             VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name ,
01723                     VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
01724         } else if (Revision < 39) {
01725             VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name ,
01726                     VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
01727         } else {
01728             VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name ,
01729                     VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
01730         }
01731 
01732         VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Type ,
01733                 VL53L0X_STRING_DEVICE_INFO_TYPE);
01734 
01735     }
01736 
01737     if (Status == VL53L0X_ERROR_NONE) {
01738         Status = VL53L0X_RdByte(Dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
01739                 &pVL53L0X_DeviceInfo->ProductType );
01740     }
01741 
01742     if (Status == VL53L0X_ERROR_NONE) {
01743         Status = VL53L0X_RdByte(Dev,
01744             VL53L0X_REG_IDENTIFICATION_REVISION_ID,
01745                 &revision_id);
01746         pVL53L0X_DeviceInfo->ProductRevisionMajor  = 1;
01747         pVL53L0X_DeviceInfo->ProductRevisionMinor  =
01748                     (revision_id & 0xF0) >> 4;
01749     }
01750 
01751     return Status;
01752 }
01753 
01754 VL53L0X_Error VL53L0X::VL53L0X_GetDeviceInfo(VL53L0X_DEV Dev,
01755     VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
01756 {
01757     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01758     LOG_FUNCTION_START("");
01759 
01760     Status = VL53L0X_get_device_info(Dev, pVL53L0X_DeviceInfo);
01761 
01762     LOG_FUNCTION_END(Status);
01763     return Status;
01764 }
01765 
01766 VL53L0X_Error VL53L0X::VL53L0X_GetInterruptMaskStatus(VL53L0X_DEV Dev,
01767     uint32_t *pInterruptMaskStatus)
01768 {
01769     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01770     uint8_t Byte;
01771     LOG_FUNCTION_START("");
01772 
01773     Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte);
01774     *pInterruptMaskStatus = Byte & 0x07;
01775 
01776     if (Byte & 0x18)
01777         Status = VL53L0X_ERROR_RANGE_ERROR;
01778 
01779     LOG_FUNCTION_END(Status);
01780     return Status;
01781 }
01782 
01783 VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementDataReady(VL53L0X_DEV Dev,
01784     uint8_t *pMeasurementDataReady)
01785 {
01786     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01787     uint8_t SysRangeStatusRegister;
01788     uint8_t InterruptConfig;
01789     uint32_t InterruptMask;
01790     LOG_FUNCTION_START("");
01791 
01792     InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
01793         Pin0GpioFunctionality);
01794 
01795     if (InterruptConfig ==
01796         VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
01797         Status = VL53L0X_GetInterruptMaskStatus(Dev, &InterruptMask);
01798         if (InterruptMask ==
01799         VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY)
01800             *pMeasurementDataReady = 1;
01801         else
01802             *pMeasurementDataReady = 0;
01803     } else {
01804         Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_RANGE_STATUS,
01805             &SysRangeStatusRegister);
01806         if (Status == VL53L0X_ERROR_NONE) {
01807             if (SysRangeStatusRegister & 0x01)
01808                 *pMeasurementDataReady = 1;
01809             else
01810                 *pMeasurementDataReady = 0;
01811         }
01812     }
01813 
01814     LOG_FUNCTION_END(Status);
01815     return Status;
01816 }
01817 
01818 VL53L0X_Error VL53L0X::VL53L0X_PollingDelay(VL53L0X_DEV Dev) {
01819     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01820 
01821     // do nothing
01822     VL53L0X_OsDelay();
01823     return status;
01824 }
01825 
01826 VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev)
01827 {
01828     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01829     uint8_t NewDataReady = 0;
01830     uint32_t LoopNb;
01831 
01832     LOG_FUNCTION_START("");
01833 
01834     LoopNb = 0;
01835 
01836     do {
01837         Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDataReady);
01838         if (Status != 0)
01839             break; /* the error is set */
01840 
01841         if (NewDataReady == 1)
01842             break; /* done note that status == 0 */
01843 
01844         LoopNb++;
01845         if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
01846             Status = VL53L0X_ERROR_TIME_OUT;
01847             break;
01848         }
01849 
01850         VL53L0X_PollingDelay(Dev);
01851     } while (1);
01852 
01853     LOG_FUNCTION_END(Status);
01854 
01855     return Status;
01856 }
01857 
01858 /* Group PAL Interrupt Functions */
01859 VL53L0X_Error VL53L0X::VL53L0X_ClearInterruptMask(VL53L0X_DEV Dev, uint32_t InterruptMask)
01860 {
01861     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01862     uint8_t LoopCount;
01863     uint8_t Byte;
01864     LOG_FUNCTION_START("");
01865 
01866     /* clear bit 0 range interrupt, bit 1 error interrupt */
01867     LoopCount = 0;
01868     do {
01869         Status = VL53L0X_WrByte(Dev,
01870             VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
01871         Status |= VL53L0X_WrByte(Dev,
01872             VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
01873         Status |= VL53L0X_RdByte(Dev,
01874             VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte);
01875         LoopCount++;
01876     } while (((Byte & 0x07) != 0x00)
01877             && (LoopCount < 3)
01878             && (Status == VL53L0X_ERROR_NONE));
01879 
01880 
01881     if (LoopCount >= 3)
01882         Status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
01883 
01884     LOG_FUNCTION_END(Status);
01885     return Status;
01886 }
01887 
01888 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(VL53L0X_DEV Dev,
01889         uint8_t vhv_init_byte)
01890 {
01891     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01892 
01893     if (Status == VL53L0X_ERROR_NONE)
01894         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
01895                 VL53L0X_REG_SYSRANGE_MODE_START_STOP |
01896                 vhv_init_byte);
01897 
01898     if (Status == VL53L0X_ERROR_NONE)
01899         Status = VL53L0X_measurement_poll_for_completion(Dev);
01900 
01901     if (Status == VL53L0X_ERROR_NONE)
01902         Status = VL53L0X_ClearInterruptMask(Dev, 0);
01903 
01904     if (Status == VL53L0X_ERROR_NONE)
01905         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x00);
01906 
01907     return Status;
01908 }
01909 
01910 VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(VL53L0X_DEV Dev, uint8_t read_not_write,
01911     uint8_t VhvSettings, uint8_t PhaseCal,
01912     uint8_t *pVhvSettings, uint8_t *pPhaseCal,
01913     const uint8_t vhv_enable, const uint8_t phase_enable)
01914 {
01915     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01916     uint8_t PhaseCalint = 0;
01917 
01918     /* Read VHV from device */
01919     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
01920     Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
01921     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
01922 
01923     if (read_not_write) {
01924         if (vhv_enable)
01925             Status |= VL53L0X_RdByte(Dev, 0xCB, pVhvSettings);
01926         if (phase_enable)
01927             Status |= VL53L0X_RdByte(Dev, 0xEE, &PhaseCalint);
01928     } else {
01929         if (vhv_enable)
01930             Status |= VL53L0X_WrByte(Dev, 0xCB, VhvSettings);
01931         if (phase_enable)
01932             Status |= VL53L0X_UpdateByte(Dev, 0xEE, 0x80, PhaseCal);
01933     }
01934 
01935     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
01936     Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
01937     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
01938 
01939     *pPhaseCal = (uint8_t)(PhaseCalint&0xEF);
01940 
01941     return Status;
01942 }
01943 
01944 VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev,
01945     uint8_t *pVhvSettings, const uint8_t get_data_enable,
01946     const uint8_t restore_config)
01947 {
01948     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01949     uint8_t SequenceConfig = 0;
01950     uint8_t VhvSettings = 0;
01951     uint8_t PhaseCal = 0;
01952     uint8_t PhaseCalInt = 0;
01953 
01954     /* store the value of the sequence config,
01955      * this will be reset before the end of the function
01956      */
01957 
01958     if (restore_config)
01959         SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
01960 
01961     /* Run VHV */
01962     Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
01963 
01964     if (Status == VL53L0X_ERROR_NONE)
01965         Status = VL53L0X_perform_single_ref_calibration(Dev, 0x40);
01966 
01967     /* Read VHV from device */
01968     if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
01969         Status = VL53L0X_ref_calibration_io(Dev, 1,
01970             VhvSettings, PhaseCal, /* Not used here */
01971             pVhvSettings, &PhaseCalInt,
01972             1, 0);
01973     } else
01974         *pVhvSettings = 0;
01975 
01976 
01977     if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
01978         /* restore the previous Sequence Config */
01979         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01980                 SequenceConfig);
01981         if (Status == VL53L0X_ERROR_NONE)
01982             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
01983 
01984     }
01985 
01986     return Status;
01987 }
01988 
01989 VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
01990     uint8_t *pPhaseCal, const uint8_t get_data_enable,
01991     const uint8_t restore_config)
01992 {
01993     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01994     uint8_t SequenceConfig = 0;
01995     uint8_t VhvSettings = 0;
01996     uint8_t PhaseCal = 0;
01997     uint8_t VhvSettingsint;
01998 
01999     /* store the value of the sequence config,
02000      * this will be reset before the end of the function
02001      */
02002 
02003     if (restore_config)
02004         SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
02005 
02006     /* Run PhaseCal */
02007     Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
02008 
02009     if (Status == VL53L0X_ERROR_NONE)
02010         Status = VL53L0X_perform_single_ref_calibration(Dev, 0x0);
02011 
02012     /* Read PhaseCal from device */
02013     if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
02014         Status = VL53L0X_ref_calibration_io(Dev, 1,
02015             VhvSettings, PhaseCal, /* Not used here */
02016             &VhvSettingsint, pPhaseCal,
02017             0, 1);
02018     } else
02019         *pPhaseCal = 0;
02020 
02021 
02022     if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
02023         /* restore the previous Sequence Config */
02024         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
02025                 SequenceConfig);
02026         if (Status == VL53L0X_ERROR_NONE)
02027             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
02028 
02029     }
02030 
02031     return Status;
02032 }
02033 
02034 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
02035     uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable)
02036 {
02037     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02038     uint8_t SequenceConfig = 0;
02039 
02040     /* store the value of the sequence config,
02041      * this will be reset before the end of the function
02042      */
02043 
02044     SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
02045 
02046     /* In the following function we don't save the config to optimize
02047      * writes on device. Config is saved and restored only once. */
02048     Status = VL53L0X_perform_vhv_calibration(
02049             Dev, pVhvSettings, get_data_enable, 0);
02050 
02051 
02052     if (Status == VL53L0X_ERROR_NONE)
02053         Status = VL53L0X_perform_phase_calibration(
02054             Dev, pPhaseCal, get_data_enable, 0);
02055 
02056 
02057     if (Status == VL53L0X_ERROR_NONE) {
02058         /* restore the previous Sequence Config */
02059         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
02060                 SequenceConfig);
02061         if (Status == VL53L0X_ERROR_NONE)
02062             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
02063 
02064     }
02065 
02066     return Status;
02067 }
02068 
02069 void VL53L0X::get_next_good_spad(uint8_t goodSpadArray[], uint32_t size,
02070             uint32_t curr, int32_t *next)
02071 {
02072     uint32_t startIndex;
02073     uint32_t fineOffset;
02074     uint32_t cSpadsPerByte = 8;
02075     uint32_t coarseIndex;
02076     uint32_t fineIndex;
02077     uint8_t dataByte;
02078     uint8_t success = 0;
02079 
02080     /*
02081      * Starting with the current good spad, loop through the array to find
02082      * the next. i.e. the next bit set in the sequence.
02083      *
02084      * The coarse index is the byte index of the array and the fine index is
02085      * the index of the bit within each byte.
02086      */
02087 
02088     *next = -1;
02089 
02090     startIndex = curr / cSpadsPerByte;
02091     fineOffset = curr % cSpadsPerByte;
02092 
02093     for (coarseIndex = startIndex; ((coarseIndex < size) && !success);
02094                 coarseIndex++) {
02095         fineIndex = 0;
02096         dataByte = goodSpadArray[coarseIndex];
02097 
02098         if (coarseIndex == startIndex) {
02099             /* locate the bit position of the provided current
02100              * spad bit before iterating */
02101             dataByte >>= fineOffset;
02102             fineIndex = fineOffset;
02103         }
02104 
02105         while (fineIndex < cSpadsPerByte) {
02106             if ((dataByte & 0x1) == 1) {
02107                 success = 1;
02108                 *next = coarseIndex * cSpadsPerByte + fineIndex;
02109                 break;
02110             }
02111             dataByte >>= 1;
02112             fineIndex++;
02113         }
02114     }
02115 }
02116 
02117 uint8_t VL53L0X::is_aperture(uint32_t spadIndex)
02118 {
02119     /*
02120      * This function reports if a given spad index is an aperture SPAD by
02121      * deriving the quadrant.
02122      */
02123     uint32_t quadrant;
02124     uint8_t isAperture = 1;
02125     quadrant = spadIndex >> 6;
02126     if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0)
02127         isAperture = 0;
02128 
02129     return isAperture;
02130 }
02131 
02132 VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spadArray[], uint32_t size,
02133     uint32_t spadIndex)
02134 {
02135     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02136     uint32_t cSpadsPerByte = 8;
02137     uint32_t coarseIndex;
02138     uint32_t fineIndex;
02139 
02140     coarseIndex = spadIndex / cSpadsPerByte;
02141     fineIndex = spadIndex % cSpadsPerByte;
02142     if (coarseIndex >= size)
02143         status = VL53L0X_ERROR_REF_SPAD_INIT;
02144     else
02145         spadArray[coarseIndex] |= (1 << fineIndex);
02146 
02147     return status;
02148 }
02149 
02150 VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
02151 {
02152     VL53L0X_Error status = VL53L0X_WriteMulti(Dev,
02153                 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
02154                 refSpadArray, 6);
02155 
02156     return status;
02157 }
02158 
02159 VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
02160 {
02161     VL53L0X_Error status = VL53L0X_ReadMulti(Dev,
02162                 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
02163                 refSpadArray,
02164                 6);
02165 //  VL53L0X_Error status = VL53L0X_ERROR_NONE;
02166 //  uint8_t count=0;
02167 
02168 //  for (count = 0; count < 6; count++)
02169 //        status = VL53L0X_RdByte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]);
02170     return status;
02171 }
02172 
02173 VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV Dev,
02174                 uint8_t apertureSpads,
02175                 uint8_t goodSpadArray[],
02176                 uint8_t spadArray[],
02177                 uint32_t size,
02178                 uint32_t start,
02179                 uint32_t offset,
02180                 uint32_t spadCount,
02181                 uint32_t *lastSpad)
02182 {
02183     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02184     uint32_t index;
02185     uint32_t i;
02186     int32_t nextGoodSpad = offset;
02187     uint32_t currentSpad;
02188     uint8_t checkSpadArray[6];
02189 
02190     /*
02191      * This function takes in a spad array which may or may not have SPADS
02192      * already enabled and appends from a given offset a requested number
02193      * of new SPAD enables. The 'good spad map' is applied to
02194      * determine the next SPADs to enable.
02195      *
02196      * This function applies to only aperture or only non-aperture spads.
02197      * Checks are performed to ensure this.
02198      */
02199 
02200     currentSpad = offset;
02201     for (index = 0; index < spadCount; index++) {
02202         get_next_good_spad(goodSpadArray, size, currentSpad,
02203             &nextGoodSpad);
02204 
02205         if (nextGoodSpad == -1) {
02206             status = VL53L0X_ERROR_REF_SPAD_INIT;
02207             break;
02208         }
02209 
02210         /* Confirm that the next good SPAD is non-aperture */
02211         if (is_aperture(start + nextGoodSpad) != apertureSpads) {
02212             /* if we can't get the required number of good aperture
02213              * spads from the current quadrant then this is an error
02214              */
02215             status = VL53L0X_ERROR_REF_SPAD_INIT;
02216             break;
02217         }
02218         currentSpad = (uint32_t)nextGoodSpad;
02219         enable_spad_bit(spadArray, size, currentSpad);
02220         currentSpad++;
02221     }
02222     *lastSpad = currentSpad;
02223 
02224     if (status == VL53L0X_ERROR_NONE)
02225         status = set_ref_spad_map(Dev, spadArray);
02226 
02227 
02228     if (status == VL53L0X_ERROR_NONE) {
02229         status = get_ref_spad_map(Dev, checkSpadArray);
02230 
02231         i = 0;
02232 
02233         /* Compare spad maps. If not equal report error. */
02234         while (i < size) {
02235             if (spadArray[i] != checkSpadArray[i]) {
02236                 status = VL53L0X_ERROR_REF_SPAD_INIT;
02237                 break;
02238             }
02239             i++;
02240         }
02241     }
02242     return status;
02243 }
02244 
02245 VL53L0X_Error VL53L0X::VL53L0X_SetDeviceMode(VL53L0X_DEV Dev, VL53L0X_DeviceModes DeviceMode)
02246 {
02247     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02248 
02249     LOG_FUNCTION_START("%d", (int)DeviceMode);
02250 
02251     switch (DeviceMode) {
02252     case VL53L0X_DEVICEMODE_SINGLE_RANGING:
02253     case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
02254     case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
02255     case VL53L0X_DEVICEMODE_GPIO_DRIVE:
02256     case VL53L0X_DEVICEMODE_GPIO_OSC:
02257         /* Supported modes */
02258         VL53L0X_SETPARAMETERFIELD(Dev, DeviceMode, DeviceMode);
02259         break;
02260     default:
02261         /* Unsupported mode */
02262         Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
02263     }
02264 
02265     LOG_FUNCTION_END(Status);
02266     return Status;
02267 }
02268 
02269 VL53L0X_Error VL53L0X::VL53L0X_GetInterruptThresholds(VL53L0X_DEV Dev,
02270     VL53L0X_DeviceModes DeviceMode, FixPoint1616_t *pThresholdLow,
02271     FixPoint1616_t *pThresholdHigh)
02272 {
02273     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02274     uint16_t Threshold16;
02275     LOG_FUNCTION_START("");
02276 
02277     /* no dependency on DeviceMode for Ewok */
02278 
02279     Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &Threshold16);
02280     /* Need to multiply by 2 because the FW will apply a x2 */
02281     *pThresholdLow = (FixPoint1616_t)((0x00fff & Threshold16) << 17);
02282 
02283     if (Status == VL53L0X_ERROR_NONE) {
02284         Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
02285             &Threshold16);
02286         /* Need to multiply by 2 because the FW will apply a x2 */
02287         *pThresholdHigh =
02288             (FixPoint1616_t)((0x00fff & Threshold16) << 17);
02289     }
02290 
02291     LOG_FUNCTION_END(Status);
02292     return Status;
02293 }
02294 
02295 VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
02296         uint8_t *pTuningSettingBuffer)
02297 {
02298     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02299     int i;
02300     int Index;
02301     uint8_t msb;
02302     uint8_t lsb;
02303     uint8_t SelectParam;
02304     uint8_t NumberOfWrites;
02305     uint8_t Address;
02306     uint8_t localBuffer[4]; /* max */
02307     uint16_t Temp16;
02308 
02309     LOG_FUNCTION_START("");
02310 
02311     Index = 0;
02312 
02313     while ((*(pTuningSettingBuffer + Index) != 0) &&
02314             (Status == VL53L0X_ERROR_NONE)) {
02315         NumberOfWrites = *(pTuningSettingBuffer + Index);
02316         Index++;
02317         if (NumberOfWrites == 0xFF) {
02318             /* internal parameters */
02319             SelectParam = *(pTuningSettingBuffer + Index);
02320             Index++;
02321             switch (SelectParam) {
02322             case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
02323                 msb = *(pTuningSettingBuffer + Index);
02324                 Index++;
02325                 lsb = *(pTuningSettingBuffer + Index);
02326                 Index++;
02327                 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
02328                 PALDevDataSet(Dev, SigmaEstRefArray, Temp16);
02329                 break;
02330             case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
02331                 msb = *(pTuningSettingBuffer + Index);
02332                 Index++;
02333                 lsb = *(pTuningSettingBuffer + Index);
02334                 Index++;
02335                 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
02336                 PALDevDataSet(Dev, SigmaEstEffPulseWidth,
02337                     Temp16);
02338                 break;
02339             case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
02340                 msb = *(pTuningSettingBuffer + Index);
02341                 Index++;
02342                 lsb = *(pTuningSettingBuffer + Index);
02343                 Index++;
02344                 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
02345                 PALDevDataSet(Dev, SigmaEstEffAmbWidth, Temp16);
02346                 break;
02347             case 3: /* uint16_t targetRefRate -> 2 bytes */
02348                 msb = *(pTuningSettingBuffer + Index);
02349                 Index++;
02350                 lsb = *(pTuningSettingBuffer + Index);
02351                 Index++;
02352                 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
02353                 PALDevDataSet(Dev, targetRefRate, Temp16);
02354                 break;
02355             default: /* invalid parameter */
02356                 Status = VL53L0X_ERROR_INVALID_PARAMS;
02357             }
02358 
02359         } else if (NumberOfWrites <= 4) {
02360             Address = *(pTuningSettingBuffer + Index);
02361             Index++;
02362 
02363             for (i = 0; i < NumberOfWrites; i++) {
02364                 localBuffer[i] = *(pTuningSettingBuffer +
02365                             Index);
02366                 Index++;
02367             }
02368 
02369             Status = VL53L0X_WriteMulti(Dev, Address, localBuffer,
02370                     NumberOfWrites);
02371 
02372         } else {
02373             Status = VL53L0X_ERROR_INVALID_PARAMS;
02374         }
02375     }
02376 
02377     LOG_FUNCTION_END(Status);
02378     return Status;
02379 }
02380 
02381 VL53L0X_Error VL53L0X::VL53L0X_CheckAndLoadInterruptSettings(VL53L0X_DEV Dev,
02382     uint8_t StartNotStopFlag)
02383 {
02384     uint8_t InterruptConfig;
02385     FixPoint1616_t ThresholdLow;
02386     FixPoint1616_t ThresholdHigh;
02387     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02388 
02389     InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
02390         Pin0GpioFunctionality);
02391 
02392     if ((InterruptConfig ==
02393         VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) ||
02394         (InterruptConfig ==
02395         VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) ||
02396         (InterruptConfig ==
02397         VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
02398 
02399         Status = VL53L0X_GetInterruptThresholds(Dev,
02400             VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
02401             &ThresholdLow, &ThresholdHigh);
02402 
02403         if (((ThresholdLow > 255*65536) ||
02404             (ThresholdHigh > 255*65536)) &&
02405             (Status == VL53L0X_ERROR_NONE)) {
02406 
02407             if (StartNotStopFlag != 0) {
02408                 Status = VL53L0X_load_tuning_settings(Dev,
02409                     InterruptThresholdSettings);
02410             } else {
02411                 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x04);
02412                 Status |= VL53L0X_WrByte(Dev, 0x70, 0x00);
02413                 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
02414                 Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
02415             }
02416 
02417         }
02418 
02419 
02420     }
02421 
02422     return Status;
02423 
02424 }
02425 
02426 VL53L0X_Error VL53L0X::VL53L0X_StartMeasurement(VL53L0X_DEV Dev)
02427 {
02428     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02429     VL53L0X_DeviceModes DeviceMode;
02430     uint8_t Byte;
02431     uint8_t StartStopByte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
02432     uint32_t LoopNb;
02433     LOG_FUNCTION_START("");
02434 
02435     /* Get Current DeviceMode */
02436     VL53L0X_GetDeviceMode(Dev, &DeviceMode);
02437 
02438     Status = VL53L0X_WrByte(Dev, 0x80, 0x01);
02439     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
02440     Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
02441     Status = VL53L0X_WrByte(Dev, 0x91, PALDevDataGet(Dev, StopVariable));
02442     Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
02443     Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
02444     Status = VL53L0X_WrByte(Dev, 0x80, 0x00);
02445 
02446     switch (DeviceMode) {
02447     case VL53L0X_DEVICEMODE_SINGLE_RANGING:
02448         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x01);
02449 
02450         Byte = StartStopByte;
02451         if (Status == VL53L0X_ERROR_NONE) {
02452             /* Wait until start bit has been cleared */
02453             LoopNb = 0;
02454             do {
02455                 if (LoopNb > 0)
02456                     Status = VL53L0X_RdByte(Dev,
02457                     VL53L0X_REG_SYSRANGE_START, &Byte);
02458                 LoopNb = LoopNb + 1;
02459             } while (((Byte & StartStopByte) == StartStopByte)
02460                 && (Status == VL53L0X_ERROR_NONE)
02461                 && (LoopNb < VL53L0X_DEFAULT_MAX_LOOP));
02462 
02463             if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
02464                 Status = VL53L0X_ERROR_TIME_OUT;
02465 
02466         }
02467 
02468         break;
02469     case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
02470         /* Back-to-back mode */
02471 
02472         /* Check if need to apply interrupt settings */
02473         if (Status == VL53L0X_ERROR_NONE)
02474             Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);
02475 
02476         Status = VL53L0X_WrByte(Dev,
02477         VL53L0X_REG_SYSRANGE_START,
02478         VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
02479         if (Status == VL53L0X_ERROR_NONE) {
02480             /* Set PAL State to Running */
02481             PALDevDataSet(Dev, PalState, VL53L0X_STATE_RUNNING);
02482         }
02483         break;
02484     case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
02485         /* Continuous mode */
02486         /* Check if need to apply interrupt settings */
02487         if (Status == VL53L0X_ERROR_NONE)
02488             Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);
02489 
02490         Status = VL53L0X_WrByte(Dev,
02491         VL53L0X_REG_SYSRANGE_START,
02492         VL53L0X_REG_SYSRANGE_MODE_TIMED);
02493 
02494         if (Status == VL53L0X_ERROR_NONE) {
02495             /* Set PAL State to Running */
02496             PALDevDataSet(Dev, PalState, VL53L0X_STATE_RUNNING);
02497         }
02498         break;
02499     default:
02500         /* Selected mode not supported */
02501         Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
02502     }
02503 
02504 
02505     LOG_FUNCTION_END(Status);
02506     return Status;
02507 }
02508 
02509 /* Group PAL Measurement Functions */
02510 VL53L0X_Error VL53L0X::VL53L0X_PerformSingleMeasurement(VL53L0X_DEV Dev)
02511 {
02512     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02513     VL53L0X_DeviceModes DeviceMode;
02514 
02515     LOG_FUNCTION_START("");
02516 
02517     /* Get Current DeviceMode */
02518     Status = VL53L0X_GetDeviceMode(Dev, &DeviceMode);
02519 
02520     /* Start immediately to run a single ranging measurement in case of
02521      * single ranging or single histogram */
02522     if (Status == VL53L0X_ERROR_NONE
02523         && DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
02524         Status = VL53L0X_StartMeasurement(Dev);
02525 
02526 
02527     if (Status == VL53L0X_ERROR_NONE)
02528         Status = VL53L0X_measurement_poll_for_completion(Dev);
02529 
02530 
02531     /* Change PAL State in case of single ranging or single histogram */
02532     if (Status == VL53L0X_ERROR_NONE
02533         && DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
02534         PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
02535 
02536 
02537     LOG_FUNCTION_END(Status);
02538     return Status;
02539 }
02540 
02541 VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationEnable(VL53L0X_DEV Dev,
02542     uint8_t *pXTalkCompensationEnable)
02543 {
02544     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02545     uint8_t Temp8;
02546     LOG_FUNCTION_START("");
02547 
02548     VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable, Temp8);
02549     *pXTalkCompensationEnable = Temp8;
02550 
02551     LOG_FUNCTION_END(Status);
02552     return Status;
02553 }
02554 
02555 VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
02556     VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
02557     FixPoint1616_t *ptotal_xtalk_rate_mcps)
02558 {
02559     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02560 
02561     uint8_t xtalkCompEnable;
02562     FixPoint1616_t totalXtalkMegaCps;
02563     FixPoint1616_t xtalkPerSpadMegaCps;
02564 
02565     *ptotal_xtalk_rate_mcps = 0;
02566 
02567     Status = VL53L0X_GetXTalkCompensationEnable(Dev, &xtalkCompEnable);
02568     if (Status == VL53L0X_ERROR_NONE) {
02569 
02570         if (xtalkCompEnable) {
02571 
02572             VL53L0X_GETPARAMETERFIELD(
02573                 Dev,
02574                 XTalkCompensationRateMegaCps,
02575                 xtalkPerSpadMegaCps);
02576 
02577             /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
02578             totalXtalkMegaCps =
02579                 pRangingMeasurementData->EffectiveSpadRtnCount *
02580                 xtalkPerSpadMegaCps;
02581 
02582             /* FixPoint0824 >> 8 = FixPoint1616 */
02583             *ptotal_xtalk_rate_mcps =
02584                 (totalXtalkMegaCps + 0x80) >> 8;
02585         }
02586     }
02587 
02588     return Status;
02589 }
02590 
02591 VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
02592     VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
02593     FixPoint1616_t *ptotal_signal_rate_mcps)
02594 {
02595     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02596     FixPoint1616_t totalXtalkMegaCps;
02597 
02598     LOG_FUNCTION_START("");
02599 
02600     *ptotal_signal_rate_mcps =
02601         pRangingMeasurementData->SignalRateRtnMegaCps;
02602 
02603     Status = VL53L0X_get_total_xtalk_rate(
02604         Dev, pRangingMeasurementData, &totalXtalkMegaCps);
02605 
02606     if (Status == VL53L0X_ERROR_NONE)
02607         *ptotal_signal_rate_mcps += totalXtalkMegaCps;
02608 
02609     return Status;
02610 }
02611 
02612 /* To convert ms into register value */
02613 uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
02614         uint32_t timeout_period_us,
02615         uint8_t vcsel_period_pclks)
02616 {
02617     uint32_t macro_period_ps;
02618     uint32_t macro_period_ns;
02619     uint32_t timeout_period_mclks = 0;
02620 
02621     macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
02622     macro_period_ns = (macro_period_ps + 500) / 1000;
02623 
02624     timeout_period_mclks =
02625         (uint32_t) (((timeout_period_us * 1000)
02626         + (macro_period_ns / 2)) / macro_period_ns);
02627 
02628     return timeout_period_mclks;
02629 }
02630 
02631 uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
02632 {
02633     /*
02634      * Implements an integer square root
02635      *
02636      * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
02637      */
02638 
02639     uint32_t  res = 0;
02640     uint32_t  bit = 1 << 30;
02641     /* The second-to-top bit is set:
02642      *  1 << 14 for 16-bits, 1 << 30 for 32 bits */
02643 
02644      /* "bit" starts at the highest power of four <= the argument. */
02645     while (bit > num)
02646         bit >>= 2;
02647 
02648 
02649     while (bit != 0) {
02650         if (num >= res + bit) {
02651             num -= res + bit;
02652             res = (res >> 1) + bit;
02653         } else
02654             res >>= 1;
02655 
02656         bit >>= 2;
02657     }
02658 
02659     return res;
02660 }
02661 
02662 VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(
02663     VL53L0X_DEV Dev,
02664     FixPoint1616_t totalSignalRate_mcps,
02665     FixPoint1616_t totalCorrSignalRate_mcps,
02666     FixPoint1616_t pwMult,
02667     uint32_t sigmaEstimateP1,
02668     FixPoint1616_t sigmaEstimateP2,
02669     uint32_t peakVcselDuration_us,
02670     uint32_t *pdmax_mm)
02671 {
02672     const uint32_t cSigmaLimit      = 18;
02673     const FixPoint1616_t cSignalLimit   = 0x4000; /* 0.25 */
02674     const FixPoint1616_t cSigmaEstRef   = 0x00000042; /* 0.001 */
02675     const uint32_t cAmbEffWidthSigmaEst_ns = 6;
02676     const uint32_t cAmbEffWidthDMax_ns     = 7;
02677     uint32_t dmaxCalRange_mm;
02678     FixPoint1616_t dmaxCalSignalRateRtn_mcps;
02679     FixPoint1616_t minSignalNeeded;
02680     FixPoint1616_t minSignalNeeded_p1;
02681     FixPoint1616_t minSignalNeeded_p2;
02682     FixPoint1616_t minSignalNeeded_p3;
02683     FixPoint1616_t minSignalNeeded_p4;
02684     FixPoint1616_t sigmaLimitTmp;
02685     FixPoint1616_t sigmaEstSqTmp;
02686     FixPoint1616_t signalLimitTmp;
02687     FixPoint1616_t SignalAt0mm;
02688     FixPoint1616_t dmaxDark;
02689     FixPoint1616_t dmaxAmbient;
02690     FixPoint1616_t dmaxDarkTmp;
02691     FixPoint1616_t sigmaEstP2Tmp;
02692     uint32_t signalRateTemp_mcps;
02693 
02694     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02695 
02696     LOG_FUNCTION_START("");
02697 
02698     dmaxCalRange_mm =
02699         PALDevDataGet(Dev, DmaxCalRangeMilliMeter);
02700 
02701     dmaxCalSignalRateRtn_mcps =
02702         PALDevDataGet(Dev, DmaxCalSignalRateRtnMegaCps);
02703 
02704     /* uint32 * FixPoint1616 = FixPoint1616 */
02705     SignalAt0mm = dmaxCalRange_mm * dmaxCalSignalRateRtn_mcps;
02706 
02707     /* FixPoint1616 >> 8 = FixPoint2408 */
02708     SignalAt0mm = (SignalAt0mm + 0x80) >> 8;
02709     SignalAt0mm *= dmaxCalRange_mm;
02710 
02711     minSignalNeeded_p1 = 0;
02712     if (totalCorrSignalRate_mcps > 0) {
02713 
02714         /* Shift by 10 bits to increase resolution prior to the
02715          * division */
02716         signalRateTemp_mcps = totalSignalRate_mcps << 10;
02717 
02718         /* Add rounding value prior to division */
02719         minSignalNeeded_p1 = signalRateTemp_mcps +
02720             (totalCorrSignalRate_mcps/2);
02721 
02722         /* FixPoint0626/FixPoint1616 = FixPoint2210 */
02723         minSignalNeeded_p1 /= totalCorrSignalRate_mcps;
02724 
02725         /* Apply a factored version of the speed of light.
02726          Correction to be applied at the end */
02727         minSignalNeeded_p1 *= 3;
02728 
02729         /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
02730         minSignalNeeded_p1 *= minSignalNeeded_p1;
02731 
02732         /* FixPoint1220 >> 16 = FixPoint2804 */
02733         minSignalNeeded_p1 = (minSignalNeeded_p1 + 0x8000) >> 16;
02734     }
02735 
02736     minSignalNeeded_p2 = pwMult * sigmaEstimateP1;
02737 
02738     /* FixPoint1616 >> 16 =  uint32 */
02739     minSignalNeeded_p2 = (minSignalNeeded_p2 + 0x8000) >> 16;
02740 
02741     /* uint32 * uint32  =  uint32 */
02742     minSignalNeeded_p2 *= minSignalNeeded_p2;
02743 
02744     /* Check sigmaEstimateP2
02745      * If this value is too high there is not enough signal rate
02746      * to calculate dmax value so set a suitable value to ensure
02747      * a very small dmax.
02748      */
02749     sigmaEstP2Tmp = (sigmaEstimateP2 + 0x8000) >> 16;
02750     sigmaEstP2Tmp = (sigmaEstP2Tmp + cAmbEffWidthSigmaEst_ns/2)/
02751         cAmbEffWidthSigmaEst_ns;
02752     sigmaEstP2Tmp *= cAmbEffWidthDMax_ns;
02753 
02754     if (sigmaEstP2Tmp > 0xffff) {
02755         minSignalNeeded_p3 = 0xfff00000;
02756     } else {
02757 
02758         /* DMAX uses a different ambient width from sigma, so apply
02759          * correction.
02760          * Perform division before multiplication to prevent overflow.
02761          */
02762         sigmaEstimateP2 = (sigmaEstimateP2 + cAmbEffWidthSigmaEst_ns/2)/
02763             cAmbEffWidthSigmaEst_ns;
02764         sigmaEstimateP2 *= cAmbEffWidthDMax_ns;
02765 
02766         /* FixPoint1616 >> 16 = uint32 */
02767         minSignalNeeded_p3 = (sigmaEstimateP2 + 0x8000) >> 16;
02768 
02769         minSignalNeeded_p3 *= minSignalNeeded_p3;
02770 
02771     }
02772 
02773     /* FixPoint1814 / uint32 = FixPoint1814 */
02774     sigmaLimitTmp = ((cSigmaLimit << 14) + 500) / 1000;
02775 
02776     /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
02777     sigmaLimitTmp *= sigmaLimitTmp;
02778 
02779     /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
02780     sigmaEstSqTmp = cSigmaEstRef * cSigmaEstRef;
02781 
02782     /* FixPoint3232 >> 4 = FixPoint0428 */
02783     sigmaEstSqTmp = (sigmaEstSqTmp + 0x08) >> 4;
02784 
02785     /* FixPoint0428 - FixPoint0428  = FixPoint0428 */
02786     sigmaLimitTmp -=  sigmaEstSqTmp;
02787 
02788     /* uint32_t * FixPoint0428 = FixPoint0428 */
02789     minSignalNeeded_p4 = 4 * 12 * sigmaLimitTmp;
02790 
02791     /* FixPoint0428 >> 14 = FixPoint1814 */
02792     minSignalNeeded_p4 = (minSignalNeeded_p4 + 0x2000) >> 14;
02793 
02794     /* uint32 + uint32 = uint32 */
02795     minSignalNeeded = (minSignalNeeded_p2 + minSignalNeeded_p3);
02796 
02797     /* uint32 / uint32 = uint32 */
02798     minSignalNeeded += (peakVcselDuration_us/2);
02799     minSignalNeeded /= peakVcselDuration_us;
02800 
02801     /* uint32 << 14 = FixPoint1814 */
02802     minSignalNeeded <<= 14;
02803 
02804     /* FixPoint1814 / FixPoint1814 = uint32 */
02805     minSignalNeeded += (minSignalNeeded_p4/2);
02806     minSignalNeeded /= minSignalNeeded_p4;
02807 
02808     /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
02809     minSignalNeeded *= minSignalNeeded_p1;
02810 
02811     /* Apply correction by dividing by 1000000.
02812      * This assumes 10E16 on the numerator of the equation
02813      * and 10E-22 on the denominator.
02814      * We do this because 32bit fix point calculation can't
02815      * handle the larger and smaller elements of this equation,
02816      * i.e. speed of light and pulse widths.
02817      */
02818     minSignalNeeded = (minSignalNeeded + 500) / 1000;
02819     minSignalNeeded <<= 4;
02820 
02821     minSignalNeeded = (minSignalNeeded + 500) / 1000;
02822 
02823     /* FixPoint1616 >> 8 = FixPoint2408 */
02824     signalLimitTmp = (cSignalLimit + 0x80) >> 8;
02825 
02826     /* FixPoint2408/FixPoint2408 = uint32 */
02827     if (signalLimitTmp != 0)
02828         dmaxDarkTmp = (SignalAt0mm + (signalLimitTmp / 2))
02829             / signalLimitTmp;
02830     else
02831         dmaxDarkTmp = 0;
02832 
02833     dmaxDark = VL53L0X_isqrt(dmaxDarkTmp);
02834 
02835     /* FixPoint2408/FixPoint2408 = uint32 */
02836     if (minSignalNeeded != 0)
02837         dmaxAmbient = (SignalAt0mm + minSignalNeeded/2)
02838             / minSignalNeeded;
02839     else
02840         dmaxAmbient = 0;
02841 
02842     dmaxAmbient = VL53L0X_isqrt(dmaxAmbient);
02843 
02844     *pdmax_mm = dmaxDark;
02845     if (dmaxDark > dmaxAmbient)
02846         *pdmax_mm = dmaxAmbient;
02847 
02848     LOG_FUNCTION_END(Status);
02849 
02850     return Status;
02851 }
02852 
02853 VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
02854     VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
02855     FixPoint1616_t *pSigmaEstimate,
02856     uint32_t *pDmax_mm)
02857 {
02858     /* Expressed in 100ths of a ns, i.e. centi-ns */
02859     const uint32_t cPulseEffectiveWidth_centi_ns   = 800;
02860     /* Expressed in 100ths of a ns, i.e. centi-ns */
02861     const uint32_t cAmbientEffectiveWidth_centi_ns = 600;
02862     const FixPoint1616_t cDfltFinalRangeIntegrationTimeMilliSecs    = 0x00190000; /* 25ms */
02863     const uint32_t cVcselPulseWidth_ps  = 4700; /* pico secs */
02864     const FixPoint1616_t cSigmaEstMax   = 0x028F87AE;
02865     const FixPoint1616_t cSigmaEstRtnMax    = 0xF000;
02866     const FixPoint1616_t cAmbToSignalRatioMax = 0xF0000000/
02867         cAmbientEffectiveWidth_centi_ns;
02868     /* Time Of Flight per mm (6.6 pico secs) */
02869     const FixPoint1616_t cTOF_per_mm_ps     = 0x0006999A;
02870     const uint32_t c16BitRoundingParam      = 0x00008000;
02871     const FixPoint1616_t cMaxXTalk_kcps     = 0x00320000;
02872     const uint32_t cPllPeriod_ps            = 1655;
02873 
02874     uint32_t vcselTotalEventsRtn;
02875     uint32_t finalRangeTimeoutMicroSecs;
02876     uint32_t preRangeTimeoutMicroSecs;
02877     uint32_t finalRangeIntegrationTimeMilliSecs;
02878     FixPoint1616_t sigmaEstimateP1;
02879     FixPoint1616_t sigmaEstimateP2;
02880     FixPoint1616_t sigmaEstimateP3;
02881     FixPoint1616_t deltaT_ps;
02882     FixPoint1616_t pwMult;
02883     FixPoint1616_t sigmaEstRtn;
02884     FixPoint1616_t sigmaEstimate;
02885     FixPoint1616_t xTalkCorrection;
02886     FixPoint1616_t ambientRate_kcps;
02887     FixPoint1616_t peakSignalRate_kcps;
02888     FixPoint1616_t xTalkCompRate_mcps;
02889     uint32_t xTalkCompRate_kcps;
02890     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
02891     FixPoint1616_t diff1_mcps;
02892     FixPoint1616_t diff2_mcps;
02893     FixPoint1616_t sqr1;
02894     FixPoint1616_t sqr2;
02895     FixPoint1616_t sqrSum;
02896     FixPoint1616_t sqrtResult_centi_ns;
02897     FixPoint1616_t sqrtResult;
02898     FixPoint1616_t totalSignalRate_mcps;
02899     FixPoint1616_t correctedSignalRate_mcps;
02900     FixPoint1616_t sigmaEstRef;
02901     uint32_t vcselWidth;
02902     uint32_t finalRangeMacroPCLKS;
02903     uint32_t preRangeMacroPCLKS;
02904     uint32_t peakVcselDuration_us;
02905     uint8_t finalRangeVcselPCLKS;
02906     uint8_t preRangeVcselPCLKS;
02907     /*! \addtogroup calc_sigma_estimate
02908      * @{
02909      *
02910      * Estimates the range sigma
02911      */
02912 
02913     LOG_FUNCTION_START("");
02914 
02915     VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
02916             xTalkCompRate_mcps);
02917 
02918     /*
02919      * We work in kcps rather than mcps as this helps keep within the
02920      * confines of the 32 Fix1616 type.
02921      */
02922 
02923     ambientRate_kcps =
02924         (pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16;
02925 
02926     correctedSignalRate_mcps =
02927         pRangingMeasurementData->SignalRateRtnMegaCps;
02928 
02929 
02930     Status = VL53L0X_get_total_signal_rate(
02931         Dev, pRangingMeasurementData, &totalSignalRate_mcps);
02932     Status = VL53L0X_get_total_xtalk_rate(
02933         Dev, pRangingMeasurementData, &xTalkCompRate_mcps);
02934 
02935 
02936     /* Signal rate measurement provided by device is the
02937      * peak signal rate, not average.
02938      */
02939     peakSignalRate_kcps = (totalSignalRate_mcps * 1000);
02940     peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16;
02941 
02942     xTalkCompRate_kcps = xTalkCompRate_mcps * 1000;
02943 
02944     if (xTalkCompRate_kcps > cMaxXTalk_kcps)
02945         xTalkCompRate_kcps = cMaxXTalk_kcps;
02946 
02947     if (Status == VL53L0X_ERROR_NONE) {
02948 
02949         /* Calculate final range macro periods */
02950         finalRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
02951             Dev, FinalRangeTimeoutMicroSecs);
02952 
02953         finalRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
02954             Dev, FinalRangeVcselPulsePeriod);
02955 
02956         finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
02957             Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS);
02958 
02959         /* Calculate pre-range macro periods */
02960         preRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
02961             Dev, PreRangeTimeoutMicroSecs);
02962 
02963         preRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
02964             Dev, PreRangeVcselPulsePeriod);
02965 
02966         preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
02967             Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS);
02968 
02969         vcselWidth = 3;
02970         if (finalRangeVcselPCLKS == 8)
02971             vcselWidth = 2;
02972 
02973 
02974         peakVcselDuration_us = vcselWidth * 2048 *
02975             (preRangeMacroPCLKS + finalRangeMacroPCLKS);
02976         peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
02977         peakVcselDuration_us *= cPllPeriod_ps;
02978         peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
02979 
02980         /* Fix1616 >> 8 = Fix2408 */
02981         totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8;
02982 
02983         /* Fix2408 * uint32 = Fix2408 */
02984         vcselTotalEventsRtn = totalSignalRate_mcps *
02985             peakVcselDuration_us;
02986 
02987         /* Fix2408 >> 8 = uint32 */
02988         vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8;
02989 
02990         /* Fix2408 << 8 = Fix1616 = */
02991         totalSignalRate_mcps <<= 8;
02992     }
02993 
02994     if (Status != VL53L0X_ERROR_NONE) {
02995         LOG_FUNCTION_END(Status);
02996         return Status;
02997     }
02998 
02999     if (peakSignalRate_kcps == 0) {
03000         *pSigmaEstimate = cSigmaEstMax;
03001         PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax);
03002         *pDmax_mm = 0;
03003     } else {
03004         if (vcselTotalEventsRtn < 1)
03005             vcselTotalEventsRtn = 1;
03006 
03007         sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns;
03008 
03009         /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
03010         sigmaEstimateP2 = (ambientRate_kcps << 16)/peakSignalRate_kcps;
03011         if (sigmaEstimateP2 > cAmbToSignalRatioMax) {
03012             /* Clip to prevent overflow. Will ensure safe
03013              * max result. */
03014             sigmaEstimateP2 = cAmbToSignalRatioMax;
03015         }
03016         sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns;
03017 
03018         sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12);
03019 
03020         /* uint32 * FixPoint1616 = FixPoint1616 */
03021         deltaT_ps = pRangingMeasurementData->RangeMilliMeter *
03022                     cTOF_per_mm_ps;
03023 
03024         /*
03025          * vcselRate - xtalkCompRate
03026          * (uint32 << 16) - FixPoint1616 = FixPoint1616.
03027          * Divide result by 1000 to convert to mcps.
03028          * 500 is added to ensure rounding when integer division
03029          * truncates.
03030          */
03031         diff1_mcps = (((peakSignalRate_kcps << 16) -
03032             2 * xTalkCompRate_kcps) + 500)/1000;
03033 
03034         /* vcselRate + xtalkCompRate */
03035         diff2_mcps = ((peakSignalRate_kcps << 16) + 500)/1000;
03036 
03037         /* Shift by 8 bits to increase resolution prior to the
03038          * division */
03039         diff1_mcps <<= 8;
03040 
03041         /* FixPoint0824/FixPoint1616 = FixPoint2408 */
03042 //      xTalkCorrection  = abs(diff1_mcps/diff2_mcps);
03043 // abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
03044         xTalkCorrection  = diff1_mcps/diff2_mcps;
03045 
03046         /* FixPoint2408 << 8 = FixPoint1616 */
03047         xTalkCorrection <<= 8;
03048 
03049         if(pRangingMeasurementData->RangeStatus != 0){
03050             pwMult = 1 << 16;
03051         } else {
03052             /* FixPoint1616/uint32 = FixPoint1616 */
03053             pwMult = deltaT_ps/cVcselPulseWidth_ps; /* smaller than 1.0f */
03054 
03055             /*
03056              * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
03057              * values are small enough such that32 bits will not be
03058              * exceeded.
03059              */
03060             pwMult *= ((1 << 16) - xTalkCorrection);
03061 
03062             /* (FixPoint3232 >> 16) = FixPoint1616 */
03063             pwMult =  (pwMult + c16BitRoundingParam) >> 16;
03064 
03065             /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
03066             pwMult += (1 << 16);
03067 
03068             /*
03069              * At this point the value will be 1.xx, therefore if we square
03070              * the value this will exceed 32 bits. To address this perform
03071              * a single shift to the right before the multiplication.
03072              */
03073             pwMult >>= 1;
03074             /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
03075             pwMult = pwMult * pwMult;
03076 
03077             /* (FixPoint3430 >> 14) = Fix1616 */
03078             pwMult >>= 14;
03079         }
03080 
03081         /* FixPoint1616 * uint32 = FixPoint1616 */
03082         sqr1 = pwMult * sigmaEstimateP1;
03083 
03084         /* (FixPoint1616 >> 16) = FixPoint3200 */
03085         sqr1 = (sqr1 + 0x8000) >> 16;
03086 
03087         /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
03088         sqr1 *= sqr1;
03089 
03090         sqr2 = sigmaEstimateP2;
03091 
03092         /* (FixPoint1616 >> 16) = FixPoint3200 */
03093         sqr2 = (sqr2 + 0x8000) >> 16;
03094 
03095         /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
03096         sqr2 *= sqr2;
03097 
03098         /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
03099         sqrSum = sqr1 + sqr2;
03100 
03101         /* SQRT(FixPoin6400) = FixPoint3200 */
03102         sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum);
03103 
03104         /* (FixPoint3200 << 16) = FixPoint1616 */
03105         sqrtResult_centi_ns <<= 16;
03106 
03107         /*
03108          * Note that the Speed Of Light is expressed in um per 1E-10
03109          * seconds (2997) Therefore to get mm/ns we have to divide by
03110          * 10000
03111          */
03112         sigmaEstRtn = (((sqrtResult_centi_ns+50)/100) /
03113                 sigmaEstimateP3);
03114         sigmaEstRtn      *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
03115 
03116         /* Add 5000 before dividing by 10000 to ensure rounding. */
03117         sigmaEstRtn      += 5000;
03118         sigmaEstRtn      /= 10000;
03119 
03120         if (sigmaEstRtn > cSigmaEstRtnMax) {
03121             /* Clip to prevent overflow. Will ensure safe
03122              * max result. */
03123             sigmaEstRtn = cSigmaEstRtnMax;
03124         }
03125         finalRangeIntegrationTimeMilliSecs =
03126             (finalRangeTimeoutMicroSecs + preRangeTimeoutMicroSecs + 500)/1000;
03127 
03128         /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
03129          * sqrt(FixPoint1616/int) = FixPoint2408)
03130          */
03131         sigmaEstRef =
03132             VL53L0X_isqrt((cDfltFinalRangeIntegrationTimeMilliSecs +
03133                 finalRangeIntegrationTimeMilliSecs/2)/
03134                 finalRangeIntegrationTimeMilliSecs);
03135 
03136         /* FixPoint2408 << 8 = FixPoint1616 */
03137         sigmaEstRef <<= 8;
03138         sigmaEstRef = (sigmaEstRef + 500)/1000;
03139 
03140         /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
03141         sqr1 = sigmaEstRtn * sigmaEstRtn;
03142         /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
03143         sqr2 = sigmaEstRef * sigmaEstRef;
03144 
03145         /* sqrt(FixPoint3232) = FixPoint1616 */
03146         sqrtResult = VL53L0X_isqrt((sqr1 + sqr2));
03147         /*
03148          * Note that the Shift by 4 bits increases resolution prior to
03149          * the sqrt, therefore the result must be shifted by 2 bits to
03150          * the right to revert back to the FixPoint1616 format.
03151          */
03152 
03153         sigmaEstimate    = 1000 * sqrtResult;
03154 
03155         if ((peakSignalRate_kcps < 1) || (vcselTotalEventsRtn < 1) ||
03156                 (sigmaEstimate > cSigmaEstMax)) {
03157                 sigmaEstimate = cSigmaEstMax;
03158         }
03159 
03160         *pSigmaEstimate = (uint32_t)(sigmaEstimate);
03161         PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate);
03162         Status = VL53L0X_calc_dmax(
03163             Dev,
03164             totalSignalRate_mcps,
03165             correctedSignalRate_mcps,
03166             pwMult,
03167             sigmaEstimateP1,
03168             sigmaEstimateP2,
03169             peakVcselDuration_us,
03170             pDmax_mm);
03171     }
03172 
03173     LOG_FUNCTION_END(Status);
03174     return Status;
03175 }
03176 
03177 VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
03178         uint8_t DeviceRangeStatus,
03179         FixPoint1616_t SignalRate,
03180         uint16_t EffectiveSpadRtnCount,
03181         VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
03182         uint8_t *pPalRangeStatus)
03183 {
03184     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
03185     uint8_t NoneFlag;
03186     uint8_t SigmaLimitflag = 0;
03187     uint8_t SignalRefClipflag = 0;
03188     uint8_t RangeIgnoreThresholdflag = 0;
03189     uint8_t SigmaLimitCheckEnable = 0;
03190     uint8_t SignalRateFinalRangeLimitCheckEnable = 0;
03191     uint8_t SignalRefClipLimitCheckEnable = 0;
03192     uint8_t RangeIgnoreThresholdLimitCheckEnable = 0;
03193     FixPoint1616_t SigmaEstimate;
03194     FixPoint1616_t SigmaLimitValue;
03195     FixPoint1616_t SignalRefClipValue;
03196     FixPoint1616_t RangeIgnoreThresholdValue;
03197     FixPoint1616_t SignalRatePerSpad;
03198     uint8_t DeviceRangeStatusInternal = 0;
03199     uint16_t tmpWord = 0;
03200     uint8_t Temp8;
03201     uint32_t Dmax_mm = 0;
03202     FixPoint1616_t LastSignalRefMcps;
03203 
03204     LOG_FUNCTION_START("");
03205 
03206 
03207     /*
03208      * VL53L0X has a good ranging when the value of the
03209      * DeviceRangeStatus = 11. This function will replace the value 0 with
03210      * the value 11 in the DeviceRangeStatus.
03211      * In addition, the SigmaEstimator is not included in the VL53L0X
03212      * DeviceRangeStatus, this will be added in the PalRangeStatus.
03213      */
03214 
03215     DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3);
03216 
03217     if (DeviceRangeStatusInternal == 0 ||
03218         DeviceRangeStatusInternal == 5 ||
03219         DeviceRangeStatusInternal == 7 ||
03220         DeviceRangeStatusInternal == 12 ||
03221         DeviceRangeStatusInternal == 13 ||
03222         DeviceRangeStatusInternal == 14 ||
03223         DeviceRangeStatusInternal == 15
03224             ) {
03225         NoneFlag = 1;
03226     } else {
03227         NoneFlag = 0;
03228     }
03229 
03230     /*
03231      * Check if Sigma limit is enabled, if yes then do comparison with limit
03232      * value and put the result back into pPalRangeStatus.
03233      */
03234     if (Status == VL53L0X_ERROR_NONE)
03235         Status =  VL53L0X_GetLimitCheckEnable(Dev,
03236             VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
03237             &SigmaLimitCheckEnable);
03238 
03239     if ((SigmaLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) {
03240         /*
03241         * compute the Sigma and check with limit
03242         */
03243         Status = VL53L0X_calc_sigma_estimate(
03244             Dev,
03245             pRangingMeasurementData,
03246             &SigmaEstimate,
03247             &Dmax_mm);
03248         if (Status == VL53L0X_ERROR_NONE)
03249             pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm;
03250 
03251         if (Status == VL53L0X_ERROR_NONE) {
03252             Status = VL53L0X_GetLimitCheckValue(Dev,
03253                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
03254                 &SigmaLimitValue);
03255 
03256             if ((SigmaLimitValue > 0) &&
03257                 (SigmaEstimate > SigmaLimitValue))
03258                     /* Limit Fail */
03259                     SigmaLimitflag = 1;
03260         }
03261     }
03262 
03263     /*
03264      * Check if Signal ref clip limit is enabled, if yes then do comparison
03265      * with limit value and put the result back into pPalRangeStatus.
03266      */
03267     if (Status == VL53L0X_ERROR_NONE)
03268         Status =  VL53L0X_GetLimitCheckEnable(Dev,
03269                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
03270                 &SignalRefClipLimitCheckEnable);
03271 
03272     if ((SignalRefClipLimitCheckEnable != 0) &&
03273             (Status == VL53L0X_ERROR_NONE)) {
03274 
03275         Status = VL53L0X_GetLimitCheckValue(Dev,
03276                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
03277                 &SignalRefClipValue);
03278 
03279         /* Read LastSignalRefMcps from device */
03280         if (Status == VL53L0X_ERROR_NONE)
03281             Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
03282 
03283         if (Status == VL53L0X_ERROR_NONE)
03284             Status = VL53L0X_RdWord(Dev,
03285                 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
03286                 &tmpWord);
03287 
03288         if (Status == VL53L0X_ERROR_NONE)
03289             Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
03290 
03291         LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord);
03292         PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps);
03293 
03294         if ((SignalRefClipValue > 0) &&
03295                 (LastSignalRefMcps > SignalRefClipValue)) {
03296             /* Limit Fail */
03297             SignalRefClipflag = 1;
03298         }
03299     }
03300 
03301     /*
03302      * Check if Signal ref clip limit is enabled, if yes then do comparison
03303      * with limit value and put the result back into pPalRangeStatus.
03304      * EffectiveSpadRtnCount has a format 8.8
03305      * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
03306      */
03307     if (Status == VL53L0X_ERROR_NONE)
03308         Status =  VL53L0X_GetLimitCheckEnable(Dev,
03309                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
03310                 &RangeIgnoreThresholdLimitCheckEnable);
03311 
03312     if ((RangeIgnoreThresholdLimitCheckEnable != 0) &&
03313             (Status == VL53L0X_ERROR_NONE)) {
03314 
03315         /* Compute the signal rate per spad */
03316         if (EffectiveSpadRtnCount == 0) {
03317             SignalRatePerSpad = 0;
03318         } else {
03319             SignalRatePerSpad = (FixPoint1616_t)((256 * SignalRate)
03320                 / EffectiveSpadRtnCount);
03321         }
03322 
03323         Status = VL53L0X_GetLimitCheckValue(Dev,
03324                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
03325                 &RangeIgnoreThresholdValue);
03326 
03327         if ((RangeIgnoreThresholdValue > 0) &&
03328             (SignalRatePerSpad < RangeIgnoreThresholdValue)) {
03329             /* Limit Fail add 2^6 to range status */
03330             RangeIgnoreThresholdflag = 1;
03331         }
03332     }
03333 
03334     if (Status == VL53L0X_ERROR_NONE) {
03335         if (NoneFlag == 1) {
03336             *pPalRangeStatus = 255;  /* NONE */
03337         } else if (DeviceRangeStatusInternal == 1 ||
03338                     DeviceRangeStatusInternal == 2 ||
03339                     DeviceRangeStatusInternal == 3) {
03340             *pPalRangeStatus = 5; /* HW fail */
03341         } else if (DeviceRangeStatusInternal == 6 ||
03342                     DeviceRangeStatusInternal == 9) {
03343             *pPalRangeStatus = 4;  /* Phase fail */
03344         } else if (DeviceRangeStatusInternal == 8 ||
03345                     DeviceRangeStatusInternal == 10 ||
03346                     SignalRefClipflag == 1) {
03347             *pPalRangeStatus = 3;  /* Min range */
03348         } else if (DeviceRangeStatusInternal == 4 ||
03349                     RangeIgnoreThresholdflag == 1) {
03350             *pPalRangeStatus = 2;  /* Signal Fail */
03351         } else if (SigmaLimitflag == 1) {
03352             *pPalRangeStatus = 1;  /* Sigma  Fail */
03353         } else {
03354             *pPalRangeStatus = 0; /* Range Valid */
03355         }
03356     }
03357 
03358     /* DMAX only relevant during range error */
03359     if (*pPalRangeStatus == 0)
03360         pRangingMeasurementData->RangeDMaxMilliMeter = 0;
03361 
03362     /* fill the Limit Check Status */
03363 
03364     Status =  VL53L0X_GetLimitCheckEnable(Dev,
03365             VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
03366             &SignalRateFinalRangeLimitCheckEnable);
03367 
03368     if (Status == VL53L0X_ERROR_NONE) {
03369         if ((SigmaLimitCheckEnable == 0) || (SigmaLimitflag == 1))
03370             Temp8 = 1;
03371         else
03372             Temp8 = 0;
03373         VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
03374                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
03375 
03376         if ((DeviceRangeStatusInternal == 4) ||
03377                 (SignalRateFinalRangeLimitCheckEnable == 0))
03378             Temp8 = 1;
03379         else
03380             Temp8 = 0;
03381         VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
03382                 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
03383                 Temp8);
03384 
03385         if ((SignalRefClipLimitCheckEnable == 0) ||
03386                     (SignalRefClipflag == 1))
03387             Temp8 = 1;
03388         else
03389             Temp8 = 0;
03390 
03391         VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
03392                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8);
03393 
03394         if ((RangeIgnoreThresholdLimitCheckEnable == 0) ||
03395                 (RangeIgnoreThresholdflag == 1))
03396             Temp8 = 1;
03397         else
03398             Temp8 = 0;
03399 
03400         VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
03401                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
03402                 Temp8);
03403     }
03404 
03405     LOG_FUNCTION_END(Status);
03406     return Status;
03407 
03408 }
03409 
03410 VL53L0X_Error VL53L0X::VL53L0X_GetRangingMeasurementData(VL53L0X_DEV Dev,
03411     VL53L0X_RangingMeasurementData_t *pRangingMeasurementData)
03412 {
03413     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
03414     uint8_t DeviceRangeStatus;
03415     uint8_t RangeFractionalEnable;
03416     uint8_t PalRangeStatus;
03417     uint8_t XTalkCompensationEnable;
03418     uint16_t AmbientRate;
03419     FixPoint1616_t SignalRate;
03420     uint16_t XTalkCompensationRateMegaCps;
03421     uint16_t EffectiveSpadRtnCount;
03422     uint16_t tmpuint16;
03423     uint16_t XtalkRangeMilliMeter;
03424     uint16_t LinearityCorrectiveGain;
03425     uint8_t localBuffer[12];
03426     VL53L0X_RangingMeasurementData_t LastRangeDataBuffer;
03427 
03428     LOG_FUNCTION_START("");
03429 
03430     /*
03431      * use multi read even if some registers are not useful, result will
03432      * be more efficient
03433      * start reading at 0x14 dec20
03434      * end reading at 0x21 dec33 total 14 bytes to read
03435      */
03436     Status = VL53L0X_ReadMulti(Dev, 0x14, localBuffer, 12);
03437 
03438     if (Status == VL53L0X_ERROR_NONE) {
03439 
03440         pRangingMeasurementData->ZoneId = 0; /* Only one zone */
03441         pRangingMeasurementData->TimeStamp = 0; /* Not Implemented */
03442 
03443         tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]);
03444         /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
03445          *(format 11.2) else no fractional
03446          */
03447 
03448         pRangingMeasurementData->MeasurementTimeUsec = 0;
03449 
03450         SignalRate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
03451             VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
03452         /* peak_signal_count_rate_rtn_mcps */
03453         pRangingMeasurementData->SignalRateRtnMegaCps = SignalRate;
03454 
03455         AmbientRate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
03456         pRangingMeasurementData->AmbientRateRtnMegaCps =
03457             VL53L0X_FIXPOINT97TOFIXPOINT1616(AmbientRate);
03458 
03459         EffectiveSpadRtnCount = VL53L0X_MAKEUINT16(localBuffer[3],
03460             localBuffer[2]);
03461         /* EffectiveSpadRtnCount is 8.8 format */
03462         pRangingMeasurementData->EffectiveSpadRtnCount =
03463             EffectiveSpadRtnCount;
03464 
03465         DeviceRangeStatus = localBuffer[0];
03466 
03467         /* Get Linearity Corrective Gain */
03468         LinearityCorrectiveGain = PALDevDataGet(Dev,
03469             LinearityCorrectiveGain);
03470 
03471         /* Get ranging configuration */
03472         RangeFractionalEnable = PALDevDataGet(Dev,
03473             RangeFractionalEnable);
03474 
03475         if (LinearityCorrectiveGain != 1000) {
03476 
03477             tmpuint16 = (uint16_t)((LinearityCorrectiveGain
03478                 * tmpuint16 + 500) / 1000);
03479 
03480             /* Implement Xtalk */
03481             VL53L0X_GETPARAMETERFIELD(Dev,
03482                 XTalkCompensationRateMegaCps,
03483                 XTalkCompensationRateMegaCps);
03484             VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable,
03485                 XTalkCompensationEnable);
03486 
03487             if (XTalkCompensationEnable) {
03488 
03489                 if ((SignalRate
03490                     - ((XTalkCompensationRateMegaCps
03491                     * EffectiveSpadRtnCount) >> 8))
03492                     <= 0) {
03493                     if (RangeFractionalEnable)
03494                         XtalkRangeMilliMeter = 8888;
03495                     else
03496                         XtalkRangeMilliMeter = 8888
03497                             << 2;
03498                 } else {
03499                     XtalkRangeMilliMeter =
03500                     (tmpuint16 * SignalRate)
03501                         / (SignalRate
03502                         - ((XTalkCompensationRateMegaCps
03503                         * EffectiveSpadRtnCount)
03504                         >> 8));
03505                 }
03506 
03507                 tmpuint16 = XtalkRangeMilliMeter;
03508             }
03509 
03510         }
03511 
03512         if (RangeFractionalEnable) {
03513             pRangingMeasurementData->RangeMilliMeter =
03514                 (uint16_t)((tmpuint16) >> 2);
03515             pRangingMeasurementData->RangeFractionalPart =
03516                 (uint8_t)((tmpuint16 & 0x03) << 6);
03517         } else {
03518             pRangingMeasurementData->RangeMilliMeter = tmpuint16;
03519             pRangingMeasurementData->RangeFractionalPart = 0;
03520         }
03521 
03522         /*
03523          * For a standard definition of RangeStatus, this should
03524          * return 0 in case of good result after a ranging
03525          * The range status depends on the device so call a device
03526          * specific function to obtain the right Status.
03527          */
03528         Status |= VL53L0X_get_pal_range_status(Dev, DeviceRangeStatus,
03529             SignalRate, EffectiveSpadRtnCount,
03530             pRangingMeasurementData, &PalRangeStatus);
03531 
03532         if (Status == VL53L0X_ERROR_NONE)
03533             pRangingMeasurementData->RangeStatus = PalRangeStatus;
03534 
03535     }
03536 
03537     if (Status == VL53L0X_ERROR_NONE) {
03538         /* Copy last read data into Dev buffer */
03539         LastRangeDataBuffer = PALDevDataGet(Dev, LastRangeMeasure);
03540 
03541         LastRangeDataBuffer.RangeMilliMeter =
03542             pRangingMeasurementData->RangeMilliMeter;
03543         LastRangeDataBuffer.RangeFractionalPart =
03544             pRangingMeasurementData->RangeFractionalPart;
03545         LastRangeDataBuffer.RangeDMaxMilliMeter =
03546             pRangingMeasurementData->RangeDMaxMilliMeter;
03547         LastRangeDataBuffer.MeasurementTimeUsec =
03548             pRangingMeasurementData->MeasurementTimeUsec;
03549         LastRangeDataBuffer.SignalRateRtnMegaCps =
03550             pRangingMeasurementData->SignalRateRtnMegaCps;
03551         LastRangeDataBuffer.AmbientRateRtnMegaCps =
03552             pRangingMeasurementData->AmbientRateRtnMegaCps;
03553         LastRangeDataBuffer.EffectiveSpadRtnCount =
03554             pRangingMeasurementData->EffectiveSpadRtnCount;
03555         LastRangeDataBuffer.RangeStatus =
03556             pRangingMeasurementData->RangeStatus;
03557 
03558         PALDevDataSet(Dev, LastRangeMeasure, LastRangeDataBuffer);
03559     }
03560 
03561     LOG_FUNCTION_END(Status);
03562     return Status;
03563 }
03564 
03565 VL53L0X_Error VL53L0X::VL53L0X_PerformSingleRangingMeasurement(VL53L0X_DEV Dev,
03566     VL53L0X_RangingMeasurementData_t *pRangingMeasurementData)
03567 {
03568     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
03569 
03570     LOG_FUNCTION_START("");
03571 
03572     /* This function will do a complete single ranging
03573      * Here we fix the mode! */
03574     Status = VL53L0X_SetDeviceMode(Dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
03575 
03576     if (Status == VL53L0X_ERROR_NONE)
03577         Status = VL53L0X_PerformSingleMeasurement(Dev);
03578 
03579 
03580     if (Status == VL53L0X_ERROR_NONE)
03581         Status = VL53L0X_GetRangingMeasurementData(Dev,
03582             pRangingMeasurementData);
03583 
03584 
03585     if (Status == VL53L0X_ERROR_NONE)
03586         Status = VL53L0X_ClearInterruptMask(Dev, 0);
03587 
03588 
03589     LOG_FUNCTION_END(Status);
03590     return Status;
03591 }
03592 
03593 VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV Dev,
03594         uint16_t *refSignalRate)
03595 {
03596     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03597     VL53L0X_RangingMeasurementData_t rangingMeasurementData;
03598 
03599     uint8_t SequenceConfig = 0;
03600 
03601     /* store the value of the sequence config,
03602      * this will be reset before the end of the function
03603      */
03604 
03605     SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
03606 
03607     /*
03608      * This function performs a reference signal rate measurement.
03609      */
03610     if (status == VL53L0X_ERROR_NONE)
03611         status = VL53L0X_WrByte(Dev,
03612             VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
03613 
03614     if (status == VL53L0X_ERROR_NONE)
03615         status = VL53L0X_PerformSingleRangingMeasurement(Dev,
03616                 &rangingMeasurementData);
03617 
03618     if (status == VL53L0X_ERROR_NONE)
03619         status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
03620 
03621     if (status == VL53L0X_ERROR_NONE)
03622         status = VL53L0X_RdWord(Dev,
03623             VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
03624             refSignalRate);
03625 
03626     if (status == VL53L0X_ERROR_NONE)
03627         status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
03628 
03629     if (status == VL53L0X_ERROR_NONE) {
03630         /* restore the previous Sequence Config */
03631         status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
03632                 SequenceConfig);
03633         if (status == VL53L0X_ERROR_NONE)
03634             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
03635     }
03636 
03637     return status;
03638 }
03639 
03640 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
03641                 uint32_t *refSpadCount,
03642                 uint8_t *isApertureSpads)
03643 {
03644     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
03645     uint8_t lastSpadArray[6];
03646     uint8_t startSelect = 0xB4;
03647     uint32_t minimumSpadCount = 3;
03648     uint32_t maxSpadCount = 44;
03649     uint32_t currentSpadIndex = 0;
03650     uint32_t lastSpadIndex = 0;
03651     int32_t nextGoodSpad = 0;
03652     uint16_t targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
03653     uint16_t peakSignalRateRef;
03654     uint32_t needAptSpads = 0;
03655     uint32_t index = 0;
03656     uint32_t spadArraySize = 6;
03657     uint32_t signalRateDiff = 0;
03658     uint32_t lastSignalRateDiff = 0;
03659     uint8_t complete = 0;
03660     uint8_t VhvSettings = 0;
03661     uint8_t PhaseCal = 0;
03662     uint32_t refSpadCount_int = 0;
03663     uint8_t  isApertureSpads_int = 0;
03664 
03665     /*
03666      * The reference SPAD initialization procedure determines the minimum
03667      * amount of reference spads to be enables to achieve a target reference
03668      * signal rate and should be performed once during initialization.
03669      *
03670      * Either aperture or non-aperture spads are applied but never both.
03671      * Firstly non-aperture spads are set, begining with 5 spads, and
03672      * increased one spad at a time until the closest measurement to the
03673      * target rate is achieved.
03674      *
03675      * If the target rate is exceeded when 5 non-aperture spads are enabled,
03676      * initialization is performed instead with aperture spads.
03677      *
03678      * When setting spads, a 'Good Spad Map' is applied.
03679      *
03680      * This procedure operates within a SPAD window of interest of a maximum
03681      * 44 spads.
03682      * The start point is currently fixed to 180, which lies towards the end
03683      * of the non-aperture quadrant and runs in to the adjacent aperture
03684      * quadrant.
03685      */
03686 
03687 
03688     targetRefRate = PALDevDataGet(Dev, targetRefRate);
03689 
03690     /*
03691      * Initialize Spad arrays.
03692      * Currently the good spad map is initialised to 'All good'.
03693      * This is a short term implementation. The good spad map will be
03694      * provided as an input.
03695      * Note that there are 6 bytes. Only the first 44 bits will be used to
03696      * represent spads.
03697      */
03698     for (index = 0; index < spadArraySize; index++)
03699         Dev->Data .SpadData .RefSpadEnables [index] = 0;
03700 
03701 
03702     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
03703 
03704     if (Status == VL53L0X_ERROR_NONE)
03705         Status = VL53L0X_WrByte(Dev,
03706             VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
03707 
03708     if (Status == VL53L0X_ERROR_NONE)
03709         Status = VL53L0X_WrByte(Dev,
03710             VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
03711 
03712     if (Status == VL53L0X_ERROR_NONE)
03713         Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
03714 
03715     if (Status == VL53L0X_ERROR_NONE)
03716         Status = VL53L0X_WrByte(Dev,
03717             VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
03718             startSelect);
03719 
03720 
03721     if (Status == VL53L0X_ERROR_NONE)
03722         Status = VL53L0X_WrByte(Dev,
03723                 VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
03724 
03725     /* Perform ref calibration */
03726     if (Status == VL53L0X_ERROR_NONE)
03727         Status = VL53L0X_perform_ref_calibration(Dev, &VhvSettings,
03728             &PhaseCal, 0);
03729 
03730     if (Status == VL53L0X_ERROR_NONE) {
03731         /* Enable Minimum NON-APERTURE Spads */
03732         currentSpadIndex = 0;
03733         lastSpadIndex = currentSpadIndex;
03734         needAptSpads = 0;
03735         Status = enable_ref_spads(Dev,
03736                     needAptSpads,
03737                     Dev->Data .SpadData .RefGoodSpadMap ,
03738                     Dev->Data .SpadData .RefSpadEnables ,
03739                     spadArraySize,
03740                     startSelect,
03741                     currentSpadIndex,
03742                     minimumSpadCount,
03743                     &lastSpadIndex);
03744     }
03745 
03746     if (Status == VL53L0X_ERROR_NONE) {
03747         currentSpadIndex = lastSpadIndex;
03748 
03749         Status = perform_ref_signal_measurement(Dev,
03750             &peakSignalRateRef);
03751         if ((Status == VL53L0X_ERROR_NONE) &&
03752             (peakSignalRateRef > targetRefRate)) {
03753             /* Signal rate measurement too high,
03754              * switch to APERTURE SPADs */
03755 
03756             for (index = 0; index < spadArraySize; index++)
03757                 Dev->Data .SpadData .RefSpadEnables [index] = 0;
03758 
03759 
03760             /* Increment to the first APERTURE spad */
03761             while ((is_aperture(startSelect + currentSpadIndex)
03762                 == 0) && (currentSpadIndex < maxSpadCount)) {
03763                 currentSpadIndex++;
03764             }
03765 
03766             needAptSpads = 1;
03767 
03768             Status = enable_ref_spads(Dev,
03769                     needAptSpads,
03770                     Dev->Data .SpadData .RefGoodSpadMap ,
03771                     Dev->Data .SpadData .RefSpadEnables ,
03772                     spadArraySize,
03773                     startSelect,
03774                     currentSpadIndex,
03775                     minimumSpadCount,
03776                     &lastSpadIndex);
03777 
03778             if (Status == VL53L0X_ERROR_NONE) {
03779                 currentSpadIndex = lastSpadIndex;
03780                 Status = perform_ref_signal_measurement(Dev,
03781                         &peakSignalRateRef);
03782 
03783                 if ((Status == VL53L0X_ERROR_NONE) &&
03784                     (peakSignalRateRef > targetRefRate)) {
03785                     /* Signal rate still too high after
03786                      * setting the minimum number of
03787                      * APERTURE spads. Can do no more
03788                      * therefore set the min number of
03789                      * aperture spads as the result.
03790                      */
03791                     isApertureSpads_int = 1;
03792                     refSpadCount_int = minimumSpadCount;
03793                 }
03794             }
03795         } else {
03796             needAptSpads = 0;
03797         }
03798     }
03799 
03800     if ((Status == VL53L0X_ERROR_NONE) &&
03801         (peakSignalRateRef < targetRefRate)) {
03802         /* At this point, the minimum number of either aperture
03803          * or non-aperture spads have been set. Proceed to add
03804          * spads and perform measurements until the target
03805          * reference is reached.
03806          */
03807         isApertureSpads_int = needAptSpads;
03808         refSpadCount_int    = minimumSpadCount;
03809 
03810         memcpy(lastSpadArray, Dev->Data .SpadData .RefSpadEnables ,
03811                 spadArraySize);
03812         lastSignalRateDiff = abs(peakSignalRateRef -
03813             targetRefRate);
03814         complete = 0;
03815 
03816         while (!complete) {
03817             get_next_good_spad(
03818                 Dev->Data .SpadData .RefGoodSpadMap ,
03819                 spadArraySize, currentSpadIndex,
03820                 &nextGoodSpad);
03821 
03822             if (nextGoodSpad == -1) {
03823                 Status = VL53L0X_ERROR_REF_SPAD_INIT;
03824                 break;
03825             }
03826 
03827             /* Cannot combine Aperture and Non-Aperture spads, so
03828              * ensure the current spad is of the correct type.
03829              */
03830             if (is_aperture((uint32_t)startSelect + nextGoodSpad) !=
03831                     needAptSpads) {
03832                 /* At this point we have enabled the maximum
03833                  * number of Aperture spads.
03834                  */
03835                 complete = 1;
03836                 break;
03837             }
03838 
03839             (refSpadCount_int)++;
03840 
03841             currentSpadIndex = nextGoodSpad;
03842             Status = enable_spad_bit(
03843                     Dev->Data .SpadData .RefSpadEnables ,
03844                     spadArraySize, currentSpadIndex);
03845 
03846             if (Status == VL53L0X_ERROR_NONE) {
03847                 currentSpadIndex++;
03848                 /* Proceed to apply the additional spad and
03849                  * perform measurement. */
03850                 Status = set_ref_spad_map(Dev,
03851                     Dev->Data .SpadData .RefSpadEnables );
03852             }
03853 
03854             if (Status != VL53L0X_ERROR_NONE)
03855                 break;
03856 
03857             Status = perform_ref_signal_measurement(Dev,
03858                     &peakSignalRateRef);
03859 
03860             if (Status != VL53L0X_ERROR_NONE)
03861                 break;
03862 
03863             signalRateDiff = abs(peakSignalRateRef - targetRefRate);
03864 
03865             if (peakSignalRateRef > targetRefRate) {
03866                 /* Select the spad map that provides the
03867                  * measurement closest to the target rate,
03868                  * either above or below it.
03869                  */
03870                 if (signalRateDiff > lastSignalRateDiff) {
03871                     /* Previous spad map produced a closer
03872                      * measurement, so choose this. */
03873                     Status = set_ref_spad_map(Dev,
03874                             lastSpadArray);
03875                     memcpy(
03876                     Dev->Data .SpadData .RefSpadEnables ,
03877                     lastSpadArray, spadArraySize);
03878 
03879                     (refSpadCount_int)--;
03880                 }
03881                 complete = 1;
03882             } else {
03883                 /* Continue to add spads */
03884                 lastSignalRateDiff = signalRateDiff;
03885                 memcpy(lastSpadArray,
03886                     Dev->Data .SpadData .RefSpadEnables ,
03887                     spadArraySize);
03888             }
03889 
03890         } /* while */
03891     }
03892 
03893     if (Status == VL53L0X_ERROR_NONE) {
03894         *refSpadCount = refSpadCount_int;
03895         *isApertureSpads = isApertureSpads_int;
03896 
03897         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
03898         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
03899             ReferenceSpadCount, (uint8_t)(*refSpadCount));
03900         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
03901             ReferenceSpadType, *isApertureSpads);
03902     }
03903 
03904     return Status;
03905 }
03906 
03907 VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
03908                  uint32_t count, uint8_t isApertureSpads)
03909 {
03910     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
03911     uint32_t currentSpadIndex = 0;
03912     uint8_t startSelect = 0xB4;
03913     uint32_t spadArraySize = 6;
03914     uint32_t maxSpadCount = 44;
03915     uint32_t lastSpadIndex;
03916     uint32_t index;
03917 
03918     /*
03919      * This function applies a requested number of reference spads, either
03920      * aperture or
03921      * non-aperture, as requested.
03922      * The good spad map will be applied.
03923      */
03924 
03925     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
03926 
03927     if (Status == VL53L0X_ERROR_NONE)
03928         Status = VL53L0X_WrByte(Dev,
03929             VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
03930 
03931     if (Status == VL53L0X_ERROR_NONE)
03932         Status = VL53L0X_WrByte(Dev,
03933             VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
03934 
03935     if (Status == VL53L0X_ERROR_NONE)
03936         Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
03937 
03938     if (Status == VL53L0X_ERROR_NONE)
03939         Status = VL53L0X_WrByte(Dev,
03940             VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
03941             startSelect);
03942 
03943     for (index = 0; index < spadArraySize; index++)
03944         Dev->Data .SpadData .RefSpadEnables [index] = 0;
03945 
03946     if (isApertureSpads) {
03947         /* Increment to the first APERTURE spad */
03948         while ((is_aperture(startSelect + currentSpadIndex) == 0) &&
03949               (currentSpadIndex < maxSpadCount)) {
03950             currentSpadIndex++;
03951         }
03952     }
03953     Status = enable_ref_spads(Dev,
03954                 isApertureSpads,
03955                 Dev->Data .SpadData .RefGoodSpadMap ,
03956                 Dev->Data .SpadData .RefSpadEnables ,
03957                 spadArraySize,
03958                 startSelect,
03959                 currentSpadIndex,
03960                 count,
03961                 &lastSpadIndex);
03962 
03963     if (Status == VL53L0X_ERROR_NONE) {
03964         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
03965         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
03966             ReferenceSpadCount, (uint8_t)(count));
03967         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
03968             ReferenceSpadType, isApertureSpads);
03969     }
03970 
03971     return Status;
03972 }
03973 
03974 VL53L0X_Error VL53L0X::VL53L0X_WaitDeviceBooted(VL53L0X_DEV Dev)
03975 {
03976     VL53L0X_Error Status = VL53L0X_ERROR_NOT_IMPLEMENTED;
03977     LOG_FUNCTION_START("");
03978 
03979     /* not implemented on VL53L0X */
03980 
03981     LOG_FUNCTION_END(Status);
03982     return Status;
03983 }
03984 
03985 VL53L0X_Error VL53L0X::VL53L0X_PerformRefCalibration(VL53L0X_DEV Dev, uint8_t *pVhvSettings,
03986     uint8_t *pPhaseCal)
03987 {
03988     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
03989     LOG_FUNCTION_START("");
03990 
03991     Status = VL53L0X_perform_ref_calibration(Dev, pVhvSettings,
03992         pPhaseCal, 1);
03993 
03994     LOG_FUNCTION_END(Status);
03995     return Status;
03996 }
03997 
03998 VL53L0X_Error VL53L0X::VL53L0X_PerformRefSpadManagement(VL53L0X_DEV Dev,
03999     uint32_t *refSpadCount, uint8_t *isApertureSpads)
04000 {
04001     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04002     LOG_FUNCTION_START("");
04003 
04004     Status = VL53L0X_perform_ref_spad_management(Dev, refSpadCount,
04005         isApertureSpads);
04006 
04007     LOG_FUNCTION_END(Status);
04008 
04009     return Status;
04010 }
04011 
04012 /* Group PAL Init Functions */
04013 VL53L0X_Error VL53L0X::VL53L0X_SetDeviceAddress(VL53L0X_DEV Dev, uint8_t DeviceAddress)
04014 {
04015     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04016     LOG_FUNCTION_START("");
04017 
04018     Status = VL53L0X_WrByte(Dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
04019         DeviceAddress / 2);
04020 
04021     LOG_FUNCTION_END(Status);
04022     return Status;
04023 }
04024 
04025 VL53L0X_Error VL53L0X::VL53L0X_SetGpioConfig(VL53L0X_DEV Dev, uint8_t Pin,
04026     VL53L0X_DeviceModes DeviceMode, VL53L0X_GpioFunctionality Functionality,
04027     VL53L0X_InterruptPolarity Polarity)
04028 {
04029     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04030     uint8_t data;
04031 
04032     LOG_FUNCTION_START("");
04033 
04034     if (Pin != 0) {
04035         Status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
04036     } else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
04037         if (Polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
04038             data = 0x10;
04039         else
04040             data = 1;
04041 
04042         Status = VL53L0X_WrByte(Dev,
04043         VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
04044 
04045     } else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_OSC) {
04046 
04047         Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
04048         Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
04049 
04050         Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
04051         Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
04052         Status |= VL53L0X_WrByte(Dev, 0x85, 0x02);
04053 
04054         Status |= VL53L0X_WrByte(Dev, 0xff, 0x04);
04055         Status |= VL53L0X_WrByte(Dev, 0xcd, 0x00);
04056         Status |= VL53L0X_WrByte(Dev, 0xcc, 0x11);
04057 
04058         Status |= VL53L0X_WrByte(Dev, 0xff, 0x07);
04059         Status |= VL53L0X_WrByte(Dev, 0xbe, 0x00);
04060 
04061         Status |= VL53L0X_WrByte(Dev, 0xff, 0x06);
04062         Status |= VL53L0X_WrByte(Dev, 0xcc, 0x09);
04063 
04064         Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
04065         Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
04066         Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
04067 
04068     } else {
04069 
04070         if (Status == VL53L0X_ERROR_NONE) {
04071             switch (Functionality) {
04072             case VL53L0X_GPIOFUNCTIONALITY_OFF:
04073                 data = 0x00;
04074                 break;
04075             case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
04076                 data = 0x01;
04077                 break;
04078             case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
04079                 data = 0x02;
04080                 break;
04081             case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
04082                 data = 0x03;
04083                 break;
04084             case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
04085                 data = 0x04;
04086                 break;
04087             default:
04088                 Status =
04089                 VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
04090             }
04091         }
04092 
04093         if (Status == VL53L0X_ERROR_NONE)
04094             Status = VL53L0X_WrByte(Dev,
04095             VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
04096 
04097         if (Status == VL53L0X_ERROR_NONE) {
04098             if (Polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
04099                 data = 0;
04100             else
04101                 data = (uint8_t)(1 << 4);
04102 
04103             Status = VL53L0X_UpdateByte(Dev,
04104             VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
04105         }
04106 
04107         if (Status == VL53L0X_ERROR_NONE)
04108             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
04109                 Pin0GpioFunctionality, Functionality);
04110 
04111         if (Status == VL53L0X_ERROR_NONE)
04112             Status = VL53L0X_ClearInterruptMask(Dev, 0);
04113 
04114     }
04115     LOG_FUNCTION_END(Status);
04116     return Status;
04117 }
04118 
04119 VL53L0X_Error VL53L0X::VL53L0X_GetFractionEnable(VL53L0X_DEV Dev, uint8_t *pEnabled)
04120 {
04121     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04122     LOG_FUNCTION_START("");
04123 
04124     Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, pEnabled);
04125 
04126     if (Status == VL53L0X_ERROR_NONE)
04127         *pEnabled = (*pEnabled & 1);
04128 
04129     LOG_FUNCTION_END(Status);
04130     return Status;
04131 }
04132 
04133 uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
04134 {
04135     /*!
04136      * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
04137      */
04138 
04139     uint16_t encoded_timeout = 0;
04140     uint32_t ls_byte = 0;
04141     uint16_t ms_byte = 0;
04142 
04143     if (timeout_macro_clks > 0) {
04144         ls_byte = timeout_macro_clks - 1;
04145 
04146         while ((ls_byte & 0xFFFFFF00) > 0) {
04147             ls_byte = ls_byte >> 1;
04148             ms_byte++;
04149         }
04150 
04151         encoded_timeout = (ms_byte << 8)
04152                 + (uint16_t) (ls_byte & 0x000000FF);
04153     }
04154 
04155     return encoded_timeout;
04156 
04157 }
04158 
04159 VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV Dev,
04160                     VL53L0X_SequenceStepId SequenceStepId,
04161                     uint32_t TimeOutMicroSecs)
04162 {
04163     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04164     uint8_t CurrentVCSELPulsePeriodPClk;
04165     uint8_t MsrcEncodedTimeOut;
04166     uint16_t PreRangeEncodedTimeOut;
04167     uint16_t PreRangeTimeOutMClks;
04168     uint16_t MsrcRangeTimeOutMClks;
04169     uint32_t FinalRangeTimeOutMClks;
04170     uint16_t FinalRangeEncodedTimeOut;
04171     VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
04172 
04173     if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC)     ||
04174         (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS)     ||
04175         (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
04176 
04177         Status = VL53L0X_GetVcselPulsePeriod(Dev,
04178                     VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04179                     &CurrentVCSELPulsePeriodPClk);
04180 
04181         if (Status == VL53L0X_ERROR_NONE) {
04182             MsrcRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev,
04183                     TimeOutMicroSecs,
04184                     (uint8_t)CurrentVCSELPulsePeriodPClk);
04185 
04186             if (MsrcRangeTimeOutMClks > 256)
04187                 MsrcEncodedTimeOut = 255;
04188             else
04189                 MsrcEncodedTimeOut =
04190                     (uint8_t)MsrcRangeTimeOutMClks - 1;
04191 
04192             VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
04193                 LastEncodedTimeout,
04194                 MsrcEncodedTimeOut);
04195         }
04196 
04197         if (Status == VL53L0X_ERROR_NONE) {
04198             Status = VL53L0X_WrByte(Dev,
04199                 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
04200                 MsrcEncodedTimeOut);
04201         }
04202     } else {
04203 
04204         if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
04205 
04206             if (Status == VL53L0X_ERROR_NONE) {
04207                 Status = VL53L0X_GetVcselPulsePeriod(Dev,
04208                         VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04209                         &CurrentVCSELPulsePeriodPClk);
04210                 PreRangeTimeOutMClks =
04211                     VL53L0X_calc_timeout_mclks(Dev,
04212                     TimeOutMicroSecs,
04213                     (uint8_t)CurrentVCSELPulsePeriodPClk);
04214                 PreRangeEncodedTimeOut = VL53L0X_encode_timeout(
04215                     PreRangeTimeOutMClks);
04216 
04217                 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
04218                     LastEncodedTimeout,
04219                     PreRangeEncodedTimeOut);
04220             }
04221 
04222             if (Status == VL53L0X_ERROR_NONE) {
04223                 Status = VL53L0X_WrWord(Dev,
04224                 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
04225                 PreRangeEncodedTimeOut);
04226             }
04227 
04228             if (Status == VL53L0X_ERROR_NONE) {
04229                 VL53L0X_SETDEVICESPECIFICPARAMETER(
04230                     Dev,
04231                     PreRangeTimeoutMicroSecs,
04232                     TimeOutMicroSecs);
04233             }
04234         } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
04235 
04236             /* For the final range timeout, the pre-range timeout
04237              * must be added. To do this both final and pre-range
04238              * timeouts must be expressed in macro periods MClks
04239              * because they have different vcsel periods.
04240              */
04241 
04242             VL53L0X_GetSequenceStepEnables(Dev,
04243                     &SchedulerSequenceSteps);
04244             PreRangeTimeOutMClks = 0;
04245             if (SchedulerSequenceSteps.PreRangeOn) {
04246 
04247                 /* Retrieve PRE-RANGE VCSEL Period */
04248                 Status = VL53L0X_GetVcselPulsePeriod(Dev,
04249                     VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04250                     &CurrentVCSELPulsePeriodPClk);
04251 
04252                 /* Retrieve PRE-RANGE Timeout in Macro periods
04253                  * (MCLKS) */
04254                 if (Status == VL53L0X_ERROR_NONE) {
04255                     Status = VL53L0X_RdWord(Dev, 0x51,
04256                         &PreRangeEncodedTimeOut);
04257                     PreRangeTimeOutMClks =
04258                         VL53L0X_decode_timeout(
04259                             PreRangeEncodedTimeOut);
04260                 }
04261             }
04262 
04263             /* Calculate FINAL RANGE Timeout in Macro Periods
04264              * (MCLKS) and add PRE-RANGE value
04265              */
04266             if (Status == VL53L0X_ERROR_NONE) {
04267 
04268                 Status = VL53L0X_GetVcselPulsePeriod(Dev,
04269                         VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
04270                         &CurrentVCSELPulsePeriodPClk);
04271             }
04272             if (Status == VL53L0X_ERROR_NONE) {
04273 
04274                 FinalRangeTimeOutMClks =
04275                     VL53L0X_calc_timeout_mclks(Dev,
04276                     TimeOutMicroSecs,
04277                     (uint8_t) CurrentVCSELPulsePeriodPClk);
04278 
04279                 FinalRangeTimeOutMClks += PreRangeTimeOutMClks;
04280 
04281                 FinalRangeEncodedTimeOut =
04282                 VL53L0X_encode_timeout(FinalRangeTimeOutMClks);
04283 
04284                 if (Status == VL53L0X_ERROR_NONE) {
04285                     Status = VL53L0X_WrWord(Dev, 0x71,
04286                     FinalRangeEncodedTimeOut);
04287                 }
04288 
04289                 if (Status == VL53L0X_ERROR_NONE) {
04290                     VL53L0X_SETDEVICESPECIFICPARAMETER(
04291                         Dev,
04292                         FinalRangeTimeoutMicroSecs,
04293                         TimeOutMicroSecs);
04294                 }
04295             }
04296         } else
04297             Status = VL53L0X_ERROR_INVALID_PARAMS;
04298 
04299     }
04300     return Status;
04301 }
04302 
04303 VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
04304         uint32_t MeasurementTimingBudgetMicroSeconds)
04305 {
04306     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04307     uint32_t FinalRangeTimingBudgetMicroSeconds;
04308     VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
04309     uint32_t MsrcDccTccTimeoutMicroSeconds  = 2000;
04310     uint32_t StartOverheadMicroSeconds      = 1910;
04311     uint32_t EndOverheadMicroSeconds        = 960;
04312     uint32_t MsrcOverheadMicroSeconds       = 660;
04313     uint32_t TccOverheadMicroSeconds        = 590;
04314     uint32_t DssOverheadMicroSeconds        = 690;
04315     uint32_t PreRangeOverheadMicroSeconds   = 660;
04316     uint32_t FinalRangeOverheadMicroSeconds = 550;
04317     uint32_t PreRangeTimeoutMicroSeconds    = 0;
04318     uint32_t cMinTimingBudgetMicroSeconds   = 20000;
04319     uint32_t SubTimeout = 0;
04320 
04321     LOG_FUNCTION_START("");
04322 
04323     if (MeasurementTimingBudgetMicroSeconds
04324             < cMinTimingBudgetMicroSeconds) {
04325         Status = VL53L0X_ERROR_INVALID_PARAMS;
04326         return Status;
04327     }
04328 
04329     FinalRangeTimingBudgetMicroSeconds =
04330         MeasurementTimingBudgetMicroSeconds -
04331         (StartOverheadMicroSeconds + EndOverheadMicroSeconds);
04332 
04333     Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
04334 
04335     if (Status == VL53L0X_ERROR_NONE &&
04336         (SchedulerSequenceSteps.TccOn  ||
04337         SchedulerSequenceSteps.MsrcOn ||
04338         SchedulerSequenceSteps.DssOn)) {
04339 
04340         /* TCC, MSRC and DSS all share the same timeout */
04341         Status = get_sequence_step_timeout(Dev,
04342                     VL53L0X_SEQUENCESTEP_MSRC,
04343                     &MsrcDccTccTimeoutMicroSeconds);
04344 
04345         /* Subtract the TCC, MSRC and DSS timeouts if they are
04346          * enabled. */
04347 
04348         if (Status != VL53L0X_ERROR_NONE)
04349             return Status;
04350 
04351         /* TCC */
04352         if (SchedulerSequenceSteps.TccOn) {
04353 
04354             SubTimeout = MsrcDccTccTimeoutMicroSeconds
04355                 + TccOverheadMicroSeconds;
04356 
04357             if (SubTimeout <
04358                 FinalRangeTimingBudgetMicroSeconds) {
04359                 FinalRangeTimingBudgetMicroSeconds -=
04360                             SubTimeout;
04361             } else {
04362                 /* Requested timeout too big. */
04363                 Status = VL53L0X_ERROR_INVALID_PARAMS;
04364             }
04365         }
04366 
04367         if (Status != VL53L0X_ERROR_NONE) {
04368             LOG_FUNCTION_END(Status);
04369             return Status;
04370         }
04371 
04372         /* DSS */
04373         if (SchedulerSequenceSteps.DssOn) {
04374 
04375             SubTimeout = 2 * (MsrcDccTccTimeoutMicroSeconds +
04376                 DssOverheadMicroSeconds);
04377 
04378             if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
04379                 FinalRangeTimingBudgetMicroSeconds
04380                             -= SubTimeout;
04381             } else {
04382                 /* Requested timeout too big. */
04383                 Status = VL53L0X_ERROR_INVALID_PARAMS;
04384             }
04385         } else if (SchedulerSequenceSteps.MsrcOn) {
04386             /* MSRC */
04387             SubTimeout = MsrcDccTccTimeoutMicroSeconds +
04388                         MsrcOverheadMicroSeconds;
04389 
04390             if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
04391                 FinalRangeTimingBudgetMicroSeconds
04392                             -= SubTimeout;
04393             } else {
04394                 /* Requested timeout too big. */
04395                 Status = VL53L0X_ERROR_INVALID_PARAMS;
04396             }
04397         }
04398 
04399     }
04400 
04401     if (Status != VL53L0X_ERROR_NONE) {
04402         LOG_FUNCTION_END(Status);
04403         return Status;
04404     }
04405 
04406     if (SchedulerSequenceSteps.PreRangeOn) {
04407 
04408         /* Subtract the Pre-range timeout if enabled. */
04409 
04410         Status = get_sequence_step_timeout(Dev,
04411                 VL53L0X_SEQUENCESTEP_PRE_RANGE,
04412                 &PreRangeTimeoutMicroSeconds);
04413 
04414         SubTimeout = PreRangeTimeoutMicroSeconds +
04415                 PreRangeOverheadMicroSeconds;
04416 
04417         if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
04418             FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
04419         } else {
04420             /* Requested timeout too big. */
04421             Status = VL53L0X_ERROR_INVALID_PARAMS;
04422         }
04423     }
04424 
04425 
04426     if (Status == VL53L0X_ERROR_NONE &&
04427         SchedulerSequenceSteps.FinalRangeOn) {
04428 
04429         FinalRangeTimingBudgetMicroSeconds -=
04430                 FinalRangeOverheadMicroSeconds;
04431 
04432         /* Final Range Timeout
04433          * Note that the final range timeout is determined by the timing
04434          * budget and the sum of all other timeouts within the sequence.
04435          * If there is no room for the final range timeout, then an error
04436          * will be set. Otherwise the remaining time will be applied to
04437          * the final range.
04438          */
04439         Status = set_sequence_step_timeout(Dev,
04440             VL53L0X_SEQUENCESTEP_FINAL_RANGE,
04441             FinalRangeTimingBudgetMicroSeconds);
04442 
04443         VL53L0X_SETPARAMETERFIELD(Dev,
04444             MeasurementTimingBudgetMicroSeconds,
04445             MeasurementTimingBudgetMicroSeconds);
04446     }
04447 
04448     LOG_FUNCTION_END(Status);
04449 
04450     return Status;
04451 }
04452 
04453 VL53L0X_Error VL53L0X::VL53L0X_SetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev,
04454     uint32_t MeasurementTimingBudgetMicroSeconds)
04455 {
04456     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04457     LOG_FUNCTION_START("");
04458 
04459     Status = VL53L0X_set_measurement_timing_budget_micro_seconds(Dev,
04460         MeasurementTimingBudgetMicroSeconds);
04461 
04462     LOG_FUNCTION_END(Status);
04463 
04464     return Status;
04465 }
04466 
04467 VL53L0X_Error VL53L0X::VL53L0X_SetSequenceStepEnable(VL53L0X_DEV Dev,
04468     VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled)
04469 {
04470     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04471     uint8_t SequenceConfig = 0;
04472     uint8_t SequenceConfigNew = 0;
04473     uint32_t MeasurementTimingBudgetMicroSeconds;
04474     LOG_FUNCTION_START("");
04475 
04476     Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
04477         &SequenceConfig);
04478 
04479     SequenceConfigNew = SequenceConfig;
04480 
04481     if (Status == VL53L0X_ERROR_NONE) {
04482         if (SequenceStepEnabled == 1) {
04483 
04484             /* Enable requested sequence step
04485              */
04486             switch (SequenceStepId) {
04487             case VL53L0X_SEQUENCESTEP_TCC:
04488                 SequenceConfigNew |= 0x10;
04489                 break;
04490             case VL53L0X_SEQUENCESTEP_DSS:
04491                 SequenceConfigNew |= 0x28;
04492                 break;
04493             case VL53L0X_SEQUENCESTEP_MSRC:
04494                 SequenceConfigNew |= 0x04;
04495                 break;
04496             case VL53L0X_SEQUENCESTEP_PRE_RANGE:
04497                 SequenceConfigNew |= 0x40;
04498                 break;
04499             case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
04500                 SequenceConfigNew |= 0x80;
04501                 break;
04502             default:
04503                 Status = VL53L0X_ERROR_INVALID_PARAMS;
04504             }
04505         } else {
04506             /* Disable requested sequence step
04507              */
04508             switch (SequenceStepId) {
04509             case VL53L0X_SEQUENCESTEP_TCC:
04510                 SequenceConfigNew &= 0xef;
04511                 break;
04512             case VL53L0X_SEQUENCESTEP_DSS:
04513                 SequenceConfigNew &= 0xd7;
04514                 break;
04515             case VL53L0X_SEQUENCESTEP_MSRC:
04516                 SequenceConfigNew &= 0xfb;
04517                 break;
04518             case VL53L0X_SEQUENCESTEP_PRE_RANGE:
04519                 SequenceConfigNew &= 0xbf;
04520                 break;
04521             case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
04522                 SequenceConfigNew &= 0x7f;
04523                 break;
04524             default:
04525                 Status = VL53L0X_ERROR_INVALID_PARAMS;
04526             }
04527         }
04528     }
04529 
04530     if (SequenceConfigNew != SequenceConfig) {
04531         /* Apply New Setting */
04532         if (Status == VL53L0X_ERROR_NONE) {
04533             Status = VL53L0X_WrByte(Dev,
04534             VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, SequenceConfigNew);
04535         }
04536         if (Status == VL53L0X_ERROR_NONE)
04537             PALDevDataSet(Dev, SequenceConfig, SequenceConfigNew);
04538 
04539 
04540         /* Recalculate timing budget */
04541         if (Status == VL53L0X_ERROR_NONE) {
04542             VL53L0X_GETPARAMETERFIELD(Dev,
04543                 MeasurementTimingBudgetMicroSeconds,
04544                 MeasurementTimingBudgetMicroSeconds);
04545 
04546             VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
04547                 MeasurementTimingBudgetMicroSeconds);
04548         }
04549     }
04550 
04551     LOG_FUNCTION_END(Status);
04552 
04553     return Status;
04554 }
04555 
04556 VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId,
04557     uint8_t LimitCheckEnable)
04558 {
04559     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04560     FixPoint1616_t TempFix1616 = 0;
04561     uint8_t LimitCheckEnableInt = 0;
04562     uint8_t LimitCheckDisable = 0;
04563     uint8_t Temp8;
04564 
04565     LOG_FUNCTION_START("");
04566 
04567     if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
04568         Status = VL53L0X_ERROR_INVALID_PARAMS;
04569     } else {
04570         if (LimitCheckEnable == 0) {
04571             TempFix1616 = 0;
04572             LimitCheckEnableInt = 0;
04573             LimitCheckDisable = 1;
04574 
04575         } else {
04576             VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
04577                 LimitCheckId, TempFix1616);
04578             LimitCheckDisable = 0;
04579             /* this to be sure to have either 0 or 1 */
04580             LimitCheckEnableInt = 1;
04581         }
04582 
04583         switch (LimitCheckId) {
04584 
04585         case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
04586             /* internal computation: */
04587             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
04588                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
04589                 LimitCheckEnableInt);
04590 
04591             break;
04592 
04593         case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
04594 
04595             Status = VL53L0X_WrWord(Dev,
04596             VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
04597                 VL53L0X_FIXPOINT1616TOFIXPOINT97(TempFix1616));
04598 
04599             break;
04600 
04601         case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
04602 
04603             /* internal computation: */
04604             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
04605                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
04606                 LimitCheckEnableInt);
04607 
04608             break;
04609 
04610         case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
04611 
04612             /* internal computation: */
04613             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
04614                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
04615                 LimitCheckEnableInt);
04616 
04617             break;
04618 
04619         case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
04620 
04621             Temp8 = (uint8_t)(LimitCheckDisable << 1);
04622             Status = VL53L0X_UpdateByte(Dev,
04623                 VL53L0X_REG_MSRC_CONFIG_CONTROL,
04624                 0xFE, Temp8);
04625 
04626             break;
04627 
04628         case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
04629 
04630             Temp8 = (uint8_t)(LimitCheckDisable << 4);
04631             Status = VL53L0X_UpdateByte(Dev,
04632                 VL53L0X_REG_MSRC_CONFIG_CONTROL,
04633                 0xEF, Temp8);
04634 
04635             break;
04636 
04637 
04638         default:
04639             Status = VL53L0X_ERROR_INVALID_PARAMS;
04640 
04641         }
04642 
04643     }
04644 
04645     if (Status == VL53L0X_ERROR_NONE) {
04646         if (LimitCheckEnable == 0) {
04647             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
04648                 LimitCheckId, 0);
04649         } else {
04650             VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
04651                 LimitCheckId, 1);
04652         }
04653     }
04654 
04655     LOG_FUNCTION_END(Status);
04656     return Status;
04657 }
04658 
04659 VL53L0X_Error VL53L0X::VL53L0X_StaticInit(VL53L0X_DEV Dev)
04660 {
04661     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04662     VL53L0X_DeviceParameters_t CurrentParameters = {0};
04663     uint8_t *pTuningSettingBuffer;
04664     uint16_t tempword = 0;
04665     uint8_t tempbyte = 0;
04666     uint8_t UseInternalTuningSettings = 0;
04667     uint32_t count = 0;
04668     uint8_t isApertureSpads = 0;
04669     uint32_t refSpadCount = 0;
04670     uint8_t ApertureSpads = 0;
04671     uint8_t vcselPulsePeriodPCLK;
04672     uint32_t seqTimeoutMicroSecs;
04673 
04674     LOG_FUNCTION_START("");
04675 
04676     Status = VL53L0X_get_info_from_device(Dev, 1);
04677 
04678     /* set the ref spad from NVM */
04679     count   = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
04680         ReferenceSpadCount);
04681     ApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
04682         ReferenceSpadType);
04683 
04684     /* NVM value invalid */
04685     if ((ApertureSpads > 1) ||
04686         ((ApertureSpads == 1) && (count > 32)) ||
04687         ((ApertureSpads == 0) && (count > 12)))
04688         Status = VL53L0X_perform_ref_spad_management(Dev, &refSpadCount,
04689             &isApertureSpads);
04690     else
04691         Status = VL53L0X_set_reference_spads(Dev, count, ApertureSpads);
04692 
04693 
04694     /* Initialize tuning settings buffer to prevent compiler warning. */
04695     pTuningSettingBuffer = DefaultTuningSettings;
04696 
04697     if (Status == VL53L0X_ERROR_NONE) {
04698         UseInternalTuningSettings = PALDevDataGet(Dev,
04699             UseInternalTuningSettings);
04700 
04701         if (UseInternalTuningSettings == 0)
04702             pTuningSettingBuffer = PALDevDataGet(Dev,
04703                 pTuningSettingsPointer);
04704         else
04705             pTuningSettingBuffer = DefaultTuningSettings;
04706 
04707     }
04708 
04709     if (Status == VL53L0X_ERROR_NONE)
04710         Status = VL53L0X_load_tuning_settings(Dev, pTuningSettingBuffer);
04711 
04712 
04713     /* Set interrupt config to new sample ready */
04714     if (Status == VL53L0X_ERROR_NONE) {
04715         Status = VL53L0X_SetGpioConfig(Dev, 0, 0,
04716         VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
04717         VL53L0X_INTERRUPTPOLARITY_LOW);
04718     }
04719 
04720     if (Status == VL53L0X_ERROR_NONE) {
04721         Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
04722         Status |= VL53L0X_RdWord(Dev, 0x84, &tempword);
04723         Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
04724     }
04725 
04726     if (Status == VL53L0X_ERROR_NONE) {
04727         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, OscFrequencyMHz,
04728             VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword));
04729     }
04730 
04731     /* After static init, some device parameters may be changed,
04732      * so update them */
04733     if (Status == VL53L0X_ERROR_NONE)
04734         Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters);
04735 
04736 
04737     if (Status == VL53L0X_ERROR_NONE) {
04738         Status = VL53L0X_GetFractionEnable(Dev, &tempbyte);
04739         if (Status == VL53L0X_ERROR_NONE)
04740             PALDevDataSet(Dev, RangeFractionalEnable, tempbyte);
04741 
04742     }
04743 
04744     if (Status == VL53L0X_ERROR_NONE)
04745         PALDevDataSet(Dev, CurrentParameters, CurrentParameters);
04746 
04747 
04748     /* read the sequence config and save it */
04749     if (Status == VL53L0X_ERROR_NONE) {
04750         Status = VL53L0X_RdByte(Dev,
04751         VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
04752         if (Status == VL53L0X_ERROR_NONE)
04753             PALDevDataSet(Dev, SequenceConfig, tempbyte);
04754 
04755     }
04756 
04757     /* Disable MSRC and TCC by default */
04758     if (Status == VL53L0X_ERROR_NONE)
04759         Status = VL53L0X_SetSequenceStepEnable(Dev,
04760                     VL53L0X_SEQUENCESTEP_TCC, 0);
04761 
04762 
04763     if (Status == VL53L0X_ERROR_NONE)
04764         Status = VL53L0X_SetSequenceStepEnable(Dev,
04765         VL53L0X_SEQUENCESTEP_MSRC, 0);
04766 
04767 
04768     /* Set PAL State to standby */
04769     if (Status == VL53L0X_ERROR_NONE)
04770         PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
04771 
04772 
04773 
04774     /* Store pre-range vcsel period */
04775     if (Status == VL53L0X_ERROR_NONE) {
04776         Status = VL53L0X_GetVcselPulsePeriod(
04777             Dev,
04778             VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04779             &vcselPulsePeriodPCLK);
04780     }
04781 
04782     if (Status == VL53L0X_ERROR_NONE) {
04783             VL53L0X_SETDEVICESPECIFICPARAMETER(
04784                 Dev,
04785                 PreRangeVcselPulsePeriod,
04786                 vcselPulsePeriodPCLK);
04787     }
04788 
04789     /* Store final-range vcsel period */
04790     if (Status == VL53L0X_ERROR_NONE) {
04791         Status = VL53L0X_GetVcselPulsePeriod(
04792             Dev,
04793             VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
04794             &vcselPulsePeriodPCLK);
04795     }
04796 
04797     if (Status == VL53L0X_ERROR_NONE) {
04798             VL53L0X_SETDEVICESPECIFICPARAMETER(
04799                 Dev,
04800                 FinalRangeVcselPulsePeriod,
04801                 vcselPulsePeriodPCLK);
04802     }
04803 
04804     /* Store pre-range timeout */
04805     if (Status == VL53L0X_ERROR_NONE) {
04806         Status = get_sequence_step_timeout(
04807             Dev,
04808             VL53L0X_SEQUENCESTEP_PRE_RANGE,
04809             &seqTimeoutMicroSecs);
04810     }
04811 
04812     if (Status == VL53L0X_ERROR_NONE) {
04813         VL53L0X_SETDEVICESPECIFICPARAMETER(
04814             Dev,
04815             PreRangeTimeoutMicroSecs,
04816             seqTimeoutMicroSecs);
04817     }
04818 
04819     /* Store final-range timeout */
04820     if (Status == VL53L0X_ERROR_NONE) {
04821         Status = get_sequence_step_timeout(
04822             Dev,
04823             VL53L0X_SEQUENCESTEP_FINAL_RANGE,
04824             &seqTimeoutMicroSecs);
04825     }
04826 
04827     if (Status == VL53L0X_ERROR_NONE) {
04828         VL53L0X_SETDEVICESPECIFICPARAMETER(
04829             Dev,
04830             FinalRangeTimeoutMicroSecs,
04831             seqTimeoutMicroSecs);
04832     }
04833 
04834     LOG_FUNCTION_END(Status);
04835     return Status;
04836 }
04837 
04838 
04839 VL53L0X_Error VL53L0X::VL53L0X_StopMeasurement(VL53L0X_DEV Dev)
04840 {
04841     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04842     LOG_FUNCTION_START("");
04843 
04844     Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
04845     VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
04846 
04847     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
04848     Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
04849     Status = VL53L0X_WrByte(Dev, 0x91, 0x00);
04850     Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
04851     Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
04852 
04853     if (Status == VL53L0X_ERROR_NONE) {
04854         /* Set PAL State to Idle */
04855         PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
04856     }
04857 
04858     /* Check if need to apply interrupt settings */
04859     if (Status == VL53L0X_ERROR_NONE)
04860         Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 0);
04861 
04862     LOG_FUNCTION_END(Status);
04863     return Status;
04864 }
04865 
04866 VL53L0X_Error VL53L0X::VL53L0X_GetStopCompletedStatus(VL53L0X_DEV Dev,
04867     uint32_t *pStopStatus)
04868 {
04869     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
04870     uint8_t Byte = 0;
04871     LOG_FUNCTION_START("");
04872 
04873     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
04874 
04875     if (Status == VL53L0X_ERROR_NONE)
04876         Status = VL53L0X_RdByte(Dev, 0x04, &Byte);
04877 
04878     if (Status == VL53L0X_ERROR_NONE)
04879         Status = VL53L0X_WrByte(Dev, 0xFF, 0x0);
04880 
04881     *pStopStatus = Byte;
04882 
04883     if (Byte == 0) {
04884         Status = VL53L0X_WrByte(Dev, 0x80, 0x01);
04885         Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
04886         Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
04887         Status = VL53L0X_WrByte(Dev, 0x91,
04888             PALDevDataGet(Dev, StopVariable));
04889         Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
04890         Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
04891         Status = VL53L0X_WrByte(Dev, 0x80, 0x00);
04892     }
04893 
04894     LOG_FUNCTION_END(Status);
04895     return Status;
04896 }
04897 
04898 /****************** Write and read functions from I2C *************************/
04899 
04900 VL53L0X_Error VL53L0X::VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
04901 {
04902    int  status;
04903  
04904    status = VL53L0X_I2CWrite(Dev->I2cDevAddr , index, pdata, (uint16_t)count);
04905    return status;
04906 }
04907 
04908 VL53L0X_Error VL53L0X::VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
04909 {
04910     int status;
04911 
04912     if (count>=VL53L0X_MAX_I2C_XFER_SIZE){
04913         status = VL53L0X_ERROR_INVALID_PARAMS;
04914     }
04915 
04916     status = VL53L0X_I2CRead(Dev->I2cDevAddr , index, pdata, (uint16_t)count);
04917 
04918     return status;
04919 }
04920 
04921 
04922 VL53L0X_Error VL53L0X::VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
04923 {
04924    int  status;
04925  
04926    status=VL53L0X_I2CWrite(Dev->I2cDevAddr , index, &data, 1);
04927    return status;
04928 }
04929  
04930 VL53L0X_Error VL53L0X::VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
04931 {
04932    int  status;
04933    uint8_t buffer[2];
04934     
04935      buffer[0] = data >> 8;
04936      buffer[1] = data & 0x00FF;
04937    status=VL53L0X_I2CWrite(Dev->I2cDevAddr , index, (uint8_t *)buffer, 2);
04938    return status;
04939 }
04940  
04941 VL53L0X_Error VL53L0X::VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
04942 {
04943    int  status;
04944    uint8_t buffer[4];
04945     
04946      buffer[0] = (data >> 24) & 0xFF;
04947      buffer[1] = (data >> 16) & 0xFF;
04948      buffer[2] = (data >>  8) & 0xFF;
04949      buffer[3] = (data >>  0) & 0xFF;
04950    status=VL53L0X_I2CWrite(Dev->I2cDevAddr , index, (uint8_t *)buffer, 4);
04951    return status;
04952 }
04953 
04954 
04955 VL53L0X_Error VL53L0X::VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
04956 {
04957    int  status;
04958 
04959    status = VL53L0X_I2CRead(Dev->I2cDevAddr , index, data, 1);
04960 
04961    if(status)
04962      return -1;
04963 
04964    return 0;
04965 }
04966  
04967 VL53L0X_Error VL53L0X::VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
04968 {
04969    int  status;
04970    uint8_t buffer[2] = {0,0};
04971 
04972    status = VL53L0X_I2CRead(Dev->I2cDevAddr , index, buffer, 2);
04973    if (!status)
04974    {
04975        *data = (buffer[0] << 8) + buffer[1];
04976    }
04977    return status;
04978 
04979 }
04980  
04981 VL53L0X_Error VL53L0X::VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
04982 {
04983    int status;
04984    uint8_t buffer[4] = {0,0,0,0};
04985 
04986    status = VL53L0X_I2CRead(Dev->I2cDevAddr , index, buffer, 4);
04987    if(!status)
04988    {
04989        *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
04990    }
04991    return status;
04992 
04993 }
04994 
04995 VL53L0X_Error VL53L0X::VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData)
04996 {
04997    int  status;
04998    uint8_t buffer = 0;
04999 
05000    /* read data direct onto buffer */
05001    status = VL53L0X_I2CRead(Dev->I2cDevAddr , index, &buffer,1);
05002    if (!status)
05003    {
05004       buffer = (buffer & AndData) | OrData;
05005       status = VL53L0X_I2CWrite(Dev->I2cDevAddr , index, &buffer, (uint8_t)1);
05006    }
05007    return status;
05008 }
05009  
05010 VL53L0X_Error VL53L0X::VL53L0X_I2CWrite(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
05011 {
05012    int ret;
05013 
05014    ret = dev_i2c.i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
05015 
05016    if(ret) 
05017       return -1;
05018    return 0;
05019 }
05020  
05021 VL53L0X_Error VL53L0X::VL53L0X_I2CRead(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
05022 {
05023    int ret;
05024 
05025    ret = dev_i2c.i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
05026     
05027    if(ret) 
05028       return -1;
05029    return 0;
05030 } 
05031 
05032 int VL53L0X::read_id(uint8_t *id)
05033 {
05034     int status = 0;
05035     uint16_t rl_id=0;
05036     
05037     status = VL53L0X_RdWord(Device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id);
05038     if (rl_id == 0xEEAA)
05039         return status;
05040         
05041     return -1;
05042 }
05043 
05044 int VL53L0X::ReadID(uint8_t *id)
05045 {
05046     return read_id(id);
05047 }
05048  
05049  
05050 VL53L0X_Error VL53L0X::WaitMeasurementDataReady(VL53L0X_DEV Dev)
05051 {
05052     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
05053     uint8_t NewDatReady=0;
05054     uint32_t LoopNb;
05055 
05056     // Wait until it finished
05057     // use timeout to avoid deadlock
05058     if (Status == VL53L0X_ERROR_NONE) {
05059         LoopNb = 0;
05060         do {
05061             Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
05062             if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) {
05063                 break;
05064             }
05065             LoopNb = LoopNb + 1;
05066             VL53L0X_PollingDelay(Dev);
05067         } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
05068 
05069         if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
05070             Status = VL53L0X_ERROR_TIME_OUT;
05071         }
05072     }
05073 
05074     return Status;
05075 }
05076 
05077 VL53L0X_Error VL53L0X::WaitStopCompleted(VL53L0X_DEV Dev)
05078 {
05079     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
05080     uint32_t StopCompleted=0;
05081     uint32_t LoopNb;
05082 
05083     // Wait until it finished
05084     // use timeout to avoid deadlock
05085     if (Status == VL53L0X_ERROR_NONE) {
05086         LoopNb = 0;
05087         do {
05088             Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
05089             if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) {
05090                 break;
05091             }
05092             LoopNb = LoopNb + 1;
05093             VL53L0X_PollingDelay(Dev);
05094         } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
05095 
05096         if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
05097             Status = VL53L0X_ERROR_TIME_OUT;
05098         }
05099 
05100     }
05101 
05102     return Status;
05103 }
05104 
05105 
05106 int VL53L0X::InitSensor(uint8_t NewAddr)
05107 {
05108    int status;
05109 
05110    VL53L0X_Off();
05111    VL53L0X_On();
05112 
05113 //   status=VL53L0X_WaitDeviceBooted(Device);
05114 //   if(status)
05115 //      printf("WaitDeviceBooted fail\n\r");
05116    status=IsPresent();
05117    if(!status)
05118    {
05119       status=Init(&MyDevice);
05120       if(status != VL53L0X_ERROR_NONE)
05121       {
05122            printf("Failed to init VL53L0X sensor!\n\r");
05123            return status;
05124       }
05125 
05126       // deduce silicon version
05127       status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
05128 
05129             
05130       status=Prepare();
05131       if(status != VL53L0X_ERROR_NONE)
05132       {
05133          printf("Failed to prepare VL53L0X!\n\r");
05134          return status;
05135       }
05136 
05137       if(NewAddr!=DEFAULT_DEVICE_ADDRESS)
05138       {
05139          status=SetDeviceAddress(NewAddr);
05140          if(status)
05141          {
05142             printf("Failed to change I2C address!\n\r");
05143             return status;
05144          }
05145       }
05146       else
05147       {
05148          printf("Invalid new address!\n\r");
05149          return VL53L0X_ERROR_INVALID_PARAMS;
05150       }
05151    }
05152    return status; 
05153 }
05154 
05155 int VL53L0X::RangeMeasIntContinuousMode(void (*fptr)(void))
05156 {
05157    int status, ClrStatus;
05158 
05159    status = VL53L0X_SetGpioConfig(Device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
05160         VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
05161         VL53L0X_INTERRUPTPOLARITY_HIGH);
05162    
05163    if (!status)
05164    {   
05165         AttachInterruptMeasureDetectionIRQ(fptr);
05166         EnableInterruptMeasureDetectionIRQ();
05167    }
05168     
05169    ClrStatus=ClearInterrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS|VL53L0X_REG_RESULT_RANGE_STATUS);
05170    if(ClrStatus)
05171        VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
05172    if(!status)
05173    {
05174       status=RangeStartContinuousMode();
05175    }
05176    return status;
05177 }
05178 
05179 
05180 int VL53L0X::StartMeasurement(OperatingMode operating_mode, void (*fptr)(void))
05181 {
05182     int Status = VL53L0X_ERROR_NONE;
05183 
05184     uint8_t VhvSettings;
05185     uint8_t PhaseCal;
05186     // *** from mass market cube expansion v1.1, ranging with satellites.
05187     // default settings, for normal range.
05188     FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536);
05189     FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536);
05190     uint32_t timingBudget = 33000;
05191     uint8_t preRangeVcselPeriod = 14;
05192     uint8_t finalRangeVcselPeriod = 10;
05193 
05194     if (operating_mode == range_continuous_interrupt)
05195     {
05196         if (gpio1Int==NULL) 
05197         {
05198             printf ("GPIO1 Error\r\n");
05199             return 1;
05200         }
05201         printf ("Setting up GPIO Config\r\n");
05202         Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
05203         
05204         if (Status == VL53L0X_ERROR_NONE) {
05205             Status = RangeConfigInterrupt(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
05206         }
05207         if (Status == VL53L0X_ERROR_NONE) {
05208             Status = RangeMeasIntContinuousMode(fptr);
05209         }
05210                         
05211         if(Status == VL53L0X_ERROR_NONE)
05212         {
05213             printf ("Call of VL53L0X_StartMeasurement\n");
05214             Status = VL53L0X_StartMeasurement(Device);
05215         }        
05216             return Status;
05217     }
05218 
05219     if (operating_mode == range_single_shot_polling)
05220     {
05221         // singelshot, polled ranging
05222         if(Status == VL53L0X_ERROR_NONE)
05223         {
05224             // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
05225             Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
05226         }
05227 
05228         // Enable/Disable Sigma and Signal check
05229         if (Status == VL53L0X_ERROR_NONE) {
05230             Status = VL53L0X_SetLimitCheckEnable(Device,
05231                     VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
05232         }
05233         if (Status == VL53L0X_ERROR_NONE) {
05234             Status = VL53L0X_SetLimitCheckEnable(Device,
05235                     VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
05236         }
05237 
05238 // *** from mass market cube expansion v1.1, ranging with satellites.
05239         /* Ranging configuration */
05240 //*
05241 //        switch(rangingConfig) {
05242 //        case LONG_RANGE:
05243             signalLimit = (FixPoint1616_t)(0.1*65536);
05244             sigmaLimit = (FixPoint1616_t)(60*65536);
05245             timingBudget = 33000;
05246             preRangeVcselPeriod = 18;
05247             finalRangeVcselPeriod = 14;
05248 /*          break;
05249         case HIGH_ACCURACY:
05250             signalLimit = (FixPoint1616_t)(0.25*65536);
05251             sigmaLimit = (FixPoint1616_t)(18*65536);
05252             timingBudget = 200000;
05253             preRangeVcselPeriod = 14;
05254             finalRangeVcselPeriod = 10;
05255             break;
05256         case HIGH_SPEED:
05257             signalLimit = (FixPoint1616_t)(0.25*65536);
05258             sigmaLimit = (FixPoint1616_t)(32*65536);
05259             timingBudget = 20000;
05260             preRangeVcselPeriod = 14;
05261             finalRangeVcselPeriod = 10;
05262             break;
05263         default:
05264             debug_printf("Not Supported");
05265         }
05266 */
05267 
05268         if (Status == VL53L0X_ERROR_NONE) {
05269             Status = VL53L0X_SetLimitCheckValue(Device,
05270                             VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
05271         }
05272 
05273         if (Status == VL53L0X_ERROR_NONE) {
05274             Status = VL53L0X_SetLimitCheckValue(Device,
05275                             VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
05276         }
05277 
05278         if (Status == VL53L0X_ERROR_NONE) {
05279             Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Device, timingBudget);
05280         }
05281 
05282         if (Status == VL53L0X_ERROR_NONE) {
05283             Status = VL53L0X_SetVcselPulsePeriod(Device,
05284                             VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
05285         }
05286 
05287         if (Status == VL53L0X_ERROR_NONE) {
05288             Status = VL53L0X_SetVcselPulsePeriod(Device,
05289                             VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
05290         }
05291 
05292         if (Status == VL53L0X_ERROR_NONE) {
05293             Status = VL53L0X_PerformRefCalibration(Device, &VhvSettings, &PhaseCal);
05294         }
05295 
05296     }
05297 
05298     if (operating_mode == range_continuous_polling)
05299     {
05300         if(Status == VL53L0X_ERROR_NONE)
05301         {
05302             printf ("Call of VL53L0X_SetDeviceMode\n");
05303             Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
05304         }
05305 
05306         if(Status == VL53L0X_ERROR_NONE)
05307         {
05308             printf ("Call of VL53L0X_StartMeasurement\n");
05309             Status = VL53L0X_StartMeasurement(Device);
05310         }
05311     }
05312 
05313     return Status;
05314 }
05315  
05316  
05317 int VL53L0X::GetMeasurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *Data)
05318 {
05319     int Status = VL53L0X_ERROR_NONE;
05320 
05321     if (operating_mode == range_single_shot_polling)
05322     {
05323         Status = VL53L0X_PerformSingleRangingMeasurement(Device, Data);
05324     }
05325 
05326     if (operating_mode == range_continuous_polling)
05327     {
05328         if (Status == VL53L0X_ERROR_NONE)
05329             Status = VL53L0X_measurement_poll_for_completion(Device);
05330 
05331         if(Status == VL53L0X_ERROR_NONE)
05332         {
05333             Status = VL53L0X_GetRangingMeasurementData(Device, Data);
05334 
05335         // Clear the interrupt
05336             VL53L0X_ClearInterruptMask(Device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
05337             VL53L0X_PollingDelay(Device);
05338         }
05339     }
05340 
05341     if (operating_mode == range_continuous_interrupt)
05342     {
05343         Status = VL53L0X_GetRangingMeasurementData(Device, Data);
05344         VL53L0X_ClearInterruptMask(Device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS);      
05345     }
05346 
05347     return Status;
05348 }
05349      
05350 
05351 int VL53L0X::StopMeasurement(OperatingMode operating_mode)
05352 {
05353     int status = VL53L0X_ERROR_NONE;
05354 
05355 
05356     // don't need to stop for a singleshot range!
05357     if (operating_mode==range_single_shot_polling)
05358     {
05359     }
05360 
05361     if (operating_mode==range_continuous_interrupt || operating_mode==range_continuous_polling)
05362     {
05363     // continuous mode
05364         if(status == VL53L0X_ERROR_NONE)
05365         {
05366             printf ("Call of VL53L0X_StopMeasurement\n");
05367             status = VL53L0X_StopMeasurement(Device);
05368         }
05369 
05370         if(status == VL53L0X_ERROR_NONE)
05371         {
05372             printf ("Wait Stop to be competed\n");
05373             status = WaitStopCompleted(Device);
05374         }
05375 
05376         if(status == VL53L0X_ERROR_NONE)
05377             status = VL53L0X_ClearInterruptMask(Device,
05378                              VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
05379     }
05380 
05381     return status;
05382 }
05383 
05384 
05385 int VL53L0X::HandleIRQ(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *Data)
05386 {
05387    int status;
05388    status=GetMeasurement(operating_mode, Data);
05389    EnableInterruptMeasureDetectionIRQ();
05390    return status;
05391 }
05392      
05393  
05394  
05395 /******************************************************************************/
05396 
05397 
05398 
05399 
05400