vl53l0x_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    vl53l0x_class.cpp
00004  * @author  IMG
00005  * @version V0.0.1
00006  * @date    28-June-2016
00007  * @brief   Implementation file for the VL53L0X driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
00012  *
00013  * Redistribution and use in source and binary forms, with or without modification,
00014  * are permitted provided that the following conditions are met:
00015  *   1. Redistributions of source code must retain the above copyright notice,
00016  *      this list of conditions and the following disclaimer.
00017  *   2. Redistributions in binary form must reproduce the above copyright notice,
00018  *      this list of conditions and the following disclaimer in the documentation
00019  *      and/or other materials provided with the distribution.
00020  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021  *      may be used to endorse or promote products derived from this software
00022  *      without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  ******************************************************************************
00036 */
00037 
00038 /* Includes */
00039 #include <stdlib.h>
00040 
00041 #include "vl53l0x_class.h"
00042 
00043 //#include "vl53l0x_api_core.h"
00044 //#include "vl53l0x_api_calibration.h"
00045 //#include "vl53l0x_api_strings.h"
00046 #include "vl53l0x_interrupt_threshold_settings.h"
00047 #include "vl53l0x_tuning.h"
00048 #include "vl53l0x_types.h"
00049 
00050 
00051 /****************** define for i2c configuration *******************************/
00052 
00053 #define TEMP_BUF_SIZE   64
00054 
00055 /** Maximum buffer size to be used in i2c */
00056 #define VL53L0X_MAX_I2C_XFER_SIZE   64 /* Maximum buffer size to be used in i2c */
00057 #define VL53L0X_I2C_USER_VAR         /* none but could be for a flag var to get/pass to mutex interruptible  return flags and try again */
00058 
00059 
00060 #define LOG_FUNCTION_START(fmt, ...) \
00061     _LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
00062 #define LOG_FUNCTION_END(status, ...) \
00063     _LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
00064 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00065     _LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
00066 
00067 #ifdef VL53L0X_LOG_ENABLE
00068 #define trace_print(level, ...) trace_print_module_function(TRACE_MODULE_API, \
00069     level, TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00070 #endif
00071 
00072 #define REF_ARRAY_SPAD_0  0
00073 #define REF_ARRAY_SPAD_5  5
00074 #define REF_ARRAY_SPAD_10 10
00075 
00076 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5,
00077                                  REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5
00078                                 };
00079 
00080 
00081 
00082 
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             loop_nb = loop_nb + 1;
00102         } while ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP );
00103 
00104         if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP )
00105             status = VL53L0X_ERROR_TIME_OUT;
00106 
00107     }
00108 
00109     status |= vl53l0x_write_byte( dev, 0x83, 0x01 );
00110 
00111     LOG_FUNCTION_END( status );
00112     return status;
00113 
00114 }
00115 
00116 VL53L0X_Error VL53L0X::vl53l0x_get_info_from_device( VL53L0X_DEV dev, uint8_t option )
00117 {
00118 
00119     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00120     uint8_t byte;
00121     uint32_t tmp_dword;
00122     uint8_t module_id;
00123     uint8_t revision;
00124     uint8_t reference_spad_count = 0;
00125     uint8_t reference_spad_type = 0;
00126     uint32_t part_uid_upper = 0;
00127     uint32_t part_uid_lower = 0;
00128     uint32_t offset_fixed1104_mm = 0;
00129     int16_t offset_micro_meters = 0;
00130     uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
00131     uint32_t dist_meas_fixed1104_400_mm = 0;
00132     uint32_t signal_rate_meas_fixed1104_400_mm = 0;
00133     char product_id[19];
00134     char *product_id_tmp;
00135     uint8_t read_data_from_device_done;
00136     FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0;
00137     uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE];
00138     int i;
00139 
00140 
00141     LOG_FUNCTION_START( "" );
00142 
00143     read_data_from_device_done = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
00144                                  ReadDataFromDeviceDone );
00145 
00146     /* This access is done only once after that a GetDeviceInfo or
00147      * datainit is done*/
00148     if ( read_data_from_device_done != 7 ) {
00149 
00150         status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
00151         status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
00152         status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
00153 
00154         status |= vl53l0x_write_byte( dev, 0xFF, 0x06 );
00155         status |= vl53l0x_read_byte( dev, 0x83, &byte );
00156         status |= vl53l0x_write_byte( dev, 0x83, byte | 4 );
00157         status |= vl53l0x_write_byte( dev, 0xFF, 0x07 );
00158         status |= vl53l0x_write_byte( dev, 0x81, 0x01 );
00159 
00160         status |= vl53l0x_polling_delay( dev );
00161 
00162         status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
00163 
00164         if ( ( ( option & 1 ) == 1 ) &&
00165                 ( ( read_data_from_device_done & 1 ) == 0 ) ) {
00166             status |= vl53l0x_write_byte( dev, 0x94, 0x6b );
00167             status |= vl53l0x_device_read_strobe( dev );
00168             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00169 
00170             reference_spad_count = ( uint8_t )( ( tmp_dword >> 8 ) & 0x07f );
00171             reference_spad_type  = ( uint8_t )( ( tmp_dword >> 15 ) & 0x01 );
00172 
00173             status |= vl53l0x_write_byte( dev, 0x94, 0x24 );
00174             status |= vl53l0x_device_read_strobe( dev );
00175             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00176 
00177 
00178             nvm_ref_good_spad_map[0] = ( uint8_t )( ( tmp_dword >> 24 )
00179                                                     & 0xff );
00180             nvm_ref_good_spad_map[1] = ( uint8_t )( ( tmp_dword >> 16 )
00181                                                     & 0xff );
00182             nvm_ref_good_spad_map[2] = ( uint8_t )( ( tmp_dword >> 8 )
00183                                                     & 0xff );
00184             nvm_ref_good_spad_map[3] = ( uint8_t )( tmp_dword & 0xff );
00185 
00186             status |= vl53l0x_write_byte( dev, 0x94, 0x25 );
00187             status |= vl53l0x_device_read_strobe( dev );
00188             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00189 
00190             nvm_ref_good_spad_map[4] = ( uint8_t )( ( tmp_dword >> 24 )
00191                                                     & 0xff );
00192             nvm_ref_good_spad_map[5] = ( uint8_t )( ( tmp_dword >> 16 )
00193                                                     & 0xff );
00194         }
00195 
00196         if ( ( ( option & 2 ) == 2 ) &&
00197                 ( ( read_data_from_device_done & 2 ) == 0 ) ) {
00198 
00199             status |= vl53l0x_write_byte( dev, 0x94, 0x02 );
00200             status |= vl53l0x_device_read_strobe( dev );
00201             status |= vl53l0x_read_byte( dev, 0x90, &module_id );
00202 
00203             status |= vl53l0x_write_byte( dev, 0x94, 0x7B );
00204             status |= vl53l0x_device_read_strobe( dev );
00205             status |= vl53l0x_read_byte( dev, 0x90, &revision );
00206 
00207             status |= vl53l0x_write_byte( dev, 0x94, 0x77 );
00208             status |= vl53l0x_device_read_strobe( dev );
00209             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00210 
00211             product_id[0] = ( char )( ( tmp_dword >> 25 ) & 0x07f );
00212             product_id[1] = ( char )( ( tmp_dword >> 18 ) & 0x07f );
00213             product_id[2] = ( char )( ( tmp_dword >> 11 ) & 0x07f );
00214             product_id[3] = ( char )( ( tmp_dword >> 4 ) & 0x07f );
00215 
00216             byte = ( uint8_t )( ( tmp_dword & 0x00f ) << 3 );
00217 
00218             status |= vl53l0x_write_byte( dev, 0x94, 0x78 );
00219             status |= vl53l0x_device_read_strobe( dev );
00220             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00221 
00222             product_id[4] = ( char )( byte +
00223                                       ( ( tmp_dword >> 29 ) & 0x07f ) );
00224             product_id[5] = ( char )( ( tmp_dword >> 22 ) & 0x07f );
00225             product_id[6] = ( char )( ( tmp_dword >> 15 ) & 0x07f );
00226             product_id[7] = ( char )( ( tmp_dword >> 8 ) & 0x07f );
00227             product_id[8] = ( char )( ( tmp_dword >> 1 ) & 0x07f );
00228 
00229             byte = ( uint8_t )( ( tmp_dword & 0x001 ) << 6 );
00230 
00231             status |= vl53l0x_write_byte( dev, 0x94, 0x79 );
00232 
00233             status |= vl53l0x_device_read_strobe( dev );
00234 
00235             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00236 
00237             product_id[9] = ( char )( byte +
00238                                       ( ( tmp_dword >> 26 ) & 0x07f ) );
00239             product_id[10] = ( char )( ( tmp_dword >> 19 ) & 0x07f );
00240             product_id[11] = ( char )( ( tmp_dword >> 12 ) & 0x07f );
00241             product_id[12] = ( char )( ( tmp_dword >> 5 ) & 0x07f );
00242 
00243             byte = ( uint8_t )( ( tmp_dword & 0x01f ) << 2 );
00244 
00245             status |= vl53l0x_write_byte( dev, 0x94, 0x7A );
00246 
00247             status |= vl53l0x_device_read_strobe( dev );
00248 
00249             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00250 
00251             product_id[13] = ( char )( byte +
00252                                        ( ( tmp_dword >> 30 ) & 0x07f ) );
00253             product_id[14] = ( char )( ( tmp_dword >> 23 ) & 0x07f );
00254             product_id[15] = ( char )( ( tmp_dword >> 16 ) & 0x07f );
00255             product_id[16] = ( char )( ( tmp_dword >> 9 ) & 0x07f );
00256             product_id[17] = ( char )( ( tmp_dword >> 2 ) & 0x07f );
00257             product_id[18] = '\0';
00258 
00259         }
00260 
00261         if ( ( ( option & 4 ) == 4 ) &&
00262                 ( ( read_data_from_device_done & 4 ) == 0 ) ) {
00263 
00264             status |= vl53l0x_write_byte( dev, 0x94, 0x7B );
00265             status |= vl53l0x_device_read_strobe( dev );
00266             status |= vl53l0x_read_dword( dev, 0x90, &part_uid_upper );
00267 
00268             status |= vl53l0x_write_byte( dev, 0x94, 0x7C );
00269             status |= vl53l0x_device_read_strobe( dev );
00270             status |= vl53l0x_read_dword( dev, 0x90, &part_uid_lower );
00271 
00272             status |= vl53l0x_write_byte( dev, 0x94, 0x73 );
00273             status |= vl53l0x_device_read_strobe( dev );
00274             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00275 
00276             signal_rate_meas_fixed1104_400_mm = ( tmp_dword &
00277                                                   0x0000000ff ) << 8;
00278 
00279             status |= vl53l0x_write_byte( dev, 0x94, 0x74 );
00280             status |= vl53l0x_device_read_strobe( dev );
00281             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00282 
00283             signal_rate_meas_fixed1104_400_mm |= ( ( tmp_dword &
00284                                                    0xff000000 ) >> 24 );
00285 
00286             status |= vl53l0x_write_byte( dev, 0x94, 0x75 );
00287             status |= vl53l0x_device_read_strobe( dev );
00288             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00289 
00290             dist_meas_fixed1104_400_mm = ( tmp_dword & 0x0000000ff )
00291                                          << 8;
00292 
00293             status |= vl53l0x_write_byte( dev, 0x94, 0x76 );
00294             status |= vl53l0x_device_read_strobe( dev );
00295             status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
00296 
00297             dist_meas_fixed1104_400_mm |= ( ( tmp_dword & 0xff000000 )
00298                                             >> 24 );
00299         }
00300 
00301         status |= vl53l0x_write_byte( dev, 0x81, 0x00 );
00302         status |= vl53l0x_write_byte( dev, 0xFF, 0x06 );
00303         status |= vl53l0x_read_byte( dev, 0x83, &byte );
00304         status |= vl53l0x_write_byte( dev, 0x83, byte & 0xfb );
00305         status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
00306         status |= vl53l0x_write_byte( dev, 0x00, 0x01 );
00307 
00308         status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
00309         status |= vl53l0x_write_byte( dev, 0x80, 0x00 );
00310     }
00311 
00312     if ( ( status == VL53L0X_ERROR_NONE ) &&
00313             ( read_data_from_device_done != 7 ) ) {
00314         /* Assign to variable if status is ok */
00315         if ( ( ( option & 1 ) == 1 ) &&
00316                 ( ( read_data_from_device_done & 1 ) == 0 ) ) {
00317             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00318                                                 ReferenceSpadCount, reference_spad_count );
00319 
00320             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00321                                                 ReferenceSpadType, reference_spad_type );
00322 
00323             for ( i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++ ) {
00324                 dev->Data .SpadData .RefGoodSpadMap [i] =
00325                     nvm_ref_good_spad_map[i];
00326             }
00327         }
00328 
00329         if ( ( ( option & 2 ) == 2 ) &&
00330                 ( ( read_data_from_device_done & 2 ) == 0 ) ) {
00331             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00332                                                 ModuleId, module_id );
00333 
00334             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00335                                                 Revision, revision );
00336 
00337             product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
00338                              ProductId );
00339             VL53L0X_COPYSTRING( product_id_tmp, product_id );
00340 
00341         }
00342 
00343         if ( ( ( option & 4 ) == 4 ) &&
00344                 ( ( read_data_from_device_done & 4 ) == 0 ) ) {
00345             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00346                                                 PartUIDUpper, part_uid_upper );
00347 
00348             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00349                                                 PartUIDLower, part_uid_lower );
00350 
00351             signal_rate_meas_fixed400_mm_fix =
00352                 VL53L0X_FIXPOINT97TOFIXPOINT1616(
00353                     signal_rate_meas_fixed1104_400_mm );
00354 
00355             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
00356                                                 SignalRateMeasFixed400mm,
00357                                                 signal_rate_meas_fixed400_mm_fix );
00358 
00359             offset_micro_meters = 0;
00360             if ( dist_meas_fixed1104_400_mm != 0 ) {
00361                 offset_fixed1104_mm =
00362                     dist_meas_fixed1104_400_mm -
00363                     dist_meas_tgt_fixed1104_mm;
00364                 offset_micro_meters = ( offset_fixed1104_mm
00365                                         * 1000 ) >> 4;
00366                 offset_micro_meters *= -1;
00367             }
00368 
00369             PALDevDataSet( dev,
00370                            Part2PartOffsetAdjustmentNVMMicroMeter,
00371                            offset_micro_meters );
00372         }
00373         byte = ( uint8_t )( read_data_from_device_done | option );
00374         VL53L0X_SETDEVICESPECIFICPARAMETER( dev, ReadDataFromDeviceDone,
00375                                             byte );
00376     }
00377 
00378     LOG_FUNCTION_END( status );
00379     return status;
00380 }
00381 
00382 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_offset_calibration_data_micro_meter( VL53L0X_DEV dev,
00383         int32_t *p_offset_calibration_data_micro_meter )
00384 {
00385     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00386     uint16_t range_offset_register;
00387     int16_t c_max_offset = 2047;
00388     int16_t c_offset_range = 4096;
00389 
00390     /* Note that offset has 10.2 format */
00391 
00392     status = vl53l0x_read_word( dev,
00393                                 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
00394                                 &range_offset_register );
00395 
00396     if ( status == VL53L0X_ERROR_NONE ) {
00397         range_offset_register = ( range_offset_register & 0x0fff );
00398 
00399         /* Apply 12 bit 2's compliment conversion */
00400         if ( range_offset_register > c_max_offset )
00401             *p_offset_calibration_data_micro_meter =
00402                 ( int16_t )( range_offset_register - c_offset_range )
00403                 * 250;
00404         else
00405             *p_offset_calibration_data_micro_meter =
00406                 ( int16_t )range_offset_register * 250;
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 if ( offset_calibration_data_micro_meter < c_min_offset_micro_meter )
00440         offset_calibration_data_micro_meter = c_min_offset_micro_meter;
00441 
00442     /* The offset register is 10.2 format and units are mm
00443      * therefore conversion is applied by a division of
00444      * 250.
00445      */
00446     if ( offset_calibration_data_micro_meter >= 0 ) {
00447         encoded_offset_val =
00448             offset_calibration_data_micro_meter / 250;
00449     } else {
00450         encoded_offset_val =
00451             c_offset_range +
00452             offset_calibration_data_micro_meter / 250;
00453     }
00454 
00455     status = vl53l0x_write_word( dev,
00456                                  VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
00457                                  encoded_offset_val );
00458 
00459     LOG_FUNCTION_END( status );
00460     return status;
00461 }
00462 
00463 VL53L0X_Error VL53L0X::vl53l0x_set_offset_calibration_data_micro_meter( VL53L0X_DEV dev,
00464         int32_t offset_calibration_data_micro_meter )
00465 {
00466     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00467     LOG_FUNCTION_START( "" );
00468 
00469     status = wrapped_vl53l0x_set_offset_calibration_data_micro_meter( dev,
00470              offset_calibration_data_micro_meter );
00471 
00472     LOG_FUNCTION_END( status );
00473     return status;
00474 }
00475 
00476 VL53L0X_Error VL53L0X::vl53l0x_apply_offset_adjustment( VL53L0X_DEV dev )
00477 {
00478     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00479     int32_t corrected_offset_micro_meters;
00480     int32_t current_offset_micro_meters;
00481 
00482     /* if we run on this function we can read all the NVM info
00483      * used by the API */
00484     status = vl53l0x_get_info_from_device( dev, 7 );
00485 
00486     /* Read back current device offset */
00487     if ( status == VL53L0X_ERROR_NONE ) {
00488         status = vl53l0x_get_offset_calibration_data_micro_meter( dev,
00489                  &current_offset_micro_meters );
00490     }
00491 
00492     /* Apply Offset Adjustment derived from 400mm measurements */
00493     if ( status == VL53L0X_ERROR_NONE ) {
00494 
00495         /* Store initial device offset */
00496         PALDevDataSet( dev, Part2PartOffsetNVMMicroMeter,
00497                        current_offset_micro_meters );
00498 
00499         corrected_offset_micro_meters = current_offset_micro_meters +
00500                                         ( int32_t )PALDevDataGet( dev,
00501                                                 Part2PartOffsetAdjustmentNVMMicroMeter );
00502 
00503         status = vl53l0x_set_offset_calibration_data_micro_meter( dev,
00504                  corrected_offset_micro_meters );
00505 
00506         /* store current, adjusted offset */
00507         if ( status == VL53L0X_ERROR_NONE ) {
00508             VL53L0X_SETPARAMETERFIELD( dev, RangeOffsetMicroMeters,
00509                                        corrected_offset_micro_meters );
00510         }
00511     }
00512 
00513     return status;
00514 }
00515 
00516 VL53L0X_Error VL53L0X::vl53l0x_get_device_mode( VL53L0X_DEV dev,
00517         VL53L0X_DeviceModes *p_device_mode )
00518 {
00519     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00520     LOG_FUNCTION_START( "" );
00521 
00522     VL53L0X_GETPARAMETERFIELD( dev, DeviceMode, *p_device_mode );
00523 
00524     LOG_FUNCTION_END( status );
00525     return status;
00526 }
00527 
00528 VL53L0X_Error VL53L0X::vl53l0x_get_inter_measurement_period_milli_seconds( VL53L0X_DEV dev,
00529         uint32_t *p_inter_measurement_period_milli_seconds )
00530 {
00531     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00532     uint16_t osc_calibrate_val;
00533     uint32_t im_period_milli_seconds;
00534 
00535     LOG_FUNCTION_START( "" );
00536 
00537     status = vl53l0x_read_word( dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
00538                                 &osc_calibrate_val );
00539 
00540     if ( status == VL53L0X_ERROR_NONE ) {
00541         status = vl53l0x_read_dword( dev,
00542                                      VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
00543                                      &im_period_milli_seconds );
00544     }
00545 
00546     if ( status == VL53L0X_ERROR_NONE ) {
00547         if ( osc_calibrate_val != 0 ) {
00548             *p_inter_measurement_period_milli_seconds =
00549                 im_period_milli_seconds / osc_calibrate_val;
00550         }
00551         VL53L0X_SETPARAMETERFIELD( dev,
00552                                    InterMeasurementPeriodMilliSeconds,
00553                                    *p_inter_measurement_period_milli_seconds );
00554     }
00555 
00556     LOG_FUNCTION_END( status );
00557     return status;
00558 }
00559 
00560 VL53L0X_Error VL53L0X::vl53l0x_get_x_talk_compensation_rate_mega_cps( VL53L0X_DEV dev,
00561         FixPoint1616_t *p_xtalk_compensation_rate_mega_cps )
00562 {
00563     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00564     uint16_t value;
00565     FixPoint1616_t temp_fix1616;
00566 
00567     LOG_FUNCTION_START( "" );
00568 
00569     status = vl53l0x_read_word( dev,
00570                                 VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, ( uint16_t * )&value );
00571     if ( status == VL53L0X_ERROR_NONE ) {
00572         if ( value == 0 ) {
00573             /* the Xtalk is disabled return value from memory */
00574             VL53L0X_GETPARAMETERFIELD( dev,
00575                                        XTalkCompensationRateMegaCps, temp_fix1616 );
00576             *p_xtalk_compensation_rate_mega_cps = temp_fix1616;
00577             VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
00578                                        0 );
00579         } else {
00580             temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616( value );
00581             *p_xtalk_compensation_rate_mega_cps = temp_fix1616;
00582             VL53L0X_SETPARAMETERFIELD( dev,
00583                                        XTalkCompensationRateMegaCps, temp_fix1616 );
00584             VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
00585                                        1 );
00586         }
00587     }
00588 
00589     LOG_FUNCTION_END( status );
00590     return status;
00591 }
00592 
00593 VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_value( VL53L0X_DEV dev, uint16_t limit_check_id,
00594         FixPoint1616_t *p_limit_check_value )
00595 {
00596     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00597     uint8_t enable_zero_value = 0;
00598     uint16_t temp16;
00599     FixPoint1616_t temp_fix1616;
00600 
00601     LOG_FUNCTION_START( "" );
00602 
00603     switch ( limit_check_id ) {
00604 
00605         case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
00606             /* internal computation: */
00607             VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
00608                                             VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp_fix1616 );
00609             enable_zero_value = 0;
00610             break;
00611 
00612         case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
00613             status = vl53l0x_read_word( dev,
00614                                         VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
00615                                         &temp16 );
00616             if ( status == VL53L0X_ERROR_NONE )
00617                 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616( temp16 );
00618 
00619 
00620             enable_zero_value = 1;
00621             break;
00622 
00623         case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
00624             /* internal computation: */
00625             VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
00626                                             VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp_fix1616 );
00627             enable_zero_value = 0;
00628             break;
00629 
00630         case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
00631             /* internal computation: */
00632             VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
00633                                             VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, temp_fix1616 );
00634             enable_zero_value = 0;
00635             break;
00636 
00637         case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
00638         case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
00639             status = vl53l0x_read_word( dev,
00640                                         VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
00641                                         &temp16 );
00642             if ( status == VL53L0X_ERROR_NONE )
00643                 temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616( temp16 );
00644 
00645 
00646             enable_zero_value = 0;
00647             break;
00648 
00649         default:
00650             status = VL53L0X_ERROR_INVALID_PARAMS;
00651 
00652     }
00653 
00654     if ( status == VL53L0X_ERROR_NONE ) {
00655 
00656         if ( enable_zero_value == 1 ) {
00657 
00658             if ( temp_fix1616 == 0 ) {
00659                 /* disabled: return value from memory */
00660                 VL53L0X_GETARRAYPARAMETERFIELD( dev,
00661                                                 LimitChecksValue, limit_check_id,
00662                                                 temp_fix1616 );
00663                 *p_limit_check_value = temp_fix1616;
00664                 VL53L0X_SETARRAYPARAMETERFIELD( dev,
00665                                                 LimitChecksEnable, limit_check_id, 0 );
00666             } else {
00667                 *p_limit_check_value = temp_fix1616;
00668                 VL53L0X_SETARRAYPARAMETERFIELD( dev,
00669                                                 LimitChecksValue, limit_check_id,
00670                                                 temp_fix1616 );
00671                 VL53L0X_SETARRAYPARAMETERFIELD( dev,
00672                                                 LimitChecksEnable, limit_check_id, 1 );
00673             }
00674         } else {
00675             *p_limit_check_value = temp_fix1616;
00676         }
00677     }
00678 
00679     LOG_FUNCTION_END( status );
00680     return status;
00681 
00682 }
00683 
00684 VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_enable( VL53L0X_DEV dev, uint16_t limit_check_id,
00685         uint8_t *p_limit_check_enable )
00686 {
00687     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00688     uint8_t temp8;
00689 
00690     LOG_FUNCTION_START( "" );
00691 
00692     if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
00693         status = VL53L0X_ERROR_INVALID_PARAMS;
00694         *p_limit_check_enable = 0;
00695     } else {
00696         VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
00697                                         limit_check_id, temp8 );
00698         *p_limit_check_enable = temp8;
00699     }
00700 
00701     LOG_FUNCTION_END( status );
00702     return status;
00703 }
00704 
00705 VL53L0X_Error VL53L0X::vl53l0x_get_wrap_around_check_enable( VL53L0X_DEV dev,
00706         uint8_t *p_wrap_around_check_enable )
00707 {
00708     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00709     uint8_t data;
00710 
00711     LOG_FUNCTION_START( "" );
00712 
00713     status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data );
00714     if ( status == VL53L0X_ERROR_NONE ) {
00715         PALDevDataSet( dev, SequenceConfig, data );
00716         if ( data & ( 0x01 << 7 ) )
00717             *p_wrap_around_check_enable = 0x01;
00718         else
00719             *p_wrap_around_check_enable = 0x00;
00720     }
00721     if ( status == VL53L0X_ERROR_NONE ) {
00722         VL53L0X_SETPARAMETERFIELD( dev, WrapAroundCheckEnable,
00723                                    *p_wrap_around_check_enable );
00724     }
00725 
00726     LOG_FUNCTION_END( status );
00727     return status;
00728 }
00729 
00730 VL53L0X_Error VL53L0X::sequence_step_enabled( VL53L0X_DEV dev,
00731         VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config,
00732         uint8_t *p_sequence_step_enabled )
00733 {
00734     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00735     *p_sequence_step_enabled = 0;
00736     LOG_FUNCTION_START( "" );
00737 
00738     switch ( sequence_step_id ) {
00739         case VL53L0X_SEQUENCESTEP_TCC:
00740             *p_sequence_step_enabled = ( sequence_config & 0x10 ) >> 4;
00741             break;
00742         case VL53L0X_SEQUENCESTEP_DSS:
00743             *p_sequence_step_enabled = ( sequence_config & 0x08 ) >> 3;
00744             break;
00745         case VL53L0X_SEQUENCESTEP_MSRC:
00746             *p_sequence_step_enabled = ( sequence_config & 0x04 ) >> 2;
00747             break;
00748         case VL53L0X_SEQUENCESTEP_PRE_RANGE:
00749             *p_sequence_step_enabled = ( sequence_config & 0x40 ) >> 6;
00750             break;
00751         case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
00752             *p_sequence_step_enabled = ( sequence_config & 0x80 ) >> 7;
00753             break;
00754         default:
00755             Status = VL53L0X_ERROR_INVALID_PARAMS;
00756     }
00757 
00758     LOG_FUNCTION_END( status );
00759     return Status;
00760 }
00761 
00762 VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_enables( VL53L0X_DEV dev,
00763         VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps )
00764 {
00765     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00766     uint8_t sequence_config = 0;
00767     LOG_FUNCTION_START( "" );
00768 
00769     status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
00770                                 &sequence_config );
00771 
00772     if ( status == VL53L0X_ERROR_NONE ) {
00773         status = sequence_step_enabled( dev,
00774                                         VL53L0X_SEQUENCESTEP_TCC, sequence_config,
00775                                         &p_scheduler_sequence_steps->TccOn );
00776     }
00777     if ( status == VL53L0X_ERROR_NONE ) {
00778         status = sequence_step_enabled( dev,
00779                                         VL53L0X_SEQUENCESTEP_DSS, sequence_config,
00780                                         &p_scheduler_sequence_steps->DssOn );
00781     }
00782     if ( status == VL53L0X_ERROR_NONE ) {
00783         status = sequence_step_enabled( dev,
00784                                         VL53L0X_SEQUENCESTEP_MSRC, sequence_config,
00785                                         &p_scheduler_sequence_steps->MsrcOn );
00786     }
00787     if ( status == VL53L0X_ERROR_NONE ) {
00788         status = sequence_step_enabled( dev,
00789                                         VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config,
00790                                         &p_scheduler_sequence_steps->PreRangeOn );
00791     }
00792     if ( status == VL53L0X_ERROR_NONE ) {
00793         status = sequence_step_enabled( dev,
00794                                         VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config,
00795                                         &p_scheduler_sequence_steps->FinalRangeOn );
00796     }
00797 
00798     LOG_FUNCTION_END( status );
00799     return status;
00800 }
00801 
00802 uint8_t VL53L0X::vl53l0x_decode_vcsel_period( uint8_t vcsel_period_reg )
00803 {
00804     /*!
00805      * Converts the encoded VCSEL period register value into the real
00806      * period in PLL clocks
00807      */
00808 
00809     uint8_t vcsel_period_pclks = 0;
00810 
00811     vcsel_period_pclks = ( vcsel_period_reg + 1 ) << 1;
00812 
00813     return vcsel_period_pclks;
00814 }
00815 
00816 uint8_t VL53L0X::lv53l0x_encode_vcsel_period( uint8_t vcsel_period_pclks )
00817 {
00818     /*!
00819      * Converts the encoded VCSEL period register value into the real period
00820      * in PLL clocks
00821      */
00822 
00823     uint8_t vcsel_period_reg = 0;
00824 
00825     vcsel_period_reg = ( vcsel_period_pclks >> 1 ) - 1;
00826 
00827     return vcsel_period_reg;
00828 }
00829 
00830 
00831 VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_vcsel_pulse_period( VL53L0X_DEV dev,
00832         VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk )
00833 {
00834     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00835     uint8_t vcsel_period_reg;
00836     uint8_t min_pre_vcsel_period_pclk = 12;
00837     uint8_t max_pre_vcsel_period_pclk = 18;
00838     uint8_t min_final_vcsel_period_pclk = 8;
00839     uint8_t max_final_vcsel_period_pclk = 14;
00840     uint32_t measurement_timing_budget_micro_seconds;
00841     uint32_t final_range_timeout_micro_seconds;
00842     uint32_t pre_range_timeout_micro_seconds;
00843     uint32_t msrc_timeout_micro_seconds;
00844     uint8_t phase_cal_int = 0;
00845 
00846     /* Check if valid clock period requested */
00847 
00848     if ( ( vcsel_pulse_period_pclk % 2 ) != 0 ) {
00849         /* Value must be an even number */
00850         status = VL53L0X_ERROR_INVALID_PARAMS;
00851     } else if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
00852                 ( vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk ||
00853                   vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk ) ) {
00854         status = VL53L0X_ERROR_INVALID_PARAMS;
00855     } else if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
00856                 ( vcsel_pulse_period_pclk < min_final_vcsel_period_pclk ||
00857                   vcsel_pulse_period_pclk > max_final_vcsel_period_pclk ) ) {
00858 
00859         status = VL53L0X_ERROR_INVALID_PARAMS;
00860     }
00861 
00862     /* Apply specific settings for the requested clock period */
00863 
00864     if ( status != VL53L0X_ERROR_NONE )
00865         return status;
00866 
00867 
00868     if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE ) {
00869 
00870         /* Set phase check limits */
00871         if ( vcsel_pulse_period_pclk == 12 ) {
00872 
00873             status = vl53l0x_write_byte( dev,
00874                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00875                                          0x18 );
00876             status = vl53l0x_write_byte( dev,
00877                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00878                                          0x08 );
00879         } else if ( vcsel_pulse_period_pclk == 14 ) {
00880 
00881             status = vl53l0x_write_byte( dev,
00882                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00883                                          0x30 );
00884             status = vl53l0x_write_byte( dev,
00885                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00886                                          0x08 );
00887         } else if ( vcsel_pulse_period_pclk == 16 ) {
00888 
00889             status = vl53l0x_write_byte( dev,
00890                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00891                                          0x40 );
00892             status = vl53l0x_write_byte( dev,
00893                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00894                                          0x08 );
00895         } else if ( vcsel_pulse_period_pclk == 18 ) {
00896 
00897             status = vl53l0x_write_byte( dev,
00898                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
00899                                          0x50 );
00900             status = vl53l0x_write_byte( dev,
00901                                          VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
00902                                          0x08 );
00903         }
00904     } else if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE ) {
00905 
00906         if ( vcsel_pulse_period_pclk == 8 ) {
00907 
00908             status = vl53l0x_write_byte( dev,
00909                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00910                                          0x10 );
00911             status = vl53l0x_write_byte( dev,
00912                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00913                                          0x08 );
00914 
00915             status |= vl53l0x_write_byte( dev,
00916                                           VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02 );
00917             status |= vl53l0x_write_byte( dev,
00918                                           VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C );
00919 
00920             status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
00921             status |= vl53l0x_write_byte( dev,
00922                                           VL53L0X_REG_ALGO_PHASECAL_LIM,
00923                                           0x30 );
00924             status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
00925         } else if ( vcsel_pulse_period_pclk == 10 ) {
00926 
00927             status = vl53l0x_write_byte( dev,
00928                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00929                                          0x28 );
00930             status = vl53l0x_write_byte( dev,
00931                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00932                                          0x08 );
00933 
00934             status |= vl53l0x_write_byte( dev,
00935                                           VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03 );
00936             status |= vl53l0x_write_byte( dev,
00937                                           VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09 );
00938 
00939             status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
00940             status |= vl53l0x_write_byte( dev,
00941                                           VL53L0X_REG_ALGO_PHASECAL_LIM,
00942                                           0x20 );
00943             status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
00944         } else if ( vcsel_pulse_period_pclk == 12 ) {
00945 
00946             status = vl53l0x_write_byte( dev,
00947                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00948                                          0x38 );
00949             status = vl53l0x_write_byte( dev,
00950                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00951                                          0x08 );
00952 
00953             status |= vl53l0x_write_byte( dev,
00954                                           VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03 );
00955             status |= vl53l0x_write_byte( dev,
00956                                           VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08 );
00957 
00958             status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
00959             status |= vl53l0x_write_byte( dev,
00960                                           VL53L0X_REG_ALGO_PHASECAL_LIM,
00961                                           0x20 );
00962             status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
00963         } else if ( vcsel_pulse_period_pclk == 14 ) {
00964 
00965             status = vl53l0x_write_byte( dev,
00966                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
00967                                          0x048 );
00968             status = vl53l0x_write_byte( dev,
00969                                          VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
00970                                          0x08 );
00971 
00972             status |= vl53l0x_write_byte( dev,
00973                                           VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03 );
00974             status |= vl53l0x_write_byte( dev,
00975                                           VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07 );
00976 
00977             status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
00978             status |= vl53l0x_write_byte( dev,
00979                                           VL53L0X_REG_ALGO_PHASECAL_LIM,
00980                                           0x20 );
00981             status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
00982         }
00983     }
00984 
00985 
00986     /* Re-calculate and apply timeouts, in macro periods */
00987 
00988     if ( status == VL53L0X_ERROR_NONE ) {
00989         vcsel_period_reg = lv53l0x_encode_vcsel_period( ( uint8_t )
00990                            vcsel_pulse_period_pclk );
00991 
00992         /* When the VCSEL period for the pre or final range is changed,
00993         * the corresponding timeout must be read from the device using
00994         * the current VCSEL period, then the new VCSEL period can be
00995         * applied. The timeout then must be written back to the device
00996         * using the new VCSEL period.
00997         *
00998         * For the MSRC timeout, the same applies - this timeout being
00999         * dependant on the pre-range vcsel period.
01000         */
01001         switch ( vcsel_period_type ) {
01002             case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
01003                 status = get_sequence_step_timeout( dev,
01004                                                     VL53L0X_SEQUENCESTEP_PRE_RANGE,
01005                                                     &pre_range_timeout_micro_seconds );
01006 
01007                 if ( status == VL53L0X_ERROR_NONE )
01008                     status = get_sequence_step_timeout( dev,
01009                                                         VL53L0X_SEQUENCESTEP_MSRC,
01010                                                         &msrc_timeout_micro_seconds );
01011 
01012                 if ( status == VL53L0X_ERROR_NONE )
01013                     status = vl53l0x_write_byte( dev,
01014                                                  VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
01015                                                  vcsel_period_reg );
01016 
01017 
01018                 if ( status == VL53L0X_ERROR_NONE )
01019                     status = set_sequence_step_timeout( dev,
01020                                                         VL53L0X_SEQUENCESTEP_PRE_RANGE,
01021                                                         pre_range_timeout_micro_seconds );
01022 
01023 
01024                 if ( status == VL53L0X_ERROR_NONE )
01025                     status = set_sequence_step_timeout( dev,
01026                                                         VL53L0X_SEQUENCESTEP_MSRC,
01027                                                         msrc_timeout_micro_seconds );
01028 
01029                 VL53L0X_SETDEVICESPECIFICPARAMETER(
01030                     dev,
01031                     PreRangeVcselPulsePeriod,
01032                     vcsel_pulse_period_pclk );
01033                 break;
01034             case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
01035                 status = get_sequence_step_timeout( dev,
01036                                                     VL53L0X_SEQUENCESTEP_FINAL_RANGE,
01037                                                     &final_range_timeout_micro_seconds );
01038 
01039                 if ( status == VL53L0X_ERROR_NONE )
01040                     status = vl53l0x_write_byte( dev,
01041                                                  VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
01042                                                  vcsel_period_reg );
01043 
01044 
01045                 if ( status == VL53L0X_ERROR_NONE )
01046                     status = set_sequence_step_timeout( dev,
01047                                                         VL53L0X_SEQUENCESTEP_FINAL_RANGE,
01048                                                         final_range_timeout_micro_seconds );
01049 
01050                 VL53L0X_SETDEVICESPECIFICPARAMETER(
01051                     dev,
01052                     FinalRangeVcselPulsePeriod,
01053                     vcsel_pulse_period_pclk );
01054                 break;
01055             default:
01056                 status = VL53L0X_ERROR_INVALID_PARAMS;
01057         }
01058     }
01059 
01060     /* Finally, the timing budget must be re-applied */
01061     if ( status == VL53L0X_ERROR_NONE ) {
01062         VL53L0X_GETPARAMETERFIELD( dev,
01063                                    MeasurementTimingBudgetMicroSeconds,
01064                                    measurement_timing_budget_micro_seconds );
01065 
01066         status = vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
01067                  measurement_timing_budget_micro_seconds );
01068     }
01069 
01070     /* Perform the phase calibration. This is needed after changing on
01071      * vcsel period.
01072      * get_data_enable = 0, restore_config = 1 */
01073     if ( status == VL53L0X_ERROR_NONE )
01074         status = vl53l0x_perform_phase_calibration(
01075                      dev, &phase_cal_int, 0, 1 );
01076 
01077     return status;
01078 }
01079 
01080 VL53L0X_Error VL53L0X::vl53l0x_set_vcsel_pulse_period( VL53L0X_DEV dev,
01081         VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period )
01082 {
01083     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01084     LOG_FUNCTION_START( "" );
01085 
01086     status = wrapped_vl53l0x_set_vcsel_pulse_period( dev, vcsel_period_type,
01087              vcsel_pulse_period );
01088 
01089     LOG_FUNCTION_END( status );
01090     return status;
01091 }
01092 
01093 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_vcsel_pulse_period( VL53L0X_DEV dev,
01094         VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk )
01095 {
01096     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01097     uint8_t vcsel_period_reg;
01098 
01099     switch ( vcsel_period_type ) {
01100         case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
01101             status = vl53l0x_read_byte( dev,
01102                                         VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
01103                                         &vcsel_period_reg );
01104             break;
01105         case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
01106             status = vl53l0x_read_byte( dev,
01107                                         VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
01108                                         &vcsel_period_reg );
01109             break;
01110         default:
01111             status = VL53L0X_ERROR_INVALID_PARAMS;
01112     }
01113 
01114     if ( status == VL53L0X_ERROR_NONE )
01115         *p_vcsel_pulse_period_pclk =
01116             vl53l0x_decode_vcsel_period( vcsel_period_reg );
01117 
01118     return status;
01119 }
01120 
01121 VL53L0X_Error VL53L0X::vl53l0x_get_vcsel_pulse_period( VL53L0X_DEV dev,
01122         VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk )
01123 {
01124     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01125     LOG_FUNCTION_START( "" );
01126 
01127     status = wrapped_vl53l0x_get_vcsel_pulse_period( dev, vcsel_period_type,
01128              p_vcsel_pulse_period_pclk );
01129 
01130     LOG_FUNCTION_END( status );
01131     return status;
01132 }
01133 
01134 uint32_t VL53L0X::vl53l0x_decode_timeout( uint16_t encoded_timeout )
01135 {
01136     /*!
01137      * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
01138      */
01139 
01140     uint32_t timeout_macro_clks = 0;
01141 
01142     timeout_macro_clks = ( ( uint32_t ) ( encoded_timeout & 0x00FF )
01143                            << ( uint32_t ) ( ( encoded_timeout & 0xFF00 ) >> 8 ) ) + 1;
01144 
01145     return timeout_macro_clks;
01146 }
01147 
01148 uint32_t VL53L0X::vl53l0x_calc_macro_period_ps( VL53L0X_DEV dev, uint8_t vcsel_period_pclks )
01149 {
01150     uint64_t pll_period_ps;
01151     uint32_t macro_period_vclks;
01152     uint32_t macro_period_ps;
01153 
01154     LOG_FUNCTION_START( "" );
01155 
01156     /* The above calculation will produce rounding errors,
01157        therefore set fixed value
01158     */
01159     pll_period_ps = 1655;
01160 
01161     macro_period_vclks = 2304;
01162     macro_period_ps = ( uint32_t )( macro_period_vclks
01163                                     * vcsel_period_pclks * pll_period_ps );
01164 
01165     LOG_FUNCTION_END( "" );
01166     return macro_period_ps;
01167 }
01168 
01169 /* To convert register value into us */
01170 uint32_t VL53L0X::vl53l0x_calc_timeout_us( VL53L0X_DEV dev,
01171         uint16_t timeout_period_mclks,
01172         uint8_t vcsel_period_pclks )
01173 {
01174     uint32_t macro_period_ps;
01175     uint32_t macro_period_ns;
01176     uint32_t actual_timeout_period_us = 0;
01177 
01178     macro_period_ps = vl53l0x_calc_macro_period_ps( dev, vcsel_period_pclks );
01179     macro_period_ns = ( macro_period_ps + 500 ) / 1000;
01180 
01181     actual_timeout_period_us =
01182         ( ( timeout_period_mclks * macro_period_ns ) + 500 ) / 1000;
01183 
01184     return actual_timeout_period_us;
01185 }
01186 
01187 VL53L0X_Error VL53L0X::get_sequence_step_timeout( VL53L0X_DEV dev,
01188         VL53L0X_SequenceStepId sequence_step_id,
01189         uint32_t *p_time_out_micro_secs )
01190 {
01191     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01192     uint8_t current_vcsel_pulse_period_p_clk;
01193     uint8_t encoded_time_out_byte = 0;
01194     uint32_t timeout_micro_seconds = 0;
01195     uint16_t pre_range_encoded_time_out = 0;
01196     uint16_t msrc_time_out_m_clks;
01197     uint16_t pre_range_time_out_m_clks;
01198     uint16_t final_range_time_out_m_clks = 0;
01199     uint16_t final_range_encoded_time_out;
01200     VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
01201 
01202     if ( ( sequence_step_id == VL53L0X_SEQUENCESTEP_TCC )    ||
01203             ( sequence_step_id == VL53L0X_SEQUENCESTEP_DSS )     ||
01204             ( sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC ) ) {
01205 
01206         status = vl53l0x_get_vcsel_pulse_period( dev,
01207                  VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01208                  &current_vcsel_pulse_period_p_clk );
01209         if ( status == VL53L0X_ERROR_NONE ) {
01210             status = vl53l0x_read_byte( dev,
01211                                         VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
01212                                         &encoded_time_out_byte );
01213         }
01214         msrc_time_out_m_clks = vl53l0x_decode_timeout( encoded_time_out_byte );
01215 
01216         timeout_micro_seconds = vl53l0x_calc_timeout_us( dev,
01217                                 msrc_time_out_m_clks,
01218                                 current_vcsel_pulse_period_p_clk );
01219     } else if ( sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE ) {
01220         /* Retrieve PRE-RANGE VCSEL Period */
01221         status = vl53l0x_get_vcsel_pulse_period( dev,
01222                  VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01223                  &current_vcsel_pulse_period_p_clk );
01224 
01225         /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
01226         if ( status == VL53L0X_ERROR_NONE ) {
01227 
01228             /* Retrieve PRE-RANGE VCSEL Period */
01229             status = vl53l0x_get_vcsel_pulse_period( dev,
01230                      VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01231                      &current_vcsel_pulse_period_p_clk );
01232 
01233             if ( status == VL53L0X_ERROR_NONE ) {
01234                 status = vl53l0x_read_word( dev,
01235                                             VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
01236                                             &pre_range_encoded_time_out );
01237             }
01238 
01239             pre_range_time_out_m_clks = vl53l0x_decode_timeout(
01240                                             pre_range_encoded_time_out );
01241 
01242             timeout_micro_seconds = vl53l0x_calc_timeout_us( dev,
01243                                     pre_range_time_out_m_clks,
01244                                     current_vcsel_pulse_period_p_clk );
01245         }
01246     } else if ( sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE ) {
01247 
01248         vl53l0x_get_sequence_step_enables( dev, &scheduler_sequence_steps );
01249         pre_range_time_out_m_clks = 0;
01250 
01251         if ( scheduler_sequence_steps.PreRangeOn ) {
01252             /* Retrieve PRE-RANGE VCSEL Period */
01253             status = vl53l0x_get_vcsel_pulse_period( dev,
01254                      VL53L0X_VCSEL_PERIOD_PRE_RANGE,
01255                      &current_vcsel_pulse_period_p_clk );
01256 
01257             /* Retrieve PRE-RANGE Timeout in Macro periods
01258              * (MCLKS) */
01259             if ( status == VL53L0X_ERROR_NONE ) {
01260                 status = vl53l0x_read_word( dev,
01261                                             VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
01262                                             &pre_range_encoded_time_out );
01263                 pre_range_time_out_m_clks = vl53l0x_decode_timeout(
01264                                                 pre_range_encoded_time_out );
01265             }
01266         }
01267 
01268         if ( status == VL53L0X_ERROR_NONE ) {
01269             /* Retrieve FINAL-RANGE VCSEL Period */
01270             status = vl53l0x_get_vcsel_pulse_period( dev,
01271                      VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
01272                      &current_vcsel_pulse_period_p_clk );
01273         }
01274 
01275         /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
01276         if ( status == VL53L0X_ERROR_NONE ) {
01277             status = vl53l0x_read_word( dev,
01278                                         VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
01279                                         &final_range_encoded_time_out );
01280             final_range_time_out_m_clks = vl53l0x_decode_timeout(
01281                                               final_range_encoded_time_out );
01282         }
01283 
01284         final_range_time_out_m_clks -= pre_range_time_out_m_clks;
01285         timeout_micro_seconds = vl53l0x_calc_timeout_us( dev,
01286                                 final_range_time_out_m_clks,
01287                                 current_vcsel_pulse_period_p_clk );
01288     }
01289 
01290     *p_time_out_micro_secs = timeout_micro_seconds;
01291 
01292     return status;
01293 }
01294 
01295 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
01296         uint32_t *p_measurement_timing_budget_micro_seconds )
01297 {
01298     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01299     VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
01300     uint32_t final_range_timeout_micro_seconds;
01301     uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
01302     uint32_t start_overhead_micro_seconds       = 1910;
01303     uint32_t end_overhead_micro_seconds     = 960;
01304     uint32_t msrc_overhead_micro_seconds        = 660;
01305     uint32_t tcc_overhead_micro_seconds     = 590;
01306     uint32_t dss_overhead_micro_seconds     = 690;
01307     uint32_t pre_range_overhead_micro_seconds   = 660;
01308     uint32_t final_range_overhead_micro_seconds = 550;
01309     uint32_t pre_range_timeout_micro_seconds    = 0;
01310 
01311     LOG_FUNCTION_START( "" );
01312 
01313     /* Start and end overhead times always present */
01314     *p_measurement_timing_budget_micro_seconds
01315         = start_overhead_micro_seconds + end_overhead_micro_seconds;
01316 
01317     status = vl53l0x_get_sequence_step_enables( dev, &scheduler_sequence_steps );
01318 
01319     if ( status != VL53L0X_ERROR_NONE ) {
01320         LOG_FUNCTION_END( status );
01321         return status;
01322     }
01323 
01324 
01325     if ( scheduler_sequence_steps.TccOn  ||
01326             scheduler_sequence_steps.MsrcOn ||
01327             scheduler_sequence_steps.DssOn ) {
01328 
01329         status = get_sequence_step_timeout( dev,
01330                                             VL53L0X_SEQUENCESTEP_MSRC,
01331                                             &msrc_dcc_tcc_timeout_micro_seconds );
01332 
01333         if ( status == VL53L0X_ERROR_NONE ) {
01334             if ( scheduler_sequence_steps.TccOn ) {
01335                 *p_measurement_timing_budget_micro_seconds +=
01336                     msrc_dcc_tcc_timeout_micro_seconds +
01337                     tcc_overhead_micro_seconds;
01338             }
01339 
01340             if ( scheduler_sequence_steps.DssOn ) {
01341                 *p_measurement_timing_budget_micro_seconds +=
01342                     2 * ( msrc_dcc_tcc_timeout_micro_seconds +
01343                           dss_overhead_micro_seconds );
01344             } else if ( scheduler_sequence_steps.MsrcOn ) {
01345                 *p_measurement_timing_budget_micro_seconds +=
01346                     msrc_dcc_tcc_timeout_micro_seconds +
01347                     msrc_overhead_micro_seconds;
01348             }
01349         }
01350     }
01351 
01352     if ( status == VL53L0X_ERROR_NONE ) {
01353         if ( scheduler_sequence_steps.PreRangeOn ) {
01354             status = get_sequence_step_timeout( dev,
01355                                                 VL53L0X_SEQUENCESTEP_PRE_RANGE,
01356                                                 &pre_range_timeout_micro_seconds );
01357             *p_measurement_timing_budget_micro_seconds +=
01358                 pre_range_timeout_micro_seconds +
01359                 pre_range_overhead_micro_seconds;
01360         }
01361     }
01362 
01363     if ( status == VL53L0X_ERROR_NONE ) {
01364         if ( scheduler_sequence_steps.FinalRangeOn ) {
01365             status = get_sequence_step_timeout( dev,
01366                                                 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
01367                                                 &final_range_timeout_micro_seconds );
01368             *p_measurement_timing_budget_micro_seconds +=
01369                 ( final_range_timeout_micro_seconds +
01370                   final_range_overhead_micro_seconds );
01371         }
01372     }
01373 
01374     if ( status == VL53L0X_ERROR_NONE ) {
01375         VL53L0X_SETPARAMETERFIELD( dev,
01376                                    MeasurementTimingBudgetMicroSeconds,
01377                                    *p_measurement_timing_budget_micro_seconds );
01378     }
01379 
01380     LOG_FUNCTION_END( status );
01381     return status;
01382 }
01383 
01384 VL53L0X_Error VL53L0X::vl53l0x_get_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
01385         uint32_t *p_measurement_timing_budget_micro_seconds )
01386 {
01387     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01388     LOG_FUNCTION_START( "" );
01389 
01390     status = wrapped_vl53l0x_get_measurement_timing_budget_micro_seconds( dev,
01391              p_measurement_timing_budget_micro_seconds );
01392 
01393     LOG_FUNCTION_END( status );
01394     return status;
01395 }
01396 
01397 VL53L0X_Error VL53L0X::vl53l0x_get_device_parameters( VL53L0X_DEV dev,
01398         VL53L0X_DeviceParameters_t *p_device_parameters )
01399 {
01400     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01401     int i;
01402 
01403     LOG_FUNCTION_START( "" );
01404 
01405     status = vl53l0x_get_device_mode( dev, &( p_device_parameters->DeviceMode  ) );
01406 
01407     if ( status == VL53L0X_ERROR_NONE )
01408         status = vl53l0x_get_inter_measurement_period_milli_seconds( dev,
01409                  &( p_device_parameters->InterMeasurementPeriodMilliSeconds  ) );
01410 
01411 
01412     if ( status == VL53L0X_ERROR_NONE )
01413         p_device_parameters->XTalkCompensationEnable  = 0;
01414 
01415     if ( status == VL53L0X_ERROR_NONE )
01416         status = vl53l0x_get_x_talk_compensation_rate_mega_cps( dev,
01417                  &( p_device_parameters->XTalkCompensationRateMegaCps  ) );
01418 
01419 
01420     if ( status == VL53L0X_ERROR_NONE )
01421         status = vl53l0x_get_offset_calibration_data_micro_meter( dev,
01422                  &( p_device_parameters->RangeOffsetMicroMeters  ) );
01423 
01424 
01425     if ( status == VL53L0X_ERROR_NONE ) {
01426         for ( i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++ ) {
01427             /* get first the values, then the enables.
01428              * VL53L0X_GetLimitCheckValue will modify the enable
01429              * flags
01430              */
01431             if ( status == VL53L0X_ERROR_NONE ) {
01432                 status |= vl53l0x_get_limit_check_value( dev, i,
01433                           &( p_device_parameters->LimitChecksValue [i] ) );
01434             } else {
01435                 break;
01436             }
01437             if ( status == VL53L0X_ERROR_NONE ) {
01438                 status |= vl53l0x_get_limit_check_enable( dev, i,
01439                           &( p_device_parameters->LimitChecksEnable [i] ) );
01440             } else {
01441                 break;
01442             }
01443         }
01444     }
01445 
01446     if ( status == VL53L0X_ERROR_NONE ) {
01447         status = vl53l0x_get_wrap_around_check_enable( dev,
01448                  &( p_device_parameters->WrapAroundCheckEnable  ) );
01449     }
01450 
01451     /* Need to be done at the end as it uses VCSELPulsePeriod */
01452     if ( status == VL53L0X_ERROR_NONE ) {
01453         status = vl53l0x_get_measurement_timing_budget_micro_seconds( dev,
01454                  &( p_device_parameters->MeasurementTimingBudgetMicroSeconds  ) );
01455     }
01456 
01457     LOG_FUNCTION_END( status );
01458     return status;
01459 }
01460 
01461 VL53L0X_Error VL53L0X::vl53l0x_set_limit_check_value( VL53L0X_DEV dev, uint16_t limit_check_id,
01462         FixPoint1616_t limit_check_value )
01463 {
01464     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01465     uint8_t temp8;
01466 
01467     LOG_FUNCTION_START( "" );
01468 
01469     VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksEnable, limit_check_id,
01470                                     temp8 );
01471 
01472     if ( temp8 == 0 ) { /* disabled write only internal value */
01473         VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
01474                                         limit_check_id, limit_check_value );
01475     } else {
01476 
01477         switch ( limit_check_id ) {
01478 
01479             case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
01480                 /* internal computation: */
01481                 VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
01482                                                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
01483                                                 limit_check_value );
01484                 break;
01485 
01486             case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
01487 
01488                 status = vl53l0x_write_word( dev,
01489                                              VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
01490                                              VL53L0X_FIXPOINT1616TOFIXPOINT97(
01491                                                  limit_check_value ) );
01492 
01493                 break;
01494 
01495             case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
01496 
01497                 /* internal computation: */
01498                 VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
01499                                                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
01500                                                 limit_check_value );
01501 
01502                 break;
01503 
01504             case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
01505 
01506                 /* internal computation: */
01507                 VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
01508                                                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
01509                                                 limit_check_value );
01510 
01511                 break;
01512 
01513             case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
01514             case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
01515 
01516                 status = vl53l0x_write_word( dev,
01517                                              VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
01518                                              VL53L0X_FIXPOINT1616TOFIXPOINT97(
01519                                                  limit_check_value ) );
01520 
01521                 break;
01522 
01523             default:
01524                 status = VL53L0X_ERROR_INVALID_PARAMS;
01525 
01526         }
01527 
01528         if ( status == VL53L0X_ERROR_NONE ) {
01529             VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
01530                                             limit_check_id, limit_check_value );
01531         }
01532     }
01533 
01534     LOG_FUNCTION_END( status );
01535     return status;
01536 }
01537 
01538 VL53L0X_Error VL53L0X::vl53l0x_data_init( VL53L0X_DEV dev )
01539 {
01540     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01541     VL53L0X_DeviceParameters_t CurrentParameters;
01542     int i;
01543     uint8_t StopVariable;
01544 
01545     LOG_FUNCTION_START( "" );
01546 
01547     /* by default the I2C is running at 1V8 if you want to change it you
01548      * need to include this define at compilation level. */
01549 #ifdef USE_I2C_2V8
01550     Status = vl53l0x_update_word( Dev,
01551                                  VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
01552                                  0xFE,
01553                                  0x01 );
01554 #endif
01555 
01556     /* Set I2C standard mode */
01557     if ( status == VL53L0X_ERROR_NONE )
01558         status = vl53l0x_write_byte( dev, 0x88, 0x00 );
01559 
01560     VL53L0X_SETDEVICESPECIFICPARAMETER( dev, ReadDataFromDeviceDone, 0 );
01561 
01562 #ifdef USE_IQC_STATION
01563     if ( Status == VL53L0X_ERROR_NONE )
01564         Status = VL53L0X_apply_offset_adjustment( Dev );
01565 #endif
01566 
01567     /* Default value is 1000 for Linearity Corrective Gain */
01568     PALDevDataSet( dev, LinearityCorrectiveGain, 1000 );
01569 
01570     /* Dmax default Parameter */
01571     PALDevDataSet( dev, DmaxCalRangeMilliMeter, 400 );
01572     PALDevDataSet( dev, DmaxCalSignalRateRtnMegaCps,
01573                    ( FixPoint1616_t )( ( 0x00016B85 ) ) ); /* 1.42 No Cover Glass*/
01574 
01575     /* Set Default static parameters
01576      *set first temporary values 9.44MHz * 65536 = 618660 */
01577     VL53L0X_SETDEVICESPECIFICPARAMETER( dev, OscFrequencyMHz, 618660 );
01578 
01579     /* Set Default XTalkCompensationRateMegaCps to 0  */
01580     VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps, 0 );
01581 
01582     /* Get default parameters */
01583     status = vl53l0x_get_device_parameters( dev, &CurrentParameters );
01584     if ( status == VL53L0X_ERROR_NONE ) {
01585         /* initialize PAL values */
01586         CurrentParameters.DeviceMode  = VL53L0X_DEVICEMODE_SINGLE_RANGING;
01587         CurrentParameters.HistogramMode  = VL53L0X_HISTOGRAMMODE_DISABLED;
01588         PALDevDataSet( dev, CurrentParameters, CurrentParameters );
01589     }
01590 
01591     /* Sigma estimator variable */
01592     PALDevDataSet( dev, SigmaEstRefArray, 100 );
01593     PALDevDataSet( dev, SigmaEstEffPulseWidth, 900 );
01594     PALDevDataSet( dev, SigmaEstEffAmbWidth, 500 );
01595     PALDevDataSet( dev, targetRefRate, 0x0A00 ); /* 20 MCPS in 9:7 format */
01596 
01597     /* Use internal default settings */
01598     PALDevDataSet( dev, UseInternalTuningSettings, 1 );
01599 
01600     status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
01601     status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
01602     status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
01603     status |= vl53l0x_read_byte( dev, 0x91, &StopVariable );
01604     PALDevDataSet( dev, StopVariable, StopVariable );
01605     status |= vl53l0x_write_byte( dev, 0x00, 0x01 );
01606     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
01607     status |= vl53l0x_write_byte( dev, 0x80, 0x00 );
01608 
01609     /* Enable all check */
01610     for ( i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++ ) {
01611         if ( status == VL53L0X_ERROR_NONE )
01612             status |= vl53l0x_set_limit_check_enable( dev, i, 1 );
01613         else
01614             break;
01615 
01616     }
01617 
01618     /* Disable the following checks */
01619     if ( status == VL53L0X_ERROR_NONE )
01620         status = vl53l0x_set_limit_check_enable( dev,
01621                  VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0 );
01622 
01623     if ( status == VL53L0X_ERROR_NONE )
01624         status = vl53l0x_set_limit_check_enable( dev,
01625                  VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0 );
01626 
01627     if ( status == VL53L0X_ERROR_NONE )
01628         status = vl53l0x_set_limit_check_enable( dev,
01629                  VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0 );
01630 
01631     if ( status == VL53L0X_ERROR_NONE )
01632         status = vl53l0x_set_limit_check_enable( dev,
01633                  VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0 );
01634 
01635     /* Limit default values */
01636     if ( status == VL53L0X_ERROR_NONE ) {
01637         status = vl53l0x_set_limit_check_value( dev,
01638                                                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
01639                                                 ( FixPoint1616_t )( 18 * 65536 ) );
01640     }
01641     if ( status == VL53L0X_ERROR_NONE ) {
01642         status = vl53l0x_set_limit_check_value( dev,
01643                                                 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
01644                                                 ( FixPoint1616_t )( 25 * 65536 / 100 ) );
01645         /* 0.25 * 65536 */
01646     }
01647 
01648     if ( status == VL53L0X_ERROR_NONE ) {
01649         status = vl53l0x_set_limit_check_value( dev,
01650                                                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
01651                                                 ( FixPoint1616_t )( 35 * 65536 ) );
01652     }
01653 
01654     if ( status == VL53L0X_ERROR_NONE ) {
01655         status = vl53l0x_set_limit_check_value( dev,
01656                                                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
01657                                                 ( FixPoint1616_t )( 0 * 65536 ) );
01658     }
01659 
01660     if ( status == VL53L0X_ERROR_NONE ) {
01661 
01662         PALDevDataSet( dev, SequenceConfig, 0xFF );
01663         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01664                                      0xFF );
01665 
01666         /* Set PAL state to tell that we are waiting for call to
01667          * VL53L0X_StaticInit */
01668         PALDevDataSet( dev, PalState, VL53L0X_STATE_WAIT_STATICINIT );
01669     }
01670 
01671     if ( status == VL53L0X_ERROR_NONE )
01672         VL53L0X_SETDEVICESPECIFICPARAMETER( dev, RefSpadsInitialised, 0 );
01673 
01674 
01675     LOG_FUNCTION_END( status );
01676     return status;
01677 }
01678 
01679 VL53L0X_Error VL53L0X::vl53l0x_check_part_used( VL53L0X_DEV dev,
01680         uint8_t *revision,
01681         VL53L0X_DeviceInfo_t *p_vl53l0x_device_info )
01682 {
01683     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01684     uint8_t module_id_int;
01685     char *product_id_tmp;
01686 
01687     LOG_FUNCTION_START( "" );
01688 
01689     status = vl53l0x_get_info_from_device( dev, 2 );
01690 
01691     if ( status == VL53L0X_ERROR_NONE ) {
01692         module_id_int = VL53L0X_GETDEVICESPECIFICPARAMETER( dev, ModuleId );
01693 
01694         if ( module_id_int == 0 ) {
01695             *revision = 0;
01696             VL53L0X_COPYSTRING( p_vl53l0x_device_info->ProductId , "" );
01697         } else {
01698             *revision = VL53L0X_GETDEVICESPECIFICPARAMETER( dev, Revision );
01699             product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
01700                              ProductId );
01701             VL53L0X_COPYSTRING( p_vl53l0x_device_info->ProductId , product_id_tmp );
01702         }
01703     }
01704 
01705     LOG_FUNCTION_END( status );
01706     return status;
01707 }
01708 
01709 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_device_info( VL53L0X_DEV dev,
01710         VL53L0X_DeviceInfo_t *p_vl53l0x_device_info )
01711 {
01712     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01713     uint8_t revision_id;
01714     uint8_t revision;
01715 
01716     status = vl53l0x_check_part_used( dev, &revision, p_vl53l0x_device_info );
01717 
01718     if ( status == VL53L0X_ERROR_NONE ) {
01719         if ( revision == 0 ) {
01720             VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name ,
01721                                 VL53L0X_STRING_DEVICE_INFO_NAME_TS0 );
01722         } else if ( ( revision <= 34 ) && ( revision != 32 ) ) {
01723             VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name ,
01724                                 VL53L0X_STRING_DEVICE_INFO_NAME_TS1 );
01725         } else if ( revision < 39 ) {
01726             VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name ,
01727                                 VL53L0X_STRING_DEVICE_INFO_NAME_TS2 );
01728         } else {
01729             VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name ,
01730                                 VL53L0X_STRING_DEVICE_INFO_NAME_ES1 );
01731         }
01732 
01733         VL53L0X_COPYSTRING( p_vl53l0x_device_info->Type ,
01734                             VL53L0X_STRING_DEVICE_INFO_TYPE );
01735 
01736     }
01737 
01738     if ( status == VL53L0X_ERROR_NONE ) {
01739         status = vl53l0x_read_byte( dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
01740                                     &p_vl53l0x_device_info->ProductType  );
01741     }
01742 
01743     if ( status == VL53L0X_ERROR_NONE ) {
01744         status = vl53l0x_read_byte( dev,
01745                                     VL53L0X_REG_IDENTIFICATION_REVISION_ID,
01746                                     &revision_id );
01747         p_vl53l0x_device_info->ProductRevisionMajor  = 1;
01748         p_vl53l0x_device_info->ProductRevisionMinor  =
01749             ( revision_id & 0xF0 ) >> 4;
01750     }
01751 
01752     return status;
01753 }
01754 
01755 VL53L0X_Error VL53L0X::vl53l0x_get_device_info( VL53L0X_DEV dev,
01756         VL53L0X_DeviceInfo_t *p_vl53l0x_device_info )
01757 {
01758     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01759     LOG_FUNCTION_START( "" );
01760 
01761     status = wrapped_vl53l0x_get_device_info( dev, p_vl53l0x_device_info );
01762 
01763     LOG_FUNCTION_END( status );
01764     return status;
01765 }
01766 
01767 VL53L0X_Error VL53L0X::vl53l0x_get_interrupt_mask_status( VL53L0X_DEV dev,
01768         uint32_t *p_interrupt_mask_status )
01769 {
01770     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01771     uint8_t byte;
01772     LOG_FUNCTION_START( "" );
01773 
01774     status = vl53l0x_read_byte( dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte );
01775     *p_interrupt_mask_status = byte & 0x07;
01776 
01777     if ( byte & 0x18 )
01778         status = VL53L0X_ERROR_RANGE_ERROR;
01779 
01780     LOG_FUNCTION_END( status );
01781     return status;
01782 }
01783 
01784 VL53L0X_Error VL53L0X::vl53l0x_get_measurement_data_ready( VL53L0X_DEV dev,
01785         uint8_t *p_measurement_data_ready )
01786 {
01787     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01788     uint8_t sys_range_status_register;
01789     uint8_t interrupt_config;
01790     uint32_t interrupt_mask;
01791     LOG_FUNCTION_START( "" );
01792 
01793     interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
01794                        Pin0GpioFunctionality );
01795 
01796     if ( interrupt_config ==
01797             VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY ) {
01798         status = vl53l0x_get_interrupt_mask_status( dev, &interrupt_mask );
01799         if ( interrupt_mask ==
01800                 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY )
01801             *p_measurement_data_ready = 1;
01802         else
01803             *p_measurement_data_ready = 0;
01804     } else {
01805         status = vl53l0x_read_byte( dev, VL53L0X_REG_RESULT_RANGE_STATUS,
01806                                     &sys_range_status_register );
01807         if ( status == VL53L0X_ERROR_NONE ) {
01808             if ( sys_range_status_register & 0x01 )
01809                 *p_measurement_data_ready = 1;
01810             else
01811                 *p_measurement_data_ready = 0;
01812         }
01813     }
01814 
01815     LOG_FUNCTION_END( status );
01816     return status;
01817 }
01818 
01819 VL53L0X_Error VL53L0X::vl53l0x_polling_delay( VL53L0X_DEV dev )
01820 {
01821     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01822 
01823     // do nothing
01824     VL53L0X_OsDelay();
01825     return status;
01826 }
01827 
01828 VL53L0X_Error VL53L0X::vl53l0x_measurement_poll_for_completion( VL53L0X_DEV dev )
01829 {
01830     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01831     uint8_t new_data_ready = 0;
01832     uint32_t loop_nb;
01833 
01834     LOG_FUNCTION_START( "" );
01835 
01836     loop_nb = 0;
01837 
01838     do {
01839         status = vl53l0x_get_measurement_data_ready( dev, &new_data_ready );
01840         if ( status != 0 )
01841             break; /* the error is set */
01842 
01843         if ( new_data_ready == 1 )
01844             break; /* done note that status == 0 */
01845 
01846         loop_nb++;
01847         if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP ) {
01848             status = VL53L0X_ERROR_TIME_OUT;
01849             break;
01850         }
01851 
01852         vl53l0x_polling_delay( dev );
01853     } while ( 1 );
01854 
01855     LOG_FUNCTION_END( status );
01856 
01857     return status;
01858 }
01859 
01860 /* Group PAL Interrupt Functions */
01861 VL53L0X_Error VL53L0X::vl53l0x_clear_interrupt_mask( VL53L0X_DEV dev, uint32_t interrupt_mask )
01862 {
01863     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01864     uint8_t loop_count;
01865     uint8_t byte;
01866     LOG_FUNCTION_START( "" );
01867 
01868     /* clear bit 0 range interrupt, bit 1 error interrupt */
01869     loop_count = 0;
01870     do {
01871         status = vl53l0x_write_byte( dev,
01872                                      VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01 );
01873         status |= vl53l0x_write_byte( dev,
01874                                       VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00 );
01875         status |= vl53l0x_read_byte( dev,
01876                                      VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte );
01877         loop_count++;
01878     } while ( ( ( byte & 0x07 ) != 0x00 )
01879               && ( loop_count < 3 )
01880               && ( status == VL53L0X_ERROR_NONE ) );
01881 
01882 
01883     if ( loop_count >= 3 )
01884         status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
01885 
01886     LOG_FUNCTION_END( status );
01887     return status;
01888 }
01889 
01890 VL53L0X_Error VL53L0X::vl53l0x_perform_single_ref_calibration( VL53L0X_DEV dev,
01891         uint8_t vhv_init_byte )
01892 {
01893     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01894 
01895     if ( status == VL53L0X_ERROR_NONE )
01896         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START,
01897                                      VL53L0X_REG_SYSRANGE_MODE_START_STOP |
01898                                      vhv_init_byte );
01899 
01900     if ( status == VL53L0X_ERROR_NONE )
01901         status = vl53l0x_measurement_poll_for_completion( dev );
01902 
01903     if ( status == VL53L0X_ERROR_NONE )
01904         status = vl53l0x_clear_interrupt_mask( dev, 0 );
01905 
01906     if ( status == VL53L0X_ERROR_NONE )
01907         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START, 0x00 );
01908 
01909     return status;
01910 }
01911 
01912 VL53L0X_Error VL53L0X::vl53l0x_ref_calibration_io( VL53L0X_DEV dev, uint8_t read_not_write,
01913         uint8_t vhv_settings, uint8_t phase_cal,
01914         uint8_t *p_vhv_settings, uint8_t *p_phase_cal,
01915         const uint8_t vhv_enable, const uint8_t phase_enable )
01916 {
01917     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01918     uint8_t phase_calint = 0;
01919 
01920     /* Read VHV from device */
01921     status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
01922     status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
01923     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
01924 
01925     if ( read_not_write ) {
01926         if ( vhv_enable )
01927             status |= vl53l0x_read_byte( dev, 0xCB, p_vhv_settings );
01928         if ( phase_enable )
01929             status |= vl53l0x_read_byte( dev, 0xEE, &phase_calint );
01930     } else {
01931         if ( vhv_enable )
01932             status |= vl53l0x_write_byte( dev, 0xCB, vhv_settings );
01933         if ( phase_enable )
01934             status |= vl53l0x_update_byte( dev, 0xEE, 0x80, phase_cal );
01935     }
01936 
01937     status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
01938     status |= vl53l0x_write_byte( dev, 0x00, 0x01 );
01939     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
01940 
01941     *p_phase_cal = ( uint8_t )( phase_calint & 0xEF );
01942 
01943     return status;
01944 }
01945 
01946 VL53L0X_Error VL53L0X::vl53l0x_perform_vhv_calibration( VL53L0X_DEV dev,
01947         uint8_t *p_vhv_settings, const uint8_t get_data_enable,
01948         const uint8_t restore_config )
01949 {
01950     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01951     uint8_t sequence_config = 0;
01952     uint8_t vhv_settings = 0;
01953     uint8_t phase_cal = 0;
01954     uint8_t phase_cal_int = 0;
01955 
01956     /* store the value of the sequence config,
01957      * this will be reset before the end of the function
01958      */
01959 
01960     if ( restore_config )
01961         sequence_config = PALDevDataGet( dev, SequenceConfig );
01962 
01963     /* Run VHV */
01964     status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01 );
01965 
01966     if ( status == VL53L0X_ERROR_NONE )
01967         status = vl53l0x_perform_single_ref_calibration( dev, 0x40 );
01968 
01969     /* Read VHV from device */
01970     if ( ( status == VL53L0X_ERROR_NONE ) && ( get_data_enable == 1 ) ) {
01971         status = vl53l0x_ref_calibration_io( dev, 1,
01972                                              vhv_settings, phase_cal, /* Not used here */
01973                                              p_vhv_settings, &phase_cal_int,
01974                                              1, 0 );
01975     } else
01976         *p_vhv_settings = 0;
01977 
01978 
01979     if ( ( status == VL53L0X_ERROR_NONE ) && restore_config ) {
01980         /* restore the previous Sequence Config */
01981         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01982                                      sequence_config );
01983         if ( status == VL53L0X_ERROR_NONE )
01984             PALDevDataSet( dev, SequenceConfig, sequence_config );
01985 
01986     }
01987 
01988     return status;
01989 }
01990 
01991 VL53L0X_Error VL53L0X::vl53l0x_perform_phase_calibration( VL53L0X_DEV dev,
01992         uint8_t *p_phase_cal, const uint8_t get_data_enable,
01993         const uint8_t restore_config )
01994 {
01995     VL53L0X_Error status = VL53L0X_ERROR_NONE;
01996     uint8_t sequence_config = 0;
01997     uint8_t vhv_settings = 0;
01998     uint8_t phase_cal = 0;
01999     uint8_t vhv_settingsint;
02000 
02001     /* store the value of the sequence config,
02002      * this will be reset before the end of the function
02003      */
02004 
02005     if ( restore_config )
02006         sequence_config = PALDevDataGet( dev, SequenceConfig );
02007 
02008     /* Run PhaseCal */
02009     status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02 );
02010 
02011     if ( status == VL53L0X_ERROR_NONE )
02012         status = vl53l0x_perform_single_ref_calibration( dev, 0x0 );
02013 
02014     /* Read PhaseCal from device */
02015     if ( ( status == VL53L0X_ERROR_NONE ) && ( get_data_enable == 1 ) ) {
02016         status = vl53l0x_ref_calibration_io( dev, 1,
02017                                              vhv_settings, phase_cal, /* Not used here */
02018                                              &vhv_settingsint, p_phase_cal,
02019                                              0, 1 );
02020     } else
02021         *p_phase_cal = 0;
02022 
02023 
02024     if ( ( status == VL53L0X_ERROR_NONE ) && restore_config ) {
02025         /* restore the previous Sequence Config */
02026         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
02027                                      sequence_config );
02028         if ( status == VL53L0X_ERROR_NONE )
02029             PALDevDataSet( dev, SequenceConfig, sequence_config );
02030 
02031     }
02032 
02033     return status;
02034 }
02035 
02036 VL53L0X_Error VL53L0X::vl53l0x_perform_ref_calibration( VL53L0X_DEV dev,
02037         uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable )
02038 {
02039     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02040     uint8_t sequence_config = 0;
02041 
02042     /* store the value of the sequence config,
02043      * this will be reset before the end of the function
02044      */
02045 
02046     sequence_config = PALDevDataGet( dev, SequenceConfig );
02047 
02048     /* In the following function we don't save the config to optimize
02049      * writes on device. Config is saved and restored only once. */
02050     status = vl53l0x_perform_vhv_calibration(
02051                  dev, p_vhv_settings, get_data_enable, 0 );
02052 
02053 
02054     if ( status == VL53L0X_ERROR_NONE )
02055         status = vl53l0x_perform_phase_calibration(
02056                      dev, p_phase_cal, get_data_enable, 0 );
02057 
02058 
02059     if ( status == VL53L0X_ERROR_NONE ) {
02060         /* restore the previous Sequence Config */
02061         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
02062                                      sequence_config );
02063         if ( status == VL53L0X_ERROR_NONE )
02064             PALDevDataSet( dev, SequenceConfig, sequence_config );
02065 
02066     }
02067 
02068     return status;
02069 }
02070 
02071 void VL53L0X::get_next_good_spad( uint8_t good_spad_array[], uint32_t size,
02072                                   uint32_t curr, int32_t *p_next )
02073 {
02074     uint32_t start_index;
02075     uint32_t fine_offset;
02076     uint32_t c_spads_per_byte = 8;
02077     uint32_t coarse_index;
02078     uint32_t fine_index;
02079     uint8_t data_byte;
02080     uint8_t success = 0;
02081 
02082     /*
02083      * Starting with the current good spad, loop through the array to find
02084      * the next. i.e. the next bit set in the sequence.
02085      *
02086      * The coarse index is the byte index of the array and the fine index is
02087      * the index of the bit within each byte.
02088      */
02089 
02090     *p_next = -1;
02091 
02092     start_index = curr / c_spads_per_byte;
02093     fine_offset = curr % c_spads_per_byte;
02094 
02095     for ( coarse_index = start_index; ( ( coarse_index < size ) && !success );
02096             coarse_index++ ) {
02097         fine_index = 0;
02098         data_byte = good_spad_array[coarse_index];
02099 
02100         if ( coarse_index == start_index ) {
02101             /* locate the bit position of the provided current
02102              * spad bit before iterating */
02103             data_byte >>= fine_offset;
02104             fine_index = fine_offset;
02105         }
02106 
02107         while ( fine_index < c_spads_per_byte ) {
02108             if ( ( data_byte & 0x1 ) == 1 ) {
02109                 success = 1;
02110                 *p_next = coarse_index * c_spads_per_byte + fine_index;
02111                 break;
02112             }
02113             data_byte >>= 1;
02114             fine_index++;
02115         }
02116     }
02117 }
02118 
02119 uint8_t VL53L0X::is_aperture( uint32_t spad_index )
02120 {
02121     /*
02122      * This function reports if a given spad index is an aperture SPAD by
02123      * deriving the quadrant.
02124      */
02125     uint32_t quadrant;
02126     uint8_t is_aperture = 1;
02127     quadrant = spad_index >> 6;
02128     if ( refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0 )
02129         is_aperture = 0;
02130 
02131     return is_aperture;
02132 }
02133 
02134 VL53L0X_Error VL53L0X::enable_spad_bit( uint8_t spad_array[], uint32_t size,
02135                                         uint32_t spad_index )
02136 {
02137     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02138     uint32_t c_spads_per_byte = 8;
02139     uint32_t coarse_index;
02140     uint32_t fine_index;
02141 
02142     coarse_index = spad_index / c_spads_per_byte;
02143     fine_index = spad_index % c_spads_per_byte;
02144     if ( coarse_index >= size )
02145         status = VL53L0X_ERROR_REF_SPAD_INIT;
02146     else
02147         spad_array[coarse_index] |= ( 1 << fine_index );
02148 
02149     return status;
02150 }
02151 
02152 VL53L0X_Error VL53L0X::set_ref_spad_map( VL53L0X_DEV dev, uint8_t *p_ref_spad_array )
02153 {
02154     VL53L0X_Error status = vl53l0x_write_multi( dev,
02155                            VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
02156                            p_ref_spad_array, 6 );
02157 
02158     return status;
02159 }
02160 
02161 VL53L0X_Error VL53L0X::get_ref_spad_map( VL53L0X_DEV dev, uint8_t *p_ref_spad_array )
02162 {
02163     VL53L0X_Error status = vl53l0x_read_multi( dev,
02164                            VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
02165                            p_ref_spad_array,
02166                            6 );
02167 //  VL53L0X_Error status = VL53L0X_ERROR_NONE;
02168 //  uint8_t count=0;
02169 
02170 //  for (count = 0; count < 6; count++)
02171 //        status = vl53l0x_read_byte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]);
02172     return status;
02173 }
02174 
02175 VL53L0X_Error VL53L0X::enable_ref_spads( VL53L0X_DEV dev,
02176         uint8_t aperture_spads,
02177         uint8_t good_spad_array[],
02178         uint8_t spad_array[],
02179         uint32_t size,
02180         uint32_t start,
02181         uint32_t offset,
02182         uint32_t spad_count,
02183         uint32_t *p_last_spad )
02184 {
02185     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02186     uint32_t index;
02187     uint32_t i;
02188     int32_t next_good_spad = offset;
02189     uint32_t current_spad;
02190     uint8_t check_spad_array[6];
02191 
02192     /*
02193      * This function takes in a spad array which may or may not have SPADS
02194      * already enabled and appends from a given offset a requested number
02195      * of new SPAD enables. The 'good spad map' is applied to
02196      * determine the next SPADs to enable.
02197      *
02198      * This function applies to only aperture or only non-aperture spads.
02199      * Checks are performed to ensure this.
02200      */
02201 
02202     current_spad = offset;
02203     for ( index = 0; index < spad_count; index++ ) {
02204         get_next_good_spad( good_spad_array, size, current_spad,
02205                             &next_good_spad );
02206 
02207         if ( next_good_spad == -1 ) {
02208             status = VL53L0X_ERROR_REF_SPAD_INIT;
02209             break;
02210         }
02211 
02212         /* Confirm that the next good SPAD is non-aperture */
02213         if ( is_aperture( start + next_good_spad ) != aperture_spads ) {
02214             /* if we can't get the required number of good aperture
02215              * spads from the current quadrant then this is an error
02216              */
02217             status = VL53L0X_ERROR_REF_SPAD_INIT;
02218             break;
02219         }
02220         current_spad = ( uint32_t )next_good_spad;
02221         enable_spad_bit( spad_array, size, current_spad );
02222         current_spad++;
02223     }
02224     *p_last_spad = current_spad;
02225 
02226     if ( status == VL53L0X_ERROR_NONE )
02227         status = set_ref_spad_map( dev, spad_array );
02228 
02229 
02230     if ( status == VL53L0X_ERROR_NONE ) {
02231         status = get_ref_spad_map( dev, check_spad_array );
02232 
02233         i = 0;
02234 
02235         /* Compare spad maps. If not equal report error. */
02236         while ( i < size ) {
02237             if ( spad_array[i] != check_spad_array[i] ) {
02238                 status = VL53L0X_ERROR_REF_SPAD_INIT;
02239                 break;
02240             }
02241             i++;
02242         }
02243     }
02244     return status;
02245 }
02246 
02247 VL53L0X_Error VL53L0X::vl53l0x_set_device_mode( VL53L0X_DEV dev, VL53L0X_DeviceModes device_mode )
02248 {
02249     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02250 
02251     LOG_FUNCTION_START( "%d", ( int )DeviceMode );
02252 
02253     switch ( device_mode ) {
02254         case VL53L0X_DEVICEMODE_SINGLE_RANGING:
02255         case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
02256         case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
02257         case VL53L0X_DEVICEMODE_GPIO_DRIVE:
02258         case VL53L0X_DEVICEMODE_GPIO_OSC:
02259             /* Supported modes */
02260             VL53L0X_SETPARAMETERFIELD( dev, DeviceMode, device_mode );
02261             break;
02262         default:
02263             /* Unsupported mode */
02264             status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
02265     }
02266 
02267     LOG_FUNCTION_END( status );
02268     return status;
02269 }
02270 
02271 VL53L0X_Error VL53L0X::vl53l0x_set_interrupt_thresholds( VL53L0X_DEV dev,
02272         VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low,
02273         FixPoint1616_t threshold_high )
02274 {
02275     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02276     uint16_t threshold16;
02277     LOG_FUNCTION_START( "" );
02278 
02279     /* no dependency on DeviceMode for Ewok */
02280     /* Need to divide by 2 because the FW will apply a x2 */
02281     threshold16 = ( uint16_t )( ( threshold_low >> 17 ) & 0x00fff );
02282     status = vl53l0x_write_word( dev, VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16 );
02283 
02284     if ( status == VL53L0X_ERROR_NONE ) {
02285         /* Need to divide by 2 because the FW will apply a x2 */
02286         threshold16 = ( uint16_t )( ( threshold_high >> 17 ) & 0x00fff );
02287         status = vl53l0x_write_word( dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
02288                                      threshold16 );
02289     }
02290 
02291     LOG_FUNCTION_END( status );
02292     return status;
02293 }
02294 
02295 VL53L0X_Error VL53L0X::vl53l0x_get_interrupt_thresholds( VL53L0X_DEV dev,
02296         VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low,
02297         FixPoint1616_t *p_threshold_high )
02298 {
02299     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02300     uint16_t threshold16;
02301     LOG_FUNCTION_START( "" );
02302 
02303     /* no dependency on DeviceMode for Ewok */
02304 
02305     status = vl53l0x_read_word( dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16 );
02306     /* Need to multiply by 2 because the FW will apply a x2 */
02307     *p_threshold_low = ( FixPoint1616_t )( ( 0x00fff & threshold16 ) << 17 );
02308 
02309     if ( status == VL53L0X_ERROR_NONE ) {
02310         status = vl53l0x_read_word( dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
02311                                     &threshold16 );
02312         /* Need to multiply by 2 because the FW will apply a x2 */
02313         *p_threshold_high =
02314             ( FixPoint1616_t )( ( 0x00fff & threshold16 ) << 17 );
02315     }
02316 
02317     LOG_FUNCTION_END( status );
02318     return status;
02319 }
02320 
02321 VL53L0X_Error VL53L0X::vl53l0x_load_tuning_settings( VL53L0X_DEV dev,
02322         uint8_t *p_tuning_setting_buffer )
02323 {
02324     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02325     int i;
02326     int index;
02327     uint8_t msb;
02328     uint8_t lsb;
02329     uint8_t select_param;
02330     uint8_t number_of_writes;
02331     uint8_t address;
02332     uint8_t local_buffer[4]; /* max */
02333     uint16_t temp16;
02334 
02335     LOG_FUNCTION_START( "" );
02336 
02337     index = 0;
02338 
02339     while ( ( *( p_tuning_setting_buffer + index ) != 0 ) &&
02340             ( status == VL53L0X_ERROR_NONE ) ) {
02341         number_of_writes = *( p_tuning_setting_buffer + index );
02342         index++;
02343         if ( number_of_writes == 0xFF ) {
02344             /* internal parameters */
02345             select_param = *( p_tuning_setting_buffer + index );
02346             index++;
02347             switch ( select_param ) {
02348                 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
02349                     msb = *( p_tuning_setting_buffer + index );
02350                     index++;
02351                     lsb = *( p_tuning_setting_buffer + index );
02352                     index++;
02353                     temp16 = VL53L0X_MAKEUINT16( lsb, msb );
02354                     PALDevDataSet( dev, SigmaEstRefArray, temp16 );
02355                     break;
02356                 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
02357                     msb = *( p_tuning_setting_buffer + index );
02358                     index++;
02359                     lsb = *( p_tuning_setting_buffer + index );
02360                     index++;
02361                     temp16 = VL53L0X_MAKEUINT16( lsb, msb );
02362                     PALDevDataSet( dev, SigmaEstEffPulseWidth,
02363                                    temp16 );
02364                     break;
02365                 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
02366                     msb = *( p_tuning_setting_buffer + index );
02367                     index++;
02368                     lsb = *( p_tuning_setting_buffer + index );
02369                     index++;
02370                     temp16 = VL53L0X_MAKEUINT16( lsb, msb );
02371                     PALDevDataSet( dev, SigmaEstEffAmbWidth, temp16 );
02372                     break;
02373                 case 3: /* uint16_t targetRefRate -> 2 bytes */
02374                     msb = *( p_tuning_setting_buffer + index );
02375                     index++;
02376                     lsb = *( p_tuning_setting_buffer + index );
02377                     index++;
02378                     temp16 = VL53L0X_MAKEUINT16( lsb, msb );
02379                     PALDevDataSet( dev, targetRefRate, temp16 );
02380                     break;
02381                 default: /* invalid parameter */
02382                     status = VL53L0X_ERROR_INVALID_PARAMS;
02383             }
02384 
02385         } else if ( number_of_writes <= 4 ) {
02386             address = *( p_tuning_setting_buffer + index );
02387             index++;
02388 
02389             for ( i = 0; i < number_of_writes; i++ ) {
02390                 local_buffer[i] = *( p_tuning_setting_buffer +
02391                                      index );
02392                 index++;
02393             }
02394 
02395             status = vl53l0x_write_multi( dev, address, local_buffer,
02396                                           number_of_writes );
02397 
02398         } else {
02399             status = VL53L0X_ERROR_INVALID_PARAMS;
02400         }
02401     }
02402 
02403     LOG_FUNCTION_END( status );
02404     return status;
02405 }
02406 
02407 VL53L0X_Error VL53L0X::vl53l0x_check_and_load_interrupt_settings( VL53L0X_DEV dev,
02408         uint8_t start_not_stopflag )
02409 {
02410     uint8_t interrupt_config;
02411     FixPoint1616_t threshold_low;
02412     FixPoint1616_t threshold_high;
02413     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02414 
02415     interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
02416                        Pin0GpioFunctionality );
02417 
02418     if ( ( interrupt_config ==
02419             VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW ) ||
02420             ( interrupt_config ==
02421               VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH ) ||
02422             ( interrupt_config ==
02423               VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT ) ) {
02424 
02425         status = vl53l0x_get_interrupt_thresholds( dev,
02426                  VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
02427                  &threshold_low, &threshold_high );
02428 
02429         if ( ( ( threshold_low > 255 * 65536 ) ||
02430                 ( threshold_high > 255 * 65536 ) ) &&
02431                 ( status == VL53L0X_ERROR_NONE ) ) {
02432 
02433             if ( start_not_stopflag != 0 ) {
02434                 status = vl53l0x_load_tuning_settings( dev,
02435                                                        InterruptThresholdSettings );
02436             } else {
02437                 status |= vl53l0x_write_byte( dev, 0xFF, 0x04 );
02438                 status |= vl53l0x_write_byte( dev, 0x70, 0x00 );
02439                 status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
02440                 status |= vl53l0x_write_byte( dev, 0x80, 0x00 );
02441             }
02442 
02443         }
02444 
02445 
02446     }
02447 
02448     return status;
02449 
02450 }
02451 
02452 VL53L0X_Error VL53L0X::vl53l0x_start_measurement( VL53L0X_DEV dev )
02453 {
02454     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02455     VL53L0X_DeviceModes device_mode;
02456     uint8_t byte;
02457     uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
02458     uint32_t loop_nb;
02459     LOG_FUNCTION_START( "" );
02460 
02461     /* Get Current DeviceMode */
02462     vl53l0x_get_device_mode( dev, &device_mode );
02463 
02464     status = vl53l0x_write_byte( dev, 0x80, 0x01 );
02465     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
02466     status = vl53l0x_write_byte( dev, 0x00, 0x00 );
02467     status = vl53l0x_write_byte( dev, 0x91, PALDevDataGet( dev, StopVariable ) );
02468     status = vl53l0x_write_byte( dev, 0x00, 0x01 );
02469     status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
02470     status = vl53l0x_write_byte( dev, 0x80, 0x00 );
02471 
02472     switch ( device_mode ) {
02473         case VL53L0X_DEVICEMODE_SINGLE_RANGING:
02474             status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START, 0x01 );
02475 
02476             byte = start_stop_byte;
02477             if ( status == VL53L0X_ERROR_NONE ) {
02478                 /* Wait until start bit has been cleared */
02479                 loop_nb = 0;
02480                 do {
02481                     if ( loop_nb > 0 )
02482                         status = vl53l0x_read_byte( dev,
02483                                                     VL53L0X_REG_SYSRANGE_START, &byte );
02484                     loop_nb = loop_nb + 1;
02485                 } while ( ( ( byte & start_stop_byte ) == start_stop_byte )
02486                           && ( status == VL53L0X_ERROR_NONE )
02487                           && ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP ) );
02488 
02489                 if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP )
02490                     status = VL53L0X_ERROR_TIME_OUT;
02491 
02492             }
02493 
02494             break;
02495         case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
02496             /* Back-to-back mode */
02497 
02498             /* Check if need to apply interrupt settings */
02499             if ( status == VL53L0X_ERROR_NONE )
02500                 status = vl53l0x_check_and_load_interrupt_settings( dev, 1 );
02501 
02502             status = vl53l0x_write_byte( dev,
02503                                          VL53L0X_REG_SYSRANGE_START,
02504                                          VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK );
02505             if ( status == VL53L0X_ERROR_NONE ) {
02506                 /* Set PAL State to Running */
02507                 PALDevDataSet( dev, PalState, VL53L0X_STATE_RUNNING );
02508             }
02509             break;
02510         case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
02511             /* Continuous mode */
02512             /* Check if need to apply interrupt settings */
02513             if ( status == VL53L0X_ERROR_NONE )
02514                 status = vl53l0x_check_and_load_interrupt_settings( dev, 1 );
02515 
02516             status = vl53l0x_write_byte( dev,
02517                                          VL53L0X_REG_SYSRANGE_START,
02518                                          VL53L0X_REG_SYSRANGE_MODE_TIMED );
02519 
02520             if ( status == VL53L0X_ERROR_NONE ) {
02521                 /* Set PAL State to Running */
02522                 PALDevDataSet( dev, PalState, VL53L0X_STATE_RUNNING );
02523             }
02524             break;
02525         default:
02526             /* Selected mode not supported */
02527             status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
02528     }
02529 
02530 
02531     LOG_FUNCTION_END( status );
02532     return status;
02533 }
02534 
02535 /* Group PAL Measurement Functions */
02536 VL53L0X_Error VL53L0X::vl53l0x_perform_single_measurement( VL53L0X_DEV dev )
02537 {
02538     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02539     VL53L0X_DeviceModes device_mode;
02540 
02541     LOG_FUNCTION_START( "" );
02542 
02543     /* Get Current DeviceMode */
02544     status = vl53l0x_get_device_mode( dev, &device_mode );
02545 
02546     /* Start immediately to run a single ranging measurement in case of
02547      * single ranging or single histogram */
02548     if ( status == VL53L0X_ERROR_NONE
02549             && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING )
02550         status = vl53l0x_start_measurement( dev );
02551 
02552 
02553     if ( status == VL53L0X_ERROR_NONE )
02554         status = vl53l0x_measurement_poll_for_completion( dev );
02555 
02556 
02557     /* Change PAL State in case of single ranging or single histogram */
02558     if ( status == VL53L0X_ERROR_NONE
02559             && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING )
02560         PALDevDataSet( dev, PalState, VL53L0X_STATE_IDLE );
02561 
02562 
02563     LOG_FUNCTION_END( status );
02564     return status;
02565 }
02566 
02567 VL53L0X_Error VL53L0X::vl53l0x_get_x_talk_compensation_enable( VL53L0X_DEV dev,
02568         uint8_t *p_x_talk_compensation_enable )
02569 {
02570     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02571     uint8_t temp8;
02572     LOG_FUNCTION_START( "" );
02573 
02574     VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationEnable, temp8 );
02575     *p_x_talk_compensation_enable = temp8;
02576 
02577     LOG_FUNCTION_END( status );
02578     return status;
02579 }
02580 
02581 VL53L0X_Error VL53L0X::vl53l0x_get_total_xtalk_rate( VL53L0X_DEV dev,
02582         VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
02583         FixPoint1616_t *p_total_xtalk_rate_mcps )
02584 {
02585     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02586 
02587     uint8_t xtalk_comp_enable;
02588     FixPoint1616_t total_xtalk_mega_cps;
02589     FixPoint1616_t xtalk_per_spad_mega_cps;
02590 
02591     *p_total_xtalk_rate_mcps = 0;
02592 
02593     status = vl53l0x_get_x_talk_compensation_enable( dev, &xtalk_comp_enable );
02594     if ( status == VL53L0X_ERROR_NONE ) {
02595 
02596         if ( xtalk_comp_enable ) {
02597 
02598             VL53L0X_GETPARAMETERFIELD(
02599                 dev,
02600                 XTalkCompensationRateMegaCps,
02601                 xtalk_per_spad_mega_cps );
02602 
02603             /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
02604             total_xtalk_mega_cps =
02605                 p_ranging_measurement_data->EffectiveSpadRtnCount *
02606                 xtalk_per_spad_mega_cps;
02607 
02608             /* FixPoint0824 >> 8 = FixPoint1616 */
02609             *p_total_xtalk_rate_mcps =
02610                 ( total_xtalk_mega_cps + 0x80 ) >> 8;
02611         }
02612     }
02613 
02614     return status;
02615 }
02616 
02617 VL53L0X_Error VL53L0X::vl53l0x_get_total_signal_rate( VL53L0X_DEV dev,
02618         VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
02619         FixPoint1616_t *p_total_signal_rate_mcps )
02620 {
02621     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02622     FixPoint1616_t total_xtalk_mega_cps;
02623 
02624     LOG_FUNCTION_START( "" );
02625 
02626     *p_total_signal_rate_mcps =
02627         p_ranging_measurement_data->SignalRateRtnMegaCps;
02628 
02629     status = vl53l0x_get_total_xtalk_rate(
02630                  dev, p_ranging_measurement_data, &total_xtalk_mega_cps );
02631 
02632     if ( status == VL53L0X_ERROR_NONE )
02633         *p_total_signal_rate_mcps += total_xtalk_mega_cps;
02634 
02635     return status;
02636 }
02637 
02638 /* To convert ms into register value */
02639 uint32_t VL53L0X::vl53l0x_calc_timeout_mclks( VL53L0X_DEV dev,
02640         uint32_t timeout_period_us,
02641         uint8_t vcsel_period_pclks )
02642 {
02643     uint32_t macro_period_ps;
02644     uint32_t macro_period_ns;
02645     uint32_t timeout_period_mclks = 0;
02646 
02647     macro_period_ps = vl53l0x_calc_macro_period_ps( dev, vcsel_period_pclks );
02648     macro_period_ns = ( macro_period_ps + 500 ) / 1000;
02649 
02650     timeout_period_mclks =
02651         ( uint32_t ) ( ( ( timeout_period_us * 1000 )
02652                          + ( macro_period_ns / 2 ) ) / macro_period_ns );
02653 
02654     return timeout_period_mclks;
02655 }
02656 
02657 uint32_t VL53L0X::vl53l0x_isqrt( uint32_t num )
02658 {
02659     /*
02660      * Implements an integer square root
02661      *
02662      * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
02663      */
02664 
02665     uint32_t  res = 0;
02666     uint32_t  bit = 1 << 30;
02667     /* The second-to-top bit is set:
02668      *  1 << 14 for 16-bits, 1 << 30 for 32 bits */
02669 
02670     /* "bit" starts at the highest power of four <= the argument. */
02671     while ( bit > num )
02672         bit >>= 2;
02673 
02674 
02675     while ( bit != 0 ) {
02676         if ( num >= res + bit ) {
02677             num -= res + bit;
02678             res = ( res >> 1 ) + bit;
02679         } else
02680             res >>= 1;
02681 
02682         bit >>= 2;
02683     }
02684 
02685     return res;
02686 }
02687 
02688 uint32_t VL53L0X::vl53l0x_quadrature_sum( uint32_t a, uint32_t b )
02689 {
02690     /*
02691      * Implements a quadrature sum
02692      *
02693      * rea = sqrt(a^2 + b^2)
02694      *
02695      * Trap overflow case max input value is 65535 (16-bit value)
02696      * as internal calc are 32-bit wide
02697      *
02698      * If overflow then seta output to maximum
02699      */
02700     uint32_t  res = 0;
02701 
02702     if ( a > 65535 || b > 65535 )
02703         res = 65535;
02704     else
02705         res = vl53l0x_isqrt( a * a + b * b );
02706 
02707     return res;
02708 }
02709 
02710 VL53L0X_Error VL53L0X::vl53l0x_calc_dmax(
02711     VL53L0X_DEV dev,
02712     FixPoint1616_t total_signal_rate_mcps,
02713     FixPoint1616_t total_corr_signal_rate_mcps,
02714     FixPoint1616_t pw_mult,
02715     uint32_t sigma_estimate_p1,
02716     FixPoint1616_t sigma_estimate_p2,
02717     uint32_t peak_vcsel_duration_us,
02718     uint32_t *pd_max_mm )
02719 {
02720     const uint32_t c_sigma_limit        = 18;
02721     const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
02722     const FixPoint1616_t c_sigma_est_ref    = 0x00000042; /* 0.001 */
02723     const uint32_t c_amb_eff_width_sigma_est_ns = 6;
02724     const uint32_t c_amb_eff_width_d_max_ns    = 7;
02725     uint32_t dmax_cal_range_mm;
02726     FixPoint1616_t dmax_cal_signal_rate_rtn_mcps;
02727     FixPoint1616_t min_signal_needed;
02728     FixPoint1616_t min_signal_needed_p1;
02729     FixPoint1616_t min_signal_needed_p2;
02730     FixPoint1616_t min_signal_needed_p3;
02731     FixPoint1616_t min_signal_needed_p4;
02732     FixPoint1616_t sigma_limit_tmp;
02733     FixPoint1616_t sigma_est_sq_tmp;
02734     FixPoint1616_t signal_limit_tmp;
02735     FixPoint1616_t signal_at0_mm;
02736     FixPoint1616_t dmax_dark;
02737     FixPoint1616_t dmax_ambient;
02738     FixPoint1616_t dmax_dark_tmp;
02739     FixPoint1616_t sigma_est_p2_tmp;
02740     uint32_t signal_rate_temp_mcps;
02741 
02742     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02743 
02744     LOG_FUNCTION_START( "" );
02745 
02746     dmax_cal_range_mm =
02747         PALDevDataGet( dev, DmaxCalRangeMilliMeter );
02748 
02749     dmax_cal_signal_rate_rtn_mcps =
02750         PALDevDataGet( dev, DmaxCalSignalRateRtnMegaCps );
02751 
02752     /* uint32 * FixPoint1616 = FixPoint1616 */
02753     signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
02754 
02755     /* FixPoint1616 >> 8 = FixPoint2408 */
02756     signal_at0_mm = ( signal_at0_mm + 0x80 ) >> 8;
02757     signal_at0_mm *= dmax_cal_range_mm;
02758 
02759     min_signal_needed_p1 = 0;
02760     if ( total_corr_signal_rate_mcps > 0 ) {
02761 
02762         /* Shift by 10 bits to increase resolution prior to the
02763          * division */
02764         signal_rate_temp_mcps = total_signal_rate_mcps << 10;
02765 
02766         /* Add rounding value prior to division */
02767         min_signal_needed_p1 = signal_rate_temp_mcps +
02768                                ( total_corr_signal_rate_mcps / 2 );
02769 
02770         /* FixPoint0626/FixPoint1616 = FixPoint2210 */
02771         min_signal_needed_p1 /= total_corr_signal_rate_mcps;
02772 
02773         /* Apply a factored version of the speed of light.
02774          Correction to be applied at the end */
02775         min_signal_needed_p1 *= 3;
02776 
02777         /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
02778         min_signal_needed_p1 *= min_signal_needed_p1;
02779 
02780         /* FixPoint1220 >> 16 = FixPoint2804 */
02781         min_signal_needed_p1 = ( min_signal_needed_p1 + 0x8000 ) >> 16;
02782     }
02783 
02784     min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
02785 
02786     /* FixPoint1616 >> 16 =  uint32 */
02787     min_signal_needed_p2 = ( min_signal_needed_p2 + 0x8000 ) >> 16;
02788 
02789     /* uint32 * uint32  =  uint32 */
02790     min_signal_needed_p2 *= min_signal_needed_p2;
02791 
02792     /* Check sigmaEstimateP2
02793      * If this value is too high there is not enough signal rate
02794      * to calculate dmax value so set a suitable value to ensure
02795      * a very small dmax.
02796      */
02797     sigma_est_p2_tmp = ( sigma_estimate_p2 + 0x8000 ) >> 16;
02798     sigma_est_p2_tmp = ( sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2 ) /
02799                        c_amb_eff_width_sigma_est_ns;
02800     sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
02801 
02802     if ( sigma_est_p2_tmp > 0xffff ) {
02803         min_signal_needed_p3 = 0xfff00000;
02804     } else {
02805 
02806         /* DMAX uses a different ambient width from sigma, so apply
02807          * correction.
02808          * Perform division before multiplication to prevent overflow.
02809          */
02810         sigma_estimate_p2 = ( sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2 ) /
02811                             c_amb_eff_width_sigma_est_ns;
02812         sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
02813 
02814         /* FixPoint1616 >> 16 = uint32 */
02815         min_signal_needed_p3 = ( sigma_estimate_p2 + 0x8000 ) >> 16;
02816 
02817         min_signal_needed_p3 *= min_signal_needed_p3;
02818 
02819     }
02820 
02821     /* FixPoint1814 / uint32 = FixPoint1814 */
02822     sigma_limit_tmp = ( ( c_sigma_limit << 14 ) + 500 ) / 1000;
02823 
02824     /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
02825     sigma_limit_tmp *= sigma_limit_tmp;
02826 
02827     /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
02828     sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
02829 
02830     /* FixPoint3232 >> 4 = FixPoint0428 */
02831     sigma_est_sq_tmp = ( sigma_est_sq_tmp + 0x08 ) >> 4;
02832 
02833     /* FixPoint0428 - FixPoint0428  = FixPoint0428 */
02834     sigma_limit_tmp -=  sigma_est_sq_tmp;
02835 
02836     /* uint32_t * FixPoint0428 = FixPoint0428 */
02837     min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
02838 
02839     /* FixPoint0428 >> 14 = FixPoint1814 */
02840     min_signal_needed_p4 = ( min_signal_needed_p4 + 0x2000 ) >> 14;
02841 
02842     /* uint32 + uint32 = uint32 */
02843     min_signal_needed = ( min_signal_needed_p2 + min_signal_needed_p3 );
02844 
02845     /* uint32 / uint32 = uint32 */
02846     min_signal_needed += ( peak_vcsel_duration_us / 2 );
02847     min_signal_needed /= peak_vcsel_duration_us;
02848 
02849     /* uint32 << 14 = FixPoint1814 */
02850     min_signal_needed <<= 14;
02851 
02852     /* FixPoint1814 / FixPoint1814 = uint32 */
02853     min_signal_needed += ( min_signal_needed_p4 / 2 );
02854     min_signal_needed /= min_signal_needed_p4;
02855 
02856     /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
02857     min_signal_needed *= min_signal_needed_p1;
02858 
02859     /* Apply correction by dividing by 1000000.
02860      * This assumes 10E16 on the numerator of the equation
02861      * and 10E-22 on the denominator.
02862      * We do this because 32bit fix point calculation can't
02863      * handle the larger and smaller elements of this equation,
02864      * i.e. speed of light and pulse widths.
02865      */
02866     min_signal_needed = ( min_signal_needed + 500 ) / 1000;
02867     min_signal_needed <<= 4;
02868 
02869     min_signal_needed = ( min_signal_needed + 500 ) / 1000;
02870 
02871     /* FixPoint1616 >> 8 = FixPoint2408 */
02872     signal_limit_tmp = ( c_signal_limit + 0x80 ) >> 8;
02873 
02874     /* FixPoint2408/FixPoint2408 = uint32 */
02875     if ( signal_limit_tmp != 0 )
02876         dmax_dark_tmp = ( signal_at0_mm + ( signal_limit_tmp / 2 ) )
02877                         / signal_limit_tmp;
02878     else
02879         dmax_dark_tmp = 0;
02880 
02881     dmax_dark = vl53l0x_isqrt( dmax_dark_tmp );
02882 
02883     /* FixPoint2408/FixPoint2408 = uint32 */
02884     if ( min_signal_needed != 0 )
02885         dmax_ambient = ( signal_at0_mm + min_signal_needed / 2 )
02886                        / min_signal_needed;
02887     else
02888         dmax_ambient = 0;
02889 
02890     dmax_ambient = vl53l0x_isqrt( dmax_ambient );
02891 
02892     *pd_max_mm = dmax_dark;
02893     if ( dmax_dark > dmax_ambient )
02894         *pd_max_mm = dmax_ambient;
02895 
02896     LOG_FUNCTION_END( status );
02897 
02898     return status;
02899 }
02900 
02901 VL53L0X_Error VL53L0X::vl53l0x_calc_sigma_estimate( VL53L0X_DEV dev,
02902         VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
02903         FixPoint1616_t *p_sigma_estimate,
02904         uint32_t *p_dmax_mm )
02905 {
02906     /* Expressed in 100ths of a ns, i.e. centi-ns */
02907     const uint32_t c_pulse_effective_width_centi_ns   = 800;
02908     /* Expressed in 100ths of a ns, i.e. centi-ns */
02909     const uint32_t c_ambient_effective_width_centi_ns = 600;
02910     const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
02911     const uint32_t c_vcsel_pulse_width_ps   = 4700; /* pico secs */
02912     const FixPoint1616_t c_sigma_est_max    = 0x028F87AE;
02913     const FixPoint1616_t c_sigma_est_rtn_max    = 0xF000;
02914     const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
02915             c_ambient_effective_width_centi_ns;
02916     /* Time Of Flight per mm (6.6 pico secs) */
02917     const FixPoint1616_t c_tof_per_mm_ps        = 0x0006999A;
02918     const uint32_t c_16bit_rounding_param       = 0x00008000;
02919     const FixPoint1616_t c_max_x_talk_kcps      = 0x00320000;
02920     const uint32_t c_pll_period_ps          = 1655;
02921 
02922     uint32_t vcsel_total_events_rtn;
02923     uint32_t final_range_timeout_micro_secs;
02924     uint32_t pre_range_timeout_micro_secs;
02925     uint32_t final_range_integration_time_milli_secs;
02926     FixPoint1616_t sigma_estimate_p1;
02927     FixPoint1616_t sigma_estimate_p2;
02928     FixPoint1616_t sigma_estimate_p3;
02929     FixPoint1616_t delta_t_ps;
02930     FixPoint1616_t pw_mult;
02931     FixPoint1616_t sigma_est_rtn;
02932     FixPoint1616_t sigma_estimate;
02933     FixPoint1616_t x_talk_correction;
02934     FixPoint1616_t ambient_rate_kcps;
02935     FixPoint1616_t peak_signal_rate_kcps;
02936     FixPoint1616_t x_talk_comp_rate_mcps;
02937     uint32_t x_talk_comp_rate_kcps;
02938     VL53L0X_Error status = VL53L0X_ERROR_NONE;
02939     FixPoint1616_t diff1_mcps;
02940     FixPoint1616_t diff2_mcps;
02941     FixPoint1616_t sqr1;
02942     FixPoint1616_t sqr2;
02943     FixPoint1616_t sqr_sum;
02944     FixPoint1616_t sqrt_result_centi_ns;
02945     FixPoint1616_t sqrt_result;
02946     FixPoint1616_t total_signal_rate_mcps;
02947     FixPoint1616_t corrected_signal_rate_mcps;
02948     FixPoint1616_t sigma_est_ref;
02949     uint32_t vcsel_width;
02950     uint32_t final_range_macro_pclks;
02951     uint32_t pre_range_macro_pclks;
02952     uint32_t peak_vcsel_duration_us;
02953     uint8_t final_range_vcsel_pclks;
02954     uint8_t pre_range_vcsel_pclks;
02955     /*! \addtogroup calc_sigma_estimate
02956      * @{
02957      *
02958      * Estimates the range sigma
02959      */
02960 
02961     LOG_FUNCTION_START( "" );
02962 
02963     VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps,
02964                                x_talk_comp_rate_mcps );
02965 
02966     /*
02967      * We work in kcps rather than mcps as this helps keep within the
02968      * confines of the 32 Fix1616 type.
02969      */
02970 
02971     ambient_rate_kcps =
02972         ( p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000 ) >> 16;
02973 
02974     corrected_signal_rate_mcps =
02975         p_ranging_measurement_data->SignalRateRtnMegaCps;
02976 
02977 
02978     status = vl53l0x_get_total_signal_rate(
02979                  dev, p_ranging_measurement_data, &total_signal_rate_mcps );
02980     status = vl53l0x_get_total_xtalk_rate(
02981                  dev, p_ranging_measurement_data, &x_talk_comp_rate_mcps );
02982 
02983 
02984     /* Signal rate measurement provided by device is the
02985      * peak signal rate, not average.
02986      */
02987     peak_signal_rate_kcps = ( total_signal_rate_mcps * 1000 );
02988     peak_signal_rate_kcps = ( peak_signal_rate_kcps + 0x8000 ) >> 16;
02989 
02990     x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
02991 
02992     if ( x_talk_comp_rate_kcps > c_max_x_talk_kcps )
02993         x_talk_comp_rate_kcps = c_max_x_talk_kcps;
02994 
02995     if ( status == VL53L0X_ERROR_NONE ) {
02996 
02997         /* Calculate final range macro periods */
02998         final_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
02999                                              dev, FinalRangeTimeoutMicroSecs );
03000 
03001         final_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
03002                                       dev, FinalRangeVcselPulsePeriod );
03003 
03004         final_range_macro_pclks = vl53l0x_calc_timeout_mclks(
03005                                       dev, final_range_timeout_micro_secs, final_range_vcsel_pclks );
03006 
03007         /* Calculate pre-range macro periods */
03008         pre_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
03009                                            dev, PreRangeTimeoutMicroSecs );
03010 
03011         pre_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
03012                                     dev, PreRangeVcselPulsePeriod );
03013 
03014         pre_range_macro_pclks = vl53l0x_calc_timeout_mclks(
03015                                     dev, pre_range_timeout_micro_secs, pre_range_vcsel_pclks );
03016 
03017         vcsel_width = 3;
03018         if ( final_range_vcsel_pclks == 8 )
03019             vcsel_width = 2;
03020 
03021 
03022         peak_vcsel_duration_us = vcsel_width * 2048 *
03023                                  ( pre_range_macro_pclks + final_range_macro_pclks );
03024         peak_vcsel_duration_us = ( peak_vcsel_duration_us + 500 ) / 1000;
03025         peak_vcsel_duration_us *= c_pll_period_ps;
03026         peak_vcsel_duration_us = ( peak_vcsel_duration_us + 500 ) / 1000;
03027 
03028         /* Fix1616 >> 8 = Fix2408 */
03029         total_signal_rate_mcps = ( total_signal_rate_mcps + 0x80 ) >> 8;
03030 
03031         /* Fix2408 * uint32 = Fix2408 */
03032         vcsel_total_events_rtn = total_signal_rate_mcps *
03033                                  peak_vcsel_duration_us;
03034 
03035         /* Fix2408 >> 8 = uint32 */
03036         vcsel_total_events_rtn = ( vcsel_total_events_rtn + 0x80 ) >> 8;
03037 
03038         /* Fix2408 << 8 = Fix1616 = */
03039         total_signal_rate_mcps <<= 8;
03040     }
03041 
03042     if ( status != VL53L0X_ERROR_NONE ) {
03043         LOG_FUNCTION_END( status );
03044         return status;
03045     }
03046 
03047     if ( peak_signal_rate_kcps == 0 ) {
03048         *p_sigma_estimate = c_sigma_est_max;
03049         PALDevDataSet( dev, SigmaEstimate, c_sigma_est_max );
03050         *p_dmax_mm = 0;
03051     } else {
03052         if ( vcsel_total_events_rtn < 1 )
03053             vcsel_total_events_rtn = 1;
03054 
03055         sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
03056 
03057         /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
03058         sigma_estimate_p2 = ( ambient_rate_kcps << 16 ) / peak_signal_rate_kcps;
03059         if ( sigma_estimate_p2 > c_amb_to_signal_ratio_max ) {
03060             /* Clip to prevent overflow. Will ensure safe
03061              * max result. */
03062             sigma_estimate_p2 = c_amb_to_signal_ratio_max;
03063         }
03064         sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
03065 
03066         sigma_estimate_p3 = 2 * vl53l0x_isqrt( vcsel_total_events_rtn * 12 );
03067 
03068         /* uint32 * FixPoint1616 = FixPoint1616 */
03069         delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
03070                      c_tof_per_mm_ps;
03071 
03072         /*
03073          * vcselRate - xtalkCompRate
03074          * (uint32 << 16) - FixPoint1616 = FixPoint1616.
03075          * Divide result by 1000 to convert to mcps.
03076          * 500 is added to ensure rounding when integer division
03077          * truncates.
03078          */
03079         diff1_mcps = ( ( ( peak_signal_rate_kcps << 16 ) -
03080                          2 * x_talk_comp_rate_kcps ) + 500 ) / 1000;
03081 
03082         /* vcselRate + xtalkCompRate */
03083         diff2_mcps = ( ( peak_signal_rate_kcps << 16 ) + 500 ) / 1000;
03084 
03085         /* Shift by 8 bits to increase resolution prior to the
03086          * division */
03087         diff1_mcps <<= 8;
03088 
03089         /* FixPoint0824/FixPoint1616 = FixPoint2408 */
03090 //      xTalkCorrection  = abs(diff1_mcps/diff2_mcps);
03091 // abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
03092         x_talk_correction    = diff1_mcps / diff2_mcps;
03093 
03094         /* FixPoint2408 << 8 = FixPoint1616 */
03095         x_talk_correction <<= 8;
03096 
03097         if( p_ranging_measurement_data->RangeStatus != 0 ) {
03098             pw_mult = 1 << 16;
03099         } else {
03100             /* FixPoint1616/uint32 = FixPoint1616 */
03101             pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
03102 
03103             /*
03104              * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
03105              * values are small enough such that32 bits will not be
03106              * exceeded.
03107              */
03108             pw_mult *= ( ( 1 << 16 ) - x_talk_correction );
03109 
03110             /* (FixPoint3232 >> 16) = FixPoint1616 */
03111             pw_mult =  ( pw_mult + c_16bit_rounding_param ) >> 16;
03112 
03113             /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
03114             pw_mult += ( 1 << 16 );
03115 
03116             /*
03117              * At this point the value will be 1.xx, therefore if we square
03118              * the value this will exceed 32 bits. To address this perform
03119              * a single shift to the right before the multiplication.
03120              */
03121             pw_mult >>= 1;
03122             /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
03123             pw_mult = pw_mult * pw_mult;
03124 
03125             /* (FixPoint3430 >> 14) = Fix1616 */
03126             pw_mult >>= 14;
03127         }
03128 
03129         /* FixPoint1616 * uint32 = FixPoint1616 */
03130         sqr1 = pw_mult * sigma_estimate_p1;
03131 
03132         /* (FixPoint1616 >> 16) = FixPoint3200 */
03133         sqr1 = ( sqr1 + 0x8000 ) >> 16;
03134 
03135         /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
03136         sqr1 *= sqr1;
03137 
03138         sqr2 = sigma_estimate_p2;
03139 
03140         /* (FixPoint1616 >> 16) = FixPoint3200 */
03141         sqr2 = ( sqr2 + 0x8000 ) >> 16;
03142 
03143         /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
03144         sqr2 *= sqr2;
03145 
03146         /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
03147         sqr_sum = sqr1 + sqr2;
03148 
03149         /* SQRT(FixPoin6400) = FixPoint3200 */
03150         sqrt_result_centi_ns = vl53l0x_isqrt( sqr_sum );
03151 
03152         /* (FixPoint3200 << 16) = FixPoint1616 */
03153         sqrt_result_centi_ns <<= 16;
03154 
03155         /*
03156          * Note that the Speed Of Light is expressed in um per 1E-10
03157          * seconds (2997) Therefore to get mm/ns we have to divide by
03158          * 10000
03159          */
03160         sigma_est_rtn = ( ( ( sqrt_result_centi_ns + 50 ) / 100 ) /
03161                           sigma_estimate_p3 );
03162         sigma_est_rtn        *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
03163 
03164         /* Add 5000 before dividing by 10000 to ensure rounding. */
03165         sigma_est_rtn        += 5000;
03166         sigma_est_rtn        /= 10000;
03167 
03168         if ( sigma_est_rtn > c_sigma_est_rtn_max ) {
03169             /* Clip to prevent overflow. Will ensure safe
03170              * max result. */
03171             sigma_est_rtn = c_sigma_est_rtn_max;
03172         }
03173         final_range_integration_time_milli_secs =
03174             ( final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500 ) / 1000;
03175 
03176         /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
03177          * sqrt(FixPoint1616/int) = FixPoint2408)
03178          */
03179         sigma_est_ref =
03180             vl53l0x_isqrt( ( c_dflt_final_range_integration_time_milli_secs +
03181                              final_range_integration_time_milli_secs / 2 ) /
03182                            final_range_integration_time_milli_secs );
03183 
03184         /* FixPoint2408 << 8 = FixPoint1616 */
03185         sigma_est_ref <<= 8;
03186         sigma_est_ref = ( sigma_est_ref + 500 ) / 1000;
03187 
03188         /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
03189         sqr1 = sigma_est_rtn * sigma_est_rtn;
03190         /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
03191         sqr2 = sigma_est_ref * sigma_est_ref;
03192 
03193         /* sqrt(FixPoint3232) = FixPoint1616 */
03194         sqrt_result = vl53l0x_isqrt( ( sqr1 + sqr2 ) );
03195         /*
03196          * Note that the Shift by 4 bits increases resolution prior to
03197          * the sqrt, therefore the result must be shifted by 2 bits to
03198          * the right to revert back to the FixPoint1616 format.
03199          */
03200 
03201         sigma_estimate   = 1000 * sqrt_result;
03202 
03203         if ( ( peak_signal_rate_kcps < 1 ) || ( vcsel_total_events_rtn < 1 ) ||
03204                 ( sigma_estimate > c_sigma_est_max ) ) {
03205             sigma_estimate = c_sigma_est_max;
03206         }
03207 
03208         *p_sigma_estimate = ( uint32_t )( sigma_estimate );
03209         PALDevDataSet( dev, SigmaEstimate, *p_sigma_estimate );
03210         status = vl53l0x_calc_dmax(
03211                      dev,
03212                      total_signal_rate_mcps,
03213                      corrected_signal_rate_mcps,
03214                      pw_mult,
03215                      sigma_estimate_p1,
03216                      sigma_estimate_p2,
03217                      peak_vcsel_duration_us,
03218                      p_dmax_mm );
03219     }
03220 
03221     LOG_FUNCTION_END( status );
03222     return status;
03223 }
03224 
03225 VL53L0X_Error VL53L0X::vl53l0x_get_pal_range_status( VL53L0X_DEV dev,
03226         uint8_t device_range_status,
03227         FixPoint1616_t signal_rate,
03228         uint16_t effective_spad_rtn_count,
03229         VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
03230         uint8_t *p_pal_range_status )
03231 {
03232     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03233     uint8_t none_flag;
03234     uint8_t sigma_limitflag = 0;
03235     uint8_t signal_ref_clipflag = 0;
03236     uint8_t range_ignore_thresholdflag = 0;
03237     uint8_t sigma_limit_check_enable = 0;
03238     uint8_t signal_rate_final_range_limit_check_enable = 0;
03239     uint8_t signal_ref_clip_limit_check_enable = 0;
03240     uint8_t range_ignore_threshold_limit_check_enable = 0;
03241     FixPoint1616_t sigma_estimate;
03242     FixPoint1616_t sigma_limit_value;
03243     FixPoint1616_t signal_ref_clip_value;
03244     FixPoint1616_t range_ignore_threshold_value;
03245     FixPoint1616_t signal_rate_per_spad;
03246     uint8_t device_range_status_internal = 0;
03247     uint16_t tmp_word = 0;
03248     uint8_t temp8;
03249     uint32_t dmax_mm = 0;
03250     FixPoint1616_t last_signal_ref_mcps;
03251 
03252     LOG_FUNCTION_START( "" );
03253 
03254 
03255     /*
03256      * VL53L0X has a good ranging when the value of the
03257      * DeviceRangeStatus = 11. This function will replace the value 0 with
03258      * the value 11 in the DeviceRangeStatus.
03259      * In addition, the SigmaEstimator is not included in the VL53L0X
03260      * DeviceRangeStatus, this will be added in the PalRangeStatus.
03261      */
03262 
03263     device_range_status_internal = ( ( device_range_status & 0x78 ) >> 3 );
03264 
03265     if ( device_range_status_internal == 0 ||
03266             device_range_status_internal == 5 ||
03267             device_range_status_internal == 7 ||
03268             device_range_status_internal == 12 ||
03269             device_range_status_internal == 13 ||
03270             device_range_status_internal == 14 ||
03271             device_range_status_internal == 15
03272        ) {
03273         none_flag = 1;
03274     } else {
03275         none_flag = 0;
03276     }
03277 
03278     /*
03279      * Check if Sigma limit is enabled, if yes then do comparison with limit
03280      * value and put the result back into pPalRangeStatus.
03281      */
03282     if ( status == VL53L0X_ERROR_NONE )
03283         status =  vl53l0x_get_limit_check_enable( dev,
03284                   VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
03285                   &sigma_limit_check_enable );
03286 
03287     if ( ( sigma_limit_check_enable != 0 ) && ( status == VL53L0X_ERROR_NONE ) ) {
03288         /*
03289         * compute the Sigma and check with limit
03290         */
03291         status = vl53l0x_calc_sigma_estimate(
03292                      dev,
03293                      p_ranging_measurement_data,
03294                      &sigma_estimate,
03295                      &dmax_mm );
03296         if ( status == VL53L0X_ERROR_NONE )
03297             p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
03298 
03299         if ( status == VL53L0X_ERROR_NONE ) {
03300             status = vl53l0x_get_limit_check_value( dev,
03301                                                     VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
03302                                                     &sigma_limit_value );
03303 
03304             if ( ( sigma_limit_value > 0 ) &&
03305                     ( sigma_estimate > sigma_limit_value ) )
03306                 /* Limit Fail */
03307                 sigma_limitflag = 1;
03308         }
03309     }
03310 
03311     /*
03312      * Check if Signal ref clip limit is enabled, if yes then do comparison
03313      * with limit value and put the result back into pPalRangeStatus.
03314      */
03315     if ( status == VL53L0X_ERROR_NONE )
03316         status =  vl53l0x_get_limit_check_enable( dev,
03317                   VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
03318                   &signal_ref_clip_limit_check_enable );
03319 
03320     if ( ( signal_ref_clip_limit_check_enable != 0 ) &&
03321             ( status == VL53L0X_ERROR_NONE ) ) {
03322 
03323         status = vl53l0x_get_limit_check_value( dev,
03324                                                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
03325                                                 &signal_ref_clip_value );
03326 
03327         /* Read LastSignalRefMcps from device */
03328         if ( status == VL53L0X_ERROR_NONE )
03329             status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
03330 
03331         if ( status == VL53L0X_ERROR_NONE )
03332             status = vl53l0x_read_word( dev,
03333                                         VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
03334                                         &tmp_word );
03335 
03336         if ( status == VL53L0X_ERROR_NONE )
03337             status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
03338 
03339         last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616( tmp_word );
03340         PALDevDataSet( dev, LastSignalRefMcps, last_signal_ref_mcps );
03341 
03342         if ( ( signal_ref_clip_value > 0 ) &&
03343                 ( last_signal_ref_mcps > signal_ref_clip_value ) ) {
03344             /* Limit Fail */
03345             signal_ref_clipflag = 1;
03346         }
03347     }
03348 
03349     /*
03350      * Check if Signal ref clip limit is enabled, if yes then do comparison
03351      * with limit value and put the result back into pPalRangeStatus.
03352      * EffectiveSpadRtnCount has a format 8.8
03353      * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
03354      */
03355     if ( status == VL53L0X_ERROR_NONE )
03356         status =  vl53l0x_get_limit_check_enable( dev,
03357                   VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
03358                   &range_ignore_threshold_limit_check_enable );
03359 
03360     if ( ( range_ignore_threshold_limit_check_enable != 0 ) &&
03361             ( status == VL53L0X_ERROR_NONE ) ) {
03362 
03363         /* Compute the signal rate per spad */
03364         if ( effective_spad_rtn_count == 0 ) {
03365             signal_rate_per_spad = 0;
03366         } else {
03367             signal_rate_per_spad = ( FixPoint1616_t )( ( 256 * signal_rate )
03368                                    / effective_spad_rtn_count );
03369         }
03370 
03371         status = vl53l0x_get_limit_check_value( dev,
03372                                                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
03373                                                 &range_ignore_threshold_value );
03374 
03375         if ( ( range_ignore_threshold_value > 0 ) &&
03376                 ( signal_rate_per_spad < range_ignore_threshold_value ) ) {
03377             /* Limit Fail add 2^6 to range status */
03378             range_ignore_thresholdflag = 1;
03379         }
03380     }
03381 
03382     if ( status == VL53L0X_ERROR_NONE ) {
03383         if ( none_flag == 1 ) {
03384             *p_pal_range_status = 255;   /* NONE */
03385         } else if ( device_range_status_internal == 1 ||
03386                     device_range_status_internal == 2 ||
03387                     device_range_status_internal == 3 ) {
03388             *p_pal_range_status = 5; /* HW fail */
03389         } else if ( device_range_status_internal == 6 ||
03390                     device_range_status_internal == 9 ) {
03391             *p_pal_range_status = 4;  /* Phase fail */
03392         } else if ( device_range_status_internal == 8 ||
03393                     device_range_status_internal == 10 ||
03394                     signal_ref_clipflag == 1 ) {
03395             *p_pal_range_status = 3;  /* Min range */
03396         } else if ( device_range_status_internal == 4 ||
03397                     range_ignore_thresholdflag == 1 ) {
03398             *p_pal_range_status = 2;  /* Signal Fail */
03399         } else if ( sigma_limitflag == 1 ) {
03400             *p_pal_range_status = 1;  /* Sigma   Fail */
03401         } else {
03402             *p_pal_range_status = 0; /* Range Valid */
03403         }
03404     }
03405 
03406     /* DMAX only relevant during range error */
03407     if ( *p_pal_range_status == 0 )
03408         p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
03409 
03410     /* fill the Limit Check Status */
03411 
03412     status =  vl53l0x_get_limit_check_enable( dev,
03413               VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
03414               &signal_rate_final_range_limit_check_enable );
03415 
03416     if ( status == VL53L0X_ERROR_NONE ) {
03417         if ( ( sigma_limit_check_enable == 0 ) || ( sigma_limitflag == 1 ) )
03418             temp8 = 1;
03419         else
03420             temp8 = 0;
03421         VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
03422                                         VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp8 );
03423 
03424         if ( ( device_range_status_internal == 4 ) ||
03425                 ( signal_rate_final_range_limit_check_enable == 0 ) )
03426             temp8 = 1;
03427         else
03428             temp8 = 0;
03429         VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
03430                                         VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
03431                                         temp8 );
03432 
03433         if ( ( signal_ref_clip_limit_check_enable == 0 ) ||
03434                 ( signal_ref_clipflag == 1 ) )
03435             temp8 = 1;
03436         else
03437             temp8 = 0;
03438 
03439         VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
03440                                         VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp8 );
03441 
03442         if ( ( range_ignore_threshold_limit_check_enable == 0 ) ||
03443                 ( range_ignore_thresholdflag == 1 ) )
03444             temp8 = 1;
03445         else
03446             temp8 = 0;
03447 
03448         VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
03449                                         VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
03450                                         temp8 );
03451     }
03452 
03453     LOG_FUNCTION_END( status );
03454     return status;
03455 
03456 }
03457 
03458 VL53L0X_Error VL53L0X::vl53l0x_get_ranging_measurement_data( VL53L0X_DEV dev,
03459         VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data )
03460 {
03461     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03462     uint8_t device_range_status;
03463     uint8_t range_fractional_enable;
03464     uint8_t pal_range_status;
03465     uint8_t x_talk_compensation_enable;
03466     uint16_t ambient_rate;
03467     FixPoint1616_t signal_rate;
03468     uint16_t x_talk_compensation_rate_mega_cps;
03469     uint16_t effective_spad_rtn_count;
03470     uint16_t tmpuint16;
03471     uint16_t xtalk_range_milli_meter;
03472     uint16_t linearity_corrective_gain;
03473     uint8_t localBuffer[12];
03474     VL53L0X_RangingMeasurementData_t last_range_data_buffer;
03475 
03476     LOG_FUNCTION_START( "" );
03477 
03478     /*
03479      * use multi read even if some registers are not useful, result will
03480      * be more efficient
03481      * start reading at 0x14 dec20
03482      * end reading at 0x21 dec33 total 14 bytes to read
03483      */
03484     status = vl53l0x_read_multi( dev, 0x14, localBuffer, 12 );
03485 
03486     if ( status == VL53L0X_ERROR_NONE ) {
03487 
03488         p_ranging_measurement_data->ZoneId = 0; /* Only one zone */
03489         p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */
03490 
03491         tmpuint16 = VL53L0X_MAKEUINT16( localBuffer[11], localBuffer[10] );
03492         /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
03493          *(format 11.2) else no fractional
03494          */
03495 
03496         p_ranging_measurement_data->MeasurementTimeUsec = 0;
03497 
03498         signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
03499                           VL53L0X_MAKEUINT16( localBuffer[7], localBuffer[6] ) );
03500         /* peak_signal_count_rate_rtn_mcps */
03501         p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
03502 
03503         ambient_rate = VL53L0X_MAKEUINT16( localBuffer[9], localBuffer[8] );
03504         p_ranging_measurement_data->AmbientRateRtnMegaCps =
03505             VL53L0X_FIXPOINT97TOFIXPOINT1616( ambient_rate );
03506 
03507         effective_spad_rtn_count = VL53L0X_MAKEUINT16( localBuffer[3],
03508                                    localBuffer[2] );
03509         /* EffectiveSpadRtnCount is 8.8 format */
03510         p_ranging_measurement_data->EffectiveSpadRtnCount =
03511             effective_spad_rtn_count;
03512 
03513         device_range_status = localBuffer[0];
03514 
03515         /* Get Linearity Corrective Gain */
03516         linearity_corrective_gain = PALDevDataGet( dev,
03517                                     LinearityCorrectiveGain );
03518 
03519         /* Get ranging configuration */
03520         range_fractional_enable = PALDevDataGet( dev,
03521                                   RangeFractionalEnable );
03522 
03523         if ( linearity_corrective_gain != 1000 ) {
03524 
03525             tmpuint16 = ( uint16_t )( ( linearity_corrective_gain
03526                                         * tmpuint16 + 500 ) / 1000 );
03527 
03528             /* Implement Xtalk */
03529             VL53L0X_GETPARAMETERFIELD( dev,
03530                                        XTalkCompensationRateMegaCps,
03531                                        x_talk_compensation_rate_mega_cps );
03532             VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationEnable,
03533                                        x_talk_compensation_enable );
03534 
03535             if ( x_talk_compensation_enable ) {
03536 
03537                 if ( ( signal_rate
03538                         - ( ( x_talk_compensation_rate_mega_cps
03539                               * effective_spad_rtn_count ) >> 8 ) )
03540                         <= 0 ) {
03541                     if ( range_fractional_enable )
03542                         xtalk_range_milli_meter = 8888;
03543                     else
03544                         xtalk_range_milli_meter = 8888
03545                                                   << 2;
03546                 } else {
03547                     xtalk_range_milli_meter =
03548                         ( tmpuint16 * signal_rate )
03549                         / ( signal_rate
03550                             - ( ( x_talk_compensation_rate_mega_cps
03551                                   * effective_spad_rtn_count )
03552                                 >> 8 ) );
03553                 }
03554 
03555                 tmpuint16 = xtalk_range_milli_meter;
03556             }
03557 
03558         }
03559 
03560         if ( range_fractional_enable ) {
03561             p_ranging_measurement_data->RangeMilliMeter =
03562                 ( uint16_t )( ( tmpuint16 ) >> 2 );
03563             p_ranging_measurement_data->RangeFractionalPart =
03564                 ( uint8_t )( ( tmpuint16 & 0x03 ) << 6 );
03565         } else {
03566             p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
03567             p_ranging_measurement_data->RangeFractionalPart = 0;
03568         }
03569 
03570         /*
03571          * For a standard definition of RangeStatus, this should
03572          * return 0 in case of good result after a ranging
03573          * The range status depends on the device so call a device
03574          * specific function to obtain the right Status.
03575          */
03576         status |= vl53l0x_get_pal_range_status( dev, device_range_status,
03577                                                 signal_rate, effective_spad_rtn_count,
03578                                                 p_ranging_measurement_data, &pal_range_status );
03579 
03580         if ( status == VL53L0X_ERROR_NONE )
03581             p_ranging_measurement_data->RangeStatus = pal_range_status;
03582 
03583     }
03584 
03585     if ( status == VL53L0X_ERROR_NONE ) {
03586         /* Copy last read data into Dev buffer */
03587         last_range_data_buffer = PALDevDataGet( dev, LastRangeMeasure );
03588 
03589         last_range_data_buffer.RangeMilliMeter =
03590             p_ranging_measurement_data->RangeMilliMeter;
03591         last_range_data_buffer.RangeFractionalPart =
03592             p_ranging_measurement_data->RangeFractionalPart;
03593         last_range_data_buffer.RangeDMaxMilliMeter =
03594             p_ranging_measurement_data->RangeDMaxMilliMeter;
03595         last_range_data_buffer.MeasurementTimeUsec =
03596             p_ranging_measurement_data->MeasurementTimeUsec;
03597         last_range_data_buffer.SignalRateRtnMegaCps =
03598             p_ranging_measurement_data->SignalRateRtnMegaCps;
03599         last_range_data_buffer.AmbientRateRtnMegaCps =
03600             p_ranging_measurement_data->AmbientRateRtnMegaCps;
03601         last_range_data_buffer.EffectiveSpadRtnCount =
03602             p_ranging_measurement_data->EffectiveSpadRtnCount;
03603         last_range_data_buffer.RangeStatus =
03604             p_ranging_measurement_data->RangeStatus;
03605 
03606         PALDevDataSet( dev, LastRangeMeasure, last_range_data_buffer );
03607     }
03608 
03609     LOG_FUNCTION_END( status );
03610     return status;
03611 }
03612 
03613 VL53L0X_Error VL53L0X::vl53l0x_perform_single_ranging_measurement( VL53L0X_DEV dev,
03614         VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data )
03615 {
03616     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03617 
03618     LOG_FUNCTION_START( "" );
03619 
03620     /* This function will do a complete single ranging
03621      * Here we fix the mode! */
03622     status = vl53l0x_set_device_mode( dev, VL53L0X_DEVICEMODE_SINGLE_RANGING );
03623 
03624     if ( status == VL53L0X_ERROR_NONE )
03625         status = vl53l0x_perform_single_measurement( dev );
03626 
03627 
03628     if ( status == VL53L0X_ERROR_NONE )
03629         status = vl53l0x_get_ranging_measurement_data( dev,
03630                  p_ranging_measurement_data );
03631 
03632 
03633     if ( status == VL53L0X_ERROR_NONE )
03634         status = vl53l0x_clear_interrupt_mask( dev, 0 );
03635 
03636 
03637     LOG_FUNCTION_END( status );
03638     return status;
03639 }
03640 
03641 VL53L0X_Error VL53L0X::perform_ref_signal_measurement( VL53L0X_DEV dev,
03642         uint16_t *p_ref_signal_rate )
03643 {
03644     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03645     VL53L0X_RangingMeasurementData_t ranging_measurement_data;
03646 
03647     uint8_t sequence_config = 0;
03648 
03649     /* store the value of the sequence config,
03650      * this will be reset before the end of the function
03651      */
03652 
03653     sequence_config = PALDevDataGet( dev, SequenceConfig );
03654 
03655     /*
03656      * This function performs a reference signal rate measurement.
03657      */
03658     if ( status == VL53L0X_ERROR_NONE )
03659         status = vl53l0x_write_byte( dev,
03660                                      VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0 );
03661 
03662     if ( status == VL53L0X_ERROR_NONE )
03663         status = vl53l0x_perform_single_ranging_measurement( dev,
03664                  &ranging_measurement_data );
03665 
03666     if ( status == VL53L0X_ERROR_NONE )
03667         status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
03668 
03669     if ( status == VL53L0X_ERROR_NONE )
03670         status = vl53l0x_read_word( dev,
03671                                     VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
03672                                     p_ref_signal_rate );
03673 
03674     if ( status == VL53L0X_ERROR_NONE )
03675         status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
03676 
03677     if ( status == VL53L0X_ERROR_NONE ) {
03678         /* restore the previous Sequence Config */
03679         status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
03680                                      sequence_config );
03681         if ( status == VL53L0X_ERROR_NONE )
03682             PALDevDataSet( dev, SequenceConfig, sequence_config );
03683     }
03684 
03685     return status;
03686 }
03687 
03688 VL53L0X_Error VL53L0X::wrapped_vl53l0x_perform_ref_spad_management( VL53L0X_DEV dev,
03689         uint32_t *ref_spad_count,
03690         uint8_t *is_aperture_spads )
03691 {
03692     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03693     uint8_t last_spad_array[6];
03694     uint8_t start_select = 0xB4;
03695     uint32_t minimum_spad_count = 3;
03696     uint32_t max_spad_count = 44;
03697     uint32_t current_spad_index = 0;
03698     uint32_t last_spad_index = 0;
03699     int32_t next_good_spad = 0;
03700     uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */
03701     uint16_t peak_signal_rate_ref;
03702     uint32_t need_apt_spads = 0;
03703     uint32_t index = 0;
03704     uint32_t spad_array_size = 6;
03705     uint32_t signal_rate_diff = 0;
03706     uint32_t last_signal_rate_diff = 0;
03707     uint8_t complete = 0;
03708     uint8_t vhv_settings = 0;
03709     uint8_t phase_cal = 0;
03710     uint32_t ref_spad_count_int = 0;
03711     uint8_t  is_aperture_spads_int = 0;
03712 
03713     /*
03714      * The reference SPAD initialization procedure determines the minimum
03715      * amount of reference spads to be enables to achieve a target reference
03716      * signal rate and should be performed once during initialization.
03717      *
03718      * Either aperture or non-aperture spads are applied but never both.
03719      * Firstly non-aperture spads are set, begining with 5 spads, and
03720      * increased one spad at a time until the closest measurement to the
03721      * target rate is achieved.
03722      *
03723      * If the target rate is exceeded when 5 non-aperture spads are enabled,
03724      * initialization is performed instead with aperture spads.
03725      *
03726      * When setting spads, a 'Good Spad Map' is applied.
03727      *
03728      * This procedure operates within a SPAD window of interest of a maximum
03729      * 44 spads.
03730      * The start point is currently fixed to 180, which lies towards the end
03731      * of the non-aperture quadrant and runs in to the adjacent aperture
03732      * quadrant.
03733      */
03734 
03735 
03736     target_ref_rate = PALDevDataGet( dev, targetRefRate );
03737 
03738     /*
03739      * Initialize Spad arrays.
03740      * Currently the good spad map is initialised to 'All good'.
03741      * This is a short term implementation. The good spad map will be
03742      * provided as an input.
03743      * Note that there are 6 bytes. Only the first 44 bits will be used to
03744      * represent spads.
03745      */
03746     for ( index = 0; index < spad_array_size; index++ )
03747         dev->Data .SpadData .RefSpadEnables [index] = 0;
03748 
03749 
03750     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
03751 
03752     if ( status == VL53L0X_ERROR_NONE )
03753         status = vl53l0x_write_byte( dev,
03754                                      VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00 );
03755 
03756     if ( status == VL53L0X_ERROR_NONE )
03757         status = vl53l0x_write_byte( dev,
03758                                      VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C );
03759 
03760     if ( status == VL53L0X_ERROR_NONE )
03761         status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
03762 
03763     if ( status == VL53L0X_ERROR_NONE )
03764         status = vl53l0x_write_byte( dev,
03765                                      VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
03766                                      start_select );
03767 
03768 
03769     if ( status == VL53L0X_ERROR_NONE )
03770         status = vl53l0x_write_byte( dev,
03771                                      VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0 );
03772 
03773     /* Perform ref calibration */
03774     if ( status == VL53L0X_ERROR_NONE )
03775         status = vl53l0x_perform_ref_calibration( dev, &vhv_settings,
03776                  &phase_cal, 0 );
03777 
03778     if ( status == VL53L0X_ERROR_NONE ) {
03779         /* Enable Minimum NON-APERTURE Spads */
03780         current_spad_index = 0;
03781         last_spad_index = current_spad_index;
03782         need_apt_spads = 0;
03783         status = enable_ref_spads( dev,
03784                                    need_apt_spads,
03785                                    dev->Data .SpadData .RefGoodSpadMap ,
03786                                    dev->Data .SpadData .RefSpadEnables ,
03787                                    spad_array_size,
03788                                    start_select,
03789                                    current_spad_index,
03790                                    minimum_spad_count,
03791                                    &last_spad_index );
03792     }
03793 
03794     if ( status == VL53L0X_ERROR_NONE ) {
03795         current_spad_index = last_spad_index;
03796 
03797         status = perform_ref_signal_measurement( dev,
03798                  &peak_signal_rate_ref );
03799         if ( ( status == VL53L0X_ERROR_NONE ) &&
03800                 ( peak_signal_rate_ref > target_ref_rate ) ) {
03801             /* Signal rate measurement too high,
03802              * switch to APERTURE SPADs */
03803 
03804             for ( index = 0; index < spad_array_size; index++ )
03805                 dev->Data .SpadData .RefSpadEnables [index] = 0;
03806 
03807 
03808             /* Increment to the first APERTURE spad */
03809             while ( ( is_aperture( start_select + current_spad_index )
03810                       == 0 ) && ( current_spad_index < max_spad_count ) ) {
03811                 current_spad_index++;
03812             }
03813 
03814             need_apt_spads = 1;
03815 
03816             status = enable_ref_spads( dev,
03817                                        need_apt_spads,
03818                                        dev->Data .SpadData .RefGoodSpadMap ,
03819                                        dev->Data .SpadData .RefSpadEnables ,
03820                                        spad_array_size,
03821                                        start_select,
03822                                        current_spad_index,
03823                                        minimum_spad_count,
03824                                        &last_spad_index );
03825 
03826             if ( status == VL53L0X_ERROR_NONE ) {
03827                 current_spad_index = last_spad_index;
03828                 status = perform_ref_signal_measurement( dev,
03829                          &peak_signal_rate_ref );
03830 
03831                 if ( ( status == VL53L0X_ERROR_NONE ) &&
03832                         ( peak_signal_rate_ref > target_ref_rate ) ) {
03833                     /* Signal rate still too high after
03834                      * setting the minimum number of
03835                      * APERTURE spads. Can do no more
03836                      * therefore set the min number of
03837                      * aperture spads as the result.
03838                      */
03839                     is_aperture_spads_int = 1;
03840                     ref_spad_count_int = minimum_spad_count;
03841                 }
03842             }
03843         } else {
03844             need_apt_spads = 0;
03845         }
03846     }
03847 
03848     if ( ( status == VL53L0X_ERROR_NONE ) &&
03849             ( peak_signal_rate_ref < target_ref_rate ) ) {
03850         /* At this point, the minimum number of either aperture
03851          * or non-aperture spads have been set. Proceed to add
03852          * spads and perform measurements until the target
03853          * reference is reached.
03854          */
03855         is_aperture_spads_int = need_apt_spads;
03856         ref_spad_count_int  = minimum_spad_count;
03857 
03858         memcpy( last_spad_array, dev->Data .SpadData .RefSpadEnables ,
03859                 spad_array_size );
03860         last_signal_rate_diff = abs( peak_signal_rate_ref -
03861                                      target_ref_rate );
03862         complete = 0;
03863 
03864         while ( !complete ) {
03865             get_next_good_spad(
03866                 dev->Data .SpadData .RefGoodSpadMap ,
03867                 spad_array_size, current_spad_index,
03868                 &next_good_spad );
03869 
03870             if ( next_good_spad == -1 ) {
03871                 status = VL53L0X_ERROR_REF_SPAD_INIT;
03872                 break;
03873             }
03874 
03875             /* Cannot combine Aperture and Non-Aperture spads, so
03876              * ensure the current spad is of the correct type.
03877              */
03878             if ( is_aperture( ( uint32_t )start_select + next_good_spad ) !=
03879                     need_apt_spads ) {
03880                 /* At this point we have enabled the maximum
03881                  * number of Aperture spads.
03882                  */
03883                 complete = 1;
03884                 break;
03885             }
03886 
03887             ( ref_spad_count_int )++;
03888 
03889             current_spad_index = next_good_spad;
03890             status = enable_spad_bit(
03891                          dev->Data .SpadData .RefSpadEnables ,
03892                          spad_array_size, current_spad_index );
03893 
03894             if ( status == VL53L0X_ERROR_NONE ) {
03895                 current_spad_index++;
03896                 /* Proceed to apply the additional spad and
03897                  * perform measurement. */
03898                 status = set_ref_spad_map( dev,
03899                                            dev->Data .SpadData .RefSpadEnables  );
03900             }
03901 
03902             if ( status != VL53L0X_ERROR_NONE )
03903                 break;
03904 
03905             status = perform_ref_signal_measurement( dev,
03906                      &peak_signal_rate_ref );
03907 
03908             if ( status != VL53L0X_ERROR_NONE )
03909                 break;
03910 
03911             signal_rate_diff = abs( peak_signal_rate_ref - target_ref_rate );
03912 
03913             if ( peak_signal_rate_ref > target_ref_rate ) {
03914                 /* Select the spad map that provides the
03915                  * measurement closest to the target rate,
03916                  * either above or below it.
03917                  */
03918                 if ( signal_rate_diff > last_signal_rate_diff ) {
03919                     /* Previous spad map produced a closer
03920                      * measurement, so choose this. */
03921                     status = set_ref_spad_map( dev,
03922                                                last_spad_array );
03923                     memcpy(
03924                         dev->Data .SpadData .RefSpadEnables ,
03925                         last_spad_array, spad_array_size );
03926 
03927                     ( ref_spad_count_int )--;
03928                 }
03929                 complete = 1;
03930             } else {
03931                 /* Continue to add spads */
03932                 last_signal_rate_diff = signal_rate_diff;
03933                 memcpy( last_spad_array,
03934                         dev->Data .SpadData .RefSpadEnables ,
03935                         spad_array_size );
03936             }
03937 
03938         } /* while */
03939     }
03940 
03941     if ( status == VL53L0X_ERROR_NONE ) {
03942         *ref_spad_count = ref_spad_count_int;
03943         *is_aperture_spads = is_aperture_spads_int;
03944 
03945         VL53L0X_SETDEVICESPECIFICPARAMETER( dev, RefSpadsInitialised, 1 );
03946         VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
03947                                             ReferenceSpadCount, ( uint8_t )( *ref_spad_count ) );
03948         VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
03949                                             ReferenceSpadType, *is_aperture_spads );
03950     }
03951 
03952     return status;
03953 }
03954 
03955 VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_reference_spads( VL53L0X_DEV dev,
03956         uint32_t count, uint8_t is_aperture_spads )
03957 {
03958     VL53L0X_Error status = VL53L0X_ERROR_NONE;
03959     uint32_t current_spad_index = 0;
03960     uint8_t start_select = 0xB4;
03961     uint32_t spad_array_size = 6;
03962     uint32_t max_spad_count = 44;
03963     uint32_t last_spad_index;
03964     uint32_t index;
03965 
03966     /*
03967      * This function applies a requested number of reference spads, either
03968      * aperture or
03969      * non-aperture, as requested.
03970      * The good spad map will be applied.
03971      */
03972 
03973     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
03974 
03975     if ( status == VL53L0X_ERROR_NONE )
03976         status = vl53l0x_write_byte( dev,
03977                                      VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00 );
03978 
03979     if ( status == VL53L0X_ERROR_NONE )
03980         status = vl53l0x_write_byte( dev,
03981                                      VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C );
03982 
03983     if ( status == VL53L0X_ERROR_NONE )
03984         status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
03985 
03986     if ( status == VL53L0X_ERROR_NONE )
03987         status = vl53l0x_write_byte( dev,
03988                                      VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
03989                                      start_select );
03990 
03991     for ( index = 0; index < spad_array_size; index++ )
03992         dev->Data .SpadData .RefSpadEnables [index] = 0;
03993 
03994     if ( is_aperture_spads ) {
03995         /* Increment to the first APERTURE spad */
03996         while ( ( is_aperture( start_select + current_spad_index ) == 0 ) &&
03997                 ( current_spad_index < max_spad_count ) ) {
03998             current_spad_index++;
03999         }
04000     }
04001     status = enable_ref_spads( dev,
04002                                is_aperture_spads,
04003                                dev->Data .SpadData .RefGoodSpadMap ,
04004                                dev->Data .SpadData .RefSpadEnables ,
04005                                spad_array_size,
04006                                start_select,
04007                                current_spad_index,
04008                                count,
04009                                &last_spad_index );
04010 
04011     if ( status == VL53L0X_ERROR_NONE ) {
04012         VL53L0X_SETDEVICESPECIFICPARAMETER( dev, RefSpadsInitialised, 1 );
04013         VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
04014                                             ReferenceSpadCount, ( uint8_t )( count ) );
04015         VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
04016                                             ReferenceSpadType, is_aperture_spads );
04017     }
04018 
04019     return status;
04020 }
04021 
04022 VL53L0X_Error VL53L0X::vl53l0x_wait_device_booted( VL53L0X_DEV dev )
04023 {
04024     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
04025     LOG_FUNCTION_START( "" );
04026 
04027     /* not implemented on VL53L0X */
04028 
04029     LOG_FUNCTION_END( status );
04030     return status;
04031 }
04032 
04033 VL53L0X_Error VL53L0X::vl53l0x_perform_ref_calibration( VL53L0X_DEV dev, uint8_t *p_vhv_settings,
04034         uint8_t *p_phase_cal )
04035 {
04036     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04037     LOG_FUNCTION_START( "" );
04038 
04039     status = vl53l0x_perform_ref_calibration( dev, p_vhv_settings,
04040              p_phase_cal, 1 );
04041 
04042     LOG_FUNCTION_END( status );
04043     return status;
04044 }
04045 
04046 VL53L0X_Error VL53L0X::vl53l0x_perform_ref_spad_management( VL53L0X_DEV dev,
04047         uint32_t *ref_spad_count, uint8_t *is_aperture_spads )
04048 {
04049     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04050     LOG_FUNCTION_START( "" );
04051 
04052     status = wrapped_vl53l0x_perform_ref_spad_management( dev, ref_spad_count,
04053              is_aperture_spads );
04054 
04055     LOG_FUNCTION_END( status );
04056 
04057     return status;
04058 }
04059 
04060 /* Group PAL Init Functions */
04061 VL53L0X_Error VL53L0X::vl53l0x_set_device_address( VL53L0X_DEV dev, uint8_t device_address )
04062 {
04063     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04064     LOG_FUNCTION_START( "" );
04065 
04066     status = vl53l0x_write_byte( dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
04067                                  device_address / 2 );
04068 
04069     LOG_FUNCTION_END( status );
04070     return status;
04071 }
04072 
04073 VL53L0X_Error VL53L0X::vl53l0x_set_gpio_config( VL53L0X_DEV dev, uint8_t pin,
04074         VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
04075         VL53L0X_InterruptPolarity polarity )
04076 {
04077     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04078     uint8_t data;
04079 
04080     LOG_FUNCTION_START( "" );
04081 
04082     if ( pin != 0 ) {
04083         status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
04084     } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE ) {
04085         if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
04086             data = 0x10;
04087         else
04088             data = 1;
04089 
04090         status = vl53l0x_write_byte( dev,
04091                                      VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data );
04092 
04093     } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_OSC ) {
04094 
04095         status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
04096         status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
04097 
04098         status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
04099         status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
04100         status |= vl53l0x_write_byte( dev, 0x85, 0x02 );
04101 
04102         status |= vl53l0x_write_byte( dev, 0xff, 0x04 );
04103         status |= vl53l0x_write_byte( dev, 0xcd, 0x00 );
04104         status |= vl53l0x_write_byte( dev, 0xcc, 0x11 );
04105 
04106         status |= vl53l0x_write_byte( dev, 0xff, 0x07 );
04107         status |= vl53l0x_write_byte( dev, 0xbe, 0x00 );
04108 
04109         status |= vl53l0x_write_byte( dev, 0xff, 0x06 );
04110         status |= vl53l0x_write_byte( dev, 0xcc, 0x09 );
04111 
04112         status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
04113         status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
04114         status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
04115 
04116     } else {
04117 
04118         if ( status == VL53L0X_ERROR_NONE ) {
04119             switch ( functionality ) {
04120                 case VL53L0X_GPIOFUNCTIONALITY_OFF:
04121                     data = 0x00;
04122                     break;
04123                 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
04124                     data = 0x01;
04125                     break;
04126                 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
04127                     data = 0x02;
04128                     break;
04129                 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
04130                     data = 0x03;
04131                     break;
04132                 case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
04133                     data = 0x04;
04134                     break;
04135                 default:
04136                     status =
04137                         VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
04138             }
04139         }
04140 
04141         if ( status == VL53L0X_ERROR_NONE )
04142             status = vl53l0x_write_byte( dev,
04143                                          VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data );
04144 
04145         if ( status == VL53L0X_ERROR_NONE ) {
04146             if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
04147                 data = 0;
04148             else
04149                 data = ( uint8_t )( 1 << 4 );
04150 
04151             status = vl53l0x_update_byte( dev,
04152                                           VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data );
04153         }
04154 
04155         if ( status == VL53L0X_ERROR_NONE )
04156             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
04157                                                 Pin0GpioFunctionality, functionality );
04158 
04159         if ( status == VL53L0X_ERROR_NONE )
04160             status = vl53l0x_clear_interrupt_mask( dev, 0 );
04161 
04162     }
04163     LOG_FUNCTION_END( status );
04164     return status;
04165 }
04166 
04167 VL53L0X_Error VL53L0X::vl53l0x_get_fraction_enable( VL53L0X_DEV dev, uint8_t *p_enabled )
04168 {
04169     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04170     LOG_FUNCTION_START( "" );
04171 
04172     status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled );
04173 
04174     if ( status == VL53L0X_ERROR_NONE )
04175         *p_enabled = ( *p_enabled & 1 );
04176 
04177     LOG_FUNCTION_END( status );
04178     return status;
04179 }
04180 
04181 uint16_t VL53L0X::vl53l0x_encode_timeout( uint32_t timeout_macro_clks )
04182 {
04183     /*!
04184      * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
04185      */
04186 
04187     uint16_t encoded_timeout = 0;
04188     uint32_t ls_byte = 0;
04189     uint16_t ms_byte = 0;
04190 
04191     if ( timeout_macro_clks > 0 ) {
04192         ls_byte = timeout_macro_clks - 1;
04193 
04194         while ( ( ls_byte & 0xFFFFFF00 ) > 0 ) {
04195             ls_byte = ls_byte >> 1;
04196             ms_byte++;
04197         }
04198 
04199         encoded_timeout = ( ms_byte << 8 )
04200                           + ( uint16_t ) ( ls_byte & 0x000000FF );
04201     }
04202 
04203     return encoded_timeout;
04204 
04205 }
04206 
04207 VL53L0X_Error VL53L0X::set_sequence_step_timeout( VL53L0X_DEV dev,
04208         VL53L0X_SequenceStepId sequence_step_id,
04209         uint32_t timeout_micro_secs )
04210 {
04211     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04212     uint8_t current_vcsel_pulse_period_p_clk;
04213     uint8_t msrc_encoded_time_out;
04214     uint16_t pre_range_encoded_time_out;
04215     uint16_t pre_range_time_out_m_clks;
04216     uint16_t msrc_range_time_out_m_clks;
04217     uint32_t final_range_time_out_m_clks;
04218     uint16_t final_range_encoded_time_out;
04219     VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
04220 
04221     if ( ( sequence_step_id == VL53L0X_SEQUENCESTEP_TCC )    ||
04222             ( sequence_step_id == VL53L0X_SEQUENCESTEP_DSS )     ||
04223             ( sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC ) ) {
04224 
04225         status = vl53l0x_get_vcsel_pulse_period( dev,
04226                  VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04227                  &current_vcsel_pulse_period_p_clk );
04228 
04229         if ( status == VL53L0X_ERROR_NONE ) {
04230             msrc_range_time_out_m_clks = vl53l0x_calc_timeout_mclks( dev,
04231                                          timeout_micro_secs,
04232                                          ( uint8_t )current_vcsel_pulse_period_p_clk );
04233 
04234             if ( msrc_range_time_out_m_clks > 256 )
04235                 msrc_encoded_time_out = 255;
04236             else
04237                 msrc_encoded_time_out =
04238                     ( uint8_t )msrc_range_time_out_m_clks - 1;
04239 
04240             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
04241                                                 LastEncodedTimeout,
04242                                                 msrc_encoded_time_out );
04243         }
04244 
04245         if ( status == VL53L0X_ERROR_NONE ) {
04246             status = vl53l0x_write_byte( dev,
04247                                          VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
04248                                          msrc_encoded_time_out );
04249         }
04250     } else {
04251 
04252         if ( sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE ) {
04253 
04254             if ( status == VL53L0X_ERROR_NONE ) {
04255                 status = vl53l0x_get_vcsel_pulse_period( dev,
04256                          VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04257                          &current_vcsel_pulse_period_p_clk );
04258                 pre_range_time_out_m_clks =
04259                     vl53l0x_calc_timeout_mclks( dev,
04260                                                 timeout_micro_secs,
04261                                                 ( uint8_t )current_vcsel_pulse_period_p_clk );
04262                 pre_range_encoded_time_out = vl53l0x_encode_timeout(
04263                                                  pre_range_time_out_m_clks );
04264 
04265                 VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
04266                                                     LastEncodedTimeout,
04267                                                     pre_range_encoded_time_out );
04268             }
04269 
04270             if ( status == VL53L0X_ERROR_NONE ) {
04271                 status = vl53l0x_write_word( dev,
04272                                              VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
04273                                              pre_range_encoded_time_out );
04274             }
04275 
04276             if ( status == VL53L0X_ERROR_NONE ) {
04277                 VL53L0X_SETDEVICESPECIFICPARAMETER(
04278                     dev,
04279                     PreRangeTimeoutMicroSecs,
04280                     timeout_micro_secs );
04281             }
04282         } else if ( sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE ) {
04283 
04284             /* For the final range timeout, the pre-range timeout
04285              * must be added. To do this both final and pre-range
04286              * timeouts must be expressed in macro periods MClks
04287              * because they have different vcsel periods.
04288              */
04289 
04290             vl53l0x_get_sequence_step_enables( dev,
04291                                                &scheduler_sequence_steps );
04292             pre_range_time_out_m_clks = 0;
04293             if ( scheduler_sequence_steps.PreRangeOn ) {
04294 
04295                 /* Retrieve PRE-RANGE VCSEL Period */
04296                 status = vl53l0x_get_vcsel_pulse_period( dev,
04297                          VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04298                          &current_vcsel_pulse_period_p_clk );
04299 
04300                 /* Retrieve PRE-RANGE Timeout in Macro periods
04301                  * (MCLKS) */
04302                 if ( status == VL53L0X_ERROR_NONE ) {
04303                     status = vl53l0x_read_word( dev, 0x51,
04304                                                 &pre_range_encoded_time_out );
04305                     pre_range_time_out_m_clks =
04306                         vl53l0x_decode_timeout(
04307                             pre_range_encoded_time_out );
04308                 }
04309             }
04310 
04311             /* Calculate FINAL RANGE Timeout in Macro Periods
04312              * (MCLKS) and add PRE-RANGE value
04313              */
04314             if ( status == VL53L0X_ERROR_NONE ) {
04315 
04316                 status = vl53l0x_get_vcsel_pulse_period( dev,
04317                          VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
04318                          &current_vcsel_pulse_period_p_clk );
04319             }
04320             if ( status == VL53L0X_ERROR_NONE ) {
04321 
04322                 final_range_time_out_m_clks =
04323                     vl53l0x_calc_timeout_mclks( dev,
04324                                                 timeout_micro_secs,
04325                                                 ( uint8_t ) current_vcsel_pulse_period_p_clk );
04326 
04327                 final_range_time_out_m_clks += pre_range_time_out_m_clks;
04328 
04329                 final_range_encoded_time_out =
04330                     vl53l0x_encode_timeout( final_range_time_out_m_clks );
04331 
04332                 if ( status == VL53L0X_ERROR_NONE ) {
04333                     status = vl53l0x_write_word( dev, 0x71,
04334                                                  final_range_encoded_time_out );
04335                 }
04336 
04337                 if ( status == VL53L0X_ERROR_NONE ) {
04338                     VL53L0X_SETDEVICESPECIFICPARAMETER(
04339                         dev,
04340                         FinalRangeTimeoutMicroSecs,
04341                         timeout_micro_secs );
04342                 }
04343             }
04344         } else
04345             status = VL53L0X_ERROR_INVALID_PARAMS;
04346 
04347     }
04348     return status;
04349 }
04350 
04351 VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
04352         uint32_t measurement_timing_budget_micro_seconds )
04353 {
04354     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04355     uint32_t final_range_timing_budget_micro_seconds;
04356     VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
04357     uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
04358     uint32_t start_overhead_micro_seconds       = 1910;
04359     uint32_t end_overhead_micro_seconds     = 960;
04360     uint32_t msrc_overhead_micro_seconds        = 660;
04361     uint32_t tcc_overhead_micro_seconds     = 590;
04362     uint32_t dss_overhead_micro_seconds     = 690;
04363     uint32_t pre_range_overhead_micro_seconds   = 660;
04364     uint32_t final_range_overhead_micro_seconds = 550;
04365     uint32_t pre_range_timeout_micro_seconds    = 0;
04366     uint32_t c_min_timing_budget_micro_seconds  = 20000;
04367     uint32_t sub_timeout = 0;
04368 
04369     LOG_FUNCTION_START( "" );
04370 
04371     if ( measurement_timing_budget_micro_seconds
04372             < c_min_timing_budget_micro_seconds ) {
04373         status = VL53L0X_ERROR_INVALID_PARAMS;
04374         return status;
04375     }
04376 
04377     final_range_timing_budget_micro_seconds =
04378         measurement_timing_budget_micro_seconds -
04379         ( start_overhead_micro_seconds + end_overhead_micro_seconds );
04380 
04381     status = vl53l0x_get_sequence_step_enables( dev, &scheduler_sequence_steps );
04382 
04383     if ( status == VL53L0X_ERROR_NONE &&
04384             ( scheduler_sequence_steps.TccOn  ||
04385               scheduler_sequence_steps.MsrcOn ||
04386               scheduler_sequence_steps.DssOn ) ) {
04387 
04388         /* TCC, MSRC and DSS all share the same timeout */
04389         status = get_sequence_step_timeout( dev,
04390                                             VL53L0X_SEQUENCESTEP_MSRC,
04391                                             &msrc_dcc_tcc_timeout_micro_seconds );
04392 
04393         /* Subtract the TCC, MSRC and DSS timeouts if they are
04394          * enabled. */
04395 
04396         if ( status != VL53L0X_ERROR_NONE )
04397             return status;
04398 
04399         /* TCC */
04400         if ( scheduler_sequence_steps.TccOn ) {
04401 
04402             sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
04403                           + tcc_overhead_micro_seconds;
04404 
04405             if ( sub_timeout <
04406                     final_range_timing_budget_micro_seconds ) {
04407                 final_range_timing_budget_micro_seconds -=
04408                     sub_timeout;
04409             } else {
04410                 /* Requested timeout too big. */
04411                 status = VL53L0X_ERROR_INVALID_PARAMS;
04412             }
04413         }
04414 
04415         if ( status != VL53L0X_ERROR_NONE ) {
04416             LOG_FUNCTION_END( status );
04417             return status;
04418         }
04419 
04420         /* DSS */
04421         if ( scheduler_sequence_steps.DssOn ) {
04422 
04423             sub_timeout = 2 * ( msrc_dcc_tcc_timeout_micro_seconds +
04424                                 dss_overhead_micro_seconds );
04425 
04426             if ( sub_timeout < final_range_timing_budget_micro_seconds ) {
04427                 final_range_timing_budget_micro_seconds
04428                 -= sub_timeout;
04429             } else {
04430                 /* Requested timeout too big. */
04431                 status = VL53L0X_ERROR_INVALID_PARAMS;
04432             }
04433         } else if ( scheduler_sequence_steps.MsrcOn ) {
04434             /* MSRC */
04435             sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
04436                           msrc_overhead_micro_seconds;
04437 
04438             if ( sub_timeout < final_range_timing_budget_micro_seconds ) {
04439                 final_range_timing_budget_micro_seconds
04440                 -= sub_timeout;
04441             } else {
04442                 /* Requested timeout too big. */
04443                 status = VL53L0X_ERROR_INVALID_PARAMS;
04444             }
04445         }
04446 
04447     }
04448 
04449     if ( status != VL53L0X_ERROR_NONE ) {
04450         LOG_FUNCTION_END( status );
04451         return status;
04452     }
04453 
04454     if ( scheduler_sequence_steps.PreRangeOn ) {
04455 
04456         /* Subtract the Pre-range timeout if enabled. */
04457 
04458         status = get_sequence_step_timeout( dev,
04459                                             VL53L0X_SEQUENCESTEP_PRE_RANGE,
04460                                             &pre_range_timeout_micro_seconds );
04461 
04462         sub_timeout = pre_range_timeout_micro_seconds +
04463                       pre_range_overhead_micro_seconds;
04464 
04465         if ( sub_timeout < final_range_timing_budget_micro_seconds ) {
04466             final_range_timing_budget_micro_seconds -= sub_timeout;
04467         } else {
04468             /* Requested timeout too big. */
04469             status = VL53L0X_ERROR_INVALID_PARAMS;
04470         }
04471     }
04472 
04473 
04474     if ( status == VL53L0X_ERROR_NONE &&
04475             scheduler_sequence_steps.FinalRangeOn ) {
04476 
04477         final_range_timing_budget_micro_seconds -=
04478             final_range_overhead_micro_seconds;
04479 
04480         /* Final Range Timeout
04481          * Note that the final range timeout is determined by the timing
04482          * budget and the sum of all other timeouts within the sequence.
04483          * If there is no room for the final range timeout, then an error
04484          * will be set. Otherwise the remaining time will be applied to
04485          * the final range.
04486          */
04487         status = set_sequence_step_timeout( dev,
04488                                             VL53L0X_SEQUENCESTEP_FINAL_RANGE,
04489                                             final_range_timing_budget_micro_seconds );
04490 
04491         VL53L0X_SETPARAMETERFIELD( dev,
04492                                    MeasurementTimingBudgetMicroSeconds,
04493                                    measurement_timing_budget_micro_seconds );
04494     }
04495 
04496     LOG_FUNCTION_END( status );
04497 
04498     return status;
04499 }
04500 
04501 VL53L0X_Error VL53L0X::vl53l0x_set_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
04502         uint32_t measurement_timing_budget_micro_seconds )
04503 {
04504     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04505     LOG_FUNCTION_START( "" );
04506 
04507     status = wrapped_vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
04508              measurement_timing_budget_micro_seconds );
04509 
04510     LOG_FUNCTION_END( status );
04511 
04512     return status;
04513 }
04514 
04515 VL53L0X_Error VL53L0X::vl53l0x_set_sequence_step_enable( VL53L0X_DEV dev,
04516         VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled )
04517 {
04518     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04519     uint8_t sequence_config = 0;
04520     uint8_t sequence_config_new = 0;
04521     uint32_t measurement_timing_budget_micro_seconds;
04522     LOG_FUNCTION_START( "" );
04523 
04524     status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
04525                                 &sequence_config );
04526 
04527     sequence_config_new = sequence_config;
04528 
04529     if ( status == VL53L0X_ERROR_NONE ) {
04530         if ( sequence_step_enabled == 1 ) {
04531 
04532             /* Enable requested sequence step
04533              */
04534             switch ( sequence_step_id ) {
04535                 case VL53L0X_SEQUENCESTEP_TCC:
04536                     sequence_config_new |= 0x10;
04537                     break;
04538                 case VL53L0X_SEQUENCESTEP_DSS:
04539                     sequence_config_new |= 0x28;
04540                     break;
04541                 case VL53L0X_SEQUENCESTEP_MSRC:
04542                     sequence_config_new |= 0x04;
04543                     break;
04544                 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
04545                     sequence_config_new |= 0x40;
04546                     break;
04547                 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
04548                     sequence_config_new |= 0x80;
04549                     break;
04550                 default:
04551                     status = VL53L0X_ERROR_INVALID_PARAMS;
04552             }
04553         } else {
04554             /* Disable requested sequence step
04555              */
04556             switch ( sequence_step_id ) {
04557                 case VL53L0X_SEQUENCESTEP_TCC:
04558                     sequence_config_new &= 0xef;
04559                     break;
04560                 case VL53L0X_SEQUENCESTEP_DSS:
04561                     sequence_config_new &= 0xd7;
04562                     break;
04563                 case VL53L0X_SEQUENCESTEP_MSRC:
04564                     sequence_config_new &= 0xfb;
04565                     break;
04566                 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
04567                     sequence_config_new &= 0xbf;
04568                     break;
04569                 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
04570                     sequence_config_new &= 0x7f;
04571                     break;
04572                 default:
04573                     status = VL53L0X_ERROR_INVALID_PARAMS;
04574             }
04575         }
04576     }
04577 
04578     if ( sequence_config_new != sequence_config ) {
04579         /* Apply New Setting */
04580         if ( status == VL53L0X_ERROR_NONE ) {
04581             status = vl53l0x_write_byte( dev,
04582                                          VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new );
04583         }
04584         if ( status == VL53L0X_ERROR_NONE )
04585             PALDevDataSet( dev, SequenceConfig, sequence_config_new );
04586 
04587 
04588         /* Recalculate timing budget */
04589         if ( status == VL53L0X_ERROR_NONE ) {
04590             VL53L0X_GETPARAMETERFIELD( dev,
04591                                        MeasurementTimingBudgetMicroSeconds,
04592                                        measurement_timing_budget_micro_seconds );
04593 
04594             vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
04595                     measurement_timing_budget_micro_seconds );
04596         }
04597     }
04598 
04599     LOG_FUNCTION_END( status );
04600 
04601     return status;
04602 }
04603 
04604 VL53L0X_Error VL53L0X::vl53l0x_set_limit_check_enable( VL53L0X_DEV dev, uint16_t limit_check_id,
04605         uint8_t limit_check_enable )
04606 {
04607     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04608     FixPoint1616_t temp_fix1616 = 0;
04609     uint8_t limit_check_enable_int = 0;
04610     uint8_t limit_check_disable = 0;
04611     uint8_t temp8;
04612 
04613     LOG_FUNCTION_START( "" );
04614 
04615     if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
04616         status = VL53L0X_ERROR_INVALID_PARAMS;
04617     } else {
04618         if ( limit_check_enable == 0 ) {
04619             temp_fix1616 = 0;
04620             limit_check_enable_int = 0;
04621             limit_check_disable = 1;
04622 
04623         } else {
04624             VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
04625                                             limit_check_id, temp_fix1616 );
04626             limit_check_disable = 0;
04627             /* this to be sure to have either 0 or 1 */
04628             limit_check_enable_int = 1;
04629         }
04630 
04631         switch ( limit_check_id ) {
04632 
04633             case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
04634                 /* internal computation: */
04635                 VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
04636                                                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
04637                                                 limit_check_enable_int );
04638 
04639                 break;
04640 
04641             case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
04642 
04643                 status = vl53l0x_write_word( dev,
04644                                              VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
04645                                              VL53L0X_FIXPOINT1616TOFIXPOINT97( temp_fix1616 ) );
04646 
04647                 break;
04648 
04649             case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
04650 
04651                 /* internal computation: */
04652                 VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
04653                                                 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
04654                                                 limit_check_enable_int );
04655 
04656                 break;
04657 
04658             case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
04659 
04660                 /* internal computation: */
04661                 VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
04662                                                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
04663                                                 limit_check_enable_int );
04664 
04665                 break;
04666 
04667             case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
04668 
04669                 temp8 = ( uint8_t )( limit_check_disable << 1 );
04670                 status = vl53l0x_update_byte( dev,
04671                                               VL53L0X_REG_MSRC_CONFIG_CONTROL,
04672                                               0xFE, temp8 );
04673 
04674                 break;
04675 
04676             case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
04677 
04678                 temp8 = ( uint8_t )( limit_check_disable << 4 );
04679                 status = vl53l0x_update_byte( dev,
04680                                               VL53L0X_REG_MSRC_CONFIG_CONTROL,
04681                                               0xEF, temp8 );
04682 
04683                 break;
04684 
04685 
04686             default:
04687                 status = VL53L0X_ERROR_INVALID_PARAMS;
04688 
04689         }
04690 
04691     }
04692 
04693     if ( status == VL53L0X_ERROR_NONE ) {
04694         if ( limit_check_enable == 0 ) {
04695             VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
04696                                             limit_check_id, 0 );
04697         } else {
04698             VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
04699                                             limit_check_id, 1 );
04700         }
04701     }
04702 
04703     LOG_FUNCTION_END( status );
04704     return status;
04705 }
04706 
04707 VL53L0X_Error VL53L0X::vl53l0x_static_init( VL53L0X_DEV dev )
04708 {
04709     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04710     VL53L0X_DeviceParameters_t current_parameters = {0};
04711     uint8_t *p_tuning_setting_buffer;
04712     uint16_t tempword = 0;
04713     uint8_t tempbyte = 0;
04714     uint8_t use_internal_tuning_settings = 0;
04715     uint32_t count = 0;
04716     uint8_t is_aperture_spads = 0;
04717     uint32_t ref_spad_count = 0;
04718     uint8_t aperture_spads = 0;
04719     uint8_t vcsel_pulse_period_pclk;
04720     uint32_t seq_timeout_micro_secs;
04721 
04722     LOG_FUNCTION_START( "" );
04723 
04724     status = vl53l0x_get_info_from_device( dev, 1 );
04725 
04726     /* set the ref spad from NVM */
04727     count   = ( uint32_t )VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
04728               ReferenceSpadCount );
04729     aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
04730                      ReferenceSpadType );
04731 
04732     /* NVM value invalid */
04733     if ( ( aperture_spads > 1 ) ||
04734             ( ( aperture_spads == 1 ) && ( count > 32 ) ) ||
04735             ( ( aperture_spads == 0 ) && ( count > 12 ) ) )
04736         status = wrapped_vl53l0x_perform_ref_spad_management( dev, &ref_spad_count,
04737                  &is_aperture_spads );
04738     else
04739         status = wrapped_vl53l0x_set_reference_spads( dev, count, aperture_spads );
04740 
04741 
04742     /* Initialize tuning settings buffer to prevent compiler warning. */
04743     p_tuning_setting_buffer = DefaultTuningSettings;
04744 
04745     if ( status == VL53L0X_ERROR_NONE ) {
04746         use_internal_tuning_settings = PALDevDataGet( dev,
04747                                        UseInternalTuningSettings );
04748 
04749         if ( use_internal_tuning_settings == 0 )
04750             p_tuning_setting_buffer = PALDevDataGet( dev,
04751                                       pTuningSettingsPointer );
04752         else
04753             p_tuning_setting_buffer = DefaultTuningSettings;
04754 
04755     }
04756 
04757     if ( status == VL53L0X_ERROR_NONE )
04758         status = vl53l0x_load_tuning_settings( dev, p_tuning_setting_buffer );
04759 
04760 
04761     /* Set interrupt config to new sample ready */
04762     if ( status == VL53L0X_ERROR_NONE ) {
04763         status = vl53l0x_set_gpio_config( dev, 0, 0,
04764                                           VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
04765                                           VL53L0X_INTERRUPTPOLARITY_LOW );
04766     }
04767 
04768     if ( status == VL53L0X_ERROR_NONE ) {
04769         status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
04770         status |= vl53l0x_read_word( dev, 0x84, &tempword );
04771         status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
04772     }
04773 
04774     if ( status == VL53L0X_ERROR_NONE ) {
04775         VL53L0X_SETDEVICESPECIFICPARAMETER( dev, OscFrequencyMHz,
04776                                             VL53L0X_FIXPOINT412TOFIXPOINT1616( tempword ) );
04777     }
04778 
04779     /* After static init, some device parameters may be changed,
04780      * so update them */
04781     if ( status == VL53L0X_ERROR_NONE )
04782         status = vl53l0x_get_device_parameters( dev, &current_parameters );
04783 
04784 
04785     if ( status == VL53L0X_ERROR_NONE ) {
04786         status = vl53l0x_get_fraction_enable( dev, &tempbyte );
04787         if ( status == VL53L0X_ERROR_NONE )
04788             PALDevDataSet( dev, RangeFractionalEnable, tempbyte );
04789 
04790     }
04791 
04792     if ( status == VL53L0X_ERROR_NONE )
04793         PALDevDataSet( dev, CurrentParameters, current_parameters );
04794 
04795 
04796     /* read the sequence config and save it */
04797     if ( status == VL53L0X_ERROR_NONE ) {
04798         status = vl53l0x_read_byte( dev,
04799                                     VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte );
04800         if ( status == VL53L0X_ERROR_NONE )
04801             PALDevDataSet( dev, SequenceConfig, tempbyte );
04802 
04803     }
04804 
04805     /* Disable MSRC and TCC by default */
04806     if ( status == VL53L0X_ERROR_NONE )
04807         status = vl53l0x_set_sequence_step_enable( dev,
04808                  VL53L0X_SEQUENCESTEP_TCC, 0 );
04809 
04810 
04811     if ( status == VL53L0X_ERROR_NONE )
04812         status = vl53l0x_set_sequence_step_enable( dev,
04813                  VL53L0X_SEQUENCESTEP_MSRC, 0 );
04814 
04815 
04816     /* Set PAL State to standby */
04817     if ( status == VL53L0X_ERROR_NONE )
04818         PALDevDataSet( dev, PalState, VL53L0X_STATE_IDLE );
04819 
04820 
04821 
04822     /* Store pre-range vcsel period */
04823     if ( status == VL53L0X_ERROR_NONE ) {
04824         status = vl53l0x_get_vcsel_pulse_period(
04825                      dev,
04826                      VL53L0X_VCSEL_PERIOD_PRE_RANGE,
04827                      &vcsel_pulse_period_pclk );
04828     }
04829 
04830     if ( status == VL53L0X_ERROR_NONE ) {
04831         VL53L0X_SETDEVICESPECIFICPARAMETER(
04832             dev,
04833             PreRangeVcselPulsePeriod,
04834             vcsel_pulse_period_pclk );
04835     }
04836 
04837     /* Store final-range vcsel period */
04838     if ( status == VL53L0X_ERROR_NONE ) {
04839         status = vl53l0x_get_vcsel_pulse_period(
04840                      dev,
04841                      VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
04842                      &vcsel_pulse_period_pclk );
04843     }
04844 
04845     if ( status == VL53L0X_ERROR_NONE ) {
04846         VL53L0X_SETDEVICESPECIFICPARAMETER(
04847             dev,
04848             FinalRangeVcselPulsePeriod,
04849             vcsel_pulse_period_pclk );
04850     }
04851 
04852     /* Store pre-range timeout */
04853     if ( status == VL53L0X_ERROR_NONE ) {
04854         status = get_sequence_step_timeout(
04855                      dev,
04856                      VL53L0X_SEQUENCESTEP_PRE_RANGE,
04857                      &seq_timeout_micro_secs );
04858     }
04859 
04860     if ( status == VL53L0X_ERROR_NONE ) {
04861         VL53L0X_SETDEVICESPECIFICPARAMETER(
04862             dev,
04863             PreRangeTimeoutMicroSecs,
04864             seq_timeout_micro_secs );
04865     }
04866 
04867     /* Store final-range timeout */
04868     if ( status == VL53L0X_ERROR_NONE ) {
04869         status = get_sequence_step_timeout(
04870                      dev,
04871                      VL53L0X_SEQUENCESTEP_FINAL_RANGE,
04872                      &seq_timeout_micro_secs );
04873     }
04874 
04875     if ( status == VL53L0X_ERROR_NONE ) {
04876         VL53L0X_SETDEVICESPECIFICPARAMETER(
04877             dev,
04878             FinalRangeTimeoutMicroSecs,
04879             seq_timeout_micro_secs );
04880     }
04881 
04882     LOG_FUNCTION_END( status );
04883     return status;
04884 }
04885 
04886 
04887 VL53L0X_Error VL53L0X::vl53l0x_stop_measurement( VL53L0X_DEV dev )
04888 {
04889     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04890     LOG_FUNCTION_START( "" );
04891 
04892     status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START,
04893                                  VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT );
04894 
04895     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
04896     status = vl53l0x_write_byte( dev, 0x00, 0x00 );
04897     status = vl53l0x_write_byte( dev, 0x91, 0x00 );
04898     status = vl53l0x_write_byte( dev, 0x00, 0x01 );
04899     status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
04900 
04901     if ( status == VL53L0X_ERROR_NONE ) {
04902         /* Set PAL State to Idle */
04903         PALDevDataSet( dev, PalState, VL53L0X_STATE_IDLE );
04904     }
04905 
04906     /* Check if need to apply interrupt settings */
04907     if ( status == VL53L0X_ERROR_NONE )
04908         status = vl53l0x_check_and_load_interrupt_settings( dev, 0 );
04909 
04910     LOG_FUNCTION_END( status );
04911     return status;
04912 }
04913 
04914 VL53L0X_Error VL53L0X::vl53l0x_get_stop_completed_status( VL53L0X_DEV dev,
04915         uint32_t *p_stop_status )
04916 {
04917     VL53L0X_Error status = VL53L0X_ERROR_NONE;
04918     uint8_t byte = 0;
04919     LOG_FUNCTION_START( "" );
04920 
04921     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
04922 
04923     if ( status == VL53L0X_ERROR_NONE )
04924         status = vl53l0x_read_byte( dev, 0x04, &byte );
04925 
04926     if ( status == VL53L0X_ERROR_NONE )
04927         status = vl53l0x_write_byte( dev, 0xFF, 0x0 );
04928 
04929     *p_stop_status = byte;
04930 
04931     if ( byte == 0 ) {
04932         status = vl53l0x_write_byte( dev, 0x80, 0x01 );
04933         status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
04934         status = vl53l0x_write_byte( dev, 0x00, 0x00 );
04935         status = vl53l0x_write_byte( dev, 0x91,
04936                                      PALDevDataGet( dev, StopVariable ) );
04937         status = vl53l0x_write_byte( dev, 0x00, 0x01 );
04938         status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
04939         status = vl53l0x_write_byte( dev, 0x80, 0x00 );
04940     }
04941 
04942     LOG_FUNCTION_END( status );
04943     return status;
04944 }
04945 
04946 /****************** Write and read functions from I2C *************************/
04947 
04948 VL53L0X_Error VL53L0X::vl53l0x_write_multi( VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count )
04949 {
04950     int  status;
04951 
04952     status = vl53l0x_i2c_write( dev->I2cDevAddr , index, p_data, ( uint16_t )count );
04953     return status;
04954 }
04955 
04956 VL53L0X_Error VL53L0X::vl53l0x_read_multi( VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count )
04957 {
04958     int status;
04959 
04960     if ( count >= VL53L0X_MAX_I2C_XFER_SIZE ) {
04961         status = VL53L0X_ERROR_INVALID_PARAMS;
04962     }
04963 
04964     status = vl53l0x_i2c_read( dev->I2cDevAddr , index, p_data, ( uint16_t )count );
04965 
04966     return status;
04967 }
04968 
04969 
04970 VL53L0X_Error VL53L0X::vl53l0x_write_byte( VL53L0X_DEV Dev, uint8_t index, uint8_t data )
04971 {
04972     int  status;
04973 
04974     status = vl53l0x_i2c_write( Dev->I2cDevAddr , index, &data, 1 );
04975     return status;
04976 }
04977 
04978 VL53L0X_Error VL53L0X::vl53l0x_write_word( VL53L0X_DEV dev, uint8_t index, uint16_t data )
04979 {
04980     int  status;
04981     uint8_t buffer[2];
04982 
04983     buffer[0] = data >> 8;
04984     buffer[1] = data & 0x00FF;
04985     status = vl53l0x_i2c_write( dev->I2cDevAddr , index, ( uint8_t * )buffer, 2 );
04986     return status;
04987 }
04988 
04989 VL53L0X_Error VL53L0X::vl53l0x_write_dword( VL53L0X_DEV Dev, uint8_t index, uint32_t data )
04990 {
04991     int  status;
04992     uint8_t buffer[4];
04993 
04994     buffer[0] = ( data >> 24 ) & 0xFF;
04995     buffer[1] = ( data >> 16 ) & 0xFF;
04996     buffer[2] = ( data >>  8 ) & 0xFF;
04997     buffer[3] = ( data >>  0 ) & 0xFF;
04998     status = vl53l0x_i2c_write( Dev->I2cDevAddr , index, ( uint8_t * )buffer, 4 );
04999     return status;
05000 }
05001 
05002 
05003 VL53L0X_Error VL53L0X::vl53l0x_read_byte( VL53L0X_DEV Dev, uint8_t index, uint8_t *p_data )
05004 {
05005     int  status;
05006 
05007     status = vl53l0x_i2c_read( Dev->I2cDevAddr , index, p_data, 1 );
05008 
05009     if( status )
05010         return -1;
05011 
05012     return 0;
05013 }
05014 
05015 VL53L0X_Error VL53L0X::vl53l0x_read_word( VL53L0X_DEV Dev, uint8_t index, uint16_t *p_data )
05016 {
05017     int  status;
05018     uint8_t buffer[2] = {0, 0};
05019 
05020     status = vl53l0x_i2c_read( Dev->I2cDevAddr , index, buffer, 2 );
05021     if ( !status ) {
05022         *p_data = ( buffer[0] << 8 ) + buffer[1];
05023     }
05024     return status;
05025 
05026 }
05027 
05028 VL53L0X_Error VL53L0X::vl53l0x_read_dword( VL53L0X_DEV Dev, uint8_t index, uint32_t *p_data )
05029 {
05030     int status;
05031     uint8_t buffer[4] = {0, 0, 0, 0};
05032 
05033     status = vl53l0x_i2c_read( Dev->I2cDevAddr , index, buffer, 4 );
05034     if( !status ) {
05035         *p_data = ( buffer[0] << 24 ) + ( buffer[1] << 16 ) + ( buffer[2] << 8 ) + buffer[3];
05036     }
05037     return status;
05038 
05039 }
05040 
05041 VL53L0X_Error VL53L0X::vl53l0x_update_byte( VL53L0X_DEV Dev, uint8_t index, uint8_t and_data, uint8_t or_data )
05042 {
05043     int  status;
05044     uint8_t buffer = 0;
05045 
05046     /* read data direct onto buffer */
05047     status = vl53l0x_i2c_read( Dev->I2cDevAddr , index, &buffer, 1 );
05048     if ( !status ) {
05049         buffer = ( buffer & and_data ) | or_data;
05050         status = vl53l0x_i2c_write( Dev->I2cDevAddr , index, &buffer, ( uint8_t )1 );
05051     }
05052     return status;
05053 }
05054 
05055 VL53L0X_Error VL53L0X::vl53l0x_i2c_write( uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
05056         uint16_t NumByteToWrite )
05057 {
05058     int ret;
05059 
05060     ret = dev_i2c.i2c_write( p_data, DeviceAddr, RegisterAddr, NumByteToWrite );
05061 
05062     if( ret )
05063         return -1;
05064     return 0;
05065 }
05066 
05067 VL53L0X_Error VL53L0X::vl53l0x_i2c_read( uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
05068         uint16_t NumByteToRead )
05069 {
05070     int ret;
05071 
05072     ret = dev_i2c.i2c_read( p_data, DeviceAddr, RegisterAddr, NumByteToRead );
05073 
05074     if( ret )
05075         return -1;
05076     return 0;
05077 }
05078 
05079 int VL53L0X::read_id( uint8_t *id )
05080 {
05081     int status = 0;
05082     uint16_t rl_id = 0;
05083 
05084     status = vl53l0x_read_word( _device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id );
05085     if ( rl_id == 0xEEAA )
05086         return status;
05087 
05088     return -1;
05089 }
05090 
05091 
05092 VL53L0X_Error VL53L0X::wait_measurement_data_ready( VL53L0X_DEV dev )
05093 {
05094     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05095     uint8_t new_dat_ready = 0;
05096     uint32_t loop_nb;
05097 
05098     // Wait until it finished
05099     // use timeout to avoid deadlock
05100     if ( status == VL53L0X_ERROR_NONE ) {
05101         loop_nb = 0;
05102         do {
05103             status = vl53l0x_get_measurement_data_ready( dev, &new_dat_ready );
05104             if ( ( new_dat_ready == 0x01 ) || status != VL53L0X_ERROR_NONE ) {
05105                 break;
05106             }
05107             loop_nb = loop_nb + 1;
05108             vl53l0x_polling_delay( dev );
05109         } while ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP );
05110 
05111         if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP ) {
05112             status = VL53L0X_ERROR_TIME_OUT;
05113         }
05114     }
05115 
05116     return status;
05117 }
05118 
05119 VL53L0X_Error VL53L0X::wait_stop_completed( VL53L0X_DEV dev )
05120 {
05121     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05122     uint32_t stop_completed = 0;
05123     uint32_t loop_nb;
05124 
05125     // Wait until it finished
05126     // use timeout to avoid deadlock
05127     if ( status == VL53L0X_ERROR_NONE ) {
05128         loop_nb = 0;
05129         do {
05130             status = vl53l0x_get_stop_completed_status( dev, &stop_completed );
05131             if ( ( stop_completed == 0x00 ) || status != VL53L0X_ERROR_NONE ) {
05132                 break;
05133             }
05134             loop_nb = loop_nb + 1;
05135             vl53l0x_polling_delay( dev );
05136         } while ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP );
05137 
05138         if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP ) {
05139             status = VL53L0X_ERROR_TIME_OUT;
05140         }
05141 
05142     }
05143 
05144     return status;
05145 }
05146 
05147 
05148 int VL53L0X::init_sensor( uint8_t new_addr )
05149 {
05150     int status;
05151 
05152     vl53l0x_off();
05153     vl53l0x_on();
05154 
05155 //   status=VL53L0X_WaitDeviceBooted(Device);
05156 //   if(status)
05157 //      printf("WaitDeviceBooted fail\n\r");
05158     status = is_present();
05159     if( !status ) {
05160         status = init( &_my_device );
05161         if( status != VL53L0X_ERROR_NONE ) {
05162             printf( "Failed to init VL53L0X sensor!\n\r" );
05163             return status;
05164         }
05165 
05166         // deduce silicon version
05167         status = vl53l0x_get_device_info( &_my_device, &_device_info );
05168 
05169 
05170         status = prepare();
05171         if( status != VL53L0X_ERROR_NONE ) {
05172             printf( "Failed to prepare VL53L0X!\n\r" );
05173             return status;
05174         }
05175 
05176         if( new_addr != DEFAULT_DEVICE_ADDRESS ) {
05177             status = set_device_address( new_addr );
05178             if( status ) {
05179                 printf( "Failed to change I2C address!\n\r" );
05180                 return status;
05181             }
05182         } else {
05183             printf( "Invalid new address!\n\r" );
05184             return VL53L0X_ERROR_INVALID_PARAMS;
05185         }
05186     }
05187     return status;
05188 }
05189 
05190 int VL53L0X::range_meas_int_continuous_mode( void ( *fptr )( void ) )
05191 {
05192     int status, clr_status;
05193 
05194     status = vl53l0x_stop_measurement( _device ); // it is safer to do this while sensor is stopped
05195 
05196 //   status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
05197 
05198     status = vl53l0x_set_gpio_config( _device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
05199                                       VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
05200                                       VL53L0X_INTERRUPTPOLARITY_HIGH );
05201 
05202     if ( !status ) {
05203         attach_interrupt_measure_detection_irq( fptr );
05204         enable_interrupt_measure_detection_irq();
05205     }
05206 
05207     clr_status = clear_interrupt( VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS );
05208     if( clr_status )
05209         VL53L0X_ErrLog( "VL53L0X_ClearErrorInterrupt fail\r\n" );
05210 
05211     if( !status ) {
05212         status = range_start_continuous_mode();
05213     }
05214     return status;
05215 }
05216 
05217 
05218 int VL53L0X::start_measurement( OperatingMode operating_mode, void ( *fptr )( void ) )
05219 {
05220     int Status = VL53L0X_ERROR_NONE;
05221     int ClrStatus;
05222 
05223     uint8_t VhvSettings;
05224     uint8_t PhaseCal;
05225     // *** from mass market cube expansion v1.1, ranging with satellites.
05226     // default settings, for normal range.
05227     FixPoint1616_t signalLimit = ( FixPoint1616_t )( 0.25 * 65536 );
05228     FixPoint1616_t sigmaLimit = ( FixPoint1616_t )( 18 * 65536 );
05229     uint32_t timingBudget = 33000;
05230     uint8_t preRangeVcselPeriod = 14;
05231     uint8_t finalRangeVcselPeriod = 10;
05232 
05233     if ( operating_mode == range_continuous_interrupt ) {
05234         if ( gpio1Int == NULL ) {
05235             printf ( "GPIO1 Error\r\n" );
05236             return 1;
05237         }
05238 
05239         Status = vl53l0x_stop_measurement( _device ); // it is safer to do this while sensor is stopped
05240 
05241 //        Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
05242 
05243         Status = vl53l0x_set_gpio_config( _device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
05244                                           VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
05245                                           VL53L0X_INTERRUPTPOLARITY_HIGH );
05246 
05247         if ( Status == VL53L0X_ERROR_NONE ) {
05248             attach_interrupt_measure_detection_irq( fptr );
05249             enable_interrupt_measure_detection_irq();
05250         }
05251 
05252         ClrStatus = clear_interrupt( VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS );
05253         if( ClrStatus )
05254             VL53L0X_ErrLog( "VL53L0X_ClearErrorInterrupt fail\r\n" );
05255 
05256         if( Status == VL53L0X_ERROR_NONE ) {
05257             Status = vl53l0x_set_device_mode( _device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ); // Setup in continuous ranging mode
05258         }
05259 
05260         if( Status == VL53L0X_ERROR_NONE ) {
05261             Status = vl53l0x_start_measurement( _device );
05262         }
05263     }
05264 
05265     if ( operating_mode == range_single_shot_polling ) {
05266         // singelshot, polled ranging
05267         if( Status == VL53L0X_ERROR_NONE ) {
05268             // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
05269             Status = vl53l0x_set_device_mode( _device, VL53L0X_DEVICEMODE_SINGLE_RANGING ); // Setup in single ranging mode
05270         }
05271 
05272         // Enable/Disable Sigma and Signal check
05273         if ( Status == VL53L0X_ERROR_NONE ) {
05274             Status = vl53l0x_set_limit_check_enable( _device,
05275                      VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1 );
05276         }
05277         if ( Status == VL53L0X_ERROR_NONE ) {
05278             Status = vl53l0x_set_limit_check_enable( _device,
05279                      VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1 );
05280         }
05281 
05282 // *** from mass market cube expansion v1.1, ranging with satellites.
05283         /* Ranging configuration */
05284 //*
05285 //        switch(rangingConfig) {
05286 //        case LONG_RANGE:
05287         signalLimit = ( FixPoint1616_t )( 0.1 * 65536 );
05288         sigmaLimit = ( FixPoint1616_t )( 60 * 65536 );
05289         timingBudget = 33000;
05290         preRangeVcselPeriod = 18;
05291         finalRangeVcselPeriod = 14;
05292         /*          break;
05293                 case HIGH_ACCURACY:
05294                     signalLimit = (FixPoint1616_t)(0.25*65536);
05295                     sigmaLimit = (FixPoint1616_t)(18*65536);
05296                     timingBudget = 200000;
05297                     preRangeVcselPeriod = 14;
05298                     finalRangeVcselPeriod = 10;
05299                     break;
05300                 case HIGH_SPEED:
05301                     signalLimit = (FixPoint1616_t)(0.25*65536);
05302                     sigmaLimit = (FixPoint1616_t)(32*65536);
05303                     timingBudget = 20000;
05304                     preRangeVcselPeriod = 14;
05305                     finalRangeVcselPeriod = 10;
05306                     break;
05307                 default:
05308                     debug_printf("Not Supported");
05309                 }
05310         */
05311 
05312         if ( Status == VL53L0X_ERROR_NONE ) {
05313             Status = vl53l0x_set_limit_check_value( _device,
05314                                                     VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit );
05315         }
05316 
05317         if ( Status == VL53L0X_ERROR_NONE ) {
05318             Status = vl53l0x_set_limit_check_value( _device,
05319                                                     VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit );
05320         }
05321 
05322         if ( Status == VL53L0X_ERROR_NONE ) {
05323             Status = vl53l0x_set_measurement_timing_budget_micro_seconds( _device, timingBudget );
05324         }
05325 
05326         if ( Status == VL53L0X_ERROR_NONE ) {
05327             Status = vl53l0x_set_vcsel_pulse_period( _device,
05328                      VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod );
05329         }
05330 
05331         if ( Status == VL53L0X_ERROR_NONE ) {
05332             Status = vl53l0x_set_vcsel_pulse_period( _device,
05333                      VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod );
05334         }
05335 
05336         if ( Status == VL53L0X_ERROR_NONE ) {
05337             Status = vl53l0x_perform_ref_calibration( _device, &VhvSettings, &PhaseCal );
05338         }
05339 
05340     }
05341 
05342     if ( operating_mode == range_continuous_polling ) {
05343         if( Status == VL53L0X_ERROR_NONE ) {
05344             printf ( "Call of VL53L0X_SetDeviceMode\n" );
05345             Status = vl53l0x_set_device_mode( _device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ); // Setup in continuous ranging mode
05346         }
05347 
05348         if( Status == VL53L0X_ERROR_NONE ) {
05349             printf ( "Call of VL53L0X_StartMeasurement\n" );
05350             Status = vl53l0x_start_measurement( _device );
05351         }
05352     }
05353 
05354     return Status;
05355 }
05356 
05357 
05358 int VL53L0X::get_measurement( OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data )
05359 {
05360     int Status = VL53L0X_ERROR_NONE;
05361 
05362     if ( operating_mode == range_single_shot_polling ) {
05363         Status = vl53l0x_perform_single_ranging_measurement( _device, p_data );
05364     }
05365 
05366     if ( operating_mode == range_continuous_polling ) {
05367         if ( Status == VL53L0X_ERROR_NONE )
05368             Status = vl53l0x_measurement_poll_for_completion( _device );
05369 
05370         if( Status == VL53L0X_ERROR_NONE ) {
05371             Status = vl53l0x_get_ranging_measurement_data( _device, p_data );
05372 
05373             // Clear the interrupt
05374             vl53l0x_clear_interrupt_mask( _device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY );
05375             vl53l0x_polling_delay( _device );
05376         }
05377     }
05378 
05379     if ( operating_mode == range_continuous_interrupt ) {
05380         Status = vl53l0x_get_ranging_measurement_data( _device, p_data );
05381         vl53l0x_clear_interrupt_mask( _device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS );
05382     }
05383 
05384     return Status;
05385 }
05386 
05387 
05388 int VL53L0X::stop_measurement( OperatingMode operating_mode )
05389 {
05390     int status = VL53L0X_ERROR_NONE;
05391 
05392 
05393     // don't need to stop for a singleshot range!
05394     if ( operating_mode == range_single_shot_polling ) {
05395     }
05396 
05397     if ( operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling ) {
05398         // continuous mode
05399         if( status == VL53L0X_ERROR_NONE ) {
05400             printf ( "Call of VL53L0X_StopMeasurement\n" );
05401             status = vl53l0x_stop_measurement( _device );
05402         }
05403 
05404         if( status == VL53L0X_ERROR_NONE ) {
05405             printf ( "Wait Stop to be competed\n" );
05406             status = wait_stop_completed( _device );
05407         }
05408 
05409         if( status == VL53L0X_ERROR_NONE )
05410             status = vl53l0x_clear_interrupt_mask( _device,
05411                                                    VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY );
05412     }
05413 
05414     return status;
05415 }
05416 
05417 
05418 int VL53L0X::handle_irq( OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data )
05419 {
05420     int status;
05421     status = get_measurement( operating_mode, data );
05422     enable_interrupt_measure_detection_irq();
05423     return status;
05424 }
05425 
05426 int32_t VL53L0X::vl53l0x_cycle_power( void )
05427 {
05428     int32_t status = STATUS_OK;
05429 #ifdef VL53L0X_LOG_ENABLE
05430     trace_i2c( "// cycle sensor power\n" );
05431 #endif
05432     return status;
05433 }
05434 
05435 uint8_t VL53L0X::vl53l0x_encode_vcsel_period( uint8_t vcsel_period_pclks )
05436 {
05437     /*!
05438      * Converts the encoded VCSEL period register value into the real period
05439      * in PLL clocks
05440      */
05441 
05442     uint8_t vcsel_period_reg = 0;
05443 
05444     vcsel_period_reg = ( vcsel_period_pclks >> 1 ) - 1;
05445 
05446     return vcsel_period_reg;
05447 }
05448 
05449 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_device_error_string( VL53L0X_DeviceError error_code,
05450         char *p_device_error_string )
05451 {
05452     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05453 
05454     LOG_FUNCTION_START( "" );
05455 
05456     switch ( error_code ) {
05457         case VL53L0X_DEVICEERROR_NONE:
05458             VL53L0X_COPYSTRING( p_device_error_string,
05459                                 VL53L0X_STRING_DEVICEERROR_NONE );
05460             break;
05461         case VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
05462             VL53L0X_COPYSTRING( p_device_error_string,
05463                                 VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE );
05464             break;
05465         case VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
05466             VL53L0X_COPYSTRING( p_device_error_string,
05467                                 VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE );
05468             break;
05469         case VL53L0X_DEVICEERROR_NOVHVVALUEFOUND:
05470             VL53L0X_COPYSTRING( p_device_error_string,
05471                                 VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND );
05472             break;
05473         case VL53L0X_DEVICEERROR_MSRCNOTARGET:
05474             VL53L0X_COPYSTRING( p_device_error_string,
05475                                 VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET );
05476             break;
05477         case VL53L0X_DEVICEERROR_SNRCHECK:
05478             VL53L0X_COPYSTRING( p_device_error_string,
05479                                 VL53L0X_STRING_DEVICEERROR_SNRCHECK );
05480             break;
05481         case VL53L0X_DEVICEERROR_RANGEPHASECHECK:
05482             VL53L0X_COPYSTRING( p_device_error_string,
05483                                 VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK );
05484             break;
05485         case VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK:
05486             VL53L0X_COPYSTRING( p_device_error_string,
05487                                 VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK );
05488             break;
05489         case VL53L0X_DEVICEERROR_TCC:
05490             VL53L0X_COPYSTRING( p_device_error_string,
05491                                 VL53L0X_STRING_DEVICEERROR_TCC );
05492             break;
05493         case VL53L0X_DEVICEERROR_PHASECONSISTENCY:
05494             VL53L0X_COPYSTRING( p_device_error_string,
05495                                 VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY );
05496             break;
05497         case VL53L0X_DEVICEERROR_MINCLIP:
05498             VL53L0X_COPYSTRING( p_device_error_string,
05499                                 VL53L0X_STRING_DEVICEERROR_MINCLIP );
05500             break;
05501         case VL53L0X_DEVICEERROR_RANGECOMPLETE:
05502             VL53L0X_COPYSTRING( p_device_error_string,
05503                                 VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE );
05504             break;
05505         case VL53L0X_DEVICEERROR_ALGOUNDERFLOW:
05506             VL53L0X_COPYSTRING( p_device_error_string,
05507                                 VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW );
05508             break;
05509         case VL53L0X_DEVICEERROR_ALGOOVERFLOW:
05510             VL53L0X_COPYSTRING( p_device_error_string,
05511                                 VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW );
05512             break;
05513         case VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD:
05514             VL53L0X_COPYSTRING( p_device_error_string,
05515                                 VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD );
05516             break;
05517 
05518         default:
05519             VL53L0X_COPYSTRING( p_device_error_string,
05520                                 VL53L0X_STRING_UNKNOW_ERROR_CODE );
05521 
05522     }
05523 
05524     LOG_FUNCTION_END( status );
05525     return status;
05526 }
05527 
05528 
05529 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_limit_check_info( VL53L0X_DEV dev, uint16_t limit_check_id,
05530         char *p_limit_check_string )
05531 {
05532 
05533     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05534 
05535     LOG_FUNCTION_START( "" );
05536 
05537     switch ( limit_check_id ) {
05538         case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
05539             VL53L0X_COPYSTRING( p_limit_check_string,
05540                                 VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE );
05541             break;
05542         case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
05543             VL53L0X_COPYSTRING( p_limit_check_string,
05544                                 VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE );
05545             break;
05546         case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
05547             VL53L0X_COPYSTRING( p_limit_check_string,
05548                                 VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP );
05549             break;
05550         case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
05551             VL53L0X_COPYSTRING( p_limit_check_string,
05552                                 VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD );
05553             break;
05554 
05555         case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
05556             VL53L0X_COPYSTRING( p_limit_check_string,
05557                                 VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_MSRC );
05558             break;
05559 
05560         case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
05561             VL53L0X_COPYSTRING( p_limit_check_string,
05562                                 VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_PRE_RANGE );
05563             break;
05564 
05565         default:
05566             VL53L0X_COPYSTRING( p_limit_check_string,
05567                                 VL53L0X_STRING_UNKNOW_ERROR_CODE );
05568 
05569     }
05570 
05571     LOG_FUNCTION_END( status );
05572     return status;
05573 }
05574 
05575 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_pal_error_string( VL53L0X_Error pal_error_code,
05576         char *p_pal_error_string )
05577 {
05578     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05579 
05580     LOG_FUNCTION_START( "" );
05581 
05582     switch ( pal_error_code ) {
05583         case VL53L0X_ERROR_NONE:
05584             VL53L0X_COPYSTRING( p_pal_error_string,
05585                                 VL53L0X_STRING_ERROR_NONE );
05586             break;
05587         case VL53L0X_ERROR_CALIBRATION_WARNING:
05588             VL53L0X_COPYSTRING( p_pal_error_string,
05589                                 VL53L0X_STRING_ERROR_CALIBRATION_WARNING );
05590             break;
05591         case VL53L0X_ERROR_MIN_CLIPPED:
05592             VL53L0X_COPYSTRING( p_pal_error_string,
05593                                 VL53L0X_STRING_ERROR_MIN_CLIPPED );
05594             break;
05595         case VL53L0X_ERROR_UNDEFINED:
05596             VL53L0X_COPYSTRING( p_pal_error_string,
05597                                 VL53L0X_STRING_ERROR_UNDEFINED );
05598             break;
05599         case VL53L0X_ERROR_INVALID_PARAMS:
05600             VL53L0X_COPYSTRING( p_pal_error_string,
05601                                 VL53L0X_STRING_ERROR_INVALID_PARAMS );
05602             break;
05603         case VL53L0X_ERROR_NOT_SUPPORTED:
05604             VL53L0X_COPYSTRING( p_pal_error_string,
05605                                 VL53L0X_STRING_ERROR_NOT_SUPPORTED );
05606             break;
05607         case VL53L0X_ERROR_INTERRUPT_NOT_CLEARED:
05608             VL53L0X_COPYSTRING( p_pal_error_string,
05609                                 VL53L0X_STRING_ERROR_INTERRUPT_NOT_CLEARED );
05610             break;
05611         case VL53L0X_ERROR_RANGE_ERROR:
05612             VL53L0X_COPYSTRING( p_pal_error_string,
05613                                 VL53L0X_STRING_ERROR_RANGE_ERROR );
05614             break;
05615         case VL53L0X_ERROR_TIME_OUT:
05616             VL53L0X_COPYSTRING( p_pal_error_string,
05617                                 VL53L0X_STRING_ERROR_TIME_OUT );
05618             break;
05619         case VL53L0X_ERROR_MODE_NOT_SUPPORTED:
05620             VL53L0X_COPYSTRING( p_pal_error_string,
05621                                 VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED );
05622             break;
05623         case VL53L0X_ERROR_BUFFER_TOO_SMALL:
05624             VL53L0X_COPYSTRING( p_pal_error_string,
05625                                 VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL );
05626             break;
05627         case VL53L0X_ERROR_GPIO_NOT_EXISTING:
05628             VL53L0X_COPYSTRING( p_pal_error_string,
05629                                 VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING );
05630             break;
05631         case VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED:
05632             VL53L0X_COPYSTRING( p_pal_error_string,
05633                                 VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED );
05634             break;
05635         case VL53L0X_ERROR_CONTROL_INTERFACE:
05636             VL53L0X_COPYSTRING( p_pal_error_string,
05637                                 VL53L0X_STRING_ERROR_CONTROL_INTERFACE );
05638             break;
05639         case VL53L0X_ERROR_INVALID_COMMAND:
05640             VL53L0X_COPYSTRING( p_pal_error_string,
05641                                 VL53L0X_STRING_ERROR_INVALID_COMMAND );
05642             break;
05643         case VL53L0X_ERROR_DIVISION_BY_ZERO:
05644             VL53L0X_COPYSTRING( p_pal_error_string,
05645                                 VL53L0X_STRING_ERROR_DIVISION_BY_ZERO );
05646             break;
05647         case VL53L0X_ERROR_REF_SPAD_INIT:
05648             VL53L0X_COPYSTRING( p_pal_error_string,
05649                                 VL53L0X_STRING_ERROR_REF_SPAD_INIT );
05650             break;
05651         case VL53L0X_ERROR_NOT_IMPLEMENTED:
05652             VL53L0X_COPYSTRING( p_pal_error_string,
05653                                 VL53L0X_STRING_ERROR_NOT_IMPLEMENTED );
05654             break;
05655 
05656         default:
05657             VL53L0X_COPYSTRING( p_pal_error_string,
05658                                 VL53L0X_STRING_UNKNOW_ERROR_CODE );
05659     }
05660 
05661     LOG_FUNCTION_END( status );
05662     return status;
05663 }
05664 
05665 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_pal_state_string( VL53L0X_State pal_state_code,
05666         char *p_pal_state_string )
05667 {
05668     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05669 
05670     LOG_FUNCTION_START( "" );
05671 
05672     switch ( pal_state_code ) {
05673         case VL53L0X_STATE_POWERDOWN:
05674             VL53L0X_COPYSTRING( p_pal_state_string,
05675                                 VL53L0X_STRING_STATE_POWERDOWN );
05676             break;
05677         case VL53L0X_STATE_WAIT_STATICINIT:
05678             VL53L0X_COPYSTRING( p_pal_state_string,
05679                                 VL53L0X_STRING_STATE_WAIT_STATICINIT );
05680             break;
05681         case VL53L0X_STATE_STANDBY:
05682             VL53L0X_COPYSTRING( p_pal_state_string,
05683                                 VL53L0X_STRING_STATE_STANDBY );
05684             break;
05685         case VL53L0X_STATE_IDLE:
05686             VL53L0X_COPYSTRING( p_pal_state_string,
05687                                 VL53L0X_STRING_STATE_IDLE );
05688             break;
05689         case VL53L0X_STATE_RUNNING:
05690             VL53L0X_COPYSTRING( p_pal_state_string,
05691                                 VL53L0X_STRING_STATE_RUNNING );
05692             break;
05693         case VL53L0X_STATE_UNKNOWN:
05694             VL53L0X_COPYSTRING( p_pal_state_string,
05695                                 VL53L0X_STRING_STATE_UNKNOWN );
05696             break;
05697         case VL53L0X_STATE_ERROR:
05698             VL53L0X_COPYSTRING( p_pal_state_string,
05699                                 VL53L0X_STRING_STATE_ERROR );
05700             break;
05701 
05702         default:
05703             VL53L0X_COPYSTRING( p_pal_state_string,
05704                                 VL53L0X_STRING_STATE_UNKNOWN );
05705     }
05706 
05707     LOG_FUNCTION_END( status );
05708     return status;
05709 }
05710 
05711 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_range_status_string( uint8_t range_status,
05712         char *p_range_status_string )
05713 {
05714     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05715 
05716     LOG_FUNCTION_START( "" );
05717 
05718     switch ( range_status ) {
05719         case 0:
05720             VL53L0X_COPYSTRING( p_range_status_string,
05721                                 VL53L0X_STRING_RANGESTATUS_RANGEVALID );
05722             break;
05723         case 1:
05724             VL53L0X_COPYSTRING( p_range_status_string,
05725                                 VL53L0X_STRING_RANGESTATUS_SIGMA );
05726             break;
05727         case 2:
05728             VL53L0X_COPYSTRING( p_range_status_string,
05729                                 VL53L0X_STRING_RANGESTATUS_SIGNAL );
05730             break;
05731         case 3:
05732             VL53L0X_COPYSTRING( p_range_status_string,
05733                                 VL53L0X_STRING_RANGESTATUS_MINRANGE );
05734             break;
05735         case 4:
05736             VL53L0X_COPYSTRING( p_range_status_string,
05737                                 VL53L0X_STRING_RANGESTATUS_PHASE );
05738             break;
05739         case 5:
05740             VL53L0X_COPYSTRING( p_range_status_string,
05741                                 VL53L0X_STRING_RANGESTATUS_HW );
05742             break;
05743 
05744         default: /**/
05745             VL53L0X_COPYSTRING( p_range_status_string,
05746                                 VL53L0X_STRING_RANGESTATUS_NONE );
05747     }
05748 
05749     LOG_FUNCTION_END( status );
05750     return status;
05751 }
05752 
05753 
05754 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_ref_calibration( VL53L0X_DEV dev,
05755         uint8_t *p_vhv_settings, uint8_t *p_phase_cal )
05756 {
05757     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05758     uint8_t vhv_settings = 0;
05759     uint8_t phase_cal = 0;
05760 
05761     status = vl53l0x_ref_calibration_io( dev, 1,
05762                                          vhv_settings, phase_cal,
05763                                          p_vhv_settings, p_phase_cal,
05764                                          1, 1 );
05765 
05766     return status;
05767 }
05768 
05769 VL53L0X_Error VL53L0X::count_enabled_spads( uint8_t spad_array[],
05770         uint32_t byte_count, uint32_t max_spads,
05771         uint32_t *p_total_spads_enabled, uint8_t *p_is_aperture )
05772 {
05773     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05774     uint32_t c_spads_per_byte = 8;
05775     uint32_t last_byte;
05776     uint32_t last_bit;
05777     uint32_t byte_index = 0;
05778     uint32_t bit_index = 0;
05779     uint8_t temp_byte;
05780     uint8_t spad_type_identified = 0;
05781 
05782     /* The entire array will not be used for spads, therefore the last
05783      * byte and last bit is determined from the max spads value.
05784      */
05785 
05786     last_byte = max_spads / c_spads_per_byte;
05787     last_bit = max_spads % c_spads_per_byte;
05788 
05789     /* Check that the max spads value does not exceed the array bounds. */
05790     if ( last_byte >= byte_count )
05791         status = VL53L0X_ERROR_REF_SPAD_INIT;
05792 
05793     *p_total_spads_enabled = 0;
05794 
05795     /* Count the bits enabled in the whole bytes */
05796     for ( byte_index = 0; byte_index <= ( last_byte - 1 ); byte_index++ ) {
05797         temp_byte = spad_array[byte_index];
05798 
05799         for ( bit_index = 0; bit_index <= c_spads_per_byte; bit_index++ ) {
05800             if ( ( temp_byte & 0x01 ) == 1 ) {
05801                 ( *p_total_spads_enabled )++;
05802 
05803                 if ( !spad_type_identified ) {
05804                     *p_is_aperture = 1;
05805                     if ( ( byte_index < 2 ) && ( bit_index < 4 ) )
05806                         *p_is_aperture = 0;
05807                     spad_type_identified = 1;
05808                 }
05809             }
05810             temp_byte >>= 1;
05811         }
05812     }
05813 
05814     /* Count the number of bits enabled in the last byte accounting
05815      * for the fact that not all bits in the byte may be used.
05816      */
05817     temp_byte = spad_array[last_byte];
05818 
05819     for ( bit_index = 0; bit_index <= last_bit; bit_index++ ) {
05820         if ( ( temp_byte & 0x01 ) == 1 )
05821             ( *p_total_spads_enabled )++;
05822     }
05823 
05824     return status;
05825 }
05826 
05827 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_reference_spads( VL53L0X_DEV dev,
05828         uint32_t *p_spad_count, uint8_t *p_is_aperture_spads )
05829 {
05830     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05831     uint8_t ref_spads_initialised;
05832     uint8_t ref_spad_array[6];
05833     uint32_t c_max_spad_count = 44;
05834     uint32_t c_spad_array_size = 6;
05835     uint32_t spads_enabled;
05836     uint8_t is_aperture_spads = 0;
05837 
05838     ref_spads_initialised = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
05839                             RefSpadsInitialised );
05840 
05841     if ( ref_spads_initialised == 1 ) {
05842 
05843         *p_spad_count = ( uint32_t )VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
05844                         ReferenceSpadCount );
05845         *p_is_aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
05846                                ReferenceSpadType );
05847     } else {
05848 
05849         /* obtain spad info from device.*/
05850         status = get_ref_spad_map( dev, ref_spad_array );
05851 
05852         if ( status == VL53L0X_ERROR_NONE ) {
05853             /* count enabled spads within spad map array and
05854              * determine if Aperture or Non-Aperture.
05855              */
05856             status = count_enabled_spads( ref_spad_array,
05857                                           c_spad_array_size,
05858                                           c_max_spad_count,
05859                                           &spads_enabled,
05860                                           &is_aperture_spads );
05861 
05862             if ( status == VL53L0X_ERROR_NONE ) {
05863 
05864                 *p_spad_count = spads_enabled;
05865                 *p_is_aperture_spads = is_aperture_spads;
05866 
05867                 VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
05868                                                     RefSpadsInitialised, 1 );
05869                 VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
05870                                                     ReferenceSpadCount,
05871                                                     ( uint8_t )spads_enabled );
05872                 VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
05873                                                     ReferenceSpadType, is_aperture_spads );
05874             }
05875         }
05876     }
05877 
05878     return status;
05879 }
05880 
05881 VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_sequence_steps_info(
05882     VL53L0X_SequenceStepId sequence_step_id,
05883     char *p_sequence_steps_string )
05884 {
05885     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05886     LOG_FUNCTION_START( "" );
05887 
05888     switch ( sequence_step_id ) {
05889         case VL53L0X_SEQUENCESTEP_TCC:
05890             VL53L0X_COPYSTRING( p_sequence_steps_string,
05891                                 VL53L0X_STRING_SEQUENCESTEP_TCC );
05892             break;
05893         case VL53L0X_SEQUENCESTEP_DSS:
05894             VL53L0X_COPYSTRING( p_sequence_steps_string,
05895                                 VL53L0X_STRING_SEQUENCESTEP_DSS );
05896             break;
05897         case VL53L0X_SEQUENCESTEP_MSRC:
05898             VL53L0X_COPYSTRING( p_sequence_steps_string,
05899                                 VL53L0X_STRING_SEQUENCESTEP_MSRC );
05900             break;
05901         case VL53L0X_SEQUENCESTEP_PRE_RANGE:
05902             VL53L0X_COPYSTRING( p_sequence_steps_string,
05903                                 VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE );
05904             break;
05905         case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
05906             VL53L0X_COPYSTRING( p_sequence_steps_string,
05907                                 VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE );
05908             break;
05909 
05910         default:
05911             status = VL53L0X_ERROR_INVALID_PARAMS;
05912     }
05913 
05914     LOG_FUNCTION_END( status );
05915 
05916     return status;
05917 }
05918 
05919 VL53L0X_Error VL53L0X::vl53l0x_get_sequence_steps_info( VL53L0X_SequenceStepId sequence_step_id,
05920         char *p_sequence_steps_string )
05921 {
05922     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05923     LOG_FUNCTION_START( "" );
05924 
05925     status = wrapped_vl53l0x_get_sequence_steps_info(
05926                  sequence_step_id,
05927                  p_sequence_steps_string );
05928 
05929     LOG_FUNCTION_END( status );
05930 
05931     return status;
05932 }
05933 
05934 int32_t VL53L0X::vl53l0x_get_timer_frequency( int32_t *p_timer_freq_hz )
05935 {
05936     *p_timer_freq_hz = 0;
05937     return STATUS_FAIL;
05938 }
05939 
05940 int32_t VL53L0X::vl53l0x_get_timer_value( int32_t *p_timer_count )
05941 {
05942     *p_timer_count = 0;
05943     return STATUS_FAIL;
05944 }
05945 
05946 VL53L0X_Error VL53L0X::vl53l0x_enable_interrupt_mask( VL53L0X_DEV dev, uint32_t interrupt_mask )
05947 {
05948     VL53L0X_Error Status = VL53L0X_ERROR_NOT_IMPLEMENTED;
05949     LOG_FUNCTION_START( "" );
05950 
05951     /* not implemented for VL53L0X */
05952 
05953     LOG_FUNCTION_END( Status );
05954     return Status;
05955 }
05956 
05957 VL53L0X_Error VL53L0X::vl53l0x_get_device_error_status( VL53L0X_DEV dev,
05958         VL53L0X_DeviceError *p_device_error_status )
05959 {
05960     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05961     uint8_t range_status;
05962 
05963     LOG_FUNCTION_START( "" );
05964 
05965     status = vl53l0x_read_byte( dev, VL53L0X_REG_RESULT_RANGE_STATUS,
05966                              &range_status );
05967 
05968     *p_device_error_status = ( VL53L0X_DeviceError )( ( range_status & 0x78 ) >> 3 );
05969 
05970     LOG_FUNCTION_END( status );
05971     return status;
05972 }
05973 
05974 
05975 VL53L0X_Error VL53L0X::vl53l0x_get_device_error_string( VL53L0X_DeviceError error_code,
05976         char *p_device_error_string )
05977 {
05978     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05979 
05980     LOG_FUNCTION_START( "" );
05981 
05982     status = wrapped_vl53l0x_get_device_error_string( error_code, p_device_error_string );
05983 
05984     LOG_FUNCTION_END( status );
05985     return status;
05986 }
05987 
05988 VL53L0X_Error VL53L0X::vl53l0x_get_dmax_cal_parameters( VL53L0X_DEV dev,
05989         uint16_t *p_range_milli_meter, FixPoint1616_t *p_signal_rate_rtn_mega_cps )
05990 {
05991     VL53L0X_Error status = VL53L0X_ERROR_NONE;
05992 
05993     LOG_FUNCTION_START( "" );
05994 
05995     *p_range_milli_meter = PALDevDataGet( dev, DmaxCalRangeMilliMeter );
05996     *p_signal_rate_rtn_mega_cps = PALDevDataGet( dev,
05997                                   DmaxCalSignalRateRtnMegaCps );
05998 
05999     LOG_FUNCTION_END( status );
06000     return status;
06001 }
06002 
06003 VL53L0X_Error VL53L0X::vl53l0x_get_dmax_cal_parameters( VL53L0X_DEV dev,
06004         uint16_t range_milli_meter, FixPoint1616_t signal_rate_rtn_mega_cps )
06005 {
06006     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06007     FixPoint1616_t signal_rate_rtn_mega_cps_temp = 0;
06008 
06009     LOG_FUNCTION_START( "" );
06010 
06011     /* Check if one of input parameter is zero, in that case the
06012      * value are get from NVM */
06013     if ( ( range_milli_meter == 0 ) || ( signal_rate_rtn_mega_cps == 0 ) ) {
06014         /* NVM parameters */
06015         /* Run VL53L0X_get_info_from_device wit option 4 to get
06016          * signal rate at 400 mm if the value have been already
06017          * get this function will return with no access to device */
06018         vl53l0x_get_info_from_device( dev, 4 );
06019 
06020         signal_rate_rtn_mega_cps_temp = VL53L0X_GETDEVICESPECIFICPARAMETER(
06021                                             dev, SignalRateMeasFixed400mm );
06022 
06023         PALDevDataSet( dev, DmaxCalRangeMilliMeter, 400 );
06024         PALDevDataSet( dev, DmaxCalSignalRateRtnMegaCps,
06025                        signal_rate_rtn_mega_cps_temp );
06026     } else {
06027         /* User parameters */
06028         PALDevDataSet( dev, DmaxCalRangeMilliMeter, range_milli_meter );
06029         PALDevDataSet( dev, DmaxCalSignalRateRtnMegaCps,
06030                        signal_rate_rtn_mega_cps );
06031     }
06032 
06033     LOG_FUNCTION_END( status );
06034     return status;
06035 }
06036 
06037 VL53L0X_Error VL53L0X::vl53L0x_set_gpio_config( VL53L0X_DEV dev, uint8_t pin,
06038         VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
06039         VL53L0X_InterruptPolarity polarity )
06040 {
06041     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06042     uint8_t data;
06043 
06044     LOG_FUNCTION_START( "" );
06045 
06046     if ( pin != 0 ) {
06047         status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
06048     } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE ) {
06049         if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
06050             data = 0x10;
06051         else
06052             data = 1;
06053 
06054         status = vl53l0x_write_byte( dev,
06055                                  VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data );
06056 
06057     } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_OSC ) {
06058 
06059         status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
06060         status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
06061 
06062         status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
06063         status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
06064         status |= vl53l0x_write_byte( dev, 0x85, 0x02 );
06065 
06066         status |= vl53l0x_write_byte( dev, 0xff, 0x04 );
06067         status |= vl53l0x_write_byte( dev, 0xcd, 0x00 );
06068         status |= vl53l0x_write_byte( dev, 0xcc, 0x11 );
06069 
06070         status |= vl53l0x_write_byte( dev, 0xff, 0x07 );
06071         status |= vl53l0x_write_byte( dev, 0xbe, 0x00 );
06072 
06073         status |= vl53l0x_write_byte( dev, 0xff, 0x06 );
06074         status |= vl53l0x_write_byte( dev, 0xcc, 0x09 );
06075 
06076         status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
06077         status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
06078         status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
06079 
06080     } else {
06081 
06082         if ( status == VL53L0X_ERROR_NONE ) {
06083             switch ( functionality ) {
06084                 case VL53L0X_GPIOFUNCTIONALITY_OFF:
06085                     data = 0x00;
06086                     break;
06087                 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
06088                     data = 0x01;
06089                     break;
06090                 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
06091                     data = 0x02;
06092                     break;
06093                 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
06094                     data = 0x03;
06095                     break;
06096                 case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
06097                     data = 0x04;
06098                     break;
06099                 default:
06100                     status =
06101                         VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
06102             }
06103         }
06104 
06105         if ( status == VL53L0X_ERROR_NONE )
06106             status = vl53l0x_write_byte( dev,
06107                                      VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data );
06108 
06109         if ( status == VL53L0X_ERROR_NONE ) {
06110             if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
06111                 data = 0;
06112             else
06113                 data = ( uint8_t )( 1 << 4 );
06114 
06115             status = vl53l0x_update_byte( dev,
06116                                          VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data );
06117         }
06118 
06119         if ( status == VL53L0X_ERROR_NONE )
06120             VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
06121                                                 Pin0GpioFunctionality, functionality );
06122 
06123         if ( status == VL53L0X_ERROR_NONE )
06124             status = vl53l0x_clear_interrupt_mask( dev, 0 );
06125 
06126     }
06127 
06128     LOG_FUNCTION_END( status );
06129     return status;
06130 }
06131 
06132 
06133 VL53L0X_Error VL53L0X::vl53l0x_get_gpio_config( VL53L0X_DEV dev, uint8_t pin,
06134         VL53L0X_DeviceModes *p_device_mode,
06135         VL53L0X_GpioFunctionality *p_functionality,
06136         VL53L0X_InterruptPolarity *p_polarity )
06137 {
06138     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06139     VL53L0X_GpioFunctionality gpio_functionality;
06140     uint8_t data;
06141 
06142     LOG_FUNCTION_START( "" );
06143 
06144     /* pDeviceMode not managed by Ewok it return the current mode */
06145 
06146     status = vl53l0x_get_device_mode( dev, p_device_mode );
06147 
06148     if ( status == VL53L0X_ERROR_NONE ) {
06149         if ( pin != 0 ) {
06150             status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
06151         } else {
06152             status = vl53l0x_read_byte( dev,
06153                                      VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, &data );
06154         }
06155     }
06156 
06157     if ( status == VL53L0X_ERROR_NONE ) {
06158         switch ( data & 0x07 ) {
06159             case 0x00:
06160                 gpio_functionality = VL53L0X_GPIOFUNCTIONALITY_OFF;
06161                 break;
06162             case 0x01:
06163                 gpio_functionality =
06164                     VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW;
06165                 break;
06166             case 0x02:
06167                 gpio_functionality =
06168                     VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH;
06169                 break;
06170             case 0x03:
06171                 gpio_functionality =
06172                     VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT;
06173                 break;
06174             case 0x04:
06175                 gpio_functionality =
06176                     VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY;
06177                 break;
06178             default:
06179                 status = VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
06180         }
06181     }
06182 
06183     if ( status == VL53L0X_ERROR_NONE )
06184         status = vl53l0x_read_byte( dev, VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,
06185                                  &data );
06186 
06187     if ( status == VL53L0X_ERROR_NONE ) {
06188         if ( ( data & ( uint8_t )( 1 << 4 ) ) == 0 )
06189             *p_polarity = VL53L0X_INTERRUPTPOLARITY_LOW;
06190         else
06191             *p_polarity = VL53L0X_INTERRUPTPOLARITY_HIGH;
06192     }
06193 
06194     if ( status == VL53L0X_ERROR_NONE ) {
06195         *p_functionality = gpio_functionality;
06196         VL53L0X_SETDEVICESPECIFICPARAMETER( dev, Pin0GpioFunctionality,
06197                                             gpio_functionality );
06198     }
06199 
06200     LOG_FUNCTION_END( status );
06201     return status;
06202 }
06203 
06204 VL53L0X_Error VL53L0X::vl53l0x_get_histogram_measurement_data( VL53L0X_DEV dev,
06205         VL53L0X_HistogramMeasurementData_t *p_histogram_measurement_data )
06206 {
06207     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
06208     LOG_FUNCTION_START( "" );
06209 
06210     LOG_FUNCTION_END( status );
06211     return status;
06212 }
06213 
06214 VL53L0X_Error VL53L0X::vl53l0x_get_histogram_mode( VL53L0X_DEV dev,
06215         VL53L0X_HistogramModes *p_histogram_mode )
06216 {
06217     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
06218     LOG_FUNCTION_START( "" );
06219 
06220     /* not implemented on VL53L0X */
06221 
06222     LOG_FUNCTION_END( status );
06223     return status;
06224 }
06225 
06226 VL53L0X_Error VL53L0X::vl53l0x_set_histogram_mode( VL53L0X_DEV dev,
06227         VL53L0X_HistogramModes histogram_mode )
06228 {
06229     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
06230     LOG_FUNCTION_START( "" );
06231 
06232     /* not implemented on VL53L0X */
06233 
06234     LOG_FUNCTION_END( Status );
06235     return status;
06236 }
06237 
06238 VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_current( VL53L0X_DEV dev, uint16_t limit_check_id,
06239         FixPoint1616_t *p_limit_check_current )
06240 {
06241     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06242     VL53L0X_RangingMeasurementData_t last_range_data_buffer;
06243 
06244     LOG_FUNCTION_START( "" );
06245 
06246     if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
06247         status = VL53L0X_ERROR_INVALID_PARAMS;
06248     } else {
06249         switch ( limit_check_id ) {
06250             case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
06251                 /* Need to run a ranging to have the latest values */
06252                 *p_limit_check_current = PALDevDataGet( dev, SigmaEstimate );
06253 
06254                 break;
06255 
06256             case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
06257                 /* Need to run a ranging to have the latest values */
06258                 last_range_data_buffer = PALDevDataGet( dev,
06259                                                         LastRangeMeasure );
06260                 *p_limit_check_current =
06261                     last_range_data_buffer.SignalRateRtnMegaCps;
06262 
06263                 break;
06264 
06265             case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
06266                 /* Need to run a ranging to have the latest values */
06267                 *p_limit_check_current = PALDevDataGet( dev,
06268                                                         LastSignalRefMcps );
06269 
06270                 break;
06271 
06272             case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
06273                 /* Need to run a ranging to have the latest values */
06274                 last_range_data_buffer = PALDevDataGet( dev,
06275                                                         LastRangeMeasure );
06276                 *p_limit_check_current =
06277                     last_range_data_buffer.SignalRateRtnMegaCps;
06278 
06279                 break;
06280 
06281             case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
06282                 /* Need to run a ranging to have the latest values */
06283                 last_range_data_buffer = PALDevDataGet( dev,
06284                                                         LastRangeMeasure );
06285                 *p_limit_check_current =
06286                     last_range_data_buffer.SignalRateRtnMegaCps;
06287 
06288                 break;
06289 
06290             case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
06291                 /* Need to run a ranging to have the latest values */
06292                 last_range_data_buffer = PALDevDataGet( dev,
06293                                                         LastRangeMeasure );
06294                 *p_limit_check_current =
06295                     last_range_data_buffer.SignalRateRtnMegaCps;
06296 
06297                 break;
06298 
06299             default:
06300                 status = VL53L0X_ERROR_INVALID_PARAMS;
06301         }
06302     }
06303 
06304     LOG_FUNCTION_END( status );
06305     return status;
06306 
06307 }
06308 
06309 VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_info( VL53L0X_DEV dev, uint16_t limit_check_id,
06310         char *p_limit_check_string )
06311 {
06312     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06313 
06314     LOG_FUNCTION_START( "" );
06315 
06316     status = wrapped_vl53l0x_get_limit_check_info( dev, limit_check_id,
06317              p_limit_check_string );
06318 
06319     LOG_FUNCTION_END( status );
06320     return status;
06321 }
06322 
06323 VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_status( VL53L0X_DEV dev, uint16_t limit_check_id,
06324         uint8_t *p_limit_check_status )
06325 {
06326     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06327     uint8_t temp8;
06328 
06329     LOG_FUNCTION_START( "" );
06330 
06331     if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
06332         status = VL53L0X_ERROR_INVALID_PARAMS;
06333     } else {
06334 
06335         VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
06336                                         limit_check_id, temp8 );
06337 
06338         *p_limit_check_status = temp8;
06339 
06340     }
06341 
06342     LOG_FUNCTION_END( status );
06343     return status;
06344 }
06345 
06346 VL53L0X_Error VL53L0X::vl53l0x_set_linearity_corrective_gain( VL53L0X_DEV dev,
06347         int16_t linearity_corrective_gain )
06348 {
06349     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06350     LOG_FUNCTION_START( "" );
06351 
06352     if ( ( linearity_corrective_gain < 0 ) || ( linearity_corrective_gain > 1000 ) )
06353         status = VL53L0X_ERROR_INVALID_PARAMS;
06354     else {
06355         PALDevDataSet( dev, LinearityCorrectiveGain,
06356                        linearity_corrective_gain );
06357 
06358         if ( linearity_corrective_gain != 1000 ) {
06359             /* Disable FW Xtalk */
06360             status = vl53l0x_write_word( dev,
06361                                      VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, 0 );
06362         }
06363     }
06364 
06365     LOG_FUNCTION_END( status );
06366     return status;
06367 }
06368 
06369 VL53L0X_Error VL53L0X::vl53l0x_get_linearity_corrective_gain( VL53L0X_DEV dev,
06370         uint16_t *p_linearity_corrective_gain )
06371 {
06372     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06373     LOG_FUNCTION_START( "" );
06374 
06375     *p_linearity_corrective_gain = PALDevDataGet( dev, LinearityCorrectiveGain );
06376 
06377     LOG_FUNCTION_END( status );
06378     return status;
06379 }
06380 
06381 VL53L0X_Error VL53L0X::vl53l0x_get_max_number_of_roi_zones( VL53L0X_DEV dev,
06382         uint8_t *p_max_number_of_roi_zones )
06383 {
06384     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06385 
06386     LOG_FUNCTION_START( "" );
06387 
06388     *p_max_number_of_roi_zones = 1;
06389 
06390     LOG_FUNCTION_END( status );
06391     return status;
06392 }
06393 
06394 VL53L0X_Error VL53L0X::vl53l0x_get_measurement_ref_signal( VL53L0X_DEV dev,
06395         FixPoint1616_t *p_measurement_ref_signal )
06396 {
06397     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06398     uint8_t signal_ref_clip_limit_check_enable = 0;
06399     LOG_FUNCTION_START( "" );
06400 
06401     status = vl53l0x_get_limit_check_enable( dev,
06402              VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
06403              &signal_ref_clip_limit_check_enable );
06404     if ( signal_ref_clip_limit_check_enable != 0 ) {
06405         *p_measurement_ref_signal = PALDevDataGet( dev, LastSignalRefMcps );
06406     } else {
06407         status = VL53L0X_ERROR_INVALID_COMMAND;
06408     }
06409     LOG_FUNCTION_END( status );
06410 
06411     return status;
06412 }
06413 
06414 
06415 VL53L0X_Error VL53L0X::vl53l0x_get_number_of_limit_check( uint16_t *p_number_of_limit_check )
06416 {
06417     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06418     LOG_FUNCTION_START( "" );
06419 
06420     *p_number_of_limit_check = VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS;
06421 
06422     LOG_FUNCTION_END( status );
06423     return status;
06424 }
06425 
06426 VL53L0X_Error VL53L0X::vl53l0x_get_number_of_roi_zones( VL53L0X_DEV dev,
06427         uint8_t *p_number_of_roi_zones )
06428 {
06429     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06430 
06431     LOG_FUNCTION_START( "" );
06432 
06433     *p_number_of_roi_zones = 1;
06434 
06435     LOG_FUNCTION_END( status );
06436     return status;
06437 }
06438 
06439 VL53L0X_Error VL53L0X::vl53l0x_set_number_of_roi_zones( VL53L0X_DEV dev,
06440         uint8_t number_of_roi_zones )
06441 {
06442     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06443 
06444     LOG_FUNCTION_START( "" );
06445 
06446     if ( number_of_roi_zones != 1 )
06447         status = VL53L0X_ERROR_INVALID_PARAMS;
06448 
06449 
06450     LOG_FUNCTION_END( status );
06451     return status;
06452 }
06453 
06454 VL53L0X_Error VL53L0X::vl53l0x_get_number_of_sequence_steps( VL53L0X_DEV dev,
06455         uint8_t *p_number_of_sequence_steps )
06456 {
06457     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06458     LOG_FUNCTION_START( "" );
06459 
06460     *p_number_of_sequence_steps = VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS;
06461 
06462     LOG_FUNCTION_END( status );
06463     return status;
06464 }
06465 
06466 VL53L0X_Error VL53L0X::vl53l0x_get_pal_error_string( VL53L0X_Error pal_error_code,
06467         char *p_pal_error_string )
06468 {
06469     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06470     LOG_FUNCTION_START( "" );
06471 
06472     status = wrapped_vl53l0x_get_pal_error_string( pal_error_code, p_pal_error_string );
06473 
06474     LOG_FUNCTION_END( status );
06475     return status;
06476 }
06477 
06478 VL53L0X_Error VL53L0X::vl53l0x_get_pal_spec_version( VL53L0X_Version_t *p_pal_spec_version )
06479 {
06480     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06481 
06482     LOG_FUNCTION_START( "" );
06483 
06484     p_pal_spec_version->major  = VL53L0X_SPECIFICATION_VER_MAJOR;
06485     p_pal_spec_version->minor  = VL53L0X_SPECIFICATION_VER_MINOR;
06486     p_pal_spec_version->build  = VL53L0X_SPECIFICATION_VER_SUB;
06487 
06488     p_pal_spec_version->revision  = VL53L0X_SPECIFICATION_VER_REVISION;
06489 
06490     LOG_FUNCTION_END( status );
06491     return status;
06492 }
06493 
06494 VL53L0X_Error VL53L0X::vl53l0x_get_pal_state( VL53L0X_DEV dev, VL53L0X_State *p_pal_state )
06495 {
06496     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06497     LOG_FUNCTION_START( "" );
06498 
06499     *p_pal_state = PALDevDataGet( dev, PalState );
06500 
06501     LOG_FUNCTION_END( status );
06502     return status;
06503 }
06504 
06505 VL53L0X_Error VL53L0X::vl53l0x_get_pal_state_string( VL53L0X_State pal_state_code,
06506         char *p_pal_state_string )
06507 {
06508     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06509     LOG_FUNCTION_START( "" );
06510 
06511     status = wrapped_vl53l0x_get_pal_state_string( pal_state_code, p_pal_state_string );
06512 
06513     LOG_FUNCTION_END( status );
06514     return status;
06515 }
06516 
06517 VL53L0X_Error VL53L0X::vl53l0x_get_power_mode( VL53L0X_DEV dev, VL53L0X_PowerModes *p_power_mode )
06518 {
06519     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06520     uint8_t byte;
06521     LOG_FUNCTION_START( "" );
06522 
06523     /* Only level1 of Power mode exists */
06524     status = vl53l0x_read_byte( dev, 0x80, &byte );
06525 
06526     if ( status == VL53L0X_ERROR_NONE ) {
06527         if ( byte == 1 ) {
06528             PALDevDataSet( dev, PowerMode,
06529                            VL53L0X_POWERMODE_IDLE_LEVEL1 );
06530         } else {
06531             PALDevDataSet( dev, PowerMode,
06532                            VL53L0X_POWERMODE_STANDBY_LEVEL1 );
06533         }
06534     }
06535 
06536     LOG_FUNCTION_END( status );
06537     return status;
06538 }
06539 
06540 VL53L0X_Error VL53L0X::vl53l0x_set_power_mode( VL53L0X_DEV dev, VL53L0X_PowerModes power_mode )
06541 {
06542     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06543     LOG_FUNCTION_START( "" );
06544 
06545     /* Only level1 of Power mode exists */
06546     if ( ( power_mode != VL53L0X_POWERMODE_STANDBY_LEVEL1 )
06547             && ( power_mode != VL53L0X_POWERMODE_IDLE_LEVEL1 ) ) {
06548         status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
06549     } else if ( power_mode == VL53L0X_POWERMODE_STANDBY_LEVEL1 ) {
06550         /* set the standby level1 of power mode */
06551         status = vl53l0x_write_byte( dev, 0x80, 0x00 );
06552         if ( status == VL53L0X_ERROR_NONE ) {
06553             /* Set PAL State to standby */
06554             PALDevDataSet( dev, PalState, VL53L0X_STATE_STANDBY );
06555             PALDevDataSet( dev, PowerMode,
06556                            VL53L0X_POWERMODE_STANDBY_LEVEL1 );
06557         }
06558 
06559     } else {
06560         /* VL53L0X_POWERMODE_IDLE_LEVEL1 */
06561         status = vl53l0x_write_byte( dev, 0x80, 0x00 );
06562         if ( status == VL53L0X_ERROR_NONE )
06563             status = vl53l0x_static_init( dev );
06564 
06565         if ( status == VL53L0X_ERROR_NONE )
06566             PALDevDataSet( dev, PowerMode,
06567                            VL53L0X_POWERMODE_IDLE_LEVEL1 );
06568 
06569     }
06570 
06571     LOG_FUNCTION_END( status );
06572     return status;
06573 }
06574 
06575 VL53L0X_Error VL53L0X::vl53l0x_get_product_revision( VL53L0X_DEV dev,
06576         uint8_t *p_product_revision_major, uint8_t *p_product_revision_minor )
06577 {
06578     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06579     uint8_t revision_id;
06580 
06581     LOG_FUNCTION_START( "" );
06582 
06583     status = vl53l0x_read_byte( dev, VL53L0X_REG_IDENTIFICATION_REVISION_ID,
06584                              &revision_id );
06585     *p_product_revision_major = 1;
06586     *p_product_revision_minor = ( revision_id & 0xF0 ) >> 4;
06587 
06588     LOG_FUNCTION_END( status );
06589     return status;
06590 
06591 }
06592 
06593 VL53L0X_Error VL53L0X::vl53l0x_get_range_status_string( uint8_t range_status,
06594         char *p_range_status_string )
06595 {
06596     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06597     LOG_FUNCTION_START( "" );
06598 
06599     status = wrapped_vl53l0x_get_range_status_string( range_status,
06600              p_range_status_string );
06601 
06602     LOG_FUNCTION_END( status );
06603     return status;
06604 }
06605 
06606 VL53L0X_Error VL53L0X::vl53l0x_get_ref_calibration( VL53L0X_DEV dev, uint8_t *p_vhv_settings,
06607         uint8_t *p_phase_cal )
06608 {
06609     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06610     LOG_FUNCTION_START( "" );
06611 
06612     status = wrapped_vl53l0x_get_ref_calibration( dev, p_vhv_settings, p_phase_cal );
06613 
06614     LOG_FUNCTION_END( status );
06615     return status;
06616 }
06617 
06618 VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_enable( VL53L0X_DEV dev,
06619         VL53L0X_SequenceStepId sequence_step_id, uint8_t *p_sequence_step_enabled )
06620 {
06621     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06622     uint8_t SequenceConfig = 0;
06623     LOG_FUNCTION_START( "" );
06624 
06625     status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
06626                              &SequenceConfig );
06627 
06628     if ( status == VL53L0X_ERROR_NONE ) {
06629         status = sequence_step_enabled( dev, sequence_step_id,
06630                                         SequenceConfig, p_sequence_step_enabled );
06631     }
06632 
06633     LOG_FUNCTION_END( status );
06634     return status;
06635 }
06636 
06637 VL53L0X_Error VL53L0X::vl53l0x_set_sequence_step_timeout( VL53L0X_DEV dev,
06638         VL53L0X_SequenceStepId sequence_step_id, FixPoint1616_t time_out_milli_secs )
06639 {
06640     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06641     VL53L0X_Error status1 = VL53L0X_ERROR_NONE;
06642     uint32_t TimeoutMicroSeconds = ( ( time_out_milli_secs * 1000 ) + 0x8000 )
06643                                    >> 16;
06644     uint32_t MeasurementTimingBudgetMicroSeconds;
06645     FixPoint1616_t OldTimeOutMicroSeconds;
06646 
06647     LOG_FUNCTION_START( "" );
06648 
06649     /* Read back the current value in case we need to revert back to this.
06650      */
06651     status = get_sequence_step_timeout( dev, sequence_step_id,
06652                                         &OldTimeOutMicroSeconds );
06653 
06654     if ( status == VL53L0X_ERROR_NONE ) {
06655         status = set_sequence_step_timeout( dev, sequence_step_id,
06656                                             TimeoutMicroSeconds );
06657     }
06658 
06659     if ( status == VL53L0X_ERROR_NONE ) {
06660         VL53L0X_GETPARAMETERFIELD( dev,
06661                                    MeasurementTimingBudgetMicroSeconds,
06662                                    MeasurementTimingBudgetMicroSeconds );
06663 
06664         /* At this point we don't know if the requested value is valid,
06665          therefore proceed to update the entire timing budget and
06666          if this fails, revert back to the previous value.
06667          */
06668         status = vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
06669                  MeasurementTimingBudgetMicroSeconds );
06670 
06671         if ( status != VL53L0X_ERROR_NONE ) {
06672             status1 = set_sequence_step_timeout( dev, sequence_step_id,
06673                                                  OldTimeOutMicroSeconds );
06674 
06675             if ( status1 == VL53L0X_ERROR_NONE ) {
06676                 status1 =
06677                     vl53l0x_set_measurement_timing_budget_micro_seconds(
06678                         dev,
06679                         MeasurementTimingBudgetMicroSeconds );
06680             }
06681 
06682             status = status1;
06683         }
06684     }
06685 
06686     LOG_FUNCTION_END( status );
06687 
06688     return status;
06689 }
06690 
06691 VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_timeout( VL53L0X_DEV dev,
06692         VL53L0X_SequenceStepId sequence_step_id, FixPoint1616_t *p_time_out_milli_secs )
06693 {
06694     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06695     uint32_t timeout_micro_seconds;
06696     LOG_FUNCTION_START( "" );
06697 
06698     status = get_sequence_step_timeout( dev, sequence_step_id,
06699                                         &timeout_micro_seconds );
06700     if ( status == VL53L0X_ERROR_NONE ) {
06701         timeout_micro_seconds <<= 8;
06702         *p_time_out_milli_secs = ( timeout_micro_seconds + 500 ) / 1000;
06703         *p_time_out_milli_secs <<= 8;
06704     }
06705 
06706     LOG_FUNCTION_END( status );
06707     return status;
06708 }
06709 
06710 VL53L0X_Error VL53L0X::vl53l0x_set_spad_ambient_damper_factor( VL53L0X_DEV dev,
06711         uint16_t spad_ambient_damper_factor )
06712 {
06713     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06714     uint8_t byte;
06715     LOG_FUNCTION_START( "" );
06716 
06717     byte = ( uint8_t )( spad_ambient_damper_factor & 0x00FF );
06718 
06719     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
06720     status |= vl53l0x_write_byte( dev, 0x42, byte );
06721     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
06722 
06723     LOG_FUNCTION_END( status );
06724     return status;
06725 }
06726 
06727 
06728 VL53L0X_Error VL53L0X::vl53l0x_get_spad_ambient_damper_factor( VL53L0X_DEV dev,
06729         uint16_t *p_spad_ambient_damper_factor )
06730 {
06731     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06732     uint8_t Byte;
06733     LOG_FUNCTION_START( "" );
06734 
06735     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
06736     status |= vl53l0x_read_byte( dev, 0x42, &Byte );
06737     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
06738     *p_spad_ambient_damper_factor = ( uint16_t )Byte;
06739 
06740     LOG_FUNCTION_END( status );
06741     return status;
06742 }
06743 
06744 VL53L0X_Error VL53L0X::vl53l0x_set_spad_ambient_damper_threshold( VL53L0X_DEV dev,
06745         uint16_t spad_ambient_damper_threshold )
06746 {
06747     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06748     LOG_FUNCTION_START( "" );
06749 
06750     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
06751     status |= vl53l0x_write_word( dev, 0x40, spad_ambient_damper_threshold );
06752     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
06753 
06754     LOG_FUNCTION_END( status );
06755     return status;
06756 }
06757 
06758 VL53L0X_Error VL53L0X::vl53l0x_get_spad_ambient_damper_threshold( VL53L0X_DEV dev,
06759         uint16_t *p_spad_ambient_damper_threshold )
06760 {
06761     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06762     LOG_FUNCTION_START( "" );
06763 
06764     status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
06765     status |= vl53l0x_read_word( dev, 0x40, p_spad_ambient_damper_threshold );
06766     status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
06767 
06768     LOG_FUNCTION_END( status );
06769     return status;
06770 }
06771 
06772 VL53L0X_Error VL53L0X::vl53l0x_get_upper_limit_milli_meter( VL53L0X_DEV dev,
06773         uint16_t *p_upper_limit_milli_meter )
06774 {
06775     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
06776     LOG_FUNCTION_START( "" );
06777 
06778     /* not implemented on VL53L0X */
06779 
06780     LOG_FUNCTION_END( status );
06781     return status;
06782 }
06783 
06784 VL53L0X_Error VL53L0X::vl53l0x_set_tuning_setting_buffer( VL53L0X_DEV dev,
06785         uint8_t *p_tuning_setting_buffer, uint8_t use_internal_tuning_settings )
06786 {
06787     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06788 
06789     LOG_FUNCTION_START( "" );
06790 
06791     if ( use_internal_tuning_settings == 1 ) {
06792         /* Force use internal settings */
06793         PALDevDataSet( dev, UseInternalTuningSettings, 1 );
06794     } else {
06795 
06796         /* check that the first byte is not 0 */
06797         if ( *p_tuning_setting_buffer != 0 ) {
06798             PALDevDataSet( dev, pTuningSettingsPointer,
06799                            p_tuning_setting_buffer );
06800             PALDevDataSet( dev, UseInternalTuningSettings, 0 );
06801 
06802         } else {
06803             status = VL53L0X_ERROR_INVALID_PARAMS;
06804         }
06805     }
06806 
06807     LOG_FUNCTION_END( status );
06808     return status;
06809 }
06810 
06811 VL53L0X_Error VL53L0X::vl53l0x_get_tuning_setting_buffer( VL53L0X_DEV dev,
06812         uint8_t **pp_tuning_setting_buffer, uint8_t *p_use_internal_tuning_settings )
06813 {
06814     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06815 
06816     LOG_FUNCTION_START( "" );
06817 
06818     *pp_tuning_setting_buffer = PALDevDataGet( dev, pTuningSettingsPointer );
06819     *p_use_internal_tuning_settings = PALDevDataGet( dev,
06820                                       UseInternalTuningSettings );
06821 
06822     LOG_FUNCTION_END( status );
06823     return status;
06824 }
06825 
06826 VL53L0X_Error VL53L0X::vl53l0x_get_version( VL53L0X_Version_t *p_version )
06827 {
06828     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06829     LOG_FUNCTION_START( "" );
06830 
06831     p_version->major  = VL53L0X_IMPLEMENTATION_VER_MAJOR;
06832     p_version->minor  = VL53L0X_IMPLEMENTATION_VER_MINOR;
06833     p_version->build  = VL53L0X_IMPLEMENTATION_VER_SUB;
06834 
06835     p_version->revision  = VL53L0X_IMPLEMENTATION_VER_REVISION;
06836 
06837     LOG_FUNCTION_END( status );
06838     return status;
06839 }
06840 
06841 VL53L0X_Error VL53L0X::vl53l0x_lock_sequence_access( VL53L0X_DEV dev )
06842 {
06843     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06844 
06845     return status;
06846 }
06847 
06848 VL53L0X_Error VL53L0X::vl53l0x_unlock_sequence_access( VL53L0X_DEV dev )
06849 {
06850     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06851 
06852     return status;
06853 }
06854 
06855 VL53L0X_Error VL53L0X::vl53l0x_perform_offset_calibration( VL53L0X_DEV dev,
06856         FixPoint1616_t cal_distance_milli_meter,
06857         int32_t *p_offset_micro_meter )
06858 {
06859     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06860     uint16_t sum_ranging = 0;
06861     FixPoint1616_t total_count = 0;
06862     VL53L0X_RangingMeasurementData_t ranging_measurement_data;
06863     FixPoint1616_t stored_mean_range;
06864     uint32_t stored_mean_range_as_int;
06865     uint32_t cal_distance_as_int_mm;
06866     uint8_t sequence_step_enabled;
06867     int meas = 0;
06868 
06869     if ( cal_distance_milli_meter <= 0 )
06870         status = VL53L0X_ERROR_INVALID_PARAMS;
06871 
06872     if ( status == VL53L0X_ERROR_NONE )
06873         status = wrapped_vl53l0x_set_offset_calibration_data_micro_meter( dev, 0 );
06874 
06875 
06876     /* Get the value of the TCC */
06877     if ( status == VL53L0X_ERROR_NONE )
06878         status = vl53l0x_get_sequence_step_enable( dev,
06879                  VL53L0X_SEQUENCESTEP_TCC, &sequence_step_enabled );
06880 
06881 
06882     /* Disable the TCC */
06883     if ( status == VL53L0X_ERROR_NONE )
06884         status = vl53l0x_set_sequence_step_enable( dev,
06885                  VL53L0X_SEQUENCESTEP_TCC, 0 );
06886 
06887 
06888     /* Disable the RIT */
06889     if ( status == VL53L0X_ERROR_NONE )
06890         status = vl53l0x_set_limit_check_enable( dev,
06891                  VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0 );
06892 
06893     /* Perform 50 measurements and compute the averages */
06894     if ( status == VL53L0X_ERROR_NONE ) {
06895         sum_ranging = 0;
06896         total_count = 0;
06897         for ( meas = 0; meas < 50; meas++ ) {
06898             status = vl53l0x_perform_single_ranging_measurement( dev,
06899                      &ranging_measurement_data );
06900 
06901             if ( status != VL53L0X_ERROR_NONE )
06902                 break;
06903 
06904             /* The range is valid when RangeStatus = 0 */
06905             if ( ranging_measurement_data.RangeStatus == 0 ) {
06906                 sum_ranging = sum_ranging +
06907                               ranging_measurement_data.RangeMilliMeter;
06908                 total_count = total_count + 1;
06909             }
06910         }
06911 
06912         /* no valid values found */
06913         if ( total_count == 0 )
06914             status = VL53L0X_ERROR_RANGE_ERROR;
06915     }
06916 
06917 
06918     if ( status == VL53L0X_ERROR_NONE ) {
06919         /* FixPoint1616_t / uint16_t = FixPoint1616_t */
06920         stored_mean_range = ( FixPoint1616_t )( ( uint32_t )( sum_ranging << 16 )
06921                                                 / total_count );
06922 
06923         stored_mean_range_as_int = ( stored_mean_range + 0x8000 ) >> 16;
06924 
06925         /* Round Cal Distance to Whole Number.
06926          * Note that the cal distance is in mm, therefore no resolution
06927          * is lost.*/
06928         cal_distance_as_int_mm = ( cal_distance_milli_meter + 0x8000 ) >> 16;
06929 
06930         *p_offset_micro_meter = ( cal_distance_as_int_mm -
06931                                   stored_mean_range_as_int ) * 1000;
06932 
06933         /* Apply the calculated offset */
06934         if ( status == VL53L0X_ERROR_NONE ) {
06935             VL53L0X_SETPARAMETERFIELD( dev, RangeOffsetMicroMeters,
06936                                        *p_offset_micro_meter );
06937             status = vl53l0x_set_offset_calibration_data_micro_meter( dev,
06938                      *p_offset_micro_meter );
06939         }
06940 
06941     }
06942 
06943     /* Restore the TCC */
06944     if ( status == VL53L0X_ERROR_NONE ) {
06945         if ( sequence_step_enabled != 0 )
06946             status = vl53l0x_set_sequence_step_enable( dev,
06947                      VL53L0X_SEQUENCESTEP_TCC, 1 );
06948     }
06949 
06950     return status;
06951 }
06952 
06953 VL53L0X_Error VL53L0X::vl53l0x_set_x_talk_compensation_enable( VL53L0X_DEV dev,
06954         uint8_t x_talk_compensation_enable )
06955 {
06956     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06957     FixPoint1616_t temp_fix1616;
06958     uint16_t linearity_corrective_gain;
06959 
06960     LOG_FUNCTION_START( "" );
06961 
06962     linearity_corrective_gain = PALDevDataGet( dev, LinearityCorrectiveGain );
06963 
06964     if ( ( x_talk_compensation_enable == 0 )
06965             || ( linearity_corrective_gain != 1000 ) ) {
06966         temp_fix1616 = 0;
06967     } else {
06968         VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps,
06969                                    temp_fix1616 );
06970     }
06971 
06972     /* the following register has a format 3.13 */
06973     status = vl53l0x_write_word( dev,
06974                              VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS,
06975                              VL53L0X_FIXPOINT1616TOFIXPOINT313( temp_fix1616 ) );
06976 
06977     if ( status == VL53L0X_ERROR_NONE ) {
06978         if ( x_talk_compensation_enable == 0 ) {
06979             VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
06980                                        0 );
06981         } else {
06982             VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
06983                                        1 );
06984         }
06985     }
06986 
06987     LOG_FUNCTION_END( status );
06988     return status;
06989 }
06990 
06991 
06992 VL53L0X_Error VL53L0X::vl53l0x_perform_xtalk_calibration( VL53L0X_DEV dev,
06993         FixPoint1616_t xtalk_cal_distance,
06994         FixPoint1616_t *p_xtalk_compensation_rate_mega_cps )
06995 {
06996     VL53L0X_Error status = VL53L0X_ERROR_NONE;
06997     uint16_t sum_ranging = 0;
06998     uint16_t sum_spads = 0;
06999     FixPoint1616_t sum_signal_rate = 0;
07000     FixPoint1616_t total_count = 0;
07001     uint8_t xtalk_meas = 0;
07002     VL53L0X_RangingMeasurementData_t ranging_measurement_data;
07003     FixPoint1616_t x_talk_stored_mean_signal_rate;
07004     FixPoint1616_t x_talk_stored_mean_range;
07005     FixPoint1616_t x_talk_stored_mean_rtn_spads;
07006     uint32_t signal_x_talk_total_per_spad;
07007     uint32_t x_talk_stored_mean_rtn_spads_as_int;
07008     uint32_t x_talk_cal_distance_as_int;
07009     FixPoint1616_t x_talk_compensation_rate_mega_cps;
07010 
07011     if ( xtalk_cal_distance <= 0 )
07012         status = VL53L0X_ERROR_INVALID_PARAMS;
07013 
07014     /* Disable the XTalk compensation */
07015     if ( status == VL53L0X_ERROR_NONE )
07016         status = vl53l0x_set_x_talk_compensation_enable( dev, 0 );
07017 
07018     /* Disable the RIT */
07019     if ( status == VL53L0X_ERROR_NONE ) {
07020         status = vl53l0x_set_limit_check_enable( dev,
07021                  VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0 );
07022     }
07023 
07024     /* Perform 50 measurements and compute the averages */
07025     if ( status == VL53L0X_ERROR_NONE ) {
07026         sum_ranging = 0;
07027         sum_spads = 0;
07028         sum_signal_rate = 0;
07029         total_count = 0;
07030         for ( xtalk_meas = 0; xtalk_meas < 50; xtalk_meas++ ) {
07031             status = vl53l0x_perform_single_ranging_measurement( dev,
07032                      &ranging_measurement_data );
07033 
07034             if ( status != VL53L0X_ERROR_NONE )
07035                 break;
07036 
07037             /* The range is valid when RangeStatus = 0 */
07038             if ( ranging_measurement_data.RangeStatus == 0 ) {
07039                 sum_ranging = sum_ranging +
07040                               ranging_measurement_data.RangeMilliMeter;
07041                 sum_signal_rate = sum_signal_rate +
07042                                   ranging_measurement_data.SignalRateRtnMegaCps;
07043                 sum_spads = sum_spads +
07044                             ranging_measurement_data.EffectiveSpadRtnCount
07045                             / 256;
07046                 total_count = total_count + 1;
07047             }
07048         }
07049 
07050         /* no valid values found */
07051         if ( total_count == 0 )
07052             status = VL53L0X_ERROR_RANGE_ERROR;
07053 
07054     }
07055 
07056 
07057     if ( status == VL53L0X_ERROR_NONE ) {
07058         /* FixPoint1616_t / uint16_t = FixPoint1616_t */
07059         x_talk_stored_mean_signal_rate = sum_signal_rate / total_count;
07060         x_talk_stored_mean_range = ( FixPoint1616_t )( ( uint32_t )(
07061                                        sum_ranging << 16 ) / total_count );
07062         x_talk_stored_mean_rtn_spads = ( FixPoint1616_t )( ( uint32_t )(
07063                                            sum_spads << 16 ) / total_count );
07064 
07065         /* Round Mean Spads to Whole Number.
07066          * Typically the calculated mean SPAD count is a whole number
07067          * or very close to a whole
07068          * number, therefore any truncation will not result in a
07069          * significant loss in accuracy.
07070          * Also, for a grey target at a typical distance of around
07071          * 400mm, around 220 SPADs will
07072          * be enabled, therefore, any truncation will result in a loss
07073          * of accuracy of less than
07074          * 0.5%.
07075          */
07076         x_talk_stored_mean_rtn_spads_as_int = ( x_talk_stored_mean_rtn_spads +
07077                                                 0x8000 ) >> 16;
07078 
07079         /* Round Cal Distance to Whole Number.
07080          * Note that the cal distance is in mm, therefore no resolution
07081          * is lost.*/
07082         x_talk_cal_distance_as_int = ( xtalk_cal_distance + 0x8000 ) >> 16;
07083 
07084         if ( x_talk_stored_mean_rtn_spads_as_int == 0 ||
07085                 x_talk_cal_distance_as_int == 0 ||
07086                 x_talk_stored_mean_range >= xtalk_cal_distance ) {
07087             x_talk_compensation_rate_mega_cps = 0;
07088         } else {
07089             /* Round Cal Distance to Whole Number.
07090                Note that the cal distance is in mm, therefore no
07091                resolution is lost.*/
07092             x_talk_cal_distance_as_int = ( xtalk_cal_distance +
07093                                            0x8000 ) >> 16;
07094 
07095             /* Apply division by mean spad count early in the
07096              * calculation to keep the numbers small.
07097              * This ensures we can maintain a 32bit calculation.
07098              * Fixed1616 / int := Fixed1616 */
07099             signal_x_talk_total_per_spad = ( x_talk_stored_mean_signal_rate ) /
07100                                            x_talk_stored_mean_rtn_spads_as_int;
07101 
07102             /* Complete the calculation for total Signal XTalk per
07103              * SPAD
07104              * Fixed1616 * (Fixed1616 - Fixed1616/int) :=
07105              * (2^16 * Fixed1616)
07106              */
07107             signal_x_talk_total_per_spad *= ( ( 1 << 16 ) -
07108                                               ( x_talk_stored_mean_range / x_talk_cal_distance_as_int ) );
07109 
07110             /* Round from 2^16 * Fixed1616, to Fixed1616. */
07111             x_talk_compensation_rate_mega_cps = ( signal_x_talk_total_per_spad
07112                                                   + 0x8000 ) >> 16;
07113         }
07114 
07115         *p_xtalk_compensation_rate_mega_cps = x_talk_compensation_rate_mega_cps;
07116 
07117         /* Enable the XTalk compensation */
07118         if ( status == VL53L0X_ERROR_NONE )
07119             status = vl53l0x_set_x_talk_compensation_enable( dev, 1 );
07120 
07121         /* Enable the XTalk compensation */
07122         if ( status == VL53L0X_ERROR_NONE )
07123             status = vl53l0x_set_x_talk_compensation_rate_mega_cps( dev,
07124                      x_talk_compensation_rate_mega_cps );
07125 
07126     }
07127 
07128     return status;
07129 }
07130 
07131 VL53L0X_Error VL53L0X::vl53l0x_set_x_talk_compensation_rate_mega_cps( VL53L0X_DEV dev,
07132         FixPoint1616_t x_talk_compensation_rate_mega_cps )
07133 {
07134     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07135     uint8_t temp8;
07136     uint16_t linearity_corrective_gain;
07137     uint16_t data;
07138     LOG_FUNCTION_START( "" );
07139 
07140     VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationEnable, temp8 );
07141     linearity_corrective_gain = PALDevDataGet( dev, LinearityCorrectiveGain );
07142 
07143     if ( temp8 == 0 ) { /* disabled write only internal value */
07144         VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps,
07145                                    x_talk_compensation_rate_mega_cps );
07146     } else {
07147         /* the following register has a format 3.13 */
07148         if ( linearity_corrective_gain == 1000 ) {
07149             data = VL53L0X_FIXPOINT1616TOFIXPOINT313(
07150                        x_talk_compensation_rate_mega_cps );
07151         } else {
07152             data = 0;
07153         }
07154 
07155         status = vl53l0x_write_word( dev,
07156                                  VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, data );
07157 
07158         if ( status == VL53L0X_ERROR_NONE ) {
07159             VL53L0X_SETPARAMETERFIELD( dev,
07160                                        XTalkCompensationRateMegaCps,
07161                                        x_talk_compensation_rate_mega_cps );
07162         }
07163     }
07164 
07165     LOG_FUNCTION_END( status );
07166     return status;
07167 }
07168 
07169 VL53L0X_Error VL53L0X::vl53l0x_perform_xtalk_measurement( VL53L0X_DEV dev,
07170         uint32_t timeout_ms, FixPoint1616_t *p_xtalk_per_spad,
07171         uint8_t *p_ambient_too_high )
07172 {
07173     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
07174     LOG_FUNCTION_START( "" );
07175 
07176     /* not implemented on VL53L0X */
07177 
07178     LOG_FUNCTION_END( status );
07179     return status;
07180 }
07181 
07182 VL53L0X_Error VL53L0X::vl53l0x_reset_device( VL53L0X_DEV dev )
07183 {
07184     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07185     uint8_t byte;
07186     LOG_FUNCTION_START( "" );
07187 
07188     /* Set reset bit */
07189     status = vl53l0x_write_byte( dev, VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N,
07190                              0x00 );
07191 
07192     /* Wait for some time */
07193     if ( status == VL53L0X_ERROR_NONE ) {
07194         do {
07195             status = vl53l0x_read_byte( dev,
07196                                      VL53L0X_REG_IDENTIFICATION_MODEL_ID, &byte );
07197         } while ( byte != 0x00 );
07198     }
07199 
07200     vl53l0x_polling_delay( dev );
07201 
07202     /* Release reset */
07203     status = vl53l0x_write_byte( dev, VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N,
07204                              0x01 );
07205 
07206     /* Wait until correct boot-up of the device */
07207     if ( status == VL53L0X_ERROR_NONE ) {
07208         do {
07209             status = vl53l0x_read_byte( dev,
07210                                      VL53L0X_REG_IDENTIFICATION_MODEL_ID, &byte );
07211         } while ( byte == 0x00 );
07212     }
07213 
07214     vl53l0x_polling_delay( dev );
07215 
07216     /* Set PAL State to VL53L0X_STATE_POWERDOWN */
07217     if ( status == VL53L0X_ERROR_NONE )
07218         PALDevDataSet( dev, PalState, VL53L0X_STATE_POWERDOWN );
07219 
07220 
07221     LOG_FUNCTION_END( status );
07222     return status;
07223 }
07224 
07225 VL53L0X_Error VL53L0X::VL53L0X_reverse_bytes( uint8_t *data, uint32_t size )
07226 {
07227     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07228     uint8_t temp_data;
07229     uint32_t mirror_index;
07230     uint32_t middle = size / 2;
07231     uint32_t index;
07232 
07233     for ( index = 0; index < middle; index++ ) {
07234         mirror_index         = size - index - 1;
07235         temp_data        = data[index];
07236         data[index]      = data[mirror_index];
07237         data[mirror_index] = temp_data;
07238     }
07239     return status;
07240 }
07241 
07242 VL53L0X_Error VL53L0X::vl53l0x_set_ref_calibration( VL53L0X_DEV dev,
07243         uint8_t vhv_settings, uint8_t phase_cal )
07244 {
07245     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07246     uint8_t p_vhv_settings;
07247     uint8_t p_phase_cal;
07248 
07249     status = vl53l0x_ref_calibration_io( dev, 0,
07250                                          vhv_settings, phase_cal,
07251                                          &p_vhv_settings, &p_phase_cal,
07252                                          1, 1 );
07253 
07254     return status;
07255 }
07256 
07257 VL53L0X_Error VL53L0X::VL53L0X_SetDeviceParameters( VL53L0X_DEV Dev,
07258         const VL53L0X_DeviceParameters_t *pDeviceParameters )
07259 {
07260     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
07261     int i;
07262     LOG_FUNCTION_START( "" );
07263     Status = vl53l0x_set_device_mode( Dev, pDeviceParameters->DeviceMode  );
07264 
07265     if ( Status == VL53L0X_ERROR_NONE )
07266         Status = vl53l0x_set_inter_measurement_period_milli_seconds( Dev,
07267                  pDeviceParameters->InterMeasurementPeriodMilliSeconds  );
07268 
07269 
07270     if ( Status == VL53L0X_ERROR_NONE )
07271         Status = vl53l0x_set_x_talk_compensation_rate_mega_cps( Dev,
07272                  pDeviceParameters->XTalkCompensationRateMegaCps  );
07273 
07274 
07275     if ( Status == VL53L0X_ERROR_NONE )
07276         Status = vl53l0x_set_offset_calibration_data_micro_meter( Dev,
07277                  pDeviceParameters->RangeOffsetMicroMeters  );
07278 
07279 
07280     for ( i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++ ) {
07281         if ( Status == VL53L0X_ERROR_NONE )
07282             Status |= vl53l0x_set_limit_check_enable( Dev, i,
07283                       pDeviceParameters->LimitChecksEnable [i] );
07284         else
07285             break;
07286 
07287         if ( Status == VL53L0X_ERROR_NONE )
07288             Status |= vl53l0x_set_limit_check_value( Dev, i,
07289                       pDeviceParameters->LimitChecksValue [i] );
07290         else
07291             break;
07292 
07293     }
07294 
07295     if ( Status == VL53L0X_ERROR_NONE )
07296         Status = vl53l0x_set_wrap_around_check_enable( Dev,
07297                  pDeviceParameters->WrapAroundCheckEnable  );
07298 
07299     if ( Status == VL53L0X_ERROR_NONE )
07300         Status = vl53l0x_set_measurement_timing_budget_micro_seconds( Dev,
07301                  pDeviceParameters->MeasurementTimingBudgetMicroSeconds  );
07302 
07303 
07304     LOG_FUNCTION_END( Status );
07305     return Status;
07306 }
07307 
07308 VL53L0X_Error VL53L0X::vl53l0x_set_inter_measurement_period_milli_seconds( VL53L0X_DEV dev,
07309         uint32_t inter_measurement_period_milli_seconds )
07310 {
07311     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07312     uint16_t osc_calibrate_val;
07313     uint32_t im_period_milli_seconds;
07314 
07315     LOG_FUNCTION_START( "" );
07316 
07317     status = vl53l0x_read_word( dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
07318                              &osc_calibrate_val );
07319 
07320     if ( status == VL53L0X_ERROR_NONE ) {
07321         if ( osc_calibrate_val != 0 ) {
07322             im_period_milli_seconds =
07323                 inter_measurement_period_milli_seconds
07324                 * osc_calibrate_val;
07325         } else {
07326             im_period_milli_seconds =
07327                 inter_measurement_period_milli_seconds;
07328         }
07329         status = vl53l0x_write_dword( dev,
07330                                   VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
07331                                   im_period_milli_seconds );
07332     }
07333 
07334     if ( status == VL53L0X_ERROR_NONE ) {
07335         VL53L0X_SETPARAMETERFIELD( dev,
07336                                    InterMeasurementPeriodMilliSeconds,
07337                                    inter_measurement_period_milli_seconds );
07338     }
07339 
07340     LOG_FUNCTION_END( status );
07341     return status;
07342 }
07343 
07344 VL53L0X_Error VL53L0X::vl53l0x_set_wrap_around_check_enable( VL53L0X_DEV dev,
07345         uint8_t wrap_around_check_enable )
07346 {
07347     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07348     uint8_t data;
07349     uint8_t wrap_around_check_enable_int;
07350 
07351     LOG_FUNCTION_START( "" );
07352 
07353     status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data );
07354     if ( wrap_around_check_enable == 0 ) {
07355         /* Disable wraparound */
07356         data = data & 0x7F;
07357         wrap_around_check_enable_int = 0;
07358     } else {
07359         /*Enable wraparound */
07360         data = data | 0x80;
07361         wrap_around_check_enable_int = 1;
07362     }
07363 
07364     status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, data );
07365 
07366     if ( status == VL53L0X_ERROR_NONE ) {
07367         PALDevDataSet( dev, SequenceConfig, data );
07368         VL53L0X_SETPARAMETERFIELD( dev, WrapAroundCheckEnable,
07369                                    wrap_around_check_enable_int );
07370     }
07371 
07372     LOG_FUNCTION_END( status );
07373     return status;
07374 }
07375 
07376 VL53L0X_Error VL53L0X::vl53l0x_set_group_param_hold( VL53L0X_DEV dev, uint8_t group_param_hold )
07377 {
07378     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
07379     LOG_FUNCTION_START( "" );
07380 
07381     /* not implemented on VL53L0X */
07382 
07383     LOG_FUNCTION_END( status );
07384     return status;
07385 }
07386 
07387 VL53L0X_Error VL53L0X::vl53l0x_set_range_fraction_enable( VL53L0X_DEV dev, uint8_t enable )
07388 {
07389     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07390 
07391     LOG_FUNCTION_START( "%d", ( int )Enable );
07392 
07393     status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, enable );
07394 
07395     if ( status == VL53L0X_ERROR_NONE )
07396         PALDevDataSet( dev, RangeFractionalEnable, enable );
07397 
07398     LOG_FUNCTION_END( status );
07399     return status;
07400 }
07401 
07402 VL53L0X_Error VL53L0X::vl53l0x_set_reference_spads( VL53L0X_DEV dev, uint32_t count,
07403         uint8_t is_aperture_spads )
07404 {
07405     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07406     LOG_FUNCTION_START( "" );
07407 
07408     status = wrapped_vl53l0x_set_reference_spads( dev, count, is_aperture_spads );
07409 
07410     LOG_FUNCTION_END( status );
07411 
07412     return status;
07413 }
07414 
07415 VL53L0X_Error VL53L0X::vl53l0x_get_reference_spads( VL53L0X_DEV dev, uint32_t *p_spad_count,
07416         uint8_t *p_is_aperture_spads )
07417 {
07418     VL53L0X_Error status = VL53L0X_ERROR_NONE;
07419     LOG_FUNCTION_START( "" );
07420 
07421     status = wrapped_vl53l0x_get_reference_spads( dev, p_spad_count, p_is_aperture_spads );
07422 
07423     LOG_FUNCTION_END( status );
07424 
07425     return status;
07426 }
07427 
07428 VL53L0X_Error VL53L0X::wrapped_vl53l0x_wait_device_booted( VL53L0X_DEV dev )
07429 {
07430     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
07431     LOG_FUNCTION_START( "" );
07432 
07433     /* not implemented on VL53L0X */
07434 
07435     LOG_FUNCTION_END( status );
07436     return status;
07437 }
07438 
07439 VL53L0X_Error VL53L0X::vl53l0x_wait_device_ready_for_new_measurement( VL53L0X_DEV dev,
07440         uint32_t max_loop )
07441 {
07442     VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
07443     LOG_FUNCTION_START( "" );
07444 
07445     /* not implemented for VL53L0X */
07446 
07447     LOG_FUNCTION_END( satus );
07448     return status;
07449 }
07450 /******************************************************************************/
07451