vl53l1_hist_algos_gen4.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_hist_core.h"
00032 #include "vl53l1_hist_algos_gen3.h"
00033 #include "vl53l1_hist_algos_gen4.h"
00034 #include "vl53l1_sigma_estimate.h"
00035 #include "vl53l1_dmax.h"
00036 #ifdef __KERNEL__
00037 #include <linux/math64.h>
00038 #include <linux/kernel.h>
00039 #endif
00040 
00041 
00042 
00043 #define LOG_FUNCTION_START(fmt, ...) \
00044     _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
00045 #define LOG_FUNCTION_END(status, ...) \
00046     _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
00047 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00048     _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00049     status, fmt, ##__VA_ARGS__)
00050 
00051 #define trace_print(level, ...) \
00052     _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00053     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00054 
00055 
00056 void VL53L1_f_032(
00057     VL53L1_hist_gen4_algo_filtered_data_t   *palgo)
00058 {
00059 
00060 
00061     uint8_t  lb                 = 0;
00062 
00063     palgo->VL53L1_p_023              = VL53L1_HISTOGRAM_BUFFER_SIZE;
00064     palgo->VL53L1_p_022                = 0;
00065     palgo->VL53L1_p_024           = 0;
00066 
00067     for (lb = palgo->VL53L1_p_022; lb < palgo->VL53L1_p_023; lb++) {
00068         palgo->VL53L1_p_003[lb]      = 0;
00069         palgo->VL53L1_p_018[lb]      = 0;
00070         palgo->VL53L1_p_001[lb]      = 0;
00071         palgo->VL53L1_p_039[lb] = 0;
00072         palgo->VL53L1_p_040[lb] = 0;
00073         palgo->VL53L1_p_043[lb]  = 0;
00074     }
00075 }
00076 
00077 
00078 VL53L1_Error VL53L1_f_033(
00079     VL53L1_dmax_calibration_data_t         *pdmax_cal,
00080     VL53L1_hist_gen3_dmax_config_t         *pdmax_cfg,
00081     VL53L1_hist_post_process_config_t      *ppost_cfg,
00082     VL53L1_histogram_bin_data_t            *pbins_input,
00083     VL53L1_histogram_bin_data_t            *pxtalk,
00084     VL53L1_hist_gen3_algo_private_data_t   *palgo3,
00085     VL53L1_hist_gen4_algo_filtered_data_t  *pfiltered,
00086     VL53L1_hist_gen3_dmax_private_data_t   *pdmax_algo,
00087     VL53L1_range_results_t                 *presults,
00088     uint8_t                                histo_merge_nb)
00089 {
00090 
00091 
00092     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00093 
00094     VL53L1_hist_pulse_data_t     *ppulse_data;
00095     VL53L1_range_data_t          *prange_data;
00096 
00097     uint8_t                       p = 0;
00098     VL53L1_histogram_bin_data_t *pB = &(palgo3->VL53L1_p_010);
00099     uint16_t enabled_spads_ninety_percent;
00100     int32_t amb_threshold_sigma;
00101 
00102     LOG_FUNCTION_START("");
00103 
00104 
00105 
00106 
00107 
00108     VL53L1_f_016(palgo3);
00109 
00110 
00111 
00112     memcpy(
00113         &(palgo3->VL53L1_p_010),
00114         pbins_input,
00115         sizeof(VL53L1_histogram_bin_data_t));
00116 
00117 
00118 
00119     presults->cfg_device_state = pbins_input->cfg_device_state;
00120     presults->rd_device_state  = pbins_input->rd_device_state;
00121     presults->zone_id          = pbins_input->zone_id;
00122     presults->stream_count     = pbins_input->result__stream_count;
00123     presults->wrap_dmax_mm     = 0;
00124     presults->max_results      = VL53L1_MAX_RANGE_RESULTS;
00125     presults->active_results   = 0;
00126 
00127     for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++)
00128         presults->VL53L1_p_007[p] = 0;
00129 
00130 
00131 
00132     VL53L1_hist_calc_zero_distance_phase(&(palgo3->VL53L1_p_010));
00133 
00134 
00135 
00136     if (ppost_cfg->hist_amb_est_method ==
00137         VL53L1_HIST_AMB_EST_METHOD__THRESHOLDED_BINS)
00138         VL53L1_hist_estimate_ambient_from_thresholded_bins(
00139             (int32_t)ppost_cfg->ambient_thresh_sigma0,
00140             &(palgo3->VL53L1_p_010));
00141     else
00142         VL53L1_hist_estimate_ambient_from_ambient_bins(
00143                 &(palgo3->VL53L1_p_010));
00144 
00145 
00146     VL53L1_hist_remove_ambient_bins(&(palgo3->VL53L1_p_010));
00147 
00148 
00149     if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
00150         VL53L1_f_004(
00151                 pxtalk,
00152                 &(palgo3->VL53L1_p_010),
00153                 &(palgo3->VL53L1_p_038));
00154 
00155 
00156     amb_threshold_sigma = ppost_cfg->ambient_thresh_sigma1;
00157     enabled_spads_ninety_percent =
00158         presults->fmt_total_enabled_spads * 230;
00159     if ((pbins_input->result__dss_actual_effective_spads <
00160          enabled_spads_ninety_percent) &&
00161         (presults->VL53L1_p_002[0].ambient_count_rate_mcps < (5 * 128))){
00162         amb_threshold_sigma *= histo_merge_nb;
00163     }
00164 
00165     pdmax_cfg->ambient_thresh_sigma = amb_threshold_sigma;
00166 
00167     for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++) {
00168         if (status == VL53L1_ERROR_NONE) {
00169             status =
00170             VL53L1_f_001(
00171                 pdmax_cfg->target_reflectance_for_dmax_calc[p],
00172                 pdmax_cal,
00173                 pdmax_cfg,
00174                 &(palgo3->VL53L1_p_010),
00175                 pdmax_algo,
00176                 &(presults->VL53L1_p_007[p]));
00177         }
00178     }
00179 
00180 
00181 
00182 
00183 
00184     if (status == VL53L1_ERROR_NONE)
00185         status =
00186             VL53L1_f_018(
00187             ppost_cfg->ambient_thresh_events_scaler,
00188             (int32_t)amb_threshold_sigma,
00189             (int32_t)ppost_cfg->min_ambient_thresh_events,
00190             ppost_cfg->algo__crosstalk_compensation_enable,
00191             &(palgo3->VL53L1_p_010),
00192             &(palgo3->VL53L1_p_038),
00193             palgo3);
00194 
00195 
00196 
00197 
00198 
00199     if (status == VL53L1_ERROR_NONE)
00200         status =
00201             VL53L1_f_019(palgo3);
00202 
00203 
00204 
00205     if (status == VL53L1_ERROR_NONE)
00206         status =
00207             VL53L1_f_020(palgo3);
00208 
00209 
00210 
00211     if (status == VL53L1_ERROR_NONE)
00212         status =
00213             VL53L1_f_021(palgo3);
00214 
00215 
00216 
00217     for (p = 0; p < palgo3->VL53L1_p_051; p++) {
00218 
00219         ppulse_data = &(palgo3->VL53L1_p_002[p]);
00220 
00221 
00222 
00223         if (status == VL53L1_ERROR_NONE)
00224             status =
00225                 VL53L1_f_022(
00226                     p,
00227                     &(palgo3->VL53L1_p_010),
00228                     palgo3);
00229 
00230 
00231 
00232         if (status == VL53L1_ERROR_NONE)
00233             status =
00234                 VL53L1_f_023(
00235                     p,
00236                     &(palgo3->VL53L1_p_010),
00237                     palgo3,
00238                     pB->VL53L1_p_004,
00239                     &(palgo3->VL53L1_p_052));
00240 
00241 
00242 
00243         if (status == VL53L1_ERROR_NONE) {
00244             status =
00245                 VL53L1_f_023(
00246                     p,
00247                     &(palgo3->VL53L1_p_010),
00248                     palgo3,
00249                     0,
00250                     &(palgo3->VL53L1_p_053));
00251         }
00252 
00253 
00254 
00255         if (status == VL53L1_ERROR_NONE) {
00256             status =
00257                 VL53L1_f_023(
00258                     p,
00259                     &(palgo3->VL53L1_p_038),
00260                     palgo3,
00261                     0,
00262                     &(palgo3->VL53L1_p_054));
00263         }
00264 
00265 
00266 
00267         if (status == VL53L1_ERROR_NONE)
00268             status =
00269                 VL53L1_f_034(
00270                     p,
00271                     &(palgo3->VL53L1_p_052),
00272                     palgo3,
00273                     pfiltered);
00274 
00275 
00276 
00277         if (status == VL53L1_ERROR_NONE)
00278             status =
00279                 VL53L1_f_035(
00280                     p,
00281                     ppost_cfg->noise_threshold,
00282                     pfiltered,
00283                     palgo3);
00284 
00285         if (status == VL53L1_ERROR_NONE)
00286             status =
00287             VL53L1_f_026(
00288             ppulse_data->VL53L1_p_025,
00289             ppost_cfg->sigma_estimator__sigma_ref_mm,
00290             palgo3->VL53L1_p_031,
00291             ppulse_data->VL53L1_p_055,
00292             ppost_cfg->algo__crosstalk_compensation_enable,
00293             &(palgo3->VL53L1_p_052),
00294             &(palgo3->VL53L1_p_053),
00295             &(palgo3->VL53L1_p_054),
00296             &(ppulse_data->VL53L1_p_005));
00297 
00298 
00299 
00300         if (status == VL53L1_ERROR_NONE)
00301             status =
00302                 VL53L1_f_027(
00303                     p,
00304                     1,
00305                     &(palgo3->VL53L1_p_010),
00306                     palgo3);
00307 
00308     }
00309 
00310 
00311 
00312     if (status == VL53L1_ERROR_NONE)
00313         status =
00314             VL53L1_f_028(
00315                 ppost_cfg->hist_target_order,
00316                 palgo3);
00317 
00318 
00319 
00320     for (p = 0; p < palgo3->VL53L1_p_051; p++) {
00321 
00322         ppulse_data = &(palgo3->VL53L1_p_002[p]);
00323 
00324 
00325         if (!(presults->active_results < presults->max_results))
00326             continue;
00327 
00328 
00329 
00330 
00331         if (ppulse_data->VL53L1_p_013 >
00332             ppost_cfg->signal_total_events_limit &&
00333             ppulse_data->VL53L1_p_025 < 0xFF) {
00334 
00335             prange_data =
00336             &(presults->VL53L1_p_002[presults->active_results]);
00337 
00338             if (status == VL53L1_ERROR_NONE)
00339                 VL53L1_f_029(
00340                         presults->active_results,
00341                         ppost_cfg->valid_phase_low,
00342                         ppost_cfg->valid_phase_high,
00343                         ppost_cfg->sigma_thresh,
00344                         &(palgo3->VL53L1_p_010),
00345                         ppulse_data,
00346                         prange_data);
00347 
00348             if (status == VL53L1_ERROR_NONE)
00349                 status =
00350                 VL53L1_f_011(
00351                 pB->vcsel_width,
00352                 pB->VL53L1_p_019,
00353                 pB->total_periods_elapsed,
00354                 pB->result__dss_actual_effective_spads,
00355                 prange_data,
00356                 histo_merge_nb);
00357 
00358             if (status == VL53L1_ERROR_NONE)
00359                 VL53L1_f_012(
00360                     ppost_cfg->gain_factor,
00361                     ppost_cfg->range_offset_mm,
00362                     prange_data);
00363 
00364             presults->active_results++;
00365         }
00366 
00367     }
00368 
00369 
00370 
00371     LOG_FUNCTION_END(status);
00372 
00373     return status;
00374 }
00375 
00376 
00377 
00378 VL53L1_Error VL53L1_f_034(
00379     uint8_t                                pulse_no,
00380     VL53L1_histogram_bin_data_t           *ppulse,
00381     VL53L1_hist_gen3_algo_private_data_t  *palgo3,
00382     VL53L1_hist_gen4_algo_filtered_data_t *pfiltered)
00383 {
00384 
00385 
00386 
00387 
00388     VL53L1_Error  status       = VL53L1_ERROR_NONE;
00389 
00390     VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
00391 
00392     uint8_t  lb     = 0;
00393     uint8_t  i     = 0;
00394     int32_t  suma  = 0;
00395     int32_t  sumb  = 0;
00396     int32_t  sumc  = 0;
00397 
00398     LOG_FUNCTION_START("");
00399 
00400     pfiltered->VL53L1_p_023    = palgo3->VL53L1_p_023;
00401     pfiltered->VL53L1_p_022      = palgo3->VL53L1_p_022;
00402     pfiltered->VL53L1_p_024 = palgo3->VL53L1_p_024;
00403 
00404 
00405 
00406     for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) {
00407 
00408         i =  lb  % palgo3->VL53L1_p_031;
00409 
00410 
00411         VL53L1_f_013(
00412                 i,
00413                 pdata->VL53L1_p_055,
00414                 ppulse,
00415                 &suma,
00416                 &sumb,
00417                 &sumc);
00418 
00419 
00420         pfiltered->VL53L1_p_003[i] = suma;
00421         pfiltered->VL53L1_p_018[i] = sumb;
00422         pfiltered->VL53L1_p_001[i] = sumc;
00423 
00424 
00425 
00426         pfiltered->VL53L1_p_039[i] =
00427             (suma + sumb) -
00428             (sumc + palgo3->VL53L1_p_004);
00429 
00430 
00431 
00432         pfiltered->VL53L1_p_040[i] =
00433             (sumb + sumc) -
00434             (suma + palgo3->VL53L1_p_004);
00435     }
00436 
00437     return status;
00438 }
00439 
00440 
00441 VL53L1_Error VL53L1_f_035(
00442     uint8_t                                pulse_no,
00443     uint16_t                               noise_threshold,
00444     VL53L1_hist_gen4_algo_filtered_data_t *pfiltered,
00445     VL53L1_hist_gen3_algo_private_data_t  *palgo3)
00446 {
00447 
00448 
00449 
00450     VL53L1_Error  status       = VL53L1_ERROR_NONE;
00451     VL53L1_Error  func_status  = VL53L1_ERROR_NONE;
00452 
00453     VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
00454 
00455     uint8_t  lb            = 0;
00456     uint8_t  i            = 0;
00457     uint8_t  j            = 0;
00458 
00459     SUPPRESS_UNUSED_WARNING(noise_threshold);
00460 
00461     for (lb = pdata->VL53L1_p_015; lb < pdata->VL53L1_p_016; lb++) {
00462 
00463         i =  lb    % palgo3->VL53L1_p_031;
00464         j = (lb+1) % palgo3->VL53L1_p_031;
00465 
00466         if (i < palgo3->VL53L1_p_024 &&
00467             j < palgo3->VL53L1_p_024) {
00468 
00469             if (pfiltered->VL53L1_p_039[i] == 0 &&
00470                 pfiltered->VL53L1_p_040[i] == 0)
00471 
00472                 pfiltered->VL53L1_p_043[i] = 0;
00473 
00474             else if (pfiltered->VL53L1_p_039[i] >= 0 &&
00475                      pfiltered->VL53L1_p_040[i] >= 0)
00476                 pfiltered->VL53L1_p_043[i] = 1;
00477 
00478             else if (pfiltered->VL53L1_p_039[i] <  0 &&
00479                      pfiltered->VL53L1_p_040[i] >= 0 &&
00480                      pfiltered->VL53L1_p_039[j] >= 0 &&
00481                      pfiltered->VL53L1_p_040[j] <  0)
00482                 pfiltered->VL53L1_p_043[i] = 1;
00483 
00484             else
00485                 pfiltered->VL53L1_p_043[i] = 0;
00486 
00487 
00488             if (pfiltered->VL53L1_p_043[i] > 0) {
00489 
00490                 pdata->VL53L1_p_025 = lb;
00491 
00492                 func_status =
00493                     VL53L1_f_036(
00494                     lb,
00495                     pfiltered->VL53L1_p_003[i],
00496                     pfiltered->VL53L1_p_018[i],
00497                     pfiltered->VL53L1_p_001[i],
00498                     0,
00499                     0,
00500                     0,
00501                     palgo3->VL53L1_p_004,
00502                     palgo3->VL53L1_p_031,
00503                     &(pdata->VL53L1_p_014));
00504 
00505                 if (func_status ==
00506                     VL53L1_ERROR_DIVISION_BY_ZERO)
00507                     pfiltered->VL53L1_p_043[i] = 0;
00508 
00509             }
00510         }
00511     }
00512 
00513     return status;
00514 }
00515 
00516 
00517 VL53L1_Error VL53L1_f_036(
00518     uint8_t   bin,
00519     int32_t   VL53L1_p_003,
00520     int32_t   VL53L1_p_018,
00521     int32_t   VL53L1_p_001,
00522     int32_t   ax,
00523     int32_t   bx,
00524     int32_t   cx,
00525     int32_t   VL53L1_p_004,
00526     uint8_t   VL53L1_p_031,
00527     uint32_t *pmean_phase)
00528 {
00529 
00530 
00531     VL53L1_Error  status = VL53L1_ERROR_DIVISION_BY_ZERO;
00532 
00533     int64_t  mean_phase  = VL53L1_MAX_ALLOWED_PHASE;
00534     int32_t  mean_phase32;
00535     int64_t  VL53L1_p_041   = 0;
00536     int64_t  half_b_minus_amb = 0;
00537 
00538 
00539     VL53L1_p_041    = 4096 * ((int64_t)VL53L1_p_001 -
00540         (int64_t)cx - (int64_t)VL53L1_p_003 -  (int64_t)ax);
00541     half_b_minus_amb  = 4096 * ((int64_t)VL53L1_p_018 -
00542         (int64_t)bx - (int64_t)VL53L1_p_004);
00543 
00544     if (half_b_minus_amb != 0) {
00545         mean_phase = (4096 * VL53L1_p_041) + half_b_minus_amb;
00546         mean_phase = do_division_s(mean_phase, (half_b_minus_amb * 2));
00547         mean_phase += 2048;
00548         mean_phase += (4096 * (int64_t)bin);
00549 
00550         mean_phase  = do_division_s((mean_phase + 1), 2);
00551 
00552         if (mean_phase  < 0)
00553             mean_phase = 0;
00554         if (mean_phase > VL53L1_MAX_ALLOWED_PHASE)
00555             mean_phase = VL53L1_MAX_ALLOWED_PHASE;
00556 
00557         mean_phase32 = (int32_t)mean_phase;
00558         mean_phase32 = mean_phase32 %
00559             ((int32_t)VL53L1_p_031 * 2048);
00560         mean_phase = mean_phase32;
00561 
00562         status = VL53L1_ERROR_NONE;
00563     }
00564 
00565     *pmean_phase = (uint32_t)mean_phase;
00566 
00567     return status;
00568 }
00569 
00570