vl53l1_core.c

00001 
00002 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
00003 /******************************************************************************
00004  * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
00005 
00006  This file is part of VL53L1 and is dual licensed,
00007  either GPL-2.0+
00008  or 'BSD 3-clause "New" or "Revised" License' , at your option.
00009  ******************************************************************************
00010  */
00011 
00012 
00013 
00014 
00015 #include "vl53l1_ll_def.h"
00016 #include "vl53l1_ll_device.h"
00017 #include "vl53l1_platform.h"
00018 #include "vl53l1_register_map.h"
00019 #include "vl53l1_register_funcs.h"
00020 #include "vl53l1_register_settings.h"
00021 #include "vl53l1_hist_structs.h"
00022 #include "vl53l1_api_preset_modes.h"
00023 #include "vl53l1_core.h"
00024 #include "vl53l1_tuning_parm_defaults.h"
00025 
00026 
00027 
00028 #define LOG_FUNCTION_START(fmt, ...) \
00029     _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
00030 #define LOG_FUNCTION_END(status, ...) \
00031     _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
00032 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00033     _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \
00034         status, fmt, ##__VA_ARGS__)
00035 
00036 #define trace_print(level, ...) \
00037     _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
00038     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00039 
00040 
00041 void  VL53L1_init_version(
00042     VL53L1_DEV        Dev)
00043 {
00044 
00045 
00046     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
00047 
00048     pdev->version.ll_major    = VL53L1_LL_API_IMPLEMENTATION_VER_MAJOR;
00049     pdev->version.ll_minor    = VL53L1_LL_API_IMPLEMENTATION_VER_MINOR;
00050     pdev->version.ll_build    = VL53L1_LL_API_IMPLEMENTATION_VER_SUB;
00051     pdev->version.ll_revision = VL53L1_LL_API_IMPLEMENTATION_VER_REVISION;
00052 }
00053 
00054 
00055 void  VL53L1_init_ll_driver_state(
00056     VL53L1_DEV         Dev,
00057     VL53L1_DeviceState device_state)
00058 {
00059 
00060 
00061     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
00062     VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
00063 
00064     pstate->cfg_device_state  = device_state;
00065     pstate->cfg_stream_count  = 0;
00066     pstate->cfg_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00067     pstate->cfg_timing_status = 0;
00068     pstate->cfg_zone_id       = 0;
00069 
00070     pstate->rd_device_state   = device_state;
00071     pstate->rd_stream_count   = 0;
00072     pstate->rd_gph_id         = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00073     pstate->rd_timing_status  = 0;
00074     pstate->rd_zone_id        = 0;
00075 
00076 }
00077 
00078 
00079 VL53L1_Error  VL53L1_update_ll_driver_rd_state(
00080     VL53L1_DEV         Dev)
00081 {
00082 
00083 
00084     VL53L1_Error        status  = VL53L1_ERROR_NONE;
00085     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
00086     VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
00087 
00088 
00089 
00090     LOG_FUNCTION_START("");
00091 
00092 
00093 
00094     if ((pdev->sys_ctrl.system__mode_start &
00095         VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
00096 
00097         pstate->rd_device_state  = VL53L1_DEVICESTATE_SW_STANDBY;
00098         pstate->rd_stream_count  = 0;
00099         pstate->rd_internal_stream_count = 0;
00100         pstate->rd_internal_stream_count_val = 0;
00101         pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00102         pstate->rd_timing_status = 0;
00103         pstate->rd_zone_id       = 0;
00104 
00105     } else {
00106 
00107 
00108 
00109         if (pstate->rd_stream_count == 0xFF)
00110             pstate->rd_stream_count = 0x80;
00111         else
00112             pstate->rd_stream_count++;
00113 
00114 
00115         status = VL53L1_update_internal_stream_counters(Dev,
00116             pstate->rd_stream_count,
00117             &(pstate->rd_internal_stream_count),
00118             &(pstate->rd_internal_stream_count_val));
00119 
00120 
00121 
00122         pstate->rd_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00123 
00124 
00125 
00126         switch (pstate->rd_device_state) {
00127 
00128         case VL53L1_DEVICESTATE_SW_STANDBY:
00129 
00130             if ((pdev->dyn_cfg.system__grouped_parameter_hold &
00131                 VL53L1_GROUPEDPARAMETERHOLD_ID_MASK) > 0) {
00132                 pstate->rd_device_state =
00133                 VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC;
00134             } else {
00135                 if (pstate->rd_zone_id >=
00136                     pdev->zone_cfg.active_zones)
00137                     pstate->rd_device_state =
00138                     VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
00139                 else
00140                     pstate->rd_device_state =
00141                     VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
00142             }
00143 
00144             pstate->rd_stream_count  = 0;
00145             pstate->rd_internal_stream_count = 0;
00146             pstate->rd_internal_stream_count_val = 0;
00147             pstate->rd_timing_status = 0;
00148             pstate->rd_zone_id       = 0;
00149 
00150             break;
00151 
00152         case VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC:
00153             pstate->rd_stream_count = 0;
00154             pstate->rd_internal_stream_count = 0;
00155             pstate->rd_internal_stream_count_val = 0;
00156             pstate->rd_zone_id      = 0;
00157             if (pstate->rd_zone_id >=
00158                 pdev->zone_cfg.active_zones)
00159                 pstate->rd_device_state =
00160                     VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
00161             else
00162                 pstate->rd_device_state =
00163                     VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
00164 
00165             break;
00166 
00167         case VL53L1_DEVICESTATE_RANGING_GATHER_DATA:
00168             pstate->rd_zone_id++;
00169             if (pstate->rd_zone_id >=
00170                 pdev->zone_cfg.active_zones)
00171                 pstate->rd_device_state =
00172                     VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
00173             else
00174                 pstate->rd_device_state =
00175                     VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
00176 
00177             break;
00178 
00179         case VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA:
00180             pstate->rd_zone_id        = 0;
00181             pstate->rd_timing_status ^= 0x01;
00182 
00183             if (pstate->rd_zone_id >=
00184                 pdev->zone_cfg.active_zones)
00185                 pstate->rd_device_state =
00186                     VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
00187             else
00188                 pstate->rd_device_state =
00189                     VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
00190             break;
00191 
00192         default:
00193             pstate->rd_device_state  =
00194                 VL53L1_DEVICESTATE_SW_STANDBY;
00195             pstate->rd_stream_count  = 0;
00196             pstate->rd_internal_stream_count = 0;
00197             pstate->rd_internal_stream_count_val = 0;
00198             pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00199             pstate->rd_timing_status = 0;
00200             pstate->rd_zone_id       = 0;
00201             break;
00202         }
00203     }
00204 
00205 
00206 
00207     LOG_FUNCTION_END(status);
00208 
00209     return status;
00210 }
00211 
00212 
00213 VL53L1_Error VL53L1_check_ll_driver_rd_state(
00214     VL53L1_DEV         Dev)
00215 {
00216 
00217 
00218     VL53L1_Error         status = VL53L1_ERROR_NONE;
00219     VL53L1_LLDriverData_t  *pdev =
00220             VL53L1DevStructGetLLDriverHandle(Dev);
00221     VL53L1_LLDriverResults_t  *pres =
00222             VL53L1DevStructGetLLResultsHandle(Dev);
00223 
00224     VL53L1_ll_driver_state_t  *pstate       = &(pdev->ll_state);
00225     VL53L1_system_results_t   *psys_results = &(pdev->sys_results);
00226     VL53L1_histogram_bin_data_t *phist_data = &(pdev->hist_data);
00227     VL53L1_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs);
00228 
00229     uint8_t   device_range_status   = 0;
00230     uint8_t   device_stream_count   = 0;
00231     uint8_t   device_gph_id         = 0;
00232     uint8_t   histogram_mode        = 0;
00233     uint8_t   expected_stream_count = 0;
00234     uint8_t   expected_gph_id       = 0;
00235 
00236     LOG_FUNCTION_START("");
00237 
00238 
00239 
00240     device_range_status =
00241             psys_results->result__range_status &
00242             VL53L1_RANGE_STATUS__RANGE_STATUS_MASK;
00243 
00244     device_stream_count = psys_results->result__stream_count;
00245 
00246 
00247 
00248     histogram_mode =
00249         (pdev->sys_ctrl.system__mode_start &
00250         VL53L1_DEVICESCHEDULERMODE_HISTOGRAM) ==
00251         VL53L1_DEVICESCHEDULERMODE_HISTOGRAM;
00252 
00253 
00254     device_gph_id = (psys_results->result__interrupt_status &
00255         VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
00256 
00257     if (histogram_mode)
00258         device_gph_id = (phist_data->result__interrupt_status &
00259             VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
00260 
00261 
00262 
00263     if (!((pdev->sys_ctrl.system__mode_start &
00264         VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) ==
00265         VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK))
00266         goto ENDFUNC;
00267 
00268 
00269 
00270     if (pstate->rd_device_state ==
00271         VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC) {
00272 
00273         if (histogram_mode == 0) {
00274             if (device_range_status !=
00275             VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY)
00276                 status =
00277                 VL53L1_ERROR_GPH_SYNC_CHECK_FAIL;
00278 
00279         }
00280     } else {
00281         if (pstate->rd_stream_count != device_stream_count)
00282             status = VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL;
00283 
00284 
00285         if (pstate->rd_gph_id != device_gph_id)
00286             status = VL53L1_ERROR_GPH_ID_CHECK_FAIL;
00287 
00288 
00289 
00290 
00291         expected_stream_count =
00292         pZ->VL53L1_p_002[pstate->rd_zone_id].expected_stream_count;
00293         expected_gph_id =
00294         pZ->VL53L1_p_002[pstate->rd_zone_id].expected_gph_id;
00295 
00296 
00297 
00298         if (expected_stream_count != device_stream_count) {
00299 
00300 
00301             if (!((pdev->zone_cfg.active_zones == 0) &&
00302                 (device_stream_count == 255)))
00303                 status =
00304                 VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL;
00305 
00306 
00307         }
00308 
00309 
00310 
00311         if (expected_gph_id != device_gph_id)
00312             status = VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL;
00313 
00314     }
00315 
00316 
00317 
00318 ENDFUNC:
00319     LOG_FUNCTION_END(status);
00320     return status;
00321 }
00322 
00323 
00324 VL53L1_Error  VL53L1_update_ll_driver_cfg_state(
00325     VL53L1_DEV         Dev)
00326 {
00327 
00328 
00329     VL53L1_Error         status = VL53L1_ERROR_NONE;
00330     VL53L1_LLDriverData_t  *pdev =
00331             VL53L1DevStructGetLLDriverHandle(Dev);
00332     VL53L1_LLDriverResults_t  *pres =
00333             VL53L1DevStructGetLLResultsHandle(Dev);
00334 
00335     VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
00336     VL53L1_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs);
00337 
00338     uint8_t prev_cfg_zone_id;
00339     uint8_t prev_cfg_gph_id;
00340     uint8_t prev_cfg_stream_count;
00341 
00342     LOG_FUNCTION_START("");
00343 
00344 
00345 
00346 
00347 
00348     if ((pdev->sys_ctrl.system__mode_start &
00349         VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
00350 
00351         pstate->cfg_device_state  = VL53L1_DEVICESTATE_SW_STANDBY;
00352         pstate->cfg_stream_count  = 0;
00353         pstate->cfg_internal_stream_count = 0;
00354         pstate->cfg_internal_stream_count_val = 0;
00355         pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00356         pstate->cfg_timing_status = 0;
00357         pstate->cfg_zone_id       = 0;
00358         prev_cfg_zone_id          = 0;
00359         prev_cfg_gph_id           = 0;
00360         prev_cfg_stream_count     = 0;
00361 
00362     } else {
00363 
00364         prev_cfg_gph_id           = pstate->cfg_gph_id;
00365         prev_cfg_zone_id          = pstate->cfg_zone_id;
00366         prev_cfg_stream_count     = pstate->cfg_stream_count;
00367 
00368 
00369 
00370         if (pstate->cfg_stream_count == 0xFF)
00371             pstate->cfg_stream_count = 0x80;
00372         else
00373             pstate->cfg_stream_count++;
00374 
00375 
00376         status = VL53L1_update_internal_stream_counters(
00377             Dev,
00378             pstate->cfg_stream_count,
00379             &(pstate->cfg_internal_stream_count),
00380             &(pstate->cfg_internal_stream_count_val));
00381 
00382 
00383 
00384         pstate->cfg_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00385 
00386 
00387 
00388         switch (pstate->cfg_device_state) {
00389 
00390         case VL53L1_DEVICESTATE_SW_STANDBY:
00391             pstate->cfg_zone_id = 1;
00392             if (pstate->cfg_zone_id >
00393                 pdev->zone_cfg.active_zones) {
00394                 pstate->cfg_zone_id = 0;
00395                 pstate->cfg_timing_status ^= 0x01;
00396             }
00397             pstate->cfg_stream_count = 1;
00398 
00399             if (pdev->gen_cfg.global_config__stream_divider == 0) {
00400                 pstate->cfg_internal_stream_count = 1;
00401                 pstate->cfg_internal_stream_count_val = 0;
00402             } else {
00403                 pstate->cfg_internal_stream_count = 0;
00404                 pstate->cfg_internal_stream_count_val = 1;
00405             }
00406             pstate->cfg_device_state =
00407                     VL53L1_DEVICESTATE_RANGING_DSS_AUTO;
00408             break;
00409 
00410         case VL53L1_DEVICESTATE_RANGING_DSS_AUTO:
00411             pstate->cfg_zone_id++;
00412             if (pstate->cfg_zone_id >
00413                 pdev->zone_cfg.active_zones) {
00414 
00415                 pstate->cfg_zone_id = 0;
00416                 pstate->cfg_timing_status ^= 0x01;
00417 
00418 
00419 
00420 
00421                 if (pdev->zone_cfg.active_zones > 0) {
00422                     pstate->cfg_device_state =
00423                     VL53L1_DEVICESTATE_RANGING_DSS_MANUAL;
00424                 }
00425             }
00426             break;
00427 
00428         case VL53L1_DEVICESTATE_RANGING_DSS_MANUAL:
00429             pstate->cfg_zone_id++;
00430             if (pstate->cfg_zone_id >
00431                 pdev->zone_cfg.active_zones) {
00432                 pstate->cfg_zone_id = 0;
00433                 pstate->cfg_timing_status ^= 0x01;
00434             }
00435             break;
00436 
00437         default:
00438             pstate->cfg_device_state =
00439                     VL53L1_DEVICESTATE_SW_STANDBY;
00440             pstate->cfg_stream_count = 0;
00441             pstate->cfg_internal_stream_count = 0;
00442             pstate->cfg_internal_stream_count_val = 0;
00443             pstate->cfg_gph_id =
00444                     VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00445             pstate->cfg_timing_status = 0;
00446             pstate->cfg_zone_id       = 0;
00447             break;
00448         }
00449     }
00450 
00451 
00452     if (pdev->zone_cfg.active_zones == 0) {
00453 
00454         pZ->VL53L1_p_002[prev_cfg_zone_id].expected_stream_count
00455             = prev_cfg_stream_count - 1;
00456 
00457         pZ->VL53L1_p_002[pstate->rd_zone_id].expected_gph_id =
00458             prev_cfg_gph_id ^ VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
00459     } else {
00460         pZ->VL53L1_p_002[prev_cfg_zone_id].expected_stream_count
00461             = prev_cfg_stream_count;
00462         pZ->VL53L1_p_002[prev_cfg_zone_id].expected_gph_id =
00463             prev_cfg_gph_id;
00464     }
00465 
00466 
00467 
00468     LOG_FUNCTION_END(status);
00469 
00470     return status;
00471 }
00472 
00473 
00474 void VL53L1_copy_rtn_good_spads_to_buffer(
00475     VL53L1_nvm_copy_data_t  *pdata,
00476     uint8_t                 *pbuffer)
00477 {
00478 
00479 
00480     *(pbuffer +  0) = pdata->global_config__spad_enables_rtn_0;
00481     *(pbuffer +  1) = pdata->global_config__spad_enables_rtn_1;
00482     *(pbuffer +  2) = pdata->global_config__spad_enables_rtn_2;
00483     *(pbuffer +  3) = pdata->global_config__spad_enables_rtn_3;
00484     *(pbuffer +  4) = pdata->global_config__spad_enables_rtn_4;
00485     *(pbuffer +  5) = pdata->global_config__spad_enables_rtn_5;
00486     *(pbuffer +  6) = pdata->global_config__spad_enables_rtn_6;
00487     *(pbuffer +  7) = pdata->global_config__spad_enables_rtn_7;
00488     *(pbuffer +  8) = pdata->global_config__spad_enables_rtn_8;
00489     *(pbuffer +  9) = pdata->global_config__spad_enables_rtn_9;
00490     *(pbuffer + 10) = pdata->global_config__spad_enables_rtn_10;
00491     *(pbuffer + 11) = pdata->global_config__spad_enables_rtn_11;
00492     *(pbuffer + 12) = pdata->global_config__spad_enables_rtn_12;
00493     *(pbuffer + 13) = pdata->global_config__spad_enables_rtn_13;
00494     *(pbuffer + 14) = pdata->global_config__spad_enables_rtn_14;
00495     *(pbuffer + 15) = pdata->global_config__spad_enables_rtn_15;
00496     *(pbuffer + 16) = pdata->global_config__spad_enables_rtn_16;
00497     *(pbuffer + 17) = pdata->global_config__spad_enables_rtn_17;
00498     *(pbuffer + 18) = pdata->global_config__spad_enables_rtn_18;
00499     *(pbuffer + 19) = pdata->global_config__spad_enables_rtn_19;
00500     *(pbuffer + 20) = pdata->global_config__spad_enables_rtn_20;
00501     *(pbuffer + 21) = pdata->global_config__spad_enables_rtn_21;
00502     *(pbuffer + 22) = pdata->global_config__spad_enables_rtn_22;
00503     *(pbuffer + 23) = pdata->global_config__spad_enables_rtn_23;
00504     *(pbuffer + 24) = pdata->global_config__spad_enables_rtn_24;
00505     *(pbuffer + 25) = pdata->global_config__spad_enables_rtn_25;
00506     *(pbuffer + 26) = pdata->global_config__spad_enables_rtn_26;
00507     *(pbuffer + 27) = pdata->global_config__spad_enables_rtn_27;
00508     *(pbuffer + 28) = pdata->global_config__spad_enables_rtn_28;
00509     *(pbuffer + 29) = pdata->global_config__spad_enables_rtn_29;
00510     *(pbuffer + 30) = pdata->global_config__spad_enables_rtn_30;
00511     *(pbuffer + 31) = pdata->global_config__spad_enables_rtn_31;
00512 }
00513 
00514 
00515 void VL53L1_init_system_results(
00516         VL53L1_system_results_t  *pdata)
00517 {
00518 
00519 
00520     pdata->result__interrupt_status                       = 0xFF;
00521     pdata->result__range_status                           = 0xFF;
00522     pdata->result__report_status                          = 0xFF;
00523     pdata->result__stream_count                           = 0xFF;
00524 
00525     pdata->result__dss_actual_effective_spads_sd0         = 0xFFFF;
00526     pdata->result__peak_signal_count_rate_mcps_sd0        = 0xFFFF;
00527     pdata->result__ambient_count_rate_mcps_sd0            = 0xFFFF;
00528     pdata->result__sigma_sd0                              = 0xFFFF;
00529     pdata->result__phase_sd0                              = 0xFFFF;
00530     pdata->result__final_crosstalk_corrected_range_mm_sd0 = 0xFFFF;
00531     pdata->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 =
00532             0xFFFF;
00533     pdata->result__mm_inner_actual_effective_spads_sd0    = 0xFFFF;
00534     pdata->result__mm_outer_actual_effective_spads_sd0    = 0xFFFF;
00535     pdata->result__avg_signal_count_rate_mcps_sd0         = 0xFFFF;
00536 
00537     pdata->result__dss_actual_effective_spads_sd1         = 0xFFFF;
00538     pdata->result__peak_signal_count_rate_mcps_sd1        = 0xFFFF;
00539     pdata->result__ambient_count_rate_mcps_sd1            = 0xFFFF;
00540     pdata->result__sigma_sd1                              = 0xFFFF;
00541     pdata->result__phase_sd1                              = 0xFFFF;
00542     pdata->result__final_crosstalk_corrected_range_mm_sd1 = 0xFFFF;
00543     pdata->result__spare_0_sd1                            = 0xFFFF;
00544     pdata->result__spare_1_sd1                            = 0xFFFF;
00545     pdata->result__spare_2_sd1                            = 0xFFFF;
00546     pdata->result__spare_3_sd1                            = 0xFF;
00547 
00548 }
00549 
00550 
00551 void V53L1_init_zone_results_structure(
00552     uint8_t                 active_zones,
00553     VL53L1_zone_results_t  *pdata)
00554 {
00555 
00556 
00557 
00558     uint8_t  z = 0;
00559     VL53L1_zone_objects_t *pobjects;
00560 
00561     pdata->max_zones    = VL53L1_MAX_USER_ZONES;
00562     pdata->active_zones = active_zones;
00563 
00564     for (z = 0; z < pdata->max_zones; z++) {
00565         pobjects = &(pdata->VL53L1_p_002[z]);
00566         pobjects->cfg_device_state = VL53L1_DEVICESTATE_SW_STANDBY;
00567         pobjects->rd_device_state  = VL53L1_DEVICESTATE_SW_STANDBY;
00568         pobjects->max_objects      = VL53L1_MAX_RANGE_RESULTS;
00569         pobjects->active_objects   = 0;
00570     }
00571 }
00572 
00573 void V53L1_init_zone_dss_configs(
00574     VL53L1_DEV              Dev)
00575 {
00576 
00577 
00578 
00579     VL53L1_LLDriverResults_t  *pres =
00580             VL53L1DevStructGetLLResultsHandle(Dev);
00581     uint8_t  z = 0;
00582     uint8_t max_zones    = VL53L1_MAX_USER_ZONES;
00583     VL53L1_zone_private_dyn_cfgs_t *pdata = &(pres->zone_dyn_cfgs);
00584 
00585     for (z = 0; z < max_zones; z++) {
00586         pdata->VL53L1_p_002[z].dss_mode =
00587             VL53L1_DSS_CONTROL__MODE_TARGET_RATE;
00588         pdata->VL53L1_p_002[z].dss_requested_effective_spad_count = 0;
00589     }
00590 }
00591 
00592 
00593 void VL53L1_init_histogram_config_structure(
00594     uint8_t   even_bin0,
00595     uint8_t   even_bin1,
00596     uint8_t   even_bin2,
00597     uint8_t   even_bin3,
00598     uint8_t   even_bin4,
00599     uint8_t   even_bin5,
00600     uint8_t   odd_bin0,
00601     uint8_t   odd_bin1,
00602     uint8_t   odd_bin2,
00603     uint8_t   odd_bin3,
00604     uint8_t   odd_bin4,
00605     uint8_t   odd_bin5,
00606     VL53L1_histogram_config_t  *pdata)
00607 {
00608 
00609 
00610     pdata->histogram_config__low_amb_even_bin_0_1  =
00611             (even_bin1 << 4) + even_bin0;
00612     pdata->histogram_config__low_amb_even_bin_2_3  =
00613             (even_bin3 << 4) + even_bin2;
00614     pdata->histogram_config__low_amb_even_bin_4_5  =
00615             (even_bin5 << 4) + even_bin4;
00616 
00617     pdata->histogram_config__low_amb_odd_bin_0_1   =
00618             (odd_bin1 << 4) + odd_bin0;
00619     pdata->histogram_config__low_amb_odd_bin_2_3   =
00620             (odd_bin3 << 4) + odd_bin2;
00621     pdata->histogram_config__low_amb_odd_bin_4_5   =
00622             (odd_bin5 << 4) + odd_bin4;
00623 
00624     pdata->histogram_config__mid_amb_even_bin_0_1  =
00625             pdata->histogram_config__low_amb_even_bin_0_1;
00626     pdata->histogram_config__mid_amb_even_bin_2_3  =
00627             pdata->histogram_config__low_amb_even_bin_2_3;
00628     pdata->histogram_config__mid_amb_even_bin_4_5  =
00629             pdata->histogram_config__low_amb_even_bin_4_5;
00630 
00631     pdata->histogram_config__mid_amb_odd_bin_0_1   =
00632             pdata->histogram_config__low_amb_odd_bin_0_1;
00633     pdata->histogram_config__mid_amb_odd_bin_2     = odd_bin2;
00634     pdata->histogram_config__mid_amb_odd_bin_3_4   =
00635             (odd_bin4 << 4) + odd_bin3;
00636     pdata->histogram_config__mid_amb_odd_bin_5     = odd_bin5;
00637 
00638     pdata->histogram_config__user_bin_offset       = 0x00;
00639 
00640     pdata->histogram_config__high_amb_even_bin_0_1 =
00641             pdata->histogram_config__low_amb_even_bin_0_1;
00642     pdata->histogram_config__high_amb_even_bin_2_3 =
00643             pdata->histogram_config__low_amb_even_bin_2_3;
00644     pdata->histogram_config__high_amb_even_bin_4_5 =
00645             pdata->histogram_config__low_amb_even_bin_4_5;
00646 
00647     pdata->histogram_config__high_amb_odd_bin_0_1  =
00648             pdata->histogram_config__low_amb_odd_bin_0_1;
00649     pdata->histogram_config__high_amb_odd_bin_2_3  =
00650             pdata->histogram_config__low_amb_odd_bin_2_3;
00651     pdata->histogram_config__high_amb_odd_bin_4_5  =
00652             pdata->histogram_config__low_amb_odd_bin_4_5;
00653 
00654 
00655 
00656     pdata->histogram_config__amb_thresh_low        = 0xFFFF;
00657     pdata->histogram_config__amb_thresh_high       = 0xFFFF;
00658 
00659 
00660 
00661     pdata->histogram_config__spad_array_selection  = 0x00;
00662 
00663 }
00664 
00665 void VL53L1_init_histogram_multizone_config_structure(
00666     uint8_t   even_bin0,
00667     uint8_t   even_bin1,
00668     uint8_t   even_bin2,
00669     uint8_t   even_bin3,
00670     uint8_t   even_bin4,
00671     uint8_t   even_bin5,
00672     uint8_t   odd_bin0,
00673     uint8_t   odd_bin1,
00674     uint8_t   odd_bin2,
00675     uint8_t   odd_bin3,
00676     uint8_t   odd_bin4,
00677     uint8_t   odd_bin5,
00678     VL53L1_histogram_config_t  *pdata)
00679 {
00680 
00681 
00682     pdata->histogram_config__low_amb_even_bin_0_1  =
00683             (even_bin1 << 4) + even_bin0;
00684     pdata->histogram_config__low_amb_even_bin_2_3  =
00685             (even_bin3 << 4) + even_bin2;
00686     pdata->histogram_config__low_amb_even_bin_4_5  =
00687             (even_bin5 << 4) + even_bin4;
00688 
00689     pdata->histogram_config__low_amb_odd_bin_0_1   =
00690             pdata->histogram_config__low_amb_even_bin_0_1;
00691     pdata->histogram_config__low_amb_odd_bin_2_3
00692         = pdata->histogram_config__low_amb_even_bin_2_3;
00693     pdata->histogram_config__low_amb_odd_bin_4_5
00694         = pdata->histogram_config__low_amb_even_bin_4_5;
00695 
00696     pdata->histogram_config__mid_amb_even_bin_0_1  =
00697         pdata->histogram_config__low_amb_even_bin_0_1;
00698     pdata->histogram_config__mid_amb_even_bin_2_3
00699         = pdata->histogram_config__low_amb_even_bin_2_3;
00700     pdata->histogram_config__mid_amb_even_bin_4_5
00701         = pdata->histogram_config__low_amb_even_bin_4_5;
00702 
00703     pdata->histogram_config__mid_amb_odd_bin_0_1
00704         = pdata->histogram_config__low_amb_odd_bin_0_1;
00705     pdata->histogram_config__mid_amb_odd_bin_2     = odd_bin2;
00706     pdata->histogram_config__mid_amb_odd_bin_3_4   =
00707             (odd_bin4 << 4) + odd_bin3;
00708     pdata->histogram_config__mid_amb_odd_bin_5     = odd_bin5;
00709 
00710     pdata->histogram_config__user_bin_offset       = 0x00;
00711 
00712     pdata->histogram_config__high_amb_even_bin_0_1 =
00713             (odd_bin1 << 4) + odd_bin0;
00714     pdata->histogram_config__high_amb_even_bin_2_3 =
00715             (odd_bin3 << 4) + odd_bin2;
00716     pdata->histogram_config__high_amb_even_bin_4_5 =
00717             (odd_bin5 << 4) + odd_bin4;
00718 
00719     pdata->histogram_config__high_amb_odd_bin_0_1
00720         = pdata->histogram_config__high_amb_even_bin_0_1;
00721     pdata->histogram_config__high_amb_odd_bin_2_3
00722         = pdata->histogram_config__high_amb_even_bin_2_3;
00723     pdata->histogram_config__high_amb_odd_bin_4_5
00724         = pdata->histogram_config__high_amb_even_bin_4_5;
00725 
00726 
00727 
00728     pdata->histogram_config__amb_thresh_low        = 0xFFFF;
00729     pdata->histogram_config__amb_thresh_high       = 0xFFFF;
00730 
00731 
00732 
00733     pdata->histogram_config__spad_array_selection  = 0x00;
00734 }
00735 
00736 
00737 void VL53L1_init_xtalk_bin_data_struct(
00738     uint32_t                        bin_value,
00739     uint16_t                        VL53L1_p_024,
00740     VL53L1_xtalk_histogram_shape_t *pdata)
00741 {
00742 
00743 
00744 
00745     uint16_t          i = 0;
00746 
00747     pdata->zone_id                   = 0;
00748     pdata->time_stamp                = 0;
00749 
00750     pdata->VL53L1_p_022                 = 0;
00751     pdata->VL53L1_p_023               = VL53L1_XTALK_HISTO_BINS;
00752     pdata->VL53L1_p_024            = (uint8_t)VL53L1_p_024;
00753 
00754     pdata->phasecal_result__reference_phase   = 0;
00755     pdata->phasecal_result__vcsel_start       = 0;
00756     pdata->cal_config__vcsel_start            = 0;
00757 
00758     pdata->vcsel_width                        = 0;
00759     pdata->VL53L1_p_019                = 0;
00760 
00761     pdata->zero_distance_phase                = 0;
00762 
00763     for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++) {
00764         if (i < VL53L1_p_024)
00765             pdata->bin_data[i] = bin_value;
00766         else
00767             pdata->bin_data[i] = 0;
00768     }
00769 }
00770 
00771 
00772 void VL53L1_i2c_encode_uint16_t(
00773     uint16_t    ip_value,
00774     uint16_t    count,
00775     uint8_t    *pbuffer)
00776 {
00777 
00778 
00779     uint16_t   i    = 0;
00780     uint16_t   VL53L1_p_002 = 0;
00781 
00782     VL53L1_p_002 =  ip_value;
00783 
00784     for (i = 0; i < count; i++) {
00785         pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
00786         VL53L1_p_002 = VL53L1_p_002 >> 8;
00787     }
00788 }
00789 
00790 uint16_t VL53L1_i2c_decode_uint16_t(
00791     uint16_t    count,
00792     uint8_t    *pbuffer)
00793 {
00794 
00795 
00796     uint16_t   value = 0x00;
00797 
00798     while (count-- > 0)
00799         value = (value << 8) | (uint16_t)*pbuffer++;
00800 
00801     return value;
00802 }
00803 
00804 
00805 void VL53L1_i2c_encode_int16_t(
00806     int16_t     ip_value,
00807     uint16_t    count,
00808     uint8_t    *pbuffer)
00809 {
00810 
00811 
00812     uint16_t   i    = 0;
00813     int16_t    VL53L1_p_002 = 0;
00814 
00815     VL53L1_p_002 =  ip_value;
00816 
00817     for (i = 0; i < count; i++) {
00818         pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
00819         VL53L1_p_002 = VL53L1_p_002 >> 8;
00820     }
00821 }
00822 
00823 int16_t VL53L1_i2c_decode_int16_t(
00824     uint16_t    count,
00825     uint8_t    *pbuffer)
00826 {
00827 
00828 
00829     int16_t    value = 0x00;
00830 
00831 
00832     if (*pbuffer >= 0x80)
00833         value = 0xFFFF;
00834 
00835     while (count-- > 0)
00836         value = (value << 8) | (int16_t)*pbuffer++;
00837 
00838     return value;
00839 }
00840 
00841 void VL53L1_i2c_encode_uint32_t(
00842     uint32_t    ip_value,
00843     uint16_t    count,
00844     uint8_t    *pbuffer)
00845 {
00846 
00847 
00848     uint16_t   i    = 0;
00849     uint32_t   VL53L1_p_002 = 0;
00850 
00851     VL53L1_p_002 =  ip_value;
00852 
00853     for (i = 0; i < count; i++) {
00854         pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
00855         VL53L1_p_002 = VL53L1_p_002 >> 8;
00856     }
00857 }
00858 
00859 uint32_t VL53L1_i2c_decode_uint32_t(
00860     uint16_t    count,
00861     uint8_t    *pbuffer)
00862 {
00863 
00864 
00865     uint32_t   value = 0x00;
00866 
00867     while (count-- > 0)
00868         value = (value << 8) | (uint32_t)*pbuffer++;
00869 
00870     return value;
00871 }
00872 
00873 
00874 uint32_t VL53L1_i2c_decode_with_mask(
00875     uint16_t    count,
00876     uint8_t    *pbuffer,
00877     uint32_t    bit_mask,
00878     uint32_t    down_shift,
00879     uint32_t    offset)
00880 {
00881 
00882 
00883     uint32_t   value = 0x00;
00884 
00885 
00886     while (count-- > 0)
00887         value = (value << 8) | (uint32_t)*pbuffer++;
00888 
00889 
00890     value =  value & bit_mask;
00891     if (down_shift > 0)
00892         value = value >> down_shift;
00893 
00894 
00895     value = value + offset;
00896 
00897     return value;
00898 }
00899 
00900 
00901 void VL53L1_i2c_encode_int32_t(
00902     int32_t     ip_value,
00903     uint16_t    count,
00904     uint8_t    *pbuffer)
00905 {
00906 
00907 
00908     uint16_t   i    = 0;
00909     int32_t    VL53L1_p_002 = 0;
00910 
00911     VL53L1_p_002 =  ip_value;
00912 
00913     for (i = 0; i < count; i++) {
00914         pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
00915         VL53L1_p_002 = VL53L1_p_002 >> 8;
00916     }
00917 }
00918 
00919 int32_t VL53L1_i2c_decode_int32_t(
00920     uint16_t    count,
00921     uint8_t    *pbuffer)
00922 {
00923 
00924 
00925     int32_t    value = 0x00;
00926 
00927 
00928     if (*pbuffer >= 0x80)
00929         value = 0xFFFFFFFF;
00930 
00931     while (count-- > 0)
00932         value = (value << 8) | (int32_t)*pbuffer++;
00933 
00934     return value;
00935 }
00936 
00937 
00938 VL53L1_Error VL53L1_start_test(
00939     VL53L1_DEV    Dev,
00940     uint8_t       test_mode__ctrl)
00941 {
00942 
00943 
00944     VL53L1_Error status = VL53L1_ERROR_NONE;
00945 
00946     LOG_FUNCTION_START("");
00947 
00948     if (status == VL53L1_ERROR_NONE) {
00949         status = VL53L1_WrByte(
00950                     Dev,
00951                     VL53L1_TEST_MODE__CTRL,
00952                     test_mode__ctrl);
00953     }
00954 
00955     LOG_FUNCTION_END(status);
00956 
00957     return status;
00958 }
00959 
00960 
00961 VL53L1_Error VL53L1_set_firmware_enable_register(
00962     VL53L1_DEV    Dev,
00963     uint8_t       value)
00964 {
00965 
00966 
00967     VL53L1_Error status         = VL53L1_ERROR_NONE;
00968     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
00969 
00970     pdev->sys_ctrl.firmware__enable = value;
00971 
00972     status = VL53L1_WrByte(
00973                 Dev,
00974                 VL53L1_FIRMWARE__ENABLE,
00975                 pdev->sys_ctrl.firmware__enable);
00976 
00977     return status;
00978 }
00979 
00980 VL53L1_Error VL53L1_enable_firmware(
00981     VL53L1_DEV    Dev)
00982 {
00983 
00984 
00985     VL53L1_Error status       = VL53L1_ERROR_NONE;
00986 
00987     LOG_FUNCTION_START("");
00988 
00989     status = VL53L1_set_firmware_enable_register(Dev, 0x01);
00990 
00991     LOG_FUNCTION_END(status);
00992 
00993     return status;
00994 }
00995 
00996 
00997 VL53L1_Error VL53L1_disable_firmware(
00998     VL53L1_DEV    Dev)
00999 {
01000 
01001 
01002     VL53L1_Error status       = VL53L1_ERROR_NONE;
01003 
01004     LOG_FUNCTION_START("");
01005 
01006     status = VL53L1_set_firmware_enable_register(Dev, 0x00);
01007 
01008     LOG_FUNCTION_END(status);
01009 
01010     return status;
01011 }
01012 
01013 
01014 VL53L1_Error VL53L1_set_powerforce_register(
01015     VL53L1_DEV    Dev,
01016     uint8_t       value)
01017 {
01018 
01019 
01020     VL53L1_Error status       = VL53L1_ERROR_NONE;
01021     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
01022 
01023     pdev->sys_ctrl.power_management__go1_power_force = value;
01024 
01025     status = VL53L1_WrByte(
01026             Dev,
01027             VL53L1_POWER_MANAGEMENT__GO1_POWER_FORCE,
01028             pdev->sys_ctrl.power_management__go1_power_force);
01029 
01030     return status;
01031 }
01032 
01033 
01034 VL53L1_Error VL53L1_enable_powerforce(
01035     VL53L1_DEV    Dev)
01036 {
01037 
01038 
01039     VL53L1_Error status       = VL53L1_ERROR_NONE;
01040 
01041     LOG_FUNCTION_START("");
01042 
01043     status = VL53L1_set_powerforce_register(Dev, 0x01);
01044 
01045     LOG_FUNCTION_END(status);
01046 
01047     return status;
01048 }
01049 
01050 
01051 VL53L1_Error VL53L1_disable_powerforce(
01052     VL53L1_DEV    Dev)
01053 {
01054 
01055 
01056     VL53L1_Error status       = VL53L1_ERROR_NONE;
01057 
01058     LOG_FUNCTION_START("");
01059 
01060     status = VL53L1_set_powerforce_register(Dev, 0x00);
01061 
01062     LOG_FUNCTION_END(status);
01063 
01064     return status;
01065 }
01066 
01067 
01068 VL53L1_Error VL53L1_clear_interrupt(
01069     VL53L1_DEV    Dev)
01070 {
01071 
01072 
01073     VL53L1_Error status       = VL53L1_ERROR_NONE;
01074     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
01075 
01076     LOG_FUNCTION_START("");
01077 
01078     pdev->sys_ctrl.system__interrupt_clear = VL53L1_CLEAR_RANGE_INT;
01079 
01080     status = VL53L1_WrByte(
01081                 Dev,
01082                 VL53L1_SYSTEM__INTERRUPT_CLEAR,
01083                 pdev->sys_ctrl.system__interrupt_clear);
01084 
01085     LOG_FUNCTION_END(status);
01086 
01087     return status;
01088 }
01089 
01090 
01091 VL53L1_Error VL53L1_force_shadow_stream_count_to_zero(
01092     VL53L1_DEV    Dev)
01093 {
01094 
01095 
01096     VL53L1_Error status       = VL53L1_ERROR_NONE;
01097 
01098     if (status == VL53L1_ERROR_NONE)
01099         status = VL53L1_disable_firmware(Dev);
01100 
01101     if (status == VL53L1_ERROR_NONE) {
01102         status = VL53L1_WrByte(
01103                 Dev,
01104                 VL53L1_SHADOW_RESULT__STREAM_COUNT,
01105                 0x00);
01106     }
01107 
01108     if (status == VL53L1_ERROR_NONE)
01109         status = VL53L1_enable_firmware(Dev);
01110 
01111     return status;
01112 }
01113 
01114 
01115 uint32_t VL53L1_calc_macro_period_us(
01116     uint16_t  fast_osc_frequency,
01117     uint8_t   VL53L1_p_009)
01118 {
01119 
01120 
01121     uint32_t  pll_period_us        = 0;
01122     uint8_t   VL53L1_p_031   = 0;
01123     uint32_t  macro_period_us      = 0;
01124 
01125     LOG_FUNCTION_START("");
01126 
01127 
01128 
01129     pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency);
01130 
01131 
01132 
01133     VL53L1_p_031 = VL53L1_decode_vcsel_period(VL53L1_p_009);
01134 
01135 
01136 
01137     macro_period_us =
01138             (uint32_t)VL53L1_MACRO_PERIOD_VCSEL_PERIODS *
01139             pll_period_us;
01140     macro_period_us = macro_period_us >> 6;
01141 
01142     macro_period_us = macro_period_us * (uint32_t)VL53L1_p_031;
01143     macro_period_us = macro_period_us >> 6;
01144 
01145 
01146 
01147     LOG_FUNCTION_END(0);
01148 
01149     return macro_period_us;
01150 }
01151 
01152 
01153 uint16_t VL53L1_calc_range_ignore_threshold(
01154     uint32_t central_rate,
01155     int16_t  x_gradient,
01156     int16_t  y_gradient,
01157     uint8_t  rate_mult)
01158 {
01159 
01160 
01161     int32_t    range_ignore_thresh_int  = 0;
01162     uint16_t   range_ignore_thresh_kcps = 0;
01163     int32_t    central_rate_int         = 0;
01164     int16_t    x_gradient_int           = 0;
01165     int16_t    y_gradient_int           = 0;
01166 
01167     LOG_FUNCTION_START("");
01168 
01169 
01170 
01171     central_rate_int = ((int32_t)central_rate * (1 << 4)) / (1000);
01172 
01173     if (x_gradient < 0)
01174         x_gradient_int = x_gradient * -1;
01175 
01176     if (y_gradient < 0)
01177         y_gradient_int = y_gradient * -1;
01178 
01179 
01180 
01181 
01182 
01183     range_ignore_thresh_int = (8 * x_gradient_int * 4) +
01184             (8 * y_gradient_int * 4);
01185 
01186 
01187 
01188     range_ignore_thresh_int = range_ignore_thresh_int / 1000;
01189 
01190 
01191 
01192     range_ignore_thresh_int = range_ignore_thresh_int + central_rate_int;
01193 
01194 
01195 
01196     range_ignore_thresh_int = (int32_t)rate_mult * range_ignore_thresh_int;
01197 
01198     range_ignore_thresh_int = (range_ignore_thresh_int + (1<<4)) / (1<<5);
01199 
01200 
01201 
01202     if (range_ignore_thresh_int > 0xFFFF)
01203         range_ignore_thresh_kcps = 0xFFFF;
01204     else
01205         range_ignore_thresh_kcps = (uint16_t)range_ignore_thresh_int;
01206 
01207 
01208 
01209     LOG_FUNCTION_END(0);
01210 
01211     return range_ignore_thresh_kcps;
01212 }
01213 
01214 
01215 uint32_t VL53L1_calc_timeout_mclks(
01216     uint32_t timeout_us,
01217     uint32_t macro_period_us)
01218 {
01219 
01220 
01221     uint32_t timeout_mclks   = 0;
01222 
01223     LOG_FUNCTION_START("");
01224 
01225     if (macro_period_us == 0)
01226         timeout_mclks = 0;
01227     else
01228         timeout_mclks =
01229             ((timeout_us << 12) + (macro_period_us>>1)) /
01230             macro_period_us;
01231 
01232     LOG_FUNCTION_END(0);
01233 
01234     return timeout_mclks;
01235 }
01236 
01237 
01238 uint16_t VL53L1_calc_encoded_timeout(
01239     uint32_t timeout_us,
01240     uint32_t macro_period_us)
01241 {
01242 
01243 
01244     uint32_t timeout_mclks   = 0;
01245     uint16_t timeout_encoded = 0;
01246 
01247     LOG_FUNCTION_START("");
01248 
01249     timeout_mclks   =
01250         VL53L1_calc_timeout_mclks(timeout_us, macro_period_us);
01251 
01252     timeout_encoded =
01253         VL53L1_encode_timeout(timeout_mclks);
01254 
01255 
01256 
01257     LOG_FUNCTION_END(0);
01258 
01259     return timeout_encoded;
01260 }
01261 
01262 
01263 uint32_t VL53L1_calc_timeout_us(
01264     uint32_t timeout_mclks,
01265     uint32_t macro_period_us)
01266 {
01267 
01268 
01269     uint32_t timeout_us     = 0;
01270     uint64_t tmp            = 0;
01271 
01272     LOG_FUNCTION_START("");
01273 
01274     tmp  = (uint64_t)timeout_mclks * (uint64_t)macro_period_us;
01275     tmp += 0x00800;
01276     tmp  = tmp >> 12;
01277 
01278     timeout_us = (uint32_t)tmp;
01279 
01280 
01281 
01282     LOG_FUNCTION_END(0);
01283 
01284     return timeout_us;
01285 }
01286 
01287 uint32_t VL53L1_calc_crosstalk_plane_offset_with_margin(
01288         uint32_t     plane_offset_kcps,
01289         int16_t      margin_offset_kcps)
01290 {
01291     uint32_t plane_offset_with_margin = 0;
01292     int32_t  plane_offset_kcps_temp   = 0;
01293 
01294     LOG_FUNCTION_START("");
01295 
01296     plane_offset_kcps_temp =
01297         (int32_t)plane_offset_kcps +
01298         (int32_t)margin_offset_kcps;
01299 
01300     if (plane_offset_kcps_temp < 0)
01301         plane_offset_kcps_temp = 0;
01302     else
01303         if (plane_offset_kcps_temp > 0x3FFFF)
01304             plane_offset_kcps_temp = 0x3FFFF;
01305 
01306     plane_offset_with_margin = (uint32_t) plane_offset_kcps_temp;
01307 
01308     LOG_FUNCTION_END(0);
01309 
01310     return plane_offset_with_margin;
01311 
01312 }
01313 
01314 uint32_t VL53L1_calc_decoded_timeout_us(
01315     uint16_t timeout_encoded,
01316     uint32_t macro_period_us)
01317 {
01318 
01319 
01320     uint32_t timeout_mclks  = 0;
01321     uint32_t timeout_us     = 0;
01322 
01323     LOG_FUNCTION_START("");
01324 
01325     timeout_mclks =
01326         VL53L1_decode_timeout(timeout_encoded);
01327 
01328     timeout_us    =
01329         VL53L1_calc_timeout_us(timeout_mclks, macro_period_us);
01330 
01331     LOG_FUNCTION_END(0);
01332 
01333     return timeout_us;
01334 }
01335 
01336 
01337 uint16_t VL53L1_encode_timeout(uint32_t timeout_mclks)
01338 {
01339 
01340 
01341     uint16_t encoded_timeout = 0;
01342     uint32_t ls_byte = 0;
01343     uint16_t ms_byte = 0;
01344 
01345     if (timeout_mclks > 0) {
01346         ls_byte = timeout_mclks - 1;
01347 
01348         while ((ls_byte & 0xFFFFFF00) > 0) {
01349             ls_byte = ls_byte >> 1;
01350             ms_byte++;
01351         }
01352 
01353         encoded_timeout = (ms_byte << 8)
01354                 + (uint16_t) (ls_byte & 0x000000FF);
01355     }
01356 
01357     return encoded_timeout;
01358 }
01359 
01360 
01361 uint32_t VL53L1_decode_timeout(uint16_t encoded_timeout)
01362 {
01363 
01364 
01365     uint32_t timeout_macro_clks = 0;
01366 
01367     timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
01368             << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1;
01369 
01370     return timeout_macro_clks;
01371 }
01372 
01373 
01374 VL53L1_Error VL53L1_calc_timeout_register_values(
01375     uint32_t                 phasecal_config_timeout_us,
01376     uint32_t                 mm_config_timeout_us,
01377     uint32_t                 range_config_timeout_us,
01378     uint16_t                 fast_osc_frequency,
01379     VL53L1_general_config_t *pgeneral,
01380     VL53L1_timing_config_t  *ptiming)
01381 {
01382 
01383 
01384     VL53L1_Error status = VL53L1_ERROR_NONE;
01385 
01386     uint32_t macro_period_us    = 0;
01387     uint32_t timeout_mclks      = 0;
01388     uint16_t timeout_encoded    = 0;
01389 
01390     LOG_FUNCTION_START("");
01391 
01392     if (fast_osc_frequency == 0) {
01393         status = VL53L1_ERROR_DIVISION_BY_ZERO;
01394     } else {
01395 
01396         macro_period_us =
01397                 VL53L1_calc_macro_period_us(
01398                     fast_osc_frequency,
01399                     ptiming->range_config__vcsel_period_a);
01400 
01401 
01402         timeout_mclks =
01403             VL53L1_calc_timeout_mclks(
01404                 phasecal_config_timeout_us,
01405                 macro_period_us);
01406 
01407 
01408         if (timeout_mclks > 0xFF)
01409             timeout_mclks = 0xFF;
01410 
01411         pgeneral->phasecal_config__timeout_macrop =
01412                 (uint8_t)timeout_mclks;
01413 
01414 
01415         timeout_encoded =
01416             VL53L1_calc_encoded_timeout(
01417                 mm_config_timeout_us,
01418                 macro_period_us);
01419 
01420         ptiming->mm_config__timeout_macrop_a_hi =
01421                 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
01422         ptiming->mm_config__timeout_macrop_a_lo =
01423                 (uint8_t) (timeout_encoded & 0x00FF);
01424 
01425 
01426         timeout_encoded =
01427             VL53L1_calc_encoded_timeout(
01428                 range_config_timeout_us,
01429                 macro_period_us);
01430 
01431         ptiming->range_config__timeout_macrop_a_hi =
01432                 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
01433         ptiming->range_config__timeout_macrop_a_lo =
01434                 (uint8_t) (timeout_encoded & 0x00FF);
01435 
01436 
01437         macro_period_us =
01438                 VL53L1_calc_macro_period_us(
01439                     fast_osc_frequency,
01440                     ptiming->range_config__vcsel_period_b);
01441 
01442 
01443         timeout_encoded =
01444                 VL53L1_calc_encoded_timeout(
01445                     mm_config_timeout_us,
01446                     macro_period_us);
01447 
01448         ptiming->mm_config__timeout_macrop_b_hi =
01449                 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
01450         ptiming->mm_config__timeout_macrop_b_lo =
01451                 (uint8_t) (timeout_encoded & 0x00FF);
01452 
01453 
01454         timeout_encoded = VL53L1_calc_encoded_timeout(
01455                             range_config_timeout_us,
01456                             macro_period_us);
01457 
01458         ptiming->range_config__timeout_macrop_b_hi =
01459                 (uint8_t)((timeout_encoded & 0xFF00) >> 8);
01460         ptiming->range_config__timeout_macrop_b_lo =
01461                 (uint8_t) (timeout_encoded & 0x00FF);
01462     }
01463 
01464     LOG_FUNCTION_END(0);
01465 
01466     return status;
01467 
01468 }
01469 
01470 
01471 uint8_t VL53L1_encode_vcsel_period(uint8_t VL53L1_p_031)
01472 {
01473 
01474 
01475     uint8_t vcsel_period_reg = 0;
01476 
01477     vcsel_period_reg = (VL53L1_p_031 >> 1) - 1;
01478 
01479     return vcsel_period_reg;
01480 }
01481 
01482 
01483 uint32_t VL53L1_decode_unsigned_integer(
01484     uint8_t  *pbuffer,
01485     uint8_t   no_of_bytes)
01486 {
01487 
01488 
01489     uint8_t   i = 0;
01490     uint32_t  decoded_value = 0;
01491 
01492     for (i = 0; i < no_of_bytes; i++)
01493         decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i];
01494 
01495     return decoded_value;
01496 }
01497 
01498 
01499 void VL53L1_encode_unsigned_integer(
01500     uint32_t  ip_value,
01501     uint8_t   no_of_bytes,
01502     uint8_t  *pbuffer)
01503 {
01504 
01505 
01506     uint8_t   i    = 0;
01507     uint32_t  VL53L1_p_002 = 0;
01508 
01509     VL53L1_p_002 = ip_value;
01510     for (i = 0; i < no_of_bytes; i++) {
01511         pbuffer[no_of_bytes-i-1] = VL53L1_p_002 & 0x00FF;
01512         VL53L1_p_002 = VL53L1_p_002 >> 8;
01513     }
01514 }
01515 
01516 
01517 VL53L1_Error  VL53L1_hist_copy_and_scale_ambient_info(
01518     VL53L1_zone_hist_info_t       *pidata,
01519     VL53L1_histogram_bin_data_t   *podata)
01520 {
01521 
01522 
01523     VL53L1_Error status = VL53L1_ERROR_NONE;
01524 
01525     int64_t  evts              = 0;
01526     int64_t  tmpi              = 0;
01527     int64_t  tmpo              = 0;
01528 
01529     LOG_FUNCTION_START("");
01530 
01531 
01532     if (pidata->result__dss_actual_effective_spads == 0) {
01533         status = VL53L1_ERROR_DIVISION_BY_ZERO;
01534     } else {
01535         if (pidata->number_of_ambient_bins >  0 &&
01536             podata->number_of_ambient_bins == 0) {
01537 
01538 
01539 
01540             tmpo    = 1 + (int64_t)podata->total_periods_elapsed;
01541             tmpo   *=
01542             (int64_t)podata->result__dss_actual_effective_spads;
01543 
01544             tmpi    = 1 + (int64_t)pidata->total_periods_elapsed;
01545             tmpi   *=
01546             (int64_t)pidata->result__dss_actual_effective_spads;
01547 
01548             evts  = tmpo *
01549                 (int64_t)pidata->ambient_events_sum;
01550             evts += (tmpi/2);
01551 
01552 
01553             if (tmpi != 0)
01554                 evts = do_division_s(evts, tmpi);
01555 
01556             podata->ambient_events_sum = (int32_t)evts;
01557 
01558 
01559 
01560             podata->VL53L1_p_004 =
01561                     podata->ambient_events_sum;
01562             podata->VL53L1_p_004 +=
01563                 ((int32_t)pidata->number_of_ambient_bins / 2);
01564             podata->VL53L1_p_004 /=
01565                 (int32_t)pidata->number_of_ambient_bins;
01566         }
01567     }
01568 
01569     LOG_FUNCTION_END(0);
01570 
01571     return status;
01572 }
01573 
01574 
01575 void  VL53L1_hist_get_bin_sequence_config(
01576     VL53L1_DEV                     Dev,
01577     VL53L1_histogram_bin_data_t   *pdata)
01578 {
01579 
01580 
01581     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
01582 
01583     int32_t amb_thresh_low   = 0;
01584     int32_t amb_thresh_high  = 0;
01585 
01586     uint8_t i = 0;
01587 
01588     LOG_FUNCTION_START("");
01589 
01590 
01591 
01592     amb_thresh_low  = 1024 *
01593         (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_low;
01594     amb_thresh_high = 1024 *
01595         (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_high;
01596 
01597 
01598 
01599     if ((pdev->ll_state.rd_stream_count & 0x01) == 0) {
01600 
01601         pdata->bin_seq[5] =
01602         pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 >> 4;
01603         pdata->bin_seq[4] =
01604         pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 & 0x0F;
01605         pdata->bin_seq[3] =
01606         pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 >> 4;
01607         pdata->bin_seq[2] =
01608         pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 & 0x0F;
01609         pdata->bin_seq[1] =
01610         pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 >> 4;
01611         pdata->bin_seq[0] =
01612         pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 & 0x0F;
01613 
01614         if (pdata->ambient_events_sum > amb_thresh_high) {
01615             pdata->bin_seq[5] =
01616             pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5
01617             >> 4;
01618             pdata->bin_seq[4] =
01619             pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5
01620             & 0x0F;
01621             pdata->bin_seq[3] =
01622             pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3
01623             >> 4;
01624             pdata->bin_seq[2] =
01625             pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3
01626             & 0x0F;
01627             pdata->bin_seq[1] =
01628             pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1
01629             >> 4;
01630             pdata->bin_seq[0] =
01631             pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1
01632             & 0x0F;
01633         }
01634 
01635         if (pdata->ambient_events_sum < amb_thresh_low) {
01636             pdata->bin_seq[5] =
01637             pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5
01638             >> 4;
01639             pdata->bin_seq[4] =
01640             pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5
01641             & 0x0F;
01642             pdata->bin_seq[3] =
01643             pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3
01644             >> 4;
01645             pdata->bin_seq[2] =
01646             pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3
01647             & 0x0F;
01648             pdata->bin_seq[1] =
01649             pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1
01650             >> 4;
01651             pdata->bin_seq[0] =
01652             pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1
01653             & 0x0F;
01654         }
01655 
01656     } else {
01657         pdata->bin_seq[5] =
01658             pdev->hist_cfg.histogram_config__mid_amb_odd_bin_5
01659             & 0x0F;
01660         pdata->bin_seq[4] =
01661             pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4
01662             & 0x0F;
01663         pdata->bin_seq[3] =
01664             pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4
01665             >> 4;
01666         pdata->bin_seq[2] =
01667             pdev->hist_cfg.histogram_config__mid_amb_odd_bin_2 &
01668             0x0F;
01669         pdata->bin_seq[1] =
01670             pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1
01671             >> 4;
01672         pdata->bin_seq[0] =
01673             pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1
01674             & 0x0F;
01675 
01676         if (pdata->ambient_events_sum > amb_thresh_high) {
01677             pdata->bin_seq[5] =
01678             pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5
01679             >> 4;
01680             pdata->bin_seq[4] =
01681             pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5
01682             & 0x0F;
01683             pdata->bin_seq[3] =
01684             pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3
01685             >> 4;
01686             pdata->bin_seq[2] =
01687             pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3
01688             & 0x0F;
01689             pdata->bin_seq[1] =
01690             pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1
01691             >> 4;
01692             pdata->bin_seq[0] =
01693             pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1
01694             & 0x0F;
01695         }
01696 
01697         if (pdata->ambient_events_sum < amb_thresh_low) {
01698             pdata->bin_seq[5] =
01699             pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5
01700             >> 4;
01701             pdata->bin_seq[4] =
01702             pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5
01703             & 0x0F;
01704             pdata->bin_seq[3] =
01705             pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3
01706             >> 4;
01707             pdata->bin_seq[2] =
01708             pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3
01709             & 0x0F;
01710             pdata->bin_seq[1] =
01711             pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1
01712             >> 4;
01713             pdata->bin_seq[0] =
01714             pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1
01715             & 0x0F;
01716         }
01717     }
01718 
01719 
01720 
01721     for (i = 0; i < VL53L1_MAX_BIN_SEQUENCE_LENGTH; i++)
01722         pdata->bin_rep[i] = 1;
01723 
01724     LOG_FUNCTION_END(0);
01725 
01726 }
01727 
01728 
01729 VL53L1_Error  VL53L1_hist_phase_consistency_check(
01730     VL53L1_DEV                   Dev,
01731     VL53L1_zone_hist_info_t     *phist_prev,
01732     VL53L1_zone_objects_t       *prange_prev,
01733     VL53L1_range_results_t      *prange_curr)
01734 {
01735 
01736 
01737 
01738     VL53L1_Error  status = VL53L1_ERROR_NONE;
01739     VL53L1_LLDriverData_t *pdev =
01740         VL53L1DevStructGetLLDriverHandle(Dev);
01741 
01742     uint8_t   lc = 0;
01743     uint8_t   p = 0;
01744 
01745     uint16_t  phase_delta      = 0;
01746     uint16_t  phase_tolerance  = 0;
01747 
01748     int32_t   events_delta     = 0;
01749     int32_t   events_tolerance = 0;
01750 
01751 
01752     uint8_t event_sigma;
01753     uint16_t event_min_spad_count;
01754     uint16_t min_max_tolerance;
01755     uint8_t pht;
01756 
01757     VL53L1_DeviceError  range_status = 0;
01758 
01759     LOG_FUNCTION_START("");
01760 
01761     event_sigma =
01762         pdev->histpostprocess.algo__consistency_check__event_sigma;
01763     event_min_spad_count =
01764     pdev->histpostprocess.algo__consistency_check__event_min_spad_count;
01765     min_max_tolerance =
01766     pdev->histpostprocess.algo__consistency_check__min_max_tolerance;
01767 
01768 
01769     pht = pdev->histpostprocess.algo__consistency_check__phase_tolerance;
01770     phase_tolerance = (uint16_t)pht;
01771     phase_tolerance = phase_tolerance << 8;
01772 
01773 
01774 
01775     if (prange_prev->rd_device_state !=
01776             VL53L1_DEVICESTATE_RANGING_GATHER_DATA &&
01777         prange_prev->rd_device_state !=
01778                 VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA)
01779         return status;
01780 
01781 
01782 
01783     if (phase_tolerance == 0)
01784         return status;
01785 
01786     for (lc = 0; lc < prange_curr->active_results; lc++) {
01787 
01788         if (!((prange_curr->VL53L1_p_002[lc].range_status ==
01789             VL53L1_DEVICEERROR_RANGECOMPLETE) ||
01790             (prange_curr->VL53L1_p_002[lc].range_status ==
01791             VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK)))
01792             continue;
01793 
01794 
01795 
01796 
01797 
01798 
01799         if (prange_prev->active_objects == 0)
01800             prange_curr->VL53L1_p_002[lc].range_status =
01801             VL53L1_DEVICEERROR_PREV_RANGE_NO_TARGETS;
01802         else
01803             prange_curr->VL53L1_p_002[lc].range_status =
01804                 VL53L1_DEVICEERROR_PHASECONSISTENCY;
01805 
01806 
01807 
01808 
01809 
01810         for (p = 0; p < prange_prev->active_objects; p++) {
01811 
01812             if (prange_curr->VL53L1_p_002[lc].VL53L1_p_014 >
01813                 prange_prev->VL53L1_p_002[p].VL53L1_p_014) {
01814                 phase_delta =
01815                 prange_curr->VL53L1_p_002[lc].VL53L1_p_014 -
01816                 prange_prev->VL53L1_p_002[p].VL53L1_p_014;
01817             } else {
01818                 phase_delta =
01819                 prange_prev->VL53L1_p_002[p].VL53L1_p_014 -
01820                 prange_curr->VL53L1_p_002[lc].VL53L1_p_014;
01821             }
01822 
01823             if (phase_delta < phase_tolerance) {
01824 
01825 
01826 
01827 
01828 
01829                 if (status == VL53L1_ERROR_NONE)
01830                     status =
01831                     VL53L1_hist_events_consistency_check(
01832                     event_sigma,
01833                     event_min_spad_count,
01834                     phist_prev,
01835                     &(prange_prev->VL53L1_p_002[p]),
01836                     &(prange_curr->VL53L1_p_002[lc]),
01837                     &events_tolerance,
01838                     &events_delta,
01839                     &range_status);
01840 
01841 
01842 
01843 
01844                 if (status == VL53L1_ERROR_NONE &&
01845                     range_status ==
01846                     VL53L1_DEVICEERROR_RANGECOMPLETE)
01847                     status =
01848                     VL53L1_hist_merged_pulse_check(
01849                     min_max_tolerance,
01850                     &(prange_curr->VL53L1_p_002[lc]),
01851                     &range_status);
01852 
01853                 prange_curr->VL53L1_p_002[lc].range_status =
01854                         range_status;
01855             }
01856         }
01857 
01858     }
01859 
01860     LOG_FUNCTION_END(status);
01861 
01862     return status;
01863 }
01864 
01865 
01866 
01867 VL53L1_Error  VL53L1_hist_events_consistency_check(
01868     uint8_t                      event_sigma,
01869     uint16_t                     min_effective_spad_count,
01870     VL53L1_zone_hist_info_t     *phist_prev,
01871     VL53L1_object_data_t        *prange_prev,
01872     VL53L1_range_data_t         *prange_curr,
01873     int32_t                     *pevents_tolerance,
01874     int32_t                     *pevents_delta,
01875     VL53L1_DeviceError          *prange_status)
01876 {
01877 
01878 
01879 
01880     VL53L1_Error  status = VL53L1_ERROR_NONE;
01881 
01882     int64_t   tmpp                   = 0;
01883     int64_t   tmpc                   = 0;
01884     int64_t   events_scaler          = 0;
01885     int64_t   events_scaler_sq       = 0;
01886     int64_t   c_signal_events        = 0;
01887     int64_t   c_sig_noise_sq         = 0;
01888     int64_t   c_amb_noise_sq         = 0;
01889     int64_t   p_amb_noise_sq         = 0;
01890 
01891     int32_t   p_signal_events        = 0;
01892     uint32_t  noise_sq_sum           = 0;
01893 
01894 
01895 
01896     if (event_sigma == 0) {
01897         *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE;
01898         return status;
01899     }
01900 
01901 
01902 
01903     tmpp  = 1 + (int64_t)phist_prev->total_periods_elapsed;
01904     tmpp *= (int64_t)phist_prev->result__dss_actual_effective_spads;
01905 
01906 
01907 
01908     tmpc  = 1 + (int64_t)prange_curr->total_periods_elapsed;
01909     tmpc *= (int64_t)prange_curr->VL53L1_p_006;
01910 
01911 
01912 
01913     events_scaler  = tmpp * 4096;
01914     events_scaler += (tmpc/2);
01915     if (tmpc != 0)
01916         events_scaler = do_division_s(events_scaler, tmpc);
01917 
01918     events_scaler_sq  = events_scaler * events_scaler;
01919     events_scaler_sq += 2048;
01920     events_scaler_sq /= 4096;
01921 
01922 
01923 
01924     c_signal_events  = (int64_t)prange_curr->VL53L1_p_021;
01925     c_signal_events -= (int64_t)prange_curr->VL53L1_p_020;
01926     c_signal_events *= (int64_t)events_scaler;
01927     c_signal_events += 2048;
01928     c_signal_events /= 4096;
01929 
01930     c_sig_noise_sq  = (int64_t)events_scaler_sq;
01931     c_sig_noise_sq *= (int64_t)prange_curr->VL53L1_p_021;
01932     c_sig_noise_sq += 2048;
01933     c_sig_noise_sq /= 4096;
01934 
01935     c_amb_noise_sq  = (int64_t)events_scaler_sq;
01936     c_amb_noise_sq *= (int64_t)prange_curr->VL53L1_p_020;
01937     c_amb_noise_sq += 2048;
01938     c_amb_noise_sq /= 4096;
01939 
01940 
01941     c_amb_noise_sq += 2;
01942     c_amb_noise_sq /= 4;
01943 
01944 
01945 
01946     p_amb_noise_sq  =
01947         (int64_t)prange_prev->VL53L1_p_020;
01948 
01949 
01950     p_amb_noise_sq += 2;
01951     p_amb_noise_sq /= 4;
01952 
01953     noise_sq_sum =
01954         (uint32_t)prange_prev->VL53L1_p_021 +
01955         (uint32_t)c_sig_noise_sq +
01956         (uint32_t)p_amb_noise_sq +
01957         (uint32_t)c_amb_noise_sq;
01958 
01959     *pevents_tolerance =
01960         (int32_t)VL53L1_isqrt(noise_sq_sum * 16);
01961 
01962     *pevents_tolerance *= (int32_t)event_sigma;
01963     *pevents_tolerance += 32;
01964     *pevents_tolerance /= 64;
01965 
01966     p_signal_events  = (int32_t)prange_prev->VL53L1_p_021;
01967     p_signal_events -= (int32_t)prange_prev->VL53L1_p_020;
01968 
01969     if ((int32_t)c_signal_events > p_signal_events)
01970         *pevents_delta =
01971             (int32_t)c_signal_events - p_signal_events;
01972     else
01973         *pevents_delta =
01974             p_signal_events - (int32_t)c_signal_events;
01975 
01976     if (*pevents_delta > *pevents_tolerance &&
01977         prange_curr->VL53L1_p_006 > min_effective_spad_count)
01978         *prange_status = VL53L1_DEVICEERROR_EVENTCONSISTENCY;
01979     else
01980         *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE;
01981 
01982 
01983 
01984 
01985 
01986     return status;
01987 }
01988 
01989 
01990 
01991 
01992 VL53L1_Error  VL53L1_hist_merged_pulse_check(
01993     int16_t                      min_max_tolerance_mm,
01994     VL53L1_range_data_t         *pdata,
01995     VL53L1_DeviceError          *prange_status)
01996 {
01997 
01998 
01999     VL53L1_Error  status   = VL53L1_ERROR_NONE;
02000     int16_t       delta_mm = 0;
02001 
02002     if (pdata->max_range_mm > pdata->min_range_mm)
02003         delta_mm =
02004             pdata->max_range_mm - pdata->min_range_mm;
02005     else
02006         delta_mm =
02007             pdata->min_range_mm - pdata->max_range_mm;
02008 
02009     if (min_max_tolerance_mm > 0 &&
02010         delta_mm > min_max_tolerance_mm)
02011         *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE;
02012     else
02013         *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE;
02014 
02015     return status;
02016 }
02017 
02018 
02019 
02020 
02021 VL53L1_Error  VL53L1_hist_xmonitor_consistency_check(
02022     VL53L1_DEV                   Dev,
02023     VL53L1_zone_hist_info_t     *phist_prev,
02024     VL53L1_zone_objects_t       *prange_prev,
02025     VL53L1_range_data_t         *prange_curr)
02026 {
02027 
02028 
02029     VL53L1_Error  status = VL53L1_ERROR_NONE;
02030     VL53L1_LLDriverData_t *pdev =
02031         VL53L1DevStructGetLLDriverHandle(Dev);
02032 
02033     int32_t   events_delta     = 0;
02034     int32_t   events_tolerance = 0;
02035     uint8_t event_sigma;
02036     uint16_t min_spad_count;
02037 
02038     event_sigma = pdev->histpostprocess.algo__crosstalk_detect_event_sigma;
02039     min_spad_count =
02040     pdev->histpostprocess.algo__consistency_check__event_min_spad_count;
02041 
02042     if (prange_curr->range_status == VL53L1_DEVICEERROR_RANGECOMPLETE ||
02043         prange_curr->range_status ==
02044             VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ||
02045         prange_curr->range_status ==
02046                 VL53L1_DEVICEERROR_EVENTCONSISTENCY) {
02047 
02048         if (prange_prev->xmonitor.range_status ==
02049                 VL53L1_DEVICEERROR_RANGECOMPLETE ||
02050             prange_prev->xmonitor.range_status ==
02051             VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ||
02052             prange_prev->xmonitor.range_status ==
02053                 VL53L1_DEVICEERROR_EVENTCONSISTENCY) {
02054 
02055             prange_curr->range_status =
02056                     VL53L1_DEVICEERROR_RANGECOMPLETE;
02057 
02058             status =
02059                 VL53L1_hist_events_consistency_check(
02060                     event_sigma,
02061                     min_spad_count,
02062                     phist_prev,
02063                     &(prange_prev->xmonitor),
02064                     prange_curr,
02065                     &events_tolerance,
02066                     &events_delta,
02067                     &(prange_curr->range_status));
02068 
02069         }
02070     }
02071 
02072     return status;
02073 }
02074 
02075 
02076 
02077 
02078 VL53L1_Error  VL53L1_hist_wrap_dmax(
02079     VL53L1_hist_post_process_config_t  *phistpostprocess,
02080     VL53L1_histogram_bin_data_t        *pcurrent,
02081     int16_t                            *pwrap_dmax_mm)
02082 {
02083 
02084 
02085 
02086     VL53L1_Error  status = VL53L1_ERROR_NONE;
02087 
02088     uint32_t  pll_period_mm        = 0;
02089     uint32_t  wrap_dmax_phase      = 0;
02090     uint32_t  range_mm             = 0;
02091 
02092     LOG_FUNCTION_START("");
02093 
02094     *pwrap_dmax_mm = 0;
02095 
02096 
02097     if (pcurrent->VL53L1_p_019 != 0) {
02098 
02099 
02100 
02101         pll_period_mm =
02102             VL53L1_calc_pll_period_mm(
02103                 pcurrent->VL53L1_p_019);
02104 
02105 
02106 
02107         wrap_dmax_phase =
02108             (uint32_t)phistpostprocess->valid_phase_high << 8;
02109 
02110 
02111 
02112         range_mm = wrap_dmax_phase * pll_period_mm;
02113         range_mm = (range_mm + (1<<14)) >> 15;
02114 
02115         *pwrap_dmax_mm = (int16_t)range_mm;
02116     }
02117 
02118     LOG_FUNCTION_END(status);
02119 
02120     return status;
02121 }
02122 
02123 
02124 void VL53L1_hist_combine_mm1_mm2_offsets(
02125     int16_t                               mm1_offset_mm,
02126     int16_t                               mm2_offset_mm,
02127     uint8_t                               encoded_mm_roi_centre,
02128     uint8_t                               encoded_mm_roi_size,
02129     uint8_t                               encoded_zone_centre,
02130     uint8_t                               encoded_zone_size,
02131     VL53L1_additional_offset_cal_data_t  *pcal_data,
02132     uint8_t                              *pgood_spads,
02133     uint16_t                              aperture_attenuation,
02134     int16_t                               *prange_offset_mm)
02135 {
02136 
02137 
02138 
02139     uint16_t max_mm_inner_effective_spads = 0;
02140     uint16_t max_mm_outer_effective_spads = 0;
02141     uint16_t mm_inner_effective_spads     = 0;
02142     uint16_t mm_outer_effective_spads     = 0;
02143 
02144     uint32_t scaled_mm1_peak_rate_mcps    = 0;
02145     uint32_t scaled_mm2_peak_rate_mcps    = 0;
02146 
02147     int32_t tmp0 = 0;
02148     int32_t tmp1 = 0;
02149 
02150 
02151 
02152     VL53L1_calc_mm_effective_spads(
02153         encoded_mm_roi_centre,
02154         encoded_mm_roi_size,
02155         0xC7,
02156         0xFF,
02157         pgood_spads,
02158         aperture_attenuation,
02159         &max_mm_inner_effective_spads,
02160         &max_mm_outer_effective_spads);
02161 
02162     if ((max_mm_inner_effective_spads == 0) ||
02163         (max_mm_outer_effective_spads == 0))
02164         goto FAIL;
02165 
02166 
02167 
02168     VL53L1_calc_mm_effective_spads(
02169         encoded_mm_roi_centre,
02170         encoded_mm_roi_size,
02171         encoded_zone_centre,
02172         encoded_zone_size,
02173         pgood_spads,
02174         aperture_attenuation,
02175         &mm_inner_effective_spads,
02176         &mm_outer_effective_spads);
02177 
02178 
02179 
02180     scaled_mm1_peak_rate_mcps  =
02181     (uint32_t)pcal_data->result__mm_inner_peak_signal_count_rtn_mcps;
02182     scaled_mm1_peak_rate_mcps *= (uint32_t)mm_inner_effective_spads;
02183     scaled_mm1_peak_rate_mcps /= (uint32_t)max_mm_inner_effective_spads;
02184 
02185     scaled_mm2_peak_rate_mcps  =
02186     (uint32_t)pcal_data->result__mm_outer_peak_signal_count_rtn_mcps;
02187     scaled_mm2_peak_rate_mcps *= (uint32_t)mm_outer_effective_spads;
02188     scaled_mm2_peak_rate_mcps /= (uint32_t)max_mm_outer_effective_spads;
02189 
02190 
02191 
02192     tmp0  = ((int32_t)mm1_offset_mm * (int32_t)scaled_mm1_peak_rate_mcps);
02193     tmp0 += ((int32_t)mm2_offset_mm * (int32_t)scaled_mm2_peak_rate_mcps);
02194 
02195     tmp1 =  (int32_t)scaled_mm1_peak_rate_mcps +
02196             (int32_t)scaled_mm2_peak_rate_mcps;
02197 
02198 
02199 
02200     if (tmp1 != 0)
02201         tmp0 = (tmp0 * 4) / tmp1;
02202 FAIL:
02203     *prange_offset_mm = (int16_t)tmp0;
02204 
02205 }
02206 
02207 
02208 VL53L1_Error VL53L1_hist_xtalk_extract_calc_window(
02209     int16_t                             target_distance_mm,
02210     uint16_t                            target_width_oversize,
02211     VL53L1_histogram_bin_data_t        *phist_bins,
02212     VL53L1_hist_xtalk_extract_data_t   *pxtalk_data)
02213 {
02214 
02215 
02216     VL53L1_Error  status = VL53L1_ERROR_NONE;
02217 
02218     LOG_FUNCTION_START("");
02219 
02220 
02221     pxtalk_data->pll_period_mm =
02222         VL53L1_calc_pll_period_mm(phist_bins->VL53L1_p_019);
02223 
02224 
02225     pxtalk_data->xtalk_width_phase =
02226         (int32_t)phist_bins->vcsel_width * 128;
02227     pxtalk_data->target_width_phase =
02228         pxtalk_data->xtalk_width_phase +
02229         (int32_t)target_width_oversize * 128;
02230 
02231 
02232 
02233     pxtalk_data->xtalk_start_phase =
02234         (int32_t)phist_bins->zero_distance_phase -
02235         (pxtalk_data->xtalk_width_phase / 2);
02236     pxtalk_data->xtalk_end_phase  =
02237         (int32_t)pxtalk_data->xtalk_start_phase +
02238         pxtalk_data->xtalk_width_phase;
02239 
02240     if (pxtalk_data->xtalk_start_phase < 0)
02241         pxtalk_data->xtalk_start_phase = 0;
02242 
02243 
02244 
02245 
02246     pxtalk_data->VL53L1_p_015 =
02247         (uint8_t)(pxtalk_data->xtalk_start_phase / 2048);
02248 
02249 
02250     pxtalk_data->VL53L1_p_016 =
02251         (uint8_t)((pxtalk_data->xtalk_end_phase + 2047) / 2048);
02252 
02253 
02254 
02255     pxtalk_data->target_start_phase  =
02256             (int32_t)target_distance_mm * 2048 * 16;
02257     pxtalk_data->target_start_phase +=
02258             ((int32_t)pxtalk_data->pll_period_mm / 2);
02259     pxtalk_data->target_start_phase /= (int32_t)pxtalk_data->pll_period_mm;
02260     pxtalk_data->target_start_phase +=
02261             (int32_t)phist_bins->zero_distance_phase;
02262 
02263 
02264 
02265     pxtalk_data->target_start_phase -=
02266             (pxtalk_data->target_width_phase / 2);
02267     pxtalk_data->target_end_phase  =
02268         (int32_t)pxtalk_data->target_start_phase +
02269         pxtalk_data->target_width_phase;
02270 
02271     if (pxtalk_data->target_start_phase < 0)
02272         pxtalk_data->target_start_phase = 0;
02273 
02274 
02275     pxtalk_data->target_start =
02276         (uint8_t)(pxtalk_data->target_start_phase / 2048);
02277 
02278 
02279     if (pxtalk_data->VL53L1_p_016 > (pxtalk_data->target_start-1))
02280         pxtalk_data->VL53L1_p_016 = pxtalk_data->target_start-1;
02281 
02282 
02283     pxtalk_data->effective_width =
02284             (2048 * ((int32_t)pxtalk_data->VL53L1_p_016+1));
02285     pxtalk_data->effective_width -= pxtalk_data->xtalk_start_phase;
02286 
02287 
02288     if (pxtalk_data->effective_width > pxtalk_data->xtalk_width_phase)
02289         pxtalk_data->effective_width = pxtalk_data->xtalk_width_phase;
02290 
02291     if (pxtalk_data->effective_width < 1)
02292         pxtalk_data->effective_width = 1;
02293 
02294 
02295     pxtalk_data->event_scaler  =  pxtalk_data->xtalk_width_phase * 1000;
02296     pxtalk_data->event_scaler +=  (pxtalk_data->effective_width / 2);
02297     pxtalk_data->event_scaler /=  pxtalk_data->effective_width;
02298 
02299 
02300     if (pxtalk_data->event_scaler < 1000)
02301         pxtalk_data->event_scaler = 1000;
02302 
02303     if (pxtalk_data->event_scaler > 4000)
02304         pxtalk_data->event_scaler = 4000;
02305 
02306 
02307     pxtalk_data->event_scaler_sum += pxtalk_data->event_scaler;
02308 
02309 
02310     pxtalk_data->peak_duration_us_sum +=
02311         (uint32_t)phist_bins->peak_duration_us;
02312 
02313 
02314     pxtalk_data->effective_spad_count_sum +=
02315         (uint32_t)phist_bins->result__dss_actual_effective_spads;
02316 
02317 
02318     pxtalk_data->zero_distance_phase_sum +=
02319         (uint32_t)phist_bins->zero_distance_phase;
02320 
02321     LOG_FUNCTION_END(status);
02322 
02323     return status;
02324 }
02325 
02326 
02327 VL53L1_Error VL53L1_hist_xtalk_extract_calc_event_sums(
02328     VL53L1_histogram_bin_data_t        *phist_bins,
02329     VL53L1_hist_xtalk_extract_data_t   *pxtalk_data)
02330 {
02331 
02332 
02333 
02334     VL53L1_Error  status = VL53L1_ERROR_NONE;
02335 
02336     uint8_t   lb = 0;
02337     uint8_t   i = 0;
02338 
02339     LOG_FUNCTION_START("");
02340 
02341 
02342 
02343     for (lb  = pxtalk_data->VL53L1_p_015;
02344          lb <= pxtalk_data->VL53L1_p_016;
02345          lb++) {
02346 
02347 
02348         i = (lb + phist_bins->number_of_ambient_bins +
02349             phist_bins->VL53L1_p_024) %
02350                     phist_bins->VL53L1_p_024;
02351 
02352 
02353         pxtalk_data->signal_events_sum += phist_bins->bin_data[i];
02354         pxtalk_data->signal_events_sum -=
02355                 phist_bins->VL53L1_p_004;
02356     }
02357 
02358 
02359 
02360     for (lb  = 0; lb < VL53L1_XTALK_HISTO_BINS  &&
02361             lb < phist_bins->VL53L1_p_024; lb++) {
02362 
02363 
02364         i = (lb + phist_bins->number_of_ambient_bins +
02365             phist_bins->VL53L1_p_024) %
02366                     phist_bins->VL53L1_p_024;
02367 
02368 
02369         pxtalk_data->bin_data_sums[lb] += phist_bins->bin_data[i];
02370         pxtalk_data->bin_data_sums[lb] -=
02371                 phist_bins->VL53L1_p_004;
02372     }
02373 
02374     pxtalk_data->sample_count += 1;
02375 
02376     LOG_FUNCTION_END(status);
02377 
02378     return status;
02379 }
02380 
02381 
02382 VL53L1_Error VL53L1_hist_xtalk_extract_calc_rate_per_spad(
02383     VL53L1_hist_xtalk_extract_data_t   *pxtalk_data)
02384 {
02385 
02386 
02387 
02388     VL53L1_Error  status = VL53L1_ERROR_NONE;
02389 
02390     uint64_t tmp64_0        = 0;
02391     uint64_t tmp64_1        = 0;
02392     uint64_t xtalk_per_spad = 0;
02393 
02394     LOG_FUNCTION_START("");
02395 
02396 
02397 
02398 
02399 
02400 
02401 
02402 
02403     tmp64_1 =
02404         (uint64_t)pxtalk_data->effective_spad_count_sum *
02405         (uint64_t)pxtalk_data->peak_duration_us_sum;
02406 
02407     if (pxtalk_data->signal_events_sum < 0) {
02408         pxtalk_data->signal_events_sum = 0;
02409 
02410         tmp64_0 =
02411         ((uint64_t)pxtalk_data->sample_count *
02412          (uint64_t)pxtalk_data->event_scaler_avg * 256U) << 9U;
02413         if (tmp64_0 > 0) {
02414             pxtalk_data->signal_events_sum = (int32_t)
02415             do_division_u((50U * tmp64_1), tmp64_0);
02416         }
02417     }
02418     tmp64_0 =
02419         ((uint64_t)pxtalk_data->signal_events_sum *
02420          (uint64_t)pxtalk_data->sample_count *
02421          (uint64_t)pxtalk_data->event_scaler_avg * 256U) << 9U;
02422 
02423 
02424 
02425     if (tmp64_1 > 0U) {
02426 
02427         tmp64_0 = tmp64_0 + (tmp64_1 >> 1U);
02428         xtalk_per_spad = do_division_u(tmp64_0, tmp64_1);
02429     } else {
02430         xtalk_per_spad = (uint64_t)tmp64_0;
02431     }
02432 
02433     pxtalk_data->xtalk_rate_kcps_per_spad = (uint32_t)xtalk_per_spad;
02434 
02435     LOG_FUNCTION_END(status);
02436 
02437     return status;
02438 }
02439 
02440 
02441 VL53L1_Error VL53L1_hist_xtalk_extract_calc_shape(
02442     VL53L1_hist_xtalk_extract_data_t  *pxtalk_data,
02443     VL53L1_xtalk_histogram_shape_t    *pxtalk_shape)
02444 {
02445 
02446 
02447     VL53L1_Error  status = VL53L1_ERROR_NONE;
02448 
02449     int32_t  lb = 0;
02450     uint64_t total_events    = 0U;
02451     uint64_t tmp64_0         = 0U;
02452     int32_t  remaining_area  = 1024;
02453 
02454     LOG_FUNCTION_START("");
02455 
02456 
02457 
02458     pxtalk_shape->VL53L1_p_022      = 0;
02459     pxtalk_shape->VL53L1_p_023    = VL53L1_XTALK_HISTO_BINS;
02460     pxtalk_shape->VL53L1_p_024 = VL53L1_XTALK_HISTO_BINS;
02461 
02462     pxtalk_shape->zero_distance_phase =
02463         (uint16_t)pxtalk_data->zero_distance_phase_avg;
02464     pxtalk_shape->phasecal_result__reference_phase =
02465         (uint16_t)pxtalk_data->zero_distance_phase_avg + (3*2048);
02466 
02467 
02468 
02469     if (pxtalk_data->signal_events_sum > 0)
02470         total_events =
02471             (uint64_t)pxtalk_data->signal_events_sum *
02472             (uint64_t)pxtalk_data->event_scaler_avg;
02473     else
02474         total_events = 1;
02475     if (total_events == 0)
02476         total_events = 1;
02477 
02478 
02479     remaining_area  = 1024;
02480     pxtalk_data->max_shape_value = 0;
02481 
02482     for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS; lb++) {
02483 
02484         if ((lb < (int32_t)pxtalk_data->VL53L1_p_015 ||
02485              lb > (int32_t)pxtalk_data->VL53L1_p_016)  ||
02486                 pxtalk_data->bin_data_sums[lb] < 0) {
02487 
02488 
02489             if (remaining_area > 0 && remaining_area < 1024) {
02490                 if (remaining_area >
02491                     pxtalk_data->max_shape_value) {
02492                     pxtalk_shape->bin_data[lb] =
02493                     (uint32_t)pxtalk_data->max_shape_value;
02494                     remaining_area -=
02495                         pxtalk_data->max_shape_value;
02496                 } else {
02497                     pxtalk_shape->bin_data[lb] =
02498                         (uint32_t)remaining_area;
02499                     remaining_area = 0;
02500                 }
02501             } else {
02502                 pxtalk_shape->bin_data[lb] = 0;
02503             }
02504 
02505         } else {
02506 
02507             tmp64_0 =
02508                 (uint64_t)pxtalk_data->bin_data_sums[lb]
02509                             * 1024U * 1000U;
02510             tmp64_0 += (total_events >> 1);
02511             tmp64_0 = do_division_u(tmp64_0, total_events);
02512             if (tmp64_0 > 0xFFFFU)
02513                 tmp64_0 = 0xFFFFU;
02514 
02515             pxtalk_shape->bin_data[lb] = (uint32_t)tmp64_0;
02516 
02517 
02518             if ((int32_t)pxtalk_shape->bin_data[lb] >
02519                 pxtalk_data->max_shape_value)
02520                 pxtalk_data->max_shape_value =
02521                     (int32_t)pxtalk_shape->bin_data[lb];
02522 
02523             remaining_area -= (int32_t)pxtalk_shape->bin_data[lb];
02524         }
02525     }
02526 
02527     LOG_FUNCTION_END(status);
02528 
02529     return status;
02530 }
02531 
02532 
02533 VL53L1_Error VL53L1_hist_xtalk_shape_model(
02534     uint16_t                         events_per_bin,
02535     uint16_t                         pulse_centre,
02536     uint16_t                         pulse_width,
02537     VL53L1_xtalk_histogram_shape_t  *pxtalk_shape)
02538 {
02539 
02540 
02541     VL53L1_Error  status = VL53L1_ERROR_NONE;
02542 
02543     uint32_t phase_start  = 0;
02544     uint32_t phase_stop   = 0;
02545     uint32_t phase_bin    = 0;
02546 
02547     uint32_t bin_start    = 0;
02548     uint32_t bin_stop     = 0;
02549 
02550     uint32_t  lb           = 0;
02551     uint16_t  VL53L1_p_008      = 0;
02552 
02553     LOG_FUNCTION_START("");
02554 
02555 
02556 
02557     pxtalk_shape->VL53L1_p_022      = 0;
02558     pxtalk_shape->VL53L1_p_023    = VL53L1_XTALK_HISTO_BINS;
02559     pxtalk_shape->VL53L1_p_024 = VL53L1_XTALK_HISTO_BINS;
02560 
02561     pxtalk_shape->zero_distance_phase              = pulse_centre;
02562     pxtalk_shape->phasecal_result__reference_phase =
02563             pulse_centre + (3*2048);
02564 
02565 
02566     if (pulse_centre > (pulse_width >> 1))
02567         phase_start = (uint32_t)pulse_centre -
02568             ((uint32_t)pulse_width >> 1);
02569     else
02570         phase_start = 0;
02571 
02572     phase_stop = (uint32_t)pulse_centre  +
02573             ((uint32_t)pulse_width >> 1);
02574 
02575 
02576     bin_start = (phase_start / 2048);
02577     bin_stop  = (phase_stop  / 2048);
02578 
02579     for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS; lb++) {
02580         VL53L1_p_008 = 0;
02581 
02582 
02583         if (lb == bin_start && lb == bin_stop) {
02584             VL53L1_p_008 =
02585             VL53L1_hist_xtalk_shape_model_interp(
02586                 events_per_bin,
02587                 phase_stop - phase_start);
02588 
02589         } else if (lb > bin_start && lb < bin_stop) {
02590 
02591 
02592             VL53L1_p_008 = events_per_bin;
02593 
02594         } else if (lb == bin_start) {
02595 
02596 
02597             phase_bin = (lb + 1) * 2048;
02598             VL53L1_p_008 =
02599             VL53L1_hist_xtalk_shape_model_interp(
02600                 events_per_bin,
02601                 (phase_bin - phase_start));
02602 
02603         } else if (lb == bin_stop) {
02604 
02605 
02606             phase_bin = lb * 2048;
02607             VL53L1_p_008 =
02608             VL53L1_hist_xtalk_shape_model_interp(
02609                 events_per_bin,
02610                 (phase_stop - phase_bin));
02611         }
02612 
02613         pxtalk_shape->bin_data[lb] = VL53L1_p_008;
02614     }
02615 
02616     LOG_FUNCTION_END(status);
02617 
02618     return status;
02619 }
02620 
02621 
02622 uint16_t VL53L1_hist_xtalk_shape_model_interp(
02623     uint16_t      events_per_bin,
02624     uint32_t      phase_delta)
02625 {
02626 
02627 
02628     uint32_t  VL53L1_p_008  = 0;
02629 
02630     LOG_FUNCTION_START("");
02631 
02632 
02633     VL53L1_p_008  = (uint32_t)events_per_bin * phase_delta;
02634     VL53L1_p_008 +=  1024;
02635     VL53L1_p_008 /=  2048;
02636 
02637 
02638     if (VL53L1_p_008 > 0xFFFFU)
02639         VL53L1_p_008 = 0xFFFFU;
02640 
02641     LOG_FUNCTION_END(0);
02642 
02643     return (uint16_t)VL53L1_p_008;
02644 }
02645 
02646 
02647 void VL53L1_spad_number_to_byte_bit_index(
02648     uint8_t  spad_number,
02649     uint8_t *pbyte_index,
02650     uint8_t *pbit_index,
02651     uint8_t *pbit_mask)
02652 {
02653 
02654 
02655 
02656     *pbyte_index  = spad_number >> 3;
02657     *pbit_index   = spad_number & 0x07;
02658     *pbit_mask    = 0x01 << *pbit_index;
02659 
02660 }
02661 
02662 
02663 void VL53L1_encode_row_col(
02664     uint8_t  row,
02665     uint8_t  col,
02666     uint8_t *pspad_number)
02667 {
02668 
02669 
02670     if (row > 7)
02671         *pspad_number = 128 + (col << 3) + (15-row);
02672     else
02673         *pspad_number = ((15-col) << 3) + row;
02674 
02675 }
02676 
02677 
02678 void VL53L1_decode_zone_size(
02679     uint8_t  encoded_xy_size,
02680     uint8_t  *pwidth,
02681     uint8_t  *pheight)
02682 {
02683 
02684 
02685 
02686     *pheight = encoded_xy_size >> 4;
02687     *pwidth  = encoded_xy_size & 0x0F;
02688 
02689 }
02690 
02691 
02692 void VL53L1_encode_zone_size(
02693     uint8_t  width,
02694     uint8_t  height,
02695     uint8_t *pencoded_xy_size)
02696 {
02697 
02698 
02699     *pencoded_xy_size = (height << 4) + width;
02700 
02701 }
02702 
02703 
02704 void VL53L1_decode_zone_limits(
02705     uint8_t   encoded_xy_centre,
02706     uint8_t   encoded_xy_size,
02707     int16_t  *px_ll,
02708     int16_t  *py_ll,
02709     int16_t  *px_ur,
02710     int16_t  *py_ur)
02711 {
02712 
02713 
02714 
02715     uint8_t x_centre = 0;
02716     uint8_t y_centre = 0;
02717     uint8_t width    = 0;
02718     uint8_t height   = 0;
02719 
02720 
02721 
02722     VL53L1_decode_row_col(
02723         encoded_xy_centre,
02724         &y_centre,
02725         &x_centre);
02726 
02727     VL53L1_decode_zone_size(
02728         encoded_xy_size,
02729         &width,
02730         &height);
02731 
02732 
02733 
02734     *px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2;
02735     if (*px_ll < 0)
02736         *px_ll = 0;
02737 
02738     *px_ur = *px_ll + (int16_t)width;
02739     if (*px_ur > (VL53L1_SPAD_ARRAY_WIDTH-1))
02740         *px_ur = VL53L1_SPAD_ARRAY_WIDTH-1;
02741 
02742     *py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2;
02743     if (*py_ll < 0)
02744         *py_ll = 0;
02745 
02746     *py_ur = *py_ll + (int16_t)height;
02747     if (*py_ur > (VL53L1_SPAD_ARRAY_HEIGHT-1))
02748         *py_ur = VL53L1_SPAD_ARRAY_HEIGHT-1;
02749 }
02750 
02751 
02752 uint8_t VL53L1_is_aperture_location(
02753     uint8_t row,
02754     uint8_t col)
02755 {
02756 
02757 
02758     uint8_t is_aperture = 0;
02759     uint8_t mod_row     = row % 4;
02760     uint8_t mod_col     = col % 4;
02761 
02762     if (mod_row == 0 && mod_col == 2)
02763         is_aperture = 1;
02764 
02765     if (mod_row == 2 && mod_col == 0)
02766         is_aperture = 1;
02767 
02768     return is_aperture;
02769 }
02770 
02771 
02772 void VL53L1_calc_max_effective_spads(
02773     uint8_t     encoded_zone_centre,
02774     uint8_t     encoded_zone_size,
02775     uint8_t    *pgood_spads,
02776     uint16_t    aperture_attenuation,
02777     uint16_t   *pmax_effective_spads)
02778 {
02779 
02780 
02781 
02782     int16_t   x         = 0;
02783     int16_t   y         = 0;
02784 
02785     int16_t   zone_x_ll = 0;
02786     int16_t   zone_y_ll = 0;
02787     int16_t   zone_x_ur = 0;
02788     int16_t   zone_y_ur = 0;
02789 
02790     uint8_t   spad_number = 0;
02791     uint8_t   byte_index  = 0;
02792     uint8_t   bit_index   = 0;
02793     uint8_t   bit_mask    = 0;
02794 
02795     uint8_t   is_aperture = 0;
02796 
02797 
02798 
02799     VL53L1_decode_zone_limits(
02800         encoded_zone_centre,
02801         encoded_zone_size,
02802         &zone_x_ll,
02803         &zone_y_ll,
02804         &zone_x_ur,
02805         &zone_y_ur);
02806 
02807 
02808 
02809     *pmax_effective_spads = 0;
02810 
02811     for (y = zone_y_ll; y <= zone_y_ur; y++) {
02812         for (x = zone_x_ll; x <= zone_x_ur; x++) {
02813 
02814 
02815 
02816             VL53L1_encode_row_col(
02817                 (uint8_t)y,
02818                 (uint8_t)x,
02819                 &spad_number);
02820 
02821 
02822 
02823             VL53L1_spad_number_to_byte_bit_index(
02824                 spad_number,
02825                 &byte_index,
02826                 &bit_index,
02827                 &bit_mask);
02828 
02829 
02830 
02831             if ((pgood_spads[byte_index] & bit_mask) > 0) {
02832 
02833 
02834                 is_aperture = VL53L1_is_aperture_location(
02835                     (uint8_t)y,
02836                     (uint8_t)x);
02837 
02838                 if (is_aperture > 0)
02839                     *pmax_effective_spads +=
02840                             aperture_attenuation;
02841                 else
02842                     *pmax_effective_spads += 0x0100;
02843 
02844             }
02845         }
02846     }
02847 }
02848 
02849 
02850 void VL53L1_calc_mm_effective_spads(
02851     uint8_t     encoded_mm_roi_centre,
02852     uint8_t     encoded_mm_roi_size,
02853     uint8_t     encoded_zone_centre,
02854     uint8_t     encoded_zone_size,
02855     uint8_t    *pgood_spads,
02856     uint16_t    aperture_attenuation,
02857     uint16_t   *pmm_inner_effective_spads,
02858     uint16_t   *pmm_outer_effective_spads)
02859 {
02860 
02861 
02862 
02863     int16_t   x         = 0;
02864     int16_t   y         = 0;
02865 
02866     int16_t   mm_x_ll   = 0;
02867     int16_t   mm_y_ll   = 0;
02868     int16_t   mm_x_ur   = 0;
02869     int16_t   mm_y_ur   = 0;
02870 
02871     int16_t   zone_x_ll = 0;
02872     int16_t   zone_y_ll = 0;
02873     int16_t   zone_x_ur = 0;
02874     int16_t   zone_y_ur = 0;
02875 
02876     uint8_t   spad_number = 0;
02877     uint8_t   byte_index  = 0;
02878     uint8_t   bit_index   = 0;
02879     uint8_t   bit_mask    = 0;
02880 
02881     uint8_t   is_aperture = 0;
02882     uint16_t  spad_attenuation = 0;
02883 
02884 
02885 
02886     VL53L1_decode_zone_limits(
02887         encoded_mm_roi_centre,
02888         encoded_mm_roi_size,
02889         &mm_x_ll,
02890         &mm_y_ll,
02891         &mm_x_ur,
02892         &mm_y_ur);
02893 
02894     VL53L1_decode_zone_limits(
02895         encoded_zone_centre,
02896         encoded_zone_size,
02897         &zone_x_ll,
02898         &zone_y_ll,
02899         &zone_x_ur,
02900         &zone_y_ur);
02901 
02902 
02903 
02904     *pmm_inner_effective_spads = 0;
02905     *pmm_outer_effective_spads = 0;
02906 
02907     for (y = zone_y_ll; y <= zone_y_ur; y++) {
02908         for (x = zone_x_ll; x <= zone_x_ur; x++) {
02909 
02910 
02911 
02912             VL53L1_encode_row_col(
02913                 (uint8_t)y,
02914                 (uint8_t)x,
02915                 &spad_number);
02916 
02917 
02918 
02919             VL53L1_spad_number_to_byte_bit_index(
02920                 spad_number,
02921                 &byte_index,
02922                 &bit_index,
02923                 &bit_mask);
02924 
02925 
02926 
02927             if ((pgood_spads[byte_index] & bit_mask) > 0) {
02928 
02929 
02930                 is_aperture = VL53L1_is_aperture_location(
02931                     (uint8_t)y,
02932                     (uint8_t)x);
02933 
02934                 if (is_aperture > 0)
02935                     spad_attenuation = aperture_attenuation;
02936                 else
02937                     spad_attenuation = 0x0100;
02938 
02939 
02940 
02941                 if (x >= mm_x_ll && x <= mm_x_ur &&
02942                     y >= mm_y_ll && y <= mm_y_ur)
02943                     *pmm_inner_effective_spads +=
02944                         spad_attenuation;
02945                 else
02946                     *pmm_outer_effective_spads +=
02947                         spad_attenuation;
02948             }
02949         }
02950     }
02951 }
02952 
02953 
02954 void VL53L1_hist_copy_results_to_sys_and_core(
02955     VL53L1_histogram_bin_data_t      *pbins,
02956     VL53L1_range_results_t           *phist,
02957     VL53L1_system_results_t          *psys,
02958     VL53L1_core_results_t            *pcore)
02959 {
02960 
02961 
02962     uint8_t  i = 0;
02963 
02964     VL53L1_range_data_t  *pdata;
02965 
02966     LOG_FUNCTION_START("");
02967 
02968 
02969 
02970     VL53L1_init_system_results(psys);
02971 
02972 
02973 
02974     psys->result__interrupt_status = pbins->result__interrupt_status;
02975     psys->result__range_status     = phist->active_results;
02976     psys->result__report_status    = pbins->result__report_status;
02977     psys->result__stream_count     = pbins->result__stream_count;
02978 
02979     pdata = &(phist->VL53L1_p_002[0]);
02980 
02981     for (i = 0; i < phist->active_results; i++) {
02982 
02983         switch (i) {
02984         case 0:
02985             psys->result__dss_actual_effective_spads_sd0 =
02986                     pdata->VL53L1_p_006;
02987             psys->result__peak_signal_count_rate_mcps_sd0 =
02988                     pdata->peak_signal_count_rate_mcps;
02989             psys->result__avg_signal_count_rate_mcps_sd0 =
02990                     pdata->avg_signal_count_rate_mcps;
02991             psys->result__ambient_count_rate_mcps_sd0 =
02992                     pdata->ambient_count_rate_mcps;
02993 
02994             psys->result__sigma_sd0 = pdata->VL53L1_p_005;
02995             psys->result__phase_sd0 = pdata->VL53L1_p_014;
02996 
02997             psys->result__final_crosstalk_corrected_range_mm_sd0 =
02998                     (uint16_t)pdata->median_range_mm;
02999 
03000             psys->result__phase_sd1  = pdata->zero_distance_phase;
03001 
03002             pcore->result_core__ranging_total_events_sd0 =
03003                     pdata->VL53L1_p_021;
03004             pcore->result_core__signal_total_events_sd0 =
03005                     pdata->VL53L1_p_013;
03006             pcore->result_core__total_periods_elapsed_sd0 =
03007                     pdata->total_periods_elapsed;
03008             pcore->result_core__ambient_window_events_sd0 =
03009                     pdata->VL53L1_p_020;
03010 
03011             break;
03012         case 1:
03013             psys->result__dss_actual_effective_spads_sd1 =
03014                 pdata->VL53L1_p_006;
03015             psys->result__peak_signal_count_rate_mcps_sd1 =
03016                 pdata->peak_signal_count_rate_mcps;
03017             psys->result__ambient_count_rate_mcps_sd1 =
03018                 pdata->ambient_count_rate_mcps;
03019 
03020             psys->result__sigma_sd1 = pdata->VL53L1_p_005;
03021             psys->result__phase_sd1 = pdata->VL53L1_p_014;
03022 
03023             psys->result__final_crosstalk_corrected_range_mm_sd1 =
03024                 (uint16_t)pdata->median_range_mm;
03025 
03026             pcore->result_core__ranging_total_events_sd1 =
03027                 pdata->VL53L1_p_021;
03028             pcore->result_core__signal_total_events_sd1 =
03029                 pdata->VL53L1_p_013;
03030             pcore->result_core__total_periods_elapsed_sd1 =
03031                 pdata->total_periods_elapsed;
03032             pcore->result_core__ambient_window_events_sd1 =
03033                 pdata->VL53L1_p_020;
03034             break;
03035         }
03036 
03037         pdata++;
03038     }
03039 
03040     LOG_FUNCTION_END(0);
03041 
03042 }
03043 
03044 
03045 VL53L1_Error VL53L1_sum_histogram_data(
03046         VL53L1_histogram_bin_data_t *phist_input,
03047         VL53L1_histogram_bin_data_t *phist_output)
03048 {
03049 
03050 
03051     VL53L1_Error status = VL53L1_ERROR_NONE;
03052 
03053     uint8_t i = 0;
03054     uint8_t smallest_bin_num = 0;
03055 
03056     LOG_FUNCTION_START("");
03057 
03058 
03059 
03060     if (status == VL53L1_ERROR_NONE) {
03061         if (phist_output->VL53L1_p_024 >=
03062                 phist_input->VL53L1_p_024)
03063             smallest_bin_num = phist_input->VL53L1_p_024;
03064         else
03065             smallest_bin_num = phist_output->VL53L1_p_024;
03066     }
03067 
03068 
03069 
03070 
03071 
03072     if (status == VL53L1_ERROR_NONE)
03073         for (i = 0; i < smallest_bin_num; i++)
03074 
03075             phist_output->bin_data[i] += phist_input->bin_data[i];
03076 
03077     if (status == VL53L1_ERROR_NONE)
03078         phist_output->VL53L1_p_004 +=
03079             phist_input->VL53L1_p_004;
03080 
03081 
03082     LOG_FUNCTION_END(status);
03083 
03084     return status;
03085 }
03086 
03087 
03088 VL53L1_Error VL53L1_avg_histogram_data(
03089             uint8_t no_of_samples,
03090             VL53L1_histogram_bin_data_t *phist_sum,
03091             VL53L1_histogram_bin_data_t *phist_avg)
03092 {
03093 
03094 
03095     VL53L1_Error status = VL53L1_ERROR_NONE;
03096 
03097     uint8_t i = 0;
03098 
03099     LOG_FUNCTION_START("");
03100 
03101 
03102 
03103 
03104 
03105     if (status == VL53L1_ERROR_NONE) {
03106         for (i = 0; i < phist_sum->VL53L1_p_024; i++) {
03107 
03108 
03109 
03110             if (no_of_samples > 0)
03111                 phist_avg->bin_data[i] =
03112                     phist_sum->bin_data[i] /
03113                     (int32_t)no_of_samples;
03114             else
03115                 phist_avg->bin_data[i] = phist_sum->bin_data[i];
03116         }
03117     }
03118 
03119     if (status == VL53L1_ERROR_NONE) {
03120         if (no_of_samples > 0)
03121             phist_avg->VL53L1_p_004 =
03122                 phist_sum->VL53L1_p_004 /
03123                     (int32_t)no_of_samples;
03124         else
03125             phist_avg->VL53L1_p_004 =
03126                     phist_sum->VL53L1_p_004;
03127     }
03128 
03129     LOG_FUNCTION_END(status);
03130 
03131     return status;
03132 }
03133 
03134 
03135 VL53L1_Error VL53L1_save_cfg_data(
03136     VL53L1_DEV  Dev)
03137 {
03138 
03139 
03140     VL53L1_Error status = VL53L1_ERROR_NONE;
03141 
03142     VL53L1_LLDriverData_t  *pdev =
03143             VL53L1DevStructGetLLDriverHandle(Dev);
03144     VL53L1_LLDriverResults_t  *pres =
03145             VL53L1DevStructGetLLResultsHandle(Dev);
03146 
03147     VL53L1_zone_private_dyn_cfg_t *pzone_dyn_cfg;
03148     VL53L1_dynamic_config_t       *pdynamic = &(pdev->dyn_cfg);
03149 
03150     LOG_FUNCTION_START("");
03151 
03152     pzone_dyn_cfg =
03153         &(pres->zone_dyn_cfgs.VL53L1_p_002[pdev->ll_state.cfg_zone_id]);
03154 
03155     pzone_dyn_cfg->expected_stream_count =
03156             pdev->ll_state.cfg_stream_count;
03157 
03158     pzone_dyn_cfg->expected_gph_id =
03159             pdev->ll_state.cfg_gph_id;
03160 
03161     pzone_dyn_cfg->roi_config__user_roi_centre_spad =
03162         pdynamic->roi_config__user_roi_centre_spad;
03163 
03164     pzone_dyn_cfg->roi_config__user_roi_requested_global_xy_size =
03165         pdynamic->roi_config__user_roi_requested_global_xy_size;
03166 
03167     LOG_FUNCTION_END(status);
03168 
03169     return status;
03170 }
03171 
03172 
03173 VL53L1_Error VL53L1_dynamic_zone_update(
03174     VL53L1_DEV  Dev,
03175     VL53L1_range_results_t *presults)
03176 {
03177 
03178 
03179     VL53L1_Error status = VL53L1_ERROR_NONE;
03180 
03181     VL53L1_LLDriverData_t  *pdev =
03182             VL53L1DevStructGetLLDriverHandle(Dev);
03183     VL53L1_LLDriverResults_t  *pres =
03184             VL53L1DevStructGetLLResultsHandle(Dev);
03185     VL53L1_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs);
03186 
03187     uint8_t   zone_id = pdev->ll_state.rd_zone_id;
03188     uint8_t   i;
03189     uint16_t  max_total_rate_per_spads;
03190     uint16_t  target_rate =
03191         pdev->stat_cfg.dss_config__target_total_rate_mcps;
03192     uint32_t  temp = 0xFFFF;
03193 #ifdef VL53L1_LOG_ENABLE
03194     uint16_t eff_spad_cnt =
03195         pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count;
03196 #endif
03197 
03198     LOG_FUNCTION_START("");
03199 
03200     pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count = 0;
03201 
03202     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03203         "    DYNZONEUPDATE: peak signal count rate mcps:");
03204 
03205     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03206         "%u actual effective spads: %u\n",
03207         presults->VL53L1_p_002[0].peak_signal_count_rate_mcps,
03208         presults->VL53L1_p_002[0].VL53L1_p_006);
03209 
03210     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03211         "    DYNZONEUPDATE: active results: %u\n",
03212         presults->active_results);
03213 
03214     max_total_rate_per_spads =
03215         presults->VL53L1_p_002[0].total_rate_per_spad_mcps;
03216 
03217     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03218         "    DYNZONEUPDATE: max total rate per spad at start: %u\n",
03219         max_total_rate_per_spads);
03220 
03221     for (i = 1; i < presults->active_results; i++) {
03222         trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03223         "    DYNZONEUPDATE: zone total rate per spad: zone_id: %u,",
03224         i);
03225 
03226         trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03227         "total rate per spad: %u\n",
03228         presults->VL53L1_p_002[i].total_rate_per_spad_mcps);
03229 
03230         if (presults->VL53L1_p_002[i].total_rate_per_spad_mcps >
03231             max_total_rate_per_spads)
03232             max_total_rate_per_spads =
03233             presults->VL53L1_p_002[i].total_rate_per_spad_mcps;
03234 
03235     }
03236 
03237     if (max_total_rate_per_spads == 0) {
03238 
03239         temp = 0xFFFF;
03240     } else {
03241 
03242         temp = target_rate << 14;
03243         trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03244             "    DYNZONEUPDATE: 1: temp: %u\n",
03245             temp);
03246 
03247 
03248         temp = temp / max_total_rate_per_spads;
03249 
03250         trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03251             "    DYNZONEUPDATE: 2: temp: %u\n",
03252             temp);
03253 
03254 
03255         if (temp > 0xFFFF)
03256             temp = 0xFFFF;
03257 
03258         trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03259             "    DYNZONEUPDATE: 3: temp: %u\n",
03260             temp);
03261     }
03262 
03263     pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count =
03264             (uint16_t)temp;
03265 
03266     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03267         "    DYNZONEUPDATE: zone_id: %u, target_rate: %u,",
03268         zone_id,
03269         target_rate);
03270 
03271     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03272         "max_total_rate_per_spads: %u, requested_spads: %u\n",
03273         max_total_rate_per_spads,
03274         eff_spad_cnt);
03275 
03276     LOG_FUNCTION_END(status);
03277 
03278     return status;
03279 }
03280 
03281 VL53L1_Error VL53L1_multizone_hist_bins_update(
03282     VL53L1_DEV  Dev)
03283 {
03284 
03285 
03286     VL53L1_Error status = VL53L1_ERROR_NONE;
03287 
03288     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
03289     VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
03290     VL53L1_zone_config_t *pzone_cfg = &(pdev->zone_cfg);
03291     VL53L1_histogram_config_t *phist_cfg = &(pdev->hist_cfg);
03292     VL53L1_histogram_config_t *pmulti_hist =
03293             &(pzone_cfg->multizone_hist_cfg);
03294 
03295     uint8_t   next_range_is_odd_timing = (pstate->cfg_stream_count) % 2;
03296 
03297     LOG_FUNCTION_START("");
03298 
03299 
03300     if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] ==
03301         VL53L1_ZONECONFIG_BINCONFIG__LOWAMB) {
03302         if (!next_range_is_odd_timing) {
03303             trace_print (VL53L1_TRACE_LEVEL_DEBUG,
03304             "   HISTBINCONFIGUPDATE: Setting LOWAMB EVEN timing\n");
03305             phist_cfg->histogram_config__low_amb_even_bin_0_1  =
03306             pmulti_hist->histogram_config__low_amb_even_bin_0_1;
03307             phist_cfg->histogram_config__low_amb_even_bin_2_3  =
03308             pmulti_hist->histogram_config__low_amb_even_bin_2_3;
03309             phist_cfg->histogram_config__low_amb_even_bin_4_5  =
03310             pmulti_hist->histogram_config__low_amb_even_bin_4_5;
03311         }
03312 
03313         if (next_range_is_odd_timing) {
03314             trace_print (VL53L1_TRACE_LEVEL_DEBUG,
03315             "    HISTBINCONFIGUPDATE: Setting LOWAMB ODD timing\n");
03316             phist_cfg->histogram_config__low_amb_odd_bin_0_1  =
03317             pmulti_hist->histogram_config__low_amb_even_bin_0_1;
03318             phist_cfg->histogram_config__low_amb_odd_bin_2_3  =
03319             pmulti_hist->histogram_config__low_amb_even_bin_2_3;
03320             phist_cfg->histogram_config__low_amb_odd_bin_4_5  =
03321             pmulti_hist->histogram_config__low_amb_even_bin_4_5;
03322         }
03323     } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] ==
03324         VL53L1_ZONECONFIG_BINCONFIG__MIDAMB) {
03325         trace_print (VL53L1_TRACE_LEVEL_DEBUG,
03326             "    HISTBINCONFIGUPDATE: Setting MIDAMB timing\n");
03327         if (!next_range_is_odd_timing) {
03328             trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03329             "   HISTBINCONFIGUPDATE: Setting MIDAMB EVEN timing\n");
03330             phist_cfg->histogram_config__low_amb_even_bin_0_1  =
03331             pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
03332             phist_cfg->histogram_config__low_amb_even_bin_2_3  =
03333             pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
03334             phist_cfg->histogram_config__low_amb_even_bin_4_5  =
03335             pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
03336         }
03337 
03338         if (next_range_is_odd_timing) {
03339             trace_print (VL53L1_TRACE_LEVEL_DEBUG,
03340             "    HISTBINCONFIGUPDATE: Setting MIDAMB ODD timing\n");
03341             phist_cfg->histogram_config__low_amb_odd_bin_0_1  =
03342             pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
03343             phist_cfg->histogram_config__low_amb_odd_bin_2_3  =
03344             pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
03345             phist_cfg->histogram_config__low_amb_odd_bin_4_5  =
03346             pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
03347         }
03348     } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] ==
03349             VL53L1_ZONECONFIG_BINCONFIG__HIGHAMB) {
03350         if (!next_range_is_odd_timing) {
03351             trace_print (VL53L1_TRACE_LEVEL_DEBUG,
03352             "    HISTBINCONFIGUPDATE: Setting HIGHAMB EVEN timing\n"
03353                     );
03354             phist_cfg->histogram_config__low_amb_even_bin_0_1  =
03355             pmulti_hist->histogram_config__high_amb_even_bin_0_1;
03356             phist_cfg->histogram_config__low_amb_even_bin_2_3  =
03357             pmulti_hist->histogram_config__high_amb_even_bin_2_3;
03358             phist_cfg->histogram_config__low_amb_even_bin_4_5  =
03359             pmulti_hist->histogram_config__high_amb_even_bin_4_5;
03360         }
03361 
03362         if (next_range_is_odd_timing) {
03363             trace_print (VL53L1_TRACE_LEVEL_DEBUG,
03364             "   HISTBINCONFIGUPDATE: Setting HIGHAMB ODD timing\n");
03365             phist_cfg->histogram_config__low_amb_odd_bin_0_1  =
03366             pmulti_hist->histogram_config__high_amb_even_bin_0_1;
03367             phist_cfg->histogram_config__low_amb_odd_bin_2_3  =
03368             pmulti_hist->histogram_config__high_amb_even_bin_2_3;
03369             phist_cfg->histogram_config__low_amb_odd_bin_4_5  =
03370             pmulti_hist->histogram_config__high_amb_even_bin_4_5;
03371         }
03372     }
03373 
03374 
03375 
03376     if (status == VL53L1_ERROR_NONE) {
03377         VL53L1_copy_hist_bins_to_static_cfg(
03378             phist_cfg,
03379             &(pdev->stat_cfg),
03380             &(pdev->tim_cfg));
03381     }
03382 
03383     LOG_FUNCTION_END(status);
03384 
03385     return status;
03386 }
03387 
03388 
03389 
03390 VL53L1_Error VL53L1_update_internal_stream_counters(
03391     VL53L1_DEV  Dev,
03392     uint8_t     external_stream_count,
03393     uint8_t    *pinternal_stream_count,
03394     uint8_t    *pinternal_stream_count_val)
03395 {
03396 
03397     VL53L1_Error status = VL53L1_ERROR_NONE;
03398     uint8_t stream_divider;
03399 
03400     VL53L1_LLDriverData_t  *pdev =
03401             VL53L1DevStructGetLLDriverHandle(Dev);
03402 
03403     LOG_FUNCTION_START("");
03404 
03405     stream_divider = pdev->gen_cfg.global_config__stream_divider;
03406 
03407     if (stream_divider == 0) {
03408 
03409 
03410         *pinternal_stream_count = external_stream_count;
03411 
03412     } else if (*pinternal_stream_count_val == (stream_divider-1)) {
03413 
03414 
03415         if (*pinternal_stream_count == 0xFF)
03416             *pinternal_stream_count = 0x80;
03417         else
03418             *pinternal_stream_count = *pinternal_stream_count + 1;
03419 
03420 
03421         *pinternal_stream_count_val = 0;
03422 
03423     } else {
03424 
03425 
03426         *pinternal_stream_count_val = *pinternal_stream_count_val + 1;
03427     }
03428 
03429     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03430         "UPDINTSTREAMCOUNT   internal_steam_count:  %d,",
03431         *pinternal_stream_count);
03432 
03433     trace_print(VL53L1_TRACE_LEVEL_DEBUG,
03434         "internal_stream_count_val: %d, divider: %d\n",
03435         *pinternal_stream_count_val,
03436         stream_divider);
03437 
03438     LOG_FUNCTION_END(status);
03439 
03440     return status;
03441 }
03442 
03443 
03444 
03445 VL53L1_Error VL53L1_set_histogram_multizone_initial_bin_config(
03446     VL53L1_zone_config_t        *pzone_cfg,
03447     VL53L1_histogram_config_t   *phist_cfg,
03448     VL53L1_histogram_config_t   *pmulti_hist)
03449 {
03450     VL53L1_Error  status = VL53L1_ERROR_NONE;
03451 
03452     LOG_FUNCTION_START("");
03453 
03454 
03455     if (pzone_cfg->bin_config[0] ==
03456             VL53L1_ZONECONFIG_BINCONFIG__LOWAMB) {
03457         phist_cfg->histogram_config__low_amb_even_bin_0_1  =
03458             pmulti_hist->histogram_config__low_amb_even_bin_0_1;
03459         phist_cfg->histogram_config__low_amb_even_bin_2_3  =
03460             pmulti_hist->histogram_config__low_amb_even_bin_2_3;
03461         phist_cfg->histogram_config__low_amb_even_bin_4_5  =
03462             pmulti_hist->histogram_config__low_amb_even_bin_4_5;
03463 
03464         phist_cfg->histogram_config__low_amb_odd_bin_0_1  =
03465             pmulti_hist->histogram_config__low_amb_even_bin_0_1;
03466         phist_cfg->histogram_config__low_amb_odd_bin_2_3  =
03467             pmulti_hist->histogram_config__low_amb_even_bin_2_3;
03468         phist_cfg->histogram_config__low_amb_odd_bin_4_5  =
03469             pmulti_hist->histogram_config__low_amb_even_bin_4_5;
03470     } else if (pzone_cfg->bin_config[0] ==
03471             VL53L1_ZONECONFIG_BINCONFIG__MIDAMB) {
03472         phist_cfg->histogram_config__low_amb_even_bin_0_1  =
03473             pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
03474         phist_cfg->histogram_config__low_amb_even_bin_2_3  =
03475             pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
03476         phist_cfg->histogram_config__low_amb_even_bin_4_5  =
03477             pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
03478 
03479         phist_cfg->histogram_config__low_amb_odd_bin_0_1  =
03480             pmulti_hist->histogram_config__mid_amb_even_bin_0_1;
03481         phist_cfg->histogram_config__low_amb_odd_bin_2_3  =
03482             pmulti_hist->histogram_config__mid_amb_even_bin_2_3;
03483         phist_cfg->histogram_config__low_amb_odd_bin_4_5  =
03484             pmulti_hist->histogram_config__mid_amb_even_bin_4_5;
03485     } else if (pzone_cfg->bin_config[0] ==
03486             VL53L1_ZONECONFIG_BINCONFIG__HIGHAMB) {
03487         phist_cfg->histogram_config__low_amb_even_bin_0_1  =
03488             pmulti_hist->histogram_config__high_amb_even_bin_0_1;
03489         phist_cfg->histogram_config__low_amb_even_bin_2_3  =
03490             pmulti_hist->histogram_config__high_amb_even_bin_2_3;
03491         phist_cfg->histogram_config__low_amb_even_bin_4_5  =
03492             pmulti_hist->histogram_config__high_amb_even_bin_4_5;
03493         phist_cfg->histogram_config__low_amb_odd_bin_0_1  =
03494             pmulti_hist->histogram_config__high_amb_even_bin_0_1;
03495         phist_cfg->histogram_config__low_amb_odd_bin_2_3  =
03496             pmulti_hist->histogram_config__high_amb_even_bin_2_3;
03497         phist_cfg->histogram_config__low_amb_odd_bin_4_5  =
03498             pmulti_hist->histogram_config__high_amb_even_bin_4_5;
03499     }
03500 
03501     LOG_FUNCTION_END(status);
03502     return status;
03503 }
03504 
03505 
03506 
03507 uint8_t VL53L1_encode_GPIO_interrupt_config(
03508     VL53L1_GPIO_interrupt_config_t  *pintconf)
03509 {
03510     uint8_t system__interrupt_config;
03511 
03512     system__interrupt_config = pintconf->intr_mode_distance;
03513     system__interrupt_config |= ((pintconf->intr_mode_rate) << 2);
03514     system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5);
03515     system__interrupt_config |= ((pintconf->intr_no_target) << 6);
03516     system__interrupt_config |= ((pintconf->intr_combined_mode) << 7);
03517 
03518     return system__interrupt_config;
03519 }
03520 
03521 
03522 
03523 VL53L1_GPIO_interrupt_config_t VL53L1_decode_GPIO_interrupt_config(
03524     uint8_t     system__interrupt_config)
03525 {
03526     VL53L1_GPIO_interrupt_config_t  intconf;
03527 
03528     intconf.intr_mode_distance = system__interrupt_config & 0x03;
03529     intconf.intr_mode_rate = (system__interrupt_config >> 2) & 0x03;
03530     intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01;
03531     intconf.intr_no_target = (system__interrupt_config >> 6) & 0x01;
03532     intconf.intr_combined_mode = (system__interrupt_config >> 7) & 0x01;
03533 
03534 
03535     intconf.threshold_rate_low = 0;
03536     intconf.threshold_rate_high = 0;
03537     intconf.threshold_distance_low = 0;
03538     intconf.threshold_distance_high = 0;
03539 
03540     return intconf;
03541 }
03542 
03543 
03544 
03545 VL53L1_Error VL53L1_set_GPIO_distance_threshold(
03546     VL53L1_DEV                      Dev,
03547     uint16_t            threshold_high,
03548     uint16_t            threshold_low)
03549 {
03550     VL53L1_Error  status = VL53L1_ERROR_NONE;
03551 
03552     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
03553 
03554     LOG_FUNCTION_START("");
03555 
03556     pdev->dyn_cfg.system__thresh_high = threshold_high;
03557     pdev->dyn_cfg.system__thresh_low = threshold_low;
03558 
03559     LOG_FUNCTION_END(status);
03560     return status;
03561 }
03562 
03563 
03564 
03565 VL53L1_Error VL53L1_set_GPIO_rate_threshold(
03566     VL53L1_DEV                      Dev,
03567     uint16_t            threshold_high,
03568     uint16_t            threshold_low)
03569 {
03570     VL53L1_Error  status = VL53L1_ERROR_NONE;
03571 
03572     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
03573 
03574     LOG_FUNCTION_START("");
03575 
03576     pdev->gen_cfg.system__thresh_rate_high = threshold_high;
03577     pdev->gen_cfg.system__thresh_rate_low = threshold_low;
03578 
03579     LOG_FUNCTION_END(status);
03580     return status;
03581 }
03582 
03583 
03584 
03585 VL53L1_Error VL53L1_set_GPIO_thresholds_from_struct(
03586     VL53L1_DEV                      Dev,
03587     VL53L1_GPIO_interrupt_config_t *pintconf)
03588 {
03589     VL53L1_Error  status = VL53L1_ERROR_NONE;
03590 
03591     LOG_FUNCTION_START("");
03592 
03593     status = VL53L1_set_GPIO_distance_threshold(
03594             Dev,
03595             pintconf->threshold_distance_high,
03596             pintconf->threshold_distance_low);
03597 
03598     if (status == VL53L1_ERROR_NONE) {
03599         status =
03600             VL53L1_set_GPIO_rate_threshold(
03601                 Dev,
03602                 pintconf->threshold_rate_high,
03603                 pintconf->threshold_rate_low);
03604     }
03605 
03606     LOG_FUNCTION_END(status);
03607     return status;
03608 }
03609 
03610 
03611 VL53L1_Error VL53L1_set_ref_spad_char_config(
03612     VL53L1_DEV    Dev,
03613     uint8_t       vcsel_period_a,
03614     uint32_t      phasecal_timeout_us,
03615     uint16_t      total_rate_target_mcps,
03616     uint16_t      max_count_rate_rtn_limit_mcps,
03617     uint16_t      min_count_rate_rtn_limit_mcps,
03618     uint16_t      fast_osc_frequency)
03619 {
03620 
03621 
03622     VL53L1_Error status = VL53L1_ERROR_NONE;
03623     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
03624 
03625     uint8_t buffer[2];
03626 
03627     uint32_t macro_period_us = 0;
03628     uint32_t timeout_mclks   = 0;
03629 
03630     LOG_FUNCTION_START("");
03631 
03632 
03633     macro_period_us =
03634         VL53L1_calc_macro_period_us(
03635             fast_osc_frequency,
03636             vcsel_period_a);
03637     if (macro_period_us == 0)
03638         macro_period_us = 1;
03639 
03640 
03641 
03642     timeout_mclks = phasecal_timeout_us << 12;
03643     timeout_mclks = timeout_mclks + (macro_period_us>>1);
03644     timeout_mclks = timeout_mclks / macro_period_us;
03645 
03646     if (timeout_mclks > 0xFF)
03647         pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF;
03648     else
03649         pdev->gen_cfg.phasecal_config__timeout_macrop =
03650                 (uint8_t)timeout_mclks;
03651 
03652     pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a;
03653 
03654 
03655 
03656     if (status == VL53L1_ERROR_NONE)
03657         status =
03658             VL53L1_WrByte(
03659                 Dev,
03660                 VL53L1_PHASECAL_CONFIG__TIMEOUT_MACROP,
03661                 pdev->gen_cfg.phasecal_config__timeout_macrop);
03662 
03663     if (status == VL53L1_ERROR_NONE)
03664         status =
03665             VL53L1_WrByte(
03666                 Dev,
03667                 VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A,
03668                 pdev->tim_cfg.range_config__vcsel_period_a);
03669 
03670 
03671 
03672     buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a;
03673     buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a;
03674 
03675     if (status == VL53L1_ERROR_NONE)
03676         status =
03677             VL53L1_WriteMulti(
03678                 Dev,
03679                 VL53L1_SD_CONFIG__WOI_SD0,
03680                 buffer,
03681                 2);
03682 
03683 
03684 
03685     pdev->customer.ref_spad_char__total_rate_target_mcps =
03686             total_rate_target_mcps;
03687 
03688     if (status == VL53L1_ERROR_NONE)
03689         status =
03690             VL53L1_WrWord(
03691                 Dev,
03692                 VL53L1_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS,
03693                 total_rate_target_mcps);
03694 
03695     if (status == VL53L1_ERROR_NONE)
03696         status =
03697             VL53L1_WrWord(
03698                 Dev,
03699                 VL53L1_RANGE_CONFIG__SIGMA_THRESH,
03700                 max_count_rate_rtn_limit_mcps);
03701 
03702     if (status == VL53L1_ERROR_NONE)
03703         status =
03704             VL53L1_WrWord(
03705             Dev,
03706             VL53L1_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,
03707             min_count_rate_rtn_limit_mcps);
03708 
03709     LOG_FUNCTION_END(status);
03710 
03711     return status;
03712 }
03713 
03714 
03715 VL53L1_Error VL53L1_set_ssc_config(
03716     VL53L1_DEV            Dev,
03717     VL53L1_ssc_config_t  *pssc_cfg,
03718     uint16_t              fast_osc_frequency)
03719 {
03720 
03721 
03722     VL53L1_Error status = VL53L1_ERROR_NONE;
03723     uint8_t buffer[5];
03724 
03725     uint32_t macro_period_us = 0;
03726     uint16_t timeout_encoded = 0;
03727 
03728     LOG_FUNCTION_START("");
03729 
03730 
03731     macro_period_us =
03732         VL53L1_calc_macro_period_us(
03733             fast_osc_frequency,
03734             pssc_cfg->VL53L1_p_009);
03735 
03736 
03737     timeout_encoded =
03738         VL53L1_calc_encoded_timeout(
03739             pssc_cfg->timeout_us,
03740             macro_period_us);
03741 
03742 
03743 
03744     if (status == VL53L1_ERROR_NONE)
03745         status =
03746             VL53L1_WrByte(
03747                 Dev,
03748                 VL53L1_CAL_CONFIG__VCSEL_START,
03749                 pssc_cfg->vcsel_start);
03750 
03751     if (status == VL53L1_ERROR_NONE)
03752         status =
03753             VL53L1_WrByte(
03754                 Dev,
03755                 VL53L1_GLOBAL_CONFIG__VCSEL_WIDTH,
03756                 pssc_cfg->vcsel_width);
03757 
03758 
03759 
03760     buffer[0] = (uint8_t)((timeout_encoded &  0x0000FF00) >> 8);
03761     buffer[1] = (uint8_t) (timeout_encoded &  0x000000FF);
03762     buffer[2] = pssc_cfg->VL53L1_p_009;
03763     buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps &  0x0000FF00) >> 8);
03764     buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps &  0x000000FF);
03765 
03766     if (status == VL53L1_ERROR_NONE)
03767         status =
03768             VL53L1_WriteMulti(
03769                 Dev,
03770                 VL53L1_RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
03771                 buffer,
03772                 5);
03773 
03774 
03775 
03776     buffer[0] = pssc_cfg->VL53L1_p_009;
03777     buffer[1] = pssc_cfg->VL53L1_p_009;
03778 
03779     if (status == VL53L1_ERROR_NONE)
03780         status =
03781             VL53L1_WriteMulti(
03782                 Dev,
03783                 VL53L1_SD_CONFIG__WOI_SD0,
03784                 buffer,
03785                 2);
03786 
03787 
03788     if (status == VL53L1_ERROR_NONE)
03789         status =
03790             VL53L1_WrByte(
03791                 Dev,
03792                 VL53L1_NVM_BIST__CTRL,
03793                 pssc_cfg->array_select);
03794 
03795     LOG_FUNCTION_END(status);
03796 
03797     return status;
03798 }
03799 
03800 
03801 VL53L1_Error VL53L1_get_spad_rate_data(
03802     VL53L1_DEV                Dev,
03803     VL53L1_spad_rate_data_t  *pspad_rates)
03804 {
03805 
03806 
03807 
03808     VL53L1_Error status = VL53L1_ERROR_NONE;
03809     int               i = 0;
03810 
03811     uint8_t  VL53L1_p_002[512];
03812     uint8_t *pdata = &VL53L1_p_002[0];
03813 
03814     LOG_FUNCTION_START("");
03815 
03816 
03817 
03818     if (status == VL53L1_ERROR_NONE)
03819         status = VL53L1_disable_firmware(Dev);
03820 
03821 
03822 
03823     if (status == VL53L1_ERROR_NONE)
03824         status =
03825             VL53L1_ReadMulti(
03826                 Dev,
03827                 VL53L1_PRIVATE__PATCH_BASE_ADDR_RSLV,
03828                 pdata,
03829                 512);
03830 
03831 
03832     pdata = &VL53L1_p_002[0];
03833     for (i = 0; i < VL53L1_NO_OF_SPAD_ENABLES; i++) {
03834         pspad_rates->rate_data[i] =
03835             (uint16_t)VL53L1_decode_unsigned_integer(pdata, 2);
03836         pdata += 2;
03837     }
03838 
03839 
03840 
03841     pspad_rates->VL53L1_p_023     = VL53L1_NO_OF_SPAD_ENABLES;
03842     pspad_rates->no_of_values    = VL53L1_NO_OF_SPAD_ENABLES;
03843     pspad_rates->fractional_bits = 15;
03844 
03845 
03846 
03847     if (status == VL53L1_ERROR_NONE)
03848         status = VL53L1_enable_firmware(Dev);
03849 
03850     LOG_FUNCTION_END(status);
03851 
03852     return status;
03853 }
03854 
03855 
03856 
03857 VL53L1_Error VL53L1_dynamic_xtalk_correction_calc_required_samples(
03858     VL53L1_DEV                          Dev
03859     )
03860 {
03861 
03862 
03863 
03864     VL53L1_Error  status = VL53L1_ERROR_NONE;
03865 
03866     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
03867     VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev);
03868     VL53L1_smudge_corrector_config_t *pconfig =
03869                 &(pdev->smudge_correct_config);
03870     VL53L1_smudge_corrector_internals_t *pint =
03871                 &(pdev->smudge_corrector_internals);
03872 
03873     VL53L1_range_results_t *presults = &(pres->range_results);
03874     VL53L1_range_data_t *pxmonitor = &(presults->xmonitor);
03875 
03876     uint32_t peak_duration_us = pxmonitor->peak_duration_us;
03877 
03878     uint64_t temp64a;
03879     uint64_t temp64z;
03880 
03881     LOG_FUNCTION_START("");
03882 
03883     if (peak_duration_us == 0)
03884         peak_duration_us = 1000;
03885 
03886     temp64a = pxmonitor->VL53L1_p_021 +
03887         pxmonitor->VL53L1_p_020;
03888     temp64a = do_division_u((temp64a * 1000), peak_duration_us);
03889     temp64a = do_division_u((temp64a * 1000), peak_duration_us);
03890 
03891     temp64z = pconfig->noise_margin * pxmonitor->VL53L1_p_006;
03892     if (temp64z == 0)
03893         temp64z = 1;
03894     temp64a = temp64a * 1000 * 256;
03895     temp64a = do_division_u(temp64a, temp64z);
03896     temp64a = temp64a * 1000 * 256;
03897     temp64a = do_division_u(temp64a, temp64z);
03898 
03899     pint->required_samples = (uint32_t)temp64a;
03900 
03901 
03902     if (pint->required_samples < 2)
03903         pint->required_samples = 2;
03904 
03905     LOG_FUNCTION_END(status);
03906 
03907     return status;
03908 }
03909 
03910 VL53L1_Error VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
03911     VL53L1_DEV              Dev,
03912     uint32_t                xtalk_offset_out,
03913     VL53L1_smudge_corrector_config_t    *pconfig,
03914     VL53L1_smudge_corrector_data_t      *pout,
03915     uint8_t                 add_smudge,
03916     uint8_t                 soft_update
03917     )
03918 {
03919 
03920 
03921 
03922     VL53L1_Error  status = VL53L1_ERROR_NONE;
03923     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
03924 
03925     int16_t  x_gradient_scaler;
03926     int16_t  y_gradient_scaler;
03927     uint32_t orig_xtalk_offset;
03928     int16_t  orig_x_gradient;
03929     int16_t  orig_y_gradient;
03930     uint8_t histo_merge_nb;
03931     uint8_t i;
03932     int32_t  itemp32;
03933     uint32_t SmudgeFactor;
03934     VL53L1_xtalk_config_t  *pX = &(pdev->xtalk_cfg);
03935     VL53L1_xtalk_calibration_results_t  *pC = &(pdev->xtalk_cal);
03936     uint32_t *pcpo;
03937     uint32_t max, nXtalk, cXtalk;
03938     uint8_t merge_enabled;
03939 
03940 
03941     LOG_FUNCTION_START("");
03942 
03943     merge_enabled = (pdev->tuning_parms.tp_hist_merge == 1) &&
03944         (VL53L1DevDataGet(Dev, CurrentParameters.PresetMode) ==
03945          VL53L1_PRESETMODE_RANGING);
03946 
03947 
03948     if (add_smudge == 1) {
03949         pout->algo__crosstalk_compensation_plane_offset_kcps =
03950             (uint32_t)xtalk_offset_out +
03951             (uint32_t)pconfig->smudge_margin;
03952     } else {
03953         pout->algo__crosstalk_compensation_plane_offset_kcps =
03954             (uint32_t)xtalk_offset_out;
03955     }
03956 
03957 
03958     orig_xtalk_offset =
03959     pX->nvm_default__crosstalk_compensation_plane_offset_kcps;
03960 
03961     orig_x_gradient =
03962         pX->nvm_default__crosstalk_compensation_x_plane_gradient_kcps;
03963 
03964     orig_y_gradient =
03965         pX->nvm_default__crosstalk_compensation_y_plane_gradient_kcps;
03966 
03967     if (((pconfig->user_scaler_set == 0) ||
03968         (pconfig->scaler_calc_method == 1)) &&
03969         (pC->algo__crosstalk_compensation_plane_offset_kcps != 0)) {
03970 
03971         VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb);
03972         if (histo_merge_nb == 0)
03973             histo_merge_nb = 1;
03974         if (!merge_enabled)
03975             orig_xtalk_offset =
03976             pC->algo__crosstalk_compensation_plane_offset_kcps;
03977         else
03978             orig_xtalk_offset =
03979             pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1];
03980 
03981         orig_x_gradient =
03982             pC->algo__crosstalk_compensation_x_plane_gradient_kcps;
03983 
03984         orig_y_gradient =
03985             pC->algo__crosstalk_compensation_y_plane_gradient_kcps;
03986     }
03987 
03988 
03989     if ((pconfig->user_scaler_set == 0) && (orig_x_gradient == 0))
03990         pout->gradient_zero_flag |= 0x01;
03991 
03992     if ((pconfig->user_scaler_set == 0) && (orig_y_gradient == 0))
03993         pout->gradient_zero_flag |= 0x02;
03994 
03995 
03996 
03997     if (orig_xtalk_offset == 0)
03998         orig_xtalk_offset = 1;
03999 
04000 
04001 
04002     if (pconfig->user_scaler_set == 1) {
04003         x_gradient_scaler = pconfig->x_gradient_scaler;
04004         y_gradient_scaler = pconfig->y_gradient_scaler;
04005     } else {
04006 
04007         x_gradient_scaler = (int16_t)do_division_s(
04008                 (((int32_t)orig_x_gradient) << 6),
04009                 orig_xtalk_offset);
04010         pconfig->x_gradient_scaler = x_gradient_scaler;
04011         y_gradient_scaler = (int16_t)do_division_s(
04012                 (((int32_t)orig_y_gradient) << 6),
04013                 orig_xtalk_offset);
04014         pconfig->y_gradient_scaler = y_gradient_scaler;
04015     }
04016 
04017 
04018 
04019     if (pconfig->scaler_calc_method == 0) {
04020 
04021 
04022         itemp32 = (int32_t)(
04023             pout->algo__crosstalk_compensation_plane_offset_kcps *
04024                 x_gradient_scaler);
04025         itemp32 = itemp32 >> 6;
04026         if (itemp32 > 0xFFFF)
04027             itemp32 = 0xFFFF;
04028 
04029         pout->algo__crosstalk_compensation_x_plane_gradient_kcps =
04030             (int16_t)itemp32;
04031 
04032         itemp32 = (int32_t)(
04033             pout->algo__crosstalk_compensation_plane_offset_kcps *
04034                 y_gradient_scaler);
04035         itemp32 = itemp32 >> 6;
04036         if (itemp32 > 0xFFFF)
04037             itemp32 = 0xFFFF;
04038 
04039         pout->algo__crosstalk_compensation_y_plane_gradient_kcps =
04040             (int16_t)itemp32;
04041     } else if (pconfig->scaler_calc_method == 1) {
04042 
04043 
04044         itemp32 = (int32_t)(orig_xtalk_offset -
04045             pout->algo__crosstalk_compensation_plane_offset_kcps);
04046         itemp32 = (int32_t)(do_division_s(itemp32, 16));
04047         itemp32 = itemp32 << 2;
04048         itemp32 = itemp32 + (int32_t)(orig_x_gradient);
04049         if (itemp32 > 0xFFFF)
04050             itemp32 = 0xFFFF;
04051 
04052         pout->algo__crosstalk_compensation_x_plane_gradient_kcps =
04053             (int16_t)itemp32;
04054 
04055         itemp32 = (int32_t)(orig_xtalk_offset -
04056             pout->algo__crosstalk_compensation_plane_offset_kcps);
04057         itemp32 = (int32_t)(do_division_s(itemp32, 80));
04058         itemp32 = itemp32 << 2;
04059         itemp32 = itemp32 + (int32_t)(orig_y_gradient);
04060         if (itemp32 > 0xFFFF)
04061             itemp32 = 0xFFFF;
04062 
04063         pout->algo__crosstalk_compensation_y_plane_gradient_kcps =
04064             (int16_t)itemp32;
04065     }
04066 
04067 
04068     if (pconfig->smudge_corr_apply_enabled == 1 &&
04069             (soft_update != 1)) {
04070         pout->new_xtalk_applied_flag = 1;
04071         nXtalk = pout->algo__crosstalk_compensation_plane_offset_kcps;
04072 
04073         VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb);
04074         max = pdev->tuning_parms.tp_hist_merge_max_size;
04075         pcpo = &(pC->algo__xtalk_cpo_HistoMerge_kcps[0]);
04076         if ((histo_merge_nb > 0) && merge_enabled && (nXtalk != 0)) {
04077             cXtalk =
04078             pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1];
04079             SmudgeFactor = cXtalk * 1000 / nXtalk;
04080             if (SmudgeFactor >= pconfig->max_smudge_factor)
04081                 pout->new_xtalk_applied_flag = 0;
04082             else if (SmudgeFactor > 0)
04083                 for (i = 0; i < max; i++) {
04084                 *pcpo *= 1000;
04085                 *pcpo /= SmudgeFactor;
04086                 pcpo++;
04087                 }
04088         }
04089         if (pout->new_xtalk_applied_flag) {
04090 
04091         pX->algo__crosstalk_compensation_plane_offset_kcps =
04092         pout->algo__crosstalk_compensation_plane_offset_kcps;
04093         pX->algo__crosstalk_compensation_x_plane_gradient_kcps =
04094         pout->algo__crosstalk_compensation_x_plane_gradient_kcps;
04095         pX->algo__crosstalk_compensation_y_plane_gradient_kcps =
04096         pout->algo__crosstalk_compensation_y_plane_gradient_kcps;
04097 
04098         if (pconfig->smudge_corr_single_apply == 1) {
04099 
04100             pconfig->smudge_corr_apply_enabled = 0;
04101             pconfig->smudge_corr_single_apply = 0;
04102         }
04103         }
04104     }
04105 
04106 
04107     if (soft_update != 1)
04108         pout->smudge_corr_valid = 1;
04109 
04110     LOG_FUNCTION_END(status);
04111 
04112     return status;
04113 }
04114 
04115 #define CONT_CONTINUE   0
04116 #define CONT_NEXT_LOOP  1
04117 #define CONT_RESET  2
04118 VL53L1_Error VL53L1_dynamic_xtalk_correction_corrector(
04119     VL53L1_DEV                          Dev
04120     )
04121 {
04122 
04123 
04124 
04125     VL53L1_Error  status = VL53L1_ERROR_NONE;
04126 
04127     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04128     VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev);
04129     VL53L1_smudge_corrector_config_t *pconfig =
04130                 &(pdev->smudge_correct_config);
04131     VL53L1_smudge_corrector_internals_t *pint =
04132                 &(pdev->smudge_corrector_internals);
04133     VL53L1_smudge_corrector_data_t *pout =
04134             &(pres->range_results.smudge_corrector_data);
04135     VL53L1_range_results_t  *pR = &(pres->range_results);
04136     VL53L1_xtalk_config_t  *pX = &(pdev->xtalk_cfg);
04137 
04138     uint8_t run_smudge_detection = 0;
04139     uint8_t merging_complete = 0;
04140     uint8_t run_nodetect = 0;
04141     uint8_t ambient_check = 0;
04142     int32_t itemp32 = 0;
04143     uint64_t utemp64 = 0;
04144     uint8_t continue_processing = CONT_CONTINUE;
04145     uint32_t xtalk_offset_out = 0;
04146     uint32_t xtalk_offset_in = 0;
04147     uint32_t current_xtalk = 0;
04148     uint32_t smudge_margin_adjusted = 0;
04149     uint8_t i = 0;
04150     uint8_t nodetect_index = 0;
04151     uint16_t    amr;
04152     uint32_t    cco;
04153     uint8_t histo_merge_nb;
04154     uint8_t merge_enabled;
04155 
04156 
04157     LOG_FUNCTION_START("");
04158 
04159     merge_enabled = (pdev->tuning_parms.tp_hist_merge == 1) &&
04160         (VL53L1DevDataGet(Dev, CurrentParameters.PresetMode) ==
04161          VL53L1_PRESETMODE_RANGING);
04162 
04163     VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb);
04164     if ((histo_merge_nb == 0) || (!merge_enabled))
04165         histo_merge_nb = 1;
04166 
04167 
04168     VL53L1_dynamic_xtalk_correction_output_init(pres);
04169 
04170 
04171     ambient_check = (pconfig->smudge_corr_ambient_threshold == 0) ||
04172         ((pconfig->smudge_corr_ambient_threshold * histo_merge_nb) >
04173         ((uint32_t)pR->xmonitor.ambient_count_rate_mcps));
04174 
04175 
04176     merging_complete = ((!merge_enabled) ||
04177     (histo_merge_nb == pdev->tuning_parms.tp_hist_merge_max_size));
04178 
04179     run_smudge_detection =
04180         (pconfig->smudge_corr_enabled == 1) &&
04181         ambient_check &&
04182         (pR->xmonitor.range_status
04183             == VL53L1_DEVICEERROR_RANGECOMPLETE) &&
04184         merging_complete;
04185 
04186 
04187     if ((pR->xmonitor.range_status
04188         != VL53L1_DEVICEERROR_RANGECOMPLETE) &&
04189             (pconfig->smudge_corr_enabled == 1)) {
04190 
04191         run_nodetect = 2;
04192         for (i = 0; i < pR->active_results; i++) {
04193             if (pR->VL53L1_p_002[i].range_status ==
04194                 VL53L1_DEVICEERROR_RANGECOMPLETE) {
04195                 if (pR->VL53L1_p_002[i].median_range_mm
04196                         <=
04197                     pconfig->nodetect_min_range_mm) {
04198                     run_nodetect = 0;
04199                 } else {
04200                     if (run_nodetect == 2) {
04201                         run_nodetect = 1;
04202                         nodetect_index = i;
04203                     }
04204                 }
04205             }
04206         }
04207 
04208         if (run_nodetect == 2)
04209 
04210             run_nodetect = 0;
04211 
04212         amr =
04213         pR->VL53L1_p_002[nodetect_index].ambient_count_rate_mcps;
04214 
04215         if (run_nodetect == 1) {
04216 
04217 
04218 
04219 
04220             utemp64 = 1000 * ((uint64_t)amr);
04221 
04222 
04223             utemp64 = utemp64 << 9;
04224 
04225 
04226             if (utemp64 < pconfig->nodetect_ambient_threshold)
04227                 run_nodetect = 1;
04228             else
04229                 run_nodetect = 0;
04230 
04231         }
04232     }
04233 
04234 
04235     if (run_smudge_detection) {
04236 
04237         pint->nodetect_counter = 0;
04238 
04239 
04240         VL53L1_dynamic_xtalk_correction_calc_required_samples(Dev);
04241 
04242 
04243         xtalk_offset_in =
04244             pR->xmonitor.VL53L1_p_012;
04245 
04246 
04247         cco = pX->algo__crosstalk_compensation_plane_offset_kcps;
04248         current_xtalk = ((uint32_t)cco) << 2;
04249 
04250 
04251         smudge_margin_adjusted =
04252                 ((uint32_t)(pconfig->smudge_margin)) << 2;
04253 
04254 
04255         itemp32 = xtalk_offset_in - current_xtalk +
04256             smudge_margin_adjusted;
04257 
04258         if (itemp32 < 0)
04259             itemp32 = itemp32 * (-1);
04260 
04261 
04262         if (itemp32 > ((int32_t)pconfig->single_xtalk_delta)) {
04263             if ((int32_t)xtalk_offset_in >
04264                 ((int32_t)current_xtalk -
04265                     (int32_t)smudge_margin_adjusted)) {
04266                 pout->single_xtalk_delta_flag = 1;
04267             } else {
04268                 pout->single_xtalk_delta_flag = 2;
04269             }
04270         }
04271 
04272 
04273         pint->current_samples = pint->current_samples + 1;
04274 
04275 
04276         if (pint->current_samples > pconfig->sample_limit) {
04277             pout->sample_limit_exceeded_flag = 1;
04278             continue_processing = CONT_RESET;
04279         } else {
04280             pint->accumulator = pint->accumulator +
04281                 xtalk_offset_in;
04282         }
04283 
04284         if (pint->current_samples < pint->required_samples)
04285             continue_processing = CONT_NEXT_LOOP;
04286 
04287 
04288         xtalk_offset_out =
04289         (uint32_t)(do_division_u(pint->accumulator,
04290             pint->current_samples));
04291 
04292 
04293         itemp32 = xtalk_offset_out - current_xtalk +
04294             smudge_margin_adjusted;
04295 
04296         if (itemp32 < 0)
04297             itemp32 = itemp32 * (-1);
04298 
04299         if (continue_processing == CONT_CONTINUE &&
04300             (itemp32 >= ((int32_t)(pconfig->averaged_xtalk_delta)))
04301             ) {
04302             if ((int32_t)xtalk_offset_out >
04303                 ((int32_t)current_xtalk -
04304                     (int32_t)smudge_margin_adjusted))
04305                 pout->averaged_xtalk_delta_flag = 1;
04306             else
04307                 pout->averaged_xtalk_delta_flag = 2;
04308         }
04309 
04310         if (continue_processing == CONT_CONTINUE &&
04311             (itemp32 < ((int32_t)(pconfig->averaged_xtalk_delta)))
04312             )
04313 
04314             continue_processing = CONT_RESET;
04315 
04316 
04317 
04318         pout->smudge_corr_clipped = 0;
04319         if ((continue_processing == CONT_CONTINUE) &&
04320             (pconfig->smudge_corr_clip_limit != 0)) {
04321             if (xtalk_offset_out >
04322             (pconfig->smudge_corr_clip_limit * histo_merge_nb)) {
04323                 pout->smudge_corr_clipped = 1;
04324                 continue_processing = CONT_RESET;
04325             }
04326         }
04327 
04328 
04329 
04330         if (pconfig->user_xtalk_offset_limit_hi &&
04331             (xtalk_offset_out >
04332                 pconfig->user_xtalk_offset_limit))
04333             xtalk_offset_out =
04334                 pconfig->user_xtalk_offset_limit;
04335 
04336 
04337 
04338         if ((pconfig->user_xtalk_offset_limit_hi == 0) &&
04339             (xtalk_offset_out <
04340                 pconfig->user_xtalk_offset_limit))
04341             xtalk_offset_out =
04342                 pconfig->user_xtalk_offset_limit;
04343 
04344 
04345 
04346         xtalk_offset_out = xtalk_offset_out >> 2;
04347         if (xtalk_offset_out > 0x3FFFF)
04348             xtalk_offset_out = 0x3FFFF;
04349 
04350 
04351         if (continue_processing == CONT_CONTINUE) {
04352 
04353             VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
04354                 Dev,
04355                 xtalk_offset_out,
04356                 pconfig,
04357                 pout,
04358                 1,
04359                 0
04360                 );
04361 
04362 
04363             continue_processing = CONT_RESET;
04364         } else {
04365 
04366             VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
04367                 Dev,
04368                 xtalk_offset_out,
04369                 pconfig,
04370                 pout,
04371                 1,
04372                 1
04373                 );
04374         }
04375 
04376 
04377         if (continue_processing == CONT_RESET) {
04378             pint->accumulator = 0;
04379             pint->current_samples = 0;
04380             pint->nodetect_counter = 0;
04381         }
04382 
04383     }
04384 
04385     continue_processing = CONT_CONTINUE;
04386     if (run_nodetect == 1) {
04387 
04388         pint->nodetect_counter += 1;
04389 
04390 
04391         if (pint->nodetect_counter < pconfig->nodetect_sample_limit)
04392             continue_processing = CONT_NEXT_LOOP;
04393 
04394 
04395         xtalk_offset_out = (uint32_t)(pconfig->nodetect_xtalk_offset);
04396 
04397         if (continue_processing == CONT_CONTINUE) {
04398 
04399             VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
04400                 Dev,
04401                 xtalk_offset_out,
04402                 pconfig,
04403                 pout,
04404                 0,
04405                 0
04406                 );
04407 
04408 
04409             pout->smudge_corr_valid = 2;
04410 
04411 
04412             continue_processing = CONT_RESET;
04413         } else {
04414 
04415             VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
04416                 Dev,
04417                 xtalk_offset_out,
04418                 pconfig,
04419                 pout,
04420                 0,
04421                 1
04422                 );
04423         }
04424 
04425 
04426         if (continue_processing == CONT_RESET) {
04427             pint->accumulator = 0;
04428             pint->current_samples = 0;
04429             pint->nodetect_counter = 0;
04430         }
04431     }
04432 
04433     LOG_FUNCTION_END(status);
04434 
04435     return status;
04436 }
04437 
04438 VL53L1_Error VL53L1_dynamic_xtalk_correction_data_init(
04439     VL53L1_DEV                          Dev
04440     )
04441 {
04442 
04443 
04444 
04445 
04446     VL53L1_Error  status = VL53L1_ERROR_NONE;
04447 
04448     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04449     VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev);
04450 
04451     LOG_FUNCTION_START("");
04452 
04453 
04454 
04455     pdev->smudge_correct_config.smudge_corr_enabled       = 1;
04456     pdev->smudge_correct_config.smudge_corr_apply_enabled = 1;
04457     pdev->smudge_correct_config.smudge_corr_single_apply  =
04458         VL53L1_TUNINGPARM_DYNXTALK_SMUDGE_COR_SINGLE_APPLY_DEFAULT;
04459 
04460     pdev->smudge_correct_config.smudge_margin =
04461         VL53L1_TUNINGPARM_DYNXTALK_SMUDGE_MARGIN_DEFAULT;
04462     pdev->smudge_correct_config.noise_margin =
04463         VL53L1_TUNINGPARM_DYNXTALK_NOISE_MARGIN_DEFAULT;
04464     pdev->smudge_correct_config.user_xtalk_offset_limit =
04465         VL53L1_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_DEFAULT;
04466     pdev->smudge_correct_config.user_xtalk_offset_limit_hi =
04467         VL53L1_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_HI_DEFAULT;
04468     pdev->smudge_correct_config.sample_limit =
04469         VL53L1_TUNINGPARM_DYNXTALK_SAMPLE_LIMIT_DEFAULT;
04470     pdev->smudge_correct_config.single_xtalk_delta =
04471         VL53L1_TUNINGPARM_DYNXTALK_SINGLE_XTALK_DELTA_DEFAULT;
04472     pdev->smudge_correct_config.averaged_xtalk_delta =
04473         VL53L1_TUNINGPARM_DYNXTALK_AVERAGED_XTALK_DELTA_DEFAULT;
04474     pdev->smudge_correct_config.smudge_corr_clip_limit =
04475         VL53L1_TUNINGPARM_DYNXTALK_CLIP_LIMIT_DEFAULT;
04476     pdev->smudge_correct_config.smudge_corr_ambient_threshold =
04477         VL53L1_TUNINGPARM_DYNXTALK_XTALK_AMB_THRESHOLD_DEFAULT;
04478     pdev->smudge_correct_config.scaler_calc_method =
04479         0;
04480     pdev->smudge_correct_config.x_gradient_scaler =
04481         VL53L1_TUNINGPARM_DYNXTALK_XGRADIENT_SCALER_DEFAULT;
04482     pdev->smudge_correct_config.y_gradient_scaler =
04483         VL53L1_TUNINGPARM_DYNXTALK_YGRADIENT_SCALER_DEFAULT;
04484     pdev->smudge_correct_config.user_scaler_set =
04485         VL53L1_TUNINGPARM_DYNXTALK_USER_SCALER_SET_DEFAULT;
04486     pdev->smudge_correct_config.nodetect_ambient_threshold =
04487         VL53L1_TUNINGPARM_DYNXTALK_NODETECT_AMB_THRESHOLD_KCPS_DEFAULT;
04488     pdev->smudge_correct_config.nodetect_sample_limit =
04489         VL53L1_TUNINGPARM_DYNXTALK_NODETECT_SAMPLE_LIMIT_DEFAULT;
04490     pdev->smudge_correct_config.nodetect_xtalk_offset =
04491         VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS_DEFAULT;
04492     pdev->smudge_correct_config.nodetect_min_range_mm =
04493         VL53L1_TUNINGPARM_DYNXTALK_NODETECT_MIN_RANGE_MM_DEFAULT;
04494     pdev->smudge_correct_config.max_smudge_factor =
04495         VL53L1_TUNINGPARM_DYNXTALK_MAX_SMUDGE_FACTOR_DEFAULT;
04496 
04497 
04498     pdev->smudge_corrector_internals.current_samples = 0;
04499     pdev->smudge_corrector_internals.required_samples = 0;
04500     pdev->smudge_corrector_internals.accumulator = 0;
04501     pdev->smudge_corrector_internals.nodetect_counter = 0;
04502 
04503 
04504     VL53L1_dynamic_xtalk_correction_output_init(pres);
04505 
04506     LOG_FUNCTION_END(status);
04507 
04508     return status;
04509 }
04510 
04511 VL53L1_Error VL53L1_dynamic_xtalk_correction_output_init(
04512     VL53L1_LLDriverResults_t *pres
04513     )
04514 {
04515 
04516 
04517 
04518 
04519     VL53L1_Error  status = VL53L1_ERROR_NONE;
04520 
04521     VL53L1_smudge_corrector_data_t *pdata;
04522 
04523     LOG_FUNCTION_START("");
04524 
04525 
04526     pdata = &(pres->range_results.smudge_corrector_data);
04527 
04528     pdata->smudge_corr_valid = 0;
04529     pdata->smudge_corr_clipped = 0;
04530     pdata->single_xtalk_delta_flag = 0;
04531     pdata->averaged_xtalk_delta_flag = 0;
04532     pdata->sample_limit_exceeded_flag = 0;
04533     pdata->gradient_zero_flag = 0;
04534     pdata->new_xtalk_applied_flag = 0;
04535 
04536     pdata->algo__crosstalk_compensation_plane_offset_kcps = 0;
04537     pdata->algo__crosstalk_compensation_x_plane_gradient_kcps = 0;
04538     pdata->algo__crosstalk_compensation_y_plane_gradient_kcps = 0;
04539 
04540     LOG_FUNCTION_END(status);
04541 
04542     return status;
04543 }
04544 
04545 
04546 VL53L1_Error VL53L1_xtalk_cal_data_init(
04547     VL53L1_DEV                          Dev
04548     )
04549 {
04550 
04551 
04552 
04553 
04554     VL53L1_Error  status = VL53L1_ERROR_NONE;
04555 
04556     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04557 
04558     LOG_FUNCTION_START("");
04559 
04560 
04561 
04562     pdev->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps = 0;
04563     pdev->xtalk_cal.algo__crosstalk_compensation_x_plane_gradient_kcps = 0;
04564     pdev->xtalk_cal.algo__crosstalk_compensation_y_plane_gradient_kcps = 0;
04565     memset(&pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[0], 0,
04566         sizeof(pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps));
04567 
04568     LOG_FUNCTION_END(status);
04569 
04570     return status;
04571 }
04572 
04573 
04574 
04575 
04576 
04577 
04578 VL53L1_Error VL53L1_low_power_auto_data_init(
04579     VL53L1_DEV                          Dev
04580     )
04581 {
04582 
04583 
04584 
04585 
04586     VL53L1_Error  status = VL53L1_ERROR_NONE;
04587 
04588     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04589 
04590     LOG_FUNCTION_START("");
04591 
04592     pdev->low_power_auto_data.vhv_loop_bound =
04593         VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT;
04594     pdev->low_power_auto_data.is_low_power_auto_mode = 0;
04595     pdev->low_power_auto_data.low_power_auto_range_count = 0;
04596     pdev->low_power_auto_data.saved_interrupt_config = 0;
04597     pdev->low_power_auto_data.saved_vhv_init = 0;
04598     pdev->low_power_auto_data.saved_vhv_timeout = 0;
04599     pdev->low_power_auto_data.first_run_phasecal_result = 0;
04600     pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
04601     pdev->low_power_auto_data.dss__required_spads = 0;
04602 
04603     LOG_FUNCTION_END(status);
04604 
04605     return status;
04606 }
04607 
04608 VL53L1_Error VL53L1_low_power_auto_data_stop_range(
04609     VL53L1_DEV                          Dev
04610     )
04611 {
04612 
04613 
04614 
04615 
04616     VL53L1_Error  status = VL53L1_ERROR_NONE;
04617 
04618     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04619 
04620     LOG_FUNCTION_START("");
04621 
04622 
04623 
04624     pdev->low_power_auto_data.low_power_auto_range_count = 0xFF;
04625 
04626     pdev->low_power_auto_data.first_run_phasecal_result = 0;
04627     pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
04628     pdev->low_power_auto_data.dss__required_spads = 0;
04629 
04630 
04631     if (pdev->low_power_auto_data.saved_vhv_init != 0)
04632         pdev->stat_nvm.vhv_config__init =
04633             pdev->low_power_auto_data.saved_vhv_init;
04634     if (pdev->low_power_auto_data.saved_vhv_timeout != 0)
04635         pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
04636             pdev->low_power_auto_data.saved_vhv_timeout;
04637 
04638 
04639     pdev->gen_cfg.phasecal_config__override = 0x00;
04640 
04641     LOG_FUNCTION_END(status);
04642 
04643     return status;
04644 }
04645 
04646 VL53L1_Error VL53L1_config_low_power_auto_mode(
04647     VL53L1_general_config_t   *pgeneral,
04648     VL53L1_dynamic_config_t   *pdynamic,
04649     VL53L1_low_power_auto_data_t *plpadata
04650     )
04651 {
04652 
04653 
04654 
04655 
04656     VL53L1_Error  status = VL53L1_ERROR_NONE;
04657 
04658     LOG_FUNCTION_START("");
04659 
04660 
04661     plpadata->is_low_power_auto_mode = 1;
04662 
04663 
04664     plpadata->low_power_auto_range_count = 0;
04665 
04666 
04667     pdynamic->system__sequence_config =
04668             VL53L1_SEQUENCE_VHV_EN |
04669             VL53L1_SEQUENCE_PHASECAL_EN |
04670             VL53L1_SEQUENCE_DSS1_EN |
04671 
04672 
04673 
04674             VL53L1_SEQUENCE_RANGE_EN;
04675 
04676 
04677     pgeneral->dss_config__manual_effective_spads_select = 200 << 8;
04678     pgeneral->dss_config__roi_mode_control =
04679         VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
04680 
04681     LOG_FUNCTION_END(status);
04682 
04683     return status;
04684 }
04685 
04686 VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration(
04687     VL53L1_DEV        Dev)
04688 {
04689 
04690 
04691 
04692     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04693 
04694 
04695     VL53L1_Error  status = VL53L1_ERROR_NONE;
04696 
04697     LOG_FUNCTION_START("");
04698 
04699 
04700     pdev->low_power_auto_data.saved_vhv_init =
04701         pdev->stat_nvm.vhv_config__init;
04702     pdev->low_power_auto_data.saved_vhv_timeout =
04703         pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound;
04704 
04705 
04706     pdev->stat_nvm.vhv_config__init &= 0x7F;
04707 
04708     pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
04709         (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) +
04710         (pdev->low_power_auto_data.vhv_loop_bound << 2);
04711 
04712     pdev->gen_cfg.phasecal_config__override = 0x01;
04713     pdev->low_power_auto_data.first_run_phasecal_result =
04714         pdev->dbg_results.phasecal_result__vcsel_start;
04715     pdev->gen_cfg.cal_config__vcsel_start =
04716         pdev->low_power_auto_data.first_run_phasecal_result;
04717 
04718     LOG_FUNCTION_END(status);
04719 
04720     return status;
04721 }
04722 
04723 VL53L1_Error VL53L1_low_power_auto_update_DSS(
04724     VL53L1_DEV        Dev)
04725 {
04726 
04727 
04728 
04729     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04730 
04731     VL53L1_system_results_t *pS = &(pdev->sys_results);
04732 
04733 
04734     VL53L1_Error  status = VL53L1_ERROR_NONE;
04735 
04736     uint32_t utemp32a;
04737 
04738     LOG_FUNCTION_START("");
04739 
04740 
04741 
04742 
04743     utemp32a =
04744         pS->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0
04745          + pS->result__ambient_count_rate_mcps_sd0;
04746 
04747 
04748     if (utemp32a > 0xFFFF)
04749         utemp32a = 0xFFFF;
04750 
04751 
04752 
04753     utemp32a = utemp32a << 16;
04754 
04755 
04756     if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0)
04757         status = VL53L1_ERROR_DIVISION_BY_ZERO;
04758     else {
04759 
04760         utemp32a = utemp32a /
04761         pdev->sys_results.result__dss_actual_effective_spads_sd0;
04762 
04763         pdev->low_power_auto_data.dss__total_rate_per_spad_mcps =
04764             utemp32a;
04765 
04766 
04767         utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps <<
04768             16;
04769 
04770 
04771         if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps
04772                 == 0)
04773             status = VL53L1_ERROR_DIVISION_BY_ZERO;
04774         else {
04775 
04776             utemp32a = utemp32a /
04777             pdev->low_power_auto_data.dss__total_rate_per_spad_mcps;
04778 
04779 
04780             if (utemp32a > 0xFFFF)
04781                 utemp32a = 0xFFFF;
04782 
04783 
04784             pdev->low_power_auto_data.dss__required_spads =
04785                 (uint16_t)utemp32a;
04786 
04787 
04788             pdev->gen_cfg.dss_config__manual_effective_spads_select
04789             = pdev->low_power_auto_data.dss__required_spads;
04790             pdev->gen_cfg.dss_config__roi_mode_control =
04791             VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
04792         }
04793 
04794     }
04795 
04796     if (status == VL53L1_ERROR_DIVISION_BY_ZERO) {
04797 
04798 
04799 
04800         pdev->low_power_auto_data.dss__required_spads = 0x8000;
04801 
04802 
04803         pdev->gen_cfg.dss_config__manual_effective_spads_select =
04804             pdev->low_power_auto_data.dss__required_spads;
04805         pdev->gen_cfg.dss_config__roi_mode_control =
04806             VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
04807 
04808 
04809         status = VL53L1_ERROR_NONE;
04810     }
04811 
04812     LOG_FUNCTION_END(status);
04813 
04814     return status;
04815 }
04816 
04817 
04818 
04819 
04820 VL53L1_Error VL53L1_compute_histo_merge_nb(
04821     VL53L1_DEV        Dev,  uint8_t *histo_merge_nb)
04822 {
04823     VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
04824     VL53L1_Error  status = VL53L1_ERROR_NONE;
04825     uint8_t i, timing;
04826     uint8_t sum = 0;
04827 
04828     timing = (pdev->hist_data.bin_seq[0] == 7 ? 1 : 0);
04829     for (i = 0; i < VL53L1_BIN_REC_SIZE; i++)
04830         if (pdev->multi_bins_rec[i][timing][7] > 0)
04831             sum++;
04832     *histo_merge_nb = sum;
04833 
04834     return status;
04835 }
04836