ST Expansion SW Team / VL53L3CX_mbed

Rename library

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging

modules/vl53lx_api.c

Committer:
charlesmn
Date:
2021-07-21
Revision:
7:7f1bbf370283
Parent:
5:89031b2f5316

File content as of revision 7:7f1bbf370283:


// 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, &currentDist);

	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;
}