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