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