ST Expansion SW Team
Rename library
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging
Diff: modules/vl53lx_api_calibration.c
- Revision:
- 5:89031b2f5316
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/vl53lx_api_calibration.c Wed Jul 14 12:45:49 2021 +0100
@@ -0,0 +1,1041 @@
+
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/******************************************************************************
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+
+ This file is part of VL53LX and is dual licensed,
+ either GPL-2.0+
+ or 'BSD 3-clause "New" or "Revised" License' , at your option.
+ ******************************************************************************
+ */
+
+
+
+
+
+#include "vl53lx_platform.h"
+#include "vl53lx_platform_ipp.h"
+#include "vl53lx_ll_def.h"
+#include "vl53lx_ll_device.h"
+#include "vl53lx_register_map.h"
+#include "vl53lx_register_funcs.h"
+#include "vl53lx_register_settings.h"
+#include "vl53lx_hist_map.h"
+#include "vl53lx_hist_structs.h"
+#include "vl53lx_core.h"
+#include "vl53lx_wait.h"
+#include "vl53lx_api_preset_modes.h"
+#include "vl53lx_silicon_core.h"
+#include "vl53lx_api_core.h"
+#include "vl53lx_api_calibration.h"
+
+#ifdef VL53LX_LOG_ENABLE
+ #include "vl53lx_api_debug.h"
+#endif
+
+
+#define LOG_FUNCTION_START(fmt, ...) \
+ _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
+#define LOG_FUNCTION_END(status, ...) \
+ _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
+#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
+ _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_CORE, status, \
+ fmt, ##__VA_ARGS__)
+
+#define trace_print(level, ...) \
+ _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_CORE, \
+ level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
+
+
+VL53LX_Error VL53LX_run_ref_spad_char(
+ VL53LX_DEV Dev,
+ VL53LX_Error *pcal_status)
+{
+
+
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+ VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
+
+ uint8_t comms_buffer[6];
+
+ VL53LX_refspadchar_config_t *prefspadchar = &(pdev->refspadchar);
+
+ LOG_FUNCTION_START("");
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_enable_powerforce(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_set_ref_spad_char_config(
+ Dev,
+ prefspadchar->VL53LX_p_005,
+ prefspadchar->timeout_us,
+ prefspadchar->target_count_rate_mcps,
+ prefspadchar->max_count_rate_limit_mcps,
+ prefspadchar->min_count_rate_limit_mcps,
+ pdev->stat_nvm.osc_measured__fast_osc__frequency);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_run_device_test(
+ Dev,
+ prefspadchar->device_test_mode);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_ReadMulti(
+ Dev,
+ VL53LX_REF_SPAD_CHAR_RESULT__NUM_ACTUAL_REF_SPADS,
+ comms_buffer,
+ 2);
+
+ if (status == VL53LX_ERROR_NONE) {
+ pdev->dbg_results.ref_spad_char_result__num_actual_ref_spads =
+ comms_buffer[0];
+ pdev->dbg_results.ref_spad_char_result__ref_location =
+ comms_buffer[1];
+ }
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_WriteMulti(
+ Dev,
+ VL53LX_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
+ comms_buffer,
+ 2);
+
+ if (status == VL53LX_ERROR_NONE) {
+ pdev->customer.ref_spad_man__num_requested_ref_spads =
+ comms_buffer[0];
+ pdev->customer.ref_spad_man__ref_location =
+ comms_buffer[1];
+ }
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_ReadMulti(
+ Dev,
+ VL53LX_RESULT__SPARE_0_SD1,
+ comms_buffer,
+ 6);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_WriteMulti(
+ Dev,
+ VL53LX_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
+ comms_buffer,
+ 6);
+
+ if (status == VL53LX_ERROR_NONE) {
+ pdev->customer.global_config__spad_enables_ref_0 =
+ comms_buffer[0];
+ pdev->customer.global_config__spad_enables_ref_1 =
+ comms_buffer[1];
+ pdev->customer.global_config__spad_enables_ref_2 =
+ comms_buffer[2];
+ pdev->customer.global_config__spad_enables_ref_3 =
+ comms_buffer[3];
+ pdev->customer.global_config__spad_enables_ref_4 =
+ comms_buffer[4];
+ pdev->customer.global_config__spad_enables_ref_5 =
+ comms_buffer[5];
+ }
+
+#ifdef VL53LX_LOG_ENABLE
+
+ if (status == VL53LX_ERROR_NONE)
+ VL53LX_print_customer_nvm_managed(
+ &(pdev->customer),
+ "run_ref_spad_char():pdev->lldata.customer.",
+ VL53LX_TRACE_MODULE_REF_SPAD_CHAR);
+#endif
+
+ if (status == VL53LX_ERROR_NONE) {
+
+ switch (pdev->sys_results.result__range_status) {
+
+ case VL53LX_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS:
+ status = VL53LX_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS;
+ break;
+
+ case VL53LX_DEVICEERROR_REFSPADCHARMORETHANTARGET:
+ status = VL53LX_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH;
+ break;
+
+ case VL53LX_DEVICEERROR_REFSPADCHARLESSTHANTARGET:
+ status = VL53LX_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW;
+ break;
+ }
+ }
+
+
+
+ *pcal_status = status;
+
+
+
+ IGNORE_STATUS(
+ IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
+ VL53LX_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
+ status);
+
+ IGNORE_STATUS(
+ IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH,
+ VL53LX_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH,
+ status);
+
+ IGNORE_STATUS(
+ IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW,
+ VL53LX_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW,
+ status);
+
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+VL53LX_Error VL53LX_get_and_avg_xtalk_samples(
+ VL53LX_DEV Dev,
+ uint8_t num_of_samples,
+ uint8_t measurement_mode,
+ int16_t xtalk_filter_thresh_max_mm,
+ int16_t xtalk_filter_thresh_min_mm,
+ uint16_t xtalk_max_valid_rate_kcps,
+ uint8_t xtalk_result_id,
+ uint8_t xtalk_histo_id,
+ VL53LX_xtalk_range_results_t *pXR,
+ VL53LX_histogram_bin_data_t *psum_histo,
+ VL53LX_histogram_bin_data_t *pavg_histo)
+{
+
+
+
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+ VL53LX_LLDriverData_t *pdev =
+ VL53LXDevStructGetLLDriverHandle(Dev);
+
+#ifdef VL53LX_LOG_ENABLE
+ VL53LX_LLDriverResults_t *pres =
+ VL53LXDevStructGetLLResultsHandle(Dev);
+#endif
+
+ VL53LX_range_results_t *prs =
+ (VL53LX_range_results_t *) pdev->wArea1;
+
+ VL53LX_range_data_t *prange_data;
+ VL53LX_xtalk_range_data_t *pxtalk_range_data;
+
+ uint8_t i = 0;
+ uint8_t j = 0;
+ uint8_t zone_id = 0;
+ uint8_t final_zone = pdev->zone_cfg.active_zones+1;
+ uint8_t valid_result;
+
+ uint8_t smudge_corr_en = 0;
+
+
+
+
+ smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
+
+ status = VL53LX_dynamic_xtalk_correction_disable(Dev);
+
+
+ VL53LX_load_patch(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_init_and_start_range(
+ Dev,
+ measurement_mode,
+ VL53LX_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);
+
+
+ for (i = 0; i <= (final_zone*num_of_samples); i++) {
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_wait_for_range_completion(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_get_device_results(
+ Dev,
+ VL53LX_DEVICERESULTSLEVEL_FULL,
+ prs);
+
+
+
+ if (status == VL53LX_ERROR_NONE &&
+ pdev->ll_state.rd_device_state !=
+ VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC) {
+
+ zone_id = pdev->ll_state.rd_zone_id + xtalk_result_id;
+ prange_data = &(prs->VL53LX_p_003[0]);
+
+
+ if (prs->active_results > 1) {
+ for (j = 1;
+ j < prs->active_results; j++) {
+ if (prs->VL53LX_p_003[j].median_range_mm
+ <
+ prange_data->median_range_mm)
+ prange_data =
+ &(prs->VL53LX_p_003[j]);
+
+ }
+ }
+
+ pxtalk_range_data = &(pXR->VL53LX_p_003[zone_id]);
+
+
+
+ if ((prs->active_results > 0) &&
+ (prange_data->median_range_mm <
+ xtalk_filter_thresh_max_mm) &&
+ (prange_data->median_range_mm >
+ xtalk_filter_thresh_min_mm) &&
+ (prange_data->VL53LX_p_009 <
+ (uint32_t)(xtalk_max_valid_rate_kcps * 16)))
+ valid_result = 1;
+ else
+ valid_result = 0;
+
+ if (valid_result == 1) {
+
+ pxtalk_range_data->no_of_samples++;
+
+ pxtalk_range_data->rate_per_spad_kcps_sum +=
+ prange_data->VL53LX_p_009;
+
+ pxtalk_range_data->signal_total_events_sum +=
+ prange_data->VL53LX_p_010;
+
+ pxtalk_range_data->sigma_mm_sum +=
+ (uint32_t)prange_data->VL53LX_p_002;
+
+
+
+ pxtalk_range_data->median_phase_sum +=
+ (uint32_t)prange_data->VL53LX_p_011;
+
+
+
+
+ }
+
+ if ((valid_result == 1) && (zone_id >= 4)) {
+ status = VL53LX_sum_histogram_data(
+ &(pdev->hist_data),
+ psum_histo);
+
+
+
+ if (prange_data->VL53LX_p_012 <
+ pXR->central_histogram__window_start)
+ pXR->central_histogram__window_start =
+ prange_data->VL53LX_p_012;
+
+
+ if (prange_data->VL53LX_p_013 >
+ pXR->central_histogram__window_end)
+ pXR->central_histogram__window_end =
+ prange_data->VL53LX_p_013;
+
+ }
+
+ }
+
+
+
+#ifdef VL53LX_LOG_ENABLE
+ if (status == VL53LX_ERROR_NONE) {
+ VL53LX_print_range_results(
+ &(pres->range_results),
+ "pres->range_results.",
+ VL53LX_TRACE_MODULE_CORE);
+ }
+#endif
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_wait_for_firmware_ready(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_clear_interrupt_and_enable_next_range(
+ Dev,
+ measurement_mode);
+
+
+ }
+
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_stop_range(Dev);
+
+ VL53LX_unload_patch(Dev);
+
+
+
+ for (i = 0; i < (pdev->zone_cfg.active_zones+1); i++) {
+
+ pxtalk_range_data = &(pXR->VL53LX_p_003[i+xtalk_result_id]);
+
+ if (pxtalk_range_data->no_of_samples > 0) {
+ pxtalk_range_data->rate_per_spad_kcps_avg =
+ pxtalk_range_data->rate_per_spad_kcps_sum /
+ (uint32_t)pxtalk_range_data->no_of_samples;
+
+ pxtalk_range_data->signal_total_events_avg =
+ pxtalk_range_data->signal_total_events_sum /
+ (int32_t)pxtalk_range_data->no_of_samples;
+
+ pxtalk_range_data->sigma_mm_avg =
+ pxtalk_range_data->sigma_mm_sum /
+ (uint32_t)pxtalk_range_data->no_of_samples;
+
+
+
+ pxtalk_range_data->median_phase_avg =
+ pxtalk_range_data->median_phase_sum /
+ (uint32_t)pxtalk_range_data->no_of_samples;
+
+
+
+ } else {
+ pxtalk_range_data->rate_per_spad_kcps_avg =
+ pxtalk_range_data->rate_per_spad_kcps_sum;
+ pxtalk_range_data->signal_total_events_avg =
+ pxtalk_range_data->signal_total_events_sum;
+ pxtalk_range_data->sigma_mm_avg =
+ pxtalk_range_data->sigma_mm_sum;
+
+
+
+ pxtalk_range_data->median_phase_avg =
+ pxtalk_range_data->median_phase_sum;
+
+
+ }
+ }
+
+
+
+ memcpy(pavg_histo, &(pdev->hist_data),
+ sizeof(VL53LX_histogram_bin_data_t));
+
+
+
+ if (status == VL53LX_ERROR_NONE) {
+
+ pxtalk_range_data = &(pXR->VL53LX_p_003[xtalk_histo_id]);
+
+ status = VL53LX_avg_histogram_data(
+ pxtalk_range_data->no_of_samples,
+ psum_histo,
+ pavg_histo);
+ }
+
+
+
+
+ if (status == VL53LX_ERROR_NONE) {
+ if (smudge_corr_en == 1)
+ status = VL53LX_dynamic_xtalk_correction_enable(Dev);
+ }
+
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+
+}
+
+
+VL53LX_Error VL53LX_run_phasecal_average(
+ VL53LX_DEV Dev,
+ uint8_t measurement_mode,
+ uint8_t phasecal_result__vcsel_start,
+ uint16_t phasecal_num_of_samples,
+ VL53LX_range_results_t *prange_results,
+ uint16_t *pphasecal_result__reference_phase,
+ uint16_t *pzero_distance_phase)
+{
+
+
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+ VL53LX_LLDriverData_t *pdev =
+ VL53LXDevStructGetLLDriverHandle(Dev);
+
+ uint16_t i = 0;
+ uint16_t m = 0;
+ uint32_t samples = 0;
+
+ uint32_t period = 0;
+ uint32_t VL53LX_p_014 = 0;
+ uint32_t phasecal_result__reference_phase = 0;
+ uint32_t zero_distance_phase = 0;
+
+
+ VL53LX_load_patch(Dev);
+
+ for (m = 0; m < phasecal_num_of_samples; m++) {
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_init_and_start_range(
+ Dev,
+ measurement_mode,
+ VL53LX_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);
+
+ for (i = 0; i <= 1; i++) {
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_wait_for_range_completion(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_get_device_results(
+ Dev,
+ VL53LX_DEVICERESULTSLEVEL_FULL,
+ prange_results);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_wait_for_firmware_ready(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_clear_interrupt_and_enable_next_range(
+ Dev,
+ measurement_mode);
+ }
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_stop_range(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_WaitUs(Dev, 1000);
+
+
+
+ if (status == VL53LX_ERROR_NONE) {
+
+ samples++;
+
+
+ period = 2048 *
+ (uint32_t)VL53LX_decode_vcsel_period(
+ pdev->hist_data.VL53LX_p_005);
+
+ VL53LX_p_014 = period;
+ VL53LX_p_014 += (uint32_t)(
+ pdev->hist_data.phasecal_result__reference_phase);
+ VL53LX_p_014 +=
+ (2048 *
+ (uint32_t)phasecal_result__vcsel_start);
+ VL53LX_p_014 -= (2048 *
+ (uint32_t)pdev->hist_data.cal_config__vcsel_start);
+
+ VL53LX_p_014 = VL53LX_p_014 % period;
+
+ phasecal_result__reference_phase += (uint32_t)(
+ pdev->hist_data.phasecal_result__reference_phase);
+
+ zero_distance_phase += (uint32_t)VL53LX_p_014;
+ }
+ }
+ VL53LX_unload_patch(Dev);
+
+
+
+ if (status == VL53LX_ERROR_NONE && samples > 0) {
+
+ phasecal_result__reference_phase += (samples >> 1);
+ phasecal_result__reference_phase /= samples;
+
+ zero_distance_phase += (samples >> 1);
+ zero_distance_phase /= samples;
+
+ *pphasecal_result__reference_phase =
+ (uint16_t)phasecal_result__reference_phase;
+ *pzero_distance_phase =
+ (uint16_t)zero_distance_phase;
+ }
+
+ return status;
+}
+
+
+VL53LX_Error VL53LX_run_device_test(
+ VL53LX_DEV Dev,
+ VL53LX_DeviceTestMode device_test_mode)
+{
+
+
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+ VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
+
+ uint8_t comms_buffer[2];
+ uint8_t gpio_hv_mux__ctrl = 0;
+
+ LOG_FUNCTION_START("");
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_RdByte(
+ Dev,
+ VL53LX_GPIO_HV_MUX__CTRL,
+ &gpio_hv_mux__ctrl);
+
+ if (status == VL53LX_ERROR_NONE)
+ pdev->stat_cfg.gpio_hv_mux__ctrl = gpio_hv_mux__ctrl;
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_start_test(
+ Dev,
+ device_test_mode);
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_wait_for_test_completion(Dev);
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_ReadMulti(
+ Dev,
+ VL53LX_RESULT__RANGE_STATUS,
+ comms_buffer,
+ 2);
+
+ if (status == VL53LX_ERROR_NONE) {
+ pdev->sys_results.result__range_status = comms_buffer[0];
+ pdev->sys_results.result__report_status = comms_buffer[1];
+ }
+
+
+
+ pdev->sys_results.result__range_status &=
+ VL53LX_RANGE_STATUS__RANGE_STATUS_MASK;
+
+ if (status == VL53LX_ERROR_NONE) {
+ trace_print(
+ VL53LX_TRACE_LEVEL_INFO,
+ " Device Test Complete:\n\t%-32s = %3u\n\t%-32s = %3u\n",
+ "result__range_status",
+ pdev->sys_results.result__range_status,
+ "result__report_status",
+ pdev->sys_results.result__report_status);
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_clear_interrupt(Dev);
+ }
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_start_test(
+ Dev,
+ 0x00);
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+void VL53LX_hist_xtalk_extract_data_init(
+ VL53LX_hist_xtalk_extract_data_t *pxtalk_data)
+{
+
+
+ int32_t lb = 0;
+
+ pxtalk_data->sample_count = 0U;
+ pxtalk_data->pll_period_mm = 0U;
+ pxtalk_data->peak_duration_us_sum = 0U;
+ pxtalk_data->effective_spad_count_sum = 0U;
+ pxtalk_data->zero_distance_phase_sum = 0U;
+ pxtalk_data->zero_distance_phase_avg = 0U;
+ pxtalk_data->event_scaler_sum = 0U;
+ pxtalk_data->event_scaler_avg = 4096U;
+ pxtalk_data->signal_events_sum = 0;
+ pxtalk_data->xtalk_rate_kcps_per_spad = 0U;
+ pxtalk_data->VL53LX_p_012 = 0U;
+ pxtalk_data->VL53LX_p_013 = 0U;
+ pxtalk_data->target_start = 0U;
+
+ for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS; lb++)
+ pxtalk_data->bin_data_sums[lb] = 0;
+
+}
+
+
+VL53LX_Error VL53LX_hist_xtalk_extract_update(
+ int16_t target_distance_mm,
+ uint16_t target_width_oversize,
+ VL53LX_histogram_bin_data_t *phist_bins,
+ VL53LX_hist_xtalk_extract_data_t *pxtalk_data)
+{
+
+
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+
+ LOG_FUNCTION_START("");
+
+ status =
+ VL53LX_hist_xtalk_extract_calc_window(
+ target_distance_mm,
+ target_width_oversize,
+ phist_bins,
+ pxtalk_data);
+
+ if (status == VL53LX_ERROR_NONE) {
+ status =
+ VL53LX_hist_xtalk_extract_calc_event_sums(
+ phist_bins,
+ pxtalk_data);
+ }
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+VL53LX_Error VL53LX_hist_xtalk_extract_fini(
+ VL53LX_histogram_bin_data_t *phist_bins,
+ VL53LX_hist_xtalk_extract_data_t *pxtalk_data,
+ VL53LX_xtalk_calibration_results_t *pxtalk_cal,
+ VL53LX_xtalk_histogram_shape_t *pxtalk_shape)
+{
+
+
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+ VL53LX_xtalk_calibration_results_t *pX = pxtalk_cal;
+
+ LOG_FUNCTION_START("");
+
+ if (pxtalk_data->sample_count > 0) {
+
+
+ pxtalk_data->event_scaler_avg = pxtalk_data->event_scaler_sum;
+ pxtalk_data->event_scaler_avg +=
+ (pxtalk_data->sample_count >> 1);
+ pxtalk_data->event_scaler_avg /= pxtalk_data->sample_count;
+
+
+
+ status =
+ VL53LX_hist_xtalk_extract_calc_rate_per_spad(
+ pxtalk_data);
+
+
+
+ if (status == VL53LX_ERROR_NONE) {
+
+
+ pxtalk_data->zero_distance_phase_avg =
+ pxtalk_data->zero_distance_phase_sum;
+ pxtalk_data->zero_distance_phase_avg +=
+ (pxtalk_data->sample_count >> 1);
+ pxtalk_data->zero_distance_phase_avg /=
+ pxtalk_data->sample_count;
+
+
+ status =
+ VL53LX_hist_xtalk_extract_calc_shape(
+ pxtalk_data,
+ pxtalk_shape);
+
+
+
+
+ pxtalk_shape->phasecal_result__vcsel_start =
+ phist_bins->phasecal_result__vcsel_start;
+ pxtalk_shape->cal_config__vcsel_start =
+ phist_bins->cal_config__vcsel_start;
+ pxtalk_shape->vcsel_width =
+ phist_bins->vcsel_width;
+ pxtalk_shape->VL53LX_p_015 =
+ phist_bins->VL53LX_p_015;
+ }
+
+
+ if (status == VL53LX_ERROR_NONE) {
+
+
+ pX->algo__crosstalk_compensation_plane_offset_kcps =
+ pxtalk_data->xtalk_rate_kcps_per_spad;
+ pX->algo__crosstalk_compensation_x_plane_gradient_kcps
+ = 0U;
+ pX->algo__crosstalk_compensation_y_plane_gradient_kcps
+ = 0U;
+
+ }
+ }
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+VL53LX_Error VL53LX_run_hist_xtalk_extraction(
+ VL53LX_DEV Dev,
+ int16_t cal_distance_mm,
+ VL53LX_Error *pcal_status)
+{
+
+
+ #define OVERSIZE 4
+ VL53LX_Error status = VL53LX_ERROR_NONE;
+ VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev);
+ VL53LX_xtalkextract_config_t *pX = &(pdev->xtalk_extract_cfg);
+ VL53LX_xtalk_config_t *pC = &(pdev->xtalk_cfg);
+ VL53LX_xtalk_calibration_results_t *pXC = &(pdev->xtalk_cal);
+
+
+
+ uint8_t smudge_corr_en = 0;
+ uint8_t i = 0;
+ int8_t k = 0;
+ uint8_t nbloops;
+ int32_t initMergeSize = 0;
+ int32_t MergeEnabled = 0;
+ uint32_t deltaXtalk;
+ uint32_t stepXtalk;
+ uint32_t XtalkMin;
+ uint32_t XtalkMax;
+ uint8_t measurement_mode = VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK;
+ int8_t MaxId;
+ uint8_t histo_merge_nb;
+ uint8_t wait_for_accumulation;
+ VL53LX_range_results_t *prange_results =
+ (VL53LX_range_results_t *) pdev->wArea1;
+ uint8_t Very1stRange = 0;
+
+ LOG_FUNCTION_START("");
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_set_preset_mode(
+ Dev,
+ VL53LX_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE,
+ pX->dss_config__target_total_rate_mcps,
+ pX->phasecal_config_timeout_us,
+ pX->mm_config_timeout_us,
+ pX->range_config_timeout_us,
+ 100);
+
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_disable_xtalk_compensation(Dev);
+
+
+
+ smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_dynamic_xtalk_correction_disable(Dev);
+
+
+ VL53LX_load_patch(Dev);
+
+ VL53LX_get_tuning_parm(Dev, VL53LX_TUNINGPARM_HIST_MERGE_MAX_SIZE,
+ &initMergeSize);
+ VL53LX_get_tuning_parm(Dev, VL53LX_TUNINGPARM_HIST_MERGE,
+ &MergeEnabled);
+ memset(&pdev->xtalk_cal, 0, sizeof(pdev->xtalk_cal));
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_init_and_start_range(
+ Dev, measurement_mode,
+ VL53LX_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);
+
+ MaxId = pdev->tuning_parms.tp_hist_merge_max_size - 1;
+ nbloops = (MergeEnabled == 0 ? 1 : 2);
+ for (k = 0; k < nbloops; k++) {
+
+ VL53LX_hist_xtalk_extract_data_init(
+ &(pdev->xtalk_extract));
+ VL53LX_set_tuning_parm(Dev,
+ VL53LX_TUNINGPARM_HIST_MERGE_MAX_SIZE,
+ k * MaxId + 1);
+
+ for (i = 0; i <= pX->num_of_samples; i++) {
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_wait_for_range_completion(Dev);
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_get_device_results(Dev,
+ VL53LX_DEVICERESULTSLEVEL_FULL,
+ prange_results);
+ Very1stRange =
+ (pdev->ll_state.rd_device_state ==
+ VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC);
+
+ VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb);
+ wait_for_accumulation = ((k != 0) &&
+ (MergeEnabled) &&
+ (status == VL53LX_ERROR_NONE) &&
+ (histo_merge_nb <
+ pdev->tuning_parms.tp_hist_merge_max_size));
+ if (wait_for_accumulation)
+ i = 0;
+ else {
+ if ((status == VL53LX_ERROR_NONE) &&
+ (!Very1stRange)) {
+ status =
+ VL53LX_hist_xtalk_extract_update(
+ cal_distance_mm,
+ OVERSIZE,
+ &(pdev->hist_data),
+ &(pdev->xtalk_extract));
+ }
+ }
+
+ if (status == VL53LX_ERROR_NONE)
+ status = VL53LX_wait_for_firmware_ready(Dev);
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_clear_interrupt_and_enable_next_range(
+ Dev, measurement_mode);
+
+
+ if (status == VL53LX_ERROR_NONE)
+ status =
+ VL53LX_hist_xtalk_extract_fini(
+ &(pdev->hist_data),
+ &(pdev->xtalk_extract),
+ &(pdev->xtalk_cal),
+ &(pdev->xtalk_shapes.xtalk_shape));
+ if (status != VL53LX_ERROR_NONE)
+ goto LOOPOUT;
+ pXC->algo__xtalk_cpo_HistoMerge_kcps[k * MaxId] =
+ pXC->algo__crosstalk_compensation_plane_offset_kcps;
+ }
+ }
+
+LOOPOUT:
+
+ VL53LX_stop_range(Dev);
+
+ VL53LX_set_tuning_parm(Dev, VL53LX_TUNINGPARM_HIST_MERGE_MAX_SIZE,
+ initMergeSize);
+ VL53LX_unload_patch(Dev);
+
+ if (status != VL53LX_ERROR_NONE)
+ status = VL53LX_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL;
+ else if ((MergeEnabled == 1) && (MaxId > 0)) {
+ XtalkMin = pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[0];
+ XtalkMax =
+ pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[MaxId];
+ pdev->xtalk_cal.
+ algo__crosstalk_compensation_plane_offset_kcps = XtalkMin;
+ if (XtalkMax > XtalkMin) {
+ deltaXtalk = XtalkMax - XtalkMin;
+ stepXtalk = deltaXtalk / MaxId;
+ for (k = 1; k < MaxId; k++)
+ pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[k] =
+ XtalkMin + stepXtalk * k;
+ } else
+ status =
+ VL53LX_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL;
+ }
+
+ if (status == VL53LX_ERROR_NONE) {
+ pC->algo__crosstalk_compensation_x_plane_gradient_kcps =
+ pXC->algo__crosstalk_compensation_x_plane_gradient_kcps;
+ pC->algo__crosstalk_compensation_y_plane_gradient_kcps =
+ pXC->algo__crosstalk_compensation_y_plane_gradient_kcps;
+ pC->algo__crosstalk_compensation_plane_offset_kcps =
+ pXC->algo__crosstalk_compensation_plane_offset_kcps;
+ }
+
+
+ pdev->xtalk_results.cal_status = status;
+ *pcal_status = pdev->xtalk_results.cal_status;
+
+
+ status = VL53LX_enable_xtalk_compensation(Dev);
+ if (smudge_corr_en == 1)
+ status = VL53LX_dynamic_xtalk_correction_enable(Dev);
+
+#ifdef VL53LX_LOG_ENABLE
+
+
+
+ VL53LX_print_customer_nvm_managed(
+ &(pdev->customer),
+ "run_xtalk_extraction():pdev->lldata.customer.",
+ VL53LX_TRACE_MODULE_XTALK_DATA);
+
+ VL53LX_print_xtalk_config(
+ &(pdev->xtalk_cfg),
+ "run_xtalk_extraction():pdev->lldata.xtalk_cfg.",
+ VL53LX_TRACE_MODULE_XTALK_DATA);
+
+ VL53LX_print_xtalk_histogram_data(
+ &(pdev->xtalk_shapes),
+ "pdev->lldata.xtalk_shapes.",
+ VL53LX_TRACE_MODULE_XTALK_DATA);
+
+#endif
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+