ST Expansion SW Team
/
VL53L1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Embed:
(wiki syntax)
Show/hide line numbers
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 timeout_mclks = 01226 ((timeout_us << 12) + (macro_period_us>>1)) / 01227 macro_period_us; 01228 01229 LOG_FUNCTION_END(0); 01230 01231 return timeout_mclks; 01232 } 01233 01234 01235 uint16_t VL53L1_calc_encoded_timeout( 01236 uint32_t timeout_us, 01237 uint32_t macro_period_us) 01238 { 01239 01240 01241 uint32_t timeout_mclks = 0; 01242 uint16_t timeout_encoded = 0; 01243 01244 LOG_FUNCTION_START(""); 01245 01246 timeout_mclks = 01247 VL53L1_calc_timeout_mclks(timeout_us, macro_period_us); 01248 01249 timeout_encoded = 01250 VL53L1_encode_timeout(timeout_mclks); 01251 01252 01253 01254 LOG_FUNCTION_END(0); 01255 01256 return timeout_encoded; 01257 } 01258 01259 01260 uint32_t VL53L1_calc_timeout_us( 01261 uint32_t timeout_mclks, 01262 uint32_t macro_period_us) 01263 { 01264 01265 01266 uint32_t timeout_us = 0; 01267 uint64_t tmp = 0; 01268 01269 LOG_FUNCTION_START(""); 01270 01271 tmp = (uint64_t)timeout_mclks * (uint64_t)macro_period_us; 01272 tmp += 0x00800; 01273 tmp = tmp >> 12; 01274 01275 timeout_us = (uint32_t)tmp; 01276 01277 01278 01279 LOG_FUNCTION_END(0); 01280 01281 return timeout_us; 01282 } 01283 01284 uint32_t VL53L1_calc_crosstalk_plane_offset_with_margin( 01285 uint32_t plane_offset_kcps, 01286 int16_t margin_offset_kcps) 01287 { 01288 uint32_t plane_offset_with_margin = 0; 01289 int32_t plane_offset_kcps_temp = 0; 01290 01291 LOG_FUNCTION_START(""); 01292 01293 plane_offset_kcps_temp = 01294 (int32_t)plane_offset_kcps + 01295 (int32_t)margin_offset_kcps; 01296 01297 if (plane_offset_kcps_temp < 0) 01298 plane_offset_kcps_temp = 0; 01299 else 01300 if (plane_offset_kcps_temp > 0x3FFFF) 01301 plane_offset_kcps_temp = 0x3FFFF; 01302 01303 plane_offset_with_margin = (uint32_t) plane_offset_kcps_temp; 01304 01305 LOG_FUNCTION_END(0); 01306 01307 return plane_offset_with_margin; 01308 01309 } 01310 01311 uint32_t VL53L1_calc_decoded_timeout_us( 01312 uint16_t timeout_encoded, 01313 uint32_t macro_period_us) 01314 { 01315 01316 01317 uint32_t timeout_mclks = 0; 01318 uint32_t timeout_us = 0; 01319 01320 LOG_FUNCTION_START(""); 01321 01322 timeout_mclks = 01323 VL53L1_decode_timeout(timeout_encoded); 01324 01325 timeout_us = 01326 VL53L1_calc_timeout_us(timeout_mclks, macro_period_us); 01327 01328 LOG_FUNCTION_END(0); 01329 01330 return timeout_us; 01331 } 01332 01333 01334 uint16_t VL53L1_encode_timeout(uint32_t timeout_mclks) 01335 { 01336 01337 01338 uint16_t encoded_timeout = 0; 01339 uint32_t ls_byte = 0; 01340 uint16_t ms_byte = 0; 01341 01342 if (timeout_mclks > 0) { 01343 ls_byte = timeout_mclks - 1; 01344 01345 while ((ls_byte & 0xFFFFFF00) > 0) { 01346 ls_byte = ls_byte >> 1; 01347 ms_byte++; 01348 } 01349 01350 encoded_timeout = (ms_byte << 8) 01351 + (uint16_t) (ls_byte & 0x000000FF); 01352 } 01353 01354 return encoded_timeout; 01355 } 01356 01357 01358 uint32_t VL53L1_decode_timeout(uint16_t encoded_timeout) 01359 { 01360 01361 01362 uint32_t timeout_macro_clks = 0; 01363 01364 timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF) 01365 << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1; 01366 01367 return timeout_macro_clks; 01368 } 01369 01370 01371 VL53L1_Error VL53L1_calc_timeout_register_values( 01372 uint32_t phasecal_config_timeout_us, 01373 uint32_t mm_config_timeout_us, 01374 uint32_t range_config_timeout_us, 01375 uint16_t fast_osc_frequency, 01376 VL53L1_general_config_t *pgeneral, 01377 VL53L1_timing_config_t *ptiming) 01378 { 01379 01380 01381 VL53L1_Error status = VL53L1_ERROR_NONE; 01382 01383 uint32_t macro_period_us = 0; 01384 uint32_t timeout_mclks = 0; 01385 uint16_t timeout_encoded = 0; 01386 01387 LOG_FUNCTION_START(""); 01388 01389 if (fast_osc_frequency == 0) { 01390 status = VL53L1_ERROR_DIVISION_BY_ZERO; 01391 } else { 01392 01393 macro_period_us = 01394 VL53L1_calc_macro_period_us( 01395 fast_osc_frequency, 01396 ptiming->range_config__vcsel_period_a); 01397 01398 01399 timeout_mclks = 01400 VL53L1_calc_timeout_mclks( 01401 phasecal_config_timeout_us, 01402 macro_period_us); 01403 01404 01405 if (timeout_mclks > 0xFF) 01406 timeout_mclks = 0xFF; 01407 01408 pgeneral->phasecal_config__timeout_macrop = 01409 (uint8_t)timeout_mclks; 01410 01411 01412 timeout_encoded = 01413 VL53L1_calc_encoded_timeout( 01414 mm_config_timeout_us, 01415 macro_period_us); 01416 01417 ptiming->mm_config__timeout_macrop_a_hi = 01418 (uint8_t)((timeout_encoded & 0xFF00) >> 8); 01419 ptiming->mm_config__timeout_macrop_a_lo = 01420 (uint8_t) (timeout_encoded & 0x00FF); 01421 01422 01423 timeout_encoded = 01424 VL53L1_calc_encoded_timeout( 01425 range_config_timeout_us, 01426 macro_period_us); 01427 01428 ptiming->range_config__timeout_macrop_a_hi = 01429 (uint8_t)((timeout_encoded & 0xFF00) >> 8); 01430 ptiming->range_config__timeout_macrop_a_lo = 01431 (uint8_t) (timeout_encoded & 0x00FF); 01432 01433 01434 macro_period_us = 01435 VL53L1_calc_macro_period_us( 01436 fast_osc_frequency, 01437 ptiming->range_config__vcsel_period_b); 01438 01439 01440 timeout_encoded = 01441 VL53L1_calc_encoded_timeout( 01442 mm_config_timeout_us, 01443 macro_period_us); 01444 01445 ptiming->mm_config__timeout_macrop_b_hi = 01446 (uint8_t)((timeout_encoded & 0xFF00) >> 8); 01447 ptiming->mm_config__timeout_macrop_b_lo = 01448 (uint8_t) (timeout_encoded & 0x00FF); 01449 01450 01451 timeout_encoded = VL53L1_calc_encoded_timeout( 01452 range_config_timeout_us, 01453 macro_period_us); 01454 01455 ptiming->range_config__timeout_macrop_b_hi = 01456 (uint8_t)((timeout_encoded & 0xFF00) >> 8); 01457 ptiming->range_config__timeout_macrop_b_lo = 01458 (uint8_t) (timeout_encoded & 0x00FF); 01459 } 01460 01461 LOG_FUNCTION_END(0); 01462 01463 return status; 01464 01465 } 01466 01467 01468 uint8_t VL53L1_encode_vcsel_period(uint8_t VL53L1_p_031) 01469 { 01470 01471 01472 uint8_t vcsel_period_reg = 0; 01473 01474 vcsel_period_reg = (VL53L1_p_031 >> 1) - 1; 01475 01476 return vcsel_period_reg; 01477 } 01478 01479 01480 uint32_t VL53L1_decode_unsigned_integer( 01481 uint8_t *pbuffer, 01482 uint8_t no_of_bytes) 01483 { 01484 01485 01486 uint8_t i = 0; 01487 uint32_t decoded_value = 0; 01488 01489 for (i = 0; i < no_of_bytes; i++) 01490 decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i]; 01491 01492 return decoded_value; 01493 } 01494 01495 01496 void VL53L1_encode_unsigned_integer( 01497 uint32_t ip_value, 01498 uint8_t no_of_bytes, 01499 uint8_t *pbuffer) 01500 { 01501 01502 01503 uint8_t i = 0; 01504 uint32_t VL53L1_p_002 = 0; 01505 01506 VL53L1_p_002 = ip_value; 01507 for (i = 0; i < no_of_bytes; i++) { 01508 pbuffer[no_of_bytes-i-1] = VL53L1_p_002 & 0x00FF; 01509 VL53L1_p_002 = VL53L1_p_002 >> 8; 01510 } 01511 } 01512 01513 01514 VL53L1_Error VL53L1_hist_copy_and_scale_ambient_info( 01515 VL53L1_zone_hist_info_t *pidata, 01516 VL53L1_histogram_bin_data_t *podata) 01517 { 01518 01519 01520 VL53L1_Error status = VL53L1_ERROR_NONE; 01521 01522 int64_t evts = 0; 01523 int64_t tmpi = 0; 01524 int64_t tmpo = 0; 01525 01526 LOG_FUNCTION_START(""); 01527 01528 01529 if (pidata->result__dss_actual_effective_spads == 0) { 01530 status = VL53L1_ERROR_DIVISION_BY_ZERO; 01531 } else { 01532 if (pidata->number_of_ambient_bins > 0 && 01533 podata->number_of_ambient_bins == 0) { 01534 01535 01536 01537 tmpo = 1 + (int64_t)podata->total_periods_elapsed; 01538 tmpo *= 01539 (int64_t)podata->result__dss_actual_effective_spads; 01540 01541 tmpi = 1 + (int64_t)pidata->total_periods_elapsed; 01542 tmpi *= 01543 (int64_t)pidata->result__dss_actual_effective_spads; 01544 01545 evts = tmpo * 01546 (int64_t)pidata->ambient_events_sum; 01547 evts += (tmpi/2); 01548 01549 01550 if (tmpi != 0) 01551 evts = do_division_s(evts, tmpi); 01552 01553 podata->ambient_events_sum = (int32_t)evts; 01554 01555 01556 01557 podata->VL53L1_p_004 = 01558 podata->ambient_events_sum; 01559 podata->VL53L1_p_004 += 01560 ((int32_t)pidata->number_of_ambient_bins / 2); 01561 podata->VL53L1_p_004 /= 01562 (int32_t)pidata->number_of_ambient_bins; 01563 } 01564 } 01565 01566 LOG_FUNCTION_END(0); 01567 01568 return status; 01569 } 01570 01571 01572 void VL53L1_hist_get_bin_sequence_config( 01573 VL53L1_DEV Dev, 01574 VL53L1_histogram_bin_data_t *pdata) 01575 { 01576 01577 01578 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 01579 01580 int32_t amb_thresh_low = 0; 01581 int32_t amb_thresh_high = 0; 01582 01583 uint8_t i = 0; 01584 01585 LOG_FUNCTION_START(""); 01586 01587 01588 01589 amb_thresh_low = 1024 * 01590 (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_low; 01591 amb_thresh_high = 1024 * 01592 (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_high; 01593 01594 01595 01596 if ((pdev->ll_state.rd_stream_count & 0x01) == 0) { 01597 01598 pdata->bin_seq[5] = 01599 pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 >> 4; 01600 pdata->bin_seq[4] = 01601 pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 & 0x0F; 01602 pdata->bin_seq[3] = 01603 pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 >> 4; 01604 pdata->bin_seq[2] = 01605 pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 & 0x0F; 01606 pdata->bin_seq[1] = 01607 pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 >> 4; 01608 pdata->bin_seq[0] = 01609 pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 & 0x0F; 01610 01611 if (pdata->ambient_events_sum > amb_thresh_high) { 01612 pdata->bin_seq[5] = 01613 pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5 01614 >> 4; 01615 pdata->bin_seq[4] = 01616 pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5 01617 & 0x0F; 01618 pdata->bin_seq[3] = 01619 pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3 01620 >> 4; 01621 pdata->bin_seq[2] = 01622 pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3 01623 & 0x0F; 01624 pdata->bin_seq[1] = 01625 pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1 01626 >> 4; 01627 pdata->bin_seq[0] = 01628 pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1 01629 & 0x0F; 01630 } 01631 01632 if (pdata->ambient_events_sum < amb_thresh_low) { 01633 pdata->bin_seq[5] = 01634 pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5 01635 >> 4; 01636 pdata->bin_seq[4] = 01637 pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5 01638 & 0x0F; 01639 pdata->bin_seq[3] = 01640 pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3 01641 >> 4; 01642 pdata->bin_seq[2] = 01643 pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3 01644 & 0x0F; 01645 pdata->bin_seq[1] = 01646 pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1 01647 >> 4; 01648 pdata->bin_seq[0] = 01649 pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1 01650 & 0x0F; 01651 } 01652 01653 } else { 01654 pdata->bin_seq[5] = 01655 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_5 01656 & 0x0F; 01657 pdata->bin_seq[4] = 01658 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4 01659 & 0x0F; 01660 pdata->bin_seq[3] = 01661 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4 01662 >> 4; 01663 pdata->bin_seq[2] = 01664 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_2 & 01665 0x0F; 01666 pdata->bin_seq[1] = 01667 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1 01668 >> 4; 01669 pdata->bin_seq[0] = 01670 pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1 01671 & 0x0F; 01672 01673 if (pdata->ambient_events_sum > amb_thresh_high) { 01674 pdata->bin_seq[5] = 01675 pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5 01676 >> 4; 01677 pdata->bin_seq[4] = 01678 pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5 01679 & 0x0F; 01680 pdata->bin_seq[3] = 01681 pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3 01682 >> 4; 01683 pdata->bin_seq[2] = 01684 pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3 01685 & 0x0F; 01686 pdata->bin_seq[1] = 01687 pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1 01688 >> 4; 01689 pdata->bin_seq[0] = 01690 pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1 01691 & 0x0F; 01692 } 01693 01694 if (pdata->ambient_events_sum < amb_thresh_low) { 01695 pdata->bin_seq[5] = 01696 pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5 01697 >> 4; 01698 pdata->bin_seq[4] = 01699 pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5 01700 & 0x0F; 01701 pdata->bin_seq[3] = 01702 pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3 01703 >> 4; 01704 pdata->bin_seq[2] = 01705 pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3 01706 & 0x0F; 01707 pdata->bin_seq[1] = 01708 pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1 01709 >> 4; 01710 pdata->bin_seq[0] = 01711 pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1 01712 & 0x0F; 01713 } 01714 } 01715 01716 01717 01718 for (i = 0; i < VL53L1_MAX_BIN_SEQUENCE_LENGTH; i++) 01719 pdata->bin_rep[i] = 1; 01720 01721 LOG_FUNCTION_END(0); 01722 01723 } 01724 01725 01726 VL53L1_Error VL53L1_hist_phase_consistency_check( 01727 VL53L1_DEV Dev, 01728 VL53L1_zone_hist_info_t *phist_prev, 01729 VL53L1_zone_objects_t *prange_prev, 01730 VL53L1_range_results_t *prange_curr) 01731 { 01732 01733 01734 01735 VL53L1_Error status = VL53L1_ERROR_NONE; 01736 VL53L1_LLDriverData_t *pdev = 01737 VL53L1DevStructGetLLDriverHandle(Dev); 01738 01739 uint8_t lc = 0; 01740 uint8_t p = 0; 01741 01742 uint16_t phase_delta = 0; 01743 uint16_t phase_tolerance = 0; 01744 01745 int32_t events_delta = 0; 01746 int32_t events_tolerance = 0; 01747 01748 01749 uint8_t event_sigma; 01750 uint16_t event_min_spad_count; 01751 uint16_t min_max_tolerance; 01752 uint8_t pht; 01753 01754 VL53L1_DeviceError range_status = 0; 01755 01756 LOG_FUNCTION_START(""); 01757 01758 event_sigma = 01759 pdev->histpostprocess.algo__consistency_check__event_sigma; 01760 event_min_spad_count = 01761 pdev->histpostprocess.algo__consistency_check__event_min_spad_count; 01762 min_max_tolerance = 01763 pdev->histpostprocess.algo__consistency_check__min_max_tolerance; 01764 01765 01766 pht = pdev->histpostprocess.algo__consistency_check__phase_tolerance; 01767 phase_tolerance = (uint16_t)pht; 01768 phase_tolerance = phase_tolerance << 8; 01769 01770 01771 01772 if (prange_prev->rd_device_state != 01773 VL53L1_DEVICESTATE_RANGING_GATHER_DATA && 01774 prange_prev->rd_device_state != 01775 VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA) 01776 return status; 01777 01778 01779 01780 if (phase_tolerance == 0) 01781 return status; 01782 01783 for (lc = 0; lc < prange_curr->active_results; lc++) { 01784 01785 if (!((prange_curr->VL53L1_p_002[lc].range_status == 01786 VL53L1_DEVICEERROR_RANGECOMPLETE) || 01787 (prange_curr->VL53L1_p_002[lc].range_status == 01788 VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK))) 01789 continue; 01790 01791 01792 01793 01794 01795 01796 if (prange_prev->active_objects == 0) 01797 prange_curr->VL53L1_p_002[lc].range_status = 01798 VL53L1_DEVICEERROR_PREV_RANGE_NO_TARGETS; 01799 else 01800 prange_curr->VL53L1_p_002[lc].range_status = 01801 VL53L1_DEVICEERROR_PHASECONSISTENCY; 01802 01803 01804 01805 01806 01807 for (p = 0; p < prange_prev->active_objects; p++) { 01808 01809 if (prange_curr->VL53L1_p_002[lc].VL53L1_p_014 > 01810 prange_prev->VL53L1_p_002[p].VL53L1_p_014) { 01811 phase_delta = 01812 prange_curr->VL53L1_p_002[lc].VL53L1_p_014 - 01813 prange_prev->VL53L1_p_002[p].VL53L1_p_014; 01814 } else { 01815 phase_delta = 01816 prange_prev->VL53L1_p_002[p].VL53L1_p_014 - 01817 prange_curr->VL53L1_p_002[lc].VL53L1_p_014; 01818 } 01819 01820 if (phase_delta < phase_tolerance) { 01821 01822 01823 01824 01825 01826 if (status == VL53L1_ERROR_NONE) 01827 status = 01828 VL53L1_hist_events_consistency_check( 01829 event_sigma, 01830 event_min_spad_count, 01831 phist_prev, 01832 &(prange_prev->VL53L1_p_002[p]), 01833 &(prange_curr->VL53L1_p_002[lc]), 01834 &events_tolerance, 01835 &events_delta, 01836 &range_status); 01837 01838 01839 01840 01841 if (status == VL53L1_ERROR_NONE && 01842 range_status == 01843 VL53L1_DEVICEERROR_RANGECOMPLETE) 01844 status = 01845 VL53L1_hist_merged_pulse_check( 01846 min_max_tolerance, 01847 &(prange_curr->VL53L1_p_002[lc]), 01848 &range_status); 01849 01850 prange_curr->VL53L1_p_002[lc].range_status = 01851 range_status; 01852 } 01853 } 01854 01855 } 01856 01857 LOG_FUNCTION_END(status); 01858 01859 return status; 01860 } 01861 01862 01863 01864 VL53L1_Error VL53L1_hist_events_consistency_check( 01865 uint8_t event_sigma, 01866 uint16_t min_effective_spad_count, 01867 VL53L1_zone_hist_info_t *phist_prev, 01868 VL53L1_object_data_t *prange_prev, 01869 VL53L1_range_data_t *prange_curr, 01870 int32_t *pevents_tolerance, 01871 int32_t *pevents_delta, 01872 VL53L1_DeviceError *prange_status) 01873 { 01874 01875 01876 01877 VL53L1_Error status = VL53L1_ERROR_NONE; 01878 01879 int64_t tmpp = 0; 01880 int64_t tmpc = 0; 01881 int64_t events_scaler = 0; 01882 int64_t events_scaler_sq = 0; 01883 int64_t c_signal_events = 0; 01884 int64_t c_sig_noise_sq = 0; 01885 int64_t c_amb_noise_sq = 0; 01886 int64_t p_amb_noise_sq = 0; 01887 01888 int32_t p_signal_events = 0; 01889 uint32_t noise_sq_sum = 0; 01890 01891 01892 01893 if (event_sigma == 0) { 01894 *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE; 01895 return status; 01896 } 01897 01898 01899 01900 tmpp = 1 + (int64_t)phist_prev->total_periods_elapsed; 01901 tmpp *= (int64_t)phist_prev->result__dss_actual_effective_spads; 01902 01903 01904 01905 tmpc = 1 + (int64_t)prange_curr->total_periods_elapsed; 01906 tmpc *= (int64_t)prange_curr->VL53L1_p_006; 01907 01908 01909 01910 events_scaler = tmpp * 4096; 01911 events_scaler += (tmpc/2); 01912 if (tmpc != 0) 01913 events_scaler = do_division_s(events_scaler, tmpc); 01914 01915 events_scaler_sq = events_scaler * events_scaler; 01916 events_scaler_sq += 2048; 01917 events_scaler_sq /= 4096; 01918 01919 01920 01921 c_signal_events = (int64_t)prange_curr->VL53L1_p_021; 01922 c_signal_events -= (int64_t)prange_curr->VL53L1_p_020; 01923 c_signal_events *= (int64_t)events_scaler; 01924 c_signal_events += 2048; 01925 c_signal_events /= 4096; 01926 01927 c_sig_noise_sq = (int64_t)events_scaler_sq; 01928 c_sig_noise_sq *= (int64_t)prange_curr->VL53L1_p_021; 01929 c_sig_noise_sq += 2048; 01930 c_sig_noise_sq /= 4096; 01931 01932 c_amb_noise_sq = (int64_t)events_scaler_sq; 01933 c_amb_noise_sq *= (int64_t)prange_curr->VL53L1_p_020; 01934 c_amb_noise_sq += 2048; 01935 c_amb_noise_sq /= 4096; 01936 01937 01938 c_amb_noise_sq += 2; 01939 c_amb_noise_sq /= 4; 01940 01941 01942 01943 p_amb_noise_sq = 01944 (int64_t)prange_prev->VL53L1_p_020; 01945 01946 01947 p_amb_noise_sq += 2; 01948 p_amb_noise_sq /= 4; 01949 01950 noise_sq_sum = 01951 (uint32_t)prange_prev->VL53L1_p_021 + 01952 (uint32_t)c_sig_noise_sq + 01953 (uint32_t)p_amb_noise_sq + 01954 (uint32_t)c_amb_noise_sq; 01955 01956 *pevents_tolerance = 01957 (int32_t)VL53L1_isqrt(noise_sq_sum * 16); 01958 01959 *pevents_tolerance *= (int32_t)event_sigma; 01960 *pevents_tolerance += 32; 01961 *pevents_tolerance /= 64; 01962 01963 p_signal_events = (int32_t)prange_prev->VL53L1_p_021; 01964 p_signal_events -= (int32_t)prange_prev->VL53L1_p_020; 01965 01966 if ((int32_t)c_signal_events > p_signal_events) 01967 *pevents_delta = 01968 (int32_t)c_signal_events - p_signal_events; 01969 else 01970 *pevents_delta = 01971 p_signal_events - (int32_t)c_signal_events; 01972 01973 if (*pevents_delta > *pevents_tolerance && 01974 prange_curr->VL53L1_p_006 > min_effective_spad_count) 01975 *prange_status = VL53L1_DEVICEERROR_EVENTCONSISTENCY; 01976 else 01977 *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE; 01978 01979 01980 01981 01982 01983 return status; 01984 } 01985 01986 01987 01988 01989 VL53L1_Error VL53L1_hist_merged_pulse_check( 01990 int16_t min_max_tolerance_mm, 01991 VL53L1_range_data_t *pdata, 01992 VL53L1_DeviceError *prange_status) 01993 { 01994 01995 01996 VL53L1_Error status = VL53L1_ERROR_NONE; 01997 int16_t delta_mm = 0; 01998 01999 if (pdata->max_range_mm > pdata->min_range_mm) 02000 delta_mm = 02001 pdata->max_range_mm - pdata->min_range_mm; 02002 else 02003 delta_mm = 02004 pdata->min_range_mm - pdata->max_range_mm; 02005 02006 if (min_max_tolerance_mm > 0 && 02007 delta_mm > min_max_tolerance_mm) 02008 *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE; 02009 else 02010 *prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE; 02011 02012 return status; 02013 } 02014 02015 02016 02017 02018 VL53L1_Error VL53L1_hist_xmonitor_consistency_check( 02019 VL53L1_DEV Dev, 02020 VL53L1_zone_hist_info_t *phist_prev, 02021 VL53L1_zone_objects_t *prange_prev, 02022 VL53L1_range_data_t *prange_curr) 02023 { 02024 02025 02026 VL53L1_Error status = VL53L1_ERROR_NONE; 02027 VL53L1_LLDriverData_t *pdev = 02028 VL53L1DevStructGetLLDriverHandle(Dev); 02029 02030 int32_t events_delta = 0; 02031 int32_t events_tolerance = 0; 02032 uint8_t event_sigma; 02033 uint16_t min_spad_count; 02034 02035 event_sigma = pdev->histpostprocess.algo__crosstalk_detect_event_sigma; 02036 min_spad_count = 02037 pdev->histpostprocess.algo__consistency_check__event_min_spad_count; 02038 02039 if (prange_curr->range_status == VL53L1_DEVICEERROR_RANGECOMPLETE || 02040 prange_curr->range_status == 02041 VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK || 02042 prange_curr->range_status == 02043 VL53L1_DEVICEERROR_EVENTCONSISTENCY) { 02044 02045 if (prange_prev->xmonitor.range_status == 02046 VL53L1_DEVICEERROR_RANGECOMPLETE || 02047 prange_prev->xmonitor.range_status == 02048 VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK || 02049 prange_prev->xmonitor.range_status == 02050 VL53L1_DEVICEERROR_EVENTCONSISTENCY) { 02051 02052 prange_curr->range_status = 02053 VL53L1_DEVICEERROR_RANGECOMPLETE; 02054 02055 status = 02056 VL53L1_hist_events_consistency_check( 02057 event_sigma, 02058 min_spad_count, 02059 phist_prev, 02060 &(prange_prev->xmonitor), 02061 prange_curr, 02062 &events_tolerance, 02063 &events_delta, 02064 &(prange_curr->range_status)); 02065 02066 } 02067 } 02068 02069 return status; 02070 } 02071 02072 02073 02074 02075 VL53L1_Error VL53L1_hist_wrap_dmax( 02076 VL53L1_hist_post_process_config_t *phistpostprocess, 02077 VL53L1_histogram_bin_data_t *pcurrent, 02078 int16_t *pwrap_dmax_mm) 02079 { 02080 02081 02082 02083 VL53L1_Error status = VL53L1_ERROR_NONE; 02084 02085 uint32_t pll_period_mm = 0; 02086 uint32_t wrap_dmax_phase = 0; 02087 uint32_t range_mm = 0; 02088 02089 LOG_FUNCTION_START(""); 02090 02091 *pwrap_dmax_mm = 0; 02092 02093 02094 if (pcurrent->VL53L1_p_019 != 0) { 02095 02096 02097 02098 pll_period_mm = 02099 VL53L1_calc_pll_period_mm( 02100 pcurrent->VL53L1_p_019); 02101 02102 02103 02104 wrap_dmax_phase = 02105 (uint32_t)phistpostprocess->valid_phase_high << 8; 02106 02107 02108 02109 range_mm = wrap_dmax_phase * pll_period_mm; 02110 range_mm = (range_mm + (1<<14)) >> 15; 02111 02112 *pwrap_dmax_mm = (int16_t)range_mm; 02113 } 02114 02115 LOG_FUNCTION_END(status); 02116 02117 return status; 02118 } 02119 02120 02121 void VL53L1_hist_combine_mm1_mm2_offsets( 02122 int16_t mm1_offset_mm, 02123 int16_t mm2_offset_mm, 02124 uint8_t encoded_mm_roi_centre, 02125 uint8_t encoded_mm_roi_size, 02126 uint8_t encoded_zone_centre, 02127 uint8_t encoded_zone_size, 02128 VL53L1_additional_offset_cal_data_t *pcal_data, 02129 uint8_t *pgood_spads, 02130 uint16_t aperture_attenuation, 02131 int16_t *prange_offset_mm) 02132 { 02133 02134 02135 02136 uint16_t max_mm_inner_effective_spads = 0; 02137 uint16_t max_mm_outer_effective_spads = 0; 02138 uint16_t mm_inner_effective_spads = 0; 02139 uint16_t mm_outer_effective_spads = 0; 02140 02141 uint32_t scaled_mm1_peak_rate_mcps = 0; 02142 uint32_t scaled_mm2_peak_rate_mcps = 0; 02143 02144 int32_t tmp0 = 0; 02145 int32_t tmp1 = 0; 02146 02147 02148 02149 VL53L1_calc_mm_effective_spads( 02150 encoded_mm_roi_centre, 02151 encoded_mm_roi_size, 02152 0xC7, 02153 0xFF, 02154 pgood_spads, 02155 aperture_attenuation, 02156 &max_mm_inner_effective_spads, 02157 &max_mm_outer_effective_spads); 02158 02159 if ((max_mm_inner_effective_spads == 0) || 02160 (max_mm_outer_effective_spads == 0)) 02161 goto FAIL; 02162 02163 02164 02165 VL53L1_calc_mm_effective_spads( 02166 encoded_mm_roi_centre, 02167 encoded_mm_roi_size, 02168 encoded_zone_centre, 02169 encoded_zone_size, 02170 pgood_spads, 02171 aperture_attenuation, 02172 &mm_inner_effective_spads, 02173 &mm_outer_effective_spads); 02174 02175 02176 02177 scaled_mm1_peak_rate_mcps = 02178 (uint32_t)pcal_data->result__mm_inner_peak_signal_count_rtn_mcps; 02179 scaled_mm1_peak_rate_mcps *= (uint32_t)mm_inner_effective_spads; 02180 scaled_mm1_peak_rate_mcps /= (uint32_t)max_mm_inner_effective_spads; 02181 02182 scaled_mm2_peak_rate_mcps = 02183 (uint32_t)pcal_data->result__mm_outer_peak_signal_count_rtn_mcps; 02184 scaled_mm2_peak_rate_mcps *= (uint32_t)mm_outer_effective_spads; 02185 scaled_mm2_peak_rate_mcps /= (uint32_t)max_mm_outer_effective_spads; 02186 02187 02188 02189 tmp0 = ((int32_t)mm1_offset_mm * (int32_t)scaled_mm1_peak_rate_mcps); 02190 tmp0 += ((int32_t)mm2_offset_mm * (int32_t)scaled_mm2_peak_rate_mcps); 02191 02192 tmp1 = (int32_t)scaled_mm1_peak_rate_mcps + 02193 (int32_t)scaled_mm2_peak_rate_mcps; 02194 02195 02196 02197 if (tmp1 != 0) 02198 tmp0 = (tmp0 * 4) / tmp1; 02199 FAIL: 02200 *prange_offset_mm = (int16_t)tmp0; 02201 02202 } 02203 02204 02205 VL53L1_Error VL53L1_hist_xtalk_extract_calc_window( 02206 int16_t target_distance_mm, 02207 uint16_t target_width_oversize, 02208 VL53L1_histogram_bin_data_t *phist_bins, 02209 VL53L1_hist_xtalk_extract_data_t *pxtalk_data) 02210 { 02211 02212 02213 VL53L1_Error status = VL53L1_ERROR_NONE; 02214 02215 LOG_FUNCTION_START(""); 02216 02217 02218 pxtalk_data->pll_period_mm = 02219 VL53L1_calc_pll_period_mm(phist_bins->VL53L1_p_019); 02220 02221 02222 pxtalk_data->xtalk_width_phase = 02223 (int32_t)phist_bins->vcsel_width * 128; 02224 pxtalk_data->target_width_phase = 02225 pxtalk_data->xtalk_width_phase + 02226 (int32_t)target_width_oversize * 128; 02227 02228 02229 02230 pxtalk_data->xtalk_start_phase = 02231 (int32_t)phist_bins->zero_distance_phase - 02232 (pxtalk_data->xtalk_width_phase / 2); 02233 pxtalk_data->xtalk_end_phase = 02234 (int32_t)pxtalk_data->xtalk_start_phase + 02235 pxtalk_data->xtalk_width_phase; 02236 02237 if (pxtalk_data->xtalk_start_phase < 0) 02238 pxtalk_data->xtalk_start_phase = 0; 02239 02240 02241 02242 02243 pxtalk_data->VL53L1_p_015 = 02244 (uint8_t)(pxtalk_data->xtalk_start_phase / 2048); 02245 02246 02247 pxtalk_data->VL53L1_p_016 = 02248 (uint8_t)((pxtalk_data->xtalk_end_phase + 2047) / 2048); 02249 02250 02251 02252 pxtalk_data->target_start_phase = 02253 (int32_t)target_distance_mm * 2048 * 16; 02254 pxtalk_data->target_start_phase += 02255 ((int32_t)pxtalk_data->pll_period_mm / 2); 02256 pxtalk_data->target_start_phase /= (int32_t)pxtalk_data->pll_period_mm; 02257 pxtalk_data->target_start_phase += 02258 (int32_t)phist_bins->zero_distance_phase; 02259 02260 02261 02262 pxtalk_data->target_start_phase -= 02263 (pxtalk_data->target_width_phase / 2); 02264 pxtalk_data->target_end_phase = 02265 (int32_t)pxtalk_data->target_start_phase + 02266 pxtalk_data->target_width_phase; 02267 02268 if (pxtalk_data->target_start_phase < 0) 02269 pxtalk_data->target_start_phase = 0; 02270 02271 02272 pxtalk_data->target_start = 02273 (uint8_t)(pxtalk_data->target_start_phase / 2048); 02274 02275 02276 if (pxtalk_data->VL53L1_p_016 > (pxtalk_data->target_start-1)) 02277 pxtalk_data->VL53L1_p_016 = pxtalk_data->target_start-1; 02278 02279 02280 pxtalk_data->effective_width = 02281 (2048 * ((int32_t)pxtalk_data->VL53L1_p_016+1)); 02282 pxtalk_data->effective_width -= pxtalk_data->xtalk_start_phase; 02283 02284 02285 if (pxtalk_data->effective_width > pxtalk_data->xtalk_width_phase) 02286 pxtalk_data->effective_width = pxtalk_data->xtalk_width_phase; 02287 02288 if (pxtalk_data->effective_width < 1) 02289 pxtalk_data->effective_width = 1; 02290 02291 02292 pxtalk_data->event_scaler = pxtalk_data->xtalk_width_phase * 1000; 02293 pxtalk_data->event_scaler += (pxtalk_data->effective_width / 2); 02294 pxtalk_data->event_scaler /= pxtalk_data->effective_width; 02295 02296 02297 if (pxtalk_data->event_scaler < 1000) 02298 pxtalk_data->event_scaler = 1000; 02299 02300 if (pxtalk_data->event_scaler > 4000) 02301 pxtalk_data->event_scaler = 4000; 02302 02303 02304 pxtalk_data->event_scaler_sum += pxtalk_data->event_scaler; 02305 02306 02307 pxtalk_data->peak_duration_us_sum += 02308 (uint32_t)phist_bins->peak_duration_us; 02309 02310 02311 pxtalk_data->effective_spad_count_sum += 02312 (uint32_t)phist_bins->result__dss_actual_effective_spads; 02313 02314 02315 pxtalk_data->zero_distance_phase_sum += 02316 (uint32_t)phist_bins->zero_distance_phase; 02317 02318 LOG_FUNCTION_END(status); 02319 02320 return status; 02321 } 02322 02323 02324 VL53L1_Error VL53L1_hist_xtalk_extract_calc_event_sums( 02325 VL53L1_histogram_bin_data_t *phist_bins, 02326 VL53L1_hist_xtalk_extract_data_t *pxtalk_data) 02327 { 02328 02329 02330 02331 VL53L1_Error status = VL53L1_ERROR_NONE; 02332 02333 uint8_t lb = 0; 02334 uint8_t i = 0; 02335 02336 LOG_FUNCTION_START(""); 02337 02338 02339 02340 for (lb = pxtalk_data->VL53L1_p_015; 02341 lb <= pxtalk_data->VL53L1_p_016; 02342 lb++) { 02343 02344 02345 i = (lb + phist_bins->number_of_ambient_bins + 02346 phist_bins->VL53L1_p_024) % 02347 phist_bins->VL53L1_p_024; 02348 02349 02350 pxtalk_data->signal_events_sum += phist_bins->bin_data[i]; 02351 pxtalk_data->signal_events_sum -= 02352 phist_bins->VL53L1_p_004; 02353 } 02354 02355 02356 02357 for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS && 02358 lb < phist_bins->VL53L1_p_024; lb++) { 02359 02360 02361 i = (lb + phist_bins->number_of_ambient_bins + 02362 phist_bins->VL53L1_p_024) % 02363 phist_bins->VL53L1_p_024; 02364 02365 02366 pxtalk_data->bin_data_sums[lb] += phist_bins->bin_data[i]; 02367 pxtalk_data->bin_data_sums[lb] -= 02368 phist_bins->VL53L1_p_004; 02369 } 02370 02371 pxtalk_data->sample_count += 1; 02372 02373 LOG_FUNCTION_END(status); 02374 02375 return status; 02376 } 02377 02378 02379 VL53L1_Error VL53L1_hist_xtalk_extract_calc_rate_per_spad( 02380 VL53L1_hist_xtalk_extract_data_t *pxtalk_data) 02381 { 02382 02383 02384 02385 VL53L1_Error status = VL53L1_ERROR_NONE; 02386 02387 uint64_t tmp64_0 = 0; 02388 uint64_t tmp64_1 = 0; 02389 uint64_t xtalk_per_spad = 0; 02390 02391 LOG_FUNCTION_START(""); 02392 02393 02394 02395 02396 02397 02398 if (pxtalk_data->signal_events_sum > 0) { 02399 tmp64_0 = 02400 ((uint64_t)pxtalk_data->signal_events_sum * 02401 (uint64_t)pxtalk_data->sample_count * 02402 (uint64_t)pxtalk_data->event_scaler_avg * 256U) << 9U; 02403 tmp64_1 = 02404 (uint64_t)pxtalk_data->effective_spad_count_sum * 02405 (uint64_t)pxtalk_data->peak_duration_us_sum; 02406 02407 02408 02409 if (tmp64_1 > 0U) { 02410 02411 tmp64_0 = tmp64_0 + (tmp64_1 >> 1U); 02412 xtalk_per_spad = do_division_u(tmp64_0, tmp64_1); 02413 } else { 02414 xtalk_per_spad = (uint64_t)tmp64_0; 02415 } 02416 02417 } else { 02418 status = VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL; 02419 } 02420 02421 pxtalk_data->xtalk_rate_kcps_per_spad = (uint32_t)xtalk_per_spad; 02422 02423 LOG_FUNCTION_END(status); 02424 02425 return status; 02426 } 02427 02428 02429 VL53L1_Error VL53L1_hist_xtalk_extract_calc_shape( 02430 VL53L1_hist_xtalk_extract_data_t *pxtalk_data, 02431 VL53L1_xtalk_histogram_shape_t *pxtalk_shape) 02432 { 02433 02434 02435 VL53L1_Error status = VL53L1_ERROR_NONE; 02436 02437 int32_t lb = 0; 02438 uint64_t total_events = 0U; 02439 uint64_t tmp64_0 = 0U; 02440 int32_t remaining_area = 1024; 02441 02442 LOG_FUNCTION_START(""); 02443 02444 02445 02446 pxtalk_shape->VL53L1_p_022 = 0; 02447 pxtalk_shape->VL53L1_p_023 = VL53L1_XTALK_HISTO_BINS; 02448 pxtalk_shape->VL53L1_p_024 = VL53L1_XTALK_HISTO_BINS; 02449 02450 pxtalk_shape->zero_distance_phase = 02451 (uint16_t)pxtalk_data->zero_distance_phase_avg; 02452 pxtalk_shape->phasecal_result__reference_phase = 02453 (uint16_t)pxtalk_data->zero_distance_phase_avg + (3*2048); 02454 02455 02456 02457 if (pxtalk_data->signal_events_sum > 0) 02458 total_events = 02459 (uint64_t)pxtalk_data->signal_events_sum * 02460 (uint64_t)pxtalk_data->event_scaler_avg; 02461 else 02462 total_events = 1; 02463 if (total_events == 0) 02464 total_events = 1; 02465 02466 02467 remaining_area = 1024; 02468 pxtalk_data->max_shape_value = 0; 02469 02470 for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS; lb++) { 02471 02472 if ((lb < (int32_t)pxtalk_data->VL53L1_p_015 || 02473 lb > (int32_t)pxtalk_data->VL53L1_p_016) || 02474 pxtalk_data->bin_data_sums[lb] < 0) { 02475 02476 02477 if (remaining_area > 0 && remaining_area < 1024) { 02478 if (remaining_area > 02479 pxtalk_data->max_shape_value) { 02480 pxtalk_shape->bin_data[lb] = 02481 (uint32_t)pxtalk_data->max_shape_value; 02482 remaining_area -= 02483 pxtalk_data->max_shape_value; 02484 } else { 02485 pxtalk_shape->bin_data[lb] = 02486 (uint32_t)remaining_area; 02487 remaining_area = 0; 02488 } 02489 } else { 02490 pxtalk_shape->bin_data[lb] = 0; 02491 } 02492 02493 } else { 02494 02495 tmp64_0 = 02496 (uint64_t)pxtalk_data->bin_data_sums[lb] 02497 * 1024U * 1000U; 02498 tmp64_0 += (total_events >> 1); 02499 tmp64_0 = do_division_u(tmp64_0, total_events); 02500 if (tmp64_0 > 0xFFFFU) 02501 tmp64_0 = 0xFFFFU; 02502 02503 pxtalk_shape->bin_data[lb] = (uint32_t)tmp64_0; 02504 02505 02506 if ((int32_t)pxtalk_shape->bin_data[lb] > 02507 pxtalk_data->max_shape_value) 02508 pxtalk_data->max_shape_value = 02509 (int32_t)pxtalk_shape->bin_data[lb]; 02510 02511 remaining_area -= (int32_t)pxtalk_shape->bin_data[lb]; 02512 } 02513 } 02514 02515 LOG_FUNCTION_END(status); 02516 02517 return status; 02518 } 02519 02520 02521 VL53L1_Error VL53L1_hist_xtalk_shape_model( 02522 uint16_t events_per_bin, 02523 uint16_t pulse_centre, 02524 uint16_t pulse_width, 02525 VL53L1_xtalk_histogram_shape_t *pxtalk_shape) 02526 { 02527 02528 02529 VL53L1_Error status = VL53L1_ERROR_NONE; 02530 02531 uint32_t phase_start = 0; 02532 uint32_t phase_stop = 0; 02533 uint32_t phase_bin = 0; 02534 02535 uint32_t bin_start = 0; 02536 uint32_t bin_stop = 0; 02537 02538 uint32_t lb = 0; 02539 uint16_t VL53L1_p_008 = 0; 02540 02541 LOG_FUNCTION_START(""); 02542 02543 02544 02545 pxtalk_shape->VL53L1_p_022 = 0; 02546 pxtalk_shape->VL53L1_p_023 = VL53L1_XTALK_HISTO_BINS; 02547 pxtalk_shape->VL53L1_p_024 = VL53L1_XTALK_HISTO_BINS; 02548 02549 pxtalk_shape->zero_distance_phase = pulse_centre; 02550 pxtalk_shape->phasecal_result__reference_phase = 02551 pulse_centre + (3*2048); 02552 02553 02554 if (pulse_centre > (pulse_width >> 1)) 02555 phase_start = (uint32_t)pulse_centre - 02556 ((uint32_t)pulse_width >> 1); 02557 else 02558 phase_start = 0; 02559 02560 phase_stop = (uint32_t)pulse_centre + 02561 ((uint32_t)pulse_width >> 1); 02562 02563 02564 bin_start = (phase_start / 2048); 02565 bin_stop = (phase_stop / 2048); 02566 02567 for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS; lb++) { 02568 VL53L1_p_008 = 0; 02569 02570 02571 if (lb == bin_start && lb == bin_stop) { 02572 VL53L1_p_008 = 02573 VL53L1_hist_xtalk_shape_model_interp( 02574 events_per_bin, 02575 phase_stop - phase_start); 02576 02577 } else if (lb > bin_start && lb < bin_stop) { 02578 02579 02580 VL53L1_p_008 = events_per_bin; 02581 02582 } else if (lb == bin_start) { 02583 02584 02585 phase_bin = (lb + 1) * 2048; 02586 VL53L1_p_008 = 02587 VL53L1_hist_xtalk_shape_model_interp( 02588 events_per_bin, 02589 (phase_bin - phase_start)); 02590 02591 } else if (lb == bin_stop) { 02592 02593 02594 phase_bin = lb * 2048; 02595 VL53L1_p_008 = 02596 VL53L1_hist_xtalk_shape_model_interp( 02597 events_per_bin, 02598 (phase_stop - phase_bin)); 02599 } 02600 02601 pxtalk_shape->bin_data[lb] = VL53L1_p_008; 02602 } 02603 02604 LOG_FUNCTION_END(status); 02605 02606 return status; 02607 } 02608 02609 02610 uint16_t VL53L1_hist_xtalk_shape_model_interp( 02611 uint16_t events_per_bin, 02612 uint32_t phase_delta) 02613 { 02614 02615 02616 uint32_t VL53L1_p_008 = 0; 02617 02618 LOG_FUNCTION_START(""); 02619 02620 02621 VL53L1_p_008 = (uint32_t)events_per_bin * phase_delta; 02622 VL53L1_p_008 += 1024; 02623 VL53L1_p_008 /= 2048; 02624 02625 02626 if (VL53L1_p_008 > 0xFFFFU) 02627 VL53L1_p_008 = 0xFFFFU; 02628 02629 LOG_FUNCTION_END(0); 02630 02631 return (uint16_t)VL53L1_p_008; 02632 } 02633 02634 02635 void VL53L1_spad_number_to_byte_bit_index( 02636 uint8_t spad_number, 02637 uint8_t *pbyte_index, 02638 uint8_t *pbit_index, 02639 uint8_t *pbit_mask) 02640 { 02641 02642 02643 02644 *pbyte_index = spad_number >> 3; 02645 *pbit_index = spad_number & 0x07; 02646 *pbit_mask = 0x01 << *pbit_index; 02647 02648 } 02649 02650 02651 void VL53L1_encode_row_col( 02652 uint8_t row, 02653 uint8_t col, 02654 uint8_t *pspad_number) 02655 { 02656 02657 02658 if (row > 7) 02659 *pspad_number = 128 + (col << 3) + (15-row); 02660 else 02661 *pspad_number = ((15-col) << 3) + row; 02662 02663 } 02664 02665 02666 void VL53L1_decode_zone_size( 02667 uint8_t encoded_xy_size, 02668 uint8_t *pwidth, 02669 uint8_t *pheight) 02670 { 02671 02672 02673 02674 *pheight = encoded_xy_size >> 4; 02675 *pwidth = encoded_xy_size & 0x0F; 02676 02677 } 02678 02679 02680 void VL53L1_encode_zone_size( 02681 uint8_t width, 02682 uint8_t height, 02683 uint8_t *pencoded_xy_size) 02684 { 02685 02686 02687 *pencoded_xy_size = (height << 4) + width; 02688 02689 } 02690 02691 02692 void VL53L1_decode_zone_limits( 02693 uint8_t encoded_xy_centre, 02694 uint8_t encoded_xy_size, 02695 int16_t *px_ll, 02696 int16_t *py_ll, 02697 int16_t *px_ur, 02698 int16_t *py_ur) 02699 { 02700 02701 02702 02703 uint8_t x_centre = 0; 02704 uint8_t y_centre = 0; 02705 uint8_t width = 0; 02706 uint8_t height = 0; 02707 02708 02709 02710 VL53L1_decode_row_col( 02711 encoded_xy_centre, 02712 &y_centre, 02713 &x_centre); 02714 02715 VL53L1_decode_zone_size( 02716 encoded_xy_size, 02717 &width, 02718 &height); 02719 02720 02721 02722 *px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2; 02723 if (*px_ll < 0) 02724 *px_ll = 0; 02725 02726 *px_ur = *px_ll + (int16_t)width; 02727 if (*px_ur > (VL53L1_SPAD_ARRAY_WIDTH-1)) 02728 *px_ur = VL53L1_SPAD_ARRAY_WIDTH-1; 02729 02730 *py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2; 02731 if (*py_ll < 0) 02732 *py_ll = 0; 02733 02734 *py_ur = *py_ll + (int16_t)height; 02735 if (*py_ur > (VL53L1_SPAD_ARRAY_HEIGHT-1)) 02736 *py_ur = VL53L1_SPAD_ARRAY_HEIGHT-1; 02737 } 02738 02739 02740 uint8_t VL53L1_is_aperture_location( 02741 uint8_t row, 02742 uint8_t col) 02743 { 02744 02745 02746 uint8_t is_aperture = 0; 02747 uint8_t mod_row = row % 4; 02748 uint8_t mod_col = col % 4; 02749 02750 if (mod_row == 0 && mod_col == 2) 02751 is_aperture = 1; 02752 02753 if (mod_row == 2 && mod_col == 0) 02754 is_aperture = 1; 02755 02756 return is_aperture; 02757 } 02758 02759 02760 void VL53L1_calc_max_effective_spads( 02761 uint8_t encoded_zone_centre, 02762 uint8_t encoded_zone_size, 02763 uint8_t *pgood_spads, 02764 uint16_t aperture_attenuation, 02765 uint16_t *pmax_effective_spads) 02766 { 02767 02768 02769 02770 int16_t x = 0; 02771 int16_t y = 0; 02772 02773 int16_t zone_x_ll = 0; 02774 int16_t zone_y_ll = 0; 02775 int16_t zone_x_ur = 0; 02776 int16_t zone_y_ur = 0; 02777 02778 uint8_t spad_number = 0; 02779 uint8_t byte_index = 0; 02780 uint8_t bit_index = 0; 02781 uint8_t bit_mask = 0; 02782 02783 uint8_t is_aperture = 0; 02784 02785 02786 02787 VL53L1_decode_zone_limits( 02788 encoded_zone_centre, 02789 encoded_zone_size, 02790 &zone_x_ll, 02791 &zone_y_ll, 02792 &zone_x_ur, 02793 &zone_y_ur); 02794 02795 02796 02797 *pmax_effective_spads = 0; 02798 02799 for (y = zone_y_ll; y <= zone_y_ur; y++) { 02800 for (x = zone_x_ll; x <= zone_x_ur; x++) { 02801 02802 02803 02804 VL53L1_encode_row_col( 02805 (uint8_t)y, 02806 (uint8_t)x, 02807 &spad_number); 02808 02809 02810 02811 VL53L1_spad_number_to_byte_bit_index( 02812 spad_number, 02813 &byte_index, 02814 &bit_index, 02815 &bit_mask); 02816 02817 02818 02819 if ((pgood_spads[byte_index] & bit_mask) > 0) { 02820 02821 02822 is_aperture = VL53L1_is_aperture_location( 02823 (uint8_t)y, 02824 (uint8_t)x); 02825 02826 if (is_aperture > 0) 02827 *pmax_effective_spads += 02828 aperture_attenuation; 02829 else 02830 *pmax_effective_spads += 0x0100; 02831 02832 } 02833 } 02834 } 02835 } 02836 02837 02838 void VL53L1_calc_mm_effective_spads( 02839 uint8_t encoded_mm_roi_centre, 02840 uint8_t encoded_mm_roi_size, 02841 uint8_t encoded_zone_centre, 02842 uint8_t encoded_zone_size, 02843 uint8_t *pgood_spads, 02844 uint16_t aperture_attenuation, 02845 uint16_t *pmm_inner_effective_spads, 02846 uint16_t *pmm_outer_effective_spads) 02847 { 02848 02849 02850 02851 int16_t x = 0; 02852 int16_t y = 0; 02853 02854 int16_t mm_x_ll = 0; 02855 int16_t mm_y_ll = 0; 02856 int16_t mm_x_ur = 0; 02857 int16_t mm_y_ur = 0; 02858 02859 int16_t zone_x_ll = 0; 02860 int16_t zone_y_ll = 0; 02861 int16_t zone_x_ur = 0; 02862 int16_t zone_y_ur = 0; 02863 02864 uint8_t spad_number = 0; 02865 uint8_t byte_index = 0; 02866 uint8_t bit_index = 0; 02867 uint8_t bit_mask = 0; 02868 02869 uint8_t is_aperture = 0; 02870 uint16_t spad_attenuation = 0; 02871 02872 02873 02874 VL53L1_decode_zone_limits( 02875 encoded_mm_roi_centre, 02876 encoded_mm_roi_size, 02877 &mm_x_ll, 02878 &mm_y_ll, 02879 &mm_x_ur, 02880 &mm_y_ur); 02881 02882 VL53L1_decode_zone_limits( 02883 encoded_zone_centre, 02884 encoded_zone_size, 02885 &zone_x_ll, 02886 &zone_y_ll, 02887 &zone_x_ur, 02888 &zone_y_ur); 02889 02890 02891 02892 *pmm_inner_effective_spads = 0; 02893 *pmm_outer_effective_spads = 0; 02894 02895 for (y = zone_y_ll; y <= zone_y_ur; y++) { 02896 for (x = zone_x_ll; x <= zone_x_ur; x++) { 02897 02898 02899 02900 VL53L1_encode_row_col( 02901 (uint8_t)y, 02902 (uint8_t)x, 02903 &spad_number); 02904 02905 02906 02907 VL53L1_spad_number_to_byte_bit_index( 02908 spad_number, 02909 &byte_index, 02910 &bit_index, 02911 &bit_mask); 02912 02913 02914 02915 if ((pgood_spads[byte_index] & bit_mask) > 0) { 02916 02917 02918 is_aperture = VL53L1_is_aperture_location( 02919 (uint8_t)y, 02920 (uint8_t)x); 02921 02922 if (is_aperture > 0) 02923 spad_attenuation = aperture_attenuation; 02924 else 02925 spad_attenuation = 0x0100; 02926 02927 02928 02929 if (x >= mm_x_ll && x <= mm_x_ur && 02930 y >= mm_y_ll && y <= mm_y_ur) 02931 *pmm_inner_effective_spads += 02932 spad_attenuation; 02933 else 02934 *pmm_outer_effective_spads += 02935 spad_attenuation; 02936 } 02937 } 02938 } 02939 } 02940 02941 02942 void VL53L1_hist_copy_results_to_sys_and_core( 02943 VL53L1_histogram_bin_data_t *pbins, 02944 VL53L1_range_results_t *phist, 02945 VL53L1_system_results_t *psys, 02946 VL53L1_core_results_t *pcore) 02947 { 02948 02949 02950 uint8_t i = 0; 02951 02952 VL53L1_range_data_t *pdata; 02953 02954 LOG_FUNCTION_START(""); 02955 02956 02957 02958 VL53L1_init_system_results(psys); 02959 02960 02961 02962 psys->result__interrupt_status = pbins->result__interrupt_status; 02963 psys->result__range_status = phist->active_results; 02964 psys->result__report_status = pbins->result__report_status; 02965 psys->result__stream_count = pbins->result__stream_count; 02966 02967 pdata = &(phist->VL53L1_p_002[0]); 02968 02969 for (i = 0; i < phist->active_results; i++) { 02970 02971 switch (i) { 02972 case 0: 02973 psys->result__dss_actual_effective_spads_sd0 = 02974 pdata->VL53L1_p_006; 02975 psys->result__peak_signal_count_rate_mcps_sd0 = 02976 pdata->peak_signal_count_rate_mcps; 02977 psys->result__avg_signal_count_rate_mcps_sd0 = 02978 pdata->avg_signal_count_rate_mcps; 02979 psys->result__ambient_count_rate_mcps_sd0 = 02980 pdata->ambient_count_rate_mcps; 02981 02982 psys->result__sigma_sd0 = pdata->VL53L1_p_005; 02983 psys->result__phase_sd0 = pdata->VL53L1_p_014; 02984 02985 psys->result__final_crosstalk_corrected_range_mm_sd0 = 02986 (uint16_t)pdata->median_range_mm; 02987 02988 psys->result__phase_sd1 = pdata->zero_distance_phase; 02989 02990 pcore->result_core__ranging_total_events_sd0 = 02991 pdata->VL53L1_p_021; 02992 pcore->result_core__signal_total_events_sd0 = 02993 pdata->VL53L1_p_013; 02994 pcore->result_core__total_periods_elapsed_sd0 = 02995 pdata->total_periods_elapsed; 02996 pcore->result_core__ambient_window_events_sd0 = 02997 pdata->VL53L1_p_020; 02998 02999 break; 03000 case 1: 03001 psys->result__dss_actual_effective_spads_sd1 = 03002 pdata->VL53L1_p_006; 03003 psys->result__peak_signal_count_rate_mcps_sd1 = 03004 pdata->peak_signal_count_rate_mcps; 03005 psys->result__ambient_count_rate_mcps_sd1 = 03006 pdata->ambient_count_rate_mcps; 03007 03008 psys->result__sigma_sd1 = pdata->VL53L1_p_005; 03009 psys->result__phase_sd1 = pdata->VL53L1_p_014; 03010 03011 psys->result__final_crosstalk_corrected_range_mm_sd1 = 03012 (uint16_t)pdata->median_range_mm; 03013 03014 pcore->result_core__ranging_total_events_sd1 = 03015 pdata->VL53L1_p_021; 03016 pcore->result_core__signal_total_events_sd1 = 03017 pdata->VL53L1_p_013; 03018 pcore->result_core__total_periods_elapsed_sd1 = 03019 pdata->total_periods_elapsed; 03020 pcore->result_core__ambient_window_events_sd1 = 03021 pdata->VL53L1_p_020; 03022 break; 03023 } 03024 03025 pdata++; 03026 } 03027 03028 LOG_FUNCTION_END(0); 03029 03030 } 03031 03032 03033 VL53L1_Error VL53L1_sum_histogram_data( 03034 VL53L1_histogram_bin_data_t *phist_input, 03035 VL53L1_histogram_bin_data_t *phist_output) 03036 { 03037 03038 03039 VL53L1_Error status = VL53L1_ERROR_NONE; 03040 03041 uint8_t i = 0; 03042 uint8_t smallest_bin_num = 0; 03043 03044 LOG_FUNCTION_START(""); 03045 03046 03047 03048 if (status == VL53L1_ERROR_NONE) { 03049 if (phist_output->VL53L1_p_024 >= 03050 phist_input->VL53L1_p_024) 03051 smallest_bin_num = phist_input->VL53L1_p_024; 03052 else 03053 smallest_bin_num = phist_output->VL53L1_p_024; 03054 } 03055 03056 03057 03058 03059 03060 if (status == VL53L1_ERROR_NONE) 03061 for (i = 0; i < smallest_bin_num; i++) 03062 03063 phist_output->bin_data[i] += phist_input->bin_data[i]; 03064 03065 if (status == VL53L1_ERROR_NONE) 03066 phist_output->VL53L1_p_004 += 03067 phist_input->VL53L1_p_004; 03068 03069 03070 LOG_FUNCTION_END(status); 03071 03072 return status; 03073 } 03074 03075 03076 VL53L1_Error VL53L1_avg_histogram_data( 03077 uint8_t no_of_samples, 03078 VL53L1_histogram_bin_data_t *phist_sum, 03079 VL53L1_histogram_bin_data_t *phist_avg) 03080 { 03081 03082 03083 VL53L1_Error status = VL53L1_ERROR_NONE; 03084 03085 uint8_t i = 0; 03086 03087 LOG_FUNCTION_START(""); 03088 03089 03090 03091 03092 03093 if (status == VL53L1_ERROR_NONE) { 03094 for (i = 0; i < phist_sum->VL53L1_p_024; i++) { 03095 03096 03097 03098 if (no_of_samples > 0) 03099 phist_avg->bin_data[i] = 03100 phist_sum->bin_data[i] / 03101 (int32_t)no_of_samples; 03102 else 03103 phist_avg->bin_data[i] = phist_sum->bin_data[i]; 03104 } 03105 } 03106 03107 if (status == VL53L1_ERROR_NONE) { 03108 if (no_of_samples > 0) 03109 phist_avg->VL53L1_p_004 = 03110 phist_sum->VL53L1_p_004 / 03111 (int32_t)no_of_samples; 03112 else 03113 phist_avg->VL53L1_p_004 = 03114 phist_sum->VL53L1_p_004; 03115 } 03116 03117 LOG_FUNCTION_END(status); 03118 03119 return status; 03120 } 03121 03122 03123 VL53L1_Error VL53L1_save_cfg_data( 03124 VL53L1_DEV Dev) 03125 { 03126 03127 03128 VL53L1_Error status = VL53L1_ERROR_NONE; 03129 03130 VL53L1_LLDriverData_t *pdev = 03131 VL53L1DevStructGetLLDriverHandle(Dev); 03132 VL53L1_LLDriverResults_t *pres = 03133 VL53L1DevStructGetLLResultsHandle(Dev); 03134 03135 VL53L1_zone_private_dyn_cfg_t *pzone_dyn_cfg; 03136 VL53L1_dynamic_config_t *pdynamic = &(pdev->dyn_cfg); 03137 03138 LOG_FUNCTION_START(""); 03139 03140 pzone_dyn_cfg = 03141 &(pres->zone_dyn_cfgs.VL53L1_p_002[pdev->ll_state.cfg_zone_id]); 03142 03143 pzone_dyn_cfg->expected_stream_count = 03144 pdev->ll_state.cfg_stream_count; 03145 03146 pzone_dyn_cfg->expected_gph_id = 03147 pdev->ll_state.cfg_gph_id; 03148 03149 pzone_dyn_cfg->roi_config__user_roi_centre_spad = 03150 pdynamic->roi_config__user_roi_centre_spad; 03151 03152 pzone_dyn_cfg->roi_config__user_roi_requested_global_xy_size = 03153 pdynamic->roi_config__user_roi_requested_global_xy_size; 03154 03155 LOG_FUNCTION_END(status); 03156 03157 return status; 03158 } 03159 03160 03161 VL53L1_Error VL53L1_dynamic_zone_update( 03162 VL53L1_DEV Dev, 03163 VL53L1_range_results_t *presults) 03164 { 03165 03166 03167 VL53L1_Error status = VL53L1_ERROR_NONE; 03168 03169 VL53L1_LLDriverData_t *pdev = 03170 VL53L1DevStructGetLLDriverHandle(Dev); 03171 VL53L1_LLDriverResults_t *pres = 03172 VL53L1DevStructGetLLResultsHandle(Dev); 03173 VL53L1_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs); 03174 03175 uint8_t zone_id = pdev->ll_state.rd_zone_id; 03176 uint8_t i; 03177 uint16_t max_total_rate_per_spads; 03178 uint16_t target_rate = 03179 pdev->stat_cfg.dss_config__target_total_rate_mcps; 03180 uint32_t temp = 0xFFFF; 03181 #ifdef VL53L1_LOG_ENABLE 03182 uint16_t eff_spad_cnt = 03183 pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count; 03184 #endif 03185 03186 LOG_FUNCTION_START(""); 03187 03188 pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count = 0; 03189 03190 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03191 " DYNZONEUPDATE: peak signal count rate mcps:"); 03192 03193 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03194 "%u actual effective spads: %u\n", 03195 presults->VL53L1_p_002[0].peak_signal_count_rate_mcps, 03196 presults->VL53L1_p_002[0].VL53L1_p_006); 03197 03198 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03199 " DYNZONEUPDATE: active results: %u\n", 03200 presults->active_results); 03201 03202 max_total_rate_per_spads = 03203 presults->VL53L1_p_002[0].total_rate_per_spad_mcps; 03204 03205 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03206 " DYNZONEUPDATE: max total rate per spad at start: %u\n", 03207 max_total_rate_per_spads); 03208 03209 for (i = 1; i < presults->active_results; i++) { 03210 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03211 " DYNZONEUPDATE: zone total rate per spad: zone_id: %u,", 03212 i); 03213 03214 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03215 "total rate per spad: %u\n", 03216 presults->VL53L1_p_002[i].total_rate_per_spad_mcps); 03217 03218 if (presults->VL53L1_p_002[i].total_rate_per_spad_mcps > 03219 max_total_rate_per_spads) 03220 max_total_rate_per_spads = 03221 presults->VL53L1_p_002[i].total_rate_per_spad_mcps; 03222 03223 } 03224 03225 if (max_total_rate_per_spads == 0) { 03226 03227 temp = 0xFFFF; 03228 } else { 03229 03230 temp = target_rate << 14; 03231 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03232 " DYNZONEUPDATE: 1: temp: %u\n", 03233 temp); 03234 03235 03236 temp = temp / max_total_rate_per_spads; 03237 03238 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03239 " DYNZONEUPDATE: 2: temp: %u\n", 03240 temp); 03241 03242 03243 if (temp > 0xFFFF) 03244 temp = 0xFFFF; 03245 03246 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03247 " DYNZONEUPDATE: 3: temp: %u\n", 03248 temp); 03249 } 03250 03251 pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count = 03252 (uint16_t)temp; 03253 03254 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03255 " DYNZONEUPDATE: zone_id: %u, target_rate: %u,", 03256 zone_id, 03257 target_rate); 03258 03259 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03260 "max_total_rate_per_spads: %u, requested_spads: %u\n", 03261 max_total_rate_per_spads, 03262 eff_spad_cnt); 03263 03264 LOG_FUNCTION_END(status); 03265 03266 return status; 03267 } 03268 03269 VL53L1_Error VL53L1_multizone_hist_bins_update( 03270 VL53L1_DEV Dev) 03271 { 03272 03273 03274 VL53L1_Error status = VL53L1_ERROR_NONE; 03275 03276 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 03277 VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); 03278 VL53L1_zone_config_t *pzone_cfg = &(pdev->zone_cfg); 03279 VL53L1_histogram_config_t *phist_cfg = &(pdev->hist_cfg); 03280 VL53L1_histogram_config_t *pmulti_hist = 03281 &(pzone_cfg->multizone_hist_cfg); 03282 03283 uint8_t next_range_is_odd_timing = (pstate->cfg_stream_count) % 2; 03284 03285 LOG_FUNCTION_START(""); 03286 03287 03288 if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] == 03289 VL53L1_ZONECONFIG_BINCONFIG__LOWAMB) { 03290 if (!next_range_is_odd_timing) { 03291 trace_print (VL53L1_TRACE_LEVEL_DEBUG, 03292 " HISTBINCONFIGUPDATE: Setting LOWAMB EVEN timing\n"); 03293 phist_cfg->histogram_config__low_amb_even_bin_0_1 = 03294 pmulti_hist->histogram_config__low_amb_even_bin_0_1; 03295 phist_cfg->histogram_config__low_amb_even_bin_2_3 = 03296 pmulti_hist->histogram_config__low_amb_even_bin_2_3; 03297 phist_cfg->histogram_config__low_amb_even_bin_4_5 = 03298 pmulti_hist->histogram_config__low_amb_even_bin_4_5; 03299 } 03300 03301 if (next_range_is_odd_timing) { 03302 trace_print (VL53L1_TRACE_LEVEL_DEBUG, 03303 " HISTBINCONFIGUPDATE: Setting LOWAMB ODD timing\n"); 03304 phist_cfg->histogram_config__low_amb_odd_bin_0_1 = 03305 pmulti_hist->histogram_config__low_amb_even_bin_0_1; 03306 phist_cfg->histogram_config__low_amb_odd_bin_2_3 = 03307 pmulti_hist->histogram_config__low_amb_even_bin_2_3; 03308 phist_cfg->histogram_config__low_amb_odd_bin_4_5 = 03309 pmulti_hist->histogram_config__low_amb_even_bin_4_5; 03310 } 03311 } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] == 03312 VL53L1_ZONECONFIG_BINCONFIG__MIDAMB) { 03313 trace_print (VL53L1_TRACE_LEVEL_DEBUG, 03314 " HISTBINCONFIGUPDATE: Setting MIDAMB timing\n"); 03315 if (!next_range_is_odd_timing) { 03316 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03317 " HISTBINCONFIGUPDATE: Setting MIDAMB EVEN timing\n"); 03318 phist_cfg->histogram_config__low_amb_even_bin_0_1 = 03319 pmulti_hist->histogram_config__mid_amb_even_bin_0_1; 03320 phist_cfg->histogram_config__low_amb_even_bin_2_3 = 03321 pmulti_hist->histogram_config__mid_amb_even_bin_2_3; 03322 phist_cfg->histogram_config__low_amb_even_bin_4_5 = 03323 pmulti_hist->histogram_config__mid_amb_even_bin_4_5; 03324 } 03325 03326 if (next_range_is_odd_timing) { 03327 trace_print (VL53L1_TRACE_LEVEL_DEBUG, 03328 " HISTBINCONFIGUPDATE: Setting MIDAMB ODD timing\n"); 03329 phist_cfg->histogram_config__low_amb_odd_bin_0_1 = 03330 pmulti_hist->histogram_config__mid_amb_even_bin_0_1; 03331 phist_cfg->histogram_config__low_amb_odd_bin_2_3 = 03332 pmulti_hist->histogram_config__mid_amb_even_bin_2_3; 03333 phist_cfg->histogram_config__low_amb_odd_bin_4_5 = 03334 pmulti_hist->histogram_config__mid_amb_even_bin_4_5; 03335 } 03336 } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] == 03337 VL53L1_ZONECONFIG_BINCONFIG__HIGHAMB) { 03338 if (!next_range_is_odd_timing) { 03339 trace_print (VL53L1_TRACE_LEVEL_DEBUG, 03340 " HISTBINCONFIGUPDATE: Setting HIGHAMB EVEN timing\n" 03341 ); 03342 phist_cfg->histogram_config__low_amb_even_bin_0_1 = 03343 pmulti_hist->histogram_config__high_amb_even_bin_0_1; 03344 phist_cfg->histogram_config__low_amb_even_bin_2_3 = 03345 pmulti_hist->histogram_config__high_amb_even_bin_2_3; 03346 phist_cfg->histogram_config__low_amb_even_bin_4_5 = 03347 pmulti_hist->histogram_config__high_amb_even_bin_4_5; 03348 } 03349 03350 if (next_range_is_odd_timing) { 03351 trace_print (VL53L1_TRACE_LEVEL_DEBUG, 03352 " HISTBINCONFIGUPDATE: Setting HIGHAMB ODD timing\n"); 03353 phist_cfg->histogram_config__low_amb_odd_bin_0_1 = 03354 pmulti_hist->histogram_config__high_amb_even_bin_0_1; 03355 phist_cfg->histogram_config__low_amb_odd_bin_2_3 = 03356 pmulti_hist->histogram_config__high_amb_even_bin_2_3; 03357 phist_cfg->histogram_config__low_amb_odd_bin_4_5 = 03358 pmulti_hist->histogram_config__high_amb_even_bin_4_5; 03359 } 03360 } 03361 03362 03363 03364 if (status == VL53L1_ERROR_NONE) { 03365 VL53L1_copy_hist_bins_to_static_cfg( 03366 phist_cfg, 03367 &(pdev->stat_cfg), 03368 &(pdev->tim_cfg)); 03369 } 03370 03371 LOG_FUNCTION_END(status); 03372 03373 return status; 03374 } 03375 03376 03377 03378 VL53L1_Error VL53L1_update_internal_stream_counters( 03379 VL53L1_DEV Dev, 03380 uint8_t external_stream_count, 03381 uint8_t *pinternal_stream_count, 03382 uint8_t *pinternal_stream_count_val) 03383 { 03384 03385 VL53L1_Error status = VL53L1_ERROR_NONE; 03386 uint8_t stream_divider; 03387 03388 VL53L1_LLDriverData_t *pdev = 03389 VL53L1DevStructGetLLDriverHandle(Dev); 03390 03391 LOG_FUNCTION_START(""); 03392 03393 stream_divider = pdev->gen_cfg.global_config__stream_divider; 03394 03395 if (stream_divider == 0) { 03396 03397 03398 *pinternal_stream_count = external_stream_count; 03399 03400 } else if (*pinternal_stream_count_val == (stream_divider-1)) { 03401 03402 03403 if (*pinternal_stream_count == 0xFF) 03404 *pinternal_stream_count = 0x80; 03405 else 03406 *pinternal_stream_count = *pinternal_stream_count + 1; 03407 03408 03409 *pinternal_stream_count_val = 0; 03410 03411 } else { 03412 03413 03414 *pinternal_stream_count_val = *pinternal_stream_count_val + 1; 03415 } 03416 03417 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03418 "UPDINTSTREAMCOUNT internal_steam_count: %d,", 03419 *pinternal_stream_count); 03420 03421 trace_print(VL53L1_TRACE_LEVEL_DEBUG, 03422 "internal_stream_count_val: %d, divider: %d\n", 03423 *pinternal_stream_count_val, 03424 stream_divider); 03425 03426 LOG_FUNCTION_END(status); 03427 03428 return status; 03429 } 03430 03431 03432 03433 VL53L1_Error VL53L1_set_histogram_multizone_initial_bin_config( 03434 VL53L1_zone_config_t *pzone_cfg, 03435 VL53L1_histogram_config_t *phist_cfg, 03436 VL53L1_histogram_config_t *pmulti_hist) 03437 { 03438 VL53L1_Error status = VL53L1_ERROR_NONE; 03439 03440 LOG_FUNCTION_START(""); 03441 03442 03443 if (pzone_cfg->bin_config[0] == 03444 VL53L1_ZONECONFIG_BINCONFIG__LOWAMB) { 03445 phist_cfg->histogram_config__low_amb_even_bin_0_1 = 03446 pmulti_hist->histogram_config__low_amb_even_bin_0_1; 03447 phist_cfg->histogram_config__low_amb_even_bin_2_3 = 03448 pmulti_hist->histogram_config__low_amb_even_bin_2_3; 03449 phist_cfg->histogram_config__low_amb_even_bin_4_5 = 03450 pmulti_hist->histogram_config__low_amb_even_bin_4_5; 03451 03452 phist_cfg->histogram_config__low_amb_odd_bin_0_1 = 03453 pmulti_hist->histogram_config__low_amb_even_bin_0_1; 03454 phist_cfg->histogram_config__low_amb_odd_bin_2_3 = 03455 pmulti_hist->histogram_config__low_amb_even_bin_2_3; 03456 phist_cfg->histogram_config__low_amb_odd_bin_4_5 = 03457 pmulti_hist->histogram_config__low_amb_even_bin_4_5; 03458 } else if (pzone_cfg->bin_config[0] == 03459 VL53L1_ZONECONFIG_BINCONFIG__MIDAMB) { 03460 phist_cfg->histogram_config__low_amb_even_bin_0_1 = 03461 pmulti_hist->histogram_config__mid_amb_even_bin_0_1; 03462 phist_cfg->histogram_config__low_amb_even_bin_2_3 = 03463 pmulti_hist->histogram_config__mid_amb_even_bin_2_3; 03464 phist_cfg->histogram_config__low_amb_even_bin_4_5 = 03465 pmulti_hist->histogram_config__mid_amb_even_bin_4_5; 03466 03467 phist_cfg->histogram_config__low_amb_odd_bin_0_1 = 03468 pmulti_hist->histogram_config__mid_amb_even_bin_0_1; 03469 phist_cfg->histogram_config__low_amb_odd_bin_2_3 = 03470 pmulti_hist->histogram_config__mid_amb_even_bin_2_3; 03471 phist_cfg->histogram_config__low_amb_odd_bin_4_5 = 03472 pmulti_hist->histogram_config__mid_amb_even_bin_4_5; 03473 } else if (pzone_cfg->bin_config[0] == 03474 VL53L1_ZONECONFIG_BINCONFIG__HIGHAMB) { 03475 phist_cfg->histogram_config__low_amb_even_bin_0_1 = 03476 pmulti_hist->histogram_config__high_amb_even_bin_0_1; 03477 phist_cfg->histogram_config__low_amb_even_bin_2_3 = 03478 pmulti_hist->histogram_config__high_amb_even_bin_2_3; 03479 phist_cfg->histogram_config__low_amb_even_bin_4_5 = 03480 pmulti_hist->histogram_config__high_amb_even_bin_4_5; 03481 phist_cfg->histogram_config__low_amb_odd_bin_0_1 = 03482 pmulti_hist->histogram_config__high_amb_even_bin_0_1; 03483 phist_cfg->histogram_config__low_amb_odd_bin_2_3 = 03484 pmulti_hist->histogram_config__high_amb_even_bin_2_3; 03485 phist_cfg->histogram_config__low_amb_odd_bin_4_5 = 03486 pmulti_hist->histogram_config__high_amb_even_bin_4_5; 03487 } 03488 03489 LOG_FUNCTION_END(status); 03490 return status; 03491 } 03492 03493 03494 03495 uint8_t VL53L1_encode_GPIO_interrupt_config( 03496 VL53L1_GPIO_interrupt_config_t *pintconf) 03497 { 03498 uint8_t system__interrupt_config; 03499 03500 system__interrupt_config = pintconf->intr_mode_distance; 03501 system__interrupt_config |= ((pintconf->intr_mode_rate) << 2); 03502 system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5); 03503 system__interrupt_config |= ((pintconf->intr_no_target) << 6); 03504 system__interrupt_config |= ((pintconf->intr_combined_mode) << 7); 03505 03506 return system__interrupt_config; 03507 } 03508 03509 03510 03511 VL53L1_GPIO_interrupt_config_t VL53L1_decode_GPIO_interrupt_config( 03512 uint8_t system__interrupt_config) 03513 { 03514 VL53L1_GPIO_interrupt_config_t intconf; 03515 03516 intconf.intr_mode_distance = system__interrupt_config & 0x03; 03517 intconf.intr_mode_rate = (system__interrupt_config >> 2) & 0x03; 03518 intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01; 03519 intconf.intr_no_target = (system__interrupt_config >> 6) & 0x01; 03520 intconf.intr_combined_mode = (system__interrupt_config >> 7) & 0x01; 03521 03522 03523 intconf.threshold_rate_low = 0; 03524 intconf.threshold_rate_high = 0; 03525 intconf.threshold_distance_low = 0; 03526 intconf.threshold_distance_high = 0; 03527 03528 return intconf; 03529 } 03530 03531 03532 03533 VL53L1_Error VL53L1_set_GPIO_distance_threshold( 03534 VL53L1_DEV Dev, 03535 uint16_t threshold_high, 03536 uint16_t threshold_low) 03537 { 03538 VL53L1_Error status = VL53L1_ERROR_NONE; 03539 03540 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 03541 03542 LOG_FUNCTION_START(""); 03543 03544 pdev->dyn_cfg.system__thresh_high = threshold_high; 03545 pdev->dyn_cfg.system__thresh_low = threshold_low; 03546 03547 LOG_FUNCTION_END(status); 03548 return status; 03549 } 03550 03551 03552 03553 VL53L1_Error VL53L1_set_GPIO_rate_threshold( 03554 VL53L1_DEV Dev, 03555 uint16_t threshold_high, 03556 uint16_t threshold_low) 03557 { 03558 VL53L1_Error status = VL53L1_ERROR_NONE; 03559 03560 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 03561 03562 LOG_FUNCTION_START(""); 03563 03564 pdev->gen_cfg.system__thresh_rate_high = threshold_high; 03565 pdev->gen_cfg.system__thresh_rate_low = threshold_low; 03566 03567 LOG_FUNCTION_END(status); 03568 return status; 03569 } 03570 03571 03572 03573 VL53L1_Error VL53L1_set_GPIO_thresholds_from_struct( 03574 VL53L1_DEV Dev, 03575 VL53L1_GPIO_interrupt_config_t *pintconf) 03576 { 03577 VL53L1_Error status = VL53L1_ERROR_NONE; 03578 03579 LOG_FUNCTION_START(""); 03580 03581 status = VL53L1_set_GPIO_distance_threshold( 03582 Dev, 03583 pintconf->threshold_distance_high, 03584 pintconf->threshold_distance_low); 03585 03586 if (status == VL53L1_ERROR_NONE) { 03587 status = 03588 VL53L1_set_GPIO_rate_threshold( 03589 Dev, 03590 pintconf->threshold_rate_high, 03591 pintconf->threshold_rate_low); 03592 } 03593 03594 LOG_FUNCTION_END(status); 03595 return status; 03596 } 03597 03598 03599 VL53L1_Error VL53L1_set_ref_spad_char_config( 03600 VL53L1_DEV Dev, 03601 uint8_t vcsel_period_a, 03602 uint32_t phasecal_timeout_us, 03603 uint16_t total_rate_target_mcps, 03604 uint16_t max_count_rate_rtn_limit_mcps, 03605 uint16_t min_count_rate_rtn_limit_mcps, 03606 uint16_t fast_osc_frequency) 03607 { 03608 03609 03610 VL53L1_Error status = VL53L1_ERROR_NONE; 03611 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 03612 03613 uint8_t buffer[2]; 03614 03615 uint32_t macro_period_us = 0; 03616 uint32_t timeout_mclks = 0; 03617 03618 LOG_FUNCTION_START(""); 03619 03620 03621 macro_period_us = 03622 VL53L1_calc_macro_period_us( 03623 fast_osc_frequency, 03624 vcsel_period_a); 03625 03626 03627 03628 timeout_mclks = phasecal_timeout_us << 12; 03629 timeout_mclks = timeout_mclks + (macro_period_us>>1); 03630 timeout_mclks = timeout_mclks / macro_period_us; 03631 03632 if (timeout_mclks > 0xFF) 03633 pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF; 03634 else 03635 pdev->gen_cfg.phasecal_config__timeout_macrop = 03636 (uint8_t)timeout_mclks; 03637 03638 pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a; 03639 03640 03641 03642 if (status == VL53L1_ERROR_NONE) 03643 status = 03644 VL53L1_WrByte( 03645 Dev, 03646 VL53L1_PHASECAL_CONFIG__TIMEOUT_MACROP, 03647 pdev->gen_cfg.phasecal_config__timeout_macrop); 03648 03649 if (status == VL53L1_ERROR_NONE) 03650 status = 03651 VL53L1_WrByte( 03652 Dev, 03653 VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A, 03654 pdev->tim_cfg.range_config__vcsel_period_a); 03655 03656 03657 03658 buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a; 03659 buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a; 03660 03661 if (status == VL53L1_ERROR_NONE) 03662 status = 03663 VL53L1_WriteMulti( 03664 Dev, 03665 VL53L1_SD_CONFIG__WOI_SD0, 03666 buffer, 03667 2); 03668 03669 03670 03671 pdev->customer.ref_spad_char__total_rate_target_mcps = 03672 total_rate_target_mcps; 03673 03674 if (status == VL53L1_ERROR_NONE) 03675 status = 03676 VL53L1_WrWord( 03677 Dev, 03678 VL53L1_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS, 03679 total_rate_target_mcps); 03680 03681 if (status == VL53L1_ERROR_NONE) 03682 status = 03683 VL53L1_WrWord( 03684 Dev, 03685 VL53L1_RANGE_CONFIG__SIGMA_THRESH, 03686 max_count_rate_rtn_limit_mcps); 03687 03688 if (status == VL53L1_ERROR_NONE) 03689 status = 03690 VL53L1_WrWord( 03691 Dev, 03692 VL53L1_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 03693 min_count_rate_rtn_limit_mcps); 03694 03695 LOG_FUNCTION_END(status); 03696 03697 return status; 03698 } 03699 03700 03701 VL53L1_Error VL53L1_set_ssc_config( 03702 VL53L1_DEV Dev, 03703 VL53L1_ssc_config_t *pssc_cfg, 03704 uint16_t fast_osc_frequency) 03705 { 03706 03707 03708 VL53L1_Error status = VL53L1_ERROR_NONE; 03709 uint8_t buffer[5]; 03710 03711 uint32_t macro_period_us = 0; 03712 uint16_t timeout_encoded = 0; 03713 03714 LOG_FUNCTION_START(""); 03715 03716 03717 macro_period_us = 03718 VL53L1_calc_macro_period_us( 03719 fast_osc_frequency, 03720 pssc_cfg->VL53L1_p_009); 03721 03722 03723 timeout_encoded = 03724 VL53L1_calc_encoded_timeout( 03725 pssc_cfg->timeout_us, 03726 macro_period_us); 03727 03728 03729 03730 if (status == VL53L1_ERROR_NONE) 03731 status = 03732 VL53L1_WrByte( 03733 Dev, 03734 VL53L1_CAL_CONFIG__VCSEL_START, 03735 pssc_cfg->vcsel_start); 03736 03737 if (status == VL53L1_ERROR_NONE) 03738 status = 03739 VL53L1_WrByte( 03740 Dev, 03741 VL53L1_GLOBAL_CONFIG__VCSEL_WIDTH, 03742 pssc_cfg->vcsel_width); 03743 03744 03745 03746 buffer[0] = (uint8_t)((timeout_encoded & 0x0000FF00) >> 8); 03747 buffer[1] = (uint8_t) (timeout_encoded & 0x000000FF); 03748 buffer[2] = pssc_cfg->VL53L1_p_009; 03749 buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8); 03750 buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps & 0x000000FF); 03751 03752 if (status == VL53L1_ERROR_NONE) 03753 status = 03754 VL53L1_WriteMulti( 03755 Dev, 03756 VL53L1_RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 03757 buffer, 03758 5); 03759 03760 03761 03762 buffer[0] = pssc_cfg->VL53L1_p_009; 03763 buffer[1] = pssc_cfg->VL53L1_p_009; 03764 03765 if (status == VL53L1_ERROR_NONE) 03766 status = 03767 VL53L1_WriteMulti( 03768 Dev, 03769 VL53L1_SD_CONFIG__WOI_SD0, 03770 buffer, 03771 2); 03772 03773 03774 if (status == VL53L1_ERROR_NONE) 03775 status = 03776 VL53L1_WrByte( 03777 Dev, 03778 VL53L1_NVM_BIST__CTRL, 03779 pssc_cfg->array_select); 03780 03781 LOG_FUNCTION_END(status); 03782 03783 return status; 03784 } 03785 03786 03787 VL53L1_Error VL53L1_get_spad_rate_data( 03788 VL53L1_DEV Dev, 03789 VL53L1_spad_rate_data_t *pspad_rates) 03790 { 03791 03792 03793 03794 VL53L1_Error status = VL53L1_ERROR_NONE; 03795 int i = 0; 03796 03797 uint8_t VL53L1_p_002[512]; 03798 uint8_t *pdata = &VL53L1_p_002[0]; 03799 03800 LOG_FUNCTION_START(""); 03801 03802 03803 03804 if (status == VL53L1_ERROR_NONE) 03805 status = VL53L1_disable_firmware(Dev); 03806 03807 03808 03809 if (status == VL53L1_ERROR_NONE) 03810 status = 03811 VL53L1_ReadMulti( 03812 Dev, 03813 VL53L1_PRIVATE__PATCH_BASE_ADDR_RSLV, 03814 pdata, 03815 512); 03816 03817 03818 pdata = &VL53L1_p_002[0]; 03819 for (i = 0; i < VL53L1_NO_OF_SPAD_ENABLES; i++) { 03820 pspad_rates->rate_data[i] = 03821 (uint16_t)VL53L1_decode_unsigned_integer(pdata, 2); 03822 pdata += 2; 03823 } 03824 03825 03826 03827 pspad_rates->VL53L1_p_023 = VL53L1_NO_OF_SPAD_ENABLES; 03828 pspad_rates->no_of_values = VL53L1_NO_OF_SPAD_ENABLES; 03829 pspad_rates->fractional_bits = 15; 03830 03831 03832 03833 if (status == VL53L1_ERROR_NONE) 03834 status = VL53L1_enable_firmware(Dev); 03835 03836 LOG_FUNCTION_END(status); 03837 03838 return status; 03839 } 03840 03841 03842 03843 VL53L1_Error VL53L1_dynamic_xtalk_correction_calc_required_samples( 03844 VL53L1_DEV Dev 03845 ) 03846 { 03847 03848 03849 03850 VL53L1_Error status = VL53L1_ERROR_NONE; 03851 03852 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 03853 VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev); 03854 VL53L1_smudge_corrector_config_t *pconfig = 03855 &(pdev->smudge_correct_config); 03856 VL53L1_smudge_corrector_internals_t *pint = 03857 &(pdev->smudge_corrector_internals); 03858 03859 VL53L1_range_results_t *presults = &(pres->range_results); 03860 VL53L1_range_data_t *pxmonitor = &(presults->xmonitor); 03861 03862 uint32_t peak_duration_us = pxmonitor->peak_duration_us; 03863 03864 uint64_t temp64a; 03865 uint64_t temp64z; 03866 03867 LOG_FUNCTION_START(""); 03868 03869 if (peak_duration_us == 0) 03870 peak_duration_us = 1000; 03871 03872 temp64a = pxmonitor->VL53L1_p_021 + 03873 pxmonitor->VL53L1_p_020; 03874 temp64a = do_division_u((temp64a * 1000), peak_duration_us); 03875 temp64a = do_division_u((temp64a * 1000), peak_duration_us); 03876 03877 temp64z = pconfig->noise_margin * pxmonitor->VL53L1_p_006; 03878 if (temp64z == 0) 03879 temp64z = 1; 03880 temp64a = temp64a * 1000 * 256; 03881 temp64a = do_division_u(temp64a, temp64z); 03882 temp64a = temp64a * 1000 * 256; 03883 temp64a = do_division_u(temp64a, temp64z); 03884 03885 pint->required_samples = (uint32_t)temp64a; 03886 03887 03888 if (pint->required_samples < 2) 03889 pint->required_samples = 2; 03890 03891 LOG_FUNCTION_END(status); 03892 03893 return status; 03894 } 03895 03896 VL53L1_Error VL53L1_dynamic_xtalk_correction_calc_new_xtalk( 03897 VL53L1_DEV Dev, 03898 uint32_t xtalk_offset_out, 03899 VL53L1_smudge_corrector_config_t *pconfig, 03900 VL53L1_smudge_corrector_data_t *pout, 03901 uint8_t add_smudge, 03902 uint8_t soft_update 03903 ) 03904 { 03905 03906 03907 03908 VL53L1_Error status = VL53L1_ERROR_NONE; 03909 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 03910 03911 int16_t x_gradient_scaler; 03912 int16_t y_gradient_scaler; 03913 uint32_t orig_xtalk_offset; 03914 int16_t orig_x_gradient; 03915 int16_t orig_y_gradient; 03916 uint8_t histo_merge_nb; 03917 uint8_t i; 03918 int32_t itemp32; 03919 uint32_t SmudgeFactor; 03920 VL53L1_xtalk_config_t *pX = &(pdev->xtalk_cfg); 03921 VL53L1_xtalk_calibration_results_t *pC = &(pdev->xtalk_cal); 03922 uint32_t *pcpo; 03923 uint32_t max, nXtalk, cXtalk; 03924 uint8_t merge_enabled; 03925 03926 03927 LOG_FUNCTION_START(""); 03928 03929 merge_enabled = (pdev->tuning_parms.tp_hist_merge == 1) && 03930 (VL53L1DevDataGet(Dev, CurrentParameters.PresetMode) == 03931 VL53L1_PRESETMODE_RANGING); 03932 03933 03934 if (add_smudge == 1) { 03935 pout->algo__crosstalk_compensation_plane_offset_kcps = 03936 (uint32_t)xtalk_offset_out + 03937 (uint32_t)pconfig->smudge_margin; 03938 } else { 03939 pout->algo__crosstalk_compensation_plane_offset_kcps = 03940 (uint32_t)xtalk_offset_out; 03941 } 03942 03943 03944 orig_xtalk_offset = 03945 pX->nvm_default__crosstalk_compensation_plane_offset_kcps; 03946 03947 orig_x_gradient = 03948 pX->nvm_default__crosstalk_compensation_x_plane_gradient_kcps; 03949 03950 orig_y_gradient = 03951 pX->nvm_default__crosstalk_compensation_y_plane_gradient_kcps; 03952 03953 if (((pconfig->user_scaler_set == 0) || 03954 (pconfig->scaler_calc_method == 1)) && 03955 (pC->algo__crosstalk_compensation_plane_offset_kcps != 0)) { 03956 03957 VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb); 03958 if (histo_merge_nb == 0) 03959 histo_merge_nb = 1; 03960 if (!merge_enabled) 03961 orig_xtalk_offset = 03962 pC->algo__crosstalk_compensation_plane_offset_kcps; 03963 else 03964 orig_xtalk_offset = 03965 pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1]; 03966 03967 orig_x_gradient = 03968 pC->algo__crosstalk_compensation_x_plane_gradient_kcps; 03969 03970 orig_y_gradient = 03971 pC->algo__crosstalk_compensation_y_plane_gradient_kcps; 03972 } 03973 03974 03975 if ((pconfig->user_scaler_set == 0) && (orig_x_gradient == 0)) 03976 pout->gradient_zero_flag |= 0x01; 03977 03978 if ((pconfig->user_scaler_set == 0) && (orig_y_gradient == 0)) 03979 pout->gradient_zero_flag |= 0x02; 03980 03981 03982 03983 if (orig_xtalk_offset == 0) 03984 orig_xtalk_offset = 1; 03985 03986 03987 03988 if (pconfig->user_scaler_set == 1) { 03989 x_gradient_scaler = pconfig->x_gradient_scaler; 03990 y_gradient_scaler = pconfig->y_gradient_scaler; 03991 } else { 03992 03993 x_gradient_scaler = (int16_t)do_division_s( 03994 (((int32_t)orig_x_gradient) << 6), 03995 orig_xtalk_offset); 03996 pconfig->x_gradient_scaler = x_gradient_scaler; 03997 y_gradient_scaler = (int16_t)do_division_s( 03998 (((int32_t)orig_y_gradient) << 6), 03999 orig_xtalk_offset); 04000 pconfig->y_gradient_scaler = y_gradient_scaler; 04001 } 04002 04003 04004 04005 if (pconfig->scaler_calc_method == 0) { 04006 04007 04008 itemp32 = (int32_t)( 04009 pout->algo__crosstalk_compensation_plane_offset_kcps * 04010 x_gradient_scaler); 04011 itemp32 = itemp32 >> 6; 04012 if (itemp32 > 0xFFFF) 04013 itemp32 = 0xFFFF; 04014 04015 pout->algo__crosstalk_compensation_x_plane_gradient_kcps = 04016 (int16_t)itemp32; 04017 04018 itemp32 = (int32_t)( 04019 pout->algo__crosstalk_compensation_plane_offset_kcps * 04020 y_gradient_scaler); 04021 itemp32 = itemp32 >> 6; 04022 if (itemp32 > 0xFFFF) 04023 itemp32 = 0xFFFF; 04024 04025 pout->algo__crosstalk_compensation_y_plane_gradient_kcps = 04026 (int16_t)itemp32; 04027 } else if (pconfig->scaler_calc_method == 1) { 04028 04029 04030 itemp32 = (int32_t)(orig_xtalk_offset - 04031 pout->algo__crosstalk_compensation_plane_offset_kcps); 04032 itemp32 = (int32_t)(do_division_s(itemp32, 16)); 04033 itemp32 = itemp32 << 2; 04034 itemp32 = itemp32 + (int32_t)(orig_x_gradient); 04035 if (itemp32 > 0xFFFF) 04036 itemp32 = 0xFFFF; 04037 04038 pout->algo__crosstalk_compensation_x_plane_gradient_kcps = 04039 (int16_t)itemp32; 04040 04041 itemp32 = (int32_t)(orig_xtalk_offset - 04042 pout->algo__crosstalk_compensation_plane_offset_kcps); 04043 itemp32 = (int32_t)(do_division_s(itemp32, 80)); 04044 itemp32 = itemp32 << 2; 04045 itemp32 = itemp32 + (int32_t)(orig_y_gradient); 04046 if (itemp32 > 0xFFFF) 04047 itemp32 = 0xFFFF; 04048 04049 pout->algo__crosstalk_compensation_y_plane_gradient_kcps = 04050 (int16_t)itemp32; 04051 } 04052 04053 04054 if (pconfig->smudge_corr_apply_enabled == 1 && 04055 (soft_update != 1)) { 04056 pout->new_xtalk_applied_flag = 1; 04057 nXtalk = pout->algo__crosstalk_compensation_plane_offset_kcps; 04058 04059 VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb); 04060 max = pdev->tuning_parms.tp_hist_merge_max_size; 04061 pcpo = &(pC->algo__xtalk_cpo_HistoMerge_kcps[0]); 04062 if ((histo_merge_nb > 0) && merge_enabled && (nXtalk != 0)) { 04063 cXtalk = 04064 pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1]; 04065 SmudgeFactor = cXtalk * 1000 / nXtalk; 04066 if (SmudgeFactor >= pconfig->max_smudge_factor) 04067 pout->new_xtalk_applied_flag = 0; 04068 else if (SmudgeFactor > 0) 04069 for (i = 0; i < max; i++) { 04070 *pcpo *= 1000; 04071 *pcpo /= SmudgeFactor; 04072 pcpo++; 04073 } 04074 } 04075 if (pout->new_xtalk_applied_flag) { 04076 04077 pX->algo__crosstalk_compensation_plane_offset_kcps = 04078 pout->algo__crosstalk_compensation_plane_offset_kcps; 04079 pX->algo__crosstalk_compensation_x_plane_gradient_kcps = 04080 pout->algo__crosstalk_compensation_x_plane_gradient_kcps; 04081 pX->algo__crosstalk_compensation_y_plane_gradient_kcps = 04082 pout->algo__crosstalk_compensation_y_plane_gradient_kcps; 04083 04084 if (pconfig->smudge_corr_single_apply == 1) { 04085 04086 pconfig->smudge_corr_apply_enabled = 0; 04087 pconfig->smudge_corr_single_apply = 0; 04088 } 04089 } 04090 } 04091 04092 04093 if (soft_update != 1) 04094 pout->smudge_corr_valid = 1; 04095 04096 LOG_FUNCTION_END(status); 04097 04098 return status; 04099 } 04100 04101 #define CONT_CONTINUE 0 04102 #define CONT_NEXT_LOOP 1 04103 #define CONT_RESET 2 04104 VL53L1_Error VL53L1_dynamic_xtalk_correction_corrector( 04105 VL53L1_DEV Dev 04106 ) 04107 { 04108 04109 04110 04111 VL53L1_Error status = VL53L1_ERROR_NONE; 04112 04113 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04114 VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev); 04115 VL53L1_smudge_corrector_config_t *pconfig = 04116 &(pdev->smudge_correct_config); 04117 VL53L1_smudge_corrector_internals_t *pint = 04118 &(pdev->smudge_corrector_internals); 04119 VL53L1_smudge_corrector_data_t *pout = 04120 &(pres->range_results.smudge_corrector_data); 04121 VL53L1_range_results_t *pR = &(pres->range_results); 04122 VL53L1_xtalk_config_t *pX = &(pdev->xtalk_cfg); 04123 04124 uint8_t run_smudge_detection = 0; 04125 uint8_t merging_complete = 0; 04126 uint8_t run_nodetect = 0; 04127 uint8_t ambient_check = 0; 04128 int32_t itemp32 = 0; 04129 uint64_t utemp64 = 0; 04130 uint8_t continue_processing = CONT_CONTINUE; 04131 uint32_t xtalk_offset_out = 0; 04132 uint32_t xtalk_offset_in = 0; 04133 uint32_t current_xtalk = 0; 04134 uint32_t smudge_margin_adjusted = 0; 04135 uint8_t i = 0; 04136 uint8_t nodetect_index = 0; 04137 uint16_t amr; 04138 uint32_t cco; 04139 uint8_t histo_merge_nb; 04140 uint8_t merge_enabled; 04141 04142 04143 LOG_FUNCTION_START(""); 04144 04145 merge_enabled = (pdev->tuning_parms.tp_hist_merge == 1) && 04146 (VL53L1DevDataGet(Dev, CurrentParameters.PresetMode) == 04147 VL53L1_PRESETMODE_RANGING); 04148 04149 VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb); 04150 if ((histo_merge_nb == 0) || (!merge_enabled)) 04151 histo_merge_nb = 1; 04152 04153 04154 VL53L1_dynamic_xtalk_correction_output_init(pres); 04155 04156 04157 ambient_check = (pconfig->smudge_corr_ambient_threshold == 0) || 04158 ((pconfig->smudge_corr_ambient_threshold * histo_merge_nb) > 04159 ((uint32_t)pR->xmonitor.ambient_count_rate_mcps)); 04160 04161 04162 merging_complete = ((!merge_enabled) || 04163 (histo_merge_nb == pdev->tuning_parms.tp_hist_merge_max_size)); 04164 04165 run_smudge_detection = 04166 (pconfig->smudge_corr_enabled == 1) && 04167 ambient_check && 04168 (pR->xmonitor.range_status 04169 == VL53L1_DEVICEERROR_RANGECOMPLETE) && 04170 merging_complete; 04171 04172 04173 if ((pR->xmonitor.range_status 04174 != VL53L1_DEVICEERROR_RANGECOMPLETE) && 04175 (pconfig->smudge_corr_enabled == 1)) { 04176 04177 run_nodetect = 2; 04178 for (i = 0; i < pR->active_results; i++) { 04179 if (pR->VL53L1_p_002[i].range_status == 04180 VL53L1_DEVICEERROR_RANGECOMPLETE) { 04181 if (pR->VL53L1_p_002[i].median_range_mm 04182 <= 04183 pconfig->nodetect_min_range_mm) { 04184 run_nodetect = 0; 04185 } else { 04186 if (run_nodetect == 2) { 04187 run_nodetect = 1; 04188 nodetect_index = i; 04189 } 04190 } 04191 } 04192 } 04193 04194 if (run_nodetect == 2) 04195 04196 run_nodetect = 0; 04197 04198 amr = 04199 pR->VL53L1_p_002[nodetect_index].ambient_count_rate_mcps; 04200 04201 if (run_nodetect == 1) { 04202 04203 04204 04205 04206 utemp64 = 1000 * ((uint64_t)amr); 04207 04208 04209 utemp64 = utemp64 << 9; 04210 04211 04212 if (utemp64 < pconfig->nodetect_ambient_threshold) 04213 run_nodetect = 1; 04214 else 04215 run_nodetect = 0; 04216 04217 } 04218 } 04219 04220 04221 if (run_smudge_detection) { 04222 04223 pint->nodetect_counter = 0; 04224 04225 04226 VL53L1_dynamic_xtalk_correction_calc_required_samples(Dev); 04227 04228 04229 xtalk_offset_in = 04230 pR->xmonitor.VL53L1_p_012; 04231 04232 04233 cco = pX->algo__crosstalk_compensation_plane_offset_kcps; 04234 current_xtalk = ((uint32_t)cco) << 2; 04235 04236 04237 smudge_margin_adjusted = 04238 ((uint32_t)(pconfig->smudge_margin)) << 2; 04239 04240 04241 itemp32 = xtalk_offset_in - current_xtalk + 04242 smudge_margin_adjusted; 04243 04244 if (itemp32 < 0) 04245 itemp32 = itemp32 * (-1); 04246 04247 04248 if (itemp32 > ((int32_t)pconfig->single_xtalk_delta)) { 04249 if ((int32_t)xtalk_offset_in > 04250 ((int32_t)current_xtalk - 04251 (int32_t)smudge_margin_adjusted)) { 04252 pout->single_xtalk_delta_flag = 1; 04253 } else { 04254 pout->single_xtalk_delta_flag = 2; 04255 } 04256 } 04257 04258 04259 pint->current_samples = pint->current_samples + 1; 04260 04261 04262 if (pint->current_samples > pconfig->sample_limit) { 04263 pout->sample_limit_exceeded_flag = 1; 04264 continue_processing = CONT_RESET; 04265 } else { 04266 pint->accumulator = pint->accumulator + 04267 xtalk_offset_in; 04268 } 04269 04270 if (pint->current_samples < pint->required_samples) 04271 continue_processing = CONT_NEXT_LOOP; 04272 04273 04274 xtalk_offset_out = 04275 (uint32_t)(do_division_u(pint->accumulator, 04276 pint->current_samples)); 04277 04278 04279 itemp32 = xtalk_offset_out - current_xtalk + 04280 smudge_margin_adjusted; 04281 04282 if (itemp32 < 0) 04283 itemp32 = itemp32 * (-1); 04284 04285 if (continue_processing == CONT_CONTINUE && 04286 (itemp32 >= ((int32_t)(pconfig->averaged_xtalk_delta))) 04287 ) { 04288 if ((int32_t)xtalk_offset_out > 04289 ((int32_t)current_xtalk - 04290 (int32_t)smudge_margin_adjusted)) 04291 pout->averaged_xtalk_delta_flag = 1; 04292 else 04293 pout->averaged_xtalk_delta_flag = 2; 04294 } 04295 04296 if (continue_processing == CONT_CONTINUE && 04297 (itemp32 < ((int32_t)(pconfig->averaged_xtalk_delta))) 04298 ) 04299 04300 continue_processing = CONT_RESET; 04301 04302 04303 04304 pout->smudge_corr_clipped = 0; 04305 if ((continue_processing == CONT_CONTINUE) && 04306 (pconfig->smudge_corr_clip_limit != 0)) { 04307 if (xtalk_offset_out > 04308 (pconfig->smudge_corr_clip_limit * histo_merge_nb)) { 04309 pout->smudge_corr_clipped = 1; 04310 continue_processing = CONT_RESET; 04311 } 04312 } 04313 04314 04315 04316 if (pconfig->user_xtalk_offset_limit_hi && 04317 (xtalk_offset_out > 04318 pconfig->user_xtalk_offset_limit)) 04319 xtalk_offset_out = 04320 pconfig->user_xtalk_offset_limit; 04321 04322 04323 04324 if ((pconfig->user_xtalk_offset_limit_hi == 0) && 04325 (xtalk_offset_out < 04326 pconfig->user_xtalk_offset_limit)) 04327 xtalk_offset_out = 04328 pconfig->user_xtalk_offset_limit; 04329 04330 04331 04332 xtalk_offset_out = xtalk_offset_out >> 2; 04333 if (xtalk_offset_out > 0x3FFFF) 04334 xtalk_offset_out = 0x3FFFF; 04335 04336 04337 if (continue_processing == CONT_CONTINUE) { 04338 04339 VL53L1_dynamic_xtalk_correction_calc_new_xtalk( 04340 Dev, 04341 xtalk_offset_out, 04342 pconfig, 04343 pout, 04344 1, 04345 0 04346 ); 04347 04348 04349 continue_processing = CONT_RESET; 04350 } else { 04351 04352 VL53L1_dynamic_xtalk_correction_calc_new_xtalk( 04353 Dev, 04354 xtalk_offset_out, 04355 pconfig, 04356 pout, 04357 1, 04358 1 04359 ); 04360 } 04361 04362 04363 if (continue_processing == CONT_RESET) { 04364 pint->accumulator = 0; 04365 pint->current_samples = 0; 04366 pint->nodetect_counter = 0; 04367 } 04368 04369 } 04370 04371 continue_processing = CONT_CONTINUE; 04372 if (run_nodetect == 1) { 04373 04374 pint->nodetect_counter += 1; 04375 04376 04377 if (pint->nodetect_counter < pconfig->nodetect_sample_limit) 04378 continue_processing = CONT_NEXT_LOOP; 04379 04380 04381 xtalk_offset_out = (uint32_t)(pconfig->nodetect_xtalk_offset); 04382 04383 if (continue_processing == CONT_CONTINUE) { 04384 04385 VL53L1_dynamic_xtalk_correction_calc_new_xtalk( 04386 Dev, 04387 xtalk_offset_out, 04388 pconfig, 04389 pout, 04390 0, 04391 0 04392 ); 04393 04394 04395 pout->smudge_corr_valid = 2; 04396 04397 04398 continue_processing = CONT_RESET; 04399 } else { 04400 04401 VL53L1_dynamic_xtalk_correction_calc_new_xtalk( 04402 Dev, 04403 xtalk_offset_out, 04404 pconfig, 04405 pout, 04406 0, 04407 1 04408 ); 04409 } 04410 04411 04412 if (continue_processing == CONT_RESET) { 04413 pint->accumulator = 0; 04414 pint->current_samples = 0; 04415 pint->nodetect_counter = 0; 04416 } 04417 } 04418 04419 LOG_FUNCTION_END(status); 04420 04421 return status; 04422 } 04423 04424 VL53L1_Error VL53L1_dynamic_xtalk_correction_data_init( 04425 VL53L1_DEV Dev 04426 ) 04427 { 04428 04429 04430 04431 04432 VL53L1_Error status = VL53L1_ERROR_NONE; 04433 04434 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04435 VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev); 04436 04437 LOG_FUNCTION_START(""); 04438 04439 04440 04441 pdev->smudge_correct_config.smudge_corr_enabled = 1; 04442 pdev->smudge_correct_config.smudge_corr_apply_enabled = 1; 04443 pdev->smudge_correct_config.smudge_corr_single_apply = 04444 VL53L1_TUNINGPARM_DYNXTALK_SMUDGE_COR_SINGLE_APPLY_DEFAULT; 04445 04446 pdev->smudge_correct_config.smudge_margin = 04447 VL53L1_TUNINGPARM_DYNXTALK_SMUDGE_MARGIN_DEFAULT; 04448 pdev->smudge_correct_config.noise_margin = 04449 VL53L1_TUNINGPARM_DYNXTALK_NOISE_MARGIN_DEFAULT; 04450 pdev->smudge_correct_config.user_xtalk_offset_limit = 04451 VL53L1_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_DEFAULT; 04452 pdev->smudge_correct_config.user_xtalk_offset_limit_hi = 04453 VL53L1_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_HI_DEFAULT; 04454 pdev->smudge_correct_config.sample_limit = 04455 VL53L1_TUNINGPARM_DYNXTALK_SAMPLE_LIMIT_DEFAULT; 04456 pdev->smudge_correct_config.single_xtalk_delta = 04457 VL53L1_TUNINGPARM_DYNXTALK_SINGLE_XTALK_DELTA_DEFAULT; 04458 pdev->smudge_correct_config.averaged_xtalk_delta = 04459 VL53L1_TUNINGPARM_DYNXTALK_AVERAGED_XTALK_DELTA_DEFAULT; 04460 pdev->smudge_correct_config.smudge_corr_clip_limit = 04461 VL53L1_TUNINGPARM_DYNXTALK_CLIP_LIMIT_DEFAULT; 04462 pdev->smudge_correct_config.smudge_corr_ambient_threshold = 04463 VL53L1_TUNINGPARM_DYNXTALK_XTALK_AMB_THRESHOLD_DEFAULT; 04464 pdev->smudge_correct_config.scaler_calc_method = 04465 0; 04466 pdev->smudge_correct_config.x_gradient_scaler = 04467 VL53L1_TUNINGPARM_DYNXTALK_XGRADIENT_SCALER_DEFAULT; 04468 pdev->smudge_correct_config.y_gradient_scaler = 04469 VL53L1_TUNINGPARM_DYNXTALK_YGRADIENT_SCALER_DEFAULT; 04470 pdev->smudge_correct_config.user_scaler_set = 04471 VL53L1_TUNINGPARM_DYNXTALK_USER_SCALER_SET_DEFAULT; 04472 pdev->smudge_correct_config.nodetect_ambient_threshold = 04473 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_AMB_THRESHOLD_KCPS_DEFAULT; 04474 pdev->smudge_correct_config.nodetect_sample_limit = 04475 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_SAMPLE_LIMIT_DEFAULT; 04476 pdev->smudge_correct_config.nodetect_xtalk_offset = 04477 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS_DEFAULT; 04478 pdev->smudge_correct_config.nodetect_min_range_mm = 04479 VL53L1_TUNINGPARM_DYNXTALK_NODETECT_MIN_RANGE_MM_DEFAULT; 04480 pdev->smudge_correct_config.max_smudge_factor = 04481 VL53L1_TUNINGPARM_DYNXTALK_MAX_SMUDGE_FACTOR_DEFAULT; 04482 04483 04484 pdev->smudge_corrector_internals.current_samples = 0; 04485 pdev->smudge_corrector_internals.required_samples = 0; 04486 pdev->smudge_corrector_internals.accumulator = 0; 04487 pdev->smudge_corrector_internals.nodetect_counter = 0; 04488 04489 04490 VL53L1_dynamic_xtalk_correction_output_init(pres); 04491 04492 LOG_FUNCTION_END(status); 04493 04494 return status; 04495 } 04496 04497 VL53L1_Error VL53L1_dynamic_xtalk_correction_output_init( 04498 VL53L1_LLDriverResults_t *pres 04499 ) 04500 { 04501 04502 04503 04504 04505 VL53L1_Error status = VL53L1_ERROR_NONE; 04506 04507 VL53L1_smudge_corrector_data_t *pdata; 04508 04509 LOG_FUNCTION_START(""); 04510 04511 04512 pdata = &(pres->range_results.smudge_corrector_data); 04513 04514 pdata->smudge_corr_valid = 0; 04515 pdata->smudge_corr_clipped = 0; 04516 pdata->single_xtalk_delta_flag = 0; 04517 pdata->averaged_xtalk_delta_flag = 0; 04518 pdata->sample_limit_exceeded_flag = 0; 04519 pdata->gradient_zero_flag = 0; 04520 pdata->new_xtalk_applied_flag = 0; 04521 04522 pdata->algo__crosstalk_compensation_plane_offset_kcps = 0; 04523 pdata->algo__crosstalk_compensation_x_plane_gradient_kcps = 0; 04524 pdata->algo__crosstalk_compensation_y_plane_gradient_kcps = 0; 04525 04526 LOG_FUNCTION_END(status); 04527 04528 return status; 04529 } 04530 04531 04532 VL53L1_Error VL53L1_xtalk_cal_data_init( 04533 VL53L1_DEV Dev 04534 ) 04535 { 04536 04537 04538 04539 04540 VL53L1_Error status = VL53L1_ERROR_NONE; 04541 04542 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04543 04544 LOG_FUNCTION_START(""); 04545 04546 04547 04548 pdev->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps = 0; 04549 pdev->xtalk_cal.algo__crosstalk_compensation_x_plane_gradient_kcps = 0; 04550 pdev->xtalk_cal.algo__crosstalk_compensation_y_plane_gradient_kcps = 0; 04551 memset(&pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[0], 0, 04552 sizeof(pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps)); 04553 04554 LOG_FUNCTION_END(status); 04555 04556 return status; 04557 } 04558 04559 04560 04561 04562 04563 04564 VL53L1_Error VL53L1_low_power_auto_data_init( 04565 VL53L1_DEV Dev 04566 ) 04567 { 04568 04569 04570 04571 04572 VL53L1_Error status = VL53L1_ERROR_NONE; 04573 04574 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04575 04576 LOG_FUNCTION_START(""); 04577 04578 pdev->low_power_auto_data.vhv_loop_bound = 04579 VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT; 04580 pdev->low_power_auto_data.is_low_power_auto_mode = 0; 04581 pdev->low_power_auto_data.low_power_auto_range_count = 0; 04582 pdev->low_power_auto_data.saved_interrupt_config = 0; 04583 pdev->low_power_auto_data.saved_vhv_init = 0; 04584 pdev->low_power_auto_data.saved_vhv_timeout = 0; 04585 pdev->low_power_auto_data.first_run_phasecal_result = 0; 04586 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0; 04587 pdev->low_power_auto_data.dss__required_spads = 0; 04588 04589 LOG_FUNCTION_END(status); 04590 04591 return status; 04592 } 04593 04594 VL53L1_Error VL53L1_low_power_auto_data_stop_range( 04595 VL53L1_DEV Dev 04596 ) 04597 { 04598 04599 04600 04601 04602 VL53L1_Error status = VL53L1_ERROR_NONE; 04603 04604 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04605 04606 LOG_FUNCTION_START(""); 04607 04608 04609 04610 pdev->low_power_auto_data.low_power_auto_range_count = 0xFF; 04611 04612 pdev->low_power_auto_data.first_run_phasecal_result = 0; 04613 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0; 04614 pdev->low_power_auto_data.dss__required_spads = 0; 04615 04616 04617 if (pdev->low_power_auto_data.saved_vhv_init != 0) 04618 pdev->stat_nvm.vhv_config__init = 04619 pdev->low_power_auto_data.saved_vhv_init; 04620 if (pdev->low_power_auto_data.saved_vhv_timeout != 0) 04621 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = 04622 pdev->low_power_auto_data.saved_vhv_timeout; 04623 04624 04625 pdev->gen_cfg.phasecal_config__override = 0x00; 04626 04627 LOG_FUNCTION_END(status); 04628 04629 return status; 04630 } 04631 04632 VL53L1_Error VL53L1_config_low_power_auto_mode( 04633 VL53L1_general_config_t *pgeneral, 04634 VL53L1_dynamic_config_t *pdynamic, 04635 VL53L1_low_power_auto_data_t *plpadata 04636 ) 04637 { 04638 04639 04640 04641 04642 VL53L1_Error status = VL53L1_ERROR_NONE; 04643 04644 LOG_FUNCTION_START(""); 04645 04646 04647 plpadata->is_low_power_auto_mode = 1; 04648 04649 04650 plpadata->low_power_auto_range_count = 0; 04651 04652 04653 pdynamic->system__sequence_config = 04654 VL53L1_SEQUENCE_VHV_EN | 04655 VL53L1_SEQUENCE_PHASECAL_EN | 04656 VL53L1_SEQUENCE_DSS1_EN | 04657 04658 04659 04660 VL53L1_SEQUENCE_RANGE_EN; 04661 04662 04663 pgeneral->dss_config__manual_effective_spads_select = 200 << 8; 04664 pgeneral->dss_config__roi_mode_control = 04665 VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; 04666 04667 LOG_FUNCTION_END(status); 04668 04669 return status; 04670 } 04671 04672 VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration( 04673 VL53L1_DEV Dev) 04674 { 04675 04676 04677 04678 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04679 04680 04681 VL53L1_Error status = VL53L1_ERROR_NONE; 04682 04683 LOG_FUNCTION_START(""); 04684 04685 04686 pdev->low_power_auto_data.saved_vhv_init = 04687 pdev->stat_nvm.vhv_config__init; 04688 pdev->low_power_auto_data.saved_vhv_timeout = 04689 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound; 04690 04691 04692 pdev->stat_nvm.vhv_config__init &= 0x7F; 04693 04694 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = 04695 (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) + 04696 (pdev->low_power_auto_data.vhv_loop_bound << 2); 04697 04698 pdev->gen_cfg.phasecal_config__override = 0x01; 04699 pdev->low_power_auto_data.first_run_phasecal_result = 04700 pdev->dbg_results.phasecal_result__vcsel_start; 04701 pdev->gen_cfg.cal_config__vcsel_start = 04702 pdev->low_power_auto_data.first_run_phasecal_result; 04703 04704 LOG_FUNCTION_END(status); 04705 04706 return status; 04707 } 04708 04709 VL53L1_Error VL53L1_low_power_auto_update_DSS( 04710 VL53L1_DEV Dev) 04711 { 04712 04713 04714 04715 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04716 04717 VL53L1_system_results_t *pS = &(pdev->sys_results); 04718 04719 04720 VL53L1_Error status = VL53L1_ERROR_NONE; 04721 04722 uint32_t utemp32a; 04723 04724 LOG_FUNCTION_START(""); 04725 04726 04727 04728 04729 utemp32a = 04730 pS->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 04731 + pS->result__ambient_count_rate_mcps_sd0; 04732 04733 04734 if (utemp32a > 0xFFFF) 04735 utemp32a = 0xFFFF; 04736 04737 04738 04739 utemp32a = utemp32a << 16; 04740 04741 04742 if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0) 04743 status = VL53L1_ERROR_DIVISION_BY_ZERO; 04744 else { 04745 04746 utemp32a = utemp32a / 04747 pdev->sys_results.result__dss_actual_effective_spads_sd0; 04748 04749 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 04750 utemp32a; 04751 04752 04753 utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps << 04754 16; 04755 04756 04757 if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps 04758 == 0) 04759 status = VL53L1_ERROR_DIVISION_BY_ZERO; 04760 else { 04761 04762 utemp32a = utemp32a / 04763 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps; 04764 04765 04766 if (utemp32a > 0xFFFF) 04767 utemp32a = 0xFFFF; 04768 04769 04770 pdev->low_power_auto_data.dss__required_spads = 04771 (uint16_t)utemp32a; 04772 04773 04774 pdev->gen_cfg.dss_config__manual_effective_spads_select 04775 = pdev->low_power_auto_data.dss__required_spads; 04776 pdev->gen_cfg.dss_config__roi_mode_control = 04777 VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; 04778 } 04779 04780 } 04781 04782 if (status == VL53L1_ERROR_DIVISION_BY_ZERO) { 04783 04784 04785 04786 pdev->low_power_auto_data.dss__required_spads = 0x8000; 04787 04788 04789 pdev->gen_cfg.dss_config__manual_effective_spads_select = 04790 pdev->low_power_auto_data.dss__required_spads; 04791 pdev->gen_cfg.dss_config__roi_mode_control = 04792 VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; 04793 04794 04795 status = VL53L1_ERROR_NONE; 04796 } 04797 04798 LOG_FUNCTION_END(status); 04799 04800 return status; 04801 } 04802 04803 04804 04805 04806 VL53L1_Error VL53L1_compute_histo_merge_nb( 04807 VL53L1_DEV Dev, uint8_t *histo_merge_nb) 04808 { 04809 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 04810 VL53L1_Error status = VL53L1_ERROR_NONE; 04811 uint8_t i, timing; 04812 uint8_t sum = 0; 04813 04814 timing = (pdev->hist_data.bin_seq[0] == 7 ? 1 : 0); 04815 for (i = 0; i < VL53L1_BIN_REC_SIZE; i++) 04816 if (pdev->multi_bins_rec[i][timing][7] > 0) 04817 sum++; 04818 *histo_merge_nb = sum; 04819 04820 return status; 04821 } 04822
Generated on Thu Jul 14 2022 10:20:22 by
1.7.2