vl53l1_sigma_estimate.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 
00028 #include "vl53l1_core_support.h"
00029 #include "vl53l1_error_codes.h"
00030 
00031 #include "vl53l1_sigma_estimate.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 uint16_t  VL53L1_f_042(
00048         uint8_t  sigma_estimator__effective_pulse_width_ns,
00049         uint8_t  sigma_estimator__effective_ambient_width_ns,
00050         uint8_t  sigma_estimator__sigma_ref_mm,
00051         VL53L1_range_data_t *pdata)
00052 {
00053 
00054 
00055 
00056     uint16_t    sigma_est  = VL53L1_D_002;
00057 
00058     uint32_t    tmp0 = 0;
00059     uint32_t    tmp1 = 0;
00060     uint32_t    tmp2 = 0;
00061 
00062     uint32_t    sigma_est__rtn_array  = 0;
00063     uint32_t    sigma_est__ref_array  = 0;
00064 
00065     LOG_FUNCTION_START("");
00066 
00067     if (pdata->peak_signal_count_rate_mcps  > 0 &&
00068         pdata->VL53L1_p_013 > 0) {
00069 
00070 
00071 
00072         tmp0 =  100 *
00073             (uint32_t)sigma_estimator__effective_pulse_width_ns;
00074 
00075 
00076 
00077         tmp1 = ((uint32_t)sigma_estimator__effective_pulse_width_ns *
00078             100 *
00079             (uint32_t)sigma_estimator__effective_ambient_width_ns);
00080 
00081         tmp1 =  (tmp1 +
00082             (uint32_t)pdata->peak_signal_count_rate_mcps/2) /
00083             (uint32_t)pdata->peak_signal_count_rate_mcps;
00084 
00085 
00086 
00087         sigma_est__rtn_array =
00088             VL53L1_f_043(tmp0, tmp1);
00089 
00090 
00091 
00092         sigma_est__rtn_array =
00093             ((VL53L1_SPEED_OF_LIGHT_IN_AIR + 1000) / 2000) *
00094             sigma_est__rtn_array;
00095 
00096 
00097 
00098         tmp2 =
00099             VL53L1_isqrt(12 * (uint32_t)pdata->VL53L1_p_013);
00100 
00101         if (tmp2 > 0) {
00102 
00103             sigma_est__rtn_array =
00104                     (sigma_est__rtn_array + tmp2/2) / tmp2;
00105 
00106 
00107 
00108             sigma_est__ref_array =
00109                 100 * (uint32_t)sigma_estimator__sigma_ref_mm;
00110 
00111             sigma_est =
00112                 (uint16_t)VL53L1_f_043(
00113                         (uint32_t)sigma_est__ref_array,
00114                         sigma_est__rtn_array);
00115 
00116         } else {
00117             sigma_est = VL53L1_D_002;
00118         }
00119 
00120     }
00121 
00122     pdata->VL53L1_p_005  = sigma_est;
00123 
00124     LOG_FUNCTION_END(0);
00125 
00126     return sigma_est;
00127 
00128 }
00129 
00130 
00131 uint16_t VL53L1_f_044(
00132     uint8_t  sigma_estimator__effective_pulse_width_ns,
00133     uint8_t  sigma_estimator__effective_ambient_width_ns,
00134     uint8_t  sigma_estimator__sigma_ref_mm,
00135     VL53L1_range_data_t *pdata)
00136 {
00137 
00138 
00139     uint16_t    sigma_est  = VL53L1_D_002;
00140 
00141     uint32_t    eqn7 = 0;
00142     uint32_t    sigma_est__ref_sq  = 0;
00143     uint32_t    sigma_est__rtn_sq  = 0;
00144 
00145     uint64_t    tmp0 = 0;
00146     uint64_t    tmp1 = 0;
00147 
00148     LOG_FUNCTION_START("");
00149 
00150     if (pdata->peak_signal_count_rate_mcps > 0 &&
00151         pdata->VL53L1_p_013         > 0) {
00152 
00153 
00154 
00155         eqn7 =  4573 * 4573;
00156         eqn7 =  eqn7 / (3 * (uint32_t)pdata->VL53L1_p_013);
00157 
00158 
00159 
00160         tmp0 = ((uint64_t)sigma_estimator__effective_pulse_width_ns)
00161                 << 8;
00162 
00163 
00164 
00165         tmp1 = ((uint64_t)pdata->ambient_count_rate_mcps *
00166             (uint64_t)sigma_estimator__effective_ambient_width_ns)
00167                     << 8;
00168 
00169         tmp1 = do_division_u(tmp1,
00170             (uint64_t)pdata->peak_signal_count_rate_mcps);
00171 
00172 
00173 
00174         tmp1 = 16 * (uint64_t)eqn7 * (tmp0 * tmp0 + tmp1 * tmp1);
00175         tmp1 = do_division_u(tmp1, (15625 * 15625));
00176         sigma_est__rtn_sq = (uint32_t)tmp1;
00177 
00178 
00179 
00180         sigma_est__ref_sq = ((uint32_t)sigma_estimator__sigma_ref_mm)
00181                 << 2;
00182 
00183         sigma_est__ref_sq = sigma_est__ref_sq * sigma_est__ref_sq;
00184 
00185 
00186 
00187         sigma_est = (uint16_t)VL53L1_isqrt(sigma_est__ref_sq +
00188                 sigma_est__rtn_sq);
00189 
00190     }
00191 
00192     pdata->VL53L1_p_005  = sigma_est;
00193 
00194     LOG_FUNCTION_END(0);
00195 
00196     return sigma_est;
00197 
00198 }
00199 
00200 
00201 
00202 VL53L1_Error VL53L1_f_045(
00203     uint8_t  sigma_estimator__sigma_ref_mm,
00204     uint32_t VL53L1_p_003,
00205     uint32_t VL53L1_p_018,
00206     uint32_t VL53L1_p_001,
00207     uint32_t a_zp,
00208     uint32_t c_zp,
00209     uint32_t bx,
00210     uint32_t ax_zp,
00211     uint32_t cx_zp,
00212     uint32_t VL53L1_p_004,
00213     uint16_t fast_osc_frequency,
00214     uint16_t *psigma_est)
00215 {
00216 
00217 
00218     VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
00219     uint32_t sigma_int  = VL53L1_D_002;
00220 
00221     uint32_t pll_period_mm  = 0;
00222 
00223     uint64_t tmp0        = 0;
00224     uint64_t tmp1        = 0;
00225     uint64_t b_minus_amb = 0;
00226     uint64_t VL53L1_p_041   = 0;
00227 
00228     *psigma_est  = VL53L1_D_002;
00229 
00230 
00231 
00232     if (fast_osc_frequency != 0) {
00233 
00234 
00235 
00236         pll_period_mm = VL53L1_calc_pll_period_mm(fast_osc_frequency);
00237 
00238 
00239         pll_period_mm = (pll_period_mm + 0x02) >> 2;
00240 
00241 
00242 
00243         if (VL53L1_p_004 > VL53L1_p_018)
00244             b_minus_amb =  (uint64_t)VL53L1_p_004 -
00245             (uint64_t)VL53L1_p_018;
00246         else
00247             b_minus_amb =  (uint64_t)VL53L1_p_018 -
00248             (uint64_t)VL53L1_p_004;
00249 
00250 
00251 
00252         if (VL53L1_p_003 > VL53L1_p_001)
00253             VL53L1_p_041 =  (uint64_t)VL53L1_p_003 -
00254             (uint64_t)VL53L1_p_001;
00255         else
00256             VL53L1_p_041 =  (uint64_t)VL53L1_p_001 -
00257             (uint64_t)VL53L1_p_003;
00258 
00259 
00260 
00261 
00262 
00263         if (b_minus_amb != 0) {
00264 
00265 
00266 
00267             tmp0 = (uint64_t)pll_period_mm *
00268                     (uint64_t)pll_period_mm;
00269             tmp0 = tmp0 * ((uint64_t)c_zp +
00270                     (uint64_t)cx_zp + (uint64_t)a_zp +
00271                     (uint64_t)ax_zp);
00272             tmp0 = do_division_u((tmp0 + (b_minus_amb >> 1)),
00273                 b_minus_amb);
00274 
00275 
00276 
00277             tmp1 = (uint64_t)pll_period_mm *
00278                     (uint64_t)pll_period_mm * VL53L1_p_041;
00279             tmp1 = do_division_u((tmp1 + (b_minus_amb >> 1)),
00280                 b_minus_amb);
00281 
00282             tmp1 =  tmp1 * VL53L1_p_041;
00283             tmp1 = do_division_u((tmp1 + (b_minus_amb >> 1)),
00284                 b_minus_amb);
00285 
00286             tmp1 =  tmp1 * ((uint64_t)VL53L1_p_018 + (uint64_t)bx +
00287                     (uint64_t)VL53L1_p_004);
00288             tmp1 = do_division_u((tmp1 + (b_minus_amb >> 1)),
00289                 b_minus_amb);
00290 
00291 
00292 
00293             tmp0 = tmp0 + tmp1;
00294             tmp0 = do_division_u((tmp0 + (b_minus_amb >> 1)),
00295                 b_minus_amb);
00296             tmp0 = (tmp0 + 0x01) >> 2;
00297 
00298 
00299 
00300             tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 2;
00301             tmp1 = tmp1 * tmp1;
00302             tmp0 = tmp0 + tmp1;
00303 
00304 
00305 
00306             if (tmp0 > 0xFFFFFFFF)
00307                 tmp0 =  0xFFFFFFFF;
00308 
00309             sigma_int = VL53L1_isqrt((uint32_t)tmp0);
00310 
00311 
00312 
00313             if (sigma_int > VL53L1_D_002)
00314                 *psigma_est =
00315                 (uint16_t)VL53L1_D_002;
00316             else
00317                 *psigma_est = (uint16_t)sigma_int;
00318 
00319             status = VL53L1_ERROR_NONE;
00320         }
00321 
00322     }
00323 
00324     return status;
00325 }
00326 
00327 
00328 
00329 VL53L1_Error VL53L1_f_014(
00330     uint8_t  sigma_estimator__sigma_ref_mm,
00331     uint32_t VL53L1_p_003,
00332     uint32_t VL53L1_p_018,
00333     uint32_t VL53L1_p_001,
00334     uint32_t a_zp,
00335     uint32_t c_zp,
00336     uint32_t bx,
00337     uint32_t ax_zp,
00338     uint32_t cx_zp,
00339     uint32_t VL53L1_p_004,
00340     uint16_t fast_osc_frequency,
00341     uint16_t *psigma_est)
00342 {
00343 
00344 
00345     VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
00346     uint32_t sigma_int  = VL53L1_D_002;
00347 
00348     uint32_t pll_period_mm  = 0;
00349 
00350     uint64_t tmp0        = 0;
00351     uint64_t tmp1        = 0;
00352     uint64_t b_minus_amb = 0;
00353     uint64_t VL53L1_p_041   = 0;
00354 
00355     *psigma_est  = VL53L1_D_002;
00356 
00357 
00358 
00359     if (fast_osc_frequency != 0) {
00360 
00361 
00362 
00363         pll_period_mm = VL53L1_calc_pll_period_mm(fast_osc_frequency);
00364 
00365 
00366 
00367         if (VL53L1_p_004 > VL53L1_p_018)
00368             b_minus_amb =  (uint64_t)VL53L1_p_004 -
00369             (uint64_t)VL53L1_p_018;
00370         else
00371             b_minus_amb =  (uint64_t)VL53L1_p_018 -
00372             (uint64_t)VL53L1_p_004;
00373 
00374 
00375 
00376         if (VL53L1_p_003 > VL53L1_p_001)
00377             VL53L1_p_041 =  (uint64_t)VL53L1_p_003 -
00378             (uint64_t)VL53L1_p_001;
00379         else
00380             VL53L1_p_041 =  (uint64_t)VL53L1_p_001 -
00381             (uint64_t)VL53L1_p_003;
00382 
00383 
00384 
00385         if (b_minus_amb != 0) {
00386 
00387 
00388 
00389 
00390             tmp0 = (uint64_t)VL53L1_p_018 + (uint64_t)bx +
00391                     (uint64_t)VL53L1_p_004;
00392             if (tmp0 > VL53L1_D_003)
00393                 tmp0 = VL53L1_D_003;
00394 
00395 
00396 
00397             tmp1 = (uint64_t)VL53L1_p_041 * (uint64_t)VL53L1_p_041;
00398             tmp1 = tmp1 << 8;
00399 
00400 
00401             if (tmp1 > VL53L1_D_004)
00402                 tmp1 = VL53L1_D_004;
00403 
00404 
00405             tmp1 = do_division_u(tmp1, b_minus_amb);
00406             tmp1 = do_division_u(tmp1, b_minus_amb);
00407 
00408 
00409             if (tmp1 > (uint64_t)VL53L1_D_005)
00410                 tmp1 = (uint64_t)VL53L1_D_005;
00411 
00412 
00413             tmp0 = tmp1 * tmp0;
00414 
00415 
00416             tmp1 = (uint64_t)c_zp + (uint64_t)cx_zp +
00417                 (uint64_t)a_zp + (uint64_t)ax_zp;
00418 
00419 
00420             if (tmp1 > (uint64_t)VL53L1_D_003)
00421                 tmp1 = (uint64_t)VL53L1_D_003;
00422 
00423             tmp1 = tmp1 << 8;
00424 
00425 
00426             tmp0 = tmp1 + tmp0;
00427             if (tmp0 > (uint64_t)VL53L1_D_006)
00428                 tmp0 = (uint64_t)VL53L1_D_006;
00429 
00430 
00431 
00432 
00433 
00434 
00435             if (tmp0 > (uint64_t)VL53L1_D_007) {
00436                 tmp0 = do_division_u(tmp0, b_minus_amb);
00437                 tmp0 = tmp0 * pll_period_mm;
00438             } else {
00439                 tmp0 = tmp0 * pll_period_mm;
00440                 tmp0 = do_division_u(tmp0, b_minus_amb);
00441             }
00442 
00443 
00444             if (tmp0 > (uint64_t)VL53L1_D_006)
00445                 tmp0 = (uint64_t)VL53L1_D_006;
00446 
00447 
00448 
00449             if (tmp0 > (uint64_t)VL53L1_D_007) {
00450                 tmp0 = do_division_u(tmp0, b_minus_amb);
00451                 tmp0 = do_division_u(tmp0, 4);
00452                 tmp0 = tmp0 * pll_period_mm;
00453             } else {
00454                 tmp0 = tmp0 * pll_period_mm;
00455                 tmp0 = do_division_u(tmp0, b_minus_amb);
00456                 tmp0 = do_division_u(tmp0, 4);
00457             }
00458 
00459 
00460             if (tmp0 > (uint64_t)VL53L1_D_006)
00461                 tmp0 = (uint64_t)VL53L1_D_006;
00462 
00463 
00464             tmp0 = tmp0 >> 2;
00465 
00466 
00467             if (tmp0 > (uint64_t)VL53L1_D_007)
00468                 tmp0 = (uint64_t)VL53L1_D_007;
00469 
00470 
00471             tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 7;
00472             tmp1 = tmp1 * tmp1;
00473             tmp0 = tmp0 + tmp1;
00474 
00475 
00476             if (tmp0 > (uint64_t)VL53L1_D_007)
00477                 tmp0 = (uint64_t)VL53L1_D_007;
00478 
00479 
00480             sigma_int = VL53L1_isqrt((uint32_t)tmp0);
00481 
00482             *psigma_est = (uint16_t)sigma_int;
00483 
00484             status = VL53L1_ERROR_NONE;
00485         }
00486 
00487     }
00488 
00489     return status;
00490 }
00491 
00492 uint32_t VL53L1_f_046(
00493     uint64_t VL53L1_p_003,
00494     uint32_t size
00495     )
00496 {
00497 
00498 
00499     uint64_t next;
00500     uint64_t upper;
00501     uint64_t lower;
00502     uint32_t stepsize;
00503     uint32_t count;
00504 
00505 
00506     next = VL53L1_p_003;
00507     upper = 0;
00508     lower = 0;
00509     stepsize = size/2;
00510     count = 0;
00511 
00512     while (1) {
00513         upper = next >> stepsize;
00514         lower = next & ((1 << stepsize) - 1);
00515 
00516         if (upper != 0) {
00517             count += stepsize;
00518             next = upper;
00519         } else {
00520             next = lower;
00521         }
00522 
00523         stepsize = stepsize / 2;
00524         if (stepsize == 0)
00525             break;
00526     }
00527 
00528     return count;
00529 }
00530 
00531 
00532 
00533 uint32_t VL53L1_f_043(
00534     uint32_t VL53L1_p_003,
00535     uint32_t VL53L1_p_018)
00536 {
00537 
00538 
00539     uint32_t  res = 0;
00540 
00541     if (VL53L1_p_003 > 65535 || VL53L1_p_018 > 65535)
00542         res = 65535;
00543     else
00544         res = VL53L1_isqrt(VL53L1_p_003*VL53L1_p_003 +
00545             VL53L1_p_018*VL53L1_p_018);
00546 
00547     return res;
00548 }
00549 
00550