Diff: src/vl53l1_xtalk.c

Revision:

0:3ac96e360672

Child:

7:1add29d51e72

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/vl53l1_xtalk.c	Fri Nov 06 10:06:37 2020 +0000
@@ -0,0 +1,1190 @@
+
+/*******************************************************************************
+ 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_xtalk.h"
+#include "vl53l1_hist_core.h"
+
+
+
+
+#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__)
+
+
+VL53L1_Error VL53L1_xtalk_calibration_process_data(
+	VL53L1_xtalk_range_results_t		*pxtalk_results,
+	VL53L1_xtalk_histogram_data_t		*pxtalk_shape,
+	VL53L1_xtalk_calibration_results_t	*pxtalk_cal)
+{
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+
+	VL53L1_xtalk_algo_data_t xtalk_debug;
+	VL53L1_xtalk_algo_data_t *pdebug      = &xtalk_debug;
+	VL53L1_xtalk_range_data_t *pxtalk_data = NULL;
+
+	VL53L1_histogram_bin_data_t avg_bins;
+	VL53L1_histogram_bin_data_t *pavg_bins   = &avg_bins;
+
+	LOG_FUNCTION_START("");
+
+
+
+	memcpy(pavg_bins, &(pxtalk_results->central_histogram_avg),
+		sizeof(VL53L1_histogram_bin_data_t));
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		VL53L1_init_histogram_bin_data_struct(
+			0, 0, &(pdebug->VL53L1_p_057));
+
+	if (status == VL53L1_ERROR_NONE)
+		VL53L1_init_histogram_bin_data_struct(
+			0, 0, &(pdebug->VL53L1_p_058));
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		status = VL53L1_f_047(
+		pxtalk_results,
+		pdebug,
+		&(pxtalk_cal->algo__crosstalk_compensation_x_plane_gradient_kcps
+				),
+		&(pxtalk_cal->algo__crosstalk_compensation_y_plane_gradient_kcps
+				));
+
+
+
+
+
+	if (status != VL53L1_ERROR_NONE)
+		goto ENDFUNC;
+
+	pxtalk_data = &(pxtalk_results->VL53L1_p_002[4]);
+
+	if (pxtalk_data->no_of_samples >  0) {
+
+
+
+		if (status == VL53L1_ERROR_NONE) {
+			memcpy(&(pdebug->VL53L1_p_057),
+			pavg_bins,
+			sizeof(VL53L1_histogram_bin_data_t));
+		}
+
+
+
+		status = VL53L1_f_048(
+		pxtalk_data,
+		pdebug,
+		&(pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps));
+
+
+
+		if (status == VL53L1_ERROR_NONE)
+			status = VL53L1_f_049(
+			pavg_bins,
+			pdebug,
+			pxtalk_data,
+			pxtalk_results->central_histogram__window_start,
+			pxtalk_results->central_histogram__window_end,
+			&(pxtalk_shape->xtalk_shape));
+
+	} else {
+
+
+
+		pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps = 0;
+
+
+
+		pdebug->VL53L1_p_059 = 0;
+
+
+	}
+
+
+ENDFUNC:
+
+
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_generate_dual_reflectance_xtalk_samples(
+	VL53L1_xtalk_range_results_t	*pxtalk_results,
+	uint16_t			expected_target_distance_mm,
+	uint8_t					higher_reflectance,
+	VL53L1_histogram_bin_data_t	*pxtalk_avg_samples
+)
+{
+
+
+	VL53L1_Error status        = VL53L1_ERROR_NONE;
+
+	VL53L1_histogram_bin_data_t *pzone_avg_1   =
+			&(pxtalk_results->histogram_avg_1[0]);
+	VL53L1_histogram_bin_data_t *pzone_avg_2   =
+			&(pxtalk_results->histogram_avg_2[0]);
+
+	VL53L1_histogram_bin_data_t *pxtalk_output = pxtalk_avg_samples;
+
+
+
+	int i = 0;
+
+
+
+	for (i = 0 ; i < 5 ; i++) {
+
+		if (status == VL53L1_ERROR_NONE)
+			VL53L1_init_histogram_bin_data_struct(
+				0, 0, pzone_avg_1);
+
+		if (status == VL53L1_ERROR_NONE)
+			VL53L1_init_histogram_bin_data_struct(
+				0, 0, pzone_avg_2);
+
+		pzone_avg_1++;
+		pzone_avg_2++;
+	}
+
+
+
+
+	pzone_avg_1 = &(pxtalk_results->histogram_avg_1[0]);
+	pzone_avg_2 = &(pxtalk_results->histogram_avg_2[0]);
+
+	for (i = 0 ; i < 5 ; i++) {
+
+		if (status == VL53L1_ERROR_NONE) {
+
+			status = VL53L1_f_050(
+				pzone_avg_1,
+				pzone_avg_2,
+				expected_target_distance_mm,
+				0x01,
+				higher_reflectance,
+				pxtalk_output
+				);
+
+
+
+			pzone_avg_1++;
+			pzone_avg_2++;
+			pxtalk_output++;
+
+		}
+	}
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_f_050(
+	VL53L1_histogram_bin_data_t	*pzone_avg_1,
+	VL53L1_histogram_bin_data_t	*pzone_avg_2,
+	uint16_t			expected_target_distance,
+	uint8_t				subtract_amb,
+	uint8_t				higher_reflectance,
+	VL53L1_histogram_bin_data_t	*pxtalk_output
+)
+{
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+
+	VL53L1_histogram_bin_data_t  zone_avg_realigned;
+
+
+	 SUPPRESS_UNUSED_WARNING(pxtalk_output);
+	 SUPPRESS_UNUSED_WARNING(expected_target_distance);
+
+
+
+	if ((status == VL53L1_ERROR_NONE) && (subtract_amb == 0x01)) {
+		VL53L1_f_037(
+				pzone_avg_1,
+				pzone_avg_1->VL53L1_p_004);
+
+
+		pzone_avg_1->VL53L1_p_004 = 0x0;
+	}
+
+	if ((status == VL53L1_ERROR_NONE) && (subtract_amb == 0x01)) {
+		VL53L1_f_037(
+			pzone_avg_2,
+			pzone_avg_2->VL53L1_p_004);
+
+
+		pzone_avg_2->VL53L1_p_004 = 0x0;
+	}
+
+
+
+
+
+	if (status == VL53L1_ERROR_NONE) {
+		if (higher_reflectance == 0x01) {
+			VL53L1_f_004(
+				pzone_avg_2,
+				pzone_avg_1,
+				&zone_avg_realigned);
+		} else {
+
+
+
+			VL53L1_f_004(
+				pzone_avg_1,
+				pzone_avg_2,
+				&zone_avg_realigned);
+
+
+
+		}
+	}
+
+
+
+
+
+
+
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+VL53L1_Error VL53L1_f_049(
+		VL53L1_histogram_bin_data_t        *pavg_bins,
+		VL53L1_xtalk_algo_data_t           *pdebug,
+		VL53L1_xtalk_range_data_t          *pxtalk_data,
+		uint8_t                             histogram__window_start,
+		uint8_t                             histogram__window_end,
+		VL53L1_xtalk_histogram_shape_t     *pxtalk_shape)
+{
+
+	VL53L1_Error status        = VL53L1_ERROR_NONE;
+
+
+
+	uint32_t ambient_thresh         = 0;
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		VL53L1_f_037(
+			pavg_bins,
+			pavg_bins->VL53L1_p_004);
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		VL53L1_f_051(
+				6,
+				pavg_bins->VL53L1_p_004,
+				&ambient_thresh);
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		status = VL53L1_f_052(
+				pavg_bins,
+				ambient_thresh,
+				histogram__window_start,
+				histogram__window_end);
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		status =  VL53L1_f_053(
+			pavg_bins,
+			pxtalk_data,
+			pdebug,
+			pxtalk_shape);
+
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+
+}
+
+
+VL53L1_Error VL53L1_f_047(
+	VL53L1_xtalk_range_results_t   *pxtalk_results,
+	VL53L1_xtalk_algo_data_t       *pdebug,
+	int16_t                        *xgradient,
+	int16_t                        *ygradient
+	)
+{
+
+
+
+	VL53L1_Error status        = VL53L1_ERROR_NONE;
+
+	VL53L1_xtalk_range_data_t  *presults_int = NULL;
+
+	int          i                   = 0;
+
+	uint32_t xtalk_per_spad[4];
+	int32_t  VL53L1_p_060         = 0;
+	int32_t  VL53L1_p_061         = 0;
+
+	uint8_t  result_invalid          = 0;
+
+
+	LOG_FUNCTION_START("");
+
+
+
+	*xgradient = 0;
+	*ygradient = 0;
+
+
+
+
+	for (i = 0; i < 4; i++)
+		xtalk_per_spad[i] = 0;
+
+
+
+	for (i = 0; i < 4; i++) {
+
+		if (status == VL53L1_ERROR_NONE) {
+
+			presults_int = &(pxtalk_results->VL53L1_p_002[i]);
+
+
+
+
+			if (presults_int->no_of_samples == 0) {
+
+
+				result_invalid = 1;
+				pdebug->VL53L1_p_062[i] = 0;
+
+
+			} else {
+
+				xtalk_per_spad[i] =
+					presults_int->rate_per_spad_kcps_avg;
+
+
+
+				pdebug->VL53L1_p_062[i] =
+					(uint32_t)xtalk_per_spad[i];
+
+			}
+		}
+
+	}
+
+
+
+	if ((status == VL53L1_ERROR_NONE) && (result_invalid == 0)) {
+
+
+
+		if (status == VL53L1_ERROR_NONE) {
+			VL53L1_p_060 = ((int32_t)xtalk_per_spad[1]
+				- (int32_t)xtalk_per_spad[0]) / (8);
+			VL53L1_p_061 = ((int32_t)xtalk_per_spad[3]
+				- (int32_t)xtalk_per_spad[2]) / (8);
+		}
+
+
+
+
+		if (status == VL53L1_ERROR_NONE) {
+			if (VL53L1_p_060 < -32767) {
+				VL53L1_p_060 = -32767;
+			} else {
+				if (VL53L1_p_060 > 32767)
+					VL53L1_p_060 = 32767;
+			}
+
+			if (VL53L1_p_061 < -32767) {
+				VL53L1_p_061 = -32767;
+			} else {
+				if (VL53L1_p_061 > 32767)
+					VL53L1_p_061 = 32767;
+			}
+
+
+
+			pdebug->VL53L1_p_060 = (int16_t)VL53L1_p_060;
+			pdebug->VL53L1_p_061 = (int16_t)VL53L1_p_061;
+		}
+
+	} else {
+
+
+
+		VL53L1_p_060 = 0;
+		VL53L1_p_061 = 0;
+
+		pdebug->VL53L1_p_060 = 0;
+		pdebug->VL53L1_p_061 = 0;
+	}
+
+
+
+	if (status == VL53L1_ERROR_NONE) {
+		*xgradient = (int16_t)VL53L1_p_060;
+		*ygradient = (int16_t)VL53L1_p_061;
+	}
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_f_048(
+	VL53L1_xtalk_range_data_t *pxtalk_data,
+	VL53L1_xtalk_algo_data_t  *pdebug,
+	uint32_t                 *xtalk_mean_offset_kcps
+	)
+{
+
+
+	VL53L1_Error status        = VL53L1_ERROR_NONE;
+
+	uint32_t xtalk_per_spad          = 0;
+	uint8_t  result_invalid          = 0;
+
+	LOG_FUNCTION_START("");
+
+	*xtalk_mean_offset_kcps          = 0;
+
+
+	if (pxtalk_data->no_of_samples == 0) {
+
+
+
+		result_invalid = 1;
+
+
+
+		pdebug->VL53L1_p_059 = 0;
+
+	}
+
+
+
+
+	if ((status == VL53L1_ERROR_NONE) && (result_invalid == 0)) {
+
+
+
+		xtalk_per_spad = pxtalk_data->rate_per_spad_kcps_avg >> 2;
+
+
+
+		pdebug->VL53L1_p_059 = xtalk_per_spad;
+
+
+		if (xtalk_per_spad < 0x3FFFF)
+			*xtalk_mean_offset_kcps     = (uint32_t)xtalk_per_spad;
+		else
+			*xtalk_mean_offset_kcps     = 0x3FFFF;
+
+	} else {
+
+
+
+		*xtalk_mean_offset_kcps     = 0;
+	}
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+
+}
+
+
+
+
+VL53L1_Error VL53L1_f_053(
+	VL53L1_histogram_bin_data_t	*phist_data,
+	VL53L1_xtalk_range_data_t      *pxtalk_data,
+	VL53L1_xtalk_algo_data_t       *pdebug,
+	VL53L1_xtalk_histogram_shape_t *pxtalk_histo
+	)
+{
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+	uint8_t idx;
+	int32_t tmpi32;
+	uint8_t i = 0;
+	uint64_t bin_data[VL53L1_XTALK_HISTO_BINS];
+
+	LOG_FUNCTION_START("");
+
+
+
+
+
+
+	pxtalk_histo->VL53L1_p_023             =
+			phist_data->VL53L1_p_023;
+	pxtalk_histo->cal_config__vcsel_start =
+			phist_data->cal_config__vcsel_start;
+	pxtalk_histo->VL53L1_p_019     =
+			phist_data->VL53L1_p_019;
+	pxtalk_histo->VL53L1_p_022               =
+			phist_data->VL53L1_p_022;
+	pxtalk_histo->time_stamp              =
+			phist_data->time_stamp;
+	pxtalk_histo->vcsel_width             =
+			phist_data->vcsel_width;
+	pxtalk_histo->zero_distance_phase     =
+			phist_data->zero_distance_phase;
+	pxtalk_histo->zone_id                 =
+			phist_data->zone_id;
+	pxtalk_histo->VL53L1_p_024          =
+			VL53L1_XTALK_HISTO_BINS;
+	pxtalk_histo->phasecal_result__reference_phase =
+			phist_data->phasecal_result__reference_phase;
+	pxtalk_histo->phasecal_result__vcsel_start     =
+			phist_data->phasecal_result__vcsel_start;
+
+	memcpy(&(pdebug->VL53L1_p_058),
+		phist_data, sizeof(VL53L1_histogram_bin_data_t));
+
+
+
+
+
+	if (pxtalk_data->signal_total_events_avg == 0) {
+		for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++)
+			bin_data[i] = 0;
+		goto FAIL;
+	}
+
+	for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++) {
+		idx = i + phist_data->number_of_ambient_bins;
+		if (phist_data->bin_data[idx] > 0) {
+			bin_data[i] =
+			((uint64_t)phist_data->bin_data[idx] << 10);
+			tmpi32 = pxtalk_data->signal_total_events_avg / 2;
+			bin_data[i] = bin_data[i] + (uint64_t)tmpi32;
+			bin_data[i] = do_division_u(bin_data[i],
+			(uint64_t)pxtalk_data->signal_total_events_avg);
+		} else {
+			bin_data[i] = 0;
+		}
+	}
+
+FAIL:
+
+
+	for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++)
+		pxtalk_histo->bin_data[i] = (uint32_t)bin_data[i];
+
+
+
+	for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++)
+		pdebug->VL53L1_p_063[i] = pxtalk_histo->bin_data[i];
+
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+
+}
+
+
+VL53L1_Error VL53L1_f_054(
+	VL53L1_customer_nvm_managed_t *pcustomer,
+	VL53L1_dynamic_config_t       *pdyn_cfg,
+	VL53L1_xtalk_histogram_data_t *pxtalk_shape,
+	VL53L1_histogram_bin_data_t   *pip_hist_data,
+	VL53L1_histogram_bin_data_t   *pop_hist_data,
+	VL53L1_histogram_bin_data_t   *pxtalk_count_data)
+{
+
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+
+
+
+	uint32_t xtalk_rate_kcps = 0;
+
+	LOG_FUNCTION_START("");
+
+
+
+	memcpy(pop_hist_data, pip_hist_data,
+			sizeof(VL53L1_histogram_bin_data_t));
+
+
+
+	status =
+		VL53L1_f_040(
+		pcustomer->algo__crosstalk_compensation_plane_offset_kcps,
+		pcustomer->algo__crosstalk_compensation_x_plane_gradient_kcps,
+		pcustomer->algo__crosstalk_compensation_y_plane_gradient_kcps,
+		0,
+		0,
+		pip_hist_data->result__dss_actual_effective_spads,
+
+		pdyn_cfg->roi_config__user_roi_centre_spad,
+		pdyn_cfg->roi_config__user_roi_requested_global_xy_size,
+		&(xtalk_rate_kcps));
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		status =
+			VL53L1_f_041(
+				pip_hist_data,
+				&(pxtalk_shape->xtalk_shape),
+				xtalk_rate_kcps,
+				pxtalk_count_data);
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		status =
+			VL53L1_f_055(
+				pop_hist_data,
+				pxtalk_count_data,
+				pip_hist_data->number_of_ambient_bins);
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_f_040(
+	uint32_t                       mean_offset,
+	int16_t                        xgradient,
+	int16_t                        ygradient,
+	int8_t                         centre_offset_x,
+	int8_t                         centre_offset_y,
+	uint16_t                       roi_effective_spads,
+	uint8_t                        roi_centre_spad,
+	uint8_t                        roi_xy_size,
+	uint32_t                      *xtalk_rate_kcps
+	)
+{
+
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+
+	uint8_t row = 0;
+	uint8_t col = 0;
+
+
+
+	int16_t  bound_l_x = 0;
+	int16_t  bound_r_x = 0;
+	int16_t  bound_u_y = 0;
+	int16_t  bound_d_y = 0;
+
+	int64_t xtalk_rate_ll = 0;
+	int64_t xtalk_rate_ur = 0;
+
+	int64_t xtalk_avg = 0;
+
+	LOG_FUNCTION_START("");
+
+	SUPPRESS_UNUSED_WARNING(roi_effective_spads);
+
+
+
+
+
+
+	if (status == VL53L1_ERROR_NONE) {
+		VL53L1_decode_row_col(
+				roi_centre_spad,
+				&row,
+				&col);
+	}
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"Row", row);
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"Col", col);
+
+
+
+	if (status == VL53L1_ERROR_NONE) {
+		if ((((int16_t)roi_xy_size / 16) & 0x01) == 1)
+			bound_l_x = (int16_t) col -
+			(((int16_t)roi_xy_size / 32) + 1);
+		else
+			bound_l_x = (int16_t) col -
+			((int16_t)roi_xy_size / 32);
+
+		bound_r_x = (int16_t) col + ((int16_t)roi_xy_size / 32);
+
+		if ((((int16_t)roi_xy_size) & 0x01) == 1)
+			bound_d_y = (int16_t) row -
+			((((int16_t)roi_xy_size & 0x0f) / 2) + 1);
+		else
+			bound_d_y = (int16_t) row -
+			(((int16_t)roi_xy_size & 0x0f) / 2);
+
+		bound_u_y = (int16_t) row +
+				(((int16_t)roi_xy_size & 0xf) / 2);
+	}
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"Bound_l_x", bound_l_x);
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"Bound_r_x", bound_r_x);
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"Bound_u_y", bound_u_y);
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"Bound_d_y", bound_d_y);
+
+
+
+
+	if (status == VL53L1_ERROR_NONE) {
+		bound_l_x =  (2 * bound_l_x) - 15 +
+				(2 * (int16_t)centre_offset_x);
+		bound_r_x =  (2 * bound_r_x) - 15 +
+				(2 * (int16_t)centre_offset_x);
+		bound_u_y =  (2 * bound_u_y) - 15 +
+				(2 * (int16_t)centre_offset_y);
+		bound_d_y =  (2 * bound_d_y) - 15 +
+				(2 * (int16_t)centre_offset_y);
+	}
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"Bound_l_x", bound_l_x);
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"Bound_r_x", bound_r_x);
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"Bound_u_y", bound_u_y);
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"Bound_d_y", bound_d_y);
+
+
+
+
+	if (status == VL53L1_ERROR_NONE) {
+		xtalk_rate_ll  = ((int64_t)bound_l_x *
+			((int64_t)xgradient)) + ((int64_t)bound_d_y *
+					((int64_t)ygradient));
+		xtalk_rate_ll  = do_division_s((xtalk_rate_ll + 1), 2);
+		xtalk_rate_ll += ((int64_t)mean_offset * 4);
+
+		xtalk_rate_ur  = ((int64_t)bound_r_x *
+			((int64_t)xgradient)) + ((int64_t)bound_u_y *
+			((int64_t)ygradient));
+		xtalk_rate_ur  = do_division_s((xtalk_rate_ur + 1), 2);
+		xtalk_rate_ur += ((int64_t)mean_offset * 4);
+	}
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"xtalk_rate_ll", xtalk_rate_ll);
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"xtalk_rate_ur", xtalk_rate_ur);
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		xtalk_avg = do_division_s(
+			((xtalk_rate_ll + xtalk_rate_ur) + 1), 2);
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"xtalk_avg", xtalk_avg);
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		if (xtalk_avg < 0)
+			xtalk_avg = 0;
+
+
+
+
+
+	*xtalk_rate_kcps = (uint32_t) xtalk_avg;
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"xtalk_rate_kcps", xtalk_avg);
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+
+VL53L1_Error VL53L1_f_041(
+	VL53L1_histogram_bin_data_t    *phist_data,
+	VL53L1_xtalk_histogram_shape_t *pxtalk_data,
+	uint32_t                        xtalk_rate_kcps,
+	VL53L1_histogram_bin_data_t    *pxtalkcount_data
+	)
+{
+
+
+	VL53L1_Error status              = VL53L1_ERROR_NONE;
+
+	uint64_t xtalk_events_per_spad = 0;
+	uint64_t xtalk_total_events = 0;
+	uint64_t xtalk_temp_bin = 0;
+
+	uint8_t  i = 0;
+
+	LOG_FUNCTION_START("");
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"pk_duration_internal", phist_data->peak_duration_us);
+
+
+
+	xtalk_events_per_spad = do_division_u((((uint64_t)xtalk_rate_kcps *
+		(uint64_t)phist_data->peak_duration_us) + 500), 1000);
+
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"xtalk_events_per_spad", xtalk_events_per_spad);
+
+
+
+
+	xtalk_total_events = xtalk_events_per_spad *
+		(uint64_t)phist_data->result__dss_actual_effective_spads;
+
+	xtalk_total_events = do_division_u((xtalk_total_events), 256);
+
+	xtalk_total_events = do_division_u((xtalk_total_events + 1024), 2048);
+
+	if (xtalk_total_events > 0xFFFFFFFF)
+		xtalk_total_events = 0xFFFFFFFF;
+
+	trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"xtalk_total_events", xtalk_total_events);
+
+
+
+
+
+
+	for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) {
+		xtalk_temp_bin = (uint64_t)pxtalk_data->bin_data[i] *
+				(uint64_t)xtalk_total_events;
+		xtalk_temp_bin = do_division_u((xtalk_temp_bin + 512), 1024);
+		pxtalkcount_data->bin_data[i] = (uint32_t)xtalk_temp_bin;
+
+		trace_print(
+			VL53L1_TRACE_LEVEL_DEBUG,
+			"    %-48s : %10d\n",
+			"bin_data", pxtalkcount_data->bin_data[i]);
+	}
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_f_055(
+	VL53L1_histogram_bin_data_t	*phist_data,
+	VL53L1_histogram_bin_data_t	*pxtalk_data,
+	uint8_t					xtalk_bin_offset)
+{
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+
+	uint8_t  i = 0;
+
+	int32_t  temp_bin;
+
+	LOG_FUNCTION_START("");
+
+
+
+
+	if (status == VL53L1_ERROR_NONE)
+		for (i = xtalk_bin_offset;
+				i < pxtalk_data->VL53L1_p_024; i++) {
+
+			temp_bin = (int32_t)phist_data->bin_data[i] -
+			(int32_t)pxtalk_data->bin_data[i - xtalk_bin_offset];
+
+			if (temp_bin < 0)
+				temp_bin = 0;
+
+			phist_data->bin_data[i] = (uint32_t)temp_bin;
+		}
+
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_f_052(
+	VL53L1_histogram_bin_data_t		*pxtalk_data,
+	uint32_t						amb_threshold,
+	uint8_t						VL53L1_p_022,
+	uint8_t						VL53L1_p_026)
+{
+
+
+	VL53L1_Error status = VL53L1_ERROR_NONE;
+
+	uint8_t i = 0;
+	uint8_t first_bin_int = 0;
+	uint8_t first_bin_inc = 0;
+	uint8_t last_bin_int  = 0;
+	uint8_t realign_bin   = 0;
+	uint8_t realign_index = 0;
+	int32_t realign_bin_data[VL53L1_HISTOGRAM_BUFFER_SIZE];
+
+	LOG_FUNCTION_START("");
+
+
+
+	for (i = 0 ; i < VL53L1_HISTOGRAM_BUFFER_SIZE ; i++)
+		realign_bin_data[i] = 0;
+
+	first_bin_int = VL53L1_p_022;
+	last_bin_int  = VL53L1_p_026;
+
+
+
+
+
+	VL53L1_hist_remove_ambient_bins(pxtalk_data);
+
+
+
+	first_bin_int = (first_bin_int) %
+				pxtalk_data->VL53L1_p_024;
+
+	last_bin_int = (last_bin_int) %
+				pxtalk_data->VL53L1_p_024;
+
+	first_bin_inc = (first_bin_int + 1) % pxtalk_data->VL53L1_p_024;
+
+
+
+	if (first_bin_inc > last_bin_int) {
+
+
+		realign_bin = pxtalk_data->VL53L1_p_024 - first_bin_inc;
+
+
+		first_bin_int = (first_bin_int + realign_bin) %
+				pxtalk_data->VL53L1_p_024;
+		last_bin_int = (last_bin_int + realign_bin) %
+				pxtalk_data->VL53L1_p_024;
+
+
+		pxtalk_data->zero_distance_phase =
+			pxtalk_data->zero_distance_phase +
+			((uint16_t)realign_bin * 2048);
+	}
+
+	if (realign_bin > 0) {
+
+		for (i = 0; i < pxtalk_data->VL53L1_p_024; i++)
+			realign_bin_data[i] = pxtalk_data->bin_data[i];
+
+
+		for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) {
+			realign_index = (pxtalk_data->VL53L1_p_024 -
+				realign_bin + i)
+				% pxtalk_data->VL53L1_p_024;
+
+			pxtalk_data->bin_data[i] =
+				realign_bin_data[realign_index];
+		}
+	}
+
+
+
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"first bin int", first_bin_int);
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"last bin int", last_bin_int);
+
+	trace_print(
+		VL53L1_TRACE_LEVEL_DEBUG,
+		"    %-48s : %10d\n",
+		"amb thresh", amb_threshold);
+
+
+
+
+
+	for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) {
+
+		if (first_bin_int <= last_bin_int) {
+			if ((i >= first_bin_int) && (i <= last_bin_int)) {
+				if (pxtalk_data->bin_data[i] <
+						(int32_t)amb_threshold)
+					pxtalk_data->bin_data[i] = 0;
+			} else {
+				pxtalk_data->bin_data[i] = 0;
+			}
+		} else {
+			if ((i >= first_bin_int) || (i <= last_bin_int)) {
+				if (pxtalk_data->bin_data[i] <
+						(int32_t)amb_threshold) {
+					pxtalk_data->bin_data[i] = 0;
+				}
+			} else {
+				pxtalk_data->bin_data[i] = 0;
+			}
+		}
+	}
+
+
+
+
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+VL53L1_Error VL53L1_f_051(
+		uint8_t                      sigma_mult,
+		int32_t                      VL53L1_p_004,
+		uint32_t                    *ambient_noise)
+{
+
+
+
+	VL53L1_Error status              = VL53L1_ERROR_NONE;
+
+	uint32_t ambient_events_per_bin_int = 0;
+
+	LOG_FUNCTION_START("");
+
+	if (VL53L1_p_004 <= 0)
+		ambient_events_per_bin_int = 1;
+	else
+		ambient_events_per_bin_int = (uint32_t)VL53L1_p_004;
+
+	*ambient_noise =  VL53L1_isqrt(ambient_events_per_bin_int);
+
+	*ambient_noise = *ambient_noise * (uint32_t)sigma_mult;
+
+	LOG_FUNCTION_END(status);
+
+	return status;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+