ST Expansion SW Team
The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2
Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.
src/vl53l1_core.c
- Committer:
- lugandc
- Date:
- 2021-07-21
- Revision:
- 18:0696efe39d08
- Parent:
- 7:1add29d51e72
File content as of revision 18:0696efe39d08:
// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
/******************************************************************************
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
This file is part of VL53L1 and is dual licensed,
either GPL-2.0+
or 'BSD 3-clause "New" or "Revised" License' , at your option.
******************************************************************************
*/
#include "vl53l1_ll_def.h"
#include "vl53l1_ll_device.h"
#include "vl53l1_platform.h"
#include "vl53l1_register_map.h"
#include "vl53l1_register_funcs.h"
#include "vl53l1_register_settings.h"
#include "vl53l1_hist_structs.h"
#include "vl53l1_api_preset_modes.h"
#include "vl53l1_core.h"
#include "vl53l1_tuning_parm_defaults.h"
#define LOG_FUNCTION_START(fmt, ...) \
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \
status, fmt, ##__VA_ARGS__)
#define trace_print(level, ...) \
_LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
void VL53L1_init_version(
VL53L1_DEV Dev)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
pdev->version.ll_major = VL53L1_LL_API_IMPLEMENTATION_VER_MAJOR;
pdev->version.ll_minor = VL53L1_LL_API_IMPLEMENTATION_VER_MINOR;
pdev->version.ll_build = VL53L1_LL_API_IMPLEMENTATION_VER_SUB;
pdev->version.ll_revision = VL53L1_LL_API_IMPLEMENTATION_VER_REVISION;
}
void VL53L1_init_ll_driver_state(
VL53L1_DEV Dev,
VL53L1_DeviceState device_state)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
pstate->cfg_device_state = device_state;
pstate->cfg_stream_count = 0;
pstate->cfg_gph_id = VL53L1_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 = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
pstate->rd_timing_status = 0;
pstate->rd_zone_id = 0;
}
VL53L1_Error VL53L1_update_ll_driver_rd_state(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
LOG_FUNCTION_START("");
if ((pdev->sys_ctrl.system__mode_start &
VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
pstate->rd_device_state = VL53L1_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 = VL53L1_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 = VL53L1_update_internal_stream_counters(Dev,
pstate->rd_stream_count,
&(pstate->rd_internal_stream_count),
&(pstate->rd_internal_stream_count_val));
pstate->rd_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
switch (pstate->rd_device_state) {
case VL53L1_DEVICESTATE_SW_STANDBY:
if ((pdev->dyn_cfg.system__grouped_parameter_hold &
VL53L1_GROUPEDPARAMETERHOLD_ID_MASK) > 0) {
pstate->rd_device_state =
VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC;
} else {
if (pstate->rd_zone_id >=
pdev->zone_cfg.active_zones)
pstate->rd_device_state =
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
else
pstate->rd_device_state =
VL53L1_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 VL53L1_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 =
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
else
pstate->rd_device_state =
VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
break;
case VL53L1_DEVICESTATE_RANGING_GATHER_DATA:
pstate->rd_zone_id++;
if (pstate->rd_zone_id >=
pdev->zone_cfg.active_zones)
pstate->rd_device_state =
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
else
pstate->rd_device_state =
VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
break;
case VL53L1_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 =
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
else
pstate->rd_device_state =
VL53L1_DEVICESTATE_RANGING_GATHER_DATA;
break;
default:
pstate->rd_device_state =
VL53L1_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 = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
pstate->rd_timing_status = 0;
pstate->rd_zone_id = 0;
break;
}
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_check_ll_driver_rd_state(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres =
VL53L1DevStructGetLLResultsHandle(Dev);
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
VL53L1_system_results_t *psys_results = &(pdev->sys_results);
VL53L1_histogram_bin_data_t *phist_data = &(pdev->hist_data);
VL53L1_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 &
VL53L1_RANGE_STATUS__RANGE_STATUS_MASK;
device_stream_count = psys_results->result__stream_count;
histogram_mode =
(pdev->sys_ctrl.system__mode_start &
VL53L1_DEVICESCHEDULERMODE_HISTOGRAM) ==
VL53L1_DEVICESCHEDULERMODE_HISTOGRAM;
device_gph_id = (psys_results->result__interrupt_status &
VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
if (histogram_mode)
device_gph_id = (phist_data->result__interrupt_status &
VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
if (!((pdev->sys_ctrl.system__mode_start &
VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) ==
VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK))
goto ENDFUNC;
if (pstate->rd_device_state ==
VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC) {
if (histogram_mode == 0) {
if (device_range_status !=
VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY)
status =
VL53L1_ERROR_GPH_SYNC_CHECK_FAIL;
}
} else {
if (pstate->rd_stream_count != device_stream_count)
status = VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL;
if (pstate->rd_gph_id != device_gph_id)
status = VL53L1_ERROR_GPH_ID_CHECK_FAIL;
expected_stream_count =
pZ->VL53L1_p_002[pstate->rd_zone_id].expected_stream_count;
expected_gph_id =
pZ->VL53L1_p_002[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 =
VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL;
}
if (expected_gph_id != device_gph_id)
status = VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL;
}
ENDFUNC:
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_update_ll_driver_cfg_state(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres =
VL53L1DevStructGetLLResultsHandle(Dev);
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
VL53L1_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 &
VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
pstate->cfg_device_state = VL53L1_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 = VL53L1_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 = VL53L1_update_internal_stream_counters(
Dev,
pstate->cfg_stream_count,
&(pstate->cfg_internal_stream_count),
&(pstate->cfg_internal_stream_count_val));
pstate->cfg_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
switch (pstate->cfg_device_state) {
case VL53L1_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 =
VL53L1_DEVICESTATE_RANGING_DSS_AUTO;
break;
case VL53L1_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 =
VL53L1_DEVICESTATE_RANGING_DSS_MANUAL;
}
}
break;
case VL53L1_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 =
VL53L1_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 =
VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
pstate->cfg_timing_status = 0;
pstate->cfg_zone_id = 0;
break;
}
}
if (pdev->zone_cfg.active_zones == 0) {
pZ->VL53L1_p_002[prev_cfg_zone_id].expected_stream_count
= prev_cfg_stream_count - 1;
pZ->VL53L1_p_002[pstate->rd_zone_id].expected_gph_id =
prev_cfg_gph_id ^ VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
} else {
pZ->VL53L1_p_002[prev_cfg_zone_id].expected_stream_count
= prev_cfg_stream_count;
pZ->VL53L1_p_002[prev_cfg_zone_id].expected_gph_id =
prev_cfg_gph_id;
}
LOG_FUNCTION_END(status);
return status;
}
void VL53L1_copy_rtn_good_spads_to_buffer(
VL53L1_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 VL53L1_init_system_results(
VL53L1_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,
VL53L1_zone_results_t *pdata)
{
uint8_t z = 0;
VL53L1_zone_objects_t *pobjects;
pdata->max_zones = VL53L1_MAX_USER_ZONES;
pdata->active_zones = active_zones;
for (z = 0; z < pdata->max_zones; z++) {
pobjects = &(pdata->VL53L1_p_002[z]);
pobjects->cfg_device_state = VL53L1_DEVICESTATE_SW_STANDBY;
pobjects->rd_device_state = VL53L1_DEVICESTATE_SW_STANDBY;
pobjects->max_objects = VL53L1_MAX_RANGE_RESULTS;
pobjects->active_objects = 0;
}
}
void V53L1_init_zone_dss_configs(
VL53L1_DEV Dev)
{
VL53L1_LLDriverResults_t *pres =
VL53L1DevStructGetLLResultsHandle(Dev);
uint8_t z = 0;
uint8_t max_zones = VL53L1_MAX_USER_ZONES;
VL53L1_zone_private_dyn_cfgs_t *pdata = &(pres->zone_dyn_cfgs);
for (z = 0; z < max_zones; z++) {
pdata->VL53L1_p_002[z].dss_mode =
VL53L1_DSS_CONTROL__MODE_TARGET_RATE;
pdata->VL53L1_p_002[z].dss_requested_effective_spad_count = 0;
}
}
void VL53L1_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,
VL53L1_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 VL53L1_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,
VL53L1_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 VL53L1_init_xtalk_bin_data_struct(
uint32_t bin_value,
uint16_t VL53L1_p_024,
VL53L1_xtalk_histogram_shape_t *pdata)
{
uint16_t i = 0;
pdata->zone_id = 0;
pdata->time_stamp = 0;
pdata->VL53L1_p_022 = 0;
pdata->VL53L1_p_023 = VL53L1_XTALK_HISTO_BINS;
pdata->VL53L1_p_024 = (uint8_t)VL53L1_p_024;
pdata->phasecal_result__reference_phase = 0;
pdata->phasecal_result__vcsel_start = 0;
pdata->cal_config__vcsel_start = 0;
pdata->vcsel_width = 0;
pdata->VL53L1_p_019 = 0;
pdata->zero_distance_phase = 0;
for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++) {
if (i < VL53L1_p_024)
pdata->bin_data[i] = bin_value;
else
pdata->bin_data[i] = 0;
}
}
void VL53L1_i2c_encode_uint16_t(
uint16_t ip_value,
uint16_t count,
uint8_t *pbuffer)
{
uint16_t i = 0;
uint16_t VL53L1_p_002 = 0;
VL53L1_p_002 = ip_value;
for (i = 0; i < count; i++) {
pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
VL53L1_p_002 = VL53L1_p_002 >> 8;
}
}
uint16_t VL53L1_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 VL53L1_i2c_encode_int16_t(
int16_t ip_value,
uint16_t count,
uint8_t *pbuffer)
{
uint16_t i = 0;
int16_t VL53L1_p_002 = 0;
VL53L1_p_002 = ip_value;
for (i = 0; i < count; i++) {
pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
VL53L1_p_002 = VL53L1_p_002 >> 8;
}
}
int16_t VL53L1_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 VL53L1_i2c_encode_uint32_t(
uint32_t ip_value,
uint16_t count,
uint8_t *pbuffer)
{
uint16_t i = 0;
uint32_t VL53L1_p_002 = 0;
VL53L1_p_002 = ip_value;
for (i = 0; i < count; i++) {
pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
VL53L1_p_002 = VL53L1_p_002 >> 8;
}
}
uint32_t VL53L1_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 VL53L1_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 VL53L1_i2c_encode_int32_t(
int32_t ip_value,
uint16_t count,
uint8_t *pbuffer)
{
uint16_t i = 0;
int32_t VL53L1_p_002 = 0;
VL53L1_p_002 = ip_value;
for (i = 0; i < count; i++) {
pbuffer[count-i-1] = (uint8_t)(VL53L1_p_002 & 0x00FF);
VL53L1_p_002 = VL53L1_p_002 >> 8;
}
}
int32_t VL53L1_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;
}
VL53L1_Error VL53L1_start_test(
VL53L1_DEV Dev,
uint8_t test_mode__ctrl)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if (status == VL53L1_ERROR_NONE) {
status = VL53L1_WrByte(
Dev,
VL53L1_TEST_MODE__CTRL,
test_mode__ctrl);
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_set_firmware_enable_register(
VL53L1_DEV Dev,
uint8_t value)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
pdev->sys_ctrl.firmware__enable = value;
status = VL53L1_WrByte(
Dev,
VL53L1_FIRMWARE__ENABLE,
pdev->sys_ctrl.firmware__enable);
return status;
}
VL53L1_Error VL53L1_enable_firmware(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
status = VL53L1_set_firmware_enable_register(Dev, 0x01);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_disable_firmware(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
status = VL53L1_set_firmware_enable_register(Dev, 0x00);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_set_powerforce_register(
VL53L1_DEV Dev,
uint8_t value)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
pdev->sys_ctrl.power_management__go1_power_force = value;
status = VL53L1_WrByte(
Dev,
VL53L1_POWER_MANAGEMENT__GO1_POWER_FORCE,
pdev->sys_ctrl.power_management__go1_power_force);
return status;
}
VL53L1_Error VL53L1_enable_powerforce(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
status = VL53L1_set_powerforce_register(Dev, 0x01);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_disable_powerforce(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
status = VL53L1_set_powerforce_register(Dev, 0x00);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_clear_interrupt(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
LOG_FUNCTION_START("");
pdev->sys_ctrl.system__interrupt_clear = VL53L1_CLEAR_RANGE_INT;
status = VL53L1_WrByte(
Dev,
VL53L1_SYSTEM__INTERRUPT_CLEAR,
pdev->sys_ctrl.system__interrupt_clear);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_force_shadow_stream_count_to_zero(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
if (status == VL53L1_ERROR_NONE)
status = VL53L1_disable_firmware(Dev);
if (status == VL53L1_ERROR_NONE) {
status = VL53L1_WrByte(
Dev,
VL53L1_SHADOW_RESULT__STREAM_COUNT,
0x00);
}
if (status == VL53L1_ERROR_NONE)
status = VL53L1_enable_firmware(Dev);
return status;
}
uint32_t VL53L1_calc_macro_period_us(
uint16_t fast_osc_frequency,
uint8_t VL53L1_p_009)
{
uint32_t pll_period_us = 0;
uint8_t VL53L1_p_031 = 0;
uint32_t macro_period_us = 0;
LOG_FUNCTION_START("");
pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency);
VL53L1_p_031 = VL53L1_decode_vcsel_period(VL53L1_p_009);
macro_period_us =
(uint32_t)VL53L1_MACRO_PERIOD_VCSEL_PERIODS *
pll_period_us;
macro_period_us = macro_period_us >> 6;
macro_period_us = macro_period_us * (uint32_t)VL53L1_p_031;
macro_period_us = macro_period_us >> 6;
LOG_FUNCTION_END(0);
return macro_period_us;
}
uint16_t VL53L1_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 VL53L1_calc_timeout_mclks(
uint32_t timeout_us,
uint32_t macro_period_us)
{
uint32_t timeout_mclks = 0;
LOG_FUNCTION_START("");
if (macro_period_us == 0)
timeout_mclks = 0;
else
timeout_mclks =
((timeout_us << 12) + (macro_period_us>>1)) /
macro_period_us;
LOG_FUNCTION_END(0);
return timeout_mclks;
}
uint16_t VL53L1_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 =
VL53L1_calc_timeout_mclks(timeout_us, macro_period_us);
timeout_encoded =
VL53L1_encode_timeout(timeout_mclks);
LOG_FUNCTION_END(0);
return timeout_encoded;
}
uint32_t VL53L1_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 VL53L1_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 VL53L1_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 =
VL53L1_decode_timeout(timeout_encoded);
timeout_us =
VL53L1_calc_timeout_us(timeout_mclks, macro_period_us);
LOG_FUNCTION_END(0);
return timeout_us;
}
uint16_t VL53L1_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 VL53L1_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;
}
VL53L1_Error VL53L1_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,
VL53L1_general_config_t *pgeneral,
VL53L1_timing_config_t *ptiming)
{
VL53L1_Error status = VL53L1_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 = VL53L1_ERROR_DIVISION_BY_ZERO;
} else {
macro_period_us =
VL53L1_calc_macro_period_us(
fast_osc_frequency,
ptiming->range_config__vcsel_period_a);
timeout_mclks =
VL53L1_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 =
VL53L1_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 =
VL53L1_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 =
VL53L1_calc_macro_period_us(
fast_osc_frequency,
ptiming->range_config__vcsel_period_b);
timeout_encoded =
VL53L1_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 = VL53L1_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 VL53L1_encode_vcsel_period(uint8_t VL53L1_p_031)
{
uint8_t vcsel_period_reg = 0;
vcsel_period_reg = (VL53L1_p_031 >> 1) - 1;
return vcsel_period_reg;
}
uint32_t VL53L1_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 VL53L1_encode_unsigned_integer(
uint32_t ip_value,
uint8_t no_of_bytes,
uint8_t *pbuffer)
{
uint8_t i = 0;
uint32_t VL53L1_p_002 = 0;
VL53L1_p_002 = ip_value;
for (i = 0; i < no_of_bytes; i++) {
pbuffer[no_of_bytes-i-1] = VL53L1_p_002 & 0x00FF;
VL53L1_p_002 = VL53L1_p_002 >> 8;
}
}
VL53L1_Error VL53L1_hist_copy_and_scale_ambient_info(
VL53L1_zone_hist_info_t *pidata,
VL53L1_histogram_bin_data_t *podata)
{
VL53L1_Error status = VL53L1_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 = VL53L1_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->VL53L1_p_004 =
podata->ambient_events_sum;
podata->VL53L1_p_004 +=
((int32_t)pidata->number_of_ambient_bins / 2);
podata->VL53L1_p_004 /=
(int32_t)pidata->number_of_ambient_bins;
}
}
LOG_FUNCTION_END(0);
return status;
}
void VL53L1_hist_get_bin_sequence_config(
VL53L1_DEV Dev,
VL53L1_histogram_bin_data_t *pdata)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(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 < VL53L1_MAX_BIN_SEQUENCE_LENGTH; i++)
pdata->bin_rep[i] = 1;
LOG_FUNCTION_END(0);
}
VL53L1_Error VL53L1_hist_phase_consistency_check(
VL53L1_DEV Dev,
VL53L1_zone_hist_info_t *phist_prev,
VL53L1_zone_objects_t *prange_prev,
VL53L1_range_results_t *prange_curr)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(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;
VL53L1_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 !=
VL53L1_DEVICESTATE_RANGING_GATHER_DATA &&
prange_prev->rd_device_state !=
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA)
return status;
if (phase_tolerance == 0)
return status;
for (lc = 0; lc < prange_curr->active_results; lc++) {
if (!((prange_curr->VL53L1_p_002[lc].range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE) ||
(prange_curr->VL53L1_p_002[lc].range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK)))
continue;
if (prange_prev->active_objects == 0)
prange_curr->VL53L1_p_002[lc].range_status =
VL53L1_DEVICEERROR_PREV_RANGE_NO_TARGETS;
else
prange_curr->VL53L1_p_002[lc].range_status =
VL53L1_DEVICEERROR_PHASECONSISTENCY;
for (p = 0; p < prange_prev->active_objects; p++) {
if (prange_curr->VL53L1_p_002[lc].VL53L1_p_014 >
prange_prev->VL53L1_p_002[p].VL53L1_p_014) {
phase_delta =
prange_curr->VL53L1_p_002[lc].VL53L1_p_014 -
prange_prev->VL53L1_p_002[p].VL53L1_p_014;
} else {
phase_delta =
prange_prev->VL53L1_p_002[p].VL53L1_p_014 -
prange_curr->VL53L1_p_002[lc].VL53L1_p_014;
}
if (phase_delta < phase_tolerance) {
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_hist_events_consistency_check(
event_sigma,
event_min_spad_count,
phist_prev,
&(prange_prev->VL53L1_p_002[p]),
&(prange_curr->VL53L1_p_002[lc]),
&events_tolerance,
&events_delta,
&range_status);
if (status == VL53L1_ERROR_NONE &&
range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE)
status =
VL53L1_hist_merged_pulse_check(
min_max_tolerance,
&(prange_curr->VL53L1_p_002[lc]),
&range_status);
prange_curr->VL53L1_p_002[lc].range_status =
range_status;
}
}
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_hist_events_consistency_check(
uint8_t event_sigma,
uint16_t min_effective_spad_count,
VL53L1_zone_hist_info_t *phist_prev,
VL53L1_object_data_t *prange_prev,
VL53L1_range_data_t *prange_curr,
int32_t *pevents_tolerance,
int32_t *pevents_delta,
VL53L1_DeviceError *prange_status)
{
VL53L1_Error status = VL53L1_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 = VL53L1_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->VL53L1_p_006;
events_scaler = tmpp * 4096;
events_scaler += (tmpc/2);
if (tmpc != 0)
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->VL53L1_p_021;
c_signal_events -= (int64_t)prange_curr->VL53L1_p_020;
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->VL53L1_p_021;
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->VL53L1_p_020;
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->VL53L1_p_020;
p_amb_noise_sq += 2;
p_amb_noise_sq /= 4;
noise_sq_sum =
(uint32_t)prange_prev->VL53L1_p_021 +
(uint32_t)c_sig_noise_sq +
(uint32_t)p_amb_noise_sq +
(uint32_t)c_amb_noise_sq;
*pevents_tolerance =
(int32_t)VL53L1_isqrt(noise_sq_sum * 16);
*pevents_tolerance *= (int32_t)event_sigma;
*pevents_tolerance += 32;
*pevents_tolerance /= 64;
p_signal_events = (int32_t)prange_prev->VL53L1_p_021;
p_signal_events -= (int32_t)prange_prev->VL53L1_p_020;
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->VL53L1_p_006 > min_effective_spad_count)
*prange_status = VL53L1_DEVICEERROR_EVENTCONSISTENCY;
else
*prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE;
return status;
}
VL53L1_Error VL53L1_hist_merged_pulse_check(
int16_t min_max_tolerance_mm,
VL53L1_range_data_t *pdata,
VL53L1_DeviceError *prange_status)
{
VL53L1_Error status = VL53L1_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 = VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE;
else
*prange_status = VL53L1_DEVICEERROR_RANGECOMPLETE;
return status;
}
VL53L1_Error VL53L1_hist_xmonitor_consistency_check(
VL53L1_DEV Dev,
VL53L1_zone_hist_info_t *phist_prev,
VL53L1_zone_objects_t *prange_prev,
VL53L1_range_data_t *prange_curr)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(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 == VL53L1_DEVICEERROR_RANGECOMPLETE ||
prange_curr->range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ||
prange_curr->range_status ==
VL53L1_DEVICEERROR_EVENTCONSISTENCY) {
if (prange_prev->xmonitor.range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE ||
prange_prev->xmonitor.range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ||
prange_prev->xmonitor.range_status ==
VL53L1_DEVICEERROR_EVENTCONSISTENCY) {
prange_curr->range_status =
VL53L1_DEVICEERROR_RANGECOMPLETE;
status =
VL53L1_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;
}
VL53L1_Error VL53L1_hist_wrap_dmax(
VL53L1_hist_post_process_config_t *phistpostprocess,
VL53L1_histogram_bin_data_t *pcurrent,
int16_t *pwrap_dmax_mm)
{
VL53L1_Error status = VL53L1_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->VL53L1_p_019 != 0) {
pll_period_mm =
VL53L1_calc_pll_period_mm(
pcurrent->VL53L1_p_019);
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 VL53L1_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,
VL53L1_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;
VL53L1_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;
VL53L1_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;
}
VL53L1_Error VL53L1_hist_xtalk_extract_calc_window(
int16_t target_distance_mm,
uint16_t target_width_oversize,
VL53L1_histogram_bin_data_t *phist_bins,
VL53L1_hist_xtalk_extract_data_t *pxtalk_data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
pxtalk_data->pll_period_mm =
VL53L1_calc_pll_period_mm(phist_bins->VL53L1_p_019);
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->VL53L1_p_015 =
(uint8_t)(pxtalk_data->xtalk_start_phase / 2048);
pxtalk_data->VL53L1_p_016 =
(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->VL53L1_p_016 > (pxtalk_data->target_start-1))
pxtalk_data->VL53L1_p_016 = pxtalk_data->target_start-1;
pxtalk_data->effective_width =
(2048 * ((int32_t)pxtalk_data->VL53L1_p_016+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;
}
VL53L1_Error VL53L1_hist_xtalk_extract_calc_event_sums(
VL53L1_histogram_bin_data_t *phist_bins,
VL53L1_hist_xtalk_extract_data_t *pxtalk_data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
uint8_t lb = 0;
uint8_t i = 0;
LOG_FUNCTION_START("");
for (lb = pxtalk_data->VL53L1_p_015;
lb <= pxtalk_data->VL53L1_p_016;
lb++) {
i = (lb + phist_bins->number_of_ambient_bins +
phist_bins->VL53L1_p_024) %
phist_bins->VL53L1_p_024;
pxtalk_data->signal_events_sum += phist_bins->bin_data[i];
pxtalk_data->signal_events_sum -=
phist_bins->VL53L1_p_004;
}
for (lb = 0; lb < VL53L1_XTALK_HISTO_BINS &&
lb < phist_bins->VL53L1_p_024; lb++) {
i = (lb + phist_bins->number_of_ambient_bins +
phist_bins->VL53L1_p_024) %
phist_bins->VL53L1_p_024;
pxtalk_data->bin_data_sums[lb] += phist_bins->bin_data[i];
pxtalk_data->bin_data_sums[lb] -=
phist_bins->VL53L1_p_004;
}
pxtalk_data->sample_count += 1;
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_hist_xtalk_extract_calc_rate_per_spad(
VL53L1_hist_xtalk_extract_data_t *pxtalk_data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
uint64_t tmp64_0 = 0;
uint64_t tmp64_1 = 0;
uint64_t xtalk_per_spad = 0;
LOG_FUNCTION_START("");
tmp64_1 =
(uint64_t)pxtalk_data->effective_spad_count_sum *
(uint64_t)pxtalk_data->peak_duration_us_sum;
if (pxtalk_data->signal_events_sum < 0) {
pxtalk_data->signal_events_sum = 0;
tmp64_0 =
((uint64_t)pxtalk_data->sample_count *
(uint64_t)pxtalk_data->event_scaler_avg * 256U) << 9U;
if (tmp64_0 > 0) {
pxtalk_data->signal_events_sum = (int32_t)
do_division_u((50U * tmp64_1), tmp64_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;
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;
}
pxtalk_data->xtalk_rate_kcps_per_spad = (uint32_t)xtalk_per_spad;
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_hist_xtalk_extract_calc_shape(
VL53L1_hist_xtalk_extract_data_t *pxtalk_data,
VL53L1_xtalk_histogram_shape_t *pxtalk_shape)
{
VL53L1_Error status = VL53L1_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->VL53L1_p_022 = 0;
pxtalk_shape->VL53L1_p_023 = VL53L1_XTALK_HISTO_BINS;
pxtalk_shape->VL53L1_p_024 = VL53L1_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 < VL53L1_XTALK_HISTO_BINS; lb++) {
if ((lb < (int32_t)pxtalk_data->VL53L1_p_015 ||
lb > (int32_t)pxtalk_data->VL53L1_p_016) ||
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;
}
VL53L1_Error VL53L1_hist_xtalk_shape_model(
uint16_t events_per_bin,
uint16_t pulse_centre,
uint16_t pulse_width,
VL53L1_xtalk_histogram_shape_t *pxtalk_shape)
{
VL53L1_Error status = VL53L1_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 VL53L1_p_008 = 0;
LOG_FUNCTION_START("");
pxtalk_shape->VL53L1_p_022 = 0;
pxtalk_shape->VL53L1_p_023 = VL53L1_XTALK_HISTO_BINS;
pxtalk_shape->VL53L1_p_024 = VL53L1_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 < VL53L1_XTALK_HISTO_BINS; lb++) {
VL53L1_p_008 = 0;
if (lb == bin_start && lb == bin_stop) {
VL53L1_p_008 =
VL53L1_hist_xtalk_shape_model_interp(
events_per_bin,
phase_stop - phase_start);
} else if (lb > bin_start && lb < bin_stop) {
VL53L1_p_008 = events_per_bin;
} else if (lb == bin_start) {
phase_bin = (lb + 1) * 2048;
VL53L1_p_008 =
VL53L1_hist_xtalk_shape_model_interp(
events_per_bin,
(phase_bin - phase_start));
} else if (lb == bin_stop) {
phase_bin = lb * 2048;
VL53L1_p_008 =
VL53L1_hist_xtalk_shape_model_interp(
events_per_bin,
(phase_stop - phase_bin));
}
pxtalk_shape->bin_data[lb] = VL53L1_p_008;
}
LOG_FUNCTION_END(status);
return status;
}
uint16_t VL53L1_hist_xtalk_shape_model_interp(
uint16_t events_per_bin,
uint32_t phase_delta)
{
uint32_t VL53L1_p_008 = 0;
LOG_FUNCTION_START("");
VL53L1_p_008 = (uint32_t)events_per_bin * phase_delta;
VL53L1_p_008 += 1024;
VL53L1_p_008 /= 2048;
if (VL53L1_p_008 > 0xFFFFU)
VL53L1_p_008 = 0xFFFFU;
LOG_FUNCTION_END(0);
return (uint16_t)VL53L1_p_008;
}
void VL53L1_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 VL53L1_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 VL53L1_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 VL53L1_encode_zone_size(
uint8_t width,
uint8_t height,
uint8_t *pencoded_xy_size)
{
*pencoded_xy_size = (height << 4) + width;
}
void VL53L1_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;
VL53L1_decode_row_col(
encoded_xy_centre,
&y_centre,
&x_centre);
VL53L1_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 > (VL53L1_SPAD_ARRAY_WIDTH-1))
*px_ur = VL53L1_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 > (VL53L1_SPAD_ARRAY_HEIGHT-1))
*py_ur = VL53L1_SPAD_ARRAY_HEIGHT-1;
}
uint8_t VL53L1_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 VL53L1_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;
VL53L1_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++) {
VL53L1_encode_row_col(
(uint8_t)y,
(uint8_t)x,
&spad_number);
VL53L1_spad_number_to_byte_bit_index(
spad_number,
&byte_index,
&bit_index,
&bit_mask);
if ((pgood_spads[byte_index] & bit_mask) > 0) {
is_aperture = VL53L1_is_aperture_location(
(uint8_t)y,
(uint8_t)x);
if (is_aperture > 0)
*pmax_effective_spads +=
aperture_attenuation;
else
*pmax_effective_spads += 0x0100;
}
}
}
}
void VL53L1_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;
VL53L1_decode_zone_limits(
encoded_mm_roi_centre,
encoded_mm_roi_size,
&mm_x_ll,
&mm_y_ll,
&mm_x_ur,
&mm_y_ur);
VL53L1_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++) {
VL53L1_encode_row_col(
(uint8_t)y,
(uint8_t)x,
&spad_number);
VL53L1_spad_number_to_byte_bit_index(
spad_number,
&byte_index,
&bit_index,
&bit_mask);
if ((pgood_spads[byte_index] & bit_mask) > 0) {
is_aperture = VL53L1_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 VL53L1_hist_copy_results_to_sys_and_core(
VL53L1_histogram_bin_data_t *pbins,
VL53L1_range_results_t *phist,
VL53L1_system_results_t *psys,
VL53L1_core_results_t *pcore)
{
uint8_t i = 0;
VL53L1_range_data_t *pdata;
LOG_FUNCTION_START("");
VL53L1_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->VL53L1_p_002[0]);
for (i = 0; i < phist->active_results; i++) {
switch (i) {
case 0:
psys->result__dss_actual_effective_spads_sd0 =
pdata->VL53L1_p_006;
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->VL53L1_p_005;
psys->result__phase_sd0 = pdata->VL53L1_p_014;
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->VL53L1_p_021;
pcore->result_core__signal_total_events_sd0 =
pdata->VL53L1_p_013;
pcore->result_core__total_periods_elapsed_sd0 =
pdata->total_periods_elapsed;
pcore->result_core__ambient_window_events_sd0 =
pdata->VL53L1_p_020;
break;
case 1:
psys->result__dss_actual_effective_spads_sd1 =
pdata->VL53L1_p_006;
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->VL53L1_p_005;
psys->result__phase_sd1 = pdata->VL53L1_p_014;
psys->result__final_crosstalk_corrected_range_mm_sd1 =
(uint16_t)pdata->median_range_mm;
pcore->result_core__ranging_total_events_sd1 =
pdata->VL53L1_p_021;
pcore->result_core__signal_total_events_sd1 =
pdata->VL53L1_p_013;
pcore->result_core__total_periods_elapsed_sd1 =
pdata->total_periods_elapsed;
pcore->result_core__ambient_window_events_sd1 =
pdata->VL53L1_p_020;
break;
}
pdata++;
}
LOG_FUNCTION_END(0);
}
VL53L1_Error VL53L1_sum_histogram_data(
VL53L1_histogram_bin_data_t *phist_input,
VL53L1_histogram_bin_data_t *phist_output)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
uint8_t i = 0;
uint8_t smallest_bin_num = 0;
LOG_FUNCTION_START("");
if (status == VL53L1_ERROR_NONE) {
if (phist_output->VL53L1_p_024 >=
phist_input->VL53L1_p_024)
smallest_bin_num = phist_input->VL53L1_p_024;
else
smallest_bin_num = phist_output->VL53L1_p_024;
}
if (status == VL53L1_ERROR_NONE)
for (i = 0; i < smallest_bin_num; i++)
phist_output->bin_data[i] += phist_input->bin_data[i];
if (status == VL53L1_ERROR_NONE)
phist_output->VL53L1_p_004 +=
phist_input->VL53L1_p_004;
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_avg_histogram_data(
uint8_t no_of_samples,
VL53L1_histogram_bin_data_t *phist_sum,
VL53L1_histogram_bin_data_t *phist_avg)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
uint8_t i = 0;
LOG_FUNCTION_START("");
if (status == VL53L1_ERROR_NONE) {
for (i = 0; i < phist_sum->VL53L1_p_024; 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 == VL53L1_ERROR_NONE) {
if (no_of_samples > 0)
phist_avg->VL53L1_p_004 =
phist_sum->VL53L1_p_004 /
(int32_t)no_of_samples;
else
phist_avg->VL53L1_p_004 =
phist_sum->VL53L1_p_004;
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_save_cfg_data(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres =
VL53L1DevStructGetLLResultsHandle(Dev);
VL53L1_zone_private_dyn_cfg_t *pzone_dyn_cfg;
VL53L1_dynamic_config_t *pdynamic = &(pdev->dyn_cfg);
LOG_FUNCTION_START("");
pzone_dyn_cfg =
&(pres->zone_dyn_cfgs.VL53L1_p_002[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;
}
VL53L1_Error VL53L1_dynamic_zone_update(
VL53L1_DEV Dev,
VL53L1_range_results_t *presults)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres =
VL53L1DevStructGetLLResultsHandle(Dev);
VL53L1_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 VL53L1_LOG_ENABLE
uint16_t eff_spad_cnt =
pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count;
#endif
LOG_FUNCTION_START("");
pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count = 0;
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: peak signal count rate mcps:");
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
"%u actual effective spads: %u\n",
presults->VL53L1_p_002[0].peak_signal_count_rate_mcps,
presults->VL53L1_p_002[0].VL53L1_p_006);
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: active results: %u\n",
presults->active_results);
max_total_rate_per_spads =
presults->VL53L1_p_002[0].total_rate_per_spad_mcps;
trace_print(VL53L1_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(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: zone total rate per spad: zone_id: %u,",
i);
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
"total rate per spad: %u\n",
presults->VL53L1_p_002[i].total_rate_per_spad_mcps);
if (presults->VL53L1_p_002[i].total_rate_per_spad_mcps >
max_total_rate_per_spads)
max_total_rate_per_spads =
presults->VL53L1_p_002[i].total_rate_per_spad_mcps;
}
if (max_total_rate_per_spads == 0) {
temp = 0xFFFF;
} else {
temp = target_rate << 14;
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: 1: temp: %u\n",
temp);
temp = temp / max_total_rate_per_spads;
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: 2: temp: %u\n",
temp);
if (temp > 0xFFFF)
temp = 0xFFFF;
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: 3: temp: %u\n",
temp);
}
pZ->VL53L1_p_002[zone_id].dss_requested_effective_spad_count =
(uint16_t)temp;
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
" DYNZONEUPDATE: zone_id: %u, target_rate: %u,",
zone_id,
target_rate);
trace_print(VL53L1_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;
}
VL53L1_Error VL53L1_multizone_hist_bins_update(
VL53L1_DEV Dev)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);
VL53L1_zone_config_t *pzone_cfg = &(pdev->zone_cfg);
VL53L1_histogram_config_t *phist_cfg = &(pdev->hist_cfg);
VL53L1_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] ==
VL53L1_ZONECONFIG_BINCONFIG__LOWAMB) {
if (!next_range_is_odd_timing) {
trace_print (VL53L1_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 (VL53L1_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] ==
VL53L1_ZONECONFIG_BINCONFIG__MIDAMB) {
trace_print (VL53L1_TRACE_LEVEL_DEBUG,
" HISTBINCONFIGUPDATE: Setting MIDAMB timing\n");
if (!next_range_is_odd_timing) {
trace_print(VL53L1_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 (VL53L1_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] ==
VL53L1_ZONECONFIG_BINCONFIG__HIGHAMB) {
if (!next_range_is_odd_timing) {
trace_print (VL53L1_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 (VL53L1_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 == VL53L1_ERROR_NONE) {
VL53L1_copy_hist_bins_to_static_cfg(
phist_cfg,
&(pdev->stat_cfg),
&(pdev->tim_cfg));
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_update_internal_stream_counters(
VL53L1_DEV Dev,
uint8_t external_stream_count,
uint8_t *pinternal_stream_count,
uint8_t *pinternal_stream_count_val)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
uint8_t stream_divider;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(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(VL53L1_TRACE_LEVEL_DEBUG,
"UPDINTSTREAMCOUNT internal_steam_count: %d,",
*pinternal_stream_count);
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
"internal_stream_count_val: %d, divider: %d\n",
*pinternal_stream_count_val,
stream_divider);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_set_histogram_multizone_initial_bin_config(
VL53L1_zone_config_t *pzone_cfg,
VL53L1_histogram_config_t *phist_cfg,
VL53L1_histogram_config_t *pmulti_hist)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if (pzone_cfg->bin_config[0] ==
VL53L1_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] ==
VL53L1_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] ==
VL53L1_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 VL53L1_encode_GPIO_interrupt_config(
VL53L1_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;
}
VL53L1_GPIO_interrupt_config_t VL53L1_decode_GPIO_interrupt_config(
uint8_t system__interrupt_config)
{
VL53L1_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;
}
VL53L1_Error VL53L1_set_GPIO_distance_threshold(
VL53L1_DEV Dev,
uint16_t threshold_high,
uint16_t threshold_low)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(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;
}
VL53L1_Error VL53L1_set_GPIO_rate_threshold(
VL53L1_DEV Dev,
uint16_t threshold_high,
uint16_t threshold_low)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(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;
}
VL53L1_Error VL53L1_set_GPIO_thresholds_from_struct(
VL53L1_DEV Dev,
VL53L1_GPIO_interrupt_config_t *pintconf)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
status = VL53L1_set_GPIO_distance_threshold(
Dev,
pintconf->threshold_distance_high,
pintconf->threshold_distance_low);
if (status == VL53L1_ERROR_NONE) {
status =
VL53L1_set_GPIO_rate_threshold(
Dev,
pintconf->threshold_rate_high,
pintconf->threshold_rate_low);
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_set_ref_spad_char_config(
VL53L1_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)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
uint8_t buffer[2];
uint32_t macro_period_us = 0;
uint32_t timeout_mclks = 0;
LOG_FUNCTION_START("");
macro_period_us =
VL53L1_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 == VL53L1_ERROR_NONE)
status =
VL53L1_WrByte(
Dev,
VL53L1_PHASECAL_CONFIG__TIMEOUT_MACROP,
pdev->gen_cfg.phasecal_config__timeout_macrop);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrByte(
Dev,
VL53L1_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 == VL53L1_ERROR_NONE)
status =
VL53L1_WriteMulti(
Dev,
VL53L1_SD_CONFIG__WOI_SD0,
buffer,
2);
pdev->customer.ref_spad_char__total_rate_target_mcps =
total_rate_target_mcps;
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrWord(
Dev,
VL53L1_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS,
total_rate_target_mcps);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrWord(
Dev,
VL53L1_RANGE_CONFIG__SIGMA_THRESH,
max_count_rate_rtn_limit_mcps);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrWord(
Dev,
VL53L1_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,
min_count_rate_rtn_limit_mcps);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_set_ssc_config(
VL53L1_DEV Dev,
VL53L1_ssc_config_t *pssc_cfg,
uint16_t fast_osc_frequency)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
uint8_t buffer[5];
uint32_t macro_period_us = 0;
uint16_t timeout_encoded = 0;
LOG_FUNCTION_START("");
macro_period_us =
VL53L1_calc_macro_period_us(
fast_osc_frequency,
pssc_cfg->VL53L1_p_009);
timeout_encoded =
VL53L1_calc_encoded_timeout(
pssc_cfg->timeout_us,
macro_period_us);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrByte(
Dev,
VL53L1_CAL_CONFIG__VCSEL_START,
pssc_cfg->vcsel_start);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrByte(
Dev,
VL53L1_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->VL53L1_p_009;
buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8);
buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps & 0x000000FF);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WriteMulti(
Dev,
VL53L1_RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
buffer,
5);
buffer[0] = pssc_cfg->VL53L1_p_009;
buffer[1] = pssc_cfg->VL53L1_p_009;
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WriteMulti(
Dev,
VL53L1_SD_CONFIG__WOI_SD0,
buffer,
2);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_WrByte(
Dev,
VL53L1_NVM_BIST__CTRL,
pssc_cfg->array_select);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_get_spad_rate_data(
VL53L1_DEV Dev,
VL53L1_spad_rate_data_t *pspad_rates)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
int i = 0;
uint8_t VL53L1_p_002[512];
uint8_t *pdata = &VL53L1_p_002[0];
LOG_FUNCTION_START("");
if (status == VL53L1_ERROR_NONE)
status = VL53L1_disable_firmware(Dev);
if (status == VL53L1_ERROR_NONE)
status =
VL53L1_ReadMulti(
Dev,
VL53L1_PRIVATE__PATCH_BASE_ADDR_RSLV,
pdata,
512);
pdata = &VL53L1_p_002[0];
for (i = 0; i < VL53L1_NO_OF_SPAD_ENABLES; i++) {
pspad_rates->rate_data[i] =
(uint16_t)VL53L1_decode_unsigned_integer(pdata, 2);
pdata += 2;
}
pspad_rates->VL53L1_p_023 = VL53L1_NO_OF_SPAD_ENABLES;
pspad_rates->no_of_values = VL53L1_NO_OF_SPAD_ENABLES;
pspad_rates->fractional_bits = 15;
if (status == VL53L1_ERROR_NONE)
status = VL53L1_enable_firmware(Dev);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_dynamic_xtalk_correction_calc_required_samples(
VL53L1_DEV Dev
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev);
VL53L1_smudge_corrector_config_t *pconfig =
&(pdev->smudge_correct_config);
VL53L1_smudge_corrector_internals_t *pint =
&(pdev->smudge_corrector_internals);
VL53L1_range_results_t *presults = &(pres->range_results);
VL53L1_range_data_t *pxmonitor = &(presults->xmonitor);
uint32_t peak_duration_us = pxmonitor->peak_duration_us;
uint64_t temp64a;
uint64_t temp64z;
LOG_FUNCTION_START("");
if (peak_duration_us == 0)
peak_duration_us = 1000;
temp64a = pxmonitor->VL53L1_p_021 +
pxmonitor->VL53L1_p_020;
temp64a = do_division_u((temp64a * 1000), peak_duration_us);
temp64a = do_division_u((temp64a * 1000), peak_duration_us);
temp64z = pconfig->noise_margin * pxmonitor->VL53L1_p_006;
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;
}
VL53L1_Error VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
VL53L1_DEV Dev,
uint32_t xtalk_offset_out,
VL53L1_smudge_corrector_config_t *pconfig,
VL53L1_smudge_corrector_data_t *pout,
uint8_t add_smudge,
uint8_t soft_update
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(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;
VL53L1_xtalk_config_t *pX = &(pdev->xtalk_cfg);
VL53L1_xtalk_calibration_results_t *pC = &(pdev->xtalk_cal);
uint32_t *pcpo;
uint32_t max, nXtalk, cXtalk;
uint8_t merge_enabled;
LOG_FUNCTION_START("");
merge_enabled = (pdev->tuning_parms.tp_hist_merge == 1) &&
(VL53L1DevDataGet(Dev, CurrentParameters.PresetMode) ==
VL53L1_PRESETMODE_RANGING);
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)) {
VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb);
if (histo_merge_nb == 0)
histo_merge_nb = 1;
if (!merge_enabled)
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;
VL53L1_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) && merge_enabled && (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
VL53L1_Error VL53L1_dynamic_xtalk_correction_corrector(
VL53L1_DEV Dev
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(Dev);
VL53L1_smudge_corrector_config_t *pconfig =
&(pdev->smudge_correct_config);
VL53L1_smudge_corrector_internals_t *pint =
&(pdev->smudge_corrector_internals);
VL53L1_smudge_corrector_data_t *pout =
&(pres->range_results.smudge_corrector_data);
VL53L1_range_results_t *pR = &(pres->range_results);
VL53L1_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;
uint8_t merge_enabled;
LOG_FUNCTION_START("");
merge_enabled = (pdev->tuning_parms.tp_hist_merge == 1) &&
(VL53L1DevDataGet(Dev, CurrentParameters.PresetMode) ==
VL53L1_PRESETMODE_RANGING);
VL53L1_compute_histo_merge_nb(Dev, &histo_merge_nb);
if ((histo_merge_nb == 0) || (!merge_enabled))
histo_merge_nb = 1;
VL53L1_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 = ((!merge_enabled) ||
(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
== VL53L1_DEVICEERROR_RANGECOMPLETE) &&
merging_complete;
if ((pR->xmonitor.range_status
!= VL53L1_DEVICEERROR_RANGECOMPLETE) &&
(pconfig->smudge_corr_enabled == 1)) {
run_nodetect = 2;
for (i = 0; i < pR->active_results; i++) {
if (pR->VL53L1_p_002[i].range_status ==
VL53L1_DEVICEERROR_RANGECOMPLETE) {
if (pR->VL53L1_p_002[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->VL53L1_p_002[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;
VL53L1_dynamic_xtalk_correction_calc_required_samples(Dev);
xtalk_offset_in =
pR->xmonitor.VL53L1_p_012;
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) {
VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
Dev,
xtalk_offset_out,
pconfig,
pout,
1,
0
);
continue_processing = CONT_RESET;
} else {
VL53L1_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) {
VL53L1_dynamic_xtalk_correction_calc_new_xtalk(
Dev,
xtalk_offset_out,
pconfig,
pout,
0,
0
);
pout->smudge_corr_valid = 2;
continue_processing = CONT_RESET;
} else {
VL53L1_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;
}
VL53L1_Error VL53L1_dynamic_xtalk_correction_data_init(
VL53L1_DEV Dev
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_LLDriverResults_t *pres = VL53L1DevStructGetLLResultsHandle(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 =
VL53L1_TUNINGPARM_DYNXTALK_SMUDGE_COR_SINGLE_APPLY_DEFAULT;
pdev->smudge_correct_config.smudge_margin =
VL53L1_TUNINGPARM_DYNXTALK_SMUDGE_MARGIN_DEFAULT;
pdev->smudge_correct_config.noise_margin =
VL53L1_TUNINGPARM_DYNXTALK_NOISE_MARGIN_DEFAULT;
pdev->smudge_correct_config.user_xtalk_offset_limit =
VL53L1_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_DEFAULT;
pdev->smudge_correct_config.user_xtalk_offset_limit_hi =
VL53L1_TUNINGPARM_DYNXTALK_XTALK_OFFSET_LIMIT_HI_DEFAULT;
pdev->smudge_correct_config.sample_limit =
VL53L1_TUNINGPARM_DYNXTALK_SAMPLE_LIMIT_DEFAULT;
pdev->smudge_correct_config.single_xtalk_delta =
VL53L1_TUNINGPARM_DYNXTALK_SINGLE_XTALK_DELTA_DEFAULT;
pdev->smudge_correct_config.averaged_xtalk_delta =
VL53L1_TUNINGPARM_DYNXTALK_AVERAGED_XTALK_DELTA_DEFAULT;
pdev->smudge_correct_config.smudge_corr_clip_limit =
VL53L1_TUNINGPARM_DYNXTALK_CLIP_LIMIT_DEFAULT;
pdev->smudge_correct_config.smudge_corr_ambient_threshold =
VL53L1_TUNINGPARM_DYNXTALK_XTALK_AMB_THRESHOLD_DEFAULT;
pdev->smudge_correct_config.scaler_calc_method =
0;
pdev->smudge_correct_config.x_gradient_scaler =
VL53L1_TUNINGPARM_DYNXTALK_XGRADIENT_SCALER_DEFAULT;
pdev->smudge_correct_config.y_gradient_scaler =
VL53L1_TUNINGPARM_DYNXTALK_YGRADIENT_SCALER_DEFAULT;
pdev->smudge_correct_config.user_scaler_set =
VL53L1_TUNINGPARM_DYNXTALK_USER_SCALER_SET_DEFAULT;
pdev->smudge_correct_config.nodetect_ambient_threshold =
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_AMB_THRESHOLD_KCPS_DEFAULT;
pdev->smudge_correct_config.nodetect_sample_limit =
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_SAMPLE_LIMIT_DEFAULT;
pdev->smudge_correct_config.nodetect_xtalk_offset =
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS_DEFAULT;
pdev->smudge_correct_config.nodetect_min_range_mm =
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_MIN_RANGE_MM_DEFAULT;
pdev->smudge_correct_config.max_smudge_factor =
VL53L1_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;
VL53L1_dynamic_xtalk_correction_output_init(pres);
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_dynamic_xtalk_correction_output_init(
VL53L1_LLDriverResults_t *pres
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_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;
}
VL53L1_Error VL53L1_xtalk_cal_data_init(
VL53L1_DEV Dev
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(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;
}
VL53L1_Error VL53L1_low_power_auto_data_init(
VL53L1_DEV Dev
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
LOG_FUNCTION_START("");
pdev->low_power_auto_data.vhv_loop_bound =
VL53L1_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;
}
VL53L1_Error VL53L1_low_power_auto_data_stop_range(
VL53L1_DEV Dev
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(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;
}
VL53L1_Error VL53L1_config_low_power_auto_mode(
VL53L1_general_config_t *pgeneral,
VL53L1_dynamic_config_t *pdynamic,
VL53L1_low_power_auto_data_t *plpadata
)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
plpadata->is_low_power_auto_mode = 1;
plpadata->low_power_auto_range_count = 0;
pdynamic->system__sequence_config =
VL53L1_SEQUENCE_VHV_EN |
VL53L1_SEQUENCE_PHASECAL_EN |
VL53L1_SEQUENCE_DSS1_EN |
VL53L1_SEQUENCE_RANGE_EN;
pgeneral->dss_config__manual_effective_spads_select = 200 << 8;
pgeneral->dss_config__roi_mode_control =
VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration(
VL53L1_DEV Dev)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_Error status = VL53L1_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;
}
VL53L1_Error VL53L1_low_power_auto_update_DSS(
VL53L1_DEV Dev)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_system_results_t *pS = &(pdev->sys_results);
VL53L1_Error status = VL53L1_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 = VL53L1_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 = VL53L1_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 =
VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
}
}
if (status == VL53L1_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 =
VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
status = VL53L1_ERROR_NONE;
}
LOG_FUNCTION_END(status);
return status;
}
VL53L1_Error VL53L1_compute_histo_merge_nb(
VL53L1_DEV Dev, uint8_t *histo_merge_nb)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_Error status = VL53L1_ERROR_NONE;
uint8_t i, timing;
uint8_t sum = 0;
timing = (pdev->hist_data.bin_seq[0] == 7 ? 1 : 0);
for (i = 0; i < VL53L1_BIN_REC_SIZE; i++)
if (pdev->multi_bins_rec[i][timing][7] > 0)
sum++;
*histo_merge_nb = sum;
return status;
}