charles macneill
Initial release. Mbed library for VL53L1CB
Embed:
(wiki syntax)
Show/hide line numbers
vl53l1_xtalk.c
00001 00002 /******************************************************************************* 00003 This file is part of VL53L1 Protected 00004 00005 Copyright (c) 2020, STMicroelectronics - All Rights Reserved 00006 00007 License terms: STMicroelectronics Proprietary in accordance with licensing 00008 terms at www.st.com/sla0081 00009 00010 STMicroelectronics confidential 00011 Reproduction and Communication of this document is strictly prohibited unless 00012 specifically authorized in writing by STMicroelectronics. 00013 00014 */ 00015 00016 00017 00018 00019 #include "vl53l1_types.h" 00020 #include "vl53l1_platform_log.h" 00021 00022 #include "vl53l1_core_support.h" 00023 #include "vl53l1_error_codes.h" 00024 00025 #include "vl53l1_xtalk.h" 00026 #include "vl53l1_hist_core.h" 00027 00028 00029 00030 00031 #define LOG_FUNCTION_START(fmt, ...) \ 00032 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__) 00033 #define LOG_FUNCTION_END(status, ...) \ 00034 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__) 00035 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 00036 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \ 00037 status, fmt, ##__VA_ARGS__) 00038 00039 #define trace_print(level, ...) \ 00040 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \ 00041 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) 00042 00043 00044 VL53L1_Error VL53L1_xtalk_calibration_process_data( 00045 VL53L1_xtalk_range_results_t *pxtalk_results, 00046 VL53L1_xtalk_histogram_data_t *pxtalk_shape, 00047 VL53L1_xtalk_calibration_results_t *pxtalk_cal) 00048 { 00049 00050 00051 VL53L1_Error status = VL53L1_ERROR_NONE; 00052 00053 VL53L1_xtalk_algo_data_t xtalk_debug; 00054 VL53L1_xtalk_algo_data_t *pdebug = &xtalk_debug; 00055 VL53L1_xtalk_range_data_t *pxtalk_data = NULL; 00056 00057 VL53L1_histogram_bin_data_t avg_bins; 00058 VL53L1_histogram_bin_data_t *pavg_bins = &avg_bins; 00059 00060 LOG_FUNCTION_START(""); 00061 00062 00063 00064 memcpy(pavg_bins, &(pxtalk_results->central_histogram_avg), 00065 sizeof(VL53L1_histogram_bin_data_t)); 00066 00067 00068 00069 if (status == VL53L1_ERROR_NONE) 00070 VL53L1_init_histogram_bin_data_struct( 00071 0, 0, &(pdebug->VL53L1_p_057)); 00072 00073 if (status == VL53L1_ERROR_NONE) 00074 VL53L1_init_histogram_bin_data_struct( 00075 0, 0, &(pdebug->VL53L1_p_058)); 00076 00077 00078 00079 if (status == VL53L1_ERROR_NONE) 00080 status = VL53L1_f_047( 00081 pxtalk_results, 00082 pdebug, 00083 &(pxtalk_cal->algo__crosstalk_compensation_x_plane_gradient_kcps 00084 ), 00085 &(pxtalk_cal->algo__crosstalk_compensation_y_plane_gradient_kcps 00086 )); 00087 00088 00089 00090 00091 00092 if (status != VL53L1_ERROR_NONE) 00093 goto ENDFUNC; 00094 00095 pxtalk_data = &(pxtalk_results->VL53L1_p_002[4]); 00096 00097 if (pxtalk_data->no_of_samples > 0) { 00098 00099 00100 00101 if (status == VL53L1_ERROR_NONE) { 00102 memcpy(&(pdebug->VL53L1_p_057), 00103 pavg_bins, 00104 sizeof(VL53L1_histogram_bin_data_t)); 00105 } 00106 00107 00108 00109 status = VL53L1_f_048( 00110 pxtalk_data, 00111 pdebug, 00112 &(pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps)); 00113 00114 00115 00116 if (status == VL53L1_ERROR_NONE) 00117 status = VL53L1_f_049( 00118 pavg_bins, 00119 pdebug, 00120 pxtalk_data, 00121 pxtalk_results->central_histogram__window_start, 00122 pxtalk_results->central_histogram__window_end, 00123 &(pxtalk_shape->xtalk_shape)); 00124 00125 } else { 00126 00127 00128 00129 pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps = 0; 00130 00131 00132 00133 pdebug->VL53L1_p_059 = 0; 00134 00135 00136 } 00137 00138 00139 ENDFUNC: 00140 00141 00142 00143 LOG_FUNCTION_END(status); 00144 00145 return status; 00146 } 00147 00148 00149 VL53L1_Error VL53L1_generate_dual_reflectance_xtalk_samples( 00150 VL53L1_xtalk_range_results_t *pxtalk_results, 00151 uint16_t expected_target_distance_mm, 00152 uint8_t higher_reflectance, 00153 VL53L1_histogram_bin_data_t *pxtalk_avg_samples 00154 ) 00155 { 00156 00157 00158 VL53L1_Error status = VL53L1_ERROR_NONE; 00159 00160 VL53L1_histogram_bin_data_t *pzone_avg_1 = 00161 &(pxtalk_results->histogram_avg_1[0]); 00162 VL53L1_histogram_bin_data_t *pzone_avg_2 = 00163 &(pxtalk_results->histogram_avg_2[0]); 00164 00165 VL53L1_histogram_bin_data_t *pxtalk_output = pxtalk_avg_samples; 00166 00167 00168 00169 int i = 0; 00170 00171 00172 00173 for (i = 0 ; i < 5 ; i++) { 00174 00175 if (status == VL53L1_ERROR_NONE) 00176 VL53L1_init_histogram_bin_data_struct( 00177 0, 0, pzone_avg_1); 00178 00179 if (status == VL53L1_ERROR_NONE) 00180 VL53L1_init_histogram_bin_data_struct( 00181 0, 0, pzone_avg_2); 00182 00183 pzone_avg_1++; 00184 pzone_avg_2++; 00185 } 00186 00187 00188 00189 00190 pzone_avg_1 = &(pxtalk_results->histogram_avg_1[0]); 00191 pzone_avg_2 = &(pxtalk_results->histogram_avg_2[0]); 00192 00193 for (i = 0 ; i < 5 ; i++) { 00194 00195 if (status == VL53L1_ERROR_NONE) { 00196 00197 status = VL53L1_f_050( 00198 pzone_avg_1, 00199 pzone_avg_2, 00200 expected_target_distance_mm, 00201 0x01, 00202 higher_reflectance, 00203 pxtalk_output 00204 ); 00205 00206 00207 00208 pzone_avg_1++; 00209 pzone_avg_2++; 00210 pxtalk_output++; 00211 00212 } 00213 } 00214 00215 LOG_FUNCTION_END(status); 00216 00217 return status; 00218 } 00219 00220 00221 VL53L1_Error VL53L1_f_050( 00222 VL53L1_histogram_bin_data_t *pzone_avg_1, 00223 VL53L1_histogram_bin_data_t *pzone_avg_2, 00224 uint16_t expected_target_distance, 00225 uint8_t subtract_amb, 00226 uint8_t higher_reflectance, 00227 VL53L1_histogram_bin_data_t *pxtalk_output 00228 ) 00229 { 00230 00231 00232 VL53L1_Error status = VL53L1_ERROR_NONE; 00233 00234 VL53L1_histogram_bin_data_t zone_avg_realigned; 00235 00236 00237 SUPPRESS_UNUSED_WARNING(pxtalk_output); 00238 SUPPRESS_UNUSED_WARNING(expected_target_distance); 00239 00240 00241 00242 if ((status == VL53L1_ERROR_NONE) && (subtract_amb == 0x01)) { 00243 VL53L1_f_037( 00244 pzone_avg_1, 00245 pzone_avg_1->VL53L1_p_004); 00246 00247 00248 pzone_avg_1->VL53L1_p_004 = 0x0; 00249 } 00250 00251 if ((status == VL53L1_ERROR_NONE) && (subtract_amb == 0x01)) { 00252 VL53L1_f_037( 00253 pzone_avg_2, 00254 pzone_avg_2->VL53L1_p_004); 00255 00256 00257 pzone_avg_2->VL53L1_p_004 = 0x0; 00258 } 00259 00260 00261 00262 00263 00264 if (status == VL53L1_ERROR_NONE) { 00265 if (higher_reflectance == 0x01) { 00266 VL53L1_f_004( 00267 pzone_avg_2, 00268 pzone_avg_1, 00269 &zone_avg_realigned); 00270 } else { 00271 00272 00273 00274 VL53L1_f_004( 00275 pzone_avg_1, 00276 pzone_avg_2, 00277 &zone_avg_realigned); 00278 00279 00280 00281 } 00282 } 00283 00284 00285 00286 00287 00288 00289 00290 00291 LOG_FUNCTION_END(status); 00292 00293 return status; 00294 } 00295 00296 VL53L1_Error VL53L1_f_049( 00297 VL53L1_histogram_bin_data_t *pavg_bins, 00298 VL53L1_xtalk_algo_data_t *pdebug, 00299 VL53L1_xtalk_range_data_t *pxtalk_data, 00300 uint8_t histogram__window_start, 00301 uint8_t histogram__window_end, 00302 VL53L1_xtalk_histogram_shape_t *pxtalk_shape) 00303 { 00304 00305 VL53L1_Error status = VL53L1_ERROR_NONE; 00306 00307 00308 00309 uint32_t ambient_thresh = 0; 00310 00311 00312 00313 if (status == VL53L1_ERROR_NONE) 00314 VL53L1_f_037( 00315 pavg_bins, 00316 pavg_bins->VL53L1_p_004); 00317 00318 00319 00320 if (status == VL53L1_ERROR_NONE) 00321 VL53L1_f_051( 00322 6, 00323 pavg_bins->VL53L1_p_004, 00324 &ambient_thresh); 00325 00326 00327 00328 if (status == VL53L1_ERROR_NONE) 00329 status = VL53L1_f_052( 00330 pavg_bins, 00331 ambient_thresh, 00332 histogram__window_start, 00333 histogram__window_end); 00334 00335 00336 00337 if (status == VL53L1_ERROR_NONE) 00338 status = VL53L1_f_053( 00339 pavg_bins, 00340 pxtalk_data, 00341 pdebug, 00342 pxtalk_shape); 00343 00344 00345 LOG_FUNCTION_END(status); 00346 00347 return status; 00348 00349 } 00350 00351 00352 VL53L1_Error VL53L1_f_047( 00353 VL53L1_xtalk_range_results_t *pxtalk_results, 00354 VL53L1_xtalk_algo_data_t *pdebug, 00355 int16_t *xgradient, 00356 int16_t *ygradient 00357 ) 00358 { 00359 00360 00361 00362 VL53L1_Error status = VL53L1_ERROR_NONE; 00363 00364 VL53L1_xtalk_range_data_t *presults_int = NULL; 00365 00366 int i = 0; 00367 00368 uint32_t xtalk_per_spad[4]; 00369 int32_t VL53L1_p_060 = 0; 00370 int32_t VL53L1_p_061 = 0; 00371 00372 uint8_t result_invalid = 0; 00373 00374 00375 LOG_FUNCTION_START(""); 00376 00377 00378 00379 *xgradient = 0; 00380 *ygradient = 0; 00381 00382 00383 00384 00385 for (i = 0; i < 4; i++) 00386 xtalk_per_spad[i] = 0; 00387 00388 00389 00390 for (i = 0; i < 4; i++) { 00391 00392 if (status == VL53L1_ERROR_NONE) { 00393 00394 presults_int = &(pxtalk_results->VL53L1_p_002[i]); 00395 00396 00397 00398 00399 if (presults_int->no_of_samples == 0) { 00400 00401 00402 result_invalid = 1; 00403 pdebug->VL53L1_p_062[i] = 0; 00404 00405 00406 } else { 00407 00408 xtalk_per_spad[i] = 00409 presults_int->rate_per_spad_kcps_avg; 00410 00411 00412 00413 pdebug->VL53L1_p_062[i] = 00414 (uint32_t)xtalk_per_spad[i]; 00415 00416 } 00417 } 00418 00419 } 00420 00421 00422 00423 if ((status == VL53L1_ERROR_NONE) && (result_invalid == 0)) { 00424 00425 00426 00427 if (status == VL53L1_ERROR_NONE) { 00428 VL53L1_p_060 = ((int32_t)xtalk_per_spad[1] 00429 - (int32_t)xtalk_per_spad[0]) / (8); 00430 VL53L1_p_061 = ((int32_t)xtalk_per_spad[3] 00431 - (int32_t)xtalk_per_spad[2]) / (8); 00432 } 00433 00434 00435 00436 00437 if (status == VL53L1_ERROR_NONE) { 00438 if (VL53L1_p_060 < -32767) { 00439 VL53L1_p_060 = -32767; 00440 } else { 00441 if (VL53L1_p_060 > 32767) 00442 VL53L1_p_060 = 32767; 00443 } 00444 00445 if (VL53L1_p_061 < -32767) { 00446 VL53L1_p_061 = -32767; 00447 } else { 00448 if (VL53L1_p_061 > 32767) 00449 VL53L1_p_061 = 32767; 00450 } 00451 00452 00453 00454 pdebug->VL53L1_p_060 = (int16_t)VL53L1_p_060; 00455 pdebug->VL53L1_p_061 = (int16_t)VL53L1_p_061; 00456 } 00457 00458 } else { 00459 00460 00461 00462 VL53L1_p_060 = 0; 00463 VL53L1_p_061 = 0; 00464 00465 pdebug->VL53L1_p_060 = 0; 00466 pdebug->VL53L1_p_061 = 0; 00467 } 00468 00469 00470 00471 if (status == VL53L1_ERROR_NONE) { 00472 *xgradient = (int16_t)VL53L1_p_060; 00473 *ygradient = (int16_t)VL53L1_p_061; 00474 } 00475 00476 LOG_FUNCTION_END(status); 00477 00478 return status; 00479 } 00480 00481 00482 VL53L1_Error VL53L1_f_048( 00483 VL53L1_xtalk_range_data_t *pxtalk_data, 00484 VL53L1_xtalk_algo_data_t *pdebug, 00485 uint32_t *xtalk_mean_offset_kcps 00486 ) 00487 { 00488 00489 00490 VL53L1_Error status = VL53L1_ERROR_NONE; 00491 00492 uint32_t xtalk_per_spad = 0; 00493 uint8_t result_invalid = 0; 00494 00495 LOG_FUNCTION_START(""); 00496 00497 *xtalk_mean_offset_kcps = 0; 00498 00499 00500 if (pxtalk_data->no_of_samples == 0) { 00501 00502 00503 00504 result_invalid = 1; 00505 00506 00507 00508 pdebug->VL53L1_p_059 = 0; 00509 00510 } 00511 00512 00513 00514 00515 if ((status == VL53L1_ERROR_NONE) && (result_invalid == 0)) { 00516 00517 00518 00519 xtalk_per_spad = pxtalk_data->rate_per_spad_kcps_avg >> 2; 00520 00521 00522 00523 pdebug->VL53L1_p_059 = xtalk_per_spad; 00524 00525 00526 if (xtalk_per_spad < 0x3FFFF) 00527 *xtalk_mean_offset_kcps = (uint32_t)xtalk_per_spad; 00528 else 00529 *xtalk_mean_offset_kcps = 0x3FFFF; 00530 00531 } else { 00532 00533 00534 00535 *xtalk_mean_offset_kcps = 0; 00536 } 00537 00538 LOG_FUNCTION_END(status); 00539 00540 return status; 00541 00542 } 00543 00544 00545 00546 00547 VL53L1_Error VL53L1_f_053( 00548 VL53L1_histogram_bin_data_t *phist_data, 00549 VL53L1_xtalk_range_data_t *pxtalk_data, 00550 VL53L1_xtalk_algo_data_t *pdebug, 00551 VL53L1_xtalk_histogram_shape_t *pxtalk_histo 00552 ) 00553 { 00554 00555 00556 VL53L1_Error status = VL53L1_ERROR_NONE; 00557 uint8_t idx; 00558 int32_t tmpi32; 00559 uint8_t i = 0; 00560 uint64_t bin_data[VL53L1_XTALK_HISTO_BINS]; 00561 00562 LOG_FUNCTION_START(""); 00563 00564 00565 00566 00567 00568 00569 pxtalk_histo->VL53L1_p_023 = 00570 phist_data->VL53L1_p_023; 00571 pxtalk_histo->cal_config__vcsel_start = 00572 phist_data->cal_config__vcsel_start; 00573 pxtalk_histo->VL53L1_p_019 = 00574 phist_data->VL53L1_p_019; 00575 pxtalk_histo->VL53L1_p_022 = 00576 phist_data->VL53L1_p_022; 00577 pxtalk_histo->time_stamp = 00578 phist_data->time_stamp; 00579 pxtalk_histo->vcsel_width = 00580 phist_data->vcsel_width; 00581 pxtalk_histo->zero_distance_phase = 00582 phist_data->zero_distance_phase; 00583 pxtalk_histo->zone_id = 00584 phist_data->zone_id; 00585 pxtalk_histo->VL53L1_p_024 = 00586 VL53L1_XTALK_HISTO_BINS; 00587 pxtalk_histo->phasecal_result__reference_phase = 00588 phist_data->phasecal_result__reference_phase; 00589 pxtalk_histo->phasecal_result__vcsel_start = 00590 phist_data->phasecal_result__vcsel_start; 00591 00592 memcpy(&(pdebug->VL53L1_p_058), 00593 phist_data, sizeof(VL53L1_histogram_bin_data_t)); 00594 00595 00596 00597 00598 00599 if (pxtalk_data->signal_total_events_avg == 0) { 00600 for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++) 00601 bin_data[i] = 0; 00602 goto FAIL; 00603 } 00604 00605 for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++) { 00606 idx = i + phist_data->number_of_ambient_bins; 00607 if (phist_data->bin_data[idx] > 0) { 00608 bin_data[i] = 00609 ((uint64_t)phist_data->bin_data[idx] << 10); 00610 tmpi32 = pxtalk_data->signal_total_events_avg / 2; 00611 bin_data[i] = bin_data[i] + (uint64_t)tmpi32; 00612 bin_data[i] = do_division_u(bin_data[i], 00613 (uint64_t)pxtalk_data->signal_total_events_avg); 00614 } else { 00615 bin_data[i] = 0; 00616 } 00617 } 00618 00619 FAIL: 00620 00621 00622 for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++) 00623 pxtalk_histo->bin_data[i] = (uint32_t)bin_data[i]; 00624 00625 00626 00627 for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++) 00628 pdebug->VL53L1_p_063[i] = pxtalk_histo->bin_data[i]; 00629 00630 00631 LOG_FUNCTION_END(status); 00632 00633 return status; 00634 00635 } 00636 00637 00638 VL53L1_Error VL53L1_f_054( 00639 VL53L1_customer_nvm_managed_t *pcustomer, 00640 VL53L1_dynamic_config_t *pdyn_cfg, 00641 VL53L1_xtalk_histogram_data_t *pxtalk_shape, 00642 VL53L1_histogram_bin_data_t *pip_hist_data, 00643 VL53L1_histogram_bin_data_t *pop_hist_data, 00644 VL53L1_histogram_bin_data_t *pxtalk_count_data) 00645 { 00646 00647 00648 00649 VL53L1_Error status = VL53L1_ERROR_NONE; 00650 00651 00652 00653 uint32_t xtalk_rate_kcps = 0; 00654 00655 LOG_FUNCTION_START(""); 00656 00657 00658 00659 memcpy(pop_hist_data, pip_hist_data, 00660 sizeof(VL53L1_histogram_bin_data_t)); 00661 00662 00663 00664 status = 00665 VL53L1_f_040( 00666 pcustomer->algo__crosstalk_compensation_plane_offset_kcps, 00667 pcustomer->algo__crosstalk_compensation_x_plane_gradient_kcps, 00668 pcustomer->algo__crosstalk_compensation_y_plane_gradient_kcps, 00669 0, 00670 0, 00671 pip_hist_data->result__dss_actual_effective_spads, 00672 00673 pdyn_cfg->roi_config__user_roi_centre_spad, 00674 pdyn_cfg->roi_config__user_roi_requested_global_xy_size, 00675 &(xtalk_rate_kcps)); 00676 00677 00678 00679 if (status == VL53L1_ERROR_NONE) 00680 status = 00681 VL53L1_f_041( 00682 pip_hist_data, 00683 &(pxtalk_shape->xtalk_shape), 00684 xtalk_rate_kcps, 00685 pxtalk_count_data); 00686 00687 00688 00689 if (status == VL53L1_ERROR_NONE) 00690 status = 00691 VL53L1_f_055( 00692 pop_hist_data, 00693 pxtalk_count_data, 00694 pip_hist_data->number_of_ambient_bins); 00695 00696 LOG_FUNCTION_END(status); 00697 00698 return status; 00699 } 00700 00701 00702 VL53L1_Error VL53L1_f_040( 00703 uint32_t mean_offset, 00704 int16_t xgradient, 00705 int16_t ygradient, 00706 int8_t centre_offset_x, 00707 int8_t centre_offset_y, 00708 uint16_t roi_effective_spads, 00709 uint8_t roi_centre_spad, 00710 uint8_t roi_xy_size, 00711 uint32_t *xtalk_rate_kcps 00712 ) 00713 { 00714 00715 00716 00717 VL53L1_Error status = VL53L1_ERROR_NONE; 00718 00719 uint8_t row = 0; 00720 uint8_t col = 0; 00721 00722 00723 00724 int16_t bound_l_x = 0; 00725 int16_t bound_r_x = 0; 00726 int16_t bound_u_y = 0; 00727 int16_t bound_d_y = 0; 00728 00729 int64_t xtalk_rate_ll = 0; 00730 int64_t xtalk_rate_ur = 0; 00731 00732 int64_t xtalk_avg = 0; 00733 00734 LOG_FUNCTION_START(""); 00735 00736 SUPPRESS_UNUSED_WARNING(roi_effective_spads); 00737 00738 00739 00740 00741 00742 00743 if (status == VL53L1_ERROR_NONE) { 00744 VL53L1_decode_row_col( 00745 roi_centre_spad, 00746 &row, 00747 &col); 00748 } 00749 00750 trace_print( 00751 VL53L1_TRACE_LEVEL_DEBUG, 00752 " %-48s : %10d\n", 00753 "Row", row); 00754 00755 trace_print( 00756 VL53L1_TRACE_LEVEL_DEBUG, 00757 " %-48s : %10d\n", 00758 "Col", col); 00759 00760 00761 00762 if (status == VL53L1_ERROR_NONE) { 00763 if ((((int16_t)roi_xy_size / 16) & 0x01) == 1) 00764 bound_l_x = (int16_t) col - 00765 (((int16_t)roi_xy_size / 32) + 1); 00766 else 00767 bound_l_x = (int16_t) col - 00768 ((int16_t)roi_xy_size / 32); 00769 00770 bound_r_x = (int16_t) col + ((int16_t)roi_xy_size / 32); 00771 00772 if ((((int16_t)roi_xy_size) & 0x01) == 1) 00773 bound_d_y = (int16_t) row - 00774 ((((int16_t)roi_xy_size & 0x0f) / 2) + 1); 00775 else 00776 bound_d_y = (int16_t) row - 00777 (((int16_t)roi_xy_size & 0x0f) / 2); 00778 00779 bound_u_y = (int16_t) row + 00780 (((int16_t)roi_xy_size & 0xf) / 2); 00781 } 00782 00783 trace_print( 00784 VL53L1_TRACE_LEVEL_DEBUG, 00785 " %-48s : %10d\n", 00786 "Bound_l_x", bound_l_x); 00787 trace_print( 00788 VL53L1_TRACE_LEVEL_DEBUG, 00789 " %-48s : %10d\n", 00790 "Bound_r_x", bound_r_x); 00791 trace_print( 00792 VL53L1_TRACE_LEVEL_DEBUG, 00793 " %-48s : %10d\n", 00794 "Bound_u_y", bound_u_y); 00795 trace_print( 00796 VL53L1_TRACE_LEVEL_DEBUG, 00797 " %-48s : %10d\n", 00798 "Bound_d_y", bound_d_y); 00799 00800 00801 00802 00803 if (status == VL53L1_ERROR_NONE) { 00804 bound_l_x = (2 * bound_l_x) - 15 + 00805 (2 * (int16_t)centre_offset_x); 00806 bound_r_x = (2 * bound_r_x) - 15 + 00807 (2 * (int16_t)centre_offset_x); 00808 bound_u_y = (2 * bound_u_y) - 15 + 00809 (2 * (int16_t)centre_offset_y); 00810 bound_d_y = (2 * bound_d_y) - 15 + 00811 (2 * (int16_t)centre_offset_y); 00812 } 00813 00814 trace_print( 00815 VL53L1_TRACE_LEVEL_DEBUG, 00816 " %-48s : %10d\n", 00817 "Bound_l_x", bound_l_x); 00818 00819 trace_print( 00820 VL53L1_TRACE_LEVEL_DEBUG, 00821 " %-48s : %10d\n", 00822 "Bound_r_x", bound_r_x); 00823 00824 trace_print( 00825 VL53L1_TRACE_LEVEL_DEBUG, 00826 " %-48s : %10d\n", 00827 "Bound_u_y", bound_u_y); 00828 00829 trace_print( 00830 VL53L1_TRACE_LEVEL_DEBUG, 00831 " %-48s : %10d\n", 00832 "Bound_d_y", bound_d_y); 00833 00834 00835 00836 00837 if (status == VL53L1_ERROR_NONE) { 00838 xtalk_rate_ll = ((int64_t)bound_l_x * 00839 ((int64_t)xgradient)) + ((int64_t)bound_d_y * 00840 ((int64_t)ygradient)); 00841 xtalk_rate_ll = do_division_s((xtalk_rate_ll + 1), 2); 00842 xtalk_rate_ll += ((int64_t)mean_offset * 4); 00843 00844 xtalk_rate_ur = ((int64_t)bound_r_x * 00845 ((int64_t)xgradient)) + ((int64_t)bound_u_y * 00846 ((int64_t)ygradient)); 00847 xtalk_rate_ur = do_division_s((xtalk_rate_ur + 1), 2); 00848 xtalk_rate_ur += ((int64_t)mean_offset * 4); 00849 } 00850 00851 trace_print( 00852 VL53L1_TRACE_LEVEL_DEBUG, 00853 " %-48s : %10d\n", 00854 "xtalk_rate_ll", xtalk_rate_ll); 00855 00856 trace_print( 00857 VL53L1_TRACE_LEVEL_DEBUG, 00858 " %-48s : %10d\n", 00859 "xtalk_rate_ur", xtalk_rate_ur); 00860 00861 00862 00863 if (status == VL53L1_ERROR_NONE) 00864 xtalk_avg = do_division_s( 00865 ((xtalk_rate_ll + xtalk_rate_ur) + 1), 2); 00866 00867 trace_print( 00868 VL53L1_TRACE_LEVEL_DEBUG, 00869 " %-48s : %10d\n", 00870 "xtalk_avg", xtalk_avg); 00871 00872 00873 00874 if (status == VL53L1_ERROR_NONE) 00875 if (xtalk_avg < 0) 00876 xtalk_avg = 0; 00877 00878 00879 00880 00881 00882 *xtalk_rate_kcps = (uint32_t) xtalk_avg; 00883 00884 trace_print( 00885 VL53L1_TRACE_LEVEL_DEBUG, 00886 " %-48s : %10d\n", 00887 "xtalk_rate_kcps", xtalk_avg); 00888 00889 LOG_FUNCTION_END(status); 00890 00891 return status; 00892 } 00893 00894 00895 00896 VL53L1_Error VL53L1_f_041( 00897 VL53L1_histogram_bin_data_t *phist_data, 00898 VL53L1_xtalk_histogram_shape_t *pxtalk_data, 00899 uint32_t xtalk_rate_kcps, 00900 VL53L1_histogram_bin_data_t *pxtalkcount_data 00901 ) 00902 { 00903 00904 00905 VL53L1_Error status = VL53L1_ERROR_NONE; 00906 00907 uint64_t xtalk_events_per_spad = 0; 00908 uint64_t xtalk_total_events = 0; 00909 uint64_t xtalk_temp_bin = 0; 00910 00911 uint8_t i = 0; 00912 00913 LOG_FUNCTION_START(""); 00914 00915 trace_print( 00916 VL53L1_TRACE_LEVEL_DEBUG, 00917 " %-48s : %10d\n", 00918 "pk_duration_internal", phist_data->peak_duration_us); 00919 00920 00921 00922 xtalk_events_per_spad = do_division_u((((uint64_t)xtalk_rate_kcps * 00923 (uint64_t)phist_data->peak_duration_us) + 500), 1000); 00924 00925 00926 trace_print( 00927 VL53L1_TRACE_LEVEL_DEBUG, 00928 " %-48s : %10d\n", 00929 "xtalk_events_per_spad", xtalk_events_per_spad); 00930 00931 00932 00933 00934 xtalk_total_events = xtalk_events_per_spad * 00935 (uint64_t)phist_data->result__dss_actual_effective_spads; 00936 00937 xtalk_total_events = do_division_u((xtalk_total_events), 256); 00938 00939 xtalk_total_events = do_division_u((xtalk_total_events + 1024), 2048); 00940 00941 if (xtalk_total_events > 0xFFFFFFFF) 00942 xtalk_total_events = 0xFFFFFFFF; 00943 00944 trace_print( 00945 VL53L1_TRACE_LEVEL_DEBUG, 00946 " %-48s : %10d\n", 00947 "xtalk_total_events", xtalk_total_events); 00948 00949 00950 00951 00952 00953 00954 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) { 00955 xtalk_temp_bin = (uint64_t)pxtalk_data->bin_data[i] * 00956 (uint64_t)xtalk_total_events; 00957 xtalk_temp_bin = do_division_u((xtalk_temp_bin + 512), 1024); 00958 pxtalkcount_data->bin_data[i] = (uint32_t)xtalk_temp_bin; 00959 00960 trace_print( 00961 VL53L1_TRACE_LEVEL_DEBUG, 00962 " %-48s : %10d\n", 00963 "bin_data", pxtalkcount_data->bin_data[i]); 00964 } 00965 00966 LOG_FUNCTION_END(status); 00967 00968 return status; 00969 } 00970 00971 00972 VL53L1_Error VL53L1_f_055( 00973 VL53L1_histogram_bin_data_t *phist_data, 00974 VL53L1_histogram_bin_data_t *pxtalk_data, 00975 uint8_t xtalk_bin_offset) 00976 { 00977 00978 00979 VL53L1_Error status = VL53L1_ERROR_NONE; 00980 00981 uint8_t i = 0; 00982 00983 int32_t temp_bin; 00984 00985 LOG_FUNCTION_START(""); 00986 00987 00988 00989 00990 if (status == VL53L1_ERROR_NONE) 00991 for (i = xtalk_bin_offset; 00992 i < pxtalk_data->VL53L1_p_024; i++) { 00993 00994 temp_bin = (int32_t)phist_data->bin_data[i] - 00995 (int32_t)pxtalk_data->bin_data[i - xtalk_bin_offset]; 00996 00997 if (temp_bin < 0) 00998 temp_bin = 0; 00999 01000 phist_data->bin_data[i] = (uint32_t)temp_bin; 01001 } 01002 01003 01004 LOG_FUNCTION_END(status); 01005 01006 return status; 01007 } 01008 01009 01010 VL53L1_Error VL53L1_f_052( 01011 VL53L1_histogram_bin_data_t *pxtalk_data, 01012 uint32_t amb_threshold, 01013 uint8_t VL53L1_p_022, 01014 uint8_t VL53L1_p_026) 01015 { 01016 01017 01018 VL53L1_Error status = VL53L1_ERROR_NONE; 01019 01020 uint8_t i = 0; 01021 uint8_t first_bin_int = 0; 01022 uint8_t first_bin_inc = 0; 01023 uint8_t last_bin_int = 0; 01024 uint8_t realign_bin = 0; 01025 uint8_t realign_index = 0; 01026 int32_t realign_bin_data[VL53L1_HISTOGRAM_BUFFER_SIZE]; 01027 01028 LOG_FUNCTION_START(""); 01029 01030 01031 01032 for (i = 0 ; i < VL53L1_HISTOGRAM_BUFFER_SIZE ; i++) 01033 realign_bin_data[i] = 0; 01034 01035 first_bin_int = VL53L1_p_022; 01036 last_bin_int = VL53L1_p_026; 01037 01038 01039 01040 01041 01042 VL53L1_hist_remove_ambient_bins(pxtalk_data); 01043 01044 01045 01046 first_bin_int = (first_bin_int) % 01047 pxtalk_data->VL53L1_p_024; 01048 01049 last_bin_int = (last_bin_int) % 01050 pxtalk_data->VL53L1_p_024; 01051 01052 first_bin_inc = (first_bin_int + 1) % pxtalk_data->VL53L1_p_024; 01053 01054 01055 01056 if (first_bin_inc > last_bin_int) { 01057 01058 01059 realign_bin = pxtalk_data->VL53L1_p_024 - first_bin_inc; 01060 01061 01062 first_bin_int = (first_bin_int + realign_bin) % 01063 pxtalk_data->VL53L1_p_024; 01064 last_bin_int = (last_bin_int + realign_bin) % 01065 pxtalk_data->VL53L1_p_024; 01066 01067 01068 pxtalk_data->zero_distance_phase = 01069 pxtalk_data->zero_distance_phase + 01070 ((uint16_t)realign_bin * 2048); 01071 } 01072 01073 if (realign_bin > 0) { 01074 01075 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) 01076 realign_bin_data[i] = pxtalk_data->bin_data[i]; 01077 01078 01079 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) { 01080 realign_index = (pxtalk_data->VL53L1_p_024 - 01081 realign_bin + i) 01082 % pxtalk_data->VL53L1_p_024; 01083 01084 pxtalk_data->bin_data[i] = 01085 realign_bin_data[realign_index]; 01086 } 01087 } 01088 01089 01090 01091 01092 trace_print( 01093 VL53L1_TRACE_LEVEL_DEBUG, 01094 " %-48s : %10d\n", 01095 "first bin int", first_bin_int); 01096 01097 trace_print( 01098 VL53L1_TRACE_LEVEL_DEBUG, 01099 " %-48s : %10d\n", 01100 "last bin int", last_bin_int); 01101 01102 trace_print( 01103 VL53L1_TRACE_LEVEL_DEBUG, 01104 " %-48s : %10d\n", 01105 "amb thresh", amb_threshold); 01106 01107 01108 01109 01110 01111 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) { 01112 01113 if (first_bin_int <= last_bin_int) { 01114 if ((i >= first_bin_int) && (i <= last_bin_int)) { 01115 if (pxtalk_data->bin_data[i] < 01116 (int32_t)amb_threshold) 01117 pxtalk_data->bin_data[i] = 0; 01118 } else { 01119 pxtalk_data->bin_data[i] = 0; 01120 } 01121 } else { 01122 if ((i >= first_bin_int) || (i <= last_bin_int)) { 01123 if (pxtalk_data->bin_data[i] < 01124 (int32_t)amb_threshold) { 01125 pxtalk_data->bin_data[i] = 0; 01126 } 01127 } else { 01128 pxtalk_data->bin_data[i] = 0; 01129 } 01130 } 01131 } 01132 01133 01134 01135 01136 01137 LOG_FUNCTION_END(status); 01138 01139 return status; 01140 } 01141 01142 01143 VL53L1_Error VL53L1_f_051( 01144 uint8_t sigma_mult, 01145 int32_t VL53L1_p_004, 01146 uint32_t *ambient_noise) 01147 { 01148 01149 01150 01151 VL53L1_Error status = VL53L1_ERROR_NONE; 01152 01153 uint32_t ambient_events_per_bin_int = 0; 01154 01155 LOG_FUNCTION_START(""); 01156 01157 if (VL53L1_p_004 <= 0) 01158 ambient_events_per_bin_int = 1; 01159 else 01160 ambient_events_per_bin_int = (uint32_t)VL53L1_p_004; 01161 01162 *ambient_noise = VL53L1_isqrt(ambient_events_per_bin_int); 01163 01164 *ambient_noise = *ambient_noise * (uint32_t)sigma_mult; 01165 01166 LOG_FUNCTION_END(status); 01167 01168 return status; 01169 } 01170 01171 01172 01173 01174 01175 01176 01177 01178 01179 01180 01181 01182 01183 01184 01185 01186 01187 01188 01189 01190
Generated on Tue Jul 12 2022 20:07:15 by
1.7.2