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