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