Rename library
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging
Diff: modules/vl53lx_api.c
- Revision:
- 5:89031b2f5316
diff -r 39a9cd3c85f2 -r 89031b2f5316 modules/vl53lx_api.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/vl53lx_api.c Wed Jul 14 12:45:49 2021 +0100 @@ -0,0 +1,2031 @@ + +// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause +/****************************************************************************** + * Copyright (c) 2020, STMicroelectronics - All Rights Reserved + + This file is part of VL53LX and is dual licensed, + either GPL-2.0+ + or 'BSD 3-clause "New" or "Revised" License' , at your option. + ****************************************************************************** + */ + + + + + + +#include "vl53lx_api.h" +#include "vl53lx_register_settings.h" +#include "vl53lx_register_funcs.h" +#include "vl53lx_core.h" +#include "vl53lx_api_calibration.h" +#include "vl53lx_wait.h" +#include "vl53lx_preset_setup.h" +#include "vl53lx_api_debug.h" +#include "vl53lx_api_core.h" +#include "vl53lx_nvm.h" +#include "spi_interface.h" + + + +#define ZONE_CHECK 5 + +#define LOG_FUNCTION_START(fmt, ...) \ + _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_API, fmt, ##__VA_ARGS__) +#define LOG_FUNCTION_END(status, ...) \ + _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_API, status, ##__VA_ARGS__) +#define LOG_FUNCTION_END_FMT(status, fmt, ...) \ + _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_API, status, \ + fmt, ##__VA_ARGS__) + +#ifdef VL53LX_LOG_ENABLE +#define trace_print(level, ...) trace_print_module_function(\ + VL53LX_TRACE_MODULE_API, level, VL53LX_TRACE_FUNCTION_NONE, \ + ##__VA_ARGS__) +#endif + +#ifndef MIN +#define MIN(v1, v2) ((v1) < (v2) ? (v1) : (v2)) +#endif +#ifndef MAX +#define MAX(v1, v2) ((v1) < (v2) ? (v2) : (v1)) +#endif + +#define DMAX_REFLECTANCE_IDX 2 + + + +#define LOWPOWER_AUTO_VHV_LOOP_DURATION_US 245 +#define LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING 1448 +#define LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING 2100 + +#define FDA_MAX_TIMING_BUDGET_US 550000 +#define L4_FDA_MAX_TIMING_BUDGET_US 200000 + + + + +static int32_t BDTable[VL53LX_TUNING_MAX_TUNABLE_KEY] = { + TUNING_VERSION, + TUNING_PROXY_MIN, + TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM, + TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER, + TUNING_MIN_AMBIENT_DMAX_VALID, + TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER, + TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM, + TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT, + TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN, + TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET, + TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR_DEFAULT +}; + +static VL53LX_Error SetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev, + uint32_t InterMeasurementPeriodMilliSeconds); + +static VL53LX_Error GetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev, + uint32_t *pInterMeasurementPeriodMilliSeconds); + +static VL53LX_Error ComputeDevicePresetMode( + VL53LX_DistanceModes DistanceMode, + VL53LX_DevicePresetModes *pDevicePresetMode); + +static VL53LX_Error SetPresetModeL3CX(VL53LX_DEV Dev, + VL53LX_DistanceModes DistanceMode, + uint32_t inter_measurement_period_ms); + +static int IsL4(VL53LX_DEV Dev); + +static VL53LX_Error CheckValidRectRoi(VL53LX_UserRoi_t ROI); + + +VL53LX_Error VL53LX_GetVersion(VL53LX_Version_t *pVersion) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + pVersion->major = VL53LX_IMPLEMENTATION_VER_MAJOR; + pVersion->minor = VL53LX_IMPLEMENTATION_VER_MINOR; + pVersion->build = VL53LX_IMPLEMENTATION_VER_SUB; + + pVersion->revision = VL53LX_IMPLEMENTATION_VER_REVISION; + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetProductRevision(VL53LX_DEV Dev, + uint8_t *pProductRevisionMajor, uint8_t *pProductRevisionMinor) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint8_t revision_id; + VL53LX_LLDriverData_t *pLLData; + + LOG_FUNCTION_START(""); + + pLLData = VL53LXDevStructGetLLDriverHandle(Dev); + revision_id = pLLData->nvm_copy_data.identification__revision_id; + *pProductRevisionMajor = 1; + *pProductRevisionMinor = (revision_id & 0xF0) >> 4; + + LOG_FUNCTION_END(Status); + return Status; + +} + +VL53LX_Error VL53LX_GetDeviceInfo(VL53LX_DEV Dev, + VL53LX_DeviceInfo_t *pVL53LX_DeviceInfo) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint8_t revision_id; + VL53LX_LLDriverData_t *pLLData; + + LOG_FUNCTION_START(""); + + pLLData = VL53LXDevStructGetLLDriverHandle(Dev); + + pVL53LX_DeviceInfo->ProductType = + pLLData->nvm_copy_data.identification__module_type; + + revision_id = pLLData->nvm_copy_data.identification__revision_id; + pVL53LX_DeviceInfo->ProductRevisionMajor = 1; + pVL53LX_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4; + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetUID(VL53LX_DEV Dev, uint64_t *pUid) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint8_t fmtdata[8]; + + LOG_FUNCTION_START(""); + + Status = VL53LX_read_nvm_raw_data(Dev, + (uint8_t)(0x1F8 >> 2), + (uint8_t)(8 >> 2), + fmtdata); + memcpy(pUid, fmtdata, sizeof(uint64_t)); + + LOG_FUNCTION_END(Status); + return Status; +} + + + +VL53LX_Error VL53LX_SetDeviceAddress(VL53LX_DEV Dev, uint8_t DeviceAddress) +{ + + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_static_nvm_managed_t *pdata = &(pdev->stat_nvm); + + LOG_FUNCTION_START(""); + printf("VL53LX_SetDeviceAddress VL53LX_WrByte from %d to %d\n",Dev->IO.Address,DeviceAddress); + + Status = VL53LX_WrByte(Dev, VL53LX_I2C_SLAVE__DEVICE_ADDRESS, + DeviceAddress / 2); + printf("VL53LX_SetDeviceAddress VL53LX_WrByte status %d\n",Status); + pdata->i2c_slave__device_address = (DeviceAddress / 2) & 0x7F; + Dev->IO.Address = DeviceAddress; + + LOG_FUNCTION_END(Status); + + return Status; +} + + +VL53LX_Error VL53LX_DataInit(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev; + uint8_t measurement_mode; + + LOG_FUNCTION_START(""); + + +#ifdef USE_I2C_2V8 + Status = VL53LX_RdByte(Dev, VL53LX_PAD_I2C_HV__EXTSUP_CONFIG, &i); + if (Status == VL53LX_ERROR_NONE) { + i = (i & 0xfe) | 0x01; + Status = VL53LX_WrByte(Dev, VL53LX_PAD_I2C_HV__EXTSUP_CONFIG, + i); + } +#endif + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_data_init(Dev, 1); + + Status = SetPresetModeL3CX(Dev, + VL53LX_DISTANCEMODE_MEDIUM, + 1000); + + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_SetMeasurementTimingBudgetMicroSeconds(Dev, + 33333); + + if (Status == VL53LX_ERROR_NONE) + Status = SetInterMeasurementPeriodMilliSeconds(Dev, 1000); + + if (Status == VL53LX_ERROR_NONE) { + pdev = VL53LXDevStructGetLLDriverHandle(Dev); + memset(&pdev->per_vcsel_cal_data, 0, + sizeof(pdev->per_vcsel_cal_data)); + } + + if (Status == VL53LX_ERROR_NONE) { + Status = VL53LX_set_dmax_mode(Dev, + VL53LX_DEVICEDMAXMODE__CUST_CAL_DATA); + } + + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_SmudgeCorrectionEnable(Dev, + VL53LX_SMUDGE_CORRECTION_NONE); + + measurement_mode = VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK; + VL53LXDevDataSet(Dev, LLData.measurement_mode, measurement_mode); + + VL53LXDevDataSet(Dev, CurrentParameters.DistanceMode, + VL53LX_DISTANCEMODE_MEDIUM); + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_WaitDeviceBooted(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + Status = VL53LX_poll_for_boot_completion(Dev, + VL53LX_BOOT_COMPLETION_POLLING_TIMEOUT_MS); + + LOG_FUNCTION_END(Status); + return Status; +} + + + + +static VL53LX_Error ComputeDevicePresetMode( + VL53LX_DistanceModes DistanceMode, + VL53LX_DevicePresetModes *pDevicePresetMode) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + uint8_t DistIdx; + VL53LX_DevicePresetModes RangingModes[3] = { + VL53LX_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE, + VL53LX_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE, + VL53LX_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE}; + + switch (DistanceMode) { + case VL53LX_DISTANCEMODE_SHORT: + DistIdx = 0; + break; + case VL53LX_DISTANCEMODE_MEDIUM: + DistIdx = 1; + break; + default: + DistIdx = 2; + } + + *pDevicePresetMode = RangingModes[DistIdx]; + + return Status; +} + +static VL53LX_Error SetPresetModeL3CX(VL53LX_DEV Dev, + VL53LX_DistanceModes DistanceMode, + uint32_t inter_measurement_period_ms) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_DevicePresetModes device_preset_mode; + uint8_t measurement_mode; + uint16_t dss_config__target_total_rate_mcps; + uint32_t phasecal_config_timeout_us; + uint32_t mm_config_timeout_us; + uint32_t lld_range_config_timeout_us; + + LOG_FUNCTION_START(""); + + measurement_mode = VL53LX_DEVICEMEASUREMENTMODE_BACKTOBACK; + + Status = ComputeDevicePresetMode(DistanceMode, + &device_preset_mode); + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_get_preset_mode_timing_cfg(Dev, + device_preset_mode, + &dss_config__target_total_rate_mcps, + &phasecal_config_timeout_us, + &mm_config_timeout_us, + &lld_range_config_timeout_us); + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_set_preset_mode( + Dev, + device_preset_mode, + dss_config__target_total_rate_mcps, + phasecal_config_timeout_us, + mm_config_timeout_us, + lld_range_config_timeout_us, + inter_measurement_period_ms); + + if (Status == VL53LX_ERROR_NONE) + VL53LXDevDataSet(Dev, LLData.measurement_mode, + measurement_mode); + + LOG_FUNCTION_END(Status); + return Status; +} + +static int IsL4(VL53LX_DEV Dev) +{ + int devL4 = 0; + VL53LX_LLDriverData_t *pDev; + pDev = VL53LXDevStructGetLLDriverHandle(Dev); + + if ((pDev->nvm_copy_data.identification__module_type == 0xAA) && + (pDev->nvm_copy_data.identification__model_id == 0xEB)) + devL4 = 1; + return devL4; +} + +static VL53LX_Error CheckValidRectRoi(VL53LX_UserRoi_t ROI) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + + if ((ROI.TopLeftX > 15) || (ROI.TopLeftY > 15) || + (ROI.BotRightX > 15) || (ROI.BotRightY > 15)) + Status = VL53LX_ERROR_INVALID_PARAMS; + + if ((ROI.TopLeftX > ROI.BotRightX) || (ROI.TopLeftY < ROI.BotRightY)) + Status = VL53LX_ERROR_INVALID_PARAMS; + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_SetDistanceMode(VL53LX_DEV Dev, + VL53LX_DistanceModes DistanceMode) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint32_t inter_measurement_period_ms; + uint32_t TimingBudget; + uint32_t MmTimeoutUs; + uint32_t PhaseCalTimeoutUs; + + LOG_FUNCTION_START("%d", (int)DistanceMode); + + + + if ((DistanceMode != VL53LX_DISTANCEMODE_SHORT) && + (DistanceMode != VL53LX_DISTANCEMODE_MEDIUM) && + (DistanceMode != VL53LX_DISTANCEMODE_LONG)) + return VL53LX_ERROR_INVALID_PARAMS; + + if (IsL4(Dev) && (DistanceMode == VL53LX_DISTANCEMODE_SHORT)) + return VL53LX_ERROR_INVALID_PARAMS; + + inter_measurement_period_ms = VL53LXDevDataGet(Dev, + LLData.inter_measurement_period_ms); + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_get_timeouts_us(Dev, &PhaseCalTimeoutUs, + &MmTimeoutUs, &TimingBudget); + + if (Status == VL53LX_ERROR_NONE) + Status = SetPresetModeL3CX(Dev, + DistanceMode, + inter_measurement_period_ms); + + if (Status == VL53LX_ERROR_NONE) { + VL53LXDevDataSet(Dev, CurrentParameters.DistanceMode, + DistanceMode); + } + + if (Status == VL53LX_ERROR_NONE) { + Status = VL53LX_set_timeouts_us(Dev, PhaseCalTimeoutUs, + MmTimeoutUs, TimingBudget); + + if (Status == VL53LX_ERROR_NONE) + VL53LXDevDataSet(Dev, LLData.range_config_timeout_us, + TimingBudget); + } + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetDistanceMode(VL53LX_DEV Dev, + VL53LX_DistanceModes *pDistanceMode) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + *pDistanceMode = VL53LXDevDataGet(Dev, CurrentParameters.DistanceMode); + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_SetMeasurementTimingBudgetMicroSeconds(VL53LX_DEV Dev, + uint32_t MeasurementTimingBudgetMicroSeconds) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint32_t TimingGuard; + uint32_t divisor; + uint32_t TimingBudget; + uint32_t MmTimeoutUs; + uint32_t PhaseCalTimeoutUs; + uint32_t FDAMaxTimingBudgetUs = FDA_MAX_TIMING_BUDGET_US; + + LOG_FUNCTION_START(""); + + + if (MeasurementTimingBudgetMicroSeconds > 10000000) + Status = VL53LX_ERROR_INVALID_PARAMS; + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_get_timeouts_us(Dev, + &PhaseCalTimeoutUs, + &MmTimeoutUs, + &TimingBudget); + + TimingGuard = 1700; + divisor = 6; + + if (IsL4(Dev)) + FDAMaxTimingBudgetUs = L4_FDA_MAX_TIMING_BUDGET_US; + + if (MeasurementTimingBudgetMicroSeconds <= TimingGuard) + Status = VL53LX_ERROR_INVALID_PARAMS; + else { + TimingBudget = (MeasurementTimingBudgetMicroSeconds + - TimingGuard); + } + + if (Status == VL53LX_ERROR_NONE) { + if (TimingBudget > FDAMaxTimingBudgetUs) + Status = VL53LX_ERROR_INVALID_PARAMS; + else { + TimingBudget /= divisor; + Status = VL53LX_set_timeouts_us( + Dev, + PhaseCalTimeoutUs, + MmTimeoutUs, + TimingBudget); + } + + if (Status == VL53LX_ERROR_NONE) + VL53LXDevDataSet(Dev, + LLData.range_config_timeout_us, + TimingBudget); + } + + if (Status == VL53LX_ERROR_NONE) { + VL53LXDevDataSet(Dev, + CurrentParameters.MeasurementTimingBudgetMicroSeconds, + MeasurementTimingBudgetMicroSeconds); + } + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_GetMeasurementTimingBudgetMicroSeconds(VL53LX_DEV Dev, + uint32_t *pMeasurementTimingBudgetMicroSeconds) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint32_t MmTimeoutUs = 0; + uint32_t RangeTimeoutUs = 0; + uint32_t PhaseCalTimeoutUs = 0; + + LOG_FUNCTION_START(""); + + *pMeasurementTimingBudgetMicroSeconds = 0; + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_get_timeouts_us(Dev, + &PhaseCalTimeoutUs, + &MmTimeoutUs, + &RangeTimeoutUs); + + if (Status == VL53LX_ERROR_NONE) + *pMeasurementTimingBudgetMicroSeconds = (6 * RangeTimeoutUs) + + 1700; + + LOG_FUNCTION_END(Status); + return Status; +} + + + +static VL53LX_Error SetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev, + uint32_t InterMeasurementPeriodMilliSeconds) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint32_t adjustedIMP; + + LOG_FUNCTION_START(""); + + + adjustedIMP = InterMeasurementPeriodMilliSeconds; + adjustedIMP += (adjustedIMP * 64) / 1000; + + Status = VL53LX_set_inter_measurement_period_ms(Dev, + adjustedIMP); + + LOG_FUNCTION_END(Status); + return Status; +} + +static VL53LX_Error GetInterMeasurementPeriodMilliSeconds(VL53LX_DEV Dev, + uint32_t *pInterMeasurementPeriodMilliSeconds) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint32_t adjustedIMP; + + LOG_FUNCTION_START(""); + + Status = VL53LX_get_inter_measurement_period_ms(Dev, &adjustedIMP); + + adjustedIMP -= (adjustedIMP * 64) / 1000; + *pInterMeasurementPeriodMilliSeconds = adjustedIMP; + + + LOG_FUNCTION_END(Status); + return Status; +} + + + + + +VL53LX_Error VL53LX_SetUserROI(VL53LX_DEV Dev, + VL53LX_UserRoi_t *pRoi) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_zone_config_t zone_cfg; + uint8_t x_centre, y_centre, width, height; + + Status = CheckValidRectRoi(*pRoi); + if (Status != VL53LX_ERROR_NONE) + return VL53LX_ERROR_INVALID_PARAMS; + + x_centre = (pRoi->BotRightX + pRoi->TopLeftX + 1) / 2; + y_centre = (pRoi->TopLeftY + pRoi->BotRightY + 1) / 2; + width = (pRoi->BotRightX - pRoi->TopLeftX); + height = (pRoi->TopLeftY - pRoi->BotRightY); + zone_cfg.max_zones = 1; + zone_cfg.active_zones = 0; + zone_cfg.user_zones[0].x_centre = x_centre; + zone_cfg.user_zones[0].y_centre = y_centre; + zone_cfg.user_zones[0].width = width; + zone_cfg.user_zones[0].height = height; + if ((width < 3) || (height < 3)) + Status = VL53LX_ERROR_INVALID_PARAMS; + else + Status = VL53LX_set_zone_config(Dev, &zone_cfg); + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetUserROI(VL53LX_DEV Dev, + VL53LX_UserRoi_t *pRoi) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_zone_config_t zone_cfg; + uint8_t TopLeftX; + uint8_t TopLeftY; + uint8_t BotRightX; + uint8_t BotRightY; + + LOG_FUNCTION_START(""); + + VL53LX_get_zone_config(Dev, &zone_cfg); + + TopLeftX = (2 * zone_cfg.user_zones[0].x_centre - + zone_cfg.user_zones[0].width) >> 1; + TopLeftY = (2 * zone_cfg.user_zones[0].y_centre + + zone_cfg.user_zones[0].height) >> 1; + BotRightX = (2 * zone_cfg.user_zones[0].x_centre + + zone_cfg.user_zones[0].width) >> 1; + BotRightY = (2 * zone_cfg.user_zones[0].y_centre - + zone_cfg.user_zones[0].height) >> 1; + pRoi->TopLeftX = TopLeftX; + pRoi->TopLeftY = TopLeftY; + pRoi->BotRightX = BotRightX; + pRoi->BotRightY = BotRightY; + + LOG_FUNCTION_END(Status); + return Status; +} + + + + + +VL53LX_Error VL53LX_StartMeasurement(VL53LX_DEV Dev) +{ +#define TIMED_MODE_TIMING_GUARD_MILLISECONDS 4 + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint8_t DeviceMeasurementMode; + VL53LX_Error lStatus; + uint32_t MTBus, IMPms; + uint8_t i; + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + LOG_FUNCTION_START(""); + + VL53LX_load_patch(Dev); + for (i = 0; i < VL53LX_MAX_RANGE_RESULTS; i++) { + pdev->PreviousRangeMilliMeter[i] = 0; + pdev->PreviousRangeStatus[i] = 255; + pdev->PreviousExtendedRange[i] = 0; + } + pdev->PreviousStreamCount = 0; + + DeviceMeasurementMode = VL53LXDevDataGet(Dev, LLData.measurement_mode); + + + if ((Status == VL53LX_ERROR_NONE) && + (DeviceMeasurementMode == VL53LX_DEVICEMEASUREMENTMODE_TIMED)) { + lStatus = VL53LX_GetMeasurementTimingBudgetMicroSeconds(Dev, + &MTBus); + + MTBus /= 1000; + lStatus = GetInterMeasurementPeriodMilliSeconds(Dev, + &IMPms); + + SUPPRESS_UNUSED_WARNING(lStatus); + if (IMPms < MTBus + TIMED_MODE_TIMING_GUARD_MILLISECONDS) + Status = VL53LX_ERROR_INVALID_PARAMS; + } + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_init_and_start_range( + Dev, + DeviceMeasurementMode, + VL53LX_DEVICECONFIGLEVEL_FULL); + + if ( Dev->EnableInterrupt == 1) + { + enable_interrupt_measure_detection_irq(); + attach_interrupt_measure_detection_irq(Dev->Interrupt_Func ); + } + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_StopMeasurement(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + if ( Dev->EnableInterrupt == 1) + { + disable_interrupt_measure_detection_irq(); + } + + Status = VL53LX_stop_range(Dev); + + VL53LX_unload_patch(Dev); + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_ClearInterruptAndStartMeasurement(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint8_t DeviceMeasurementMode; + + LOG_FUNCTION_START(""); + + DeviceMeasurementMode = VL53LXDevDataGet(Dev, LLData.measurement_mode); + + Status = VL53LX_clear_interrupt_and_enable_next_range(Dev, + DeviceMeasurementMode); + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_GetMeasurementDataReady(VL53LX_DEV Dev, + uint8_t *pMeasurementDataReady) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + Status = VL53LX_is_new_data_ready(Dev, pMeasurementDataReady); + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_WaitMeasurementDataReady(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + + + Status = VL53LX_poll_for_range_completion(Dev, + VL53LX_RANGE_COMPLETION_POLLING_TIMEOUT_MS); + + LOG_FUNCTION_END(Status); + return Status; +} + +static uint8_t ConvertStatusHisto(uint8_t FilteredRangeStatus) +{ + uint8_t RangeStatus; + + switch (FilteredRangeStatus) { + case VL53LX_DEVICEERROR_RANGEPHASECHECK: + RangeStatus = VL53LX_RANGESTATUS_OUTOFBOUNDS_FAIL; + break; + case VL53LX_DEVICEERROR_SIGMATHRESHOLDCHECK: + RangeStatus = VL53LX_RANGESTATUS_SIGMA_FAIL; + break; + case VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK: + RangeStatus = + VL53LX_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL; + break; + case VL53LX_DEVICEERROR_PHASECONSISTENCY: + RangeStatus = VL53LX_RANGESTATUS_WRAP_TARGET_FAIL; + break; + case VL53LX_DEVICEERROR_PREV_RANGE_NO_TARGETS: + RangeStatus = VL53LX_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL; + break; + case VL53LX_DEVICEERROR_EVENTCONSISTENCY: + RangeStatus = VL53LX_RANGESTATUS_WRAP_TARGET_FAIL; + break; + case VL53LX_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE: + RangeStatus = VL53LX_RANGESTATUS_RANGE_VALID_MERGED_PULSE; + break; + case VL53LX_DEVICEERROR_RANGECOMPLETE: + RangeStatus = VL53LX_RANGESTATUS_RANGE_VALID; + break; + default: + RangeStatus = VL53LX_RANGESTATUS_NONE; + } + + return RangeStatus; +} + +static VL53LX_Error SetTargetData(VL53LX_DEV Dev, + uint8_t active_results, uint8_t streamcount, uint8_t iteration, + uint8_t device_status, VL53LX_range_data_t *presults_data, + VL53LX_TargetRangeData_t *pRangeData) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_tuning_parm_storage_t *tp = + &(pdev->tuning_parms); + uint8_t sequency; + uint8_t FilteredRangeStatus; + FixPoint1616_t AmbientRate; + FixPoint1616_t SignalRate; + FixPoint1616_t TempFix1616; + int16_t Range, RangeDiff, RangeMillimeterInit; + int32_t ExtendedRangeEnabled = 0; + uint8_t uwr_status; + int16_t AddOffset; + + SUPPRESS_UNUSED_WARNING(Dev); + + FilteredRangeStatus = presults_data->range_status & 0x1F; + + SignalRate = VL53LX_FIXPOINT97TOFIXPOINT1616( + presults_data->peak_signal_count_rate_mcps); + pRangeData->SignalRateRtnMegaCps + = SignalRate; + + AmbientRate = VL53LX_FIXPOINT97TOFIXPOINT1616( + presults_data->ambient_count_rate_mcps); + pRangeData->AmbientRateRtnMegaCps = AmbientRate; + + TempFix1616 = VL53LX_FIXPOINT97TOFIXPOINT1616( + presults_data->VL53LX_p_002); + + pRangeData->SigmaMilliMeter = TempFix1616; + + pRangeData->RangeMilliMeter = presults_data->median_range_mm; + pRangeData->RangeMaxMilliMeter = presults_data->max_range_mm; + pRangeData->RangeMinMilliMeter = presults_data->min_range_mm; + + + switch (device_status) { + case VL53LX_DEVICEERROR_MULTCLIPFAIL: + case VL53LX_DEVICEERROR_VCSELWATCHDOGTESTFAILURE: + case VL53LX_DEVICEERROR_VCSELCONTINUITYTESTFAILURE: + case VL53LX_DEVICEERROR_NOVHVVALUEFOUND: + pRangeData->RangeStatus = VL53LX_RANGESTATUS_HARDWARE_FAIL; + break; + case VL53LX_DEVICEERROR_USERROICLIP: + pRangeData->RangeStatus = VL53LX_RANGESTATUS_MIN_RANGE_FAIL; + break; + default: + pRangeData->RangeStatus = VL53LX_RANGESTATUS_RANGE_VALID; + } + + + if ((pRangeData->RangeStatus == VL53LX_RANGESTATUS_RANGE_VALID) && + (active_results == 0)) { + pRangeData->RangeStatus = VL53LX_RANGESTATUS_NONE; + pRangeData->SignalRateRtnMegaCps = 0; + pRangeData->SigmaMilliMeter = 0; + pRangeData->RangeMilliMeter = 8191; + pRangeData->RangeMaxMilliMeter = 8191; + pRangeData->RangeMinMilliMeter = 8191; + } + + + if (pRangeData->RangeStatus == VL53LX_RANGESTATUS_RANGE_VALID) + pRangeData->RangeStatus = + ConvertStatusHisto(FilteredRangeStatus); + + + + VL53LX_get_tuning_parm(Dev, VL53LX_TUNINGPARM_UWR_ENABLE, + &ExtendedRangeEnabled); + + sequency = streamcount % 2; + uwr_status = 1; + RangeMillimeterInit = pRangeData->RangeMilliMeter; + AddOffset = 0; + + pRangeData->ExtendedRange = 0; + + if (ExtendedRangeEnabled && + (pRangeData->RangeStatus == + VL53LX_RANGESTATUS_WRAP_TARGET_FAIL || + pRangeData->RangeStatus == + VL53LX_RANGESTATUS_OUTOFBOUNDS_FAIL) + && (pdev->PreviousRangeStatus[iteration] == + VL53LX_RANGESTATUS_WRAP_TARGET_FAIL || + pdev->PreviousRangeStatus[iteration] == + VL53LX_RANGESTATUS_OUTOFBOUNDS_FAIL || + (pdev->PreviousRangeStatus[iteration] == + VL53LX_RANGESTATUS_RANGE_VALID && + pdev->PreviousExtendedRange[iteration] == 1))) + { + if (((pdev->PreviousStreamCount) == + (pdev->hist_data.result__stream_count - 1 )) + || ((pdev->PreviousStreamCount) == + (pdev->hist_data.result__stream_count + 127))) + { + RangeDiff = pRangeData->RangeMilliMeter - + pdev->PreviousRangeMilliMeter[iteration]; + + switch (pdev->preset_mode) { + case VL53LX_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE: + + uwr_status = 0; + break; + + case VL53LX_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE: + if (RangeDiff > tp->tp_uwr_med_z_1_min && + RangeDiff < tp->tp_uwr_med_z_1_max && + sequency == 1) { + AddOffset = + tp->tp_uwr_med_corr_z_1_rangeb; + } + else + if (RangeDiff < -tp->tp_uwr_med_z_1_min && + RangeDiff > -tp->tp_uwr_med_z_1_max && + sequency == 0) { + AddOffset = + tp->tp_uwr_med_corr_z_1_rangea; + } + else + if (RangeDiff > tp->tp_uwr_med_z_2_min && + RangeDiff < tp->tp_uwr_med_z_2_max && + sequency == 0) { + AddOffset = + tp->tp_uwr_med_corr_z_2_rangea; + } + else + if (RangeDiff < -tp->tp_uwr_med_z_2_min && + RangeDiff > -tp->tp_uwr_med_z_2_max && + sequency == 1) { + AddOffset = + tp->tp_uwr_med_corr_z_2_rangeb; + } + else + if (RangeDiff > tp->tp_uwr_med_z_3_min && + RangeDiff < tp->tp_uwr_med_z_3_max && + sequency == 1) { + AddOffset = + tp->tp_uwr_med_corr_z_3_rangeb; + } + else + if (RangeDiff < -tp->tp_uwr_med_z_3_min && + RangeDiff > -tp->tp_uwr_med_z_3_max && + sequency == 0) { + AddOffset = + tp->tp_uwr_med_corr_z_3_rangea; + } + else + if (RangeDiff > tp->tp_uwr_med_z_4_min && + RangeDiff < tp->tp_uwr_med_z_4_max && + sequency == 0) { + AddOffset = + tp->tp_uwr_med_corr_z_4_rangea; + } + else + if (RangeDiff < -tp->tp_uwr_med_z_4_min && + RangeDiff > -tp->tp_uwr_med_z_4_max && + sequency == 1) { + AddOffset = + tp->tp_uwr_med_corr_z_4_rangeb; + } + else + if (RangeDiff < tp->tp_uwr_med_z_5_max && + RangeDiff > tp->tp_uwr_med_z_5_min) { + AddOffset = + tp->tp_uwr_med_corr_z_5_rangea; + } else + uwr_status = 0; + break; + + case VL53LX_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE: + if (RangeDiff > tp->tp_uwr_lng_z_1_min && + RangeDiff < tp->tp_uwr_lng_z_1_max && + sequency == 0) { + AddOffset = + tp->tp_uwr_lng_corr_z_1_rangea; + } + else + if (RangeDiff < -tp->tp_uwr_lng_z_1_min && + RangeDiff > -tp->tp_uwr_lng_z_1_max && + sequency == 1) { + AddOffset = + tp->tp_uwr_lng_corr_z_1_rangeb; + } + else + if (RangeDiff > tp->tp_uwr_lng_z_2_min && + RangeDiff < tp->tp_uwr_lng_z_2_max && + sequency == 1) { + AddOffset = + tp->tp_uwr_lng_corr_z_2_rangeb; + } + else + if (RangeDiff < -tp->tp_uwr_lng_z_2_min && + RangeDiff > -tp->tp_uwr_lng_z_2_max && + sequency == 0) { + AddOffset = + tp->tp_uwr_lng_corr_z_2_rangea; + } + else + if (RangeDiff < tp->tp_uwr_lng_z_3_max && + RangeDiff > tp->tp_uwr_lng_z_3_min) { + AddOffset = + tp->tp_uwr_lng_corr_z_3_rangea; + } + else + uwr_status = 0; + break; + + default: + uwr_status = 0; + break; + } + } + + if (uwr_status) { + pRangeData->RangeMilliMeter += AddOffset; + pRangeData->RangeMinMilliMeter += AddOffset; + pRangeData->RangeMaxMilliMeter += AddOffset; + pRangeData->ExtendedRange = 1; + pRangeData->RangeStatus = 0; + } + + } + + pdev->PreviousRangeMilliMeter[iteration] = RangeMillimeterInit; + pdev->PreviousRangeStatus[iteration] = pRangeData->RangeStatus; + pdev->PreviousExtendedRange[iteration] = pRangeData->ExtendedRange; + pdev->PreviousStreamCount = pdev->hist_data.result__stream_count; + + Range = pRangeData->RangeMilliMeter; + if ((pRangeData->RangeStatus == VL53LX_RANGESTATUS_RANGE_VALID) && + (Range < 0)) { + if (Range < BDTable[VL53LX_TUNING_PROXY_MIN]) + pRangeData->RangeStatus = + VL53LX_RANGESTATUS_RANGE_INVALID; + else + pRangeData->RangeMilliMeter = 0; + } + + return Status; +} + + +static VL53LX_Error SetMeasurementData(VL53LX_DEV Dev, + VL53LX_range_results_t *presults, + VL53LX_MultiRangingData_t *pMultiRangingData) +{ + VL53LX_LLDriverData_t *pdev = VL53LXDevStructGetLLDriverHandle(Dev); + uint8_t i; + uint8_t iteration; + VL53LX_TargetRangeData_t *pRangeData; + VL53LX_range_data_t *presults_data; + VL53LX_Error Status = VL53LX_ERROR_NONE; + uint8_t ActiveResults; + + pMultiRangingData->NumberOfObjectsFound = presults->active_results; + pMultiRangingData->HasXtalkValueChanged = + presults->smudge_corrector_data.new_xtalk_applied_flag; + + + pMultiRangingData->TimeStamp = 0; + + pMultiRangingData->StreamCount = presults->stream_count; + + ActiveResults = presults->active_results; + if (ActiveResults < 1) + + iteration = 1; + else + iteration = ActiveResults; + for (i = 0; i < iteration; i++) { + pRangeData = &(pMultiRangingData->RangeData[i]); + + presults_data = &(presults->VL53LX_p_003[i]); + if (Status == VL53LX_ERROR_NONE) + Status = SetTargetData(Dev, ActiveResults, + pMultiRangingData->StreamCount, + i, + presults->device_status, + presults_data, + pRangeData); + + pMultiRangingData->EffectiveSpadRtnCount = + presults_data->VL53LX_p_004; + + } + + for (i = iteration; i < VL53LX_MAX_RANGE_RESULTS; i++) { + pdev->PreviousRangeMilliMeter[i] = 0; + pdev->PreviousRangeStatus[i] = 255; + pdev->PreviousExtendedRange[i] = 0; + } + + return Status; +} + + +VL53LX_Error VL53LX_GetMultiRangingData(VL53LX_DEV Dev, + VL53LX_MultiRangingData_t *pMultiRangingData) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_LLDriverData_t *pdev = + VL53LXDevStructGetLLDriverHandle(Dev); + VL53LX_range_results_t *presults = + (VL53LX_range_results_t *) pdev->wArea1; + + LOG_FUNCTION_START(""); + + + memset(pMultiRangingData, 0xFF, + sizeof(VL53LX_MultiRangingData_t)); + + + Status = VL53LX_get_device_results( + Dev, + VL53LX_DEVICERESULTSLEVEL_FULL, + presults); + + Status = SetMeasurementData(Dev, + presults, + pMultiRangingData); + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetAdditionalData(VL53LX_DEV Dev, + VL53LX_AdditionalData_t *pAdditionalData) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + Status = VL53LX_get_additional_data(Dev, pAdditionalData); + + LOG_FUNCTION_END(Status); + return Status; +} + + + + + + +VL53LX_Error VL53LX_SetTuningParameter(VL53LX_DEV Dev, + uint16_t TuningParameterId, int32_t TuningParameterValue) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + if (TuningParameterId == + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS) + return VL53LX_ERROR_INVALID_PARAMS; + + if (TuningParameterId >= 32768) + Status = VL53LX_set_tuning_parm(Dev, + TuningParameterId, + TuningParameterValue); + else { + if (TuningParameterId < VL53LX_TUNING_MAX_TUNABLE_KEY) + BDTable[TuningParameterId] = TuningParameterValue; + else + Status = VL53LX_ERROR_INVALID_PARAMS; + } + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetTuningParameter(VL53LX_DEV Dev, + uint16_t TuningParameterId, int32_t *pTuningParameterValue) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + if (TuningParameterId >= 32768) + Status = VL53LX_get_tuning_parm(Dev, + TuningParameterId, + pTuningParameterValue); + else { + if (TuningParameterId < VL53LX_TUNING_MAX_TUNABLE_KEY) + *pTuningParameterValue = BDTable[TuningParameterId]; + else + Status = VL53LX_ERROR_INVALID_PARAMS; + } + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_PerformRefSpadManagement(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_Error RawStatus; + uint8_t dcrbuffer[24]; + uint8_t *commbuf; + uint8_t numloc[2] = {5, 3}; + VL53LX_LLDriverData_t *pdev; + VL53LX_customer_nvm_managed_t *pc; + VL53LX_DistanceModes DistanceMode; + + LOG_FUNCTION_START(""); + + pdev = VL53LXDevStructGetLLDriverHandle(Dev); + pc = &pdev->customer; + + if (Status == VL53LX_ERROR_NONE) { + DistanceMode = VL53LXDevDataGet(Dev, + CurrentParameters.DistanceMode); + Status = VL53LX_run_ref_spad_char(Dev, &RawStatus); + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_SetDistanceMode(Dev, DistanceMode); + } + + if (Status == VL53LX_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH) { + + Status = VL53LX_read_nvm_raw_data(Dev, + (uint8_t)(0xA0 >> 2), + (uint8_t)(24 >> 2), + dcrbuffer); + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_WriteMulti(Dev, + VL53LX_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS, + numloc, 2); + + if (Status == VL53LX_ERROR_NONE) { + pc->ref_spad_man__num_requested_ref_spads = numloc[0]; + pc->ref_spad_man__ref_location = numloc[1]; + } + + if (Status == VL53LX_ERROR_NONE) + commbuf = &dcrbuffer[16]; + + + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_WriteMulti(Dev, + VL53LX_GLOBAL_CONFIG__SPAD_ENABLES_REF_0, + commbuf, 6); + + if (Status == VL53LX_ERROR_NONE) { + pc->global_config__spad_enables_ref_0 = commbuf[0]; + pc->global_config__spad_enables_ref_1 = commbuf[1]; + pc->global_config__spad_enables_ref_2 = commbuf[2]; + pc->global_config__spad_enables_ref_3 = commbuf[3]; + pc->global_config__spad_enables_ref_4 = commbuf[4]; + pc->global_config__spad_enables_ref_5 = commbuf[5]; + } + + } + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_SmudgeCorrectionEnable(VL53LX_DEV Dev, + VL53LX_SmudgeCorrectionModes Mode) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_Error s1 = VL53LX_ERROR_NONE; + VL53LX_Error s2 = VL53LX_ERROR_NONE; + VL53LX_Error s3 = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + switch (Mode) { + case VL53LX_SMUDGE_CORRECTION_NONE: + s1 = VL53LX_dynamic_xtalk_correction_disable(Dev); + s2 = VL53LX_dynamic_xtalk_correction_apply_disable(Dev); + s3 = VL53LX_dynamic_xtalk_correction_single_apply_disable(Dev); + break; + case VL53LX_SMUDGE_CORRECTION_CONTINUOUS: + s1 = VL53LX_dynamic_xtalk_correction_enable(Dev); + s2 = VL53LX_dynamic_xtalk_correction_apply_enable(Dev); + s3 = VL53LX_dynamic_xtalk_correction_single_apply_disable(Dev); + break; + case VL53LX_SMUDGE_CORRECTION_SINGLE: + s1 = VL53LX_dynamic_xtalk_correction_enable(Dev); + s2 = VL53LX_dynamic_xtalk_correction_apply_enable(Dev); + s3 = VL53LX_dynamic_xtalk_correction_single_apply_enable(Dev); + break; + case VL53LX_SMUDGE_CORRECTION_DEBUG: + s1 = VL53LX_dynamic_xtalk_correction_enable(Dev); + s2 = VL53LX_dynamic_xtalk_correction_apply_disable(Dev); + s3 = VL53LX_dynamic_xtalk_correction_single_apply_disable(Dev); + break; + default: + Status = VL53LX_ERROR_INVALID_PARAMS; + break; + } + + if (Status == VL53LX_ERROR_NONE) { + Status = s1; + if (Status == VL53LX_ERROR_NONE) + Status = s2; + if (Status == VL53LX_ERROR_NONE) + Status = s3; + } + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_SetXTalkCompensationEnable(VL53LX_DEV Dev, + uint8_t XTalkCompensationEnable) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + if (XTalkCompensationEnable == 0) + Status = VL53LX_disable_xtalk_compensation(Dev); + else + Status = VL53LX_enable_xtalk_compensation(Dev); + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_GetXTalkCompensationEnable(VL53LX_DEV Dev, + uint8_t *pXTalkCompensationEnable) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + + LOG_FUNCTION_START(""); + + VL53LX_get_xtalk_compensation_enable( + Dev, + pXTalkCompensationEnable); + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_PerformXTalkCalibration(VL53LX_DEV Dev) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_Error UStatus; + int16_t CalDistanceMm; + VL53LX_xtalk_calibration_results_t xtalk; + + VL53LX_CalibrationData_t caldata; + VL53LX_LLDriverData_t *pLLData; + int i; + uint32_t *pPlaneOffsetKcps; + uint32_t Margin = + BDTable[VL53LX_TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN]; + uint32_t DefaultOffset = + BDTable[VL53LX_TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET]; + uint32_t *pLLDataPlaneOffsetKcps; + uint32_t sum = 0; + uint8_t binok = 0; + + LOG_FUNCTION_START(""); + + pPlaneOffsetKcps = + &caldata.customer.algo__crosstalk_compensation_plane_offset_kcps; + pLLData = VL53LXDevStructGetLLDriverHandle(Dev); + pLLDataPlaneOffsetKcps = + &pLLData->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps; + + CalDistanceMm = (int16_t) + BDTable[VL53LX_TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM]; + Status = VL53LX_run_hist_xtalk_extraction(Dev, CalDistanceMm, + &UStatus); + + VL53LX_GetCalibrationData(Dev, &caldata); + for (i = 0; i < VL53LX_XTALK_HISTO_BINS; i++) { + sum += caldata.xtalkhisto.xtalk_shape.bin_data[i]; + if (caldata.xtalkhisto.xtalk_shape.bin_data[i] > 0) + binok++; + } + if ((UStatus == + VL53LX_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL) || + (sum > (1024 + Margin)) || (sum < (1024 - Margin)) || + (binok < 3)) { + *pPlaneOffsetKcps = DefaultOffset; + *pLLDataPlaneOffsetKcps = DefaultOffset; + caldata.xtalkhisto.xtalk_shape.bin_data[0] = 307; + caldata.xtalkhisto.xtalk_shape.bin_data[1] = 410; + caldata.xtalkhisto.xtalk_shape.bin_data[2] = 410; + caldata.xtalkhisto.xtalk_shape.bin_data[3] = 307; + for (i = 4; i < VL53LX_XTALK_HISTO_BINS; i++) + caldata.xtalkhisto.xtalk_shape.bin_data[i] = 0; + for (i = 0; i < VL53LX_BIN_REC_SIZE; i++) + caldata.algo__xtalk_cpo_HistoMerge_kcps[i] = + DefaultOffset + DefaultOffset * i; + VL53LX_SetCalibrationData(Dev, &caldata); + } + + if (Status == VL53LX_ERROR_NONE) { + Status = VL53LX_get_current_xtalk_settings(Dev, &xtalk); + Status = VL53LX_set_tuning_parm(Dev, + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS, + xtalk.algo__crosstalk_compensation_plane_offset_kcps); + } + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_SetOffsetCorrectionMode(VL53LX_DEV Dev, + VL53LX_OffsetCorrectionModes OffsetCorrectionMode) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_OffsetCorrectionMode offset_cor_mode; + + LOG_FUNCTION_START(""); + + if (OffsetCorrectionMode == VL53LX_OFFSETCORRECTIONMODE_STANDARD) { + offset_cor_mode = + VL53LX_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS; + } else if (OffsetCorrectionMode == + VL53LX_OFFSETCORRECTIONMODE_PERVCSEL) { + offset_cor_mode = + VL53LX_OFFSETCORRECTIONMODE__PER_VCSEL_OFFSETS; + } else { + Status = VL53LX_ERROR_INVALID_PARAMS; + } + + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_set_offset_correction_mode(Dev, + offset_cor_mode); + + LOG_FUNCTION_END(Status); + return Status; +} + + +VL53LX_Error VL53LX_PerformOffsetSimpleCalibration(VL53LX_DEV Dev, + int32_t CalDistanceMilliMeter) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + int32_t sum_ranging; + uint8_t offset_meas; + int16_t Max, UnderMax, OverMax, Repeat; + int32_t total_count, inloopcount; + int32_t IncRounding; + int16_t meanDistance_mm; + int16_t offset; + VL53LX_MultiRangingData_t RangingMeasurementData; + VL53LX_LLDriverData_t *pdev; + uint8_t goodmeas; + VL53LX_Error SmudgeStatus = VL53LX_ERROR_NONE; + uint8_t smudge_corr_en; + VL53LX_TargetRangeData_t *pRange; + + LOG_FUNCTION_START(""); + + pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled; + SmudgeStatus = VL53LX_dynamic_xtalk_correction_disable(Dev); + + pdev->customer.algo__part_to_part_range_offset_mm = 0; + pdev->customer.mm_config__inner_offset_mm = 0; + pdev->customer.mm_config__outer_offset_mm = 0; + memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data)); + Repeat = BDTable[VL53LX_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT]; + Max = BDTable[ + VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER]; + UnderMax = 1 + (Max / 2); + OverMax = Max + (Max / 2); + sum_ranging = 0; + total_count = 0; + + while ((Repeat > 0) && (Status == VL53LX_ERROR_NONE)) { + Status = VL53LX_StartMeasurement(Dev); + + if (Status == VL53LX_ERROR_NONE) { + VL53LX_WaitMeasurementDataReady(Dev); + VL53LX_GetMultiRangingData(Dev, + &RangingMeasurementData); + VL53LX_ClearInterruptAndStartMeasurement(Dev); + } + + inloopcount = 0; + offset_meas = 0; + while ((Status == VL53LX_ERROR_NONE) && (inloopcount < Max) && + (offset_meas < OverMax)) { + Status = VL53LX_WaitMeasurementDataReady(Dev); + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_GetMultiRangingData(Dev, + &RangingMeasurementData); + pRange = &(RangingMeasurementData.RangeData[0]); + goodmeas = (pRange->RangeStatus == + VL53LX_RANGESTATUS_RANGE_VALID); + if ((Status == VL53LX_ERROR_NONE) && goodmeas) { + sum_ranging += pRange->RangeMilliMeter; + inloopcount++; + } + Status = VL53LX_ClearInterruptAndStartMeasurement(Dev); + offset_meas++; + } + total_count += inloopcount; + + + if (inloopcount < UnderMax) + Status = VL53LX_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL; + + VL53LX_StopMeasurement(Dev); + + Repeat--; + + } + + if ((SmudgeStatus == VL53LX_ERROR_NONE) && (smudge_corr_en == 1)) + SmudgeStatus = VL53LX_dynamic_xtalk_correction_enable(Dev); + + if ((sum_ranging < 0) || + (sum_ranging > ((int32_t) total_count * 0xffff))) + Status = VL53LX_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH; + + if ((Status == VL53LX_ERROR_NONE) && (total_count > 0)) { + IncRounding = total_count / 2; + meanDistance_mm = (int16_t)((sum_ranging + IncRounding) + / total_count); + offset = (int16_t)CalDistanceMilliMeter - meanDistance_mm; + pdev->customer.algo__part_to_part_range_offset_mm = 0; + pdev->customer.mm_config__inner_offset_mm = offset; + pdev->customer.mm_config__outer_offset_mm = offset; + + Status = VL53LX_set_customer_nvm_managed(Dev, + &(pdev->customer)); + } + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_PerformOffsetZeroDistanceCalibration(VL53LX_DEV Dev) +{ + #define START_OFFSET 50 + VL53LX_Error Status = VL53LX_ERROR_NONE; + int32_t sum_ranging; + uint8_t offset_meas; + int16_t Max, UnderMax, OverMax, Repeat; + int32_t total_count, inloopcount; + int32_t IncRounding; + int16_t meanDistance_mm; + int16_t offset, ZeroDistanceOffset; + VL53LX_MultiRangingData_t RangingMeasurementData; + VL53LX_LLDriverData_t *pdev; + uint8_t goodmeas; + VL53LX_Error SmudgeStatus = VL53LX_ERROR_NONE; + uint8_t smudge_corr_en; + VL53LX_TargetRangeData_t *pRange; + + LOG_FUNCTION_START(""); + + pdev = VL53LXDevStructGetLLDriverHandle(Dev); + smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled; + SmudgeStatus = VL53LX_dynamic_xtalk_correction_disable(Dev); + pdev->customer.algo__part_to_part_range_offset_mm = 0; + pdev->customer.mm_config__inner_offset_mm = START_OFFSET; + pdev->customer.mm_config__outer_offset_mm = START_OFFSET; + memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data)); + ZeroDistanceOffset = BDTable[ + VL53LX_TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR]; + Repeat = BDTable[VL53LX_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT]; + Max = + BDTable[VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER]; + UnderMax = 1 + (Max / 2); + OverMax = Max + (Max / 2); + sum_ranging = 0; + total_count = 0; + + while ((Repeat > 0) && (Status == VL53LX_ERROR_NONE)) { + Status = VL53LX_StartMeasurement(Dev); + if (Status == VL53LX_ERROR_NONE) { + VL53LX_WaitMeasurementDataReady(Dev); + VL53LX_GetMultiRangingData(Dev, + &RangingMeasurementData); + VL53LX_ClearInterruptAndStartMeasurement(Dev); + } + inloopcount = 0; + offset_meas = 0; + while ((Status == VL53LX_ERROR_NONE) && (inloopcount < Max) && + (offset_meas < OverMax)) { + Status = VL53LX_WaitMeasurementDataReady(Dev); + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_GetMultiRangingData(Dev, + &RangingMeasurementData); + pRange = &(RangingMeasurementData.RangeData[0]); + goodmeas = (pRange->RangeStatus == + VL53LX_RANGESTATUS_RANGE_VALID); + if ((Status == VL53LX_ERROR_NONE) && goodmeas) { + sum_ranging = sum_ranging + + pRange->RangeMilliMeter; + inloopcount++; + } + Status = VL53LX_ClearInterruptAndStartMeasurement(Dev); + offset_meas++; + } + total_count += inloopcount; + if (inloopcount < UnderMax) + Status = VL53LX_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL; + VL53LX_StopMeasurement(Dev); + Repeat--; + } + if ((SmudgeStatus == VL53LX_ERROR_NONE) && (smudge_corr_en == 1)) + SmudgeStatus = VL53LX_dynamic_xtalk_correction_enable(Dev); + if ((sum_ranging < 0) || + (sum_ranging > ((int32_t) total_count * 0xffff))) + Status = VL53LX_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH; + + if ((Status == VL53LX_ERROR_NONE) && (total_count > 0)) { + IncRounding = total_count / 2; + meanDistance_mm = (int16_t) + ((sum_ranging + IncRounding) / total_count); + offset = START_OFFSET - meanDistance_mm + ZeroDistanceOffset; + pdev->customer.algo__part_to_part_range_offset_mm = 0; + pdev->customer.mm_config__inner_offset_mm = offset; + pdev->customer.mm_config__outer_offset_mm = offset; + Status = VL53LX_set_customer_nvm_managed(Dev, + &(pdev->customer)); + } + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_SetCalibrationData(VL53LX_DEV Dev, + VL53LX_CalibrationData_t *pCalibrationData) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_CustomerNvmManaged_t *pC; + VL53LX_calibration_data_t cal_data; + uint32_t x; + VL53LX_xtalk_calibration_results_t xtalk; + + LOG_FUNCTION_START(""); + + cal_data.struct_version = pCalibrationData->struct_version - + VL53LX_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION; + + + memcpy( + &(cal_data.add_off_cal_data), + &(pCalibrationData->add_off_cal_data), + sizeof(VL53LX_additional_offset_cal_data_t)); + + + memcpy( + &(cal_data.optical_centre), + &(pCalibrationData->optical_centre), + sizeof(VL53LX_optical_centre_t)); + + + memcpy( + &(cal_data.xtalkhisto), + &(pCalibrationData->xtalkhisto), + sizeof(VL53LX_xtalk_histogram_data_t)); + + + memcpy( + &(cal_data.gain_cal), + &(pCalibrationData->gain_cal), + sizeof(VL53LX_gain_calibration_data_t)); + + + memcpy( + &(cal_data.cal_peak_rate_map), + &(pCalibrationData->cal_peak_rate_map), + sizeof(VL53LX_cal_peak_rate_map_t)); + + + memcpy( + &(cal_data.per_vcsel_cal_data), + &(pCalibrationData->per_vcsel_cal_data), + sizeof(VL53LX_per_vcsel_period_offset_cal_data_t)); + + pC = &pCalibrationData->customer; + x = pC->algo__crosstalk_compensation_plane_offset_kcps; + cal_data.customer.algo__crosstalk_compensation_plane_offset_kcps = + (uint16_t)(x&0x0000FFFF); + + cal_data.customer.global_config__spad_enables_ref_0 = + pC->global_config__spad_enables_ref_0; + cal_data.customer.global_config__spad_enables_ref_1 = + pC->global_config__spad_enables_ref_1; + cal_data.customer.global_config__spad_enables_ref_2 = + pC->global_config__spad_enables_ref_2; + cal_data.customer.global_config__spad_enables_ref_3 = + pC->global_config__spad_enables_ref_3; + cal_data.customer.global_config__spad_enables_ref_4 = + pC->global_config__spad_enables_ref_4; + cal_data.customer.global_config__spad_enables_ref_5 = + pC->global_config__spad_enables_ref_5; + cal_data.customer.global_config__ref_en_start_select = + pC->global_config__ref_en_start_select; + cal_data.customer.ref_spad_man__num_requested_ref_spads = + pC->ref_spad_man__num_requested_ref_spads; + cal_data.customer.ref_spad_man__ref_location = + pC->ref_spad_man__ref_location; + cal_data.customer.algo__crosstalk_compensation_x_plane_gradient_kcps = + pC->algo__crosstalk_compensation_x_plane_gradient_kcps; + cal_data.customer.algo__crosstalk_compensation_y_plane_gradient_kcps = + pC->algo__crosstalk_compensation_y_plane_gradient_kcps; + cal_data.customer.ref_spad_char__total_rate_target_mcps = + pC->ref_spad_char__total_rate_target_mcps; + cal_data.customer.algo__part_to_part_range_offset_mm = + pC->algo__part_to_part_range_offset_mm; + cal_data.customer.mm_config__inner_offset_mm = + pC->mm_config__inner_offset_mm; + cal_data.customer.mm_config__outer_offset_mm = + pC->mm_config__outer_offset_mm; + + Status = VL53LX_set_part_to_part_data(Dev, &cal_data); + + if (Status != VL53LX_ERROR_NONE) + goto ENDFUNC; + + Status = VL53LX_get_current_xtalk_settings(Dev, &xtalk); + + if (Status != VL53LX_ERROR_NONE) + goto ENDFUNC; + + xtalk.algo__crosstalk_compensation_plane_offset_kcps = x; + + Status = VL53LX_set_tuning_parm(Dev, + VL53LX_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS, + x); + + + memcpy( + &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]), + &(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]), + sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps)); + + Status = VL53LX_set_current_xtalk_settings(Dev, &xtalk); + +ENDFUNC: + LOG_FUNCTION_END(Status); + return Status; + +} + +VL53LX_Error VL53LX_GetCalibrationData(VL53LX_DEV Dev, + VL53LX_CalibrationData_t *pCalibrationData) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_calibration_data_t cal_data; + VL53LX_CustomerNvmManaged_t *pC; + VL53LX_customer_nvm_managed_t *pC2; + VL53LX_xtalk_calibration_results_t xtalk; + uint32_t tmp; + + LOG_FUNCTION_START(""); + + + Status = VL53LX_get_part_to_part_data(Dev, &cal_data); + + pCalibrationData->struct_version = cal_data.struct_version + + VL53LX_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION; + + + memcpy( + &(pCalibrationData->add_off_cal_data), + &(cal_data.add_off_cal_data), + sizeof(VL53LX_additional_offset_cal_data_t)); + + + memcpy( + &(pCalibrationData->optical_centre), + &(cal_data.optical_centre), + sizeof(VL53LX_optical_centre_t)); + + + memcpy( + &(pCalibrationData->xtalkhisto), + &(cal_data.xtalkhisto), + sizeof(VL53LX_xtalk_histogram_data_t)); + + memcpy( + &(pCalibrationData->gain_cal), + &(cal_data.gain_cal), + sizeof(VL53LX_gain_calibration_data_t)); + + + memcpy( + &(pCalibrationData->cal_peak_rate_map), + &(cal_data.cal_peak_rate_map), + sizeof(VL53LX_cal_peak_rate_map_t)); + + + memcpy( + &(pCalibrationData->per_vcsel_cal_data), + &(cal_data.per_vcsel_cal_data), + sizeof(VL53LX_per_vcsel_period_offset_cal_data_t)); + + pC = &pCalibrationData->customer; + pC2 = &cal_data.customer; + pC->global_config__spad_enables_ref_0 = + pC2->global_config__spad_enables_ref_0; + pC->global_config__spad_enables_ref_1 = + pC2->global_config__spad_enables_ref_1; + pC->global_config__spad_enables_ref_2 = + pC2->global_config__spad_enables_ref_2; + pC->global_config__spad_enables_ref_3 = + pC2->global_config__spad_enables_ref_3; + pC->global_config__spad_enables_ref_4 = + pC2->global_config__spad_enables_ref_4; + pC->global_config__spad_enables_ref_5 = + pC2->global_config__spad_enables_ref_5; + pC->global_config__ref_en_start_select = + pC2->global_config__ref_en_start_select; + pC->ref_spad_man__num_requested_ref_spads = + pC2->ref_spad_man__num_requested_ref_spads; + pC->ref_spad_man__ref_location = + pC2->ref_spad_man__ref_location; + pC->algo__crosstalk_compensation_x_plane_gradient_kcps = + pC2->algo__crosstalk_compensation_x_plane_gradient_kcps; + pC->algo__crosstalk_compensation_y_plane_gradient_kcps = + pC2->algo__crosstalk_compensation_y_plane_gradient_kcps; + pC->ref_spad_char__total_rate_target_mcps = + pC2->ref_spad_char__total_rate_target_mcps; + pC->algo__part_to_part_range_offset_mm = + pC2->algo__part_to_part_range_offset_mm; + pC->mm_config__inner_offset_mm = + pC2->mm_config__inner_offset_mm; + pC->mm_config__outer_offset_mm = + pC2->mm_config__outer_offset_mm; + + pC->algo__crosstalk_compensation_plane_offset_kcps = + (uint32_t)( + pC2->algo__crosstalk_compensation_plane_offset_kcps); + + Status = VL53LX_get_current_xtalk_settings(Dev, &xtalk); + + if (Status != VL53LX_ERROR_NONE) + goto ENDFUNC; + + tmp = xtalk.algo__crosstalk_compensation_plane_offset_kcps; + pC->algo__crosstalk_compensation_plane_offset_kcps = tmp; + tmp = xtalk.algo__crosstalk_compensation_x_plane_gradient_kcps; + pC->algo__crosstalk_compensation_x_plane_gradient_kcps = tmp; + tmp = xtalk.algo__crosstalk_compensation_y_plane_gradient_kcps; + pC->algo__crosstalk_compensation_y_plane_gradient_kcps = tmp; + + memcpy(&(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]), + &(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]), + sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps)); +ENDFUNC: + LOG_FUNCTION_END(Status); + return Status; +} + + + +VL53LX_Error VL53LX_PerformOffsetPerVcselCalibration(VL53LX_DEV Dev, + int32_t CalDistanceMilliMeter) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + int32_t sum_ranging_range_A, sum_ranging_range_B; + uint8_t offset_meas_range_A, offset_meas_range_B; + int16_t Max, UnderMax, OverMax, Repeat; + int32_t inloopcount; + int32_t IncRounding; + int16_t meanDistance_mm; + VL53LX_MultiRangingData_t RangingMeasurementData; + VL53LX_LLDriverData_t *pdev; + uint8_t goodmeas; + VL53LX_DistanceModes currentDist; + VL53LX_DistanceModes DistMode[3] = {VL53LX_DISTANCEMODE_SHORT, + VL53LX_DISTANCEMODE_MEDIUM, VL53LX_DISTANCEMODE_LONG}; + int16_t offsetA[3] = {0, 0, 0}; + int16_t offsetB[3] = {0, 0, 0}; + + VL53LX_Error SmudgeStatus = VL53LX_ERROR_NONE; + uint8_t smudge_corr_en, ics; + VL53LX_TargetRangeData_t *pRange; + + LOG_FUNCTION_START(""); + + pdev = VL53LXDevStructGetLLDriverHandle(Dev); + + smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled; + SmudgeStatus = VL53LX_dynamic_xtalk_correction_disable(Dev); + + pdev->customer.algo__part_to_part_range_offset_mm = 0; + pdev->customer.mm_config__inner_offset_mm = 0; + pdev->customer.mm_config__outer_offset_mm = 0; + pdev->customer.mm_config__outer_offset_mm = 0; + memset(&pdev->per_vcsel_cal_data, 0, sizeof(pdev->per_vcsel_cal_data)); + + Repeat = 0; + if (IsL4(Dev)) + Repeat = 1; + Max = 2 * BDTable[ + VL53LX_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER]; + UnderMax = 1 + (Max / 2); + OverMax = Max + (Max / 2); + + Status = VL53LX_GetDistanceMode(Dev, ¤tDist); + + while ((Repeat < 3) && (Status == VL53LX_ERROR_NONE)) { + Status = VL53LX_SetDistanceMode(Dev, DistMode[Repeat]); + Status = VL53LX_StartMeasurement(Dev); + + if (Status == VL53LX_ERROR_NONE) { + VL53LX_WaitMeasurementDataReady(Dev); + VL53LX_GetMultiRangingData(Dev, + &RangingMeasurementData); + VL53LX_ClearInterruptAndStartMeasurement(Dev); + } + + inloopcount = 0; + offset_meas_range_A = 0; + sum_ranging_range_A = 0; + offset_meas_range_B = 0; + sum_ranging_range_B = 0; + while ((Status == VL53LX_ERROR_NONE) && (inloopcount < Max) && + (inloopcount < OverMax)) { + Status = VL53LX_WaitMeasurementDataReady(Dev); + if (Status == VL53LX_ERROR_NONE) + Status = VL53LX_GetMultiRangingData(Dev, + &RangingMeasurementData); + pRange = &(RangingMeasurementData.RangeData[0]); + goodmeas = (pRange->RangeStatus == + VL53LX_RANGESTATUS_RANGE_VALID); + ics = pdev->ll_state.cfg_internal_stream_count; + if ((Status == VL53LX_ERROR_NONE) && goodmeas) { + if (ics & 0x01) { + sum_ranging_range_A += + pRange->RangeMilliMeter; + offset_meas_range_A++; + } else { + sum_ranging_range_B += + pRange->RangeMilliMeter; + offset_meas_range_B++; + } + inloopcount = offset_meas_range_A + + offset_meas_range_B; + } + Status = VL53LX_ClearInterruptAndStartMeasurement(Dev); + } + + + if (inloopcount < UnderMax) + Status = VL53LX_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL; + + VL53LX_StopMeasurement(Dev); + + + if ((sum_ranging_range_A < 0) || + (sum_ranging_range_B < 0) || + (sum_ranging_range_A > + ((int32_t) offset_meas_range_A * 0xffff)) || + (sum_ranging_range_B > + ((int32_t) offset_meas_range_B * 0xffff))) { + Status = VL53LX_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH; + } + + if ((Status == VL53LX_ERROR_NONE) && + (offset_meas_range_A > 0)) { + IncRounding = offset_meas_range_A / 2; + meanDistance_mm = (int16_t) + ((sum_ranging_range_A + IncRounding) + / offset_meas_range_A); + offsetA[Repeat] = (int16_t) + CalDistanceMilliMeter - meanDistance_mm; + } + + if ((Status == VL53LX_ERROR_NONE) && + (offset_meas_range_B > 0)) { + IncRounding = offset_meas_range_B / 2; + meanDistance_mm = (int16_t) + ((sum_ranging_range_B + IncRounding) + / offset_meas_range_B); + offsetB[Repeat] = (int16_t) + CalDistanceMilliMeter - meanDistance_mm; + } + Repeat++; + } + + if ((SmudgeStatus == VL53LX_ERROR_NONE) && (smudge_corr_en == 1)) + SmudgeStatus = VL53LX_dynamic_xtalk_correction_enable(Dev); + + if (Status == VL53LX_ERROR_NONE) { + pdev->per_vcsel_cal_data.short_a_offset_mm = offsetA[0]; + pdev->per_vcsel_cal_data.short_b_offset_mm = offsetB[0]; + pdev->per_vcsel_cal_data.medium_a_offset_mm = offsetA[1]; + pdev->per_vcsel_cal_data.medium_b_offset_mm = offsetB[1]; + pdev->per_vcsel_cal_data.long_a_offset_mm = offsetA[2]; + pdev->per_vcsel_cal_data.long_b_offset_mm = offsetB[2]; + } + + VL53LX_SetDistanceMode(Dev, currentDist); + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53LX_Error VL53LX_GetOpticalCenter(VL53LX_DEV Dev, + FixPoint1616_t *pOpticalCenterX, + FixPoint1616_t *pOpticalCenterY) +{ + VL53LX_Error Status = VL53LX_ERROR_NONE; + VL53LX_calibration_data_t CalibrationData; + + LOG_FUNCTION_START(""); + + *pOpticalCenterX = 0; + *pOpticalCenterY = 0; + Status = VL53LX_get_part_to_part_data(Dev, &CalibrationData); + if (Status == VL53LX_ERROR_NONE) { + *pOpticalCenterX = VL53LX_FIXPOINT44TOFIXPOINT1616( + CalibrationData.optical_centre.x_centre); + *pOpticalCenterY = VL53LX_FIXPOINT44TOFIXPOINT1616( + CalibrationData.optical_centre.y_centre); + } + + LOG_FUNCTION_END(Status); + return Status; +} + +