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