vl53l1_hist_core.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_platform_log.h"
00026 
00027 #include "vl53l1_core_support.h"
00028 #include "vl53l1_hist_structs.h"
00029 
00030 #include "vl53l1_xtalk.h"
00031 #include "vl53l1_sigma_estimate.h"
00032 
00033 #include "vl53l1_hist_core.h"
00034 
00035 
00036 
00037 #define LOG_FUNCTION_START(fmt, ...) \
00038     _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
00039 #define LOG_FUNCTION_END(status, ...) \
00040     _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
00041 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
00042     _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00043     status, fmt, ##__VA_ARGS__)
00044 
00045 #define trace_print(level, ...) \
00046     _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
00047     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
00048 
00049 
00050 void VL53L1_f_013(
00051     uint8_t                         VL53L1_p_018,
00052     uint8_t                         filter_woi,
00053     VL53L1_histogram_bin_data_t    *pbins,
00054     int32_t                        *pa,
00055     int32_t                        *pb,
00056     int32_t                        *pc)
00057 {
00058 
00059 
00060     uint8_t w = 0;
00061     uint8_t j = 0;
00062 
00063     *pa = 0;
00064     *pb = pbins->bin_data[VL53L1_p_018];
00065     *pc = 0;
00066 
00067     for (w = 0 ; w < ((filter_woi << 1)+1) ; w++) {
00068 
00069 
00070         j = ((VL53L1_p_018 + w + pbins->VL53L1_p_024) -
00071             filter_woi) % pbins->VL53L1_p_024;
00072 
00073 
00074         if (w < filter_woi)
00075             *pa += pbins->bin_data[j];
00076         else if (w > filter_woi)
00077             *pc += pbins->bin_data[j];
00078     }
00079 }
00080 
00081 
00082 VL53L1_Error VL53L1_f_011(
00083     uint16_t           vcsel_width,
00084     uint16_t           fast_osc_frequency,
00085     uint32_t           total_periods_elapsed,
00086     uint16_t           VL53L1_p_006,
00087     VL53L1_range_data_t  *pdata,
00088     uint8_t histo_merge_nb)
00089 {
00090     VL53L1_Error     status = VL53L1_ERROR_NONE;
00091 
00092     uint32_t    pll_period_us       = 0;
00093     uint32_t    periods_elapsed     = 0;
00094     uint32_t    count_rate_total    = 0;
00095 
00096     LOG_FUNCTION_START("");
00097 
00098 
00099 
00100     pdata->width                  = vcsel_width;
00101     pdata->fast_osc_frequency     = fast_osc_frequency;
00102     pdata->total_periods_elapsed  = total_periods_elapsed;
00103     pdata->VL53L1_p_006 = VL53L1_p_006;
00104 
00105 
00106 
00107     if (pdata->fast_osc_frequency == 0)
00108         status = VL53L1_ERROR_DIVISION_BY_ZERO;
00109 
00110     if (pdata->total_periods_elapsed == 0)
00111         status = VL53L1_ERROR_DIVISION_BY_ZERO;
00112 
00113     if (status == VL53L1_ERROR_NONE) {
00114 
00115 
00116 
00117         pll_period_us =
00118             VL53L1_calc_pll_period_us(pdata->fast_osc_frequency);
00119 
00120 
00121 
00122         periods_elapsed      = pdata->total_periods_elapsed + 1;
00123 
00124 
00125 
00126         pdata->peak_duration_us    = VL53L1_duration_maths(
00127             pll_period_us,
00128             (uint32_t)pdata->width,
00129             VL53L1_RANGING_WINDOW_VCSEL_PERIODS,
00130             periods_elapsed);
00131 
00132         pdata->woi_duration_us     = VL53L1_duration_maths(
00133             pll_period_us,
00134             ((uint32_t)pdata->VL53L1_p_030) << 4,
00135             VL53L1_RANGING_WINDOW_VCSEL_PERIODS,
00136             periods_elapsed);
00137 
00138 
00139 
00140         pdata->peak_signal_count_rate_mcps = VL53L1_rate_maths(
00141             (int32_t)pdata->VL53L1_p_013,
00142             pdata->peak_duration_us);
00143 
00144         pdata->avg_signal_count_rate_mcps = VL53L1_rate_maths(
00145             (int32_t)pdata->VL53L1_p_013,
00146             pdata->woi_duration_us);
00147 
00148         pdata->ambient_count_rate_mcps    = VL53L1_rate_maths(
00149             (int32_t)pdata->VL53L1_p_020,
00150             pdata->woi_duration_us);
00151 
00152 
00153 
00154         count_rate_total =
00155             (uint32_t)pdata->peak_signal_count_rate_mcps +
00156             (uint32_t)pdata->ambient_count_rate_mcps;
00157 
00158         if (histo_merge_nb > 1)
00159             count_rate_total /= histo_merge_nb;
00160 
00161         pdata->total_rate_per_spad_mcps   =
00162             VL53L1_rate_per_spad_maths(
00163                      0x06,
00164                      count_rate_total,
00165                      pdata->VL53L1_p_006,
00166                      0xFFFF);
00167 
00168 
00169 
00170         pdata->VL53L1_p_012   =
00171             VL53L1_events_per_spad_maths(
00172                 pdata->VL53L1_p_013,
00173                 pdata->VL53L1_p_006,
00174                 pdata->peak_duration_us);
00175 
00176 
00177 
00178 
00179         trace_print(
00180             VL53L1_TRACE_LEVEL_DEBUG,
00181             "    %d:%-46s : %10d\n",
00182             pdata->range_id, "peak_duration_us",
00183             pdata->peak_duration_us);
00184         trace_print(
00185             VL53L1_TRACE_LEVEL_DEBUG,
00186             "    %d:%-46s : %10d\n",
00187             pdata->range_id, "woi_duration_us",
00188             pdata->woi_duration_us);
00189         trace_print(
00190             VL53L1_TRACE_LEVEL_DEBUG,
00191             "    %d:%-46s : %10u\n",
00192             pdata->range_id, "peak_signal_count_rate_mcps",
00193             pdata->peak_signal_count_rate_mcps);
00194         trace_print(
00195             VL53L1_TRACE_LEVEL_DEBUG,
00196             "    %d:%-46s : %10u\n",
00197             pdata->range_id, "ambient_count_rate_mcps",
00198             pdata->ambient_count_rate_mcps);
00199         trace_print(
00200             VL53L1_TRACE_LEVEL_DEBUG,
00201             "    %d:%-46s : %10u\n",
00202             pdata->range_id, "total_rate_per_spad_mcps",
00203             pdata->total_rate_per_spad_mcps);
00204         trace_print(
00205             VL53L1_TRACE_LEVEL_DEBUG,
00206             "    %d:%-46s : %10u\n",
00207             pdata->range_id, "VL53L1_p_012",
00208             pdata->VL53L1_p_012);
00209 
00210     }
00211 
00212     LOG_FUNCTION_END(status);
00213 
00214     return status;
00215 }
00216 
00217 
00218 void VL53L1_f_012(
00219     uint16_t             gain_factor,
00220     int16_t              range_offset_mm,
00221     VL53L1_range_data_t *pdata)
00222 {
00223 
00224 
00225     LOG_FUNCTION_START("");
00226 
00227 
00228 
00229     pdata->min_range_mm =
00230         (int16_t)VL53L1_range_maths(
00231                 pdata->fast_osc_frequency,
00232                 pdata->VL53L1_p_028,
00233                 pdata->zero_distance_phase,
00234                 0,
00235                 (int32_t)gain_factor,
00236                 (int32_t)range_offset_mm);
00237 
00238     pdata->median_range_mm =
00239         (int16_t)VL53L1_range_maths(
00240                 pdata->fast_osc_frequency,
00241                 pdata->VL53L1_p_014,
00242                 pdata->zero_distance_phase,
00243                 0,
00244                 (int32_t)gain_factor,
00245                 (int32_t)range_offset_mm);
00246 
00247     pdata->max_range_mm =
00248         (int16_t)VL53L1_range_maths(
00249                 pdata->fast_osc_frequency,
00250                 pdata->VL53L1_p_029,
00251                 pdata->zero_distance_phase,
00252                 0,
00253                 (int32_t)gain_factor,
00254                 (int32_t)range_offset_mm);
00255 
00256 
00257 
00258 
00259 
00260     LOG_FUNCTION_END(0);
00261 }
00262 
00263 
00264 void  VL53L1_f_037(
00265     VL53L1_histogram_bin_data_t   *pdata,
00266     int32_t                        ambient_estimate_counts_per_bin)
00267 {
00268 
00269 
00270     uint8_t i = 0;
00271 
00272     for (i = 0 ; i <  pdata->VL53L1_p_024 ; i++)
00273         pdata->bin_data[i] = pdata->bin_data[i] -
00274             ambient_estimate_counts_per_bin;
00275 }
00276 
00277 
00278 void  VL53L1_f_004(
00279     VL53L1_histogram_bin_data_t   *pxtalk,
00280     VL53L1_histogram_bin_data_t   *pbins,
00281     VL53L1_histogram_bin_data_t   *pxtalk_realigned)
00282 {
00283 
00284 
00285     uint8_t i          = 0;
00286     uint8_t min_bins   = 0;
00287     int8_t  bin_offset = 0;
00288     int8_t  bin_access = 0;
00289 
00290     LOG_FUNCTION_START("");
00291 
00292 
00293 
00294 
00295 
00296     memcpy(
00297         pxtalk_realigned,
00298         pbins,
00299         sizeof(VL53L1_histogram_bin_data_t));
00300 
00301     for (i = 0 ; i < pxtalk_realigned->VL53L1_p_023 ; i++)
00302         pxtalk_realigned->bin_data[i] = 0;
00303 
00304 
00305 
00306     bin_offset =  VL53L1_f_038(
00307                         pbins,
00308                         pxtalk);
00309 
00310 
00311 
00312     if (pxtalk->VL53L1_p_024 < pbins->VL53L1_p_024)
00313         min_bins = pxtalk->VL53L1_p_024;
00314     else
00315         min_bins = pbins->VL53L1_p_024;
00316 
00317 
00318     for (i = 0 ; i <  min_bins ; i++) {
00319 
00320 
00321 
00322         if (bin_offset >= 0)
00323             bin_access = ((int8_t)i + (int8_t)bin_offset)
00324                 % (int8_t)pbins->VL53L1_p_024;
00325         else
00326             bin_access = ((int8_t)pbins->VL53L1_p_024 +
00327                 ((int8_t)i + (int8_t)bin_offset))
00328                     % (int8_t)pbins->VL53L1_p_024;
00329 
00330         trace_print(
00331             VL53L1_TRACE_LEVEL_DEBUG,
00332             "Subtract:     %8d : %8d : %8d : %8d : %8d : %8d\n",
00333             i, bin_access, bin_offset, pbins->VL53L1_p_024,
00334             pbins->bin_data[(uint8_t)bin_access],
00335             pxtalk->bin_data[i]);
00336 
00337 
00338 
00339         if (pbins->bin_data[(uint8_t)bin_access] >
00340             pxtalk->bin_data[i]) {
00341 
00342             pbins->bin_data[(uint8_t)bin_access] =
00343                 pbins->bin_data[(uint8_t)bin_access]
00344                 - pxtalk->bin_data[i];
00345 
00346         } else {
00347             pbins->bin_data[(uint8_t)bin_access] = 0;
00348         }
00349 
00350 
00351 
00352 
00353         pxtalk_realigned->bin_data[(uint8_t)bin_access] =
00354             pxtalk->bin_data[i];
00355 
00356 
00357 
00358     }
00359 
00360 
00361 
00362     LOG_FUNCTION_END(0);
00363 }
00364 
00365 
00366 int8_t  VL53L1_f_038(
00367     VL53L1_histogram_bin_data_t   *pdata1,
00368     VL53L1_histogram_bin_data_t   *pdata2)
00369 {
00370 
00371 
00372     int32_t  phase_delta      = 0;
00373     int8_t   bin_offset       = 0;
00374     uint32_t period           = 0;
00375     uint32_t remapped_phase   = 0;
00376 
00377     LOG_FUNCTION_START("");
00378 
00379 
00380 
00381     period = 2048 *
00382         (uint32_t)VL53L1_decode_vcsel_period(pdata1->VL53L1_p_009);
00383 
00384     remapped_phase = (uint32_t)pdata2->zero_distance_phase % period;
00385 
00386 
00387     phase_delta = (int32_t)pdata1->zero_distance_phase
00388                 - (int32_t)remapped_phase;
00389 
00390 
00391 
00392     if (phase_delta > 0)
00393         bin_offset = (int8_t)((phase_delta + 1024) / 2048);
00394     else
00395         bin_offset = (int8_t)((phase_delta - 1024) / 2048);
00396 
00397     LOG_FUNCTION_END(0);
00398 
00399     return bin_offset;
00400 }
00401 
00402 
00403 VL53L1_Error  VL53L1_f_039(
00404     VL53L1_histogram_bin_data_t   *pidata,
00405     VL53L1_histogram_bin_data_t   *podata)
00406 {
00407 
00408 
00409     VL53L1_Error status = VL53L1_ERROR_NONE;
00410 
00411     uint8_t  bin_initial_index[VL53L1_MAX_BIN_SEQUENCE_CODE+1];
00412     uint8_t  bin_repeat_count[VL53L1_MAX_BIN_SEQUENCE_CODE+1];
00413 
00414     uint8_t  bin_cfg        = 0;
00415     uint8_t  bin_seq_length = 0;
00416     int32_t  repeat_count   = 0;
00417 
00418     uint8_t  VL53L1_p_018       = 0;
00419     uint8_t  lc       = 0;
00420     uint8_t  i       = 0;
00421 
00422     LOG_FUNCTION_START("");
00423 
00424 
00425 
00426     memcpy(podata, pidata, sizeof(VL53L1_histogram_bin_data_t));
00427 
00428 
00429     podata->VL53L1_p_024 = 0;
00430 
00431     for (lc = 0 ; lc < VL53L1_MAX_BIN_SEQUENCE_LENGTH ; lc++)
00432         podata->bin_seq[lc] = VL53L1_MAX_BIN_SEQUENCE_CODE+1;
00433 
00434     for (lc = 0 ; lc < podata->VL53L1_p_023 ; lc++)
00435         podata->bin_data[lc] = 0;
00436 
00437 
00438 
00439     for (lc = 0 ; lc <= VL53L1_MAX_BIN_SEQUENCE_CODE ; lc++) {
00440         bin_initial_index[lc] = 0x00;
00441         bin_repeat_count[lc]  = 0x00;
00442     }
00443 
00444 
00445 
00446 
00447     bin_seq_length = 0x00;
00448 
00449     for (lc = 0 ; lc < VL53L1_MAX_BIN_SEQUENCE_LENGTH ; lc++) {
00450 
00451         bin_cfg = pidata->bin_seq[lc];
00452 
00453 
00454 
00455         if (bin_repeat_count[bin_cfg] == 0) {
00456             bin_initial_index[bin_cfg]      = bin_seq_length * 4;
00457             podata->bin_seq[bin_seq_length] = bin_cfg;
00458             bin_seq_length++;
00459         }
00460 
00461         bin_repeat_count[bin_cfg]++;
00462 
00463 
00464 
00465         VL53L1_p_018 = bin_initial_index[bin_cfg];
00466 
00467         for (i = 0 ; i < 4 ; i++)
00468             podata->bin_data[VL53L1_p_018+i] +=
00469                 pidata->bin_data[lc*4+i];
00470 
00471     }
00472 
00473 
00474 
00475     for (lc = 0 ; lc < VL53L1_MAX_BIN_SEQUENCE_LENGTH ; lc++) {
00476 
00477         bin_cfg = podata->bin_seq[lc];
00478 
00479         if (bin_cfg <= VL53L1_MAX_BIN_SEQUENCE_CODE)
00480             podata->bin_rep[lc] =
00481                 bin_repeat_count[bin_cfg];
00482         else
00483             podata->bin_rep[lc] = 0;
00484     }
00485 
00486     podata->VL53L1_p_024 = bin_seq_length * 4;
00487 
00488 
00489 
00490 
00491 
00492     for (lc = 0 ; lc <= VL53L1_MAX_BIN_SEQUENCE_CODE ; lc++) {
00493 
00494         repeat_count = (int32_t)bin_repeat_count[lc];
00495 
00496         if (repeat_count > 0) {
00497 
00498             VL53L1_p_018 = bin_initial_index[lc];
00499 
00500             for (i = 0 ; i < 4 ; i++) {
00501                 podata->bin_data[VL53L1_p_018+i] +=
00502                     (repeat_count/2);
00503                 podata->bin_data[VL53L1_p_018+i] /=
00504                     repeat_count;
00505             }
00506         }
00507     }
00508 
00509 
00510 
00511     podata->number_of_ambient_bins = 0;
00512     if ((bin_repeat_count[7] > 0) ||
00513         (bin_repeat_count[15] > 0))
00514         podata->number_of_ambient_bins = 4;
00515 
00516     LOG_FUNCTION_END(status);
00517 
00518     return status;
00519 }
00520