vl53l1_dmax.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 #include "vl53l1_platform_user_defines.h"
00022 #include "vl53l1_core_support.h"
00023 #include "vl53l1_error_codes.h"
00024 
00025 #include "vl53l1_dmax.h"
00026 
00027 
00028 #define LOG_FUNCTION_START(fmt, ...) \
00029     _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__)
00030 #define LOG_FUNCTION_END(status, ...) \
00031     _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__)
00032 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00033     _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_PROTECTED, \
00034     status, fmt, ##__VA_ARGS__)
00035 
00036 #define trace_print(level, ...) \
00037     _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_PROTECTED, \
00038     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00039 
00040 
00041 VL53L1_Error VL53L1_f_001(
00042     uint16_t                              target_reflectance,
00043     VL53L1_dmax_calibration_data_t       *pcal,
00044     VL53L1_hist_gen3_dmax_config_t       *pcfg,
00045     VL53L1_histogram_bin_data_t          *pbins,
00046     VL53L1_hist_gen3_dmax_private_data_t *pdata,
00047     int16_t                              *pambient_dmax_mm)
00048 {
00049 
00050 
00051 
00052     VL53L1_Error status  = VL53L1_ERROR_NONE;
00053 
00054     uint32_t    pll_period_us       = 0;
00055     uint32_t    periods_elapsed     = 0;
00056 
00057     uint32_t    tmp32               = 0;
00058     uint64_t    tmp64               = 0;
00059 
00060     uint32_t    amb_thres_delta     = 0;
00061 
00062     LOG_FUNCTION_START("");
00063 
00064 
00065 
00066     pdata->VL53L1_p_006     = 0x0000;
00067     pdata->VL53L1_p_033 = 0x0000;
00068     pdata->VL53L1_p_001          = 0x0000;
00069     pdata->VL53L1_p_012    = 0x0000;
00070     pdata->VL53L1_p_004     = 0x0000;
00071     pdata->VL53L1_p_034 = 0x0000;
00072     pdata->VL53L1_p_035             = 0;
00073     pdata->VL53L1_p_007            = 0;
00074 
00075     *pambient_dmax_mm  = 0;
00076 
00077 
00078     if ((pbins->VL53L1_p_019        != 0) &&
00079         (pbins->total_periods_elapsed      != 0)) {
00080 
00081 
00082 
00083         pll_period_us   =
00084             VL53L1_calc_pll_period_us(pbins->VL53L1_p_019);
00085 
00086 
00087 
00088         periods_elapsed = pbins->total_periods_elapsed + 1;
00089 
00090 
00091 
00092         pdata->VL53L1_p_036  =
00093             VL53L1_duration_maths(
00094                 pll_period_us,
00095                 1<<4,
00096                 VL53L1_RANGING_WINDOW_VCSEL_PERIODS,
00097                 periods_elapsed);
00098 
00099 
00100         pdata->VL53L1_p_001 =
00101             VL53L1_rate_maths(
00102                 pbins->VL53L1_p_004,
00103                 pdata->VL53L1_p_036);
00104 
00105 
00106 
00107         pdata->VL53L1_p_033   =
00108             VL53L1_events_per_spad_maths(
00109                 pbins->VL53L1_p_004,
00110                 pbins->result__dss_actual_effective_spads,
00111                 pdata->VL53L1_p_036);
00112 
00113 
00114 
00115         pdata->VL53L1_p_037 = pcfg->max_effective_spads;
00116         pdata->VL53L1_p_006  = pcfg->max_effective_spads;
00117 
00118         if (pdata->VL53L1_p_033 > 0) {
00119             tmp64   =
00120             (uint64_t)pcfg->dss_config__target_total_rate_mcps;
00121             tmp64  *= 1000;
00122             tmp64 <<= (11+1);
00123             tmp32 = pdata->VL53L1_p_033/2;
00124             tmp64  += (uint64_t)tmp32;
00125             tmp64 = do_division_u(tmp64,
00126                 (uint64_t)pdata->VL53L1_p_033);
00127 
00128             if (tmp64 < (uint64_t)pcfg->max_effective_spads)
00129                 pdata->VL53L1_p_006 = (uint16_t)tmp64;
00130         }
00131     }
00132 
00133 
00134 
00135     if ((pcal->ref__actual_effective_spads != 0) &&
00136         (pbins->VL53L1_p_019        != 0) &&
00137         (pcal->ref_reflectance_pc          != 0) &&
00138         (pbins->total_periods_elapsed      != 0)) {
00139 
00140 
00141 
00142         tmp64  = (uint64_t)pcal->ref__peak_signal_count_rate_mcps;
00143         tmp64 *= (1000 * 256);
00144         tmp32  = pcal->ref__actual_effective_spads/2;
00145         tmp64 += (uint64_t)tmp32;
00146         tmp64  = do_division_u(tmp64,
00147             (uint64_t)pcal->ref__actual_effective_spads);
00148 
00149         pdata->VL53L1_p_012   = (uint32_t)tmp64;
00150         pdata->VL53L1_p_012 <<= 4;
00151 
00152 
00153 
00154         tmp64   = (uint64_t)pdata->VL53L1_p_036;
00155         tmp64  *= (uint64_t)pdata->VL53L1_p_033;
00156         tmp64  *= (uint64_t)pdata->VL53L1_p_006;
00157         tmp64  += (1<<(11+7));
00158         tmp64 >>= (11+8);
00159         tmp64  +=  500;
00160         tmp64   = do_division_u(tmp64, 1000);
00161 
00162 
00163         if (tmp64 > 0x00FFFFFF)
00164             tmp64 = 0x00FFFFFF;
00165 
00166         pdata->VL53L1_p_004 = (uint32_t)tmp64;
00167 
00168 
00169 
00170         tmp64   = (uint64_t)pdata->VL53L1_p_036;
00171         tmp64  *= (uint64_t)pdata->VL53L1_p_012;
00172         tmp64  *= (uint64_t)pdata->VL53L1_p_006;
00173         tmp64  += (1<<(11+7));
00174         tmp64 >>= (11+8);
00175 
00176 
00177 
00178         tmp64  *= ((uint64_t)target_reflectance *
00179                    (uint64_t)pcal->coverglass_transmission);
00180 
00181         tmp64  += ((uint64_t)pcal->ref_reflectance_pc * 128);
00182         tmp64 = do_division_u(tmp64,
00183             ((uint64_t)pcal->ref_reflectance_pc * 256));
00184 
00185         tmp64  +=  500;
00186         tmp64 = do_division_u(tmp64, 1000);
00187 
00188 
00189         if (tmp64 > 0x00FFFFFF)
00190             tmp64 = 0x00FFFFFF;
00191 
00192         pdata->VL53L1_p_034 = (uint32_t)tmp64;
00193 
00194 
00195 
00196         tmp32  = VL53L1_isqrt(pdata->VL53L1_p_004 << 8);
00197         tmp32 *= (uint32_t)pcfg->ambient_thresh_sigma;
00198 
00199 
00200 
00201         if (pdata->VL53L1_p_004 <
00202             (uint32_t)pcfg->min_ambient_thresh_events) {
00203 
00204             amb_thres_delta =
00205                 pcfg->min_ambient_thresh_events -
00206                 (uint32_t)pdata->VL53L1_p_004;
00207 
00208 
00209             amb_thres_delta <<= 8;
00210 
00211             if (tmp32 < amb_thres_delta)
00212                 tmp32 = amb_thres_delta;
00213         }
00214 
00215 
00216 
00217         pdata->VL53L1_p_007 =
00218             (int16_t)VL53L1_f_002(
00219                 tmp32,
00220                 pdata->VL53L1_p_034,
00221                 (uint32_t)pcal->ref__distance_mm,
00222                 (uint32_t)pcfg->signal_thresh_sigma);
00223 
00224 
00225 
00226         tmp32  = (uint32_t)pdata->VL53L1_p_034;
00227         tmp32 *= (uint32_t)pbins->vcsel_width;
00228         tmp32 += (1 << 3);
00229         tmp32 /= (1 << 4);
00230 
00231         pdata->VL53L1_p_035 =
00232             (int16_t)VL53L1_f_002(
00233                 256 * (uint32_t)pcfg->signal_total_events_limit,
00234                 tmp32,
00235                 (uint32_t)pcal->ref__distance_mm,
00236                 (uint32_t)pcfg->signal_thresh_sigma);
00237 
00238 
00239 
00240 
00241         if (pdata->VL53L1_p_035 < pdata->VL53L1_p_007)
00242             *pambient_dmax_mm = pdata->VL53L1_p_035;
00243         else
00244             *pambient_dmax_mm = pdata->VL53L1_p_007;
00245 
00246     }
00247 
00248     LOG_FUNCTION_END(status);
00249 
00250     return status;
00251 
00252 }
00253 
00254 
00255 uint32_t VL53L1_f_002(
00256     uint32_t     events_threshold,
00257     uint32_t     ref_signal_events,
00258     uint32_t     ref_distance_mm,
00259     uint32_t     signal_thresh_sigma)
00260 {
00261 
00262 
00263 
00264     uint32_t    tmp32               = 0;
00265     uint32_t    range_mm            = 0;
00266 
00267     tmp32 = 4 * events_threshold;
00268 
00269 
00270 
00271     tmp32 += ((uint32_t)signal_thresh_sigma *
00272               (uint32_t)signal_thresh_sigma);
00273 
00274 
00275 
00276     tmp32  = VL53L1_isqrt(tmp32);
00277     tmp32 += (uint32_t)signal_thresh_sigma;
00278 
00279 
00280 
00281     range_mm =
00282         (uint32_t)VL53L1_isqrt(ref_signal_events << 4);
00283     range_mm *= ref_distance_mm;
00284 
00285     if (tmp32 > 0) {
00286         range_mm += (tmp32);
00287         range_mm /= (2*tmp32);
00288     }
00289 
00290     return range_mm;
00291 
00292 }
00293 
00294