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