ST Expansion SW Team
/
VL53L1CB
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2
Embed:
(wiki syntax)
Show/hide line numbers
vl53l1_api_calibration.c
00001 00002 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause 00003 /****************************************************************************** 00004 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved 00005 00006 This file is part of VL53L1 and is dual licensed, 00007 either GPL-2.0+ 00008 or 'BSD 3-clause "New" or "Revised" License' , at your option. 00009 ****************************************************************************** 00010 */ 00011 00012 00013 00014 00015 00016 #include "vl53l1_ll_def.h" 00017 #include "vl53l1_ll_device.h" 00018 #include "vl53l1_platform.h" 00019 #include "vl53l1_platform_ipp.h" 00020 #include "vl53l1_register_map.h" 00021 #include "vl53l1_register_funcs.h" 00022 #include "vl53l1_register_settings.h" 00023 #include "vl53l1_hist_map.h" 00024 #include "vl53l1_hist_structs.h" 00025 #include "vl53l1_core.h" 00026 #include "vl53l1_wait.h" 00027 #include "vl53l1_api_preset_modes.h" 00028 #include "vl53l1_silicon_core.h" 00029 #include "vl53l1_api_core.h" 00030 #include "vl53l1_api_calibration.h" 00031 00032 #ifdef VL53L1_LOG_ENABLE 00033 #include "vl53l1_api_debug.h" 00034 #endif 00035 00036 00037 #define LOG_FUNCTION_START(fmt, ...) \ 00038 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__) 00039 #define LOG_FUNCTION_END(status, ...) \ 00040 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__) 00041 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 00042 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, \ 00043 fmt, ##__VA_ARGS__) 00044 00045 #define trace_print(level, ...) \ 00046 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \ 00047 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) 00048 00049 00050 VL53L1_Error VL53L1_run_ref_spad_char( 00051 VL53L1_DEV Dev, 00052 VL53L1_Error *pcal_status) 00053 { 00054 00055 00056 VL53L1_Error status = VL53L1_ERROR_NONE; 00057 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 00058 00059 uint8_t comms_buffer[6]; 00060 00061 VL53L1_refspadchar_config_t *prefspadchar = &(pdev->refspadchar); 00062 00063 LOG_FUNCTION_START(""); 00064 00065 00066 00067 if (status == VL53L1_ERROR_NONE) 00068 status = VL53L1_enable_powerforce(Dev); 00069 00070 00071 00072 if (status == VL53L1_ERROR_NONE) 00073 status = 00074 VL53L1_set_ref_spad_char_config( 00075 Dev, 00076 prefspadchar->VL53L1_p_009, 00077 prefspadchar->timeout_us, 00078 prefspadchar->target_count_rate_mcps, 00079 prefspadchar->max_count_rate_limit_mcps, 00080 prefspadchar->min_count_rate_limit_mcps, 00081 pdev->stat_nvm.osc_measured__fast_osc__frequency); 00082 00083 00084 00085 if (status == VL53L1_ERROR_NONE) 00086 status = VL53L1_run_device_test( 00087 Dev, 00088 prefspadchar->device_test_mode); 00089 00090 00091 00092 if (status == VL53L1_ERROR_NONE) 00093 status = 00094 VL53L1_ReadMulti( 00095 Dev, 00096 VL53L1_REF_SPAD_CHAR_RESULT__NUM_ACTUAL_REF_SPADS, 00097 comms_buffer, 00098 2); 00099 00100 if (status == VL53L1_ERROR_NONE) { 00101 pdev->dbg_results.ref_spad_char_result__num_actual_ref_spads = 00102 comms_buffer[0]; 00103 pdev->dbg_results.ref_spad_char_result__ref_location = 00104 comms_buffer[1]; 00105 } 00106 00107 00108 00109 if (status == VL53L1_ERROR_NONE) 00110 status = 00111 VL53L1_WriteMulti( 00112 Dev, 00113 VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS, 00114 comms_buffer, 00115 2); 00116 00117 if (status == VL53L1_ERROR_NONE) { 00118 pdev->customer.ref_spad_man__num_requested_ref_spads = 00119 comms_buffer[0]; 00120 pdev->customer.ref_spad_man__ref_location = 00121 comms_buffer[1]; 00122 } 00123 00124 00125 00126 if (status == VL53L1_ERROR_NONE) 00127 status = 00128 VL53L1_ReadMulti( 00129 Dev, 00130 VL53L1_RESULT__SPARE_0_SD1, 00131 comms_buffer, 00132 6); 00133 00134 00135 00136 if (status == VL53L1_ERROR_NONE) 00137 status = 00138 VL53L1_WriteMulti( 00139 Dev, 00140 VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0, 00141 comms_buffer, 00142 6); 00143 00144 if (status == VL53L1_ERROR_NONE) { 00145 pdev->customer.global_config__spad_enables_ref_0 = 00146 comms_buffer[0]; 00147 pdev->customer.global_config__spad_enables_ref_1 = 00148 comms_buffer[1]; 00149 pdev->customer.global_config__spad_enables_ref_2 = 00150 comms_buffer[2]; 00151 pdev->customer.global_config__spad_enables_ref_3 = 00152 comms_buffer[3]; 00153 pdev->customer.global_config__spad_enables_ref_4 = 00154 comms_buffer[4]; 00155 pdev->customer.global_config__spad_enables_ref_5 = 00156 comms_buffer[5]; 00157 } 00158 00159 #ifdef VL53L1_LOG_ENABLE 00160 00161 if (status == VL53L1_ERROR_NONE) 00162 VL53L1_print_customer_nvm_managed( 00163 &(pdev->customer), 00164 "run_ref_spad_char():pdev->lldata.customer.", 00165 VL53L1_TRACE_MODULE_REF_SPAD_CHAR); 00166 #endif 00167 00168 if (status == VL53L1_ERROR_NONE) { 00169 00170 switch (pdev->sys_results.result__range_status) { 00171 00172 case VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS: 00173 status = VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS; 00174 break; 00175 00176 case VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET: 00177 status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH; 00178 break; 00179 00180 case VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET: 00181 status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW; 00182 break; 00183 } 00184 } 00185 00186 00187 00188 *pcal_status = status; 00189 00190 00191 00192 IGNORE_STATUS( 00193 IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS, 00194 VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS, 00195 status); 00196 00197 IGNORE_STATUS( 00198 IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH, 00199 VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH, 00200 status); 00201 00202 IGNORE_STATUS( 00203 IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW, 00204 VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW, 00205 status); 00206 00207 00208 LOG_FUNCTION_END(status); 00209 00210 return status; 00211 } 00212 00213 00214 VL53L1_Error VL53L1_run_xtalk_extraction( 00215 VL53L1_DEV Dev, 00216 VL53L1_Error *pcal_status) 00217 { 00218 00219 00220 VL53L1_Error status = VL53L1_ERROR_NONE; 00221 VL53L1_LLDriverData_t *pdev = 00222 VL53L1DevStructGetLLDriverHandle(Dev); 00223 00224 00225 00226 VL53L1_xtalkextract_config_t *pX = &(pdev->xtalk_extract_cfg); 00227 VL53L1_xtalk_config_t *pC = &(pdev->xtalk_cfg); 00228 VL53L1_xtalk_calibration_results_t *pXC = &(pdev->xtalk_cal); 00229 00230 uint8_t results_invalid = 0; 00231 00232 uint8_t i = 0; 00233 uint16_t tmp16 = 0; 00234 00235 uint8_t measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK; 00236 00237 LOG_FUNCTION_START(""); 00238 00239 00240 00241 00242 00243 VL53L1_init_histogram_bin_data_struct( 00244 0, 00245 (uint16_t)VL53L1_HISTOGRAM_BUFFER_SIZE, 00246 &(pdev->xtalk_results.central_histogram_avg)); 00247 00248 VL53L1_init_histogram_bin_data_struct( 00249 0, 00250 (uint16_t)VL53L1_HISTOGRAM_BUFFER_SIZE, 00251 &(pdev->xtalk_results.central_histogram_sum)); 00252 00253 00254 00255 if (status == VL53L1_ERROR_NONE) 00256 status = 00257 VL53L1_set_preset_mode( 00258 Dev, 00259 VL53L1_DEVICEPRESETMODE_HISTOGRAM_XTALK_PLANAR, 00260 00261 pX->dss_config__target_total_rate_mcps, 00262 pX->phasecal_config_timeout_us, 00263 pX->mm_config_timeout_us, 00264 pX->range_config_timeout_us, 00265 00266 100); 00267 00268 00269 00270 if (status == VL53L1_ERROR_NONE) 00271 status = VL53L1_disable_xtalk_compensation(Dev); 00272 00273 00274 00275 pdev->xtalk_results.max_results = VL53L1_MAX_XTALK_RANGE_RESULTS; 00276 pdev->xtalk_results.active_results = pdev->zone_cfg.active_zones+1; 00277 00278 00279 00280 pdev->xtalk_results.central_histogram__window_start = 0xFF; 00281 pdev->xtalk_results.central_histogram__window_end = 0x00; 00282 00283 pdev->xtalk_results.num_of_samples_status = 0x00; 00284 pdev->xtalk_results.zero_samples_status = 0x00; 00285 pdev->xtalk_results.max_sigma_status = 0x00; 00286 00287 for (i = 0; i < pdev->xtalk_results.max_results; i++) { 00288 pdev->xtalk_results.VL53L1_p_002[i].no_of_samples = 0; 00289 pdev->xtalk_results.VL53L1_p_002[i].signal_total_events_avg = 0; 00290 pdev->xtalk_results.VL53L1_p_002[i].signal_total_events_sum = 0; 00291 pdev->xtalk_results.VL53L1_p_002[i].rate_per_spad_kcps_sum = 0; 00292 pdev->xtalk_results.VL53L1_p_002[i].rate_per_spad_kcps_avg = 0; 00293 pdev->xtalk_results.VL53L1_p_002[i].sigma_mm_sum = 0; 00294 pdev->xtalk_results.VL53L1_p_002[i].sigma_mm_avg = 0; 00295 00296 pdev->xtalk_results.VL53L1_p_002[i].median_phase_sum = 0; 00297 pdev->xtalk_results.VL53L1_p_002[i].median_phase_avg = 0; 00298 00299 } 00300 00301 00302 if (status == VL53L1_ERROR_NONE) { 00303 00304 status = 00305 VL53L1_get_and_avg_xtalk_samples( 00306 Dev, 00307 00308 pX->num_of_samples, 00309 00310 measurement_mode, 00311 00312 pX->algo__crosstalk_extract_max_valid_range_mm, 00313 pX->algo__crosstalk_extract_min_valid_range_mm, 00314 pX->algo__crosstalk_extract_max_valid_rate_kcps, 00315 00316 0x0, 00317 0x4, 00318 &(pdev->xtalk_results), 00319 &(pdev->xtalk_results.central_histogram_sum), 00320 &(pdev->xtalk_results.central_histogram_avg)); 00321 } 00322 00323 00324 00325 00326 00327 00328 00329 if (status == VL53L1_ERROR_NONE) 00330 if ((pdev->xtalk_results.VL53L1_p_002[4].no_of_samples == 0) || 00331 (pdev->xtalk_results.VL53L1_p_002[4].sigma_mm_avg > 00332 ((uint32_t)pX->algo__crosstalk_extract_max_sigma_mm 00333 << 5))) 00334 results_invalid = 0x01; 00335 00336 00337 00338 #ifdef VL53L1_LOG_ENABLE 00339 if (status == VL53L1_ERROR_NONE) 00340 VL53L1_print_xtalk_range_results( 00341 &(pdev->xtalk_results), 00342 "pdev->xtalk_results", 00343 VL53L1_TRACE_MODULE_CORE); 00344 #endif 00345 00346 if ((status == VL53L1_ERROR_NONE) && (results_invalid == 0)) { 00347 00348 status = 00349 VL53L1_ipp_xtalk_calibration_process_data( 00350 Dev, 00351 &(pdev->xtalk_results), 00352 &(pdev->xtalk_shapes), 00353 &(pdev->xtalk_cal)); 00354 00355 } 00356 if ((status == VL53L1_ERROR_NONE) && (results_invalid == 0)) { 00357 for (i = 0; i < VL53L1_BIN_REC_SIZE; i++) 00358 pXC->algo__xtalk_cpo_HistoMerge_kcps[i] = 00359 pXC->algo__crosstalk_compensation_plane_offset_kcps; 00360 pC->algo__crosstalk_compensation_x_plane_gradient_kcps = 00361 pXC->algo__crosstalk_compensation_x_plane_gradient_kcps; 00362 pC->algo__crosstalk_compensation_y_plane_gradient_kcps = 00363 pXC->algo__crosstalk_compensation_y_plane_gradient_kcps; 00364 pC->algo__crosstalk_compensation_plane_offset_kcps = 00365 pXC->algo__crosstalk_compensation_plane_offset_kcps; 00366 } 00367 00368 00369 00370 if (status == VL53L1_ERROR_NONE) 00371 status = VL53L1_enable_xtalk_compensation(Dev); 00372 00373 00374 00375 00376 if (status == VL53L1_ERROR_NONE) { 00377 00378 for (i = 0; i < pdev->xtalk_results.max_results; i++) { 00379 00380 if (pdev->xtalk_results.VL53L1_p_002[i].no_of_samples != 00381 00382 pX->num_of_samples) { 00383 00384 pdev->xtalk_results.num_of_samples_status = 00385 pdev->xtalk_results.num_of_samples_status | 00386 (1 << i); 00387 } 00388 00389 if (pdev->xtalk_results.VL53L1_p_002[i].no_of_samples == 00390 0x00) { 00391 pdev->xtalk_results.zero_samples_status = 00392 pdev->xtalk_results.zero_samples_status | 00393 (1 << i); 00394 } 00395 00396 00397 00398 00399 tmp16 = pX->algo__crosstalk_extract_max_sigma_mm; 00400 if (pdev->xtalk_results.VL53L1_p_002[i].sigma_mm_avg > 00401 ((uint32_t)tmp16 << 5)) { 00402 pdev->xtalk_results.max_sigma_status = 00403 pdev->xtalk_results.max_sigma_status | 00404 (1 << i); 00405 } 00406 00407 } 00408 } 00409 00410 00411 if (results_invalid > 0) { 00412 00413 if (pdev->xtalk_results.VL53L1_p_002[4].no_of_samples == 0) { 00414 status = VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL; 00415 } else { 00416 00417 00418 if (pdev->xtalk_results.VL53L1_p_002[4].sigma_mm_avg > 00419 (((uint32_t)pX->algo__crosstalk_extract_max_sigma_mm) 00420 << 5)) { 00421 status = 00422 VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL; 00423 } 00424 00425 } 00426 } else { 00427 00428 if (pdev->xtalk_results.zero_samples_status != 0x00) { 00429 status = VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT; 00430 } else { 00431 if (pdev->xtalk_results.max_sigma_status != 0x00) { 00432 status = 00433 VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT; 00434 } else { 00435 if (pdev->xtalk_results.num_of_samples_status != 00436 0x00) 00437 status = 00438 VL53L1_WARNING_XTALK_MISSING_SAMPLES; 00439 } 00440 } 00441 } 00442 00443 00444 00445 pdev->xtalk_results.cal_status = status; 00446 *pcal_status = pdev->xtalk_results.cal_status; 00447 00448 00449 00450 IGNORE_STATUS( 00451 IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FAIL, 00452 VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL, 00453 status); 00454 00455 IGNORE_STATUS( 00456 IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL, 00457 VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL, 00458 status); 00459 00460 IGNORE_STATUS( 00461 IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FOR_GRADIENT_WARN, 00462 VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT, 00463 status); 00464 00465 IGNORE_STATUS( 00466 IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FOR_GRADIENT_WARN, 00467 VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT, 00468 status); 00469 00470 IGNORE_STATUS( 00471 IGNORE_XTALK_EXTRACTION_MISSING_SAMPLES_WARN, 00472 VL53L1_WARNING_XTALK_MISSING_SAMPLES, 00473 status); 00474 00475 #ifdef VL53L1_LOG_ENABLE 00476 00477 00478 00479 VL53L1_print_customer_nvm_managed( 00480 &(pdev->customer), 00481 "run_xtalk_extraction():pdev->lldata.customer.", 00482 VL53L1_TRACE_MODULE_XTALK_DATA); 00483 00484 VL53L1_print_xtalk_config( 00485 &(pdev->xtalk_cfg), 00486 "run_xtalk_extraction():pdev->lldata.xtalk_cfg.", 00487 VL53L1_TRACE_MODULE_XTALK_DATA); 00488 00489 VL53L1_print_xtalk_extract_config( 00490 &(pdev->xtalk_extract_cfg), 00491 "run_xtalk_extraction():pdev->lldata.xtalk_extract_cfg.", 00492 VL53L1_TRACE_MODULE_XTALK_DATA); 00493 00494 VL53L1_print_histogram_bin_data( 00495 &(pdev->hist_data), 00496 "run_xtalk_extraction():pdev->lldata.hist_data.", 00497 VL53L1_TRACE_MODULE_XTALK_DATA); 00498 00499 VL53L1_print_xtalk_histogram_data( 00500 &(pdev->xtalk_shapes), 00501 "pdev->lldata.xtalk_shapes.", 00502 VL53L1_TRACE_MODULE_XTALK_DATA); 00503 00504 VL53L1_print_xtalk_range_results( 00505 &(pdev->xtalk_results), 00506 "run_xtalk_extraction():pdev->lldata.xtalk_results.", 00507 VL53L1_TRACE_MODULE_XTALK_DATA); 00508 00509 #endif 00510 00511 LOG_FUNCTION_END(status); 00512 00513 return status; 00514 00515 } 00516 00517 00518 00519 VL53L1_Error VL53L1_get_and_avg_xtalk_samples( 00520 VL53L1_DEV Dev, 00521 uint8_t num_of_samples, 00522 uint8_t measurement_mode, 00523 int16_t xtalk_filter_thresh_max_mm, 00524 int16_t xtalk_filter_thresh_min_mm, 00525 uint16_t xtalk_max_valid_rate_kcps, 00526 uint8_t xtalk_result_id, 00527 uint8_t xtalk_histo_id, 00528 VL53L1_xtalk_range_results_t *pXR, 00529 VL53L1_histogram_bin_data_t *psum_histo, 00530 VL53L1_histogram_bin_data_t *pavg_histo) 00531 { 00532 00533 00534 00535 VL53L1_Error status = VL53L1_ERROR_NONE; 00536 VL53L1_LLDriverData_t *pdev = 00537 VL53L1DevStructGetLLDriverHandle(Dev); 00538 00539 #ifdef VL53L1_LOG_ENABLE 00540 VL53L1_LLDriverResults_t *pres = 00541 VL53L1DevStructGetLLResultsHandle(Dev); 00542 #endif 00543 00544 VL53L1_range_results_t *prs = 00545 (VL53L1_range_results_t *) pdev->wArea1; 00546 00547 VL53L1_range_data_t *prange_data; 00548 VL53L1_xtalk_range_data_t *pxtalk_range_data; 00549 00550 uint8_t i = 0; 00551 uint8_t j = 0; 00552 uint8_t zone_id = 0; 00553 uint8_t final_zone = pdev->zone_cfg.active_zones+1; 00554 uint8_t valid_result; 00555 00556 uint8_t smudge_corr_en = 0; 00557 00558 00559 00560 00561 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled; 00562 00563 status = VL53L1_dynamic_xtalk_correction_disable(Dev); 00564 00565 00566 VL53L1_load_patch(Dev); 00567 00568 00569 00570 if (status == VL53L1_ERROR_NONE) 00571 status = 00572 VL53L1_init_and_start_range( 00573 Dev, 00574 measurement_mode, 00575 VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS); 00576 00577 00578 for (i = 0; i <= (final_zone*num_of_samples); i++) { 00579 00580 00581 00582 if (status == VL53L1_ERROR_NONE) 00583 status = VL53L1_wait_for_range_completion(Dev); 00584 00585 00586 00587 if (status == VL53L1_ERROR_NONE) 00588 status = 00589 VL53L1_get_device_results( 00590 Dev, 00591 VL53L1_DEVICERESULTSLEVEL_FULL, 00592 prs); 00593 00594 00595 00596 if (status == VL53L1_ERROR_NONE && 00597 pdev->ll_state.rd_device_state != 00598 VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC) { 00599 00600 zone_id = pdev->ll_state.rd_zone_id + xtalk_result_id; 00601 prange_data = &(prs->VL53L1_p_002[0]); 00602 00603 00604 if (prs->active_results > 1) { 00605 for (j = 1; 00606 j < prs->active_results; j++) { 00607 if (prs->VL53L1_p_002[j].median_range_mm 00608 < 00609 prange_data->median_range_mm) 00610 prange_data = 00611 &(prs->VL53L1_p_002[j]); 00612 00613 } 00614 } 00615 00616 pxtalk_range_data = &(pXR->VL53L1_p_002[zone_id]); 00617 00618 00619 00620 if ((prs->active_results > 0) && 00621 (prange_data->median_range_mm < 00622 xtalk_filter_thresh_max_mm) && 00623 (prange_data->median_range_mm > 00624 xtalk_filter_thresh_min_mm) && 00625 (prange_data->VL53L1_p_012 < 00626 (uint32_t)(xtalk_max_valid_rate_kcps * 16))) 00627 valid_result = 1; 00628 else 00629 valid_result = 0; 00630 00631 if (valid_result == 1) { 00632 00633 pxtalk_range_data->no_of_samples++; 00634 00635 pxtalk_range_data->rate_per_spad_kcps_sum += 00636 prange_data->VL53L1_p_012; 00637 00638 pxtalk_range_data->signal_total_events_sum += 00639 prange_data->VL53L1_p_013; 00640 00641 pxtalk_range_data->sigma_mm_sum += 00642 (uint32_t)prange_data->VL53L1_p_005; 00643 00644 00645 00646 pxtalk_range_data->median_phase_sum += 00647 (uint32_t)prange_data->VL53L1_p_014; 00648 00649 00650 00651 00652 } 00653 00654 if ((valid_result == 1) && (zone_id >= 4)) { 00655 status = VL53L1_sum_histogram_data( 00656 &(pdev->hist_data), 00657 psum_histo); 00658 00659 00660 00661 if (prange_data->VL53L1_p_015 < 00662 pXR->central_histogram__window_start) 00663 pXR->central_histogram__window_start = 00664 prange_data->VL53L1_p_015; 00665 00666 00667 if (prange_data->VL53L1_p_016 > 00668 pXR->central_histogram__window_end) 00669 pXR->central_histogram__window_end = 00670 prange_data->VL53L1_p_016; 00671 00672 } 00673 00674 } 00675 00676 00677 00678 #ifdef VL53L1_LOG_ENABLE 00679 if (status == VL53L1_ERROR_NONE) { 00680 VL53L1_print_range_results( 00681 &(pres->range_results), 00682 "pres->range_results.", 00683 VL53L1_TRACE_MODULE_CORE); 00684 } 00685 #endif 00686 00687 00688 00689 if (status == VL53L1_ERROR_NONE) 00690 status = VL53L1_wait_for_firmware_ready(Dev); 00691 00692 00693 00694 if (status == VL53L1_ERROR_NONE) 00695 status = 00696 VL53L1_clear_interrupt_and_enable_next_range( 00697 Dev, 00698 measurement_mode); 00699 00700 00701 } 00702 00703 00704 00705 00706 if (status == VL53L1_ERROR_NONE) 00707 status = VL53L1_stop_range(Dev); 00708 00709 VL53L1_unload_patch(Dev); 00710 00711 00712 00713 for (i = 0; i < (pdev->zone_cfg.active_zones+1); i++) { 00714 00715 pxtalk_range_data = &(pXR->VL53L1_p_002[i+xtalk_result_id]); 00716 00717 if (pxtalk_range_data->no_of_samples > 0) { 00718 pxtalk_range_data->rate_per_spad_kcps_avg = 00719 pxtalk_range_data->rate_per_spad_kcps_sum / 00720 (uint32_t)pxtalk_range_data->no_of_samples; 00721 00722 pxtalk_range_data->signal_total_events_avg = 00723 pxtalk_range_data->signal_total_events_sum / 00724 (int32_t)pxtalk_range_data->no_of_samples; 00725 00726 pxtalk_range_data->sigma_mm_avg = 00727 pxtalk_range_data->sigma_mm_sum / 00728 (uint32_t)pxtalk_range_data->no_of_samples; 00729 00730 00731 00732 pxtalk_range_data->median_phase_avg = 00733 pxtalk_range_data->median_phase_sum / 00734 (uint32_t)pxtalk_range_data->no_of_samples; 00735 00736 00737 00738 } else { 00739 pxtalk_range_data->rate_per_spad_kcps_avg = 00740 pxtalk_range_data->rate_per_spad_kcps_sum; 00741 pxtalk_range_data->signal_total_events_avg = 00742 pxtalk_range_data->signal_total_events_sum; 00743 pxtalk_range_data->sigma_mm_avg = 00744 pxtalk_range_data->sigma_mm_sum; 00745 00746 00747 00748 pxtalk_range_data->median_phase_avg = 00749 pxtalk_range_data->median_phase_sum; 00750 00751 00752 } 00753 } 00754 00755 00756 00757 memcpy(pavg_histo, &(pdev->hist_data), 00758 sizeof(VL53L1_histogram_bin_data_t)); 00759 00760 00761 00762 if (status == VL53L1_ERROR_NONE) { 00763 00764 pxtalk_range_data = &(pXR->VL53L1_p_002[xtalk_histo_id]); 00765 00766 status = VL53L1_avg_histogram_data( 00767 pxtalk_range_data->no_of_samples, 00768 psum_histo, 00769 pavg_histo); 00770 } 00771 00772 00773 00774 00775 if (status == VL53L1_ERROR_NONE) { 00776 if (smudge_corr_en == 1) 00777 status = VL53L1_dynamic_xtalk_correction_enable(Dev); 00778 } 00779 00780 00781 LOG_FUNCTION_END(status); 00782 00783 return status; 00784 00785 } 00786 00787 00788 00789 VL53L1_Error VL53L1_run_offset_calibration( 00790 VL53L1_DEV Dev, 00791 int16_t cal_distance_mm, 00792 uint16_t cal_reflectance_pc, 00793 VL53L1_Error *pcal_status) 00794 { 00795 00796 00797 VL53L1_Error status = VL53L1_ERROR_NONE; 00798 VL53L1_LLDriverData_t *pdev = 00799 VL53L1DevStructGetLLDriverHandle(Dev); 00800 00801 VL53L1_DevicePresetModes device_preset_modes[ 00802 VL53L1_MAX_OFFSET_RANGE_RESULTS]; 00803 00804 VL53L1_range_results_t *prange_results = 00805 (VL53L1_range_results_t *) pdev->wArea1; 00806 00807 VL53L1_range_data_t *pRData = NULL; 00808 VL53L1_offset_range_data_t *pfs = NULL; 00809 VL53L1_general_config_t *pG = &(pdev->gen_cfg); 00810 VL53L1_additional_offset_cal_data_t *pAO = &(pdev->add_off_cal_data); 00811 00812 uint8_t i = 0; 00813 uint8_t m = 0; 00814 uint8_t measurement_mode = 00815 VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK; 00816 uint16_t manual_effective_spads = 00817 pG->dss_config__manual_effective_spads_select; 00818 00819 uint8_t num_of_samples[VL53L1_MAX_OFFSET_RANGE_RESULTS]; 00820 00821 uint8_t smudge_corr_en = 0; 00822 00823 LOG_FUNCTION_START(""); 00824 00825 00826 00827 switch (pdev->offset_calibration_mode) { 00828 00829 case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM: 00830 case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY: 00831 device_preset_modes[0] = 00832 VL53L1_DEVICEPRESETMODE_HISTOGRAM_RANGING; 00833 device_preset_modes[1] = 00834 VL53L1_DEVICEPRESETMODE_HISTOGRAM_RANGING_MM1_CAL; 00835 device_preset_modes[2] = 00836 VL53L1_DEVICEPRESETMODE_HISTOGRAM_RANGING_MM2_CAL; 00837 break; 00838 00839 default: 00840 device_preset_modes[0] = 00841 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING; 00842 device_preset_modes[1] = 00843 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL; 00844 device_preset_modes[2] = 00845 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL; 00846 break; 00847 } 00848 00849 00850 00851 num_of_samples[0] = pdev->offsetcal_cfg.pre_num_of_samples; 00852 num_of_samples[1] = pdev->offsetcal_cfg.mm1_num_of_samples; 00853 num_of_samples[2] = pdev->offsetcal_cfg.mm2_num_of_samples; 00854 00855 00856 00857 00858 switch (pdev->offset_calibration_mode) { 00859 00860 case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY: 00861 case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY: 00862 00863 pdev->offset_results.active_results = 1; 00864 00865 break; 00866 00867 default: 00868 00869 pdev->customer.mm_config__inner_offset_mm = 0; 00870 pdev->customer.mm_config__outer_offset_mm = 0; 00871 pdev->offset_results.active_results = 00872 VL53L1_MAX_OFFSET_RANGE_RESULTS; 00873 00874 break; 00875 } 00876 00877 pdev->customer.algo__part_to_part_range_offset_mm = 0; 00878 00879 00880 00881 pdev->offset_results.max_results = VL53L1_MAX_OFFSET_RANGE_RESULTS; 00882 pdev->offset_results.cal_distance_mm = cal_distance_mm; 00883 pdev->offset_results.cal_reflectance_pc = cal_reflectance_pc; 00884 00885 for (m = 0; m < VL53L1_MAX_OFFSET_RANGE_RESULTS; m++) { 00886 00887 pfs = &(pdev->offset_results.VL53L1_p_002[m]); 00888 pfs->preset_mode = 0; 00889 pfs->no_of_samples = 0; 00890 pfs->effective_spads = 0; 00891 pfs->peak_rate_mcps = 0; 00892 pfs->VL53L1_p_005 = 0; 00893 pfs->median_range_mm = 0; 00894 } 00895 00896 00897 00898 00899 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled; 00900 00901 status = VL53L1_dynamic_xtalk_correction_disable(Dev); 00902 00903 00904 00905 for (m = 0; m < pdev->offset_results.active_results; m++) { 00906 00907 pfs = &(pdev->offset_results.VL53L1_p_002[m]); 00908 00909 pfs->preset_mode = device_preset_modes[m]; 00910 00911 00912 00913 if (status == VL53L1_ERROR_NONE) 00914 status = 00915 VL53L1_set_preset_mode( 00916 Dev, 00917 device_preset_modes[m], 00918 00919 pdev->offsetcal_cfg.dss_config__target_total_rate_mcps, 00920 pdev->offsetcal_cfg.phasecal_config_timeout_us, 00921 pdev->offsetcal_cfg.mm_config_timeout_us, 00922 pdev->offsetcal_cfg.range_config_timeout_us, 00923 00924 100); 00925 00926 pG->dss_config__manual_effective_spads_select = 00927 manual_effective_spads; 00928 00929 00930 VL53L1_load_patch(Dev); 00931 00932 if (status == VL53L1_ERROR_NONE) 00933 status = 00934 VL53L1_init_and_start_range( 00935 Dev, 00936 measurement_mode, 00937 VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS); 00938 00939 for (i = 0; i <= (num_of_samples[m]+2); i++) { 00940 00941 00942 00943 if (status == VL53L1_ERROR_NONE) 00944 status = 00945 VL53L1_wait_for_range_completion(Dev); 00946 00947 00948 00949 if (status == VL53L1_ERROR_NONE) 00950 status = 00951 VL53L1_get_device_results( 00952 Dev, 00953 VL53L1_DEVICERESULTSLEVEL_FULL, 00954 prange_results); 00955 00956 00957 00958 pRData = &(prange_results->VL53L1_p_002[0]); 00959 00960 if ((prange_results->active_results > 0 && 00961 prange_results->stream_count > 1) && 00962 (pRData->range_status == 00963 VL53L1_DEVICEERROR_RANGECOMPLETE)) { 00964 00965 pfs->no_of_samples++; 00966 pfs->effective_spads += 00967 (uint32_t)pRData->VL53L1_p_006; 00968 pfs->peak_rate_mcps += 00969 (uint32_t)pRData->peak_signal_count_rate_mcps; 00970 pfs->VL53L1_p_005 += 00971 (uint32_t)pRData->VL53L1_p_005; 00972 pfs->median_range_mm += 00973 (int32_t)pRData->median_range_mm; 00974 00975 pfs->dss_config__roi_mode_control = 00976 pG->dss_config__roi_mode_control; 00977 pfs->dss_config__manual_effective_spads_select = 00978 pG->dss_config__manual_effective_spads_select; 00979 00980 } 00981 00982 00983 00984 if (status == VL53L1_ERROR_NONE) 00985 status = 00986 VL53L1_wait_for_firmware_ready(Dev); 00987 00988 00989 00990 if (status == VL53L1_ERROR_NONE) 00991 status = 00992 VL53L1_clear_interrupt_and_enable_next_range( 00993 Dev, 00994 measurement_mode); 00995 } 00996 00997 00998 00999 if (status == VL53L1_ERROR_NONE) 01000 status = VL53L1_stop_range(Dev); 01001 01002 01003 01004 if (status == VL53L1_ERROR_NONE) 01005 status = VL53L1_WaitUs(Dev, 1000); 01006 VL53L1_unload_patch(Dev); 01007 01008 01009 if (pfs->no_of_samples > 0) { 01010 01011 pfs->effective_spads += (pfs->no_of_samples/2); 01012 pfs->effective_spads /= pfs->no_of_samples; 01013 01014 pfs->peak_rate_mcps += (pfs->no_of_samples/2); 01015 pfs->peak_rate_mcps /= pfs->no_of_samples; 01016 01017 pfs->VL53L1_p_005 += (pfs->no_of_samples/2); 01018 pfs->VL53L1_p_005 /= pfs->no_of_samples; 01019 01020 pfs->median_range_mm += (pfs->no_of_samples/2); 01021 pfs->median_range_mm /= pfs->no_of_samples; 01022 01023 pfs->range_mm_offset = (int32_t)cal_distance_mm; 01024 pfs->range_mm_offset -= pfs->median_range_mm; 01025 01026 01027 if (pfs->preset_mode == 01028 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING) 01029 manual_effective_spads = 01030 (uint16_t)pfs->effective_spads; 01031 } 01032 } 01033 01034 01035 01036 switch (pdev->offset_calibration_mode) { 01037 01038 case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY: 01039 case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY: 01040 01041 01042 pdev->customer.mm_config__inner_offset_mm += 01043 (int16_t)pdev->offset_results.VL53L1_p_002[0].range_mm_offset; 01044 pdev->customer.mm_config__outer_offset_mm += 01045 (int16_t)pdev->offset_results.VL53L1_p_002[0].range_mm_offset; 01046 break; 01047 01048 default: 01049 01050 pdev->customer.mm_config__inner_offset_mm = 01051 (int16_t)pdev->offset_results.VL53L1_p_002[1].range_mm_offset; 01052 pdev->customer.mm_config__outer_offset_mm = 01053 (int16_t)pdev->offset_results.VL53L1_p_002[2].range_mm_offset; 01054 pdev->customer.algo__part_to_part_range_offset_mm = 0; 01055 01056 01057 01058 pAO->result__mm_inner_actual_effective_spads = 01059 (uint16_t)pdev->offset_results.VL53L1_p_002[1].effective_spads; 01060 pAO->result__mm_outer_actual_effective_spads = 01061 (uint16_t)pdev->offset_results.VL53L1_p_002[2].effective_spads; 01062 01063 pAO->result__mm_inner_peak_signal_count_rtn_mcps = 01064 (uint16_t)pdev->offset_results.VL53L1_p_002[1].peak_rate_mcps; 01065 pAO->result__mm_outer_peak_signal_count_rtn_mcps = 01066 (uint16_t)pdev->offset_results.VL53L1_p_002[2].peak_rate_mcps; 01067 01068 break; 01069 } 01070 01071 01072 01073 pdev->cust_dmax_cal.ref__actual_effective_spads = 01074 (uint16_t)pdev->offset_results.VL53L1_p_002[0].effective_spads; 01075 pdev->cust_dmax_cal.ref__peak_signal_count_rate_mcps = 01076 (uint16_t)pdev->offset_results.VL53L1_p_002[0].peak_rate_mcps; 01077 01078 01079 pdev->cust_dmax_cal.ref__distance_mm = cal_distance_mm * 16; 01080 01081 pdev->cust_dmax_cal.ref_reflectance_pc = cal_reflectance_pc; 01082 pdev->cust_dmax_cal.coverglass_transmission = 0x0100; 01083 01084 01085 01086 if (status == VL53L1_ERROR_NONE) 01087 status = 01088 VL53L1_set_customer_nvm_managed( 01089 Dev, 01090 &(pdev->customer)); 01091 01092 01093 01094 01095 if (status == VL53L1_ERROR_NONE) { 01096 if (smudge_corr_en == 1) 01097 status = VL53L1_dynamic_xtalk_correction_enable(Dev); 01098 } 01099 01100 01101 01102 01103 for (m = 0; m < pdev->offset_results.active_results; m++) { 01104 01105 pfs = &(pdev->offset_results.VL53L1_p_002[m]); 01106 01107 if (status == VL53L1_ERROR_NONE) { 01108 01109 pdev->offset_results.cal_report = m; 01110 01111 if (pfs->no_of_samples < num_of_samples[m]) 01112 status = 01113 VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES; 01114 01115 01116 if (m == 0 && pfs->VL53L1_p_005 > 01117 ((uint32_t)VL53L1_OFFSET_CAL_MAX_SIGMA_MM << 5)) 01118 status = 01119 VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH; 01120 01121 if (pfs->peak_rate_mcps > 01122 VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS) 01123 status = 01124 VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH; 01125 01126 if (pfs->dss_config__manual_effective_spads_select < 01127 VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS) 01128 status = 01129 VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW; 01130 01131 if (pfs->dss_config__manual_effective_spads_select == 0) 01132 status = 01133 VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL; 01134 01135 if (pfs->no_of_samples == 0) 01136 status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL; 01137 } 01138 } 01139 01140 01141 01142 pdev->offset_results.cal_status = status; 01143 *pcal_status = pdev->offset_results.cal_status; 01144 01145 01146 01147 IGNORE_STATUS( 01148 IGNORE_OFFSET_CAL_MISSING_SAMPLES, 01149 VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES, 01150 status); 01151 01152 IGNORE_STATUS( 01153 IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH, 01154 VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH, 01155 status); 01156 01157 IGNORE_STATUS( 01158 IGNORE_OFFSET_CAL_RATE_TOO_HIGH, 01159 VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH, 01160 status); 01161 01162 IGNORE_STATUS( 01163 IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW, 01164 VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW, 01165 status); 01166 01167 #ifdef VL53L1_LOG_ENABLE 01168 01169 01170 01171 VL53L1_print_customer_nvm_managed( 01172 &(pdev->customer), 01173 "run_offset_calibration():pdev->lldata.customer.", 01174 VL53L1_TRACE_MODULE_OFFSET_DATA); 01175 01176 VL53L1_print_dmax_calibration_data( 01177 &(pdev->fmt_dmax_cal), 01178 "run_offset_calibration():pdev->lldata.fmt_dmax_cal.", 01179 VL53L1_TRACE_MODULE_OFFSET_DATA); 01180 01181 VL53L1_print_dmax_calibration_data( 01182 &(pdev->cust_dmax_cal), 01183 "run_offset_calibration():pdev->lldata.cust_dmax_cal.", 01184 VL53L1_TRACE_MODULE_OFFSET_DATA); 01185 01186 VL53L1_print_additional_offset_cal_data( 01187 &(pdev->add_off_cal_data), 01188 "run_offset_calibration():pdev->lldata.add_off_cal_data.", 01189 VL53L1_TRACE_MODULE_OFFSET_DATA); 01190 01191 VL53L1_print_offset_range_results( 01192 &(pdev->offset_results), 01193 "run_offset_calibration():pdev->lldata.offset_results.", 01194 VL53L1_TRACE_MODULE_OFFSET_DATA); 01195 #endif 01196 01197 LOG_FUNCTION_END(status); 01198 01199 return status; 01200 } 01201 01202 01203 VL53L1_Error VL53L1_run_phasecal_average( 01204 VL53L1_DEV Dev, 01205 uint8_t measurement_mode, 01206 uint8_t phasecal_result__vcsel_start, 01207 uint16_t phasecal_num_of_samples, 01208 VL53L1_range_results_t *prange_results, 01209 uint16_t *pphasecal_result__reference_phase, 01210 uint16_t *pzero_distance_phase) 01211 { 01212 01213 01214 VL53L1_Error status = VL53L1_ERROR_NONE; 01215 VL53L1_LLDriverData_t *pdev = 01216 VL53L1DevStructGetLLDriverHandle(Dev); 01217 01218 uint16_t i = 0; 01219 uint16_t m = 0; 01220 uint32_t samples = 0; 01221 01222 uint32_t period = 0; 01223 uint32_t VL53L1_p_017 = 0; 01224 uint32_t phasecal_result__reference_phase = 0; 01225 uint32_t zero_distance_phase = 0; 01226 01227 01228 VL53L1_load_patch(Dev); 01229 01230 for (m = 0; m < phasecal_num_of_samples; m++) { 01231 01232 01233 01234 if (status == VL53L1_ERROR_NONE) 01235 status = 01236 VL53L1_init_and_start_range( 01237 Dev, 01238 measurement_mode, 01239 VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS); 01240 01241 for (i = 0; i <= 1; i++) { 01242 01243 01244 01245 if (status == VL53L1_ERROR_NONE) 01246 status = 01247 VL53L1_wait_for_range_completion(Dev); 01248 01249 01250 01251 if (status == VL53L1_ERROR_NONE) 01252 status = 01253 VL53L1_get_device_results( 01254 Dev, 01255 VL53L1_DEVICERESULTSLEVEL_FULL, 01256 prange_results); 01257 01258 01259 01260 if (status == VL53L1_ERROR_NONE) 01261 status = 01262 VL53L1_wait_for_firmware_ready(Dev); 01263 01264 01265 01266 if (status == VL53L1_ERROR_NONE) 01267 status = 01268 VL53L1_clear_interrupt_and_enable_next_range( 01269 Dev, 01270 measurement_mode); 01271 } 01272 01273 01274 01275 if (status == VL53L1_ERROR_NONE) 01276 status = VL53L1_stop_range(Dev); 01277 01278 01279 01280 if (status == VL53L1_ERROR_NONE) 01281 status = VL53L1_WaitUs(Dev, 1000); 01282 01283 01284 01285 if (status == VL53L1_ERROR_NONE) { 01286 01287 samples++; 01288 01289 01290 period = 2048 * 01291 (uint32_t)VL53L1_decode_vcsel_period( 01292 pdev->hist_data.VL53L1_p_009); 01293 01294 VL53L1_p_017 = period; 01295 VL53L1_p_017 += (uint32_t)( 01296 pdev->hist_data.phasecal_result__reference_phase); 01297 VL53L1_p_017 += 01298 (2048 * 01299 (uint32_t)phasecal_result__vcsel_start); 01300 VL53L1_p_017 -= (2048 * 01301 (uint32_t)pdev->hist_data.cal_config__vcsel_start); 01302 01303 if (period != 0) { 01304 VL53L1_p_017 = VL53L1_p_017 % period; 01305 } 01306 else { 01307 status = 01308 VL53L1_ERROR_DIVISION_BY_ZERO; 01309 VL53L1_p_017 = 0; 01310 } 01311 01312 phasecal_result__reference_phase += (uint32_t)( 01313 pdev->hist_data.phasecal_result__reference_phase); 01314 01315 zero_distance_phase += (uint32_t)VL53L1_p_017; 01316 } 01317 } 01318 VL53L1_unload_patch(Dev); 01319 01320 01321 01322 if (status == VL53L1_ERROR_NONE && samples > 0) { 01323 01324 phasecal_result__reference_phase += (samples >> 1); 01325 phasecal_result__reference_phase /= samples; 01326 01327 zero_distance_phase += (samples >> 1); 01328 zero_distance_phase /= samples; 01329 01330 *pphasecal_result__reference_phase = 01331 (uint16_t)phasecal_result__reference_phase; 01332 *pzero_distance_phase = 01333 (uint16_t)zero_distance_phase; 01334 } 01335 01336 return status; 01337 } 01338 01339 01340 VL53L1_Error VL53L1_run_zone_calibration( 01341 VL53L1_DEV Dev, 01342 VL53L1_DevicePresetModes device_preset_mode, 01343 VL53L1_DeviceZonePreset zone_preset, 01344 VL53L1_zone_config_t *pzone_cfg, 01345 int16_t cal_distance_mm, 01346 uint16_t cal_reflectance_pc, 01347 VL53L1_Error *pcal_status) 01348 { 01349 01350 01351 VL53L1_Error status = VL53L1_ERROR_NONE; 01352 VL53L1_LLDriverData_t *pdev = 01353 VL53L1DevStructGetLLDriverHandle(Dev); 01354 01355 VL53L1_LLDriverResults_t *pres = 01356 VL53L1DevStructGetLLResultsHandle(Dev); 01357 01358 VL53L1_range_results_t *pRR = 01359 (VL53L1_range_results_t *) pdev->wArea1; 01360 VL53L1_range_data_t *prange_data = NULL; 01361 VL53L1_zone_calibration_data_t *pzone_data = NULL; 01362 01363 uint16_t i = 0; 01364 uint16_t m = 0; 01365 01366 uint8_t z = 0; 01367 uint8_t measurement_mode = 01368 VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK; 01369 01370 VL53L1_OffsetCorrectionMode offset_cor_mode = 01371 VL53L1_OFFSETCORRECTIONMODE__NONE; 01372 01373 LOG_FUNCTION_START(""); 01374 01375 01376 01377 if (status == VL53L1_ERROR_NONE) 01378 status = 01379 VL53L1_set_preset_mode( 01380 Dev, 01381 device_preset_mode, 01382 01383 pdev->zonecal_cfg.dss_config__target_total_rate_mcps, 01384 pdev->zonecal_cfg.phasecal_config_timeout_us, 01385 pdev->zonecal_cfg.mm_config_timeout_us, 01386 pdev->zonecal_cfg.range_config_timeout_us, 01387 01388 100); 01389 01390 01391 01392 if (zone_preset == VL53L1_DEVICEZONEPRESET_CUSTOM) { 01393 01394 if (status == VL53L1_ERROR_NONE) 01395 status = 01396 VL53L1_set_zone_config( 01397 Dev, 01398 pzone_cfg); 01399 01400 } else if (zone_preset != VL53L1_DEVICEZONEPRESET_NONE) { 01401 01402 if (status == VL53L1_ERROR_NONE) 01403 status = 01404 VL53L1_set_zone_preset( 01405 Dev, 01406 zone_preset); 01407 } 01408 01409 01410 01411 pres->zone_cal.preset_mode = device_preset_mode; 01412 pres->zone_cal.zone_preset = zone_preset; 01413 01414 pres->zone_cal.cal_distance_mm = cal_distance_mm * 16; 01415 pres->zone_cal.cal_reflectance_pc = cal_reflectance_pc; 01416 pres->zone_cal.max_zones = VL53L1_MAX_USER_ZONES; 01417 pres->zone_cal.active_zones = pdev->zone_cfg.active_zones + 1; 01418 01419 for (i = 0; i < VL53L1_MAX_USER_ZONES; i++) { 01420 pres->zone_cal.VL53L1_p_002[i].no_of_samples = 0; 01421 pres->zone_cal.VL53L1_p_002[i].effective_spads = 0; 01422 pres->zone_cal.VL53L1_p_002[i].peak_rate_mcps = 0; 01423 pres->zone_cal.VL53L1_p_002[i].VL53L1_p_014 = 0; 01424 pres->zone_cal.VL53L1_p_002[i].VL53L1_p_005 = 0; 01425 pres->zone_cal.VL53L1_p_002[i].median_range_mm = 0; 01426 pres->zone_cal.VL53L1_p_002[i].range_mm_offset = 0; 01427 } 01428 01429 pres->zone_cal.phasecal_result__reference_phase = 0; 01430 pres->zone_cal.zero_distance_phase = 0; 01431 01432 01433 01434 status = 01435 VL53L1_get_offset_correction_mode( 01436 Dev, 01437 &offset_cor_mode); 01438 01439 if (status == VL53L1_ERROR_NONE) 01440 status = 01441 VL53L1_set_offset_correction_mode( 01442 Dev, 01443 VL53L1_OFFSETCORRECTIONMODE__NONE); 01444 01445 01446 VL53L1_load_patch(Dev); 01447 01448 if (status == VL53L1_ERROR_NONE) 01449 status = 01450 VL53L1_init_and_start_range( 01451 Dev, 01452 measurement_mode, 01453 VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS); 01454 01455 01456 01457 01458 m = (pdev->zonecal_cfg.zone_num_of_samples + 2) * 01459 (uint16_t)pres->zone_cal.active_zones; 01460 01461 01462 for (i = 0; i <= m; i++) { 01463 01464 01465 01466 if (status == VL53L1_ERROR_NONE) 01467 status = 01468 VL53L1_wait_for_range_completion(Dev); 01469 01470 01471 01472 if (status == VL53L1_ERROR_NONE) 01473 status = 01474 VL53L1_get_device_results( 01475 Dev, 01476 VL53L1_DEVICERESULTSLEVEL_FULL, 01477 pRR); 01478 01479 01480 01481 prange_data = &(pRR->VL53L1_p_002[0]); 01482 01483 if (pRR->active_results > 0 && 01484 i > (uint16_t)pres->zone_cal.active_zones) { 01485 01486 if (prange_data->range_status == 01487 VL53L1_DEVICEERROR_RANGECOMPLETE) { 01488 01489 pres->zone_cal.phasecal_result__reference_phase 01490 = 01491 pdev->hist_data.phasecal_result__reference_phase 01492 ; 01493 pres->zone_cal.zero_distance_phase = 01494 pdev->hist_data.zero_distance_phase; 01495 01496 pzone_data = 01497 &(pres->zone_cal.VL53L1_p_002[pRR->zone_id]); 01498 pzone_data->no_of_samples++; 01499 pzone_data->effective_spads += 01500 (uint32_t)prange_data->VL53L1_p_006; 01501 pzone_data->peak_rate_mcps += (uint32_t)( 01502 prange_data->peak_signal_count_rate_mcps); 01503 pzone_data->VL53L1_p_014 += 01504 (uint32_t)prange_data->VL53L1_p_014; 01505 pzone_data->VL53L1_p_005 += 01506 (uint32_t)prange_data->VL53L1_p_005; 01507 pzone_data->median_range_mm += 01508 (int32_t)prange_data->median_range_mm; 01509 01510 } 01511 } 01512 01513 01514 01515 if (status == VL53L1_ERROR_NONE) 01516 status = 01517 VL53L1_wait_for_firmware_ready(Dev); 01518 01519 01520 01521 if (status == VL53L1_ERROR_NONE) 01522 status = 01523 VL53L1_clear_interrupt_and_enable_next_range( 01524 Dev, 01525 measurement_mode); 01526 } 01527 01528 01529 01530 if (status == VL53L1_ERROR_NONE) 01531 status = VL53L1_stop_range(Dev); 01532 01533 01534 if (status == VL53L1_ERROR_NONE) 01535 status = VL53L1_WaitUs(Dev, 1000); 01536 VL53L1_unload_patch(Dev); 01537 01538 01539 if (status == VL53L1_ERROR_NONE) 01540 status = 01541 VL53L1_run_phasecal_average( 01542 Dev, 01543 measurement_mode, 01544 pdev->hist_data.phasecal_result__vcsel_start, 01545 01546 pdev->zonecal_cfg.phasecal_num_of_samples, 01547 01548 pRR, 01549 &(pres->zone_cal.phasecal_result__reference_phase), 01550 &(pres->zone_cal.zero_distance_phase)); 01551 01552 01553 01554 if (status == VL53L1_ERROR_NONE) 01555 status = 01556 VL53L1_set_offset_correction_mode( 01557 Dev, 01558 offset_cor_mode); 01559 01560 01561 01562 if (status == VL53L1_ERROR_NONE) { 01563 01564 for (z = 0; z < pres->zone_cal.active_zones; z++) { 01565 01566 pzone_data = &(pres->zone_cal.VL53L1_p_002[z]); 01567 01568 01569 if (pzone_data->no_of_samples > 0) { 01570 01571 pzone_data->effective_spads += 01572 (pzone_data->no_of_samples/2); 01573 pzone_data->effective_spads /= 01574 pzone_data->no_of_samples; 01575 01576 pzone_data->peak_rate_mcps += 01577 (pzone_data->no_of_samples/2); 01578 pzone_data->peak_rate_mcps /= 01579 pzone_data->no_of_samples; 01580 01581 pzone_data->VL53L1_p_014 += 01582 (pzone_data->no_of_samples/2); 01583 pzone_data->VL53L1_p_014 /= 01584 pzone_data->no_of_samples; 01585 01586 pzone_data->VL53L1_p_005 += 01587 (pzone_data->no_of_samples/2); 01588 pzone_data->VL53L1_p_005 /= 01589 pzone_data->no_of_samples; 01590 01591 01592 01593 pzone_data->median_range_mm = 01594 VL53L1_range_maths( 01595 pdev->stat_nvm.osc_measured__fast_osc__frequency 01596 , (uint16_t)pzone_data->VL53L1_p_014, 01597 pres->zone_cal.zero_distance_phase, 01598 2, 01599 0x0800, 01600 0); 01601 01602 pzone_data->range_mm_offset = 01603 ((int32_t)cal_distance_mm) * 4; 01604 pzone_data->range_mm_offset -= 01605 pzone_data->median_range_mm; 01606 01607 01608 if (pzone_data->no_of_samples < 01609 pdev->zonecal_cfg.zone_num_of_samples) 01610 status = 01611 VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES; 01612 01613 01614 if (pzone_data->VL53L1_p_005 > 01615 ((uint32_t)VL53L1_ZONE_CAL_MAX_SIGMA_MM 01616 << 5)) 01617 status = 01618 VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH; 01619 01620 if (pzone_data->peak_rate_mcps > 01621 VL53L1_ZONE_CAL_MAX_PRE_PEAK_RATE_MCPS) 01622 status = 01623 VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH; 01624 01625 } else { 01626 status = VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL; 01627 } 01628 } 01629 } 01630 01631 01632 01633 pres->zone_cal.cal_status = status; 01634 *pcal_status = pres->zone_cal.cal_status; 01635 01636 01637 01638 IGNORE_STATUS( 01639 IGNORE_ZONE_CAL_MISSING_SAMPLES, 01640 VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES, 01641 status); 01642 01643 IGNORE_STATUS( 01644 IGNORE_ZONE_CAL_SIGMA_TOO_HIGH, 01645 VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH, 01646 status); 01647 01648 IGNORE_STATUS( 01649 IGNORE_ZONE_CAL_RATE_TOO_HIGH, 01650 VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH, 01651 status); 01652 01653 #ifdef VL53L1_LOG_ENABLE 01654 01655 01656 01657 VL53L1_print_zone_calibration_results( 01658 &(pres->zone_cal), 01659 "run_zone_calibration():pdev->llresults.zone_cal.", 01660 VL53L1_TRACE_MODULE_OFFSET_DATA); 01661 01662 #endif 01663 01664 LOG_FUNCTION_END(status); 01665 01666 return status; 01667 } 01668 01669 01670 VL53L1_Error VL53L1_run_spad_rate_map( 01671 VL53L1_DEV Dev, 01672 VL53L1_DeviceTestMode device_test_mode, 01673 VL53L1_DeviceSscArray array_select, 01674 uint32_t ssc_config_timeout_us, 01675 VL53L1_spad_rate_data_t *pspad_rate_data) 01676 { 01677 01678 01679 01680 VL53L1_Error status = VL53L1_ERROR_NONE; 01681 01682 VL53L1_LLDriverData_t *pdev = 01683 VL53L1DevStructGetLLDriverHandle(Dev); 01684 01685 LOG_FUNCTION_START(""); 01686 01687 01688 if (status == VL53L1_ERROR_NONE) 01689 status = VL53L1_enable_powerforce(Dev); 01690 01691 01692 01693 if (status == VL53L1_ERROR_NONE) { 01694 pdev->ssc_cfg.array_select = array_select; 01695 pdev->ssc_cfg.timeout_us = ssc_config_timeout_us; 01696 status = 01697 VL53L1_set_ssc_config( 01698 Dev, 01699 &(pdev->ssc_cfg), 01700 pdev->stat_nvm.osc_measured__fast_osc__frequency); 01701 } 01702 01703 01704 01705 if (status == VL53L1_ERROR_NONE) 01706 status = 01707 VL53L1_run_device_test( 01708 Dev, 01709 device_test_mode); 01710 01711 01712 01713 if (status == VL53L1_ERROR_NONE) 01714 status = 01715 VL53L1_get_spad_rate_data( 01716 Dev, 01717 pspad_rate_data); 01718 01719 if (device_test_mode == VL53L1_DEVICETESTMODE_LCR_VCSEL_ON) 01720 pspad_rate_data->fractional_bits = 7; 01721 else 01722 pspad_rate_data->fractional_bits = 15; 01723 01724 01725 01726 if (status == VL53L1_ERROR_NONE) 01727 status = VL53L1_disable_powerforce(Dev); 01728 01729 #ifdef VL53L1_LOG_ENABLE 01730 01731 01732 if (status == VL53L1_ERROR_NONE) { 01733 VL53L1_print_spad_rate_data( 01734 pspad_rate_data, 01735 "run_spad_rate_map():", 01736 VL53L1_TRACE_MODULE_SPAD_RATE_MAP); 01737 VL53L1_print_spad_rate_map( 01738 pspad_rate_data, 01739 "run_spad_rate_map():", 01740 VL53L1_TRACE_MODULE_SPAD_RATE_MAP); 01741 } 01742 #endif 01743 01744 LOG_FUNCTION_END(status); 01745 01746 return status; 01747 } 01748 01749 01750 VL53L1_Error VL53L1_run_device_test( 01751 VL53L1_DEV Dev, 01752 VL53L1_DeviceTestMode device_test_mode) 01753 { 01754 01755 01756 VL53L1_Error status = VL53L1_ERROR_NONE; 01757 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 01758 01759 uint8_t comms_buffer[2]; 01760 uint8_t gpio_hv_mux__ctrl = 0; 01761 01762 LOG_FUNCTION_START(""); 01763 01764 01765 01766 if (status == VL53L1_ERROR_NONE) 01767 status = 01768 VL53L1_RdByte( 01769 Dev, 01770 VL53L1_GPIO_HV_MUX__CTRL, 01771 &gpio_hv_mux__ctrl); 01772 01773 if (status == VL53L1_ERROR_NONE) 01774 pdev->stat_cfg.gpio_hv_mux__ctrl = gpio_hv_mux__ctrl; 01775 01776 01777 if (status == VL53L1_ERROR_NONE) 01778 status = VL53L1_start_test( 01779 Dev, 01780 device_test_mode); 01781 01782 01783 if (status == VL53L1_ERROR_NONE) 01784 status = VL53L1_wait_for_test_completion(Dev); 01785 01786 01787 if (status == VL53L1_ERROR_NONE) 01788 status = 01789 VL53L1_ReadMulti( 01790 Dev, 01791 VL53L1_RESULT__RANGE_STATUS, 01792 comms_buffer, 01793 2); 01794 01795 if (status == VL53L1_ERROR_NONE) { 01796 pdev->sys_results.result__range_status = comms_buffer[0]; 01797 pdev->sys_results.result__report_status = comms_buffer[1]; 01798 } 01799 01800 01801 01802 pdev->sys_results.result__range_status &= 01803 VL53L1_RANGE_STATUS__RANGE_STATUS_MASK; 01804 01805 if (status == VL53L1_ERROR_NONE) { 01806 trace_print( 01807 VL53L1_TRACE_LEVEL_INFO, 01808 " Device Test Complete:\n\t%-32s = %3u\n\t%-32s = %3u\n", 01809 "result__range_status", 01810 pdev->sys_results.result__range_status, 01811 "result__report_status", 01812 pdev->sys_results.result__report_status); 01813 01814 01815 if (status == VL53L1_ERROR_NONE) 01816 status = VL53L1_clear_interrupt(Dev); 01817 } 01818 01819 01820 01821 if (status == VL53L1_ERROR_NONE) 01822 status = 01823 VL53L1_start_test( 01824 Dev, 01825 0x00); 01826 01827 LOG_FUNCTION_END(status); 01828 01829 return status; 01830 } 01831 01832 01833 void VL53L1_hist_xtalk_extract_data_init( 01834 VL53L1_hist_xtalk_extract_data_t *pxtalk_data) 01835 { 01836 01837 01838 int32_t lb = 0; 01839 01840 pxtalk_data->sample_count = 0U; 01841 pxtalk_data->pll_period_mm = 0U; 01842 pxtalk_data->peak_duration_us_sum = 0U; 01843 pxtalk_data->effective_spad_count_sum = 0U; 01844 pxtalk_data->zero_distance_phase_sum = 0U; 01845 pxtalk_data->zero_distance_phase_avg = 0U; 01846 pxtalk_data->event_scaler_sum = 0U; 01847 pxtalk_data->event_scaler_avg = 4096U; 01848 pxtalk_data->signal_events_sum = 0; 01849 pxtalk_data->xtalk_rate_kcps_per_spad = 0U; 01850 pxtalk_data->VL53L1_p_015 = 0U; 01851 pxtalk_data->VL53L1_p_016 = 0U; 01852 pxtalk_data->target_start = 0U; 01853 01854 for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS; lb++) 01855 pxtalk_data->bin_data_sums[lb] = 0; 01856 01857 } 01858 01859 01860 VL53L1_Error VL53L1_hist_xtalk_extract_update( 01861 int16_t target_distance_mm, 01862 uint16_t target_width_oversize, 01863 VL53L1_histogram_bin_data_t *phist_bins, 01864 VL53L1_hist_xtalk_extract_data_t *pxtalk_data) 01865 { 01866 01867 01868 VL53L1_Error status = VL53L1_ERROR_NONE; 01869 01870 LOG_FUNCTION_START(""); 01871 01872 status = 01873 VL53L1_hist_xtalk_extract_calc_window( 01874 target_distance_mm, 01875 target_width_oversize, 01876 phist_bins, 01877 pxtalk_data); 01878 01879 if (status == VL53L1_ERROR_NONE) { 01880 status = 01881 VL53L1_hist_xtalk_extract_calc_event_sums( 01882 phist_bins, 01883 pxtalk_data); 01884 } 01885 01886 LOG_FUNCTION_END(status); 01887 01888 return status; 01889 } 01890 01891 01892 VL53L1_Error VL53L1_hist_xtalk_extract_fini( 01893 VL53L1_histogram_bin_data_t *phist_bins, 01894 VL53L1_hist_xtalk_extract_data_t *pxtalk_data, 01895 VL53L1_xtalk_calibration_results_t *pxtalk_cal, 01896 VL53L1_xtalk_histogram_shape_t *pxtalk_shape) 01897 { 01898 01899 01900 VL53L1_Error status = VL53L1_ERROR_NONE; 01901 VL53L1_xtalk_calibration_results_t *pX = pxtalk_cal; 01902 01903 LOG_FUNCTION_START(""); 01904 01905 if (pxtalk_data->sample_count > 0) { 01906 01907 01908 pxtalk_data->event_scaler_avg = pxtalk_data->event_scaler_sum; 01909 pxtalk_data->event_scaler_avg += 01910 (pxtalk_data->sample_count >> 1); 01911 pxtalk_data->event_scaler_avg /= pxtalk_data->sample_count; 01912 01913 01914 01915 status = 01916 VL53L1_hist_xtalk_extract_calc_rate_per_spad( 01917 pxtalk_data); 01918 01919 01920 01921 if (status == VL53L1_ERROR_NONE) { 01922 01923 01924 pxtalk_data->zero_distance_phase_avg = 01925 pxtalk_data->zero_distance_phase_sum; 01926 pxtalk_data->zero_distance_phase_avg += 01927 (pxtalk_data->sample_count >> 1); 01928 pxtalk_data->zero_distance_phase_avg /= 01929 pxtalk_data->sample_count; 01930 01931 01932 status = 01933 VL53L1_hist_xtalk_extract_calc_shape( 01934 pxtalk_data, 01935 pxtalk_shape); 01936 01937 01938 01939 01940 pxtalk_shape->phasecal_result__vcsel_start = 01941 phist_bins->phasecal_result__vcsel_start; 01942 pxtalk_shape->cal_config__vcsel_start = 01943 phist_bins->cal_config__vcsel_start; 01944 pxtalk_shape->vcsel_width = 01945 phist_bins->vcsel_width; 01946 pxtalk_shape->VL53L1_p_019 = 01947 phist_bins->VL53L1_p_019; 01948 } 01949 01950 01951 if (status == VL53L1_ERROR_NONE) { 01952 01953 01954 pX->algo__crosstalk_compensation_plane_offset_kcps = 01955 pxtalk_data->xtalk_rate_kcps_per_spad; 01956 pX->algo__crosstalk_compensation_x_plane_gradient_kcps 01957 = 0U; 01958 pX->algo__crosstalk_compensation_y_plane_gradient_kcps 01959 = 0U; 01960 01961 } 01962 } 01963 01964 LOG_FUNCTION_END(status); 01965 01966 return status; 01967 } 01968 01969 01970 VL53L1_Error VL53L1_run_hist_xtalk_extraction( 01971 VL53L1_DEV Dev, 01972 int16_t cal_distance_mm, 01973 VL53L1_Error *pcal_status) 01974 { 01975 01976 01977 #define OVERSIZE 4 01978 VL53L1_Error status = VL53L1_ERROR_NONE; 01979 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); 01980 VL53L1_xtalkextract_config_t *pX = &(pdev->xtalk_extract_cfg); 01981 VL53L1_xtalk_config_t *pC = &(pdev->xtalk_cfg); 01982 VL53L1_xtalk_calibration_results_t *pXC = &(pdev->xtalk_cal); 01983 01984 01985 01986 uint8_t smudge_corr_en = 0; 01987 uint8_t i = 0; 01988 int8_t k = 0; 01989 uint8_t nbloops; 01990 int32_t initMergeSize = 0; 01991 int32_t MergeEnabled = 0; 01992 uint32_t deltaXtalk; 01993 uint32_t stepXtalk; 01994 uint32_t XtalkMin; 01995 uint32_t XtalkMax; 01996 uint8_t measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK; 01997 int8_t MaxId; 01998 uint8_t histo_merge_nb; 01999 uint8_t wait_for_accumulation; 02000 VL53L1_range_results_t *prange_results = 02001 (VL53L1_range_results_t *) pdev->wArea1; 02002 uint8_t Very1stRange = 0; 02003 02004 LOG_FUNCTION_START(""); 02005 02006 02007 02008 if (status == VL53L1_ERROR_NONE) 02009 status = 02010 VL53L1_set_preset_mode( 02011 Dev, 02012 VL53L1_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE, 02013 pX->dss_config__target_total_rate_mcps, 02014 pX->phasecal_config_timeout_us, 02015 pX->mm_config_timeout_us, 02016 pX->range_config_timeout_us, 02017 100); 02018 02019 02020 02021 if (status == VL53L1_ERROR_NONE) 02022 status = VL53L1_disable_xtalk_compensation(Dev); 02023 02024 02025 02026 smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled; 02027 02028 if (status == VL53L1_ERROR_NONE) 02029 status = VL53L1_dynamic_xtalk_correction_disable(Dev); 02030 02031 02032 VL53L1_load_patch(Dev); 02033 02034 VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE_MAX_SIZE, 02035 &initMergeSize); 02036 VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, 02037 &MergeEnabled); 02038 memset(&pdev->xtalk_cal, 0, sizeof(pdev->xtalk_cal)); 02039 02040 if (status == VL53L1_ERROR_NONE) 02041 status = VL53L1_init_and_start_range( 02042 Dev, measurement_mode, 02043 VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS); 02044 02045 MaxId = pdev->tuning_parms.tp_hist_merge_max_size - 1; 02046 nbloops = (MergeEnabled == 0 ? 1 : 2); 02047 for (k = 0; k < nbloops; k++) { 02048 02049 VL53L1_hist_xtalk_extract_data_init( 02050 &(pdev->xtalk_extract)); 02051 VL53L1_set_tuning_parm(Dev, 02052 VL53L1_TUNINGPARM_HIST_MERGE_MAX_SIZE, 02053 k * MaxId + 1); 02054 02055 for (i = 0; i <= pX->num_of_samples; i++) { 02056 if (status == VL53L1_ERROR_NONE) 02057 status = VL53L1_wait_for_range_completion(Dev); 02058 if (status == VL53L1_ERROR_NONE) 02059 status = VL53L1_get_device_results(Dev, 02060 VL53L1_DEVICERESULTSLEVEL_FULL, 02061 prange_results); 02062 Very1stRange = 02063 (pdev->ll_state.rd_device_state == 02064 VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC); 02065 02066 VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb); 02067 wait_for_accumulation = ((k != 0) && 02068 (MergeEnabled) && 02069 (status == VL53L1_ERROR_NONE) && 02070 (histo_merge_nb < 02071 pdev->tuning_parms.tp_hist_merge_max_size)); 02072 if (wait_for_accumulation) 02073 i = 0; 02074 else { 02075 if ((status == VL53L1_ERROR_NONE) && 02076 (!Very1stRange)) { 02077 status = 02078 VL53L1_hist_xtalk_extract_update( 02079 cal_distance_mm, 02080 OVERSIZE, 02081 &(pdev->hist_data), 02082 &(pdev->xtalk_extract)); 02083 } 02084 } 02085 02086 if (status == VL53L1_ERROR_NONE) 02087 status = VL53L1_wait_for_firmware_ready(Dev); 02088 if (status == VL53L1_ERROR_NONE) 02089 status = 02090 VL53L1_clear_interrupt_and_enable_next_range( 02091 Dev, measurement_mode); 02092 } 02093 02094 02095 if (status == VL53L1_ERROR_NONE) 02096 status = 02097 VL53L1_hist_xtalk_extract_fini( 02098 &(pdev->hist_data), 02099 &(pdev->xtalk_extract), 02100 &(pdev->xtalk_cal), 02101 &(pdev->xtalk_shapes.xtalk_shape)); 02102 if (status == VL53L1_ERROR_NONE) { 02103 pXC->algo__xtalk_cpo_HistoMerge_kcps[k * MaxId] = 02104 pXC->algo__crosstalk_compensation_plane_offset_kcps; 02105 } 02106 } 02107 02108 02109 VL53L1_stop_range(Dev); 02110 02111 VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE_MAX_SIZE, 02112 initMergeSize); 02113 VL53L1_unload_patch(Dev); 02114 02115 if (status != VL53L1_ERROR_NONE) 02116 status = VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL; 02117 else if ((MergeEnabled == 1) && (MaxId > 0)) { 02118 XtalkMin = pXC->algo__xtalk_cpo_HistoMerge_kcps[0]; 02119 XtalkMax = pXC->algo__xtalk_cpo_HistoMerge_kcps[MaxId]; 02120 pXC->algo__crosstalk_compensation_plane_offset_kcps = 02121 XtalkMin; 02122 if (XtalkMax >= XtalkMin) { 02123 deltaXtalk = XtalkMax - XtalkMin; 02124 stepXtalk = deltaXtalk / MaxId; 02125 for (k = 1; k < MaxId; k++) 02126 pXC->algo__xtalk_cpo_HistoMerge_kcps[k] = 02127 XtalkMin + stepXtalk * k; 02128 } else 02129 status = 02130 VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL; 02131 } 02132 02133 if (status == VL53L1_ERROR_NONE) { 02134 pC->algo__crosstalk_compensation_x_plane_gradient_kcps = 02135 pXC->algo__crosstalk_compensation_x_plane_gradient_kcps; 02136 pC->algo__crosstalk_compensation_y_plane_gradient_kcps = 02137 pXC->algo__crosstalk_compensation_y_plane_gradient_kcps; 02138 pC->algo__crosstalk_compensation_plane_offset_kcps = 02139 pXC->algo__crosstalk_compensation_plane_offset_kcps; 02140 } 02141 02142 02143 pdev->xtalk_results.cal_status = status; 02144 *pcal_status = pdev->xtalk_results.cal_status; 02145 02146 02147 status = VL53L1_enable_xtalk_compensation(Dev); 02148 if (smudge_corr_en == 1) 02149 status = VL53L1_dynamic_xtalk_correction_enable(Dev); 02150 02151 #ifdef VL53L1_LOG_ENABLE 02152 02153 02154 02155 VL53L1_print_customer_nvm_managed( 02156 &(pdev->customer), 02157 "run_xtalk_extraction():pdev->lldata.customer.", 02158 VL53L1_TRACE_MODULE_XTALK_DATA); 02159 02160 VL53L1_print_xtalk_config( 02161 &(pdev->xtalk_cfg), 02162 "run_xtalk_extraction():pdev->lldata.xtalk_cfg.", 02163 VL53L1_TRACE_MODULE_XTALK_DATA); 02164 02165 VL53L1_print_xtalk_histogram_data( 02166 &(pdev->xtalk_shapes), 02167 "pdev->lldata.xtalk_shapes.", 02168 VL53L1_TRACE_MODULE_XTALK_DATA); 02169 02170 #endif 02171 02172 LOG_FUNCTION_END(status); 02173 02174 return status; 02175 } 02176
Generated on Thu Jul 14 2022 10:14:48 by
1.7.2