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