vl53l1_xtalk.c

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