charles macneill
Initial release. Mbed library for VL53L1CB
Diff: src/vl53l1_hist_algos_gen4.c
- Revision:
- 0:3d72bef69191
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/vl53l1_hist_algos_gen4.c Fri Nov 06 12:15:24 2020 +0000
@@ -0,0 +1,556 @@
+
+/*******************************************************************************
+ This file is part of VL53L1 Protected
+
+ Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+
+ License terms: STMicroelectronics Proprietary in accordance with licensing
+ terms at www.st.com/sla0081
+
+ STMicroelectronics confidential
+ Reproduction and Communication of this document is strictly prohibited unless
+ specifically authorized in writing by STMicroelectronics.
+
+*/
+
+
+
+
+#include "vl53l1_types.h"
+#include "vl53l1_platform_log.h"
+
+#include "vl53l1_core_support.h"
+#include "vl53l1_error_codes.h"
+
+#include "vl53l1_hist_core.h"
+#include "vl53l1_hist_algos_gen3.h"
+#include "vl53l1_hist_algos_gen4.h"
+#include "vl53l1_sigma_estimate.h"
+#include "vl53l1_dmax.h"
+#ifdef __KERNEL__
+#include <linux/math64.h>
+#include <linux/kernel.h>
+#endif
+
+
+
+#define LOG_FUNCTION_START(fmt, ...) \
+ _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
+#define LOG_FUNCTION_END(status, ...) \
+ _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
+#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
+ _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
+ status, fmt, ##__VA_ARGS__)
+
+#define trace_print(level, ...) \
+ _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
+ level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
+
+
+void VL53L1_f_032(
+ VL53L1_hist_gen4_algo_filtered_data_t *palgo)
+{
+
+
+ uint8_t lb = 0;
+
+ palgo->VL53L1_p_023 = VL53L1_HISTOGRAM_BUFFER_SIZE;
+ palgo->VL53L1_p_022 = 0;
+ palgo->VL53L1_p_024 = 0;
+
+ for (lb = palgo->VL53L1_p_022; lb < palgo->VL53L1_p_023; lb++) {
+ palgo->VL53L1_p_003[lb] = 0;
+ palgo->VL53L1_p_018[lb] = 0;
+ palgo->VL53L1_p_001[lb] = 0;
+ palgo->VL53L1_p_039[lb] = 0;
+ palgo->VL53L1_p_040[lb] = 0;
+ palgo->VL53L1_p_043[lb] = 0;
+ }
+}
+
+
+VL53L1_Error VL53L1_f_033(
+ VL53L1_dmax_calibration_data_t *pdmax_cal,
+ VL53L1_hist_gen3_dmax_config_t *pdmax_cfg,
+ VL53L1_hist_post_process_config_t *ppost_cfg,
+ VL53L1_histogram_bin_data_t *pbins_input,
+ VL53L1_histogram_bin_data_t *pxtalk,
+ VL53L1_hist_gen3_algo_private_data_t *palgo3,
+ VL53L1_hist_gen4_algo_filtered_data_t *pfiltered,
+ VL53L1_hist_gen3_dmax_private_data_t *pdmax_algo,
+ VL53L1_range_results_t *presults,
+ uint8_t histo_merge_nb)
+{
+
+
+ VL53L1_Error status = VL53L1_ERROR_NONE;
+
+ VL53L1_hist_pulse_data_t *ppulse_data;
+ VL53L1_range_data_t *prange_data;
+
+ uint8_t p = 0;
+ VL53L1_histogram_bin_data_t *pB = &(palgo3->VL53L1_p_010);
+
+ LOG_FUNCTION_START("");
+
+
+
+
+
+ VL53L1_f_016(palgo3);
+
+
+
+ memcpy(
+ &(palgo3->VL53L1_p_010),
+ pbins_input,
+ sizeof(VL53L1_histogram_bin_data_t));
+
+
+
+ presults->cfg_device_state = pbins_input->cfg_device_state;
+ presults->rd_device_state = pbins_input->rd_device_state;
+ presults->zone_id = pbins_input->zone_id;
+ presults->stream_count = pbins_input->result__stream_count;
+ presults->wrap_dmax_mm = 0;
+ presults->max_results = VL53L1_MAX_RANGE_RESULTS;
+ presults->active_results = 0;
+
+ for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++)
+ presults->VL53L1_p_007[p] = 0;
+
+
+
+ VL53L1_hist_calc_zero_distance_phase(&(palgo3->VL53L1_p_010));
+
+
+
+ if (ppost_cfg->hist_amb_est_method ==
+ VL53L1_HIST_AMB_EST_METHOD__THRESHOLDED_BINS)
+ VL53L1_hist_estimate_ambient_from_thresholded_bins(
+ (int32_t)ppost_cfg->ambient_thresh_sigma0,
+ &(palgo3->VL53L1_p_010));
+ else
+ VL53L1_hist_estimate_ambient_from_ambient_bins(
+ &(palgo3->VL53L1_p_010));
+
+
+ VL53L1_hist_remove_ambient_bins(&(palgo3->VL53L1_p_010));
+
+
+ if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
+ VL53L1_f_004(
+ pxtalk,
+ &(palgo3->VL53L1_p_010),
+ &(palgo3->VL53L1_p_038));
+
+
+
+ pdmax_cfg->ambient_thresh_sigma =
+ ppost_cfg->ambient_thresh_sigma1;
+
+ for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++) {
+ if (status == VL53L1_ERROR_NONE) {
+ status =
+ VL53L1_f_001(
+ pdmax_cfg->target_reflectance_for_dmax_calc[p],
+ pdmax_cal,
+ pdmax_cfg,
+ &(palgo3->VL53L1_p_010),
+ pdmax_algo,
+ &(presults->VL53L1_p_007[p]));
+ }
+ }
+
+
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_018(
+ ppost_cfg->ambient_thresh_events_scaler,
+ (int32_t)ppost_cfg->ambient_thresh_sigma1,
+ (int32_t)ppost_cfg->min_ambient_thresh_events,
+ ppost_cfg->algo__crosstalk_compensation_enable,
+ &(palgo3->VL53L1_p_010),
+ &(palgo3->VL53L1_p_038),
+ palgo3);
+
+
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_019(palgo3);
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_020(palgo3);
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_021(palgo3);
+
+
+
+ for (p = 0; p < palgo3->VL53L1_p_051; p++) {
+
+ ppulse_data = &(palgo3->VL53L1_p_002[p]);
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_022(
+ p,
+ &(palgo3->VL53L1_p_010),
+ palgo3);
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_023(
+ p,
+ &(palgo3->VL53L1_p_010),
+ palgo3,
+ pB->VL53L1_p_004,
+ &(palgo3->VL53L1_p_052));
+
+
+
+ if (status == VL53L1_ERROR_NONE) {
+ status =
+ VL53L1_f_023(
+ p,
+ &(palgo3->VL53L1_p_010),
+ palgo3,
+ 0,
+ &(palgo3->VL53L1_p_053));
+ }
+
+
+
+ if (status == VL53L1_ERROR_NONE) {
+ status =
+ VL53L1_f_023(
+ p,
+ &(palgo3->VL53L1_p_038),
+ palgo3,
+ 0,
+ &(palgo3->VL53L1_p_054));
+ }
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_034(
+ p,
+ &(palgo3->VL53L1_p_052),
+ palgo3,
+ pfiltered);
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_035(
+ p,
+ ppost_cfg->noise_threshold,
+ pfiltered,
+ palgo3);
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_026(
+ ppulse_data->VL53L1_p_025,
+ ppost_cfg->sigma_estimator__sigma_ref_mm,
+ palgo3->VL53L1_p_031,
+ ppulse_data->VL53L1_p_055,
+ ppost_cfg->algo__crosstalk_compensation_enable,
+ &(palgo3->VL53L1_p_052),
+ &(palgo3->VL53L1_p_053),
+ &(palgo3->VL53L1_p_054),
+ &(ppulse_data->VL53L1_p_005));
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_027(
+ p,
+ 1,
+ &(palgo3->VL53L1_p_010),
+ palgo3);
+
+ }
+
+
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_028(
+ ppost_cfg->hist_target_order,
+ palgo3);
+
+
+
+ for (p = 0; p < palgo3->VL53L1_p_051; p++) {
+
+ ppulse_data = &(palgo3->VL53L1_p_002[p]);
+
+
+ if (!(presults->active_results < presults->max_results))
+ continue;
+
+
+
+
+ if (ppulse_data->VL53L1_p_013 >
+ ppost_cfg->signal_total_events_limit &&
+ ppulse_data->VL53L1_p_025 < 0xFF) {
+
+ prange_data =
+ &(presults->VL53L1_p_002[presults->active_results]);
+
+ if (status == VL53L1_ERROR_NONE)
+ VL53L1_f_029(
+ presults->active_results,
+ ppost_cfg->valid_phase_low,
+ ppost_cfg->valid_phase_high,
+ ppost_cfg->sigma_thresh,
+ &(palgo3->VL53L1_p_010),
+ ppulse_data,
+ prange_data);
+
+ if (status == VL53L1_ERROR_NONE)
+ status =
+ VL53L1_f_011(
+ pB->vcsel_width,
+ pB->VL53L1_p_019,
+ pB->total_periods_elapsed,
+ pB->result__dss_actual_effective_spads,
+ prange_data,
+ histo_merge_nb);
+
+ if (status == VL53L1_ERROR_NONE)
+ VL53L1_f_012(
+ ppost_cfg->gain_factor,
+ ppost_cfg->range_offset_mm,
+ prange_data);
+
+ presults->active_results++;
+ }
+
+ }
+
+
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+
+VL53L1_Error VL53L1_f_034(
+ uint8_t pulse_no,
+ VL53L1_histogram_bin_data_t *ppulse,
+ VL53L1_hist_gen3_algo_private_data_t *palgo3,
+ VL53L1_hist_gen4_algo_filtered_data_t *pfiltered)
+{
+
+
+
+
+ VL53L1_Error status = VL53L1_ERROR_NONE;
+
+ VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
+
+ uint8_t lb = 0;
+ uint8_t i = 0;
+ int32_t suma = 0;
+ int32_t sumb = 0;
+ int32_t sumc = 0;
+
+ LOG_FUNCTION_START("");
+
+ pfiltered->VL53L1_p_023 = palgo3->VL53L1_p_023;
+ pfiltered->VL53L1_p_022 = palgo3->VL53L1_p_022;
+ pfiltered->VL53L1_p_024 = palgo3->VL53L1_p_024;
+
+
+
+ for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) {
+
+ i = lb % palgo3->VL53L1_p_031;
+
+
+ VL53L1_f_013(
+ i,
+ pdata->VL53L1_p_055,
+ ppulse,
+ &suma,
+ &sumb,
+ &sumc);
+
+
+ pfiltered->VL53L1_p_003[i] = suma;
+ pfiltered->VL53L1_p_018[i] = sumb;
+ pfiltered->VL53L1_p_001[i] = sumc;
+
+
+
+ pfiltered->VL53L1_p_039[i] =
+ (suma + sumb) -
+ (sumc + palgo3->VL53L1_p_004);
+
+
+
+ pfiltered->VL53L1_p_040[i] =
+ (sumb + sumc) -
+ (suma + palgo3->VL53L1_p_004);
+ }
+
+ return status;
+}
+
+
+VL53L1_Error VL53L1_f_035(
+ uint8_t pulse_no,
+ uint16_t noise_threshold,
+ VL53L1_hist_gen4_algo_filtered_data_t *pfiltered,
+ VL53L1_hist_gen3_algo_private_data_t *palgo3)
+{
+
+
+
+ VL53L1_Error status = VL53L1_ERROR_NONE;
+ VL53L1_Error func_status = VL53L1_ERROR_NONE;
+
+ VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
+
+ uint8_t lb = 0;
+ uint8_t i = 0;
+ uint8_t j = 0;
+
+ SUPPRESS_UNUSED_WARNING(noise_threshold);
+
+ for (lb = pdata->VL53L1_p_015; lb < pdata->VL53L1_p_016; lb++) {
+
+ i = lb % palgo3->VL53L1_p_031;
+ j = (lb+1) % palgo3->VL53L1_p_031;
+
+ if (i < palgo3->VL53L1_p_024 &&
+ j < palgo3->VL53L1_p_024) {
+
+ if (pfiltered->VL53L1_p_039[i] == 0 &&
+ pfiltered->VL53L1_p_040[i] == 0)
+
+ pfiltered->VL53L1_p_043[i] = 0;
+
+ else if (pfiltered->VL53L1_p_039[i] >= 0 &&
+ pfiltered->VL53L1_p_040[i] >= 0)
+ pfiltered->VL53L1_p_043[i] = 1;
+
+ else if (pfiltered->VL53L1_p_039[i] < 0 &&
+ pfiltered->VL53L1_p_040[i] >= 0 &&
+ pfiltered->VL53L1_p_039[j] >= 0 &&
+ pfiltered->VL53L1_p_040[j] < 0)
+ pfiltered->VL53L1_p_043[i] = 1;
+
+ else
+ pfiltered->VL53L1_p_043[i] = 0;
+
+
+ if (pfiltered->VL53L1_p_043[i] > 0) {
+
+ pdata->VL53L1_p_025 = lb;
+
+ func_status =
+ VL53L1_f_036(
+ lb,
+ pfiltered->VL53L1_p_003[i],
+ pfiltered->VL53L1_p_018[i],
+ pfiltered->VL53L1_p_001[i],
+ 0,
+ 0,
+ 0,
+ palgo3->VL53L1_p_004,
+ palgo3->VL53L1_p_031,
+ &(pdata->VL53L1_p_014));
+
+ if (func_status ==
+ VL53L1_ERROR_DIVISION_BY_ZERO)
+ pfiltered->VL53L1_p_043[i] = 0;
+
+ }
+ }
+ }
+
+ return status;
+}
+
+
+VL53L1_Error VL53L1_f_036(
+ uint8_t bin,
+ int32_t VL53L1_p_003,
+ int32_t VL53L1_p_018,
+ int32_t VL53L1_p_001,
+ int32_t ax,
+ int32_t bx,
+ int32_t cx,
+ int32_t VL53L1_p_004,
+ uint8_t VL53L1_p_031,
+ uint32_t *pmean_phase)
+{
+
+
+ VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
+
+ int64_t mean_phase = VL53L1_MAX_ALLOWED_PHASE;
+ int32_t mean_phase32;
+ int64_t VL53L1_p_041 = 0;
+ int64_t half_b_minus_amb = 0;
+
+
+ VL53L1_p_041 = 4096 * ((int64_t)VL53L1_p_001 -
+ (int64_t)cx - (int64_t)VL53L1_p_003 - (int64_t)ax);
+ half_b_minus_amb = 4096 * ((int64_t)VL53L1_p_018 -
+ (int64_t)bx - (int64_t)VL53L1_p_004);
+
+ if (half_b_minus_amb != 0) {
+ mean_phase = (4096 * VL53L1_p_041) + half_b_minus_amb;
+ mean_phase = do_division_s(mean_phase, (half_b_minus_amb * 2));
+ mean_phase += 2048;
+ mean_phase += (4096 * (int64_t)bin);
+
+ mean_phase = do_division_s((mean_phase + 1), 2);
+
+ if (mean_phase < 0)
+ mean_phase = 0;
+ if (mean_phase > VL53L1_MAX_ALLOWED_PHASE)
+ mean_phase = VL53L1_MAX_ALLOWED_PHASE;
+
+ mean_phase32 = (int32_t)mean_phase;
+ mean_phase32 = mean_phase32 %
+ ((int32_t)VL53L1_p_031 * 2048);
+ mean_phase = mean_phase32;
+
+ status = VL53L1_ERROR_NONE;
+ }
+
+ *pmean_phase = (uint32_t)mean_phase;
+
+ return status;
+}
+
+
+