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_core.c
- Revision:
- 5:89031b2f5316
diff -r 39a9cd3c85f2 -r 89031b2f5316 modules/vl53lx_core.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/vl53lx_core.c Wed Jul 14 12:45:49 2021 +0100 @@ -0,0 +1,4818 @@ + +// 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_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_structs.h" +#include "vl53lx_api_preset_modes.h" +#include "vl53lx_core.h" +#include "vl53lx_tuning_parm_defaults.h" + + + +#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__) + + +void VL53LX_init_version( + VL53LX_DEV Dev) +{ + + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + pdev->version.ll_major = VL53LX_LL_API_IMPLEMENTATION_VER_MAJOR; + pdev->version.ll_minor = VL53LX_LL_API_IMPLEMENTATION_VER_MINOR; + pdev->version.ll_build = VL53LX_LL_API_IMPLEMENTATION_VER_SUB; + pdev->version.ll_revision = VL53LX_LL_API_IMPLEMENTATION_VER_REVISION; +} + + +void VL53LX_init_ll_driver_state( + VL53LX_DEV Dev, + VL53LX_DeviceState device_state) +{ + + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state); + + pstate->cfg_device_state = device_state; + pstate->cfg_stream_count = 0; + pstate->cfg_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + pstate->cfg_timing_status = 0; + pstate->cfg_zone_id = 0; + + pstate->rd_device_state = device_state; + pstate->rd_stream_count = 0; + pstate->rd_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + pstate->rd_timing_status = 0; + pstate->rd_zone_id = 0; + +} + + +VL53LX_Error VL53LX_update_ll_driver_rd_state( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state); + + + + LOG_FUNCTION_START(""); + + + + if ((pdev->sys_ctrl.system__mode_start & + VL53LX_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) { + + pstate->rd_device_state = VL53LX_DEVICESTATE_SW_STANDBY; + pstate->rd_stream_count = 0; + pstate->rd_internal_stream_count = 0; + pstate->rd_internal_stream_count_val = 0; + pstate->rd_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + pstate->rd_timing_status = 0; + pstate->rd_zone_id = 0; + + } else { + + + + if (pstate->rd_stream_count == 0xFF) + pstate->rd_stream_count = 0x80; + else + pstate->rd_stream_count++; + + + status = VL53LX_update_internal_stream_counters(Dev, + pstate->rd_stream_count, + &(pstate->rd_internal_stream_count), + &(pstate->rd_internal_stream_count_val)); + + + + pstate->rd_gph_id ^= VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + + + + switch (pstate->rd_device_state) { + + case VL53LX_DEVICESTATE_SW_STANDBY: + + if ((pdev->dyn_cfg.system__grouped_parameter_hold & + VL53LX_GROUPEDPARAMETERHOLD_ID_MASK) > 0) { + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC; + } else { + if (pstate->rd_zone_id >= + pdev->zone_cfg.active_zones) + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA; + else + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_GATHER_DATA; + } + + pstate->rd_stream_count = 0; + pstate->rd_internal_stream_count = 0; + pstate->rd_internal_stream_count_val = 0; + pstate->rd_timing_status = 0; + pstate->rd_zone_id = 0; + + break; + + case VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC: + pstate->rd_stream_count = 0; + pstate->rd_internal_stream_count = 0; + pstate->rd_internal_stream_count_val = 0; + pstate->rd_zone_id = 0; + if (pstate->rd_zone_id >= + pdev->zone_cfg.active_zones) + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA; + else + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_GATHER_DATA; + + break; + + case VL53LX_DEVICESTATE_RANGING_GATHER_DATA: + pstate->rd_zone_id++; + if (pstate->rd_zone_id >= + pdev->zone_cfg.active_zones) + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA; + else + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_GATHER_DATA; + + break; + + case VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA: + pstate->rd_zone_id = 0; + pstate->rd_timing_status ^= 0x01; + + if (pstate->rd_zone_id >= + pdev->zone_cfg.active_zones) + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA; + else + pstate->rd_device_state = + VL53LX_DEVICESTATE_RANGING_GATHER_DATA; + break; + + default: + pstate->rd_device_state = + VL53LX_DEVICESTATE_SW_STANDBY; + pstate->rd_stream_count = 0; + pstate->rd_internal_stream_count = 0; + pstate->rd_internal_stream_count_val = 0; + pstate->rd_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + pstate->rd_timing_status = 0; + pstate->rd_zone_id = 0; + break; + } + } + + + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_check_ll_driver_rd_state( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = + VL53LXDevStructGetLLResultsHandle(Dev); + + VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state); + VL53LX_system_results_t *psys_results = &(pdev->sys_results); + VL53LX_histogram_bin_data_t *phist_data = &(pdev->hist_data); + VL53LX_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs); + + uint8_t device_range_status = 0; + uint8_t device_stream_count = 0; + uint8_t device_gph_id = 0; + uint8_t histogram_mode = 0; + uint8_t expected_stream_count = 0; + uint8_t expected_gph_id = 0; + + LOG_FUNCTION_START(""); + + + + device_range_status = + psys_results->result__range_status & + VL53LX_RANGE_STATUS__RANGE_STATUS_MASK; + + device_stream_count = psys_results->result__stream_count; + + + + histogram_mode = + (pdev->sys_ctrl.system__mode_start & + VL53LX_DEVICESCHEDULERMODE_HISTOGRAM) == + VL53LX_DEVICESCHEDULERMODE_HISTOGRAM; + + + device_gph_id = (psys_results->result__interrupt_status & + VL53LX_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4; + + if (histogram_mode) + device_gph_id = (phist_data->result__interrupt_status & + VL53LX_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4; + + + + if (!((pdev->sys_ctrl.system__mode_start & + VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK) == + VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK)) + goto ENDFUNC; + + + + if (pstate->rd_device_state == + VL53LX_DEVICESTATE_RANGING_WAIT_GPH_SYNC) { + + if (histogram_mode == 0) { + if (device_range_status != + VL53LX_DEVICEERROR_GPHSTREAMCOUNT0READY) + status = + VL53LX_ERROR_GPH_SYNC_CHECK_FAIL; + + } + } else { + if (pstate->rd_stream_count != device_stream_count) + status = VL53LX_ERROR_STREAM_COUNT_CHECK_FAIL; + + + if (pstate->rd_gph_id != device_gph_id) + status = VL53LX_ERROR_GPH_ID_CHECK_FAIL; + + + + + expected_stream_count = + pZ->VL53LX_p_003[pstate->rd_zone_id].expected_stream_count; + expected_gph_id = + pZ->VL53LX_p_003[pstate->rd_zone_id].expected_gph_id; + + + + if (expected_stream_count != device_stream_count) { + + + if (!((pdev->zone_cfg.active_zones == 0) && + (device_stream_count == 255))) + status = + VL53LX_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL; + + + } + + + + if (expected_gph_id != device_gph_id) + status = VL53LX_ERROR_ZONE_GPH_ID_CHECK_FAIL; + + } + + + +ENDFUNC: + LOG_FUNCTION_END(status); + return status; +} + + +VL53LX_Error VL53LX_update_ll_driver_cfg_state( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = + VL53LXDevStructGetLLResultsHandle(Dev); + + VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state); + VL53LX_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs); + + uint8_t prev_cfg_zone_id; + uint8_t prev_cfg_gph_id; + uint8_t prev_cfg_stream_count; + + LOG_FUNCTION_START(""); + + + + + + if ((pdev->sys_ctrl.system__mode_start & + VL53LX_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) { + + pstate->cfg_device_state = VL53LX_DEVICESTATE_SW_STANDBY; + pstate->cfg_stream_count = 0; + pstate->cfg_internal_stream_count = 0; + pstate->cfg_internal_stream_count_val = 0; + pstate->cfg_gph_id = VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + pstate->cfg_timing_status = 0; + pstate->cfg_zone_id = 0; + prev_cfg_zone_id = 0; + prev_cfg_gph_id = 0; + prev_cfg_stream_count = 0; + + } else { + + prev_cfg_gph_id = pstate->cfg_gph_id; + prev_cfg_zone_id = pstate->cfg_zone_id; + prev_cfg_stream_count = pstate->cfg_stream_count; + + + + if (pstate->cfg_stream_count == 0xFF) + pstate->cfg_stream_count = 0x80; + else + pstate->cfg_stream_count++; + + + status = VL53LX_update_internal_stream_counters( + Dev, + pstate->cfg_stream_count, + &(pstate->cfg_internal_stream_count), + &(pstate->cfg_internal_stream_count_val)); + + + + pstate->cfg_gph_id ^= VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + + + + switch (pstate->cfg_device_state) { + + case VL53LX_DEVICESTATE_SW_STANDBY: + pstate->cfg_zone_id = 1; + if (pstate->cfg_zone_id > + pdev->zone_cfg.active_zones) { + pstate->cfg_zone_id = 0; + pstate->cfg_timing_status ^= 0x01; + } + pstate->cfg_stream_count = 1; + + if (pdev->gen_cfg.global_config__stream_divider == 0) { + pstate->cfg_internal_stream_count = 1; + pstate->cfg_internal_stream_count_val = 0; + } else { + pstate->cfg_internal_stream_count = 0; + pstate->cfg_internal_stream_count_val = 1; + } + pstate->cfg_device_state = + VL53LX_DEVICESTATE_RANGING_DSS_AUTO; + break; + + case VL53LX_DEVICESTATE_RANGING_DSS_AUTO: + pstate->cfg_zone_id++; + if (pstate->cfg_zone_id > + pdev->zone_cfg.active_zones) { + + pstate->cfg_zone_id = 0; + pstate->cfg_timing_status ^= 0x01; + + + + + if (pdev->zone_cfg.active_zones > 0) { + pstate->cfg_device_state = + VL53LX_DEVICESTATE_RANGING_DSS_MANUAL; + } + } + break; + + case VL53LX_DEVICESTATE_RANGING_DSS_MANUAL: + pstate->cfg_zone_id++; + if (pstate->cfg_zone_id > + pdev->zone_cfg.active_zones) { + pstate->cfg_zone_id = 0; + pstate->cfg_timing_status ^= 0x01; + } + break; + + default: + pstate->cfg_device_state = + VL53LX_DEVICESTATE_SW_STANDBY; + pstate->cfg_stream_count = 0; + pstate->cfg_internal_stream_count = 0; + pstate->cfg_internal_stream_count_val = 0; + pstate->cfg_gph_id = + VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + pstate->cfg_timing_status = 0; + pstate->cfg_zone_id = 0; + break; + } + } + + + if (pdev->zone_cfg.active_zones == 0) { + + pZ->VL53LX_p_003[prev_cfg_zone_id].expected_stream_count + = prev_cfg_stream_count - 1; + + pZ->VL53LX_p_003[pstate->rd_zone_id].expected_gph_id = + prev_cfg_gph_id ^ VL53LX_GROUPEDPARAMETERHOLD_ID_MASK; + } else { + pZ->VL53LX_p_003[prev_cfg_zone_id].expected_stream_count + = prev_cfg_stream_count; + pZ->VL53LX_p_003[prev_cfg_zone_id].expected_gph_id = + prev_cfg_gph_id; + } + + + + LOG_FUNCTION_END(status); + + return status; +} + + +void VL53LX_copy_rtn_good_spads_to_buffer( + VL53LX_nvm_copy_data_t *pdata, + uint8_t *pbuffer) +{ + + + *(pbuffer + 0) = pdata->global_config__spad_enables_rtn_0; + *(pbuffer + 1) = pdata->global_config__spad_enables_rtn_1; + *(pbuffer + 2) = pdata->global_config__spad_enables_rtn_2; + *(pbuffer + 3) = pdata->global_config__spad_enables_rtn_3; + *(pbuffer + 4) = pdata->global_config__spad_enables_rtn_4; + *(pbuffer + 5) = pdata->global_config__spad_enables_rtn_5; + *(pbuffer + 6) = pdata->global_config__spad_enables_rtn_6; + *(pbuffer + 7) = pdata->global_config__spad_enables_rtn_7; + *(pbuffer + 8) = pdata->global_config__spad_enables_rtn_8; + *(pbuffer + 9) = pdata->global_config__spad_enables_rtn_9; + *(pbuffer + 10) = pdata->global_config__spad_enables_rtn_10; + *(pbuffer + 11) = pdata->global_config__spad_enables_rtn_11; + *(pbuffer + 12) = pdata->global_config__spad_enables_rtn_12; + *(pbuffer + 13) = pdata->global_config__spad_enables_rtn_13; + *(pbuffer + 14) = pdata->global_config__spad_enables_rtn_14; + *(pbuffer + 15) = pdata->global_config__spad_enables_rtn_15; + *(pbuffer + 16) = pdata->global_config__spad_enables_rtn_16; + *(pbuffer + 17) = pdata->global_config__spad_enables_rtn_17; + *(pbuffer + 18) = pdata->global_config__spad_enables_rtn_18; + *(pbuffer + 19) = pdata->global_config__spad_enables_rtn_19; + *(pbuffer + 20) = pdata->global_config__spad_enables_rtn_20; + *(pbuffer + 21) = pdata->global_config__spad_enables_rtn_21; + *(pbuffer + 22) = pdata->global_config__spad_enables_rtn_22; + *(pbuffer + 23) = pdata->global_config__spad_enables_rtn_23; + *(pbuffer + 24) = pdata->global_config__spad_enables_rtn_24; + *(pbuffer + 25) = pdata->global_config__spad_enables_rtn_25; + *(pbuffer + 26) = pdata->global_config__spad_enables_rtn_26; + *(pbuffer + 27) = pdata->global_config__spad_enables_rtn_27; + *(pbuffer + 28) = pdata->global_config__spad_enables_rtn_28; + *(pbuffer + 29) = pdata->global_config__spad_enables_rtn_29; + *(pbuffer + 30) = pdata->global_config__spad_enables_rtn_30; + *(pbuffer + 31) = pdata->global_config__spad_enables_rtn_31; +} + + +void VL53LX_init_system_results( + VL53LX_system_results_t *pdata) +{ + + + pdata->result__interrupt_status = 0xFF; + pdata->result__range_status = 0xFF; + pdata->result__report_status = 0xFF; + pdata->result__stream_count = 0xFF; + + pdata->result__dss_actual_effective_spads_sd0 = 0xFFFF; + pdata->result__peak_signal_count_rate_mcps_sd0 = 0xFFFF; + pdata->result__ambient_count_rate_mcps_sd0 = 0xFFFF; + pdata->result__sigma_sd0 = 0xFFFF; + pdata->result__phase_sd0 = 0xFFFF; + pdata->result__final_crosstalk_corrected_range_mm_sd0 = 0xFFFF; + pdata->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = + 0xFFFF; + pdata->result__mm_inner_actual_effective_spads_sd0 = 0xFFFF; + pdata->result__mm_outer_actual_effective_spads_sd0 = 0xFFFF; + pdata->result__avg_signal_count_rate_mcps_sd0 = 0xFFFF; + + pdata->result__dss_actual_effective_spads_sd1 = 0xFFFF; + pdata->result__peak_signal_count_rate_mcps_sd1 = 0xFFFF; + pdata->result__ambient_count_rate_mcps_sd1 = 0xFFFF; + pdata->result__sigma_sd1 = 0xFFFF; + pdata->result__phase_sd1 = 0xFFFF; + pdata->result__final_crosstalk_corrected_range_mm_sd1 = 0xFFFF; + pdata->result__spare_0_sd1 = 0xFFFF; + pdata->result__spare_1_sd1 = 0xFFFF; + pdata->result__spare_2_sd1 = 0xFFFF; + pdata->result__spare_3_sd1 = 0xFF; + +} + + +void V53L1_init_zone_results_structure( + uint8_t active_zones, + VL53LX_zone_results_t *pdata) +{ + + + + uint8_t z = 0; + VL53LX_zone_objects_t *pobjects; + + pdata->max_zones = VL53LX_MAX_USER_ZONES; + pdata->active_zones = active_zones; + + for (z = 0; z < pdata->max_zones; z++) { + pobjects = &(pdata->VL53LX_p_003[z]); + pobjects->cfg_device_state = VL53LX_DEVICESTATE_SW_STANDBY; + pobjects->rd_device_state = VL53LX_DEVICESTATE_SW_STANDBY; + pobjects->max_objects = VL53LX_MAX_RANGE_RESULTS; + pobjects->active_objects = 0; + } +} + +void V53L1_init_zone_dss_configs( + VL53LX_DEV Dev) +{ + + + + VL53LX_LLDriverResults_t *pres = + VL53LXDevStructGetLLResultsHandle(Dev); + uint8_t z = 0; + uint8_t max_zones = VL53LX_MAX_USER_ZONES; + VL53LX_zone_private_dyn_cfgs_t *pdata = &(pres->zone_dyn_cfgs); + + for (z = 0; z < max_zones; z++) { + pdata->VL53LX_p_003[z].dss_mode = + VL53LX_DSS_CONTROL__MODE_TARGET_RATE; + pdata->VL53LX_p_003[z].dss_requested_effective_spad_count = 0; + } +} + + +void VL53LX_init_histogram_config_structure( + uint8_t even_bin0, + uint8_t even_bin1, + uint8_t even_bin2, + uint8_t even_bin3, + uint8_t even_bin4, + uint8_t even_bin5, + uint8_t odd_bin0, + uint8_t odd_bin1, + uint8_t odd_bin2, + uint8_t odd_bin3, + uint8_t odd_bin4, + uint8_t odd_bin5, + VL53LX_histogram_config_t *pdata) +{ + + + pdata->histogram_config__low_amb_even_bin_0_1 = + (even_bin1 << 4) + even_bin0; + pdata->histogram_config__low_amb_even_bin_2_3 = + (even_bin3 << 4) + even_bin2; + pdata->histogram_config__low_amb_even_bin_4_5 = + (even_bin5 << 4) + even_bin4; + + pdata->histogram_config__low_amb_odd_bin_0_1 = + (odd_bin1 << 4) + odd_bin0; + pdata->histogram_config__low_amb_odd_bin_2_3 = + (odd_bin3 << 4) + odd_bin2; + pdata->histogram_config__low_amb_odd_bin_4_5 = + (odd_bin5 << 4) + odd_bin4; + + pdata->histogram_config__mid_amb_even_bin_0_1 = + pdata->histogram_config__low_amb_even_bin_0_1; + pdata->histogram_config__mid_amb_even_bin_2_3 = + pdata->histogram_config__low_amb_even_bin_2_3; + pdata->histogram_config__mid_amb_even_bin_4_5 = + pdata->histogram_config__low_amb_even_bin_4_5; + + pdata->histogram_config__mid_amb_odd_bin_0_1 = + pdata->histogram_config__low_amb_odd_bin_0_1; + pdata->histogram_config__mid_amb_odd_bin_2 = odd_bin2; + pdata->histogram_config__mid_amb_odd_bin_3_4 = + (odd_bin4 << 4) + odd_bin3; + pdata->histogram_config__mid_amb_odd_bin_5 = odd_bin5; + + pdata->histogram_config__user_bin_offset = 0x00; + + pdata->histogram_config__high_amb_even_bin_0_1 = + pdata->histogram_config__low_amb_even_bin_0_1; + pdata->histogram_config__high_amb_even_bin_2_3 = + pdata->histogram_config__low_amb_even_bin_2_3; + pdata->histogram_config__high_amb_even_bin_4_5 = + pdata->histogram_config__low_amb_even_bin_4_5; + + pdata->histogram_config__high_amb_odd_bin_0_1 = + pdata->histogram_config__low_amb_odd_bin_0_1; + pdata->histogram_config__high_amb_odd_bin_2_3 = + pdata->histogram_config__low_amb_odd_bin_2_3; + pdata->histogram_config__high_amb_odd_bin_4_5 = + pdata->histogram_config__low_amb_odd_bin_4_5; + + + + pdata->histogram_config__amb_thresh_low = 0xFFFF; + pdata->histogram_config__amb_thresh_high = 0xFFFF; + + + + pdata->histogram_config__spad_array_selection = 0x00; + +} + +void VL53LX_init_histogram_multizone_config_structure( + uint8_t even_bin0, + uint8_t even_bin1, + uint8_t even_bin2, + uint8_t even_bin3, + uint8_t even_bin4, + uint8_t even_bin5, + uint8_t odd_bin0, + uint8_t odd_bin1, + uint8_t odd_bin2, + uint8_t odd_bin3, + uint8_t odd_bin4, + uint8_t odd_bin5, + VL53LX_histogram_config_t *pdata) +{ + + + pdata->histogram_config__low_amb_even_bin_0_1 = + (even_bin1 << 4) + even_bin0; + pdata->histogram_config__low_amb_even_bin_2_3 = + (even_bin3 << 4) + even_bin2; + pdata->histogram_config__low_amb_even_bin_4_5 = + (even_bin5 << 4) + even_bin4; + + pdata->histogram_config__low_amb_odd_bin_0_1 = + pdata->histogram_config__low_amb_even_bin_0_1; + pdata->histogram_config__low_amb_odd_bin_2_3 + = pdata->histogram_config__low_amb_even_bin_2_3; + pdata->histogram_config__low_amb_odd_bin_4_5 + = pdata->histogram_config__low_amb_even_bin_4_5; + + pdata->histogram_config__mid_amb_even_bin_0_1 = + pdata->histogram_config__low_amb_even_bin_0_1; + pdata->histogram_config__mid_amb_even_bin_2_3 + = pdata->histogram_config__low_amb_even_bin_2_3; + pdata->histogram_config__mid_amb_even_bin_4_5 + = pdata->histogram_config__low_amb_even_bin_4_5; + + pdata->histogram_config__mid_amb_odd_bin_0_1 + = pdata->histogram_config__low_amb_odd_bin_0_1; + pdata->histogram_config__mid_amb_odd_bin_2 = odd_bin2; + pdata->histogram_config__mid_amb_odd_bin_3_4 = + (odd_bin4 << 4) + odd_bin3; + pdata->histogram_config__mid_amb_odd_bin_5 = odd_bin5; + + pdata->histogram_config__user_bin_offset = 0x00; + + pdata->histogram_config__high_amb_even_bin_0_1 = + (odd_bin1 << 4) + odd_bin0; + pdata->histogram_config__high_amb_even_bin_2_3 = + (odd_bin3 << 4) + odd_bin2; + pdata->histogram_config__high_amb_even_bin_4_5 = + (odd_bin5 << 4) + odd_bin4; + + pdata->histogram_config__high_amb_odd_bin_0_1 + = pdata->histogram_config__high_amb_even_bin_0_1; + pdata->histogram_config__high_amb_odd_bin_2_3 + = pdata->histogram_config__high_amb_even_bin_2_3; + pdata->histogram_config__high_amb_odd_bin_4_5 + = pdata->histogram_config__high_amb_even_bin_4_5; + + + + pdata->histogram_config__amb_thresh_low = 0xFFFF; + pdata->histogram_config__amb_thresh_high = 0xFFFF; + + + + pdata->histogram_config__spad_array_selection = 0x00; +} + + +void VL53LX_init_xtalk_bin_data_struct( + uint32_t bin_value, + uint16_t VL53LX_p_021, + VL53LX_xtalk_histogram_shape_t *pdata) +{ + + + + uint16_t i = 0; + + pdata->zone_id = 0; + pdata->time_stamp = 0; + + pdata->VL53LX_p_019 = 0; + pdata->VL53LX_p_020 = VL53LX_XTALK_HISTO_BINS; + pdata->VL53LX_p_021 = (uint8_t)VL53LX_p_021; + + pdata->phasecal_result__reference_phase = 0; + pdata->phasecal_result__vcsel_start = 0; + pdata->cal_config__vcsel_start = 0; + + pdata->vcsel_width = 0; + pdata->VL53LX_p_015 = 0; + + pdata->zero_distance_phase = 0; + + for (i = 0; i < VL53LX_XTALK_HISTO_BINS; i++) { + if (i < VL53LX_p_021) + pdata->bin_data[i] = bin_value; + else + pdata->bin_data[i] = 0; + } +} + + +void VL53LX_i2c_encode_uint16_t( + uint16_t ip_value, + uint16_t count, + uint8_t *pbuffer) +{ + + + uint16_t i = 0; + uint16_t VL53LX_p_003 = 0; + + VL53LX_p_003 = ip_value; + + for (i = 0; i < count; i++) { + pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF); + VL53LX_p_003 = VL53LX_p_003 >> 8; + } +} + +uint16_t VL53LX_i2c_decode_uint16_t( + uint16_t count, + uint8_t *pbuffer) +{ + + + uint16_t value = 0x00; + + while (count-- > 0) + value = (value << 8) | (uint16_t)*pbuffer++; + + return value; +} + + +void VL53LX_i2c_encode_int16_t( + int16_t ip_value, + uint16_t count, + uint8_t *pbuffer) +{ + + + uint16_t i = 0; + int16_t VL53LX_p_003 = 0; + + VL53LX_p_003 = ip_value; + + for (i = 0; i < count; i++) { + pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF); + VL53LX_p_003 = VL53LX_p_003 >> 8; + } +} + +int16_t VL53LX_i2c_decode_int16_t( + uint16_t count, + uint8_t *pbuffer) +{ + + + int16_t value = 0x00; + + + if (*pbuffer >= 0x80) + value = 0xFFFF; + + while (count-- > 0) + value = (value << 8) | (int16_t)*pbuffer++; + + return value; +} + +void VL53LX_i2c_encode_uint32_t( + uint32_t ip_value, + uint16_t count, + uint8_t *pbuffer) +{ + + + uint16_t i = 0; + uint32_t VL53LX_p_003 = 0; + + VL53LX_p_003 = ip_value; + + for (i = 0; i < count; i++) { + pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF); + VL53LX_p_003 = VL53LX_p_003 >> 8; + } +} + +uint32_t VL53LX_i2c_decode_uint32_t( + uint16_t count, + uint8_t *pbuffer) +{ + + + uint32_t value = 0x00; + + while (count-- > 0) + value = (value << 8) | (uint32_t)*pbuffer++; + + return value; +} + + +uint32_t VL53LX_i2c_decode_with_mask( + uint16_t count, + uint8_t *pbuffer, + uint32_t bit_mask, + uint32_t down_shift, + uint32_t offset) +{ + + + uint32_t value = 0x00; + + + while (count-- > 0) + value = (value << 8) | (uint32_t)*pbuffer++; + + + value = value & bit_mask; + if (down_shift > 0) + value = value >> down_shift; + + + value = value + offset; + + return value; +} + + +void VL53LX_i2c_encode_int32_t( + int32_t ip_value, + uint16_t count, + uint8_t *pbuffer) +{ + + + uint16_t i = 0; + int32_t VL53LX_p_003 = 0; + + VL53LX_p_003 = ip_value; + + for (i = 0; i < count; i++) { + pbuffer[count-i-1] = (uint8_t)(VL53LX_p_003 & 0x00FF); + VL53LX_p_003 = VL53LX_p_003 >> 8; + } +} + +int32_t VL53LX_i2c_decode_int32_t( + uint16_t count, + uint8_t *pbuffer) +{ + + + int32_t value = 0x00; + + + if (*pbuffer >= 0x80) + value = 0xFFFFFFFF; + + while (count-- > 0) + value = (value << 8) | (int32_t)*pbuffer++; + + return value; +} + + +VL53LX_Error VL53LX_start_test( + VL53LX_DEV Dev, + uint8_t test_mode__ctrl) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + if (status == VL53LX_ERROR_NONE) { + status = VL53LX_WrByte( + Dev, + VL53LX_TEST_MODE__CTRL, + test_mode__ctrl); + } + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_set_firmware_enable_register( + VL53LX_DEV Dev, + uint8_t value) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + pdev->sys_ctrl.firmware__enable = value; + + status = VL53LX_WrByte( + Dev, + VL53LX_FIRMWARE__ENABLE, + pdev->sys_ctrl.firmware__enable); + + return status; +} + +VL53LX_Error VL53LX_enable_firmware( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + status = VL53LX_set_firmware_enable_register(Dev, 0x01); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_disable_firmware( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + status = VL53LX_set_firmware_enable_register(Dev, 0x00); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_set_powerforce_register( + VL53LX_DEV Dev, + uint8_t value) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + pdev->sys_ctrl.power_management__go1_power_force = value; + + status = VL53LX_WrByte( + Dev, + VL53LX_POWER_MANAGEMENT__GO1_POWER_FORCE, + pdev->sys_ctrl.power_management__go1_power_force); + + return status; +} + + +VL53LX_Error VL53LX_enable_powerforce( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + status = VL53LX_set_powerforce_register(Dev, 0x01); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_disable_powerforce( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + status = VL53LX_set_powerforce_register(Dev, 0x00); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_clear_interrupt( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + pdev->sys_ctrl.system__interrupt_clear = VL53LX_CLEAR_RANGE_INT; + + status = VL53LX_WrByte( + Dev, + VL53LX_SYSTEM__INTERRUPT_CLEAR, + pdev->sys_ctrl.system__interrupt_clear); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_force_shadow_stream_count_to_zero( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + if (status == VL53LX_ERROR_NONE) + status = VL53LX_disable_firmware(Dev); + + if (status == VL53LX_ERROR_NONE) { + status = VL53LX_WrByte( + Dev, + VL53LX_SHADOW_RESULT__STREAM_COUNT, + 0x00); + } + + if (status == VL53LX_ERROR_NONE) + status = VL53LX_enable_firmware(Dev); + + return status; +} + + +uint32_t VL53LX_calc_macro_period_us( + uint16_t fast_osc_frequency, + uint8_t VL53LX_p_005) +{ + + + uint32_t pll_period_us = 0; + uint8_t VL53LX_p_030 = 0; + uint32_t macro_period_us = 0; + + LOG_FUNCTION_START(""); + + + + pll_period_us = VL53LX_calc_pll_period_us(fast_osc_frequency); + + + + VL53LX_p_030 = VL53LX_decode_vcsel_period(VL53LX_p_005); + + + + macro_period_us = + (uint32_t)VL53LX_MACRO_PERIOD_VCSEL_PERIODS * + pll_period_us; + macro_period_us = macro_period_us >> 6; + + macro_period_us = macro_period_us * (uint32_t)VL53LX_p_030; + macro_period_us = macro_period_us >> 6; + + + + LOG_FUNCTION_END(0); + + return macro_period_us; +} + + +uint16_t VL53LX_calc_range_ignore_threshold( + uint32_t central_rate, + int16_t x_gradient, + int16_t y_gradient, + uint8_t rate_mult) +{ + + + int32_t range_ignore_thresh_int = 0; + uint16_t range_ignore_thresh_kcps = 0; + int32_t central_rate_int = 0; + int16_t x_gradient_int = 0; + int16_t y_gradient_int = 0; + + LOG_FUNCTION_START(""); + + + + central_rate_int = ((int32_t)central_rate * (1 << 4)) / (1000); + + if (x_gradient < 0) + x_gradient_int = x_gradient * -1; + + if (y_gradient < 0) + y_gradient_int = y_gradient * -1; + + + + + + range_ignore_thresh_int = (8 * x_gradient_int * 4) + + (8 * y_gradient_int * 4); + + + + range_ignore_thresh_int = range_ignore_thresh_int / 1000; + + + + range_ignore_thresh_int = range_ignore_thresh_int + central_rate_int; + + + + range_ignore_thresh_int = (int32_t)rate_mult * range_ignore_thresh_int; + + range_ignore_thresh_int = (range_ignore_thresh_int + (1<<4)) / (1<<5); + + + + if (range_ignore_thresh_int > 0xFFFF) + range_ignore_thresh_kcps = 0xFFFF; + else + range_ignore_thresh_kcps = (uint16_t)range_ignore_thresh_int; + + + + LOG_FUNCTION_END(0); + + return range_ignore_thresh_kcps; +} + + +uint32_t VL53LX_calc_timeout_mclks( + uint32_t timeout_us, + uint32_t macro_period_us) +{ + + + uint32_t timeout_mclks = 0; + + LOG_FUNCTION_START(""); + + timeout_mclks = + ((timeout_us << 12) + (macro_period_us>>1)) / + macro_period_us; + + LOG_FUNCTION_END(0); + + return timeout_mclks; +} + + +uint16_t VL53LX_calc_encoded_timeout( + uint32_t timeout_us, + uint32_t macro_period_us) +{ + + + uint32_t timeout_mclks = 0; + uint16_t timeout_encoded = 0; + + LOG_FUNCTION_START(""); + + timeout_mclks = + VL53LX_calc_timeout_mclks(timeout_us, macro_period_us); + + timeout_encoded = + VL53LX_encode_timeout(timeout_mclks); + + + + LOG_FUNCTION_END(0); + + return timeout_encoded; +} + + +uint32_t VL53LX_calc_timeout_us( + uint32_t timeout_mclks, + uint32_t macro_period_us) +{ + + + uint32_t timeout_us = 0; + uint64_t tmp = 0; + + LOG_FUNCTION_START(""); + + tmp = (uint64_t)timeout_mclks * (uint64_t)macro_period_us; + tmp += 0x00800; + tmp = tmp >> 12; + + timeout_us = (uint32_t)tmp; + + + + LOG_FUNCTION_END(0); + + return timeout_us; +} + +uint32_t VL53LX_calc_crosstalk_plane_offset_with_margin( + uint32_t plane_offset_kcps, + int16_t margin_offset_kcps) +{ + uint32_t plane_offset_with_margin = 0; + int32_t plane_offset_kcps_temp = 0; + + LOG_FUNCTION_START(""); + + plane_offset_kcps_temp = + (int32_t)plane_offset_kcps + + (int32_t)margin_offset_kcps; + + if (plane_offset_kcps_temp < 0) + plane_offset_kcps_temp = 0; + else + if (plane_offset_kcps_temp > 0x3FFFF) + plane_offset_kcps_temp = 0x3FFFF; + + plane_offset_with_margin = (uint32_t) plane_offset_kcps_temp; + + LOG_FUNCTION_END(0); + + return plane_offset_with_margin; + +} + +uint32_t VL53LX_calc_decoded_timeout_us( + uint16_t timeout_encoded, + uint32_t macro_period_us) +{ + + + uint32_t timeout_mclks = 0; + uint32_t timeout_us = 0; + + LOG_FUNCTION_START(""); + + timeout_mclks = + VL53LX_decode_timeout(timeout_encoded); + + timeout_us = + VL53LX_calc_timeout_us(timeout_mclks, macro_period_us); + + LOG_FUNCTION_END(0); + + return timeout_us; +} + + +uint16_t VL53LX_encode_timeout(uint32_t timeout_mclks) +{ + + + uint16_t encoded_timeout = 0; + uint32_t ls_byte = 0; + uint16_t ms_byte = 0; + + if (timeout_mclks > 0) { + ls_byte = timeout_mclks - 1; + + while ((ls_byte & 0xFFFFFF00) > 0) { + ls_byte = ls_byte >> 1; + ms_byte++; + } + + encoded_timeout = (ms_byte << 8) + + (uint16_t) (ls_byte & 0x000000FF); + } + + return encoded_timeout; +} + + +uint32_t VL53LX_decode_timeout(uint16_t encoded_timeout) +{ + + + uint32_t timeout_macro_clks = 0; + + timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF) + << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1; + + return timeout_macro_clks; +} + + +VL53LX_Error VL53LX_calc_timeout_register_values( + uint32_t phasecal_config_timeout_us, + uint32_t mm_config_timeout_us, + uint32_t range_config_timeout_us, + uint16_t fast_osc_frequency, + VL53LX_general_config_t *pgeneral, + VL53LX_timing_config_t *ptiming) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint32_t macro_period_us = 0; + uint32_t timeout_mclks = 0; + uint16_t timeout_encoded = 0; + + LOG_FUNCTION_START(""); + + if (fast_osc_frequency == 0) { + status = VL53LX_ERROR_DIVISION_BY_ZERO; + } else { + + macro_period_us = + VL53LX_calc_macro_period_us( + fast_osc_frequency, + ptiming->range_config__vcsel_period_a); + + + timeout_mclks = + VL53LX_calc_timeout_mclks( + phasecal_config_timeout_us, + macro_period_us); + + + if (timeout_mclks > 0xFF) + timeout_mclks = 0xFF; + + pgeneral->phasecal_config__timeout_macrop = + (uint8_t)timeout_mclks; + + + timeout_encoded = + VL53LX_calc_encoded_timeout( + mm_config_timeout_us, + macro_period_us); + + ptiming->mm_config__timeout_macrop_a_hi = + (uint8_t)((timeout_encoded & 0xFF00) >> 8); + ptiming->mm_config__timeout_macrop_a_lo = + (uint8_t) (timeout_encoded & 0x00FF); + + + timeout_encoded = + VL53LX_calc_encoded_timeout( + range_config_timeout_us, + macro_period_us); + + ptiming->range_config__timeout_macrop_a_hi = + (uint8_t)((timeout_encoded & 0xFF00) >> 8); + ptiming->range_config__timeout_macrop_a_lo = + (uint8_t) (timeout_encoded & 0x00FF); + + + macro_period_us = + VL53LX_calc_macro_period_us( + fast_osc_frequency, + ptiming->range_config__vcsel_period_b); + + + timeout_encoded = + VL53LX_calc_encoded_timeout( + mm_config_timeout_us, + macro_period_us); + + ptiming->mm_config__timeout_macrop_b_hi = + (uint8_t)((timeout_encoded & 0xFF00) >> 8); + ptiming->mm_config__timeout_macrop_b_lo = + (uint8_t) (timeout_encoded & 0x00FF); + + + timeout_encoded = VL53LX_calc_encoded_timeout( + range_config_timeout_us, + macro_period_us); + + ptiming->range_config__timeout_macrop_b_hi = + (uint8_t)((timeout_encoded & 0xFF00) >> 8); + ptiming->range_config__timeout_macrop_b_lo = + (uint8_t) (timeout_encoded & 0x00FF); + } + + LOG_FUNCTION_END(0); + + return status; + +} + + +uint8_t VL53LX_encode_vcsel_period(uint8_t VL53LX_p_030) +{ + + + uint8_t vcsel_period_reg = 0; + + vcsel_period_reg = (VL53LX_p_030 >> 1) - 1; + + return vcsel_period_reg; +} + + +uint32_t VL53LX_decode_unsigned_integer( + uint8_t *pbuffer, + uint8_t no_of_bytes) +{ + + + uint8_t i = 0; + uint32_t decoded_value = 0; + + for (i = 0; i < no_of_bytes; i++) + decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i]; + + return decoded_value; +} + + +void VL53LX_encode_unsigned_integer( + uint32_t ip_value, + uint8_t no_of_bytes, + uint8_t *pbuffer) +{ + + + uint8_t i = 0; + uint32_t VL53LX_p_003 = 0; + + VL53LX_p_003 = ip_value; + for (i = 0; i < no_of_bytes; i++) { + pbuffer[no_of_bytes-i-1] = VL53LX_p_003 & 0x00FF; + VL53LX_p_003 = VL53LX_p_003 >> 8; + } +} + + +VL53LX_Error VL53LX_hist_copy_and_scale_ambient_info( + VL53LX_zone_hist_info_t *pidata, + VL53LX_histogram_bin_data_t *podata) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + int64_t evts = 0; + int64_t tmpi = 0; + int64_t tmpo = 0; + + LOG_FUNCTION_START(""); + + + if (pidata->result__dss_actual_effective_spads == 0) { + status = VL53LX_ERROR_DIVISION_BY_ZERO; + } else { + if (pidata->number_of_ambient_bins > 0 && + podata->number_of_ambient_bins == 0) { + + + + tmpo = 1 + (int64_t)podata->total_periods_elapsed; + tmpo *= + (int64_t)podata->result__dss_actual_effective_spads; + + tmpi = 1 + (int64_t)pidata->total_periods_elapsed; + tmpi *= + (int64_t)pidata->result__dss_actual_effective_spads; + + evts = tmpo * + (int64_t)pidata->ambient_events_sum; + evts += (tmpi/2); + + + if (tmpi != 0) + evts = do_division_s(evts, tmpi); + + podata->ambient_events_sum = (int32_t)evts; + + + + podata->VL53LX_p_028 = + podata->ambient_events_sum; + podata->VL53LX_p_028 += + ((int32_t)pidata->number_of_ambient_bins / 2); + podata->VL53LX_p_028 /= + (int32_t)pidata->number_of_ambient_bins; + } + } + + LOG_FUNCTION_END(0); + + return status; +} + + +void VL53LX_hist_get_bin_sequence_config( + VL53LX_DEV Dev, + VL53LX_histogram_bin_data_t *pdata) +{ + + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + int32_t amb_thresh_low = 0; + int32_t amb_thresh_high = 0; + + uint8_t i = 0; + + LOG_FUNCTION_START(""); + + + + amb_thresh_low = 1024 * + (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_low; + amb_thresh_high = 1024 * + (int32_t)pdev->hist_cfg.histogram_config__amb_thresh_high; + + + + if ((pdev->ll_state.rd_stream_count & 0x01) == 0) { + + pdata->bin_seq[5] = + pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 >> 4; + pdata->bin_seq[4] = + pdev->hist_cfg.histogram_config__mid_amb_even_bin_4_5 & 0x0F; + pdata->bin_seq[3] = + pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 >> 4; + pdata->bin_seq[2] = + pdev->hist_cfg.histogram_config__mid_amb_even_bin_2_3 & 0x0F; + pdata->bin_seq[1] = + pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 >> 4; + pdata->bin_seq[0] = + pdev->hist_cfg.histogram_config__mid_amb_even_bin_0_1 & 0x0F; + + if (pdata->ambient_events_sum > amb_thresh_high) { + pdata->bin_seq[5] = + pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5 + >> 4; + pdata->bin_seq[4] = + pdev->hist_cfg.histogram_config__high_amb_even_bin_4_5 + & 0x0F; + pdata->bin_seq[3] = + pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3 + >> 4; + pdata->bin_seq[2] = + pdev->hist_cfg.histogram_config__high_amb_even_bin_2_3 + & 0x0F; + pdata->bin_seq[1] = + pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1 + >> 4; + pdata->bin_seq[0] = + pdev->hist_cfg.histogram_config__high_amb_even_bin_0_1 + & 0x0F; + } + + if (pdata->ambient_events_sum < amb_thresh_low) { + pdata->bin_seq[5] = + pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5 + >> 4; + pdata->bin_seq[4] = + pdev->hist_cfg.histogram_config__low_amb_even_bin_4_5 + & 0x0F; + pdata->bin_seq[3] = + pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3 + >> 4; + pdata->bin_seq[2] = + pdev->hist_cfg.histogram_config__low_amb_even_bin_2_3 + & 0x0F; + pdata->bin_seq[1] = + pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1 + >> 4; + pdata->bin_seq[0] = + pdev->hist_cfg.histogram_config__low_amb_even_bin_0_1 + & 0x0F; + } + + } else { + pdata->bin_seq[5] = + pdev->hist_cfg.histogram_config__mid_amb_odd_bin_5 + & 0x0F; + pdata->bin_seq[4] = + pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4 + & 0x0F; + pdata->bin_seq[3] = + pdev->hist_cfg.histogram_config__mid_amb_odd_bin_3_4 + >> 4; + pdata->bin_seq[2] = + pdev->hist_cfg.histogram_config__mid_amb_odd_bin_2 & + 0x0F; + pdata->bin_seq[1] = + pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1 + >> 4; + pdata->bin_seq[0] = + pdev->hist_cfg.histogram_config__mid_amb_odd_bin_0_1 + & 0x0F; + + if (pdata->ambient_events_sum > amb_thresh_high) { + pdata->bin_seq[5] = + pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5 + >> 4; + pdata->bin_seq[4] = + pdev->hist_cfg.histogram_config__high_amb_odd_bin_4_5 + & 0x0F; + pdata->bin_seq[3] = + pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3 + >> 4; + pdata->bin_seq[2] = + pdev->hist_cfg.histogram_config__high_amb_odd_bin_2_3 + & 0x0F; + pdata->bin_seq[1] = + pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1 + >> 4; + pdata->bin_seq[0] = + pdev->hist_cfg.histogram_config__high_amb_odd_bin_0_1 + & 0x0F; + } + + if (pdata->ambient_events_sum < amb_thresh_low) { + pdata->bin_seq[5] = + pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5 + >> 4; + pdata->bin_seq[4] = + pdev->hist_cfg.histogram_config__low_amb_odd_bin_4_5 + & 0x0F; + pdata->bin_seq[3] = + pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3 + >> 4; + pdata->bin_seq[2] = + pdev->hist_cfg.histogram_config__low_amb_odd_bin_2_3 + & 0x0F; + pdata->bin_seq[1] = + pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1 + >> 4; + pdata->bin_seq[0] = + pdev->hist_cfg.histogram_config__low_amb_odd_bin_0_1 + & 0x0F; + } + } + + + + for (i = 0; i < VL53LX_MAX_BIN_SEQUENCE_LENGTH; i++) + pdata->bin_rep[i] = 1; + + LOG_FUNCTION_END(0); + +} + + +VL53LX_Error VL53LX_hist_phase_consistency_check( + VL53LX_DEV Dev, + VL53LX_zone_hist_info_t *phist_prev, + VL53LX_zone_objects_t *prange_prev, + VL53LX_range_results_t *prange_curr) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + + uint8_t lc = 0; + uint8_t p = 0; + + uint16_t phase_delta = 0; + uint16_t phase_tolerance = 0; + + int32_t events_delta = 0; + int32_t events_tolerance = 0; + + + uint8_t event_sigma; + uint16_t event_min_spad_count; + uint16_t min_max_tolerance; + uint8_t pht; + + VL53LX_DeviceError range_status = 0; + + LOG_FUNCTION_START(""); + + event_sigma = + pdev->histpostprocess.algo__consistency_check__event_sigma; + event_min_spad_count = + pdev->histpostprocess.algo__consistency_check__event_min_spad_count; + min_max_tolerance = + pdev->histpostprocess.algo__consistency_check__min_max_tolerance; + + + pht = pdev->histpostprocess.algo__consistency_check__phase_tolerance; + phase_tolerance = (uint16_t)pht; + phase_tolerance = phase_tolerance << 8; + + + + if (prange_prev->rd_device_state != + VL53LX_DEVICESTATE_RANGING_GATHER_DATA && + prange_prev->rd_device_state != + VL53LX_DEVICESTATE_RANGING_OUTPUT_DATA) + return status; + + + + if (phase_tolerance == 0) + return status; + + for (lc = 0; lc < prange_curr->active_results; lc++) { + + if (!((prange_curr->VL53LX_p_003[lc].range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE) || + (prange_curr->VL53LX_p_003[lc].range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK))) + continue; + + + + + + + if (prange_prev->active_objects == 0) + prange_curr->VL53LX_p_003[lc].range_status = + VL53LX_DEVICEERROR_PREV_RANGE_NO_TARGETS; + else + prange_curr->VL53LX_p_003[lc].range_status = + VL53LX_DEVICEERROR_PHASECONSISTENCY; + + + + + + for (p = 0; p < prange_prev->active_objects; p++) { + + if (prange_curr->VL53LX_p_003[lc].VL53LX_p_011 > + prange_prev->VL53LX_p_003[p].VL53LX_p_011) { + phase_delta = + prange_curr->VL53LX_p_003[lc].VL53LX_p_011 - + prange_prev->VL53LX_p_003[p].VL53LX_p_011; + } else { + phase_delta = + prange_prev->VL53LX_p_003[p].VL53LX_p_011 - + prange_curr->VL53LX_p_003[lc].VL53LX_p_011; + } + + if (phase_delta < phase_tolerance) { + + + + + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_hist_events_consistency_check( + event_sigma, + event_min_spad_count, + phist_prev, + &(prange_prev->VL53LX_p_003[p]), + &(prange_curr->VL53LX_p_003[lc]), + &events_tolerance, + &events_delta, + &range_status); + + + + + if (status == VL53LX_ERROR_NONE && + range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE) + status = + VL53LX_hist_merged_pulse_check( + min_max_tolerance, + &(prange_curr->VL53LX_p_003[lc]), + &range_status); + + prange_curr->VL53LX_p_003[lc].range_status = + range_status; + } + } + + } + + LOG_FUNCTION_END(status); + + return status; +} + + + +VL53LX_Error VL53LX_hist_events_consistency_check( + uint8_t event_sigma, + uint16_t min_effective_spad_count, + VL53LX_zone_hist_info_t *phist_prev, + VL53LX_object_data_t *prange_prev, + VL53LX_range_data_t *prange_curr, + int32_t *pevents_tolerance, + int32_t *pevents_delta, + VL53LX_DeviceError *prange_status) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + int64_t tmpp = 0; + int64_t tmpc = 0; + int64_t events_scaler = 0; + int64_t events_scaler_sq = 0; + int64_t c_signal_events = 0; + int64_t c_sig_noise_sq = 0; + int64_t c_amb_noise_sq = 0; + int64_t p_amb_noise_sq = 0; + + int32_t p_signal_events = 0; + uint32_t noise_sq_sum = 0; + + + + if (event_sigma == 0) { + *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE; + return status; + } + + + + tmpp = 1 + (int64_t)phist_prev->total_periods_elapsed; + tmpp *= (int64_t)phist_prev->result__dss_actual_effective_spads; + + + + tmpc = 1 + (int64_t)prange_curr->total_periods_elapsed; + tmpc *= (int64_t)prange_curr->VL53LX_p_004; + + + + events_scaler = tmpp * 4096; + if (tmpc != 0) { + events_scaler += (tmpc/2); + events_scaler = do_division_s(events_scaler, tmpc); + } + + events_scaler_sq = events_scaler * events_scaler; + events_scaler_sq += 2048; + events_scaler_sq /= 4096; + + + + c_signal_events = (int64_t)prange_curr->VL53LX_p_017; + c_signal_events -= (int64_t)prange_curr->VL53LX_p_016; + c_signal_events *= (int64_t)events_scaler; + c_signal_events += 2048; + c_signal_events /= 4096; + + c_sig_noise_sq = (int64_t)events_scaler_sq; + c_sig_noise_sq *= (int64_t)prange_curr->VL53LX_p_017; + c_sig_noise_sq += 2048; + c_sig_noise_sq /= 4096; + + c_amb_noise_sq = (int64_t)events_scaler_sq; + c_amb_noise_sq *= (int64_t)prange_curr->VL53LX_p_016; + c_amb_noise_sq += 2048; + c_amb_noise_sq /= 4096; + + + c_amb_noise_sq += 2; + c_amb_noise_sq /= 4; + + + + p_amb_noise_sq = + (int64_t)prange_prev->VL53LX_p_016; + + + p_amb_noise_sq += 2; + p_amb_noise_sq /= 4; + + noise_sq_sum = + (uint32_t)prange_prev->VL53LX_p_017 + + (uint32_t)c_sig_noise_sq + + (uint32_t)p_amb_noise_sq + + (uint32_t)c_amb_noise_sq; + + *pevents_tolerance = + (int32_t)VL53LX_isqrt(noise_sq_sum * 16); + + *pevents_tolerance *= (int32_t)event_sigma; + *pevents_tolerance += 32; + *pevents_tolerance /= 64; + + p_signal_events = (int32_t)prange_prev->VL53LX_p_017; + p_signal_events -= (int32_t)prange_prev->VL53LX_p_016; + + if ((int32_t)c_signal_events > p_signal_events) + *pevents_delta = + (int32_t)c_signal_events - p_signal_events; + else + *pevents_delta = + p_signal_events - (int32_t)c_signal_events; + + if (*pevents_delta > *pevents_tolerance && + prange_curr->VL53LX_p_004 > min_effective_spad_count) + *prange_status = VL53LX_DEVICEERROR_EVENTCONSISTENCY; + else + *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE; + + + + + + return status; +} + + + + +VL53LX_Error VL53LX_hist_merged_pulse_check( + int16_t min_max_tolerance_mm, + VL53LX_range_data_t *pdata, + VL53LX_DeviceError *prange_status) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + int16_t delta_mm = 0; + + if (pdata->max_range_mm > pdata->min_range_mm) + delta_mm = + pdata->max_range_mm - pdata->min_range_mm; + else + delta_mm = + pdata->min_range_mm - pdata->max_range_mm; + + if (min_max_tolerance_mm > 0 && + delta_mm > min_max_tolerance_mm) + *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE; + else + *prange_status = VL53LX_DEVICEERROR_RANGECOMPLETE; + + return status; +} + + + + +VL53LX_Error VL53LX_hist_xmonitor_consistency_check( + VL53LX_DEV Dev, + VL53LX_zone_hist_info_t *phist_prev, + VL53LX_zone_objects_t *prange_prev, + VL53LX_range_data_t *prange_curr) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + + int32_t events_delta = 0; + int32_t events_tolerance = 0; + uint8_t event_sigma; + uint16_t min_spad_count; + + event_sigma = pdev->histpostprocess.algo__crosstalk_detect_event_sigma; + min_spad_count = + pdev->histpostprocess.algo__consistency_check__event_min_spad_count; + + if (prange_curr->range_status == VL53LX_DEVICEERROR_RANGECOMPLETE || + prange_curr->range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK || + prange_curr->range_status == + VL53LX_DEVICEERROR_EVENTCONSISTENCY) { + + if (prange_prev->xmonitor.range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE || + prange_prev->xmonitor.range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK || + prange_prev->xmonitor.range_status == + VL53LX_DEVICEERROR_EVENTCONSISTENCY) { + + prange_curr->range_status = + VL53LX_DEVICEERROR_RANGECOMPLETE; + + status = + VL53LX_hist_events_consistency_check( + event_sigma, + min_spad_count, + phist_prev, + &(prange_prev->xmonitor), + prange_curr, + &events_tolerance, + &events_delta, + &(prange_curr->range_status)); + + } + } + + return status; +} + + + + +VL53LX_Error VL53LX_hist_wrap_dmax( + VL53LX_hist_post_process_config_t *phistpostprocess, + VL53LX_histogram_bin_data_t *pcurrent, + int16_t *pwrap_dmax_mm) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint32_t pll_period_mm = 0; + uint32_t wrap_dmax_phase = 0; + uint32_t range_mm = 0; + + LOG_FUNCTION_START(""); + + *pwrap_dmax_mm = 0; + + + if (pcurrent->VL53LX_p_015 != 0) { + + + + pll_period_mm = + VL53LX_calc_pll_period_mm( + pcurrent->VL53LX_p_015); + + + + wrap_dmax_phase = + (uint32_t)phistpostprocess->valid_phase_high << 8; + + + + range_mm = wrap_dmax_phase * pll_period_mm; + range_mm = (range_mm + (1<<14)) >> 15; + + *pwrap_dmax_mm = (int16_t)range_mm; + } + + LOG_FUNCTION_END(status); + + return status; +} + + +void VL53LX_hist_combine_mm1_mm2_offsets( + int16_t mm1_offset_mm, + int16_t mm2_offset_mm, + uint8_t encoded_mm_roi_centre, + uint8_t encoded_mm_roi_size, + uint8_t encoded_zone_centre, + uint8_t encoded_zone_size, + VL53LX_additional_offset_cal_data_t *pcal_data, + uint8_t *pgood_spads, + uint16_t aperture_attenuation, + int16_t *prange_offset_mm) +{ + + + + uint16_t max_mm_inner_effective_spads = 0; + uint16_t max_mm_outer_effective_spads = 0; + uint16_t mm_inner_effective_spads = 0; + uint16_t mm_outer_effective_spads = 0; + + uint32_t scaled_mm1_peak_rate_mcps = 0; + uint32_t scaled_mm2_peak_rate_mcps = 0; + + int32_t tmp0 = 0; + int32_t tmp1 = 0; + + + + VL53LX_calc_mm_effective_spads( + encoded_mm_roi_centre, + encoded_mm_roi_size, + 0xC7, + 0xFF, + pgood_spads, + aperture_attenuation, + &max_mm_inner_effective_spads, + &max_mm_outer_effective_spads); + + if ((max_mm_inner_effective_spads == 0) || + (max_mm_outer_effective_spads == 0)) + goto FAIL; + + + VL53LX_calc_mm_effective_spads( + encoded_mm_roi_centre, + encoded_mm_roi_size, + encoded_zone_centre, + encoded_zone_size, + pgood_spads, + aperture_attenuation, + &mm_inner_effective_spads, + &mm_outer_effective_spads); + + + + scaled_mm1_peak_rate_mcps = + (uint32_t)pcal_data->result__mm_inner_peak_signal_count_rtn_mcps; + scaled_mm1_peak_rate_mcps *= (uint32_t)mm_inner_effective_spads; + scaled_mm1_peak_rate_mcps /= (uint32_t)max_mm_inner_effective_spads; + + scaled_mm2_peak_rate_mcps = + (uint32_t)pcal_data->result__mm_outer_peak_signal_count_rtn_mcps; + scaled_mm2_peak_rate_mcps *= (uint32_t)mm_outer_effective_spads; + scaled_mm2_peak_rate_mcps /= (uint32_t)max_mm_outer_effective_spads; + + + + tmp0 = ((int32_t)mm1_offset_mm * (int32_t)scaled_mm1_peak_rate_mcps); + tmp0 += ((int32_t)mm2_offset_mm * (int32_t)scaled_mm2_peak_rate_mcps); + + tmp1 = (int32_t)scaled_mm1_peak_rate_mcps + + (int32_t)scaled_mm2_peak_rate_mcps; + + + + if (tmp1 != 0) + tmp0 = (tmp0 * 4) / tmp1; +FAIL: + *prange_offset_mm = (int16_t)tmp0; + +} + + +VL53LX_Error VL53LX_hist_xtalk_extract_calc_window( + 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(""); + + + pxtalk_data->pll_period_mm = + VL53LX_calc_pll_period_mm(phist_bins->VL53LX_p_015); + if (pxtalk_data->pll_period_mm == 0) + pxtalk_data->pll_period_mm = 1; + + + pxtalk_data->xtalk_width_phase = + (int32_t)phist_bins->vcsel_width * 128; + pxtalk_data->target_width_phase = + pxtalk_data->xtalk_width_phase + + (int32_t)target_width_oversize * 128; + + + + pxtalk_data->xtalk_start_phase = + (int32_t)phist_bins->zero_distance_phase - + (pxtalk_data->xtalk_width_phase / 2); + pxtalk_data->xtalk_end_phase = + (int32_t)pxtalk_data->xtalk_start_phase + + pxtalk_data->xtalk_width_phase; + + if (pxtalk_data->xtalk_start_phase < 0) + pxtalk_data->xtalk_start_phase = 0; + + + + + pxtalk_data->VL53LX_p_012 = + (uint8_t)(pxtalk_data->xtalk_start_phase / 2048); + + + pxtalk_data->VL53LX_p_013 = + (uint8_t)((pxtalk_data->xtalk_end_phase + 2047) / 2048); + + + + pxtalk_data->target_start_phase = + (int32_t)target_distance_mm * 2048 * 16; + pxtalk_data->target_start_phase += + ((int32_t)pxtalk_data->pll_period_mm / 2); + pxtalk_data->target_start_phase /= (int32_t)pxtalk_data->pll_period_mm; + pxtalk_data->target_start_phase += + (int32_t)phist_bins->zero_distance_phase; + + + + pxtalk_data->target_start_phase -= + (pxtalk_data->target_width_phase / 2); + pxtalk_data->target_end_phase = + (int32_t)pxtalk_data->target_start_phase + + pxtalk_data->target_width_phase; + + if (pxtalk_data->target_start_phase < 0) + pxtalk_data->target_start_phase = 0; + + + pxtalk_data->target_start = + (uint8_t)(pxtalk_data->target_start_phase / 2048); + + + if (pxtalk_data->VL53LX_p_013 > (pxtalk_data->target_start-1)) + pxtalk_data->VL53LX_p_013 = pxtalk_data->target_start-1; + + + pxtalk_data->effective_width = + (2048 * ((int32_t)pxtalk_data->VL53LX_p_013+1)); + pxtalk_data->effective_width -= pxtalk_data->xtalk_start_phase; + + + if (pxtalk_data->effective_width > pxtalk_data->xtalk_width_phase) + pxtalk_data->effective_width = pxtalk_data->xtalk_width_phase; + + if (pxtalk_data->effective_width < 1) + pxtalk_data->effective_width = 1; + + + pxtalk_data->event_scaler = pxtalk_data->xtalk_width_phase * 1000; + pxtalk_data->event_scaler += (pxtalk_data->effective_width / 2); + pxtalk_data->event_scaler /= pxtalk_data->effective_width; + + + if (pxtalk_data->event_scaler < 1000) + pxtalk_data->event_scaler = 1000; + + if (pxtalk_data->event_scaler > 4000) + pxtalk_data->event_scaler = 4000; + + + pxtalk_data->event_scaler_sum += pxtalk_data->event_scaler; + + + pxtalk_data->peak_duration_us_sum += + (uint32_t)phist_bins->peak_duration_us; + + + pxtalk_data->effective_spad_count_sum += + (uint32_t)phist_bins->result__dss_actual_effective_spads; + + + pxtalk_data->zero_distance_phase_sum += + (uint32_t)phist_bins->zero_distance_phase; + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_hist_xtalk_extract_calc_event_sums( + VL53LX_histogram_bin_data_t *phist_bins, + VL53LX_hist_xtalk_extract_data_t *pxtalk_data) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint8_t lb = 0; + uint8_t i = 0; + + LOG_FUNCTION_START(""); + + + + for (lb = pxtalk_data->VL53LX_p_012; + lb <= pxtalk_data->VL53LX_p_013; + lb++) { + + + i = (lb + phist_bins->number_of_ambient_bins + + phist_bins->VL53LX_p_021) % + phist_bins->VL53LX_p_021; + + + pxtalk_data->signal_events_sum += phist_bins->bin_data[i]; + pxtalk_data->signal_events_sum -= + phist_bins->VL53LX_p_028; + } + + + + for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS && + lb < phist_bins->VL53LX_p_021; lb++) { + + + i = (lb + phist_bins->number_of_ambient_bins + + phist_bins->VL53LX_p_021) % + phist_bins->VL53LX_p_021; + + + pxtalk_data->bin_data_sums[lb] += phist_bins->bin_data[i]; + pxtalk_data->bin_data_sums[lb] -= + phist_bins->VL53LX_p_028; + } + + pxtalk_data->sample_count += 1; + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_hist_xtalk_extract_calc_rate_per_spad( + VL53LX_hist_xtalk_extract_data_t *pxtalk_data) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint64_t tmp64_0 = 0; + uint64_t tmp64_1 = 0; + uint64_t xtalk_per_spad = 0; + + LOG_FUNCTION_START(""); + + + + + + + if (pxtalk_data->signal_events_sum > 0) { + tmp64_0 = + ((uint64_t)pxtalk_data->signal_events_sum * + (uint64_t)pxtalk_data->sample_count * + (uint64_t)pxtalk_data->event_scaler_avg * 256U) << 9U; + tmp64_1 = + (uint64_t)pxtalk_data->effective_spad_count_sum * + (uint64_t)pxtalk_data->peak_duration_us_sum; + + + + if (tmp64_1 > 0U) { + + tmp64_0 = tmp64_0 + (tmp64_1 >> 1U); + xtalk_per_spad = do_division_u(tmp64_0, tmp64_1); + } else { + xtalk_per_spad = (uint64_t)tmp64_0; + } + + } else { + status = VL53LX_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL; + } + + pxtalk_data->xtalk_rate_kcps_per_spad = (uint32_t)xtalk_per_spad; + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_hist_xtalk_extract_calc_shape( + VL53LX_hist_xtalk_extract_data_t *pxtalk_data, + VL53LX_xtalk_histogram_shape_t *pxtalk_shape) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + int32_t lb = 0; + uint64_t total_events = 0U; + uint64_t tmp64_0 = 0U; + int32_t remaining_area = 1024; + + LOG_FUNCTION_START(""); + + + + pxtalk_shape->VL53LX_p_019 = 0; + pxtalk_shape->VL53LX_p_020 = VL53LX_XTALK_HISTO_BINS; + pxtalk_shape->VL53LX_p_021 = VL53LX_XTALK_HISTO_BINS; + + pxtalk_shape->zero_distance_phase = + (uint16_t)pxtalk_data->zero_distance_phase_avg; + pxtalk_shape->phasecal_result__reference_phase = + (uint16_t)pxtalk_data->zero_distance_phase_avg + (3*2048); + + + + if (pxtalk_data->signal_events_sum > 0) + total_events = + (uint64_t)pxtalk_data->signal_events_sum * + (uint64_t)pxtalk_data->event_scaler_avg; + else + total_events = 1; + if (total_events == 0) + total_events = 1; + + + remaining_area = 1024; + pxtalk_data->max_shape_value = 0; + + for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS; lb++) { + + if ((lb < (int32_t)pxtalk_data->VL53LX_p_012 || + lb > (int32_t)pxtalk_data->VL53LX_p_013) || + pxtalk_data->bin_data_sums[lb] < 0) { + + + if (remaining_area > 0 && remaining_area < 1024) { + if (remaining_area > + pxtalk_data->max_shape_value) { + pxtalk_shape->bin_data[lb] = + (uint32_t)pxtalk_data->max_shape_value; + remaining_area -= + pxtalk_data->max_shape_value; + } else { + pxtalk_shape->bin_data[lb] = + (uint32_t)remaining_area; + remaining_area = 0; + } + } else { + pxtalk_shape->bin_data[lb] = 0; + } + + } else { + + tmp64_0 = + (uint64_t)pxtalk_data->bin_data_sums[lb] + * 1024U * 1000U; + tmp64_0 += (total_events >> 1); + tmp64_0 = do_division_u(tmp64_0, total_events); + if (tmp64_0 > 0xFFFFU) + tmp64_0 = 0xFFFFU; + + pxtalk_shape->bin_data[lb] = (uint32_t)tmp64_0; + + + if ((int32_t)pxtalk_shape->bin_data[lb] > + pxtalk_data->max_shape_value) + pxtalk_data->max_shape_value = + (int32_t)pxtalk_shape->bin_data[lb]; + + remaining_area -= (int32_t)pxtalk_shape->bin_data[lb]; + } + } + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_hist_xtalk_shape_model( + uint16_t events_per_bin, + uint16_t pulse_centre, + uint16_t pulse_width, + VL53LX_xtalk_histogram_shape_t *pxtalk_shape) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint32_t phase_start = 0; + uint32_t phase_stop = 0; + uint32_t phase_bin = 0; + + uint32_t bin_start = 0; + uint32_t bin_stop = 0; + + uint32_t lb = 0; + uint16_t VL53LX_p_018 = 0; + + LOG_FUNCTION_START(""); + + + + pxtalk_shape->VL53LX_p_019 = 0; + pxtalk_shape->VL53LX_p_020 = VL53LX_XTALK_HISTO_BINS; + pxtalk_shape->VL53LX_p_021 = VL53LX_XTALK_HISTO_BINS; + + pxtalk_shape->zero_distance_phase = pulse_centre; + pxtalk_shape->phasecal_result__reference_phase = + pulse_centre + (3*2048); + + + if (pulse_centre > (pulse_width >> 1)) + phase_start = (uint32_t)pulse_centre - + ((uint32_t)pulse_width >> 1); + else + phase_start = 0; + + phase_stop = (uint32_t)pulse_centre + + ((uint32_t)pulse_width >> 1); + + + bin_start = (phase_start / 2048); + bin_stop = (phase_stop / 2048); + + for (lb = 0; lb < VL53LX_XTALK_HISTO_BINS; lb++) { + VL53LX_p_018 = 0; + + + if (lb == bin_start && lb == bin_stop) { + VL53LX_p_018 = + VL53LX_hist_xtalk_shape_model_interp( + events_per_bin, + phase_stop - phase_start); + + } else if (lb > bin_start && lb < bin_stop) { + + + VL53LX_p_018 = events_per_bin; + + } else if (lb == bin_start) { + + + phase_bin = (lb + 1) * 2048; + VL53LX_p_018 = + VL53LX_hist_xtalk_shape_model_interp( + events_per_bin, + (phase_bin - phase_start)); + + } else if (lb == bin_stop) { + + + phase_bin = lb * 2048; + VL53LX_p_018 = + VL53LX_hist_xtalk_shape_model_interp( + events_per_bin, + (phase_stop - phase_bin)); + } + + pxtalk_shape->bin_data[lb] = VL53LX_p_018; + } + + LOG_FUNCTION_END(status); + + return status; +} + + +uint16_t VL53LX_hist_xtalk_shape_model_interp( + uint16_t events_per_bin, + uint32_t phase_delta) +{ + + + uint32_t VL53LX_p_018 = 0; + + LOG_FUNCTION_START(""); + + + VL53LX_p_018 = (uint32_t)events_per_bin * phase_delta; + VL53LX_p_018 += 1024; + VL53LX_p_018 /= 2048; + + + if (VL53LX_p_018 > 0xFFFFU) + VL53LX_p_018 = 0xFFFFU; + + LOG_FUNCTION_END(0); + + return (uint16_t)VL53LX_p_018; +} + + +void VL53LX_spad_number_to_byte_bit_index( + uint8_t spad_number, + uint8_t *pbyte_index, + uint8_t *pbit_index, + uint8_t *pbit_mask) +{ + + + + *pbyte_index = spad_number >> 3; + *pbit_index = spad_number & 0x07; + *pbit_mask = 0x01 << *pbit_index; + +} + + +void VL53LX_encode_row_col( + uint8_t row, + uint8_t col, + uint8_t *pspad_number) +{ + + + if (row > 7) + *pspad_number = 128 + (col << 3) + (15-row); + else + *pspad_number = ((15-col) << 3) + row; + +} + + +void VL53LX_decode_zone_size( + uint8_t encoded_xy_size, + uint8_t *pwidth, + uint8_t *pheight) +{ + + + + *pheight = encoded_xy_size >> 4; + *pwidth = encoded_xy_size & 0x0F; + +} + + +void VL53LX_encode_zone_size( + uint8_t width, + uint8_t height, + uint8_t *pencoded_xy_size) +{ + + + *pencoded_xy_size = (height << 4) + width; + +} + + +void VL53LX_decode_zone_limits( + uint8_t encoded_xy_centre, + uint8_t encoded_xy_size, + int16_t *px_ll, + int16_t *py_ll, + int16_t *px_ur, + int16_t *py_ur) +{ + + + + uint8_t x_centre = 0; + uint8_t y_centre = 0; + uint8_t width = 0; + uint8_t height = 0; + + + + VL53LX_decode_row_col( + encoded_xy_centre, + &y_centre, + &x_centre); + + VL53LX_decode_zone_size( + encoded_xy_size, + &width, + &height); + + + + *px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2; + if (*px_ll < 0) + *px_ll = 0; + + *px_ur = *px_ll + (int16_t)width; + if (*px_ur > (VL53LX_SPAD_ARRAY_WIDTH-1)) + *px_ur = VL53LX_SPAD_ARRAY_WIDTH-1; + + *py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2; + if (*py_ll < 0) + *py_ll = 0; + + *py_ur = *py_ll + (int16_t)height; + if (*py_ur > (VL53LX_SPAD_ARRAY_HEIGHT-1)) + *py_ur = VL53LX_SPAD_ARRAY_HEIGHT-1; +} + + +uint8_t VL53LX_is_aperture_location( + uint8_t row, + uint8_t col) +{ + + + uint8_t is_aperture = 0; + uint8_t mod_row = row % 4; + uint8_t mod_col = col % 4; + + if (mod_row == 0 && mod_col == 2) + is_aperture = 1; + + if (mod_row == 2 && mod_col == 0) + is_aperture = 1; + + return is_aperture; +} + + +void VL53LX_calc_max_effective_spads( + uint8_t encoded_zone_centre, + uint8_t encoded_zone_size, + uint8_t *pgood_spads, + uint16_t aperture_attenuation, + uint16_t *pmax_effective_spads) +{ + + + + int16_t x = 0; + int16_t y = 0; + + int16_t zone_x_ll = 0; + int16_t zone_y_ll = 0; + int16_t zone_x_ur = 0; + int16_t zone_y_ur = 0; + + uint8_t spad_number = 0; + uint8_t byte_index = 0; + uint8_t bit_index = 0; + uint8_t bit_mask = 0; + + uint8_t is_aperture = 0; + + + + VL53LX_decode_zone_limits( + encoded_zone_centre, + encoded_zone_size, + &zone_x_ll, + &zone_y_ll, + &zone_x_ur, + &zone_y_ur); + + + + *pmax_effective_spads = 0; + + for (y = zone_y_ll; y <= zone_y_ur; y++) { + for (x = zone_x_ll; x <= zone_x_ur; x++) { + + + + VL53LX_encode_row_col( + (uint8_t)y, + (uint8_t)x, + &spad_number); + + + + VL53LX_spad_number_to_byte_bit_index( + spad_number, + &byte_index, + &bit_index, + &bit_mask); + + + + if ((pgood_spads[byte_index] & bit_mask) > 0) { + + + is_aperture = VL53LX_is_aperture_location( + (uint8_t)y, + (uint8_t)x); + + if (is_aperture > 0) + *pmax_effective_spads += + aperture_attenuation; + else + *pmax_effective_spads += 0x0100; + + } + } + } +} + + +void VL53LX_calc_mm_effective_spads( + uint8_t encoded_mm_roi_centre, + uint8_t encoded_mm_roi_size, + uint8_t encoded_zone_centre, + uint8_t encoded_zone_size, + uint8_t *pgood_spads, + uint16_t aperture_attenuation, + uint16_t *pmm_inner_effective_spads, + uint16_t *pmm_outer_effective_spads) +{ + + + + int16_t x = 0; + int16_t y = 0; + + int16_t mm_x_ll = 0; + int16_t mm_y_ll = 0; + int16_t mm_x_ur = 0; + int16_t mm_y_ur = 0; + + int16_t zone_x_ll = 0; + int16_t zone_y_ll = 0; + int16_t zone_x_ur = 0; + int16_t zone_y_ur = 0; + + uint8_t spad_number = 0; + uint8_t byte_index = 0; + uint8_t bit_index = 0; + uint8_t bit_mask = 0; + + uint8_t is_aperture = 0; + uint16_t spad_attenuation = 0; + + + + VL53LX_decode_zone_limits( + encoded_mm_roi_centre, + encoded_mm_roi_size, + &mm_x_ll, + &mm_y_ll, + &mm_x_ur, + &mm_y_ur); + + VL53LX_decode_zone_limits( + encoded_zone_centre, + encoded_zone_size, + &zone_x_ll, + &zone_y_ll, + &zone_x_ur, + &zone_y_ur); + + + + *pmm_inner_effective_spads = 0; + *pmm_outer_effective_spads = 0; + + for (y = zone_y_ll; y <= zone_y_ur; y++) { + for (x = zone_x_ll; x <= zone_x_ur; x++) { + + + + VL53LX_encode_row_col( + (uint8_t)y, + (uint8_t)x, + &spad_number); + + + + VL53LX_spad_number_to_byte_bit_index( + spad_number, + &byte_index, + &bit_index, + &bit_mask); + + + + if ((pgood_spads[byte_index] & bit_mask) > 0) { + + + is_aperture = VL53LX_is_aperture_location( + (uint8_t)y, + (uint8_t)x); + + if (is_aperture > 0) + spad_attenuation = aperture_attenuation; + else + spad_attenuation = 0x0100; + + + + if (x >= mm_x_ll && x <= mm_x_ur && + y >= mm_y_ll && y <= mm_y_ur) + *pmm_inner_effective_spads += + spad_attenuation; + else + *pmm_outer_effective_spads += + spad_attenuation; + } + } + } +} + + +void VL53LX_hist_copy_results_to_sys_and_core( + VL53LX_histogram_bin_data_t *pbins, + VL53LX_range_results_t *phist, + VL53LX_system_results_t *psys, + VL53LX_core_results_t *pcore) +{ + + + uint8_t i = 0; + + VL53LX_range_data_t *pdata; + + LOG_FUNCTION_START(""); + + + + VL53LX_init_system_results(psys); + + + + psys->result__interrupt_status = pbins->result__interrupt_status; + psys->result__range_status = phist->active_results; + psys->result__report_status = pbins->result__report_status; + psys->result__stream_count = pbins->result__stream_count; + + pdata = &(phist->VL53LX_p_003[0]); + + for (i = 0; i < phist->active_results; i++) { + + switch (i) { + case 0: + psys->result__dss_actual_effective_spads_sd0 = + pdata->VL53LX_p_004; + psys->result__peak_signal_count_rate_mcps_sd0 = + pdata->peak_signal_count_rate_mcps; + psys->result__avg_signal_count_rate_mcps_sd0 = + pdata->avg_signal_count_rate_mcps; + psys->result__ambient_count_rate_mcps_sd0 = + pdata->ambient_count_rate_mcps; + + psys->result__sigma_sd0 = pdata->VL53LX_p_002; + psys->result__phase_sd0 = pdata->VL53LX_p_011; + + psys->result__final_crosstalk_corrected_range_mm_sd0 = + (uint16_t)pdata->median_range_mm; + + psys->result__phase_sd1 = pdata->zero_distance_phase; + + pcore->result_core__ranging_total_events_sd0 = + pdata->VL53LX_p_017; + pcore->result_core__signal_total_events_sd0 = + pdata->VL53LX_p_010; + pcore->result_core__total_periods_elapsed_sd0 = + pdata->total_periods_elapsed; + pcore->result_core__ambient_window_events_sd0 = + pdata->VL53LX_p_016; + + break; + case 1: + psys->result__dss_actual_effective_spads_sd1 = + pdata->VL53LX_p_004; + psys->result__peak_signal_count_rate_mcps_sd1 = + pdata->peak_signal_count_rate_mcps; + psys->result__ambient_count_rate_mcps_sd1 = + pdata->ambient_count_rate_mcps; + + psys->result__sigma_sd1 = pdata->VL53LX_p_002; + psys->result__phase_sd1 = pdata->VL53LX_p_011; + + psys->result__final_crosstalk_corrected_range_mm_sd1 = + (uint16_t)pdata->median_range_mm; + + pcore->result_core__ranging_total_events_sd1 = + pdata->VL53LX_p_017; + pcore->result_core__signal_total_events_sd1 = + pdata->VL53LX_p_010; + pcore->result_core__total_periods_elapsed_sd1 = + pdata->total_periods_elapsed; + pcore->result_core__ambient_window_events_sd1 = + pdata->VL53LX_p_016; + break; + } + + pdata++; + } + + LOG_FUNCTION_END(0); + +} + + +VL53LX_Error VL53LX_sum_histogram_data( + VL53LX_histogram_bin_data_t *phist_input, + VL53LX_histogram_bin_data_t *phist_output) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint8_t i = 0; + uint8_t smallest_bin_num = 0; + + LOG_FUNCTION_START(""); + + + + if (status == VL53LX_ERROR_NONE) { + if (phist_output->VL53LX_p_021 >= + phist_input->VL53LX_p_021) + smallest_bin_num = phist_input->VL53LX_p_021; + else + smallest_bin_num = phist_output->VL53LX_p_021; + } + + + + + + if (status == VL53LX_ERROR_NONE) + for (i = 0; i < smallest_bin_num; i++) + + phist_output->bin_data[i] += phist_input->bin_data[i]; + + if (status == VL53LX_ERROR_NONE) + phist_output->VL53LX_p_028 += + phist_input->VL53LX_p_028; + + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_avg_histogram_data( + uint8_t no_of_samples, + VL53LX_histogram_bin_data_t *phist_sum, + VL53LX_histogram_bin_data_t *phist_avg) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint8_t i = 0; + + LOG_FUNCTION_START(""); + + + + + + if (status == VL53LX_ERROR_NONE) { + for (i = 0; i < phist_sum->VL53LX_p_021; i++) { + + + + if (no_of_samples > 0) + phist_avg->bin_data[i] = + phist_sum->bin_data[i] / + (int32_t)no_of_samples; + else + phist_avg->bin_data[i] = phist_sum->bin_data[i]; + } + } + + if (status == VL53LX_ERROR_NONE) { + if (no_of_samples > 0) + phist_avg->VL53LX_p_028 = + phist_sum->VL53LX_p_028 / + (int32_t)no_of_samples; + else + phist_avg->VL53LX_p_028 = + phist_sum->VL53LX_p_028; + } + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_save_cfg_data( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = + VL53LXDevStructGetLLResultsHandle(Dev); + + VL53LX_zone_private_dyn_cfg_t *pzone_dyn_cfg; + VL53LX_dynamic_config_t *pdynamic = &(pdev->dyn_cfg); + + LOG_FUNCTION_START(""); + + pzone_dyn_cfg = + &(pres->zone_dyn_cfgs.VL53LX_p_003[pdev->ll_state.cfg_zone_id]); + + pzone_dyn_cfg->expected_stream_count = + pdev->ll_state.cfg_stream_count; + + pzone_dyn_cfg->expected_gph_id = + pdev->ll_state.cfg_gph_id; + + pzone_dyn_cfg->roi_config__user_roi_centre_spad = + pdynamic->roi_config__user_roi_centre_spad; + + pzone_dyn_cfg->roi_config__user_roi_requested_global_xy_size = + pdynamic->roi_config__user_roi_requested_global_xy_size; + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_dynamic_zone_update( + VL53LX_DEV Dev, + VL53LX_range_results_t *presults) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = + VL53LXDevStructGetLLResultsHandle(Dev); + VL53LX_zone_private_dyn_cfgs_t *pZ = &(pres->zone_dyn_cfgs); + + uint8_t zone_id = pdev->ll_state.rd_zone_id; + uint8_t i; + uint16_t max_total_rate_per_spads; + uint16_t target_rate = + pdev->stat_cfg.dss_config__target_total_rate_mcps; + uint32_t temp = 0xFFFF; +#ifdef VL53LX_LOG_ENABLE + uint16_t eff_spad_cnt = + pZ->VL53LX_p_003[zone_id].dss_requested_effective_spad_count; +#endif + + LOG_FUNCTION_START(""); + + pZ->VL53LX_p_003[zone_id].dss_requested_effective_spad_count = 0; + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: peak signal count rate mcps:"); + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + "%u actual effective spads: %u\n", + presults->VL53LX_p_003[0].peak_signal_count_rate_mcps, + presults->VL53LX_p_003[0].VL53LX_p_004); + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: active results: %u\n", + presults->active_results); + + max_total_rate_per_spads = + presults->VL53LX_p_003[0].total_rate_per_spad_mcps; + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: max total rate per spad at start: %u\n", + max_total_rate_per_spads); + + for (i = 1; i < presults->active_results; i++) { + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: zone total rate per spad: zone_id: %u,", + i); + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + "total rate per spad: %u\n", + presults->VL53LX_p_003[i].total_rate_per_spad_mcps); + + if (presults->VL53LX_p_003[i].total_rate_per_spad_mcps > + max_total_rate_per_spads) + max_total_rate_per_spads = + presults->VL53LX_p_003[i].total_rate_per_spad_mcps; + + } + + if (max_total_rate_per_spads == 0) { + + temp = 0xFFFF; + } else { + + temp = target_rate << 14; + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: 1: temp: %u\n", + temp); + + + temp = temp / max_total_rate_per_spads; + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: 2: temp: %u\n", + temp); + + + if (temp > 0xFFFF) + temp = 0xFFFF; + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: 3: temp: %u\n", + temp); + } + + pZ->VL53LX_p_003[zone_id].dss_requested_effective_spad_count = + (uint16_t)temp; + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " DYNZONEUPDATE: zone_id: %u, target_rate: %u,", + zone_id, + target_rate); + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + "max_total_rate_per_spads: %u, requested_spads: %u\n", + max_total_rate_per_spads, + eff_spad_cnt); + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_multizone_hist_bins_update( + VL53LX_DEV Dev) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_ll_driver_state_t *pstate = &(pdev->ll_state); + VL53LX_zone_config_t *pzone_cfg = &(pdev->zone_cfg); + VL53LX_histogram_config_t *phist_cfg = &(pdev->hist_cfg); + VL53LX_histogram_config_t *pmulti_hist = + &(pzone_cfg->multizone_hist_cfg); + + uint8_t next_range_is_odd_timing = (pstate->cfg_stream_count) % 2; + + LOG_FUNCTION_START(""); + + + if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] == + VL53LX_ZONECONFIG_BINCONFIG__LOWAMB) { + if (!next_range_is_odd_timing) { + trace_print (VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting LOWAMB EVEN timing\n"); + phist_cfg->histogram_config__low_amb_even_bin_0_1 = + pmulti_hist->histogram_config__low_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_even_bin_2_3 = + pmulti_hist->histogram_config__low_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_even_bin_4_5 = + pmulti_hist->histogram_config__low_amb_even_bin_4_5; + } + + if (next_range_is_odd_timing) { + trace_print (VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting LOWAMB ODD timing\n"); + phist_cfg->histogram_config__low_amb_odd_bin_0_1 = + pmulti_hist->histogram_config__low_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_odd_bin_2_3 = + pmulti_hist->histogram_config__low_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_odd_bin_4_5 = + pmulti_hist->histogram_config__low_amb_even_bin_4_5; + } + } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] == + VL53LX_ZONECONFIG_BINCONFIG__MIDAMB) { + trace_print (VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting MIDAMB timing\n"); + if (!next_range_is_odd_timing) { + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting MIDAMB EVEN timing\n"); + phist_cfg->histogram_config__low_amb_even_bin_0_1 = + pmulti_hist->histogram_config__mid_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_even_bin_2_3 = + pmulti_hist->histogram_config__mid_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_even_bin_4_5 = + pmulti_hist->histogram_config__mid_amb_even_bin_4_5; + } + + if (next_range_is_odd_timing) { + trace_print (VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting MIDAMB ODD timing\n"); + phist_cfg->histogram_config__low_amb_odd_bin_0_1 = + pmulti_hist->histogram_config__mid_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_odd_bin_2_3 = + pmulti_hist->histogram_config__mid_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_odd_bin_4_5 = + pmulti_hist->histogram_config__mid_amb_even_bin_4_5; + } + } else if (pzone_cfg->bin_config[pdev->ll_state.cfg_zone_id] == + VL53LX_ZONECONFIG_BINCONFIG__HIGHAMB) { + if (!next_range_is_odd_timing) { + trace_print (VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting HIGHAMB EVEN timing\n" + ); + phist_cfg->histogram_config__low_amb_even_bin_0_1 = + pmulti_hist->histogram_config__high_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_even_bin_2_3 = + pmulti_hist->histogram_config__high_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_even_bin_4_5 = + pmulti_hist->histogram_config__high_amb_even_bin_4_5; + } + + if (next_range_is_odd_timing) { + trace_print (VL53LX_TRACE_LEVEL_DEBUG, + " HISTBINCONFIGUPDATE: Setting HIGHAMB ODD timing\n"); + phist_cfg->histogram_config__low_amb_odd_bin_0_1 = + pmulti_hist->histogram_config__high_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_odd_bin_2_3 = + pmulti_hist->histogram_config__high_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_odd_bin_4_5 = + pmulti_hist->histogram_config__high_amb_even_bin_4_5; + } + } + + + + if (status == VL53LX_ERROR_NONE) { + VL53LX_copy_hist_bins_to_static_cfg( + phist_cfg, + &(pdev->stat_cfg), + &(pdev->tim_cfg)); + } + + LOG_FUNCTION_END(status); + + return status; +} + + + +VL53LX_Error VL53LX_update_internal_stream_counters( + VL53LX_DEV Dev, + uint8_t external_stream_count, + uint8_t *pinternal_stream_count, + uint8_t *pinternal_stream_count_val) +{ + + VL53LX_Error status = VL53LX_ERROR_NONE; + uint8_t stream_divider; + + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + stream_divider = pdev->gen_cfg.global_config__stream_divider; + + if (stream_divider == 0) { + + + *pinternal_stream_count = external_stream_count; + + } else if (*pinternal_stream_count_val == (stream_divider-1)) { + + + if (*pinternal_stream_count == 0xFF) + *pinternal_stream_count = 0x80; + else + *pinternal_stream_count = *pinternal_stream_count + 1; + + + *pinternal_stream_count_val = 0; + + } else { + + + *pinternal_stream_count_val = *pinternal_stream_count_val + 1; + } + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + "UPDINTSTREAMCOUNT internal_steam_count: %d,", + *pinternal_stream_count); + + trace_print(VL53LX_TRACE_LEVEL_DEBUG, + "internal_stream_count_val: %d, divider: %d\n", + *pinternal_stream_count_val, + stream_divider); + + LOG_FUNCTION_END(status); + + return status; +} + + + +VL53LX_Error VL53LX_set_histogram_multizone_initial_bin_config( + VL53LX_zone_config_t *pzone_cfg, + VL53LX_histogram_config_t *phist_cfg, + VL53LX_histogram_config_t *pmulti_hist) +{ + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + + if (pzone_cfg->bin_config[0] == + VL53LX_ZONECONFIG_BINCONFIG__LOWAMB) { + phist_cfg->histogram_config__low_amb_even_bin_0_1 = + pmulti_hist->histogram_config__low_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_even_bin_2_3 = + pmulti_hist->histogram_config__low_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_even_bin_4_5 = + pmulti_hist->histogram_config__low_amb_even_bin_4_5; + + phist_cfg->histogram_config__low_amb_odd_bin_0_1 = + pmulti_hist->histogram_config__low_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_odd_bin_2_3 = + pmulti_hist->histogram_config__low_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_odd_bin_4_5 = + pmulti_hist->histogram_config__low_amb_even_bin_4_5; + } else if (pzone_cfg->bin_config[0] == + VL53LX_ZONECONFIG_BINCONFIG__MIDAMB) { + phist_cfg->histogram_config__low_amb_even_bin_0_1 = + pmulti_hist->histogram_config__mid_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_even_bin_2_3 = + pmulti_hist->histogram_config__mid_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_even_bin_4_5 = + pmulti_hist->histogram_config__mid_amb_even_bin_4_5; + + phist_cfg->histogram_config__low_amb_odd_bin_0_1 = + pmulti_hist->histogram_config__mid_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_odd_bin_2_3 = + pmulti_hist->histogram_config__mid_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_odd_bin_4_5 = + pmulti_hist->histogram_config__mid_amb_even_bin_4_5; + } else if (pzone_cfg->bin_config[0] == + VL53LX_ZONECONFIG_BINCONFIG__HIGHAMB) { + phist_cfg->histogram_config__low_amb_even_bin_0_1 = + pmulti_hist->histogram_config__high_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_even_bin_2_3 = + pmulti_hist->histogram_config__high_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_even_bin_4_5 = + pmulti_hist->histogram_config__high_amb_even_bin_4_5; + phist_cfg->histogram_config__low_amb_odd_bin_0_1 = + pmulti_hist->histogram_config__high_amb_even_bin_0_1; + phist_cfg->histogram_config__low_amb_odd_bin_2_3 = + pmulti_hist->histogram_config__high_amb_even_bin_2_3; + phist_cfg->histogram_config__low_amb_odd_bin_4_5 = + pmulti_hist->histogram_config__high_amb_even_bin_4_5; + } + + LOG_FUNCTION_END(status); + return status; +} + + + +uint8_t VL53LX_encode_GPIO_interrupt_config( + VL53LX_GPIO_interrupt_config_t *pintconf) +{ + uint8_t system__interrupt_config; + + system__interrupt_config = pintconf->intr_mode_distance; + system__interrupt_config |= ((pintconf->intr_mode_rate) << 2); + system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5); + system__interrupt_config |= ((pintconf->intr_no_target) << 6); + system__interrupt_config |= ((pintconf->intr_combined_mode) << 7); + + return system__interrupt_config; +} + + + +VL53LX_GPIO_interrupt_config_t VL53LX_decode_GPIO_interrupt_config( + uint8_t system__interrupt_config) +{ + VL53LX_GPIO_interrupt_config_t intconf; + + intconf.intr_mode_distance = system__interrupt_config & 0x03; + intconf.intr_mode_rate = (system__interrupt_config >> 2) & 0x03; + intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01; + intconf.intr_no_target = (system__interrupt_config >> 6) & 0x01; + intconf.intr_combined_mode = (system__interrupt_config >> 7) & 0x01; + + + intconf.threshold_rate_low = 0; + intconf.threshold_rate_high = 0; + intconf.threshold_distance_low = 0; + intconf.threshold_distance_high = 0; + + return intconf; +} + + + +VL53LX_Error VL53LX_set_GPIO_distance_threshold( + VL53LX_DEV Dev, + uint16_t threshold_high, + uint16_t threshold_low) +{ + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + pdev->dyn_cfg.system__thresh_high = threshold_high; + pdev->dyn_cfg.system__thresh_low = threshold_low; + + LOG_FUNCTION_END(status); + return status; +} + + + +VL53LX_Error VL53LX_set_GPIO_rate_threshold( + VL53LX_DEV Dev, + uint16_t threshold_high, + uint16_t threshold_low) +{ + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + pdev->gen_cfg.system__thresh_rate_high = threshold_high; + pdev->gen_cfg.system__thresh_rate_low = threshold_low; + + LOG_FUNCTION_END(status); + return status; +} + + + +VL53LX_Error VL53LX_set_GPIO_thresholds_from_struct( + VL53LX_DEV Dev, + VL53LX_GPIO_interrupt_config_t *pintconf) +{ + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + status = VL53LX_set_GPIO_distance_threshold( + Dev, + pintconf->threshold_distance_high, + pintconf->threshold_distance_low); + + if (status == VL53LX_ERROR_NONE) { + status = + VL53LX_set_GPIO_rate_threshold( + Dev, + pintconf->threshold_rate_high, + pintconf->threshold_rate_low); + } + + LOG_FUNCTION_END(status); + return status; +} + + +VL53LX_Error VL53LX_set_ref_spad_char_config( + VL53LX_DEV Dev, + uint8_t vcsel_period_a, + uint32_t phasecal_timeout_us, + uint16_t total_rate_target_mcps, + uint16_t max_count_rate_rtn_limit_mcps, + uint16_t min_count_rate_rtn_limit_mcps, + uint16_t fast_osc_frequency) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + uint8_t buffer[2]; + + uint32_t macro_period_us = 0; + uint32_t timeout_mclks = 0; + + LOG_FUNCTION_START(""); + + + macro_period_us = + VL53LX_calc_macro_period_us( + fast_osc_frequency, + vcsel_period_a); + if (macro_period_us == 0) + macro_period_us = 1; + + + timeout_mclks = phasecal_timeout_us << 12; + timeout_mclks = timeout_mclks + (macro_period_us>>1); + timeout_mclks = timeout_mclks / macro_period_us; + + if (timeout_mclks > 0xFF) + pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF; + else + pdev->gen_cfg.phasecal_config__timeout_macrop = + (uint8_t)timeout_mclks; + + pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a; + + + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrByte( + Dev, + VL53LX_PHASECAL_CONFIG__TIMEOUT_MACROP, + pdev->gen_cfg.phasecal_config__timeout_macrop); + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrByte( + Dev, + VL53LX_RANGE_CONFIG__VCSEL_PERIOD_A, + pdev->tim_cfg.range_config__vcsel_period_a); + + + + buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a; + buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a; + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WriteMulti( + Dev, + VL53LX_SD_CONFIG__WOI_SD0, + buffer, + 2); + + + + pdev->customer.ref_spad_char__total_rate_target_mcps = + total_rate_target_mcps; + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrWord( + Dev, + VL53LX_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS, + total_rate_target_mcps); + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrWord( + Dev, + VL53LX_RANGE_CONFIG__SIGMA_THRESH, + max_count_rate_rtn_limit_mcps); + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrWord( + Dev, + VL53LX_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, + min_count_rate_rtn_limit_mcps); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_set_ssc_config( + VL53LX_DEV Dev, + VL53LX_ssc_config_t *pssc_cfg, + uint16_t fast_osc_frequency) +{ + + + VL53LX_Error status = VL53LX_ERROR_NONE; + uint8_t buffer[5]; + + uint32_t macro_period_us = 0; + uint16_t timeout_encoded = 0; + + LOG_FUNCTION_START(""); + + + macro_period_us = + VL53LX_calc_macro_period_us( + fast_osc_frequency, + pssc_cfg->VL53LX_p_005); + + + timeout_encoded = + VL53LX_calc_encoded_timeout( + pssc_cfg->timeout_us, + macro_period_us); + + + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrByte( + Dev, + VL53LX_CAL_CONFIG__VCSEL_START, + pssc_cfg->vcsel_start); + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrByte( + Dev, + VL53LX_GLOBAL_CONFIG__VCSEL_WIDTH, + pssc_cfg->vcsel_width); + + + + buffer[0] = (uint8_t)((timeout_encoded & 0x0000FF00) >> 8); + buffer[1] = (uint8_t) (timeout_encoded & 0x000000FF); + buffer[2] = pssc_cfg->VL53LX_p_005; + buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8); + buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps & 0x000000FF); + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WriteMulti( + Dev, + VL53LX_RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + buffer, + 5); + + + + buffer[0] = pssc_cfg->VL53LX_p_005; + buffer[1] = pssc_cfg->VL53LX_p_005; + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WriteMulti( + Dev, + VL53LX_SD_CONFIG__WOI_SD0, + buffer, + 2); + + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_WrByte( + Dev, + VL53LX_NVM_BIST__CTRL, + pssc_cfg->array_select); + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_get_spad_rate_data( + VL53LX_DEV Dev, + VL53LX_spad_rate_data_t *pspad_rates) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + int i = 0; + + uint8_t VL53LX_p_003[512]; + uint8_t *pdata = &VL53LX_p_003[0]; + + LOG_FUNCTION_START(""); + + + + if (status == VL53LX_ERROR_NONE) + status = VL53LX_disable_firmware(Dev); + + + + if (status == VL53LX_ERROR_NONE) + status = + VL53LX_ReadMulti( + Dev, + VL53LX_PRIVATE__PATCH_BASE_ADDR_RSLV, + pdata, + 512); + + + pdata = &VL53LX_p_003[0]; + for (i = 0; i < VL53LX_NO_OF_SPAD_ENABLES; i++) { + pspad_rates->rate_data[i] = + (uint16_t)VL53LX_decode_unsigned_integer(pdata, 2); + pdata += 2; + } + + + + pspad_rates->VL53LX_p_020 = VL53LX_NO_OF_SPAD_ENABLES; + pspad_rates->no_of_values = VL53LX_NO_OF_SPAD_ENABLES; + pspad_rates->fractional_bits = 15; + + + + if (status == VL53LX_ERROR_NONE) + status = VL53LX_enable_firmware(Dev); + + LOG_FUNCTION_END(status); + + return status; +} + + + +VL53LX_Error VL53LX_dynamic_xtalk_correction_calc_required_samples( + VL53LX_DEV Dev + ) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = VL53LXDevStructGetLLResultsHandle(Dev); + VL53LX_smudge_corrector_config_t *pconfig = + &(pdev->smudge_correct_config); + VL53LX_smudge_corrector_internals_t *pint = + &(pdev->smudge_corrector_internals); + + VL53LX_range_results_t *presults = &(pres->range_results); + VL53LX_range_data_t *pxmonitor = &(presults->xmonitor); + + uint32_t peak_duration_us = pxmonitor->peak_duration_us; + + uint64_t temp64a; + uint64_t temp64z; + + LOG_FUNCTION_START(""); + + temp64a = pxmonitor->VL53LX_p_017 + + pxmonitor->VL53LX_p_016; + if (peak_duration_us == 0) + peak_duration_us = 1000; + temp64a = do_division_u((temp64a * 1000), peak_duration_us); + temp64a = do_division_u((temp64a * 1000), peak_duration_us); + + temp64z = pconfig->noise_margin * pxmonitor->VL53LX_p_004; + if (temp64z == 0) + temp64z = 1; + temp64a = temp64a * 1000 * 256; + temp64a = do_division_u(temp64a, temp64z); + temp64a = temp64a * 1000 * 256; + temp64a = do_division_u(temp64a, temp64z); + + pint->required_samples = (uint32_t)temp64a; + + + if (pint->required_samples < 2) + pint->required_samples = 2; + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_dynamic_xtalk_correction_calc_new_xtalk( + VL53LX_DEV Dev, + uint32_t xtalk_offset_out, + VL53LX_smudge_corrector_config_t *pconfig, + VL53LX_smudge_corrector_data_t *pout, + uint8_t add_smudge, + uint8_t soft_update + ) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + int16_t x_gradient_scaler; + int16_t y_gradient_scaler; + uint32_t orig_xtalk_offset; + int16_t orig_x_gradient; + int16_t orig_y_gradient; + uint8_t histo_merge_nb; + uint8_t i; + int32_t itemp32; + uint32_t SmudgeFactor; + VL53LX_xtalk_config_t *pX = &(pdev->xtalk_cfg); + VL53LX_xtalk_calibration_results_t *pC = &(pdev->xtalk_cal); + uint32_t *pcpo; + uint32_t max, nXtalk, cXtalk; + + LOG_FUNCTION_START(""); + + + if (add_smudge == 1) { + pout->algo__crosstalk_compensation_plane_offset_kcps = + (uint32_t)xtalk_offset_out + + (uint32_t)pconfig->smudge_margin; + } else { + pout->algo__crosstalk_compensation_plane_offset_kcps = + (uint32_t)xtalk_offset_out; + } + + + orig_xtalk_offset = + pX->nvm_default__crosstalk_compensation_plane_offset_kcps; + + orig_x_gradient = + pX->nvm_default__crosstalk_compensation_x_plane_gradient_kcps; + + orig_y_gradient = + pX->nvm_default__crosstalk_compensation_y_plane_gradient_kcps; + + if (((pconfig->user_scaler_set == 0) || + (pconfig->scaler_calc_method == 1)) && + (pC->algo__crosstalk_compensation_plane_offset_kcps != 0)) { + + VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb); + + if (histo_merge_nb == 0) + histo_merge_nb = 1; + if (pdev->tuning_parms.tp_hist_merge != 1) + orig_xtalk_offset = + pC->algo__crosstalk_compensation_plane_offset_kcps; + else + orig_xtalk_offset = + pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1]; + + orig_x_gradient = + pC->algo__crosstalk_compensation_x_plane_gradient_kcps; + + orig_y_gradient = + pC->algo__crosstalk_compensation_y_plane_gradient_kcps; + } + + + if ((pconfig->user_scaler_set == 0) && (orig_x_gradient == 0)) + pout->gradient_zero_flag |= 0x01; + + if ((pconfig->user_scaler_set == 0) && (orig_y_gradient == 0)) + pout->gradient_zero_flag |= 0x02; + + + + if (orig_xtalk_offset == 0) + orig_xtalk_offset = 1; + + + + if (pconfig->user_scaler_set == 1) { + x_gradient_scaler = pconfig->x_gradient_scaler; + y_gradient_scaler = pconfig->y_gradient_scaler; + } else { + + x_gradient_scaler = (int16_t)do_division_s( + (((int32_t)orig_x_gradient) << 6), + orig_xtalk_offset); + pconfig->x_gradient_scaler = x_gradient_scaler; + y_gradient_scaler = (int16_t)do_division_s( + (((int32_t)orig_y_gradient) << 6), + orig_xtalk_offset); + pconfig->y_gradient_scaler = y_gradient_scaler; + } + + + + if (pconfig->scaler_calc_method == 0) { + + + itemp32 = (int32_t)( + pout->algo__crosstalk_compensation_plane_offset_kcps * + x_gradient_scaler); + itemp32 = itemp32 >> 6; + if (itemp32 > 0xFFFF) + itemp32 = 0xFFFF; + + pout->algo__crosstalk_compensation_x_plane_gradient_kcps = + (int16_t)itemp32; + + itemp32 = (int32_t)( + pout->algo__crosstalk_compensation_plane_offset_kcps * + y_gradient_scaler); + itemp32 = itemp32 >> 6; + if (itemp32 > 0xFFFF) + itemp32 = 0xFFFF; + + pout->algo__crosstalk_compensation_y_plane_gradient_kcps = + (int16_t)itemp32; + } else if (pconfig->scaler_calc_method == 1) { + + + itemp32 = (int32_t)(orig_xtalk_offset - + pout->algo__crosstalk_compensation_plane_offset_kcps); + itemp32 = (int32_t)(do_division_s(itemp32, 16)); + itemp32 = itemp32 << 2; + itemp32 = itemp32 + (int32_t)(orig_x_gradient); + if (itemp32 > 0xFFFF) + itemp32 = 0xFFFF; + + pout->algo__crosstalk_compensation_x_plane_gradient_kcps = + (int16_t)itemp32; + + itemp32 = (int32_t)(orig_xtalk_offset - + pout->algo__crosstalk_compensation_plane_offset_kcps); + itemp32 = (int32_t)(do_division_s(itemp32, 80)); + itemp32 = itemp32 << 2; + itemp32 = itemp32 + (int32_t)(orig_y_gradient); + if (itemp32 > 0xFFFF) + itemp32 = 0xFFFF; + + pout->algo__crosstalk_compensation_y_plane_gradient_kcps = + (int16_t)itemp32; + } + + + if ((pconfig->smudge_corr_apply_enabled == 1) && + (soft_update != 1)) { + + pout->new_xtalk_applied_flag = 1; + nXtalk = pout->algo__crosstalk_compensation_plane_offset_kcps; + + VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb); + max = pdev->tuning_parms.tp_hist_merge_max_size; + pcpo = &(pC->algo__xtalk_cpo_HistoMerge_kcps[0]); + if ((histo_merge_nb > 0) && + (pdev->tuning_parms.tp_hist_merge == 1) && + (nXtalk != 0)) { + cXtalk = + pC->algo__xtalk_cpo_HistoMerge_kcps[histo_merge_nb-1]; + SmudgeFactor = cXtalk * 1000 / nXtalk; + if (SmudgeFactor >= pconfig->max_smudge_factor) + pout->new_xtalk_applied_flag = 0; + else if (SmudgeFactor > 0) + for (i = 0; i < max; i++) { + *pcpo *= 1000; + *pcpo /= SmudgeFactor; + pcpo++; + } + } + if (pout->new_xtalk_applied_flag) { + + pX->algo__crosstalk_compensation_plane_offset_kcps = + pout->algo__crosstalk_compensation_plane_offset_kcps; + pX->algo__crosstalk_compensation_x_plane_gradient_kcps = + pout->algo__crosstalk_compensation_x_plane_gradient_kcps; + pX->algo__crosstalk_compensation_y_plane_gradient_kcps = + pout->algo__crosstalk_compensation_y_plane_gradient_kcps; + + if (pconfig->smudge_corr_single_apply == 1) { + + pconfig->smudge_corr_apply_enabled = 0; + pconfig->smudge_corr_single_apply = 0; + } + } + } + + + if (soft_update != 1) + pout->smudge_corr_valid = 1; + + LOG_FUNCTION_END(status); + + return status; +} + +#define CONT_CONTINUE 0 +#define CONT_NEXT_LOOP 1 +#define CONT_RESET 2 +VL53LX_Error VL53LX_dynamic_xtalk_correction_corrector( + VL53LX_DEV Dev + ) +{ + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = VL53LXDevStructGetLLResultsHandle(Dev); + VL53LX_smudge_corrector_config_t *pconfig = + &(pdev->smudge_correct_config); + VL53LX_smudge_corrector_internals_t *pint = + &(pdev->smudge_corrector_internals); + VL53LX_smudge_corrector_data_t *pout = + &(pres->range_results.smudge_corrector_data); + VL53LX_range_results_t *pR = &(pres->range_results); + VL53LX_xtalk_config_t *pX = &(pdev->xtalk_cfg); + + uint8_t run_smudge_detection = 0; + uint8_t merging_complete = 0; + uint8_t run_nodetect = 0; + uint8_t ambient_check = 0; + int32_t itemp32 = 0; + uint64_t utemp64 = 0; + uint8_t continue_processing = CONT_CONTINUE; + uint32_t xtalk_offset_out = 0; + uint32_t xtalk_offset_in = 0; + uint32_t current_xtalk = 0; + uint32_t smudge_margin_adjusted = 0; + uint8_t i = 0; + uint8_t nodetect_index = 0; + uint16_t amr; + uint32_t cco; + uint8_t histo_merge_nb; + + + LOG_FUNCTION_START(""); + + VL53LX_compute_histo_merge_nb(Dev, &histo_merge_nb); + if ((histo_merge_nb == 0) || + (pdev->tuning_parms.tp_hist_merge != 1)) + histo_merge_nb = 1; + + + VL53LX_dynamic_xtalk_correction_output_init(pres); + + + ambient_check = (pconfig->smudge_corr_ambient_threshold == 0) || + ((pconfig->smudge_corr_ambient_threshold * histo_merge_nb) > + ((uint32_t)pR->xmonitor.ambient_count_rate_mcps)); + + + merging_complete = + ((pdev->tuning_parms.tp_hist_merge != 1) || + (histo_merge_nb == pdev->tuning_parms.tp_hist_merge_max_size)); + run_smudge_detection = + (pconfig->smudge_corr_enabled == 1) && + ambient_check && + (pR->xmonitor.range_status + == VL53LX_DEVICEERROR_RANGECOMPLETE) && + merging_complete; + + + if ((pR->xmonitor.range_status + != VL53LX_DEVICEERROR_RANGECOMPLETE) && + (pconfig->smudge_corr_enabled == 1)) { + + run_nodetect = 2; + for (i = 0; i < pR->active_results; i++) { + if (pR->VL53LX_p_003[i].range_status == + VL53LX_DEVICEERROR_RANGECOMPLETE) { + if (pR->VL53LX_p_003[i].median_range_mm + <= + pconfig->nodetect_min_range_mm) { + run_nodetect = 0; + } else { + if (run_nodetect == 2) { + run_nodetect = 1; + nodetect_index = i; + } + } + } + } + + if (run_nodetect == 2) + + run_nodetect = 0; + + amr = + pR->VL53LX_p_003[nodetect_index].ambient_count_rate_mcps; + + if (run_nodetect == 1) { + + + + + utemp64 = 1000 * ((uint64_t)amr); + + + utemp64 = utemp64 << 9; + + + if (utemp64 < pconfig->nodetect_ambient_threshold) + run_nodetect = 1; + else + run_nodetect = 0; + + } + } + + + if (run_smudge_detection) { + + pint->nodetect_counter = 0; + + + VL53LX_dynamic_xtalk_correction_calc_required_samples(Dev); + + + xtalk_offset_in = + pR->xmonitor.VL53LX_p_009; + + + cco = pX->algo__crosstalk_compensation_plane_offset_kcps; + current_xtalk = ((uint32_t)cco) << 2; + + + smudge_margin_adjusted = + ((uint32_t)(pconfig->smudge_margin)) << 2; + + + itemp32 = xtalk_offset_in - current_xtalk + + smudge_margin_adjusted; + + if (itemp32 < 0) + itemp32 = itemp32 * (-1); + + + if (itemp32 > ((int32_t)pconfig->single_xtalk_delta)) { + if ((int32_t)xtalk_offset_in > + ((int32_t)current_xtalk - + (int32_t)smudge_margin_adjusted)) { + pout->single_xtalk_delta_flag = 1; + } else { + pout->single_xtalk_delta_flag = 2; + } + } + + + pint->current_samples = pint->current_samples + 1; + + + if (pint->current_samples > pconfig->sample_limit) { + pout->sample_limit_exceeded_flag = 1; + continue_processing = CONT_RESET; + } else { + pint->accumulator = pint->accumulator + + xtalk_offset_in; + } + + if (pint->current_samples < pint->required_samples) + continue_processing = CONT_NEXT_LOOP; + + + xtalk_offset_out = + (uint32_t)(do_division_u(pint->accumulator, + pint->current_samples)); + + + itemp32 = xtalk_offset_out - current_xtalk + + smudge_margin_adjusted; + + if (itemp32 < 0) + itemp32 = itemp32 * (-1); + + if (continue_processing == CONT_CONTINUE && + (itemp32 >= ((int32_t)(pconfig->averaged_xtalk_delta))) + ) { + if ((int32_t)xtalk_offset_out > + ((int32_t)current_xtalk - + (int32_t)smudge_margin_adjusted)) + pout->averaged_xtalk_delta_flag = 1; + else + pout->averaged_xtalk_delta_flag = 2; + } + + if (continue_processing == CONT_CONTINUE && + (itemp32 < ((int32_t)(pconfig->averaged_xtalk_delta))) + ) + + continue_processing = CONT_RESET; + + + + pout->smudge_corr_clipped = 0; + if ((continue_processing == CONT_CONTINUE) && + (pconfig->smudge_corr_clip_limit != 0)) { + if (xtalk_offset_out > + (pconfig->smudge_corr_clip_limit * histo_merge_nb)) { + pout->smudge_corr_clipped = 1; + continue_processing = CONT_RESET; + } + } + + + + if (pconfig->user_xtalk_offset_limit_hi && + (xtalk_offset_out > + pconfig->user_xtalk_offset_limit)) + xtalk_offset_out = + pconfig->user_xtalk_offset_limit; + + + + if ((pconfig->user_xtalk_offset_limit_hi == 0) && + (xtalk_offset_out < + pconfig->user_xtalk_offset_limit)) + xtalk_offset_out = + pconfig->user_xtalk_offset_limit; + + + + xtalk_offset_out = xtalk_offset_out >> 2; + if (xtalk_offset_out > 0x3FFFF) + xtalk_offset_out = 0x3FFFF; + + + if (continue_processing == CONT_CONTINUE) { + + VL53LX_dynamic_xtalk_correction_calc_new_xtalk( + Dev, + xtalk_offset_out, + pconfig, + pout, + 1, + 0 + ); + + + continue_processing = CONT_RESET; + } else { + + VL53LX_dynamic_xtalk_correction_calc_new_xtalk( + Dev, + xtalk_offset_out, + pconfig, + pout, + 1, + 1 + ); + } + + + if (continue_processing == CONT_RESET) { + pint->accumulator = 0; + pint->current_samples = 0; + pint->nodetect_counter = 0; + } + + } + + continue_processing = CONT_CONTINUE; + if (run_nodetect == 1) { + + pint->nodetect_counter += 1; + + + if (pint->nodetect_counter < pconfig->nodetect_sample_limit) + continue_processing = CONT_NEXT_LOOP; + + + xtalk_offset_out = (uint32_t)(pconfig->nodetect_xtalk_offset); + + if (continue_processing == CONT_CONTINUE) { + + VL53LX_dynamic_xtalk_correction_calc_new_xtalk( + Dev, + xtalk_offset_out, + pconfig, + pout, + 0, + 0 + ); + + + pout->smudge_corr_valid = 2; + + + continue_processing = CONT_RESET; + } else { + + VL53LX_dynamic_xtalk_correction_calc_new_xtalk( + Dev, + xtalk_offset_out, + pconfig, + pout, + 0, + 1 + ); + } + + + if (continue_processing == CONT_RESET) { + pint->accumulator = 0; + pint->current_samples = 0; + pint->nodetect_counter = 0; + } + } + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_dynamic_xtalk_correction_data_init( + VL53LX_DEV Dev + ) +{ + + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_LLDriverResults_t *pres = VL53LXDevStructGetLLResultsHandle(Dev); + + LOG_FUNCTION_START(""); + + + + pdev->smudge_correct_config.smudge_corr_enabled = 1; + pdev->smudge_correct_config.smudge_corr_apply_enabled = 1; + pdev->smudge_correct_config.smudge_corr_single_apply = + VL53LX_TUNINGPARM_DYNXTALK_SMUDGE_COR_SINGLE_APPLY_DEFAULT; + + pdev->smudge_correct_config.smudge_margin = + VL53LX_TUNINGPARM_DYNXTALK_SMUDGE_MARGIN_DEFAULT; + pdev->smudge_correct_config.noise_margin = + VL53LX_TUNINGPARM_DYNXTALK_NOISE_MARGIN_DEFAULT; + pdev->smudge_correct_config.user_xtalk_offset_limit = + VL53LX_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_DEFAULT; + pdev->smudge_correct_config.user_xtalk_offset_limit_hi = + VL53LX_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_HI_DEFAULT; + pdev->smudge_correct_config.sample_limit = + VL53LX_TUNINGPARM_DYNXTALK_SAMPLE_LIMIT_DEFAULT; + pdev->smudge_correct_config.single_xtalk_delta = + VL53LX_TUNINGPARM_DYNXTALK_SINGLE_XTALK_DELTA_DEFAULT; + pdev->smudge_correct_config.averaged_xtalk_delta = + VL53LX_TUNINGPARM_DYNXTALK_AVERAGED_XTALK_DELTA_DEFAULT; + pdev->smudge_correct_config.smudge_corr_clip_limit = + VL53LX_TUNINGPARM_DYNXTALK_CLIP_LIMIT_DEFAULT; + pdev->smudge_correct_config.smudge_corr_ambient_threshold = + VL53LX_TUNINGPARM_DYNXTALK_XTALK_AMB_THRESHOLD_DEFAULT; + pdev->smudge_correct_config.scaler_calc_method = + 0; + pdev->smudge_correct_config.x_gradient_scaler = + VL53LX_TUNINGPARM_DYNXTALK_XGRADIENT_SCALER_DEFAULT; + pdev->smudge_correct_config.y_gradient_scaler = + VL53LX_TUNINGPARM_DYNXTALK_YGRADIENT_SCALER_DEFAULT; + pdev->smudge_correct_config.user_scaler_set = + VL53LX_TUNINGPARM_DYNXTALK_USER_SCALER_SET_DEFAULT; + pdev->smudge_correct_config.nodetect_ambient_threshold = + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_AMB_THRESHOLD_KCPS_DEFAULT; + pdev->smudge_correct_config.nodetect_sample_limit = + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_SAMPLE_LIMIT_DEFAULT; + pdev->smudge_correct_config.nodetect_xtalk_offset = + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS_DEFAULT; + pdev->smudge_correct_config.nodetect_min_range_mm = + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_MIN_RANGE_MM_DEFAULT; + pdev->smudge_correct_config.max_smudge_factor = + VL53LX_TUNINGPARM_DYNXTALK_MAX_SMUDGE_FACTOR_DEFAULT; + + + pdev->smudge_corrector_internals.current_samples = 0; + pdev->smudge_corrector_internals.required_samples = 0; + pdev->smudge_corrector_internals.accumulator = 0; + pdev->smudge_corrector_internals.nodetect_counter = 0; + + + VL53LX_dynamic_xtalk_correction_output_init(pres); + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_dynamic_xtalk_correction_output_init( + VL53LX_LLDriverResults_t *pres + ) +{ + + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_smudge_corrector_data_t *pdata; + + LOG_FUNCTION_START(""); + + + pdata = &(pres->range_results.smudge_corrector_data); + + pdata->smudge_corr_valid = 0; + pdata->smudge_corr_clipped = 0; + pdata->single_xtalk_delta_flag = 0; + pdata->averaged_xtalk_delta_flag = 0; + pdata->sample_limit_exceeded_flag = 0; + pdata->gradient_zero_flag = 0; + pdata->new_xtalk_applied_flag = 0; + + pdata->algo__crosstalk_compensation_plane_offset_kcps = 0; + pdata->algo__crosstalk_compensation_x_plane_gradient_kcps = 0; + pdata->algo__crosstalk_compensation_y_plane_gradient_kcps = 0; + + LOG_FUNCTION_END(status); + + return status; +} + + +VL53LX_Error VL53LX_xtalk_cal_data_init( + VL53LX_DEV Dev + ) +{ + + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + + + pdev->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps = 0; + pdev->xtalk_cal.algo__crosstalk_compensation_x_plane_gradient_kcps = 0; + pdev->xtalk_cal.algo__crosstalk_compensation_y_plane_gradient_kcps = 0; + memset(&pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps[0], 0, + sizeof(pdev->xtalk_cal.algo__xtalk_cpo_HistoMerge_kcps)); + + LOG_FUNCTION_END(status); + + return status; +} + + + + + + +VL53LX_Error VL53LX_low_power_auto_data_init( + VL53LX_DEV Dev + ) +{ + + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + pdev->low_power_auto_data.vhv_loop_bound = + VL53LX_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT; + pdev->low_power_auto_data.is_low_power_auto_mode = 0; + pdev->low_power_auto_data.low_power_auto_range_count = 0; + pdev->low_power_auto_data.saved_interrupt_config = 0; + pdev->low_power_auto_data.saved_vhv_init = 0; + pdev->low_power_auto_data.saved_vhv_timeout = 0; + pdev->low_power_auto_data.first_run_phasecal_result = 0; + pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0; + pdev->low_power_auto_data.dss__required_spads = 0; + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_low_power_auto_data_stop_range( + VL53LX_DEV Dev + ) +{ + + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + + + pdev->low_power_auto_data.low_power_auto_range_count = 0xFF; + + pdev->low_power_auto_data.first_run_phasecal_result = 0; + pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0; + pdev->low_power_auto_data.dss__required_spads = 0; + + + if (pdev->low_power_auto_data.saved_vhv_init != 0) + pdev->stat_nvm.vhv_config__init = + pdev->low_power_auto_data.saved_vhv_init; + if (pdev->low_power_auto_data.saved_vhv_timeout != 0) + pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = + pdev->low_power_auto_data.saved_vhv_timeout; + + + pdev->gen_cfg.phasecal_config__override = 0x00; + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_config_low_power_auto_mode( + VL53LX_general_config_t *pgeneral, + VL53LX_dynamic_config_t *pdynamic, + VL53LX_low_power_auto_data_t *plpadata + ) +{ + + + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + + plpadata->is_low_power_auto_mode = 1; + + + plpadata->low_power_auto_range_count = 0; + + + pdynamic->system__sequence_config = + VL53LX_SEQUENCE_VHV_EN | + VL53LX_SEQUENCE_PHASECAL_EN | + VL53LX_SEQUENCE_DSS1_EN | + + + + VL53LX_SEQUENCE_RANGE_EN; + + + pgeneral->dss_config__manual_effective_spads_select = 200 << 8; + pgeneral->dss_config__roi_mode_control = + VL53LX_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_low_power_auto_setup_manual_calibration( + VL53LX_DEV Dev) +{ + + + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + + pdev->low_power_auto_data.saved_vhv_init = + pdev->stat_nvm.vhv_config__init; + pdev->low_power_auto_data.saved_vhv_timeout = + pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound; + + + pdev->stat_nvm.vhv_config__init &= 0x7F; + + pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = + (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) + + (pdev->low_power_auto_data.vhv_loop_bound << 2); + + pdev->gen_cfg.phasecal_config__override = 0x01; + pdev->low_power_auto_data.first_run_phasecal_result = + pdev->dbg_results.phasecal_result__vcsel_start; + pdev->gen_cfg.cal_config__vcsel_start = + pdev->low_power_auto_data.first_run_phasecal_result; + + LOG_FUNCTION_END(status); + + return status; +} + +VL53LX_Error VL53LX_low_power_auto_update_DSS( + VL53LX_DEV Dev) +{ + + + + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + VL53LX_system_results_t *pS = &(pdev->sys_results); + + + VL53LX_Error status = VL53LX_ERROR_NONE; + + uint32_t utemp32a; + + LOG_FUNCTION_START(""); + + + + + utemp32a = + pS->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 + + pS->result__ambient_count_rate_mcps_sd0; + + + if (utemp32a > 0xFFFF) + utemp32a = 0xFFFF; + + + + utemp32a = utemp32a << 16; + + + if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0) + status = VL53LX_ERROR_DIVISION_BY_ZERO; + else { + + utemp32a = utemp32a / + pdev->sys_results.result__dss_actual_effective_spads_sd0; + + pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = + utemp32a; + + + utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps << + 16; + + + if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps + == 0) + status = VL53LX_ERROR_DIVISION_BY_ZERO; + else { + + utemp32a = utemp32a / + pdev->low_power_auto_data.dss__total_rate_per_spad_mcps; + + + if (utemp32a > 0xFFFF) + utemp32a = 0xFFFF; + + + pdev->low_power_auto_data.dss__required_spads = + (uint16_t)utemp32a; + + + pdev->gen_cfg.dss_config__manual_effective_spads_select + = pdev->low_power_auto_data.dss__required_spads; + pdev->gen_cfg.dss_config__roi_mode_control = + VL53LX_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; + } + + } + + if (status == VL53LX_ERROR_DIVISION_BY_ZERO) { + + + + pdev->low_power_auto_data.dss__required_spads = 0x8000; + + + pdev->gen_cfg.dss_config__manual_effective_spads_select = + pdev->low_power_auto_data.dss__required_spads; + pdev->gen_cfg.dss_config__roi_mode_control = + VL53LX_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; + + + status = VL53LX_ERROR_NONE; + } + + LOG_FUNCTION_END(status); + + return status; +} + + + + +VL53LX_Error VL53LX_compute_histo_merge_nb( + VL53LX_DEV Dev, uint8_t *histo_merge_nb) +{ + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_Error status = VL53LX_ERROR_NONE; + uint8_t i, timing; + uint8_t sum = 0; + + timing = (pdev->hist_data.bin_seq[0] == 7 ? 1 : 0); + for (i = 0; i < VL53LX_BIN_REC_SIZE; i++) + if (pdev->multi_bins_rec[i][timing][7] > 0) + sum++; + *histo_merge_nb = sum; + + return status; +} +