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