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