vl53l1_hist_funcs.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 
00026 
00027 
00028 #include "vl53l1_types.h"
00029 #include "vl53l1_platform_log.h"
00030 
00031 #include "vl53l1_core_support.h"
00032 #include "vl53l1_error_codes.h"
00033 #include "vl53l1_ll_def.h"
00034 
00035 #include "vl53l1_hist_funcs.h"
00036 #include "vl53l1_hist_core.h"
00037 #include "vl53l1_hist_private_structs.h"
00038 #include "vl53l1_dmax_private_structs.h"
00039 #include "vl53l1_xtalk.h"
00040 #include "vl53l1_hist_algos_gen3.h"
00041 #include "vl53l1_hist_algos_gen4.h"
00042 #include "vl53l1_dmax.h"
00043 
00044 
00045 
00046 #define LOG_FUNCTION_START(fmt, ...) \
00047     _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
00048 #define LOG_FUNCTION_END(status, ...) \
00049     _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
00050 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00051     _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00052     status, fmt, ##__VA_ARGS__)
00053 
00054 #define trace_print(level, ...) \
00055     _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00056     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00057 
00058 
00059 VL53L1_Error VL53L1_hist_process_data(
00060     VL53L1_dmax_calibration_data_t     *pdmax_cal,
00061     VL53L1_hist_gen3_dmax_config_t     *pdmax_cfg,
00062     VL53L1_hist_post_process_config_t  *ppost_cfg,
00063     VL53L1_histogram_bin_data_t        *pbins_input,
00064     VL53L1_xtalk_histogram_data_t      *pxtalk_shape,
00065     uint8_t                            *pArea1,
00066     uint8_t                            *pArea2,
00067     VL53L1_range_results_t             *presults,
00068     uint8_t                            *HistMergeNumber)
00069 {
00070 
00071 
00072 
00073     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00074 
00075     VL53L1_hist_gen3_algo_private_data_t  *palgo_gen3 =
00076             (VL53L1_hist_gen3_algo_private_data_t *) pArea1;
00077     VL53L1_hist_gen4_algo_filtered_data_t *pfiltered4 =
00078             (VL53L1_hist_gen4_algo_filtered_data_t *) pArea2;
00079 
00080     VL53L1_hist_gen3_dmax_private_data_t   dmax_algo_gen3;
00081     VL53L1_hist_gen3_dmax_private_data_t  *pdmax_algo_gen3 =
00082                         &dmax_algo_gen3;
00083 
00084     VL53L1_histogram_bin_data_t             bins_averaged;
00085     VL53L1_histogram_bin_data_t           *pbins_averaged = &bins_averaged;
00086 
00087     VL53L1_range_data_t                   *pdata;
00088 
00089     uint32_t xtalk_rate_kcps               = 0;
00090     uint32_t max_xtalk_rate_per_spad_kcps  = 0;
00091     uint8_t  xtalk_enable                  = 0;
00092     uint8_t  r                             = 0;
00093     uint8_t  t                             = 0;
00094     uint32_t XtalkDetectMaxSigma           = 0;
00095 
00096 
00097     int16_t  delta_mm                      = 0;
00098 
00099 
00100     LOG_FUNCTION_START("");
00101 
00102 
00103 
00104     VL53L1_f_039(
00105             pbins_input,
00106             pbins_averaged);
00107 
00108 
00109 
00110     VL53L1_init_histogram_bin_data_struct(
00111             0,
00112             pxtalk_shape->xtalk_shape.VL53L1_p_024,
00113             &(pxtalk_shape->xtalk_hist_removed));
00114 
00115 
00116 
00117     VL53L1_copy_xtalk_bin_data_to_histogram_data_struct(
00118             &(pxtalk_shape->xtalk_shape),
00119             &(pxtalk_shape->xtalk_hist_removed));
00120 
00121 
00122 
00123     if ((status == VL53L1_ERROR_NONE) &&
00124         (ppost_cfg->algo__crosstalk_compensation_enable > 0))
00125         status =
00126         VL53L1_f_040(
00127         ppost_cfg->algo__crosstalk_compensation_plane_offset_kcps,
00128         ppost_cfg->algo__crosstalk_compensation_x_plane_gradient_kcps,
00129         ppost_cfg->algo__crosstalk_compensation_y_plane_gradient_kcps,
00130         0,
00131         0,
00132         pbins_input->result__dss_actual_effective_spads,
00133         pbins_input->roi_config__user_roi_centre_spad,
00134         pbins_input->roi_config__user_roi_requested_global_xy_size,
00135         &(xtalk_rate_kcps));
00136 
00137 
00138 
00139     if ((status == VL53L1_ERROR_NONE) &&
00140         (ppost_cfg->algo__crosstalk_compensation_enable > 0))
00141         status =
00142             VL53L1_f_041(
00143               pbins_averaged,
00144               &(pxtalk_shape->xtalk_shape),
00145               xtalk_rate_kcps,
00146               &(pxtalk_shape->xtalk_hist_removed));
00147 
00148 
00149 
00150 
00151 
00152     presults->xmonitor.total_periods_elapsed =
00153         pbins_averaged->total_periods_elapsed;
00154     presults->xmonitor.VL53L1_p_006 =
00155         pbins_averaged->result__dss_actual_effective_spads;
00156 
00157     presults->xmonitor.peak_signal_count_rate_mcps = 0;
00158     presults->xmonitor.VL53L1_p_012     = 0;
00159 
00160     presults->xmonitor.range_id     = 0;
00161     presults->xmonitor.range_status = VL53L1_DEVICEERROR_NOUPDATE;
00162 
00163 
00164 
00165     xtalk_enable = 0;
00166     if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
00167         xtalk_enable = 1;
00168 
00169 
00170 
00171     for (r = 0 ; r <= xtalk_enable ; r++) {
00172 
00173 
00174         ppost_cfg->algo__crosstalk_compensation_enable = r;
00175 
00176 
00177 
00178         status =
00179         VL53L1_f_033(
00180             pdmax_cal,
00181             pdmax_cfg,
00182             ppost_cfg,
00183             pbins_averaged,
00184             &(pxtalk_shape->xtalk_hist_removed),
00185             palgo_gen3,
00186             pfiltered4,
00187             pdmax_algo_gen3,
00188             presults,
00189             *HistMergeNumber);
00190 
00191 
00192         if (!(status == VL53L1_ERROR_NONE && r == 0))
00193             continue;
00194 
00195 
00196 
00197         if (presults->active_results == 0) {
00198             pdata = &(presults->VL53L1_p_002[0]);
00199             pdata->ambient_count_rate_mcps =
00200                 pdmax_algo_gen3->VL53L1_p_001;
00201             pdata->VL53L1_p_006 =
00202                 pdmax_algo_gen3->VL53L1_p_006;
00203         }
00204 
00205 
00206 
00207         max_xtalk_rate_per_spad_kcps = (uint32_t)(
00208         ppost_cfg->algo__crosstalk_detect_max_valid_rate_kcps);
00209         max_xtalk_rate_per_spad_kcps *= (uint32_t)(*HistMergeNumber);
00210         max_xtalk_rate_per_spad_kcps <<= 4;
00211 
00212         for (t = 0 ; t < presults->active_results ; t++) {
00213 
00214             pdata = &(presults->VL53L1_p_002[t]);
00215 
00216 
00217 
00218             if (pdata->max_range_mm > pdata->min_range_mm)
00219                 delta_mm =
00220                     pdata->max_range_mm -
00221                     pdata->min_range_mm;
00222             else
00223                 delta_mm =
00224                     pdata->min_range_mm -
00225                     pdata->max_range_mm;
00226 
00227             XtalkDetectMaxSigma =
00228                 ppost_cfg->algo__crosstalk_detect_max_sigma_mm;
00229             XtalkDetectMaxSigma *= (uint32_t)(*HistMergeNumber);
00230             XtalkDetectMaxSigma <<= 5;
00231             if (pdata->median_range_mm  >
00232             ppost_cfg->algo__crosstalk_detect_min_valid_range_mm &&
00233             pdata->median_range_mm  <
00234             ppost_cfg->algo__crosstalk_detect_max_valid_range_mm &&
00235             pdata->VL53L1_p_012 <
00236             max_xtalk_rate_per_spad_kcps &&
00237             pdata->VL53L1_p_005 < XtalkDetectMaxSigma &&
00238             delta_mm <
00239             ppost_cfg->algo__crosstalk_detect_min_max_tolerance) {
00240 
00241 
00242 
00243                 memcpy(
00244                     &(presults->xmonitor),
00245                     pdata,
00246                     sizeof(VL53L1_range_data_t));
00247 
00248             }
00249         }
00250 
00251     }
00252 
00253 
00254 
00255     ppost_cfg->algo__crosstalk_compensation_enable = xtalk_enable;
00256 
00257     LOG_FUNCTION_END(status);
00258 
00259     return status;
00260 }
00261 
00262 
00263 VL53L1_Error VL53L1_hist_ambient_dmax(
00264     uint16_t                            target_reflectance,
00265     VL53L1_dmax_calibration_data_t     *pdmax_cal,
00266     VL53L1_hist_gen3_dmax_config_t     *pdmax_cfg,
00267     VL53L1_histogram_bin_data_t        *pbins,
00268     int16_t                            *pambient_dmax_mm)
00269 {
00270 
00271 
00272 
00273     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00274 
00275     VL53L1_hist_gen3_dmax_private_data_t   dmax_algo;
00276     VL53L1_hist_gen3_dmax_private_data_t  *pdmax_algo = &dmax_algo;
00277 
00278     LOG_FUNCTION_START("");
00279 
00280     status =
00281         VL53L1_f_001(
00282             target_reflectance,
00283             pdmax_cal,
00284             pdmax_cfg,
00285             pbins,
00286             pdmax_algo,
00287             pambient_dmax_mm);
00288 
00289     LOG_FUNCTION_END(status);
00290 
00291     return status;
00292 }
00293