vl53l1_hist_algos_gen3.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_hist_core.h"
00032 #include "vl53l1_hist_algos_gen3.h"
00033 #include "vl53l1_sigma_estimate.h"
00034 #include "vl53l1_dmax.h"
00035 
00036 
00037 
00038 #define LOG_FUNCTION_START(fmt, ...) \
00039     _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
00040 #define LOG_FUNCTION_END(status, ...) \
00041     _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
00042 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00043     _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM,\
00044     status, fmt, ##__VA_ARGS__)
00045 
00046 #define trace_print(level, ...) \
00047     _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00048     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00049 
00050 
00051 void VL53L1_f_016(
00052     VL53L1_hist_gen3_algo_private_data_t   *palgo)
00053 {
00054 
00055 
00056     uint8_t  lb                 = 0;
00057 
00058     palgo->VL53L1_p_023              = VL53L1_HISTOGRAM_BUFFER_SIZE;
00059     palgo->VL53L1_p_022                = 0;
00060     palgo->VL53L1_p_024           = 0;
00061     palgo->VL53L1_p_045         = 0;
00062     palgo->VL53L1_p_004   = 0;
00063     palgo->VL53L1_p_032 = 0;
00064 
00065     for (lb = palgo->VL53L1_p_022; lb < palgo->VL53L1_p_023; lb++) {
00066         palgo->VL53L1_p_043[lb]      = 0;
00067         palgo->VL53L1_p_046[lb] = 0;
00068         palgo->VL53L1_p_047[lb]     = 0;
00069         palgo->VL53L1_p_048[lb]      = 0;
00070         palgo->VL53L1_p_008[lb]     = 0;
00071     }
00072 
00073     palgo->VL53L1_p_049 = 0;
00074     palgo->VL53L1_p_050               = VL53L1_D_001;
00075     palgo->VL53L1_p_051             = 0;
00076 
00077 
00078 
00079     VL53L1_init_histogram_bin_data_struct(
00080         0,
00081         VL53L1_HISTOGRAM_BUFFER_SIZE,
00082         &(palgo->VL53L1_p_010));
00083     VL53L1_init_histogram_bin_data_struct(
00084         0,
00085         VL53L1_HISTOGRAM_BUFFER_SIZE,
00086         &(palgo->VL53L1_p_038));
00087     VL53L1_init_histogram_bin_data_struct(
00088         0,
00089         VL53L1_HISTOGRAM_BUFFER_SIZE,
00090         &(palgo->VL53L1_p_052));
00091     VL53L1_init_histogram_bin_data_struct(
00092         0,
00093         VL53L1_HISTOGRAM_BUFFER_SIZE,
00094         &(palgo->VL53L1_p_053));
00095     VL53L1_init_histogram_bin_data_struct(
00096         0,
00097         VL53L1_HISTOGRAM_BUFFER_SIZE,
00098         &(palgo->VL53L1_p_054));
00099 }
00100 
00101 
00102 
00103 VL53L1_Error VL53L1_f_018(
00104     uint16_t                          ambient_threshold_events_scaler,
00105     int32_t                           ambient_threshold_sigma,
00106     int32_t                           min_ambient_threshold_events,
00107     uint8_t                           algo__crosstalk_compensation_enable,
00108     VL53L1_histogram_bin_data_t           *pbins,
00109     VL53L1_histogram_bin_data_t           *pxtalk,
00110     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00111 {
00112 
00113 
00114 
00115     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00116     uint8_t  lb            = 0;
00117     uint8_t  VL53L1_p_001            = 0;
00118     int64_t  tmp          = 0;
00119     int32_t  amb_events   = 0;
00120     int32_t  VL53L1_p_008       = 0;
00121     int32_t  samples      = 0;
00122 
00123     LOG_FUNCTION_START("");
00124 
00125 
00126     palgo->VL53L1_p_023            = pbins->VL53L1_p_023;
00127     palgo->VL53L1_p_022              = pbins->VL53L1_p_022;
00128     palgo->VL53L1_p_024         = pbins->VL53L1_p_024;
00129     palgo->VL53L1_p_004 = pbins->VL53L1_p_004;
00130 
00131 
00132 
00133     palgo->VL53L1_p_031 =
00134             VL53L1_decode_vcsel_period(pbins->VL53L1_p_009);
00135 
00136 
00137 
00138     tmp  = (int64_t)pbins->VL53L1_p_004;
00139     tmp *= (int64_t)ambient_threshold_events_scaler;
00140     tmp += 2048;
00141     tmp = do_division_s(tmp, 4096);
00142     amb_events = (int32_t)tmp;
00143 
00144 
00145 
00146     for (lb = 0; lb < pbins->VL53L1_p_024; lb++) {
00147 
00148         VL53L1_p_001 = lb >> 2;
00149         samples = (int32_t)pbins->bin_rep[VL53L1_p_001];
00150 
00151         if (samples > 0) {
00152 
00153             if (lb < pxtalk->VL53L1_p_024 &&
00154                 algo__crosstalk_compensation_enable > 0)
00155                 VL53L1_p_008 = samples * (amb_events +
00156                     pxtalk->bin_data[lb]);
00157             else
00158                 VL53L1_p_008 = samples *  amb_events;
00159 
00160             VL53L1_p_008  = VL53L1_isqrt(VL53L1_p_008);
00161 
00162             VL53L1_p_008 += (samples/2);
00163             VL53L1_p_008 /= samples;
00164             VL53L1_p_008 *= ambient_threshold_sigma;
00165             VL53L1_p_008 += 8;
00166             VL53L1_p_008 /= 16;
00167             VL53L1_p_008 += amb_events;
00168 
00169             if (VL53L1_p_008 < min_ambient_threshold_events)
00170                 VL53L1_p_008 = min_ambient_threshold_events;
00171 
00172             palgo->VL53L1_p_056[lb]             = VL53L1_p_008;
00173             palgo->VL53L1_p_032 = VL53L1_p_008;
00174         }
00175 
00176 
00177 
00178     }
00179 
00180 
00181 
00182     palgo->VL53L1_p_045 = 0;
00183 
00184     for (lb = pbins->VL53L1_p_022; lb < pbins->VL53L1_p_024; lb++) {
00185 
00186         if (pbins->bin_data[lb] > palgo->VL53L1_p_056[lb]) {
00187             palgo->VL53L1_p_043[lb]      = 1;
00188             palgo->VL53L1_p_046[lb] = 1;
00189             palgo->VL53L1_p_045++;
00190         } else {
00191             palgo->VL53L1_p_043[lb]      = 0;
00192             palgo->VL53L1_p_046[lb] = 0;
00193         }
00194     }
00195 
00196     LOG_FUNCTION_END(status);
00197 
00198     return status;
00199 
00200 }
00201 
00202 
00203 
00204 
00205 VL53L1_Error VL53L1_f_019(
00206     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00207 {
00208 
00209 
00210 
00211     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00212 
00213     uint8_t  i            = 0;
00214     uint8_t  j            = 0;
00215     uint8_t  found        = 0;
00216 
00217     LOG_FUNCTION_START("");
00218 
00219     palgo->VL53L1_p_049 = 0;
00220 
00221     for (i = 0; i < palgo->VL53L1_p_031; i++) {
00222 
00223         j = (i + 1) % palgo->VL53L1_p_031;
00224 
00225 
00226 
00227         if (i < palgo->VL53L1_p_024 && j < palgo->VL53L1_p_024) {
00228             if (palgo->VL53L1_p_046[i] == 0 &&
00229                 palgo->VL53L1_p_046[j] == 1 &&
00230                 found == 0) {
00231                 palgo->VL53L1_p_049 = i;
00232                 found = 1;
00233             }
00234         }
00235     }
00236 
00237     LOG_FUNCTION_END(status);
00238 
00239     return status;
00240 }
00241 
00242 
00243 VL53L1_Error VL53L1_f_020(
00244     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00245 {
00246 
00247 
00248 
00249     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00250     uint8_t  i            = 0;
00251     uint8_t  j            = 0;
00252     uint8_t  lb            = 0;
00253 
00254     LOG_FUNCTION_START("");
00255 
00256     for (lb = palgo->VL53L1_p_049;
00257         lb < (palgo->VL53L1_p_049 +
00258         palgo->VL53L1_p_031);
00259         lb++) {
00260 
00261 
00262 
00263         i =  lb      % palgo->VL53L1_p_031;
00264         j = (lb + 1) % palgo->VL53L1_p_031;
00265 
00266 
00267 
00268         if (i < palgo->VL53L1_p_024 && j < palgo->VL53L1_p_024) {
00269 
00270             if (palgo->VL53L1_p_046[i] == 0 &&
00271                 palgo->VL53L1_p_046[j] == 1)
00272                 palgo->VL53L1_p_051++;
00273 
00274             if (palgo->VL53L1_p_046[i] > 0)
00275                 palgo->VL53L1_p_047[i] = palgo->VL53L1_p_051;
00276             else
00277                 palgo->VL53L1_p_047[i] = 0;
00278         }
00279 
00280     }
00281 
00282 
00283     if (palgo->VL53L1_p_051 > palgo->VL53L1_p_050)
00284         palgo->VL53L1_p_051 = palgo->VL53L1_p_050;
00285 
00286     LOG_FUNCTION_END(status);
00287 
00288     return status;
00289 
00290 }
00291 
00292 
00293 VL53L1_Error VL53L1_f_021(
00294     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00295 {
00296 
00297 
00298 
00299     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00300 
00301     uint8_t  i            = 0;
00302     uint8_t  j            = 0;
00303     uint8_t  blb            = 0;
00304     uint8_t  pulse_no     = 0;
00305 
00306     uint8_t  max_filter_half_width = 0;
00307 
00308     VL53L1_hist_pulse_data_t *pdata;
00309 
00310     LOG_FUNCTION_START("");
00311 
00312 
00313 
00314     max_filter_half_width = palgo->VL53L1_p_031 - 1;
00315     max_filter_half_width = max_filter_half_width >> 1;
00316 
00317     for (blb = palgo->VL53L1_p_049;
00318         blb < (palgo->VL53L1_p_049 +
00319         palgo->VL53L1_p_031);
00320         blb++) {
00321 
00322 
00323 
00324         i =  blb      % palgo->VL53L1_p_031;
00325         j = (blb + 1) % palgo->VL53L1_p_031;
00326 
00327 
00328 
00329         if (i < palgo->VL53L1_p_024 &&
00330                 j < palgo->VL53L1_p_024) {
00331 
00332 
00333 
00334             if (palgo->VL53L1_p_047[i] == 0 &&
00335                     palgo->VL53L1_p_047[j] > 0) {
00336 
00337                 pulse_no = palgo->VL53L1_p_047[j] - 1;
00338                 pdata   = &(palgo->VL53L1_p_002[pulse_no]);
00339 
00340                 if (pulse_no < palgo->VL53L1_p_050) {
00341                     pdata->VL53L1_p_015 = blb;
00342                     pdata->VL53L1_p_022    = blb + 1;
00343                     pdata->VL53L1_p_025   = 0xFF;
00344                     pdata->VL53L1_p_026     = 0;
00345                     pdata->VL53L1_p_016   = 0;
00346                 }
00347             }
00348 
00349 
00350 
00351             if (palgo->VL53L1_p_047[i] > 0
00352                 && palgo->VL53L1_p_047[j] == 0) {
00353 
00354                 pulse_no = palgo->VL53L1_p_047[i] - 1;
00355                 pdata   = &(palgo->VL53L1_p_002[pulse_no]);
00356 
00357                 if (pulse_no < palgo->VL53L1_p_050) {
00358 
00359                     pdata->VL53L1_p_026    = blb;
00360                     pdata->VL53L1_p_016  = blb + 1;
00361 
00362                     pdata->VL53L1_p_027 =
00363                         (pdata->VL53L1_p_026 + 1) -
00364                         pdata->VL53L1_p_022;
00365                     pdata->VL53L1_p_055 =
00366                         (pdata->VL53L1_p_016 + 1) -
00367                         pdata->VL53L1_p_015;
00368 
00369                     if (pdata->VL53L1_p_055 >
00370                         max_filter_half_width)
00371                         pdata->VL53L1_p_055 =
00372                         max_filter_half_width;
00373                 }
00374 
00375             }
00376         }
00377     }
00378 
00379     LOG_FUNCTION_END(status);
00380 
00381     return status;
00382 
00383 }
00384 
00385 
00386 VL53L1_Error VL53L1_f_028(
00387     VL53L1_HistTargetOrder                target_order,
00388     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00389 {
00390 
00391 
00392 
00393     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00394 
00395     VL53L1_hist_pulse_data_t  tmp;
00396     VL53L1_hist_pulse_data_t *ptmp = &tmp;
00397     VL53L1_hist_pulse_data_t *p0;
00398     VL53L1_hist_pulse_data_t *p1;
00399 
00400     uint8_t i       = 0;
00401     uint8_t swapped = 1;
00402 
00403     LOG_FUNCTION_START("");
00404 
00405     if (!(palgo->VL53L1_p_051 > 1))
00406         goto ENDFUNC;
00407 
00408     while (swapped > 0) {
00409 
00410         swapped = 0;
00411 
00412         for (i = 1; i < palgo->VL53L1_p_051; i++) {
00413 
00414             p0 = &(palgo->VL53L1_p_002[i-1]);
00415             p1 = &(palgo->VL53L1_p_002[i]);
00416 
00417 
00418 
00419             if (target_order
00420             == VL53L1_HIST_TARGET_ORDER__STRONGEST_FIRST) {
00421 
00422                 if (p0->VL53L1_p_013 <
00423                         p1->VL53L1_p_013) {
00424 
00425 
00426 
00427                     memcpy(ptmp,
00428                     p1, sizeof(VL53L1_hist_pulse_data_t));
00429                     memcpy(p1,
00430                     p0, sizeof(VL53L1_hist_pulse_data_t));
00431                     memcpy(p0,
00432                     ptmp, sizeof(VL53L1_hist_pulse_data_t));
00433 
00434                     swapped = 1;
00435                 }
00436 
00437             } else {
00438 
00439                 if (p0->VL53L1_p_014 > p1->VL53L1_p_014) {
00440 
00441 
00442 
00443                     memcpy(ptmp,
00444                     p1, sizeof(VL53L1_hist_pulse_data_t));
00445                     memcpy(p1,
00446                     p0,   sizeof(VL53L1_hist_pulse_data_t));
00447                     memcpy(p0,
00448                     ptmp, sizeof(VL53L1_hist_pulse_data_t));
00449 
00450                     swapped = 1;
00451                 }
00452 
00453             }
00454         }
00455     }
00456 
00457 ENDFUNC:
00458     LOG_FUNCTION_END(status);
00459 
00460     return status;
00461 
00462 }
00463 
00464 
00465 VL53L1_Error VL53L1_f_022(
00466     uint8_t                                pulse_no,
00467     VL53L1_histogram_bin_data_t           *pbins,
00468     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00469 {
00470 
00471 
00472     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00473 
00474     uint8_t  i            = 0;
00475     uint8_t  lb            = 0;
00476 
00477     VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]);
00478 
00479     LOG_FUNCTION_START("");
00480 
00481 
00482 
00483     pdata->VL53L1_p_021  = 0;
00484     pdata->VL53L1_p_020 = 0;
00485 
00486     for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) {
00487         i =  lb % palgo->VL53L1_p_031;
00488         pdata->VL53L1_p_021  += pbins->bin_data[i];
00489         pdata->VL53L1_p_020 += palgo->VL53L1_p_004;
00490     }
00491 
00492 
00493 
00494     pdata->VL53L1_p_013 =
00495         pdata->VL53L1_p_021 - pdata->VL53L1_p_020;
00496 
00497     LOG_FUNCTION_END(status);
00498 
00499     return status;
00500 }
00501 
00502 
00503 VL53L1_Error VL53L1_f_027(
00504     uint8_t                                pulse_no,
00505     uint8_t                                clip_events,
00506     VL53L1_histogram_bin_data_t           *pbins,
00507     VL53L1_hist_gen3_algo_private_data_t  *palgo)
00508 {
00509 
00510 
00511     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00512 
00513     uint8_t   i            = 0;
00514     int16_t   VL53L1_p_015 = 0;
00515     int16_t   VL53L1_p_016   = 0;
00516     int16_t   window_width = 0;
00517     uint32_t  tmp_phase    = 0;
00518 
00519     VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]);
00520 
00521     LOG_FUNCTION_START("");
00522 
00523 
00524 
00525     i = pdata->VL53L1_p_025 % palgo->VL53L1_p_031;
00526 
00527     VL53L1_p_015  = (int16_t)i;
00528     VL53L1_p_015 += (int16_t)pdata->VL53L1_p_015;
00529     VL53L1_p_015 -= (int16_t)pdata->VL53L1_p_025;
00530 
00531     VL53L1_p_016    = (int16_t)i;
00532     VL53L1_p_016   += (int16_t)pdata->VL53L1_p_016;
00533     VL53L1_p_016   -= (int16_t)pdata->VL53L1_p_025;
00534 
00535 
00536     window_width = VL53L1_p_016 - VL53L1_p_015;
00537     if (window_width > 3)
00538         window_width = 3;
00539 
00540     status =
00541         VL53L1_f_030(
00542             VL53L1_p_015,
00543             VL53L1_p_015 + window_width,
00544             palgo->VL53L1_p_031,
00545             clip_events,
00546             pbins,
00547             &(pdata->VL53L1_p_028));
00548 
00549 
00550     if (status == VL53L1_ERROR_NONE)
00551         status =
00552             VL53L1_f_030(
00553                 VL53L1_p_016 - window_width,
00554                 VL53L1_p_016,
00555                 palgo->VL53L1_p_031,
00556                 clip_events,
00557                 pbins,
00558                 &(pdata->VL53L1_p_029));
00559 
00560 
00561     if (pdata->VL53L1_p_028 > pdata->VL53L1_p_029) {
00562         tmp_phase        = pdata->VL53L1_p_028;
00563         pdata->VL53L1_p_028 = pdata->VL53L1_p_029;
00564         pdata->VL53L1_p_029 = tmp_phase;
00565     }
00566 
00567 
00568     if (pdata->VL53L1_p_014 < pdata->VL53L1_p_028)
00569         pdata->VL53L1_p_028 = pdata->VL53L1_p_014;
00570 
00571 
00572     if (pdata->VL53L1_p_014 > pdata->VL53L1_p_029)
00573         pdata->VL53L1_p_029 = pdata->VL53L1_p_014;
00574 
00575     LOG_FUNCTION_END(status);
00576 
00577     return status;
00578 }
00579 
00580 
00581 VL53L1_Error VL53L1_f_030(
00582     int16_t                            VL53L1_p_022,
00583     int16_t                            VL53L1_p_026,
00584     uint8_t                            VL53L1_p_031,
00585     uint8_t                            clip_events,
00586     VL53L1_histogram_bin_data_t       *pbins,
00587     uint32_t                          *pphase)
00588 {
00589 
00590 
00591     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00592 
00593     int16_t  i            = 0;
00594     int16_t  lb            = 0;
00595 
00596     int64_t VL53L1_p_008        = 0;
00597     int64_t event_sum     = 0;
00598     int64_t weighted_sum  = 0;
00599 
00600     LOG_FUNCTION_START("");
00601 
00602     *pphase = VL53L1_MAX_ALLOWED_PHASE;
00603 
00604     for (lb = VL53L1_p_022; lb <= VL53L1_p_026; lb++) {
00605 
00606         if (lb < 0)
00607             i = lb + (int16_t)VL53L1_p_031;
00608         else
00609             i = lb % (int16_t)VL53L1_p_031;
00610 
00611         if ((i >= 0) && (i < VL53L1_HISTOGRAM_BUFFER_SIZE)) {
00612             VL53L1_p_008 =
00613                 (int64_t)pbins->bin_data[i] -
00614                 (int64_t)pbins->VL53L1_p_004;
00615 
00616             if (clip_events > 0 && VL53L1_p_008 < 0)
00617                 VL53L1_p_008 = 0;
00618             event_sum += VL53L1_p_008;
00619             weighted_sum +=
00620                 (VL53L1_p_008 * (1024 + (2048*(int64_t)lb)));
00621         }
00622 
00623         trace_print(
00624             VL53L1_TRACE_LEVEL_INFO,
00625             "\tb = %5d : i = %5d : VL53L1_p_008 = %8d,",
00626             lb, i, VL53L1_p_008);
00627 
00628         trace_print(
00629             VL53L1_TRACE_LEVEL_INFO,
00630             " event_sum = %8d, weighted_sum = %8d\n",
00631             event_sum, weighted_sum);
00632     }
00633 
00634     if (event_sum  > 0) {
00635         weighted_sum += do_division_s(event_sum, 2);
00636         weighted_sum = do_division_s(weighted_sum, event_sum);
00637         if (weighted_sum < 0)
00638             weighted_sum = 0;
00639         *pphase = (uint32_t)weighted_sum;
00640     }
00641 
00642     LOG_FUNCTION_END(status);
00643 
00644     return status;
00645 }
00646 
00647 
00648 VL53L1_Error VL53L1_f_023(
00649     uint8_t                                pulse_no,
00650     VL53L1_histogram_bin_data_t           *pbins,
00651     VL53L1_hist_gen3_algo_private_data_t  *palgo,
00652     int32_t                                pad_value,
00653     VL53L1_histogram_bin_data_t           *ppulse)
00654 {
00655 
00656 
00657 
00658     VL53L1_Error  status  = VL53L1_ERROR_NONE;
00659 
00660     uint8_t  i            = 0;
00661     uint8_t  lb            = 0;
00662 
00663     VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]);
00664 
00665     LOG_FUNCTION_START("");
00666 
00667 
00668 
00669     memcpy(ppulse, pbins, sizeof(VL53L1_histogram_bin_data_t));
00670 
00671 
00672 
00673     for (lb = palgo->VL53L1_p_049;
00674         lb < (palgo->VL53L1_p_049 +
00675         palgo->VL53L1_p_031);
00676         lb++) {
00677 
00678         if (lb < pdata->VL53L1_p_015 || lb > pdata->VL53L1_p_016) {
00679             i =  lb % palgo->VL53L1_p_031;
00680             if (i < ppulse->VL53L1_p_024)
00681                 ppulse->bin_data[i] = pad_value;
00682         }
00683     }
00684 
00685     LOG_FUNCTION_END(status);
00686 
00687     return status;
00688 }
00689 
00690 
00691 VL53L1_Error VL53L1_f_026(
00692     uint8_t                       bin,
00693     uint8_t                       sigma_estimator__sigma_ref_mm,
00694     uint8_t                       VL53L1_p_031,
00695     uint8_t                       VL53L1_p_055,
00696     uint8_t                       crosstalk_compensation_enable,
00697     VL53L1_histogram_bin_data_t  *phist_data_ap,
00698     VL53L1_histogram_bin_data_t  *phist_data_zp,
00699     VL53L1_histogram_bin_data_t  *pxtalk_hist,
00700     uint16_t                     *psigma_est)
00701 {
00702 
00703 
00704     VL53L1_Error status      = VL53L1_ERROR_NONE;
00705     VL53L1_Error func_status = VL53L1_ERROR_NONE;
00706 
00707     uint8_t  i    = 0;
00708     int32_t  VL53L1_p_003    = 0;
00709     int32_t  VL53L1_p_018    = 0;
00710     int32_t  VL53L1_p_001    = 0;
00711     int32_t  a_zp = 0;
00712     int32_t  c_zp = 0;
00713     int32_t  ax   = 0;
00714     int32_t  bx   = 0;
00715     int32_t  cx   = 0;
00716 
00717 
00718 
00719     i = bin % VL53L1_p_031;
00720 
00721 
00722 
00723     VL53L1_f_013(
00724             i,
00725             VL53L1_p_055,
00726             phist_data_zp,
00727             &a_zp,
00728             &VL53L1_p_018,
00729             &c_zp);
00730 
00731 
00732 
00733     VL53L1_f_013(
00734             i,
00735             VL53L1_p_055,
00736             phist_data_ap,
00737             &VL53L1_p_003,
00738             &VL53L1_p_018,
00739             &VL53L1_p_001);
00740 
00741     if (crosstalk_compensation_enable > 0)
00742         VL53L1_f_013(
00743                 i,
00744                 VL53L1_p_055,
00745                 pxtalk_hist,
00746                 &ax,
00747                 &bx,
00748                 &cx);
00749 
00750 
00751 
00752 
00753 
00754 
00755 
00756     func_status =
00757         VL53L1_f_014(
00758             sigma_estimator__sigma_ref_mm,
00759             (uint32_t)VL53L1_p_003,
00760             (uint32_t)VL53L1_p_018,
00761             (uint32_t)VL53L1_p_001,
00762             (uint32_t)a_zp,
00763             (uint32_t)c_zp,
00764             (uint32_t)bx,
00765             (uint32_t)ax,
00766             (uint32_t)cx,
00767             (uint32_t)phist_data_ap->VL53L1_p_004,
00768             phist_data_ap->VL53L1_p_019,
00769             psigma_est);
00770 
00771 
00772 
00773 
00774     if (func_status == VL53L1_ERROR_DIVISION_BY_ZERO)
00775         *psigma_est = 0xFFFF;
00776 
00777 
00778     return status;
00779 }
00780 
00781 
00782 void VL53L1_f_029(
00783     uint8_t                      range_id,
00784     uint8_t                      valid_phase_low,
00785     uint8_t                      valid_phase_high,
00786     uint16_t                     sigma_thres,
00787     VL53L1_histogram_bin_data_t *pbins,
00788     VL53L1_hist_pulse_data_t    *ppulse,
00789     VL53L1_range_data_t         *pdata)
00790 {
00791 
00792     uint16_t  lower_phase_limit = 0;
00793     uint16_t  upper_phase_limit = 0;
00794 
00795 
00796 
00797     pdata->range_id              = range_id;
00798     pdata->time_stamp            = 0;
00799 
00800     pdata->VL53L1_p_015          = ppulse->VL53L1_p_015;
00801     pdata->VL53L1_p_022             = ppulse->VL53L1_p_022;
00802     pdata->VL53L1_p_025            = ppulse->VL53L1_p_025;
00803     pdata->VL53L1_p_026              = ppulse->VL53L1_p_026;
00804     pdata->VL53L1_p_016            = ppulse->VL53L1_p_016;
00805     pdata->VL53L1_p_027             = ppulse->VL53L1_p_027;
00806 
00807 
00808 
00809     pdata->VL53L1_p_030  =
00810         (ppulse->VL53L1_p_016 + 1) - ppulse->VL53L1_p_015;
00811 
00812 
00813 
00814     pdata->zero_distance_phase   = pbins->zero_distance_phase;
00815     pdata->VL53L1_p_005              = ppulse->VL53L1_p_005;
00816     pdata->VL53L1_p_028             = (uint16_t)ppulse->VL53L1_p_028;
00817     pdata->VL53L1_p_014          = (uint16_t)ppulse->VL53L1_p_014;
00818     pdata->VL53L1_p_029             = (uint16_t)ppulse->VL53L1_p_029;
00819     pdata->VL53L1_p_021  = (uint32_t)ppulse->VL53L1_p_021;
00820     pdata->VL53L1_p_013   = ppulse->VL53L1_p_013;
00821     pdata->VL53L1_p_020 = (uint32_t)ppulse->VL53L1_p_020;
00822     pdata->total_periods_elapsed = pbins->total_periods_elapsed;
00823 
00824 
00825 
00826     pdata->range_status = VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK;
00827 
00828 
00829     if (sigma_thres > 0 &&
00830         (uint32_t)ppulse->VL53L1_p_005 > ((uint32_t)sigma_thres << 5))
00831         pdata->range_status = VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK;
00832 
00833 
00834 
00835     lower_phase_limit  = (uint8_t)valid_phase_low << 8;
00836     if (lower_phase_limit < pdata->zero_distance_phase)
00837         lower_phase_limit =
00838             pdata->zero_distance_phase -
00839             lower_phase_limit;
00840     else
00841         lower_phase_limit  = 0;
00842 
00843     upper_phase_limit  = (uint8_t)valid_phase_high << 8;
00844     upper_phase_limit += pbins->zero_distance_phase;
00845 
00846     if (pdata->VL53L1_p_014 < lower_phase_limit ||
00847         pdata->VL53L1_p_014 > upper_phase_limit)
00848         pdata->range_status = VL53L1_DEVICEERROR_RANGEPHASECHECK;
00849 
00850 }
00851 
00852