sundp
/
x-cube-tof1
mirror of https://github.com/STMicroelectronics/x-cube-tof1.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
x-cube-tof1/Drivers/BSP/Components/vl53l1cb/modules/vl53l1_api.c

4172 lines
108 KiB
C
Raw Blame History

/**
******************************************************************************
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
#include "vl53l1_api.h"
#include "vl53l1_api_strings.h"
#include "vl53l1_register_settings.h"
#include "vl53l1_register_funcs.h"
#include "vl53l1_core.h"
#include "vl53l1_api_calibration.h"
#include "vl53l1_wait.h"
#include "vl53l1_preset_setup.h"
#include "vl53l1_api_debug.h"
#include "vl53l1_api_core.h"
#include "vl53l1_nvm.h"
#define ZONE_CHECK VL53L1_MAX_USER_ZONES
#if ZONE_CHECK < 5
#error Must define at least 5 zones in MAX_USER_ZONES constant
#endif
#define LOG_FUNCTION_START(fmt, ...) \
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_API, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_API, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_API, status, \
fmt, ##__VA_ARGS__)
#ifdef VL53L1_LOG_ENABLE
#define trace_print(level, ...) trace_print_module_function(\
VL53L1_TRACE_MODULE_API, level, VL53L1_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
static int32_t BDTable[VL53L1_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 VL53L1_Error SingleTargetXTalkCalibration(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint32_t sum_ranging = 0;
uint32_t sum_spads = 0;
FixPoint1616_t sum_signalRate = 0;
FixPoint1616_t total_count = 0;
uint8_t xtalk_meas = 0;
uint8_t xtalk_measmax =
BDTable[VL53L1_TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER];
VL53L1_RangingMeasurementData_t RMData;
FixPoint1616_t xTalkStoredMeanSignalRate;
FixPoint1616_t xTalkStoredMeanRange;
FixPoint1616_t xTalkStoredMeanRtnSpads;
uint32_t xTalkStoredMeanRtnSpadsAsInt;
uint32_t xTalkCalDistanceAsInt;
FixPoint1616_t XTalkCompensationRateMegaCps;
uint32_t signalXTalkTotalPerSpad;
VL53L1_PresetModes PresetMode;
VL53L1_CalibrationData_t CalibrationData;
VL53L1_CustomerNvmManaged_t *pC;
LOG_FUNCTION_START("");
PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
if ((PresetMode != VL53L1_PRESETMODE_AUTONOMOUS) &&
(PresetMode != VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS) &&
(PresetMode != VL53L1_PRESETMODE_LITE_RANGING)) {
Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
goto ENDFUNC;
}
Status = VL53L1_disable_xtalk_compensation(Dev);
if (Status != VL53L1_ERROR_NONE)
goto ENDFUNC;
Status = VL53L1_StartMeasurement(Dev);
if (Status != VL53L1_ERROR_NONE)
goto ENDFUNC;
VL53L1_WaitMeasurementDataReady(Dev);
VL53L1_GetRangingMeasurementData(Dev, &RMData);
VL53L1_ClearInterruptAndStartMeasurement(Dev);
sum_ranging = 0;
sum_spads = 0;
sum_signalRate = 0;
total_count = 0;
for (xtalk_meas = 0; xtalk_meas < xtalk_measmax; xtalk_meas++) {
VL53L1_WaitMeasurementDataReady(Dev);
VL53L1_GetRangingMeasurementData(Dev, &RMData);
VL53L1_ClearInterruptAndStartMeasurement(Dev);
if (RMData.RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
sum_ranging += RMData.RangeMilliMeter;
sum_signalRate += RMData.SignalRateRtnMegaCps;
sum_spads += RMData.EffectiveSpadRtnCount / 256;
total_count++;
}
}
Status = VL53L1_StopMeasurement(Dev);
if (total_count > 0) {
xTalkStoredMeanSignalRate = sum_signalRate / total_count;
xTalkStoredMeanRange = (FixPoint1616_t)(sum_ranging << 16);
xTalkStoredMeanRange /= total_count;
xTalkStoredMeanRtnSpads = (FixPoint1616_t)(sum_spads << 16);
xTalkStoredMeanRtnSpads /= total_count;
xTalkStoredMeanRtnSpadsAsInt = (xTalkStoredMeanRtnSpads +
0x8000) >> 16;
xTalkCalDistanceAsInt = ((uint32_t)BDTable[
VL53L1_TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM]);
if (xTalkStoredMeanRtnSpadsAsInt == 0 ||
xTalkCalDistanceAsInt == 0 ||
xTalkStoredMeanRange >= (xTalkCalDistanceAsInt << 16)) {
XTalkCompensationRateMegaCps = 0;
} else {
signalXTalkTotalPerSpad = (xTalkStoredMeanSignalRate) /
xTalkStoredMeanRtnSpadsAsInt;
signalXTalkTotalPerSpad *= (((uint32_t)1 << 16) -
(xTalkStoredMeanRange / xTalkCalDistanceAsInt));
XTalkCompensationRateMegaCps = (signalXTalkTotalPerSpad
+ 0x8000) >> 16;
}
Status = VL53L1_GetCalibrationData(Dev, &CalibrationData);
if (Status != VL53L1_ERROR_NONE)
goto ENDFUNC;
pC = &CalibrationData.customer;
pC->algo__crosstalk_compensation_plane_offset_kcps =
(uint32_t)(1000 * ((XTalkCompensationRateMegaCps +
((uint32_t)1<<6)) >> (16-9)));
Status = VL53L1_SetCalibrationData(Dev, &CalibrationData);
if (Status != VL53L1_ERROR_NONE)
goto ENDFUNC;
Status = VL53L1_enable_xtalk_compensation(Dev);
} else
Status = VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL;
ENDFUNC:
LOG_FUNCTION_END(Status);
return Status;
}
static VL53L1_Error CheckValidRectRoi(VL53L1_UserRoi_t ROI)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if ((ROI.TopLeftX > 15) || (ROI.TopLeftY > 15) ||
(ROI.BotRightX > 15) || (ROI.BotRightY > 15))
Status = VL53L1_ERROR_INVALID_PARAMS;
if ((ROI.TopLeftX > ROI.BotRightX) || (ROI.TopLeftY < ROI.BotRightY))
Status = VL53L1_ERROR_INVALID_PARAMS;
LOG_FUNCTION_END(Status);
return Status;
}
static VL53L1_GPIO_Interrupt_Mode ConvertModeToLLD(VL53L1_Error *pStatus,
VL53L1_ThresholdMode CrossMode)
{
VL53L1_GPIO_Interrupt_Mode Mode;
switch (CrossMode) {
case VL53L1_THRESHOLD_CROSSED_LOW:
Mode = VL53L1_GPIOINTMODE_LEVEL_LOW;
break;
case VL53L1_THRESHOLD_CROSSED_HIGH:
Mode = VL53L1_GPIOINTMODE_LEVEL_HIGH;
break;
case VL53L1_THRESHOLD_OUT_OF_WINDOW:
Mode = VL53L1_GPIOINTMODE_OUT_OF_WINDOW;
break;
case VL53L1_THRESHOLD_IN_WINDOW:
Mode = VL53L1_GPIOINTMODE_IN_WINDOW;
break;
default:
Mode = VL53L1_GPIOINTMODE_LEVEL_HIGH;
*pStatus = VL53L1_ERROR_INVALID_PARAMS;
}
return Mode;
}
static VL53L1_ThresholdMode ConvertModeFromLLD(VL53L1_Error *pStatus,
VL53L1_GPIO_Interrupt_Mode CrossMode)
{
VL53L1_ThresholdMode Mode;
switch (CrossMode) {
case VL53L1_GPIOINTMODE_LEVEL_LOW:
Mode = VL53L1_THRESHOLD_CROSSED_LOW;
break;
case VL53L1_GPIOINTMODE_LEVEL_HIGH:
Mode = VL53L1_THRESHOLD_CROSSED_HIGH;
break;
case VL53L1_GPIOINTMODE_OUT_OF_WINDOW:
Mode = VL53L1_THRESHOLD_OUT_OF_WINDOW;
break;
case VL53L1_GPIOINTMODE_IN_WINDOW:
Mode = VL53L1_THRESHOLD_IN_WINDOW;
break;
default:
Mode = VL53L1_THRESHOLD_CROSSED_HIGH;
*pStatus = VL53L1_ERROR_UNDEFINED;
}
return Mode;
}
VL53L1_Error VL53L1_GetVersion(VL53L1_Version_t *pVersion)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
pVersion->major = VL53L1_IMPLEMENTATION_VER_MAJOR;
pVersion->minor = VL53L1_IMPLEMENTATION_VER_MINOR;
pVersion->build = VL53L1_IMPLEMENTATION_VER_SUB;
pVersion->revision = VL53L1_IMPLEMENTATION_VER_REVISION;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetProductRevision(VL53L1_DEV Dev,
uint8_t *pProductRevisionMajor, uint8_t *pProductRevisionMinor)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t revision_id;
VL53L1_LLDriverData_t *pLLData;
LOG_FUNCTION_START("");
pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
revision_id = pLLData->nvm_copy_data.identification__revision_id;
*pProductRevisionMajor = 1;
*pProductRevisionMinor = (revision_id & 0xF0) >> 4;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetDeviceInfo(VL53L1_DEV Dev,
VL53L1_DeviceInfo_t *pVL53L1_DeviceInfo)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t revision_id;
VL53L1_LLDriverData_t *pLLData;
LOG_FUNCTION_START("");
pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
strncpy(pVL53L1_DeviceInfo->ProductId, "",
VL53L1_DEVINFO_STRLEN-1);
pVL53L1_DeviceInfo->ProductType =
pLLData->nvm_copy_data.identification__module_type;
revision_id = pLLData->nvm_copy_data.identification__revision_id;
pVL53L1_DeviceInfo->ProductRevisionMajor = 1;
pVL53L1_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4;
#ifndef VL53L1_USE_EMPTY_STRING
if (pVL53L1_DeviceInfo->ProductRevisionMinor == 0)
strncpy(pVL53L1_DeviceInfo->Name,
VL53L1_STRING_DEVICE_INFO_NAME0,
VL53L1_DEVINFO_STRLEN-1);
else
strncpy(pVL53L1_DeviceInfo->Name,
VL53L1_STRING_DEVICE_INFO_NAME1,
VL53L1_DEVINFO_STRLEN-1);
strncpy(pVL53L1_DeviceInfo->Type,
VL53L1_STRING_DEVICE_INFO_TYPE,
VL53L1_DEVINFO_STRLEN-1);
if (pVL53L1_DeviceInfo->ProductType == 0xAA) {
pVL53L1_DeviceInfo->Name[5] = '3';
pVL53L1_DeviceInfo->Type[5] = '3';
}
#else
pVL53L1_DeviceInfo->Name[0] = 0;
pVL53L1_DeviceInfo->Type[0] = 0;
#endif
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetUID(VL53L1_DEV Dev, uint64_t *pUid)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t fmtdata[8];
LOG_FUNCTION_START("");
Status = VL53L1_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;
}
VL53L1_Error VL53L1_GetRangeStatusString(uint8_t RangeStatus,
char *pRangeStatusString)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_range_status_string(RangeStatus,
pRangeStatusString);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetPalErrorString(VL53L1_Error PalErrorCode,
char *pPalErrorString)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_pal_error_string(PalErrorCode, pPalErrorString);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetPalStateString(VL53L1_State PalStateCode,
char *pPalStateString)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_pal_state_string(PalStateCode, pPalStateString);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetPalState(VL53L1_DEV Dev, VL53L1_State *pPalState)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
*pPalState = VL53L1DevDataGet(Dev, PalState);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetDeviceAddress(VL53L1_DEV Dev, uint8_t DeviceAddress)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_static_nvm_managed_t *pdata = &(pdev->stat_nvm);
LOG_FUNCTION_START("");
Status = VL53L1_WrByte(Dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS,
DeviceAddress / 2);
pdata->i2c_slave__device_address = (DeviceAddress / 2) & 0x7F;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_DataInit(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t i;
VL53L1_LLDriverData_t *pdev;
LOG_FUNCTION_START("");
#ifdef USE_I2C_2V8
Status = VL53L1_RdByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, &i);
if (Status == VL53L1_ERROR_NONE) {
i = (i & 0xfe) | 0x01;
Status = VL53L1_WrByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG,
i);
}
#endif
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_data_init(Dev, 1);
if (Status == VL53L1_ERROR_NONE) {
pdev = VL53L1DevStructGetLLDriverHandle(Dev);
memset(&pdev->per_vcsel_cal_data, 0,
sizeof(pdev->per_vcsel_cal_data));
}
if (Status == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_WAIT_STATICINIT);
for (i = 0; i < VL53L1_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
if (Status == VL53L1_ERROR_NONE)
Status |= VL53L1_SetLimitCheckEnable(Dev, i, 1);
else
break;
}
if (Status == VL53L1_ERROR_NONE) {
Status = VL53L1_set_dmax_mode(Dev,
VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_StaticInit(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t measurement_mode;
LOG_FUNCTION_START("");
VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_IDLE);
measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
VL53L1DevDataSet(Dev, LLData.measurement_mode, measurement_mode);
Status = VL53L1_SetPresetMode(Dev,
VL53L1_PRESETMODE_RANGING);
VL53L1_SetDistanceMode(Dev,
VL53L1_DISTANCEMODE_MEDIUM);
VL53L1DevDataSet(Dev, CurrentParameters.OutputMode,
VL53L1_OUTPUTMODE_NEAREST);
VL53L1_SmudgeCorrectionEnable(Dev,
VL53L1_SMUDGE_CORRECTION_NONE);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_WaitDeviceBooted(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_poll_for_boot_completion(Dev,
VL53L1_BOOT_COMPLETION_POLLING_TIMEOUT_MS);
LOG_FUNCTION_END(Status);
return Status;
}
static VL53L1_Error ComputeDevicePresetMode(
VL53L1_PresetModes PresetMode,
VL53L1_DistanceModes DistanceMode,
VL53L1_DevicePresetModes *pDevicePresetMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t DistIdx;
VL53L1_DevicePresetModes LightModes[3] = {
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_SHORT_RANGE,
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING,
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_LONG_RANGE};
VL53L1_DevicePresetModes RangingModes[3] = {
VL53L1_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE,
VL53L1_DEVICEPRESETMODE_HISTOGRAM_MEDIUM_RANGE,
VL53L1_DEVICEPRESETMODE_HISTOGRAM_LONG_RANGE};
VL53L1_DevicePresetModes ScanningModes[3] = {
VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_SHORT_RANGE,
VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE,
VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_LONG_RANGE};
VL53L1_DevicePresetModes TimedModes[3] = {
VL53L1_DEVICEPRESETMODE_TIMED_RANGING_SHORT_RANGE,
VL53L1_DEVICEPRESETMODE_TIMED_RANGING,
VL53L1_DEVICEPRESETMODE_TIMED_RANGING_LONG_RANGE};
VL53L1_DevicePresetModes LowPowerTimedModes[3] = {
VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_SHORT_RANGE,
VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_MEDIUM_RANGE,
VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_LONG_RANGE};
*pDevicePresetMode = VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
switch (DistanceMode) {
case VL53L1_DISTANCEMODE_SHORT:
DistIdx = 0;
break;
case VL53L1_DISTANCEMODE_MEDIUM:
DistIdx = 1;
break;
default:
DistIdx = 2;
}
switch (PresetMode) {
case VL53L1_PRESETMODE_LITE_RANGING:
*pDevicePresetMode = LightModes[DistIdx];
break;
case VL53L1_PRESETMODE_RANGING:
*pDevicePresetMode = RangingModes[DistIdx];
break;
case VL53L1_PRESETMODE_MULTIZONES_SCANNING:
*pDevicePresetMode = ScanningModes[DistIdx];
break;
case VL53L1_PRESETMODE_AUTONOMOUS:
*pDevicePresetMode = TimedModes[DistIdx];
break;
case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
*pDevicePresetMode = LowPowerTimedModes[DistIdx];
break;
case VL53L1_PRESETMODE_OLT:
*pDevicePresetMode = VL53L1_DEVICEPRESETMODE_OLT;
break;
case VL53L1_PRESETMODE_PROXY_RANGING_MODE:
*pDevicePresetMode =
VL53L1_DEVICEPRESETMODE_SPECIAL_HISTOGRAM_SHORT_RANGE;
break;
default:
Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
}
return Status;
}
static VL53L1_Error SetPresetMode(VL53L1_DEV Dev,
VL53L1_PresetModes PresetMode,
VL53L1_DistanceModes DistanceMode,
uint32_t inter_measurement_period_ms)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_DevicePresetModes device_preset_mode;
uint8_t measurement_mode;
uint16_t dss_config__target_total_rate_mcps = 0;
uint32_t phasecal_config_timeout_us = 0;
uint32_t mm_config_timeout_us = 0;
uint32_t lld_range_config_timeout_us = 0;
LOG_FUNCTION_START("%d", (int)PresetMode);
if ((PresetMode == VL53L1_PRESETMODE_AUTONOMOUS) ||
(PresetMode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS))
measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_TIMED;
else
measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
Status = ComputeDevicePresetMode(PresetMode, DistanceMode,
&device_preset_mode);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_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 == VL53L1_ERROR_NONE)
Status = VL53L1_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 == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev, LLData.measurement_mode,
measurement_mode);
if (Status == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev, CurrentParameters.PresetMode, PresetMode);
VL53L1DevDataSet(Dev, CurrentParameters.OutputMode,
VL53L1_OUTPUTMODE_NEAREST);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetPresetMode(VL53L1_DEV Dev, VL53L1_PresetModes PresetMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_DistanceModes DistanceMode = VL53L1_DISTANCEMODE_LONG;
LOG_FUNCTION_START("%d", (int)PresetMode);
Status = VL53L1_low_power_auto_data_init(Dev);
if (PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE)
DistanceMode = VL53L1_DISTANCEMODE_SHORT;
Status = SetPresetMode(Dev,
PresetMode,
DistanceMode,
1000);
if (Status == VL53L1_ERROR_NONE) {
if ((PresetMode == VL53L1_PRESETMODE_LITE_RANGING) ||
(PresetMode == VL53L1_PRESETMODE_AUTONOMOUS) ||
(PresetMode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS))
Status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(
Dev, 41000);
else
Status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(
Dev, 33333);
}
if (Status == VL53L1_ERROR_NONE) {
Status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev,
1000);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetPresetMode(VL53L1_DEV Dev,
VL53L1_PresetModes *pPresetMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
*pPresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetDistanceMode(VL53L1_DEV Dev,
VL53L1_DistanceModes DistanceMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_PresetModes PresetMode;
uint32_t inter_measurement_period_ms;
uint32_t TimingBudget = 0;
uint32_t MmTimeoutUs = 0;
uint32_t PhaseCalTimeoutUs = 0;
VL53L1_zone_config_t zone_config;
LOG_FUNCTION_START("%d", (int)DistanceMode);
PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
if ((PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE) &&
(DistanceMode != VL53L1_DISTANCEMODE_SHORT))
return VL53L1_ERROR_INVALID_PARAMS;
if ((DistanceMode != VL53L1_DISTANCEMODE_SHORT) &&
(DistanceMode != VL53L1_DISTANCEMODE_MEDIUM) &&
(DistanceMode != VL53L1_DISTANCEMODE_LONG))
return VL53L1_ERROR_INVALID_PARAMS;
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_get_zone_config(Dev, &zone_config);
inter_measurement_period_ms = VL53L1DevDataGet(Dev,
LLData.inter_measurement_period_ms);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_get_timeouts_us(Dev, &PhaseCalTimeoutUs,
&MmTimeoutUs, &TimingBudget);
if (Status == VL53L1_ERROR_NONE)
Status = SetPresetMode(Dev,
PresetMode,
DistanceMode,
inter_measurement_period_ms);
if (Status == VL53L1_ERROR_NONE) {
VL53L1DevDataSet(Dev, CurrentParameters.DistanceMode,
DistanceMode);
}
if (Status == VL53L1_ERROR_NONE) {
Status = VL53L1_set_timeouts_us(Dev, PhaseCalTimeoutUs,
MmTimeoutUs, TimingBudget);
if (Status == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev, LLData.range_config_timeout_us,
TimingBudget);
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_set_zone_config(Dev, &zone_config);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetDistanceMode(VL53L1_DEV Dev,
VL53L1_DistanceModes *pDistanceMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
*pDistanceMode = VL53L1DevDataGet(Dev, CurrentParameters.DistanceMode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetOutputMode(VL53L1_DEV Dev,
VL53L1_OutputModes OutputMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if ((OutputMode != VL53L1_OUTPUTMODE_NEAREST) &&
(OutputMode != VL53L1_OUTPUTMODE_STRONGEST))
Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
else
VL53L1DevDataSet(Dev, CurrentParameters.OutputMode, OutputMode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetOutputMode(VL53L1_DEV Dev,
VL53L1_OutputModes *pOutputMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
*pOutputMode = VL53L1DevDataGet(Dev, CurrentParameters.OutputMode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,
uint32_t MeasurementTimingBudgetMicroSeconds)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t Mm1Enabled = 0;
uint8_t Mm2Enabled = 0;
uint32_t TimingGuard;
uint32_t divisor;
uint32_t TimingBudget = 0;
uint32_t MmTimeoutUs = 0;
VL53L1_PresetModes PresetMode;
uint32_t PhaseCalTimeoutUs = 0;
uint32_t vhv;
int32_t vhv_loops;
uint32_t FDAMaxTimingBudgetUs = FDA_MAX_TIMING_BUDGET_US;
LOG_FUNCTION_START("");
if (MeasurementTimingBudgetMicroSeconds > 10000000)
Status = VL53L1_ERROR_INVALID_PARAMS;
if (Status == VL53L1_ERROR_NONE) {
Status = VL53L1_GetSequenceStepEnable(Dev,
VL53L1_SEQUENCESTEP_MM1, &Mm1Enabled);
}
if (Status == VL53L1_ERROR_NONE) {
Status = VL53L1_GetSequenceStepEnable(Dev,
VL53L1_SEQUENCESTEP_MM2, &Mm2Enabled);
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_get_timeouts_us(Dev,
&PhaseCalTimeoutUs,
&MmTimeoutUs,
&TimingBudget);
if (Status == VL53L1_ERROR_NONE) {
PresetMode = VL53L1DevDataGet(Dev,
CurrentParameters.PresetMode);
TimingGuard = 0;
divisor = 1;
switch (PresetMode) {
case VL53L1_PRESETMODE_LITE_RANGING:
if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
TimingGuard = 5000;
else
TimingGuard = 1000;
break;
case VL53L1_PRESETMODE_AUTONOMOUS:
FDAMaxTimingBudgetUs *= 2;
if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
TimingGuard = 26600;
else
TimingGuard = 21600;
divisor = 2;
break;
case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
FDAMaxTimingBudgetUs *= 2;
vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
VL53L1_get_tuning_parm(Dev,
VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND,
&vhv_loops);
if (vhv_loops > 0) {
vhv += vhv_loops *
LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
}
TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING +
LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING +
vhv;
divisor = 2;
break;
case VL53L1_PRESETMODE_RANGING:
case VL53L1_PRESETMODE_MULTIZONES_SCANNING:
case VL53L1_PRESETMODE_PROXY_RANGING_MODE:
TimingGuard = 1700;
divisor = 6;
break;
case VL53L1_PRESETMODE_OLT:
TimingGuard = MmTimeoutUs + 5000;
break;
default:
Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
}
if (MeasurementTimingBudgetMicroSeconds <= TimingGuard)
Status = VL53L1_ERROR_INVALID_PARAMS;
else {
TimingBudget = (MeasurementTimingBudgetMicroSeconds
- TimingGuard);
}
if (Status == VL53L1_ERROR_NONE) {
if (TimingBudget > FDAMaxTimingBudgetUs)
Status = VL53L1_ERROR_INVALID_PARAMS;
else {
TimingBudget /= divisor;
Status = VL53L1_set_timeouts_us(
Dev,
PhaseCalTimeoutUs,
MmTimeoutUs,
TimingBudget);
}
if (Status == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev,
LLData.range_config_timeout_us,
TimingBudget);
}
}
if (Status == VL53L1_ERROR_NONE) {
VL53L1DevDataSet(Dev,
CurrentParameters.MeasurementTimingBudgetMicroSeconds,
MeasurementTimingBudgetMicroSeconds);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,
uint32_t *pMeasurementTimingBudgetMicroSeconds)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t Mm1Enabled = 0;
uint8_t Mm2Enabled = 0;
uint32_t MmTimeoutUs = 0;
uint32_t RangeTimeoutUs = 0;
uint32_t MeasTimingBdg = 0;
uint32_t PhaseCalTimeoutUs = 0;
VL53L1_PresetModes PresetMode;
uint32_t TimingGuard;
uint32_t vhv;
int32_t vhv_loops;
LOG_FUNCTION_START("");
*pMeasurementTimingBudgetMicroSeconds = 0;
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_GetSequenceStepEnable(Dev,
VL53L1_SEQUENCESTEP_MM1, &Mm1Enabled);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_GetSequenceStepEnable(Dev,
VL53L1_SEQUENCESTEP_MM2, &Mm2Enabled);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_get_timeouts_us(Dev,
&PhaseCalTimeoutUs,
&MmTimeoutUs,
&RangeTimeoutUs);
if (Status == VL53L1_ERROR_NONE) {
PresetMode = VL53L1DevDataGet(Dev,
CurrentParameters.PresetMode);
switch (PresetMode) {
case VL53L1_PRESETMODE_LITE_RANGING:
if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
MeasTimingBdg = RangeTimeoutUs + 5000;
else
MeasTimingBdg = RangeTimeoutUs + 1000;
break;
case VL53L1_PRESETMODE_AUTONOMOUS:
if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
MeasTimingBdg = 2 * RangeTimeoutUs + 26600;
else
MeasTimingBdg = 2 * RangeTimeoutUs + 21600;
break;
case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
VL53L1_get_tuning_parm(Dev,
VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND,
&vhv_loops);
if (vhv_loops > 0) {
vhv += vhv_loops *
LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
}
TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING +
LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING +
vhv;
MeasTimingBdg = 2 * RangeTimeoutUs + TimingGuard;
break;
case VL53L1_PRESETMODE_RANGING:
case VL53L1_PRESETMODE_MULTIZONES_SCANNING:
case VL53L1_PRESETMODE_PROXY_RANGING_MODE:
MeasTimingBdg = (6 * RangeTimeoutUs) + 1700;
break;
case VL53L1_PRESETMODE_OLT:
MeasTimingBdg = RangeTimeoutUs + MmTimeoutUs + 5000;
break;
default:
Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
}
}
if (Status == VL53L1_ERROR_NONE)
*pMeasurementTimingBudgetMicroSeconds = MeasTimingBdg;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,
uint32_t InterMeasurementPeriodMilliSeconds)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint32_t adjustedIMP;
LOG_FUNCTION_START("");
adjustedIMP = InterMeasurementPeriodMilliSeconds;
adjustedIMP += (adjustedIMP * 64) / 1000;
Status = VL53L1_set_inter_measurement_period_ms(Dev,
adjustedIMP);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,
uint32_t *pInterMeasurementPeriodMilliSeconds)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint32_t adjustedIMP;
LOG_FUNCTION_START("");
Status = VL53L1_get_inter_measurement_period_ms(Dev, &adjustedIMP);
adjustedIMP -= (adjustedIMP * 64) / 1000;
*pInterMeasurementPeriodMilliSeconds = adjustedIMP;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetDmaxReflectance(VL53L1_DEV Dev,
FixPoint1616_t DmaxReflectance)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_dmax_reflectance_array_t dmax_reflectances;
LOG_FUNCTION_START("");
if (DmaxReflectance > 100*65536)
Status = VL53L1_ERROR_INVALID_PARAMS;
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_get_dmax_reflectance_values(Dev,
&dmax_reflectances);
if (Status == VL53L1_ERROR_NONE) {
dmax_reflectances.target_reflectance_for_dmax[
DMAX_REFLECTANCE_IDX] =
VL53L1_FIXPOINT1616TOFIXPOINT72(DmaxReflectance);
Status = VL53L1_set_dmax_reflectance_values(Dev,
&dmax_reflectances);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetDmaxReflectance(VL53L1_DEV Dev,
FixPoint1616_t *pDmaxReflectance)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_dmax_reflectance_array_t dmax_reflectances;
uint16_t r;
LOG_FUNCTION_START("");
Status = VL53L1_get_dmax_reflectance_values(Dev, &dmax_reflectances);
if (Status == VL53L1_ERROR_NONE) {
r = dmax_reflectances.target_reflectance_for_dmax[
DMAX_REFLECTANCE_IDX];
*pDmaxReflectance = VL53L1_FIXPOINT72TOFIXPOINT1616(r);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetDmaxMode(VL53L1_DEV Dev,
VL53L1_DeviceDmaxModes DmaxMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_DeviceDmaxMode dmax_mode;
LOG_FUNCTION_START("");
switch (DmaxMode) {
case VL53L1_DMAXMODE_FMT_CAL_DATA:
dmax_mode = VL53L1_DEVICEDMAXMODE__FMT_CAL_DATA;
break;
case VL53L1_DMAXMODE_CUSTCAL_DATA:
dmax_mode = VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA;
break;
case VL53L1_DMAXMODE_PER_ZONE_CAL_DATA:
dmax_mode = VL53L1_DEVICEDMAXMODE__PER_ZONE_CAL_DATA;
break;
default:
dmax_mode = VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA;
Status = VL53L1_ERROR_INVALID_PARAMS;
break;
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_set_dmax_mode(Dev, dmax_mode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetDmaxMode(VL53L1_DEV Dev,
VL53L1_DeviceDmaxModes *pDmaxMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_DeviceDmaxMode dmax_mode;
LOG_FUNCTION_START("");
Status = VL53L1_get_dmax_mode(Dev, &dmax_mode);
if (Status == VL53L1_ERROR_NONE) {
switch (dmax_mode) {
case VL53L1_DEVICEDMAXMODE__FMT_CAL_DATA:
*pDmaxMode = VL53L1_DMAXMODE_FMT_CAL_DATA;
break;
case VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA:
*pDmaxMode = VL53L1_DMAXMODE_CUSTCAL_DATA;
break;
case VL53L1_DEVICEDMAXMODE__PER_ZONE_CAL_DATA:
*pDmaxMode = VL53L1_DMAXMODE_PER_ZONE_CAL_DATA;
break;
default:
*pDmaxMode = VL53L1_DEVICEDMAXMODE__CUST_CAL_DATA;
Status = VL53L1_ERROR_NOT_IMPLEMENTED;
break;
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetNumberOfLimitCheck(uint16_t *pNumberOfLimitCheck)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
*pNumberOfLimitCheck = VL53L1_CHECKENABLE_NUMBER_OF_CHECKS;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetLimitCheckInfo(uint16_t LimitCheckId,
char *pLimitCheckString)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_limit_check_info(LimitCheckId,
pLimitCheckString);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetLimitCheckStatus(VL53L1_DEV Dev, uint16_t LimitCheckId,
uint8_t *pLimitCheckStatus)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t Temp8;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L1_ERROR_INVALID_PARAMS;
} else {
VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
LimitCheckId, Temp8);
*pLimitCheckStatus = Temp8;
}
LOG_FUNCTION_END(Status);
return Status;
}
static VL53L1_Error SetLimitValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
FixPoint1616_t value)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint16_t tmpuint16;
LOG_FUNCTION_START("");
switch (LimitCheckId) {
case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
tmpuint16 = VL53L1_FIXPOINT1616TOFIXPOINT142(value);
VL53L1_set_lite_sigma_threshold(Dev, tmpuint16);
break;
case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
tmpuint16 = VL53L1_FIXPOINT1616TOFIXPOINT97(value);
VL53L1_set_lite_min_count_rate(Dev, tmpuint16);
break;
default:
Status = VL53L1_ERROR_INVALID_PARAMS;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetLimitCheckEnable(VL53L1_DEV Dev, uint16_t LimitCheckId,
uint8_t LimitCheckEnable)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
FixPoint1616_t TempFix1616 = 0;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L1_ERROR_INVALID_PARAMS;
} else {
if (LimitCheckEnable == 0)
TempFix1616 = 0;
else
VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
LimitCheckId, TempFix1616);
Status = SetLimitValue(Dev, LimitCheckId, TempFix1616);
}
if (Status == VL53L1_ERROR_NONE)
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksEnable,
LimitCheckId,
((LimitCheckEnable == 0) ? 0 : 1));
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetLimitCheckEnable(VL53L1_DEV Dev, uint16_t LimitCheckId,
uint8_t *pLimitCheckEnable)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t Temp8;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L1_ERROR_INVALID_PARAMS;
*pLimitCheckEnable = 0;
} else {
VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
LimitCheckId, Temp8);
*pLimitCheckEnable = Temp8;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetLimitCheckValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
FixPoint1616_t LimitCheckValue)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t LimitChecksEnable;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L1_ERROR_INVALID_PARAMS;
} else {
VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
LimitCheckId,
LimitChecksEnable);
if (LimitChecksEnable == 0) {
VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
LimitCheckId, LimitCheckValue);
} else {
Status = SetLimitValue(Dev, LimitCheckId,
LimitCheckValue);
if (Status == VL53L1_ERROR_NONE) {
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksValue,
LimitCheckId, LimitCheckValue);
}
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetLimitCheckValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
FixPoint1616_t *pLimitCheckValue)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint16_t MinCountRate;
FixPoint1616_t TempFix1616 = 0;
uint16_t SigmaThresh;
LOG_FUNCTION_START("");
switch (LimitCheckId) {
case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
Status = VL53L1_get_lite_sigma_threshold(Dev, &SigmaThresh);
TempFix1616 = VL53L1_FIXPOINT142TOFIXPOINT1616(SigmaThresh);
break;
case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
Status = VL53L1_get_lite_min_count_rate(Dev, &MinCountRate);
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(MinCountRate);
break;
default:
Status = VL53L1_ERROR_INVALID_PARAMS;
}
if (Status == VL53L1_ERROR_NONE) {
if (TempFix1616 == 0) {
VL53L1_GETARRAYPARAMETERFIELD(Dev,
LimitChecksValue, LimitCheckId,
TempFix1616);
*pLimitCheckValue = TempFix1616;
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksEnable, LimitCheckId, 0);
} else {
*pLimitCheckValue = TempFix1616;
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksValue, LimitCheckId,
TempFix1616);
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksEnable, LimitCheckId, 1);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetLimitCheckCurrent(VL53L1_DEV Dev, uint16_t LimitCheckId,
FixPoint1616_t *pLimitCheckCurrent)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
FixPoint1616_t TempFix1616 = 0;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L1_ERROR_INVALID_PARAMS;
} else {
VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksCurrent,
LimitCheckId, TempFix1616);
*pLimitCheckCurrent = TempFix1616;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetMaxNumberOfROI(VL53L1_DEV Dev,
uint8_t *pMaxNumberOfROI)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_PresetModes PresetMode;
LOG_FUNCTION_START("");
PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
if (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING)
*pMaxNumberOfROI = VL53L1_MAX_USER_ZONES;
else
*pMaxNumberOfROI = 1;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetROI(VL53L1_DEV Dev,
VL53L1_RoiConfig_t *pRoiConfig)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_PresetModes PresetMode;
uint8_t MaxNumberOfROI = 1;
VL53L1_zone_config_t zone_cfg;
VL53L1_UserRoi_t CurrROI;
uint8_t i;
uint8_t x_centre;
uint8_t y_centre;
uint8_t width, height;
LOG_FUNCTION_START("");
PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
if (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING)
MaxNumberOfROI = VL53L1_MAX_USER_ZONES;
if ((pRoiConfig->NumberOfRoi > MaxNumberOfROI) ||
(pRoiConfig->NumberOfRoi < 1))
Status = VL53L1_ERROR_INVALID_PARAMS;
if (Status == VL53L1_ERROR_NONE) {
zone_cfg.max_zones = MaxNumberOfROI;
zone_cfg.active_zones = pRoiConfig->NumberOfRoi - 1;
for (i = 0; i < pRoiConfig->NumberOfRoi; i++) {
CurrROI = pRoiConfig->UserRois[i];
Status = CheckValidRectRoi(CurrROI);
if (Status != VL53L1_ERROR_NONE)
break;
x_centre = (CurrROI.BotRightX + CurrROI.TopLeftX + 1)
/ 2;
y_centre = (CurrROI.TopLeftY + CurrROI.BotRightY + 1)
/ 2;
width = (CurrROI.BotRightX - CurrROI.TopLeftX);
height = (CurrROI.TopLeftY - CurrROI.BotRightY);
if ((width < 3) || (height < 3)) {
Status = VL53L1_ERROR_INVALID_PARAMS;
break;
}
zone_cfg.user_zones[i].x_centre = x_centre;
zone_cfg.user_zones[i].y_centre = y_centre;
zone_cfg.user_zones[i].width = width;
zone_cfg.user_zones[i].height = height;
}
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_set_zone_config(Dev, &zone_cfg);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetROI(VL53L1_DEV Dev,
VL53L1_RoiConfig_t *pRoiConfig)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_zone_config_t zone_cfg;
uint8_t i;
uint8_t TopLeftX;
uint8_t TopLeftY;
uint8_t BotRightX;
uint8_t BotRightY;
LOG_FUNCTION_START("");
VL53L1_get_zone_config(Dev, &zone_cfg);
pRoiConfig->NumberOfRoi = zone_cfg.active_zones + 1;
for (i = 0; i < pRoiConfig->NumberOfRoi; i++) {
TopLeftX = (2 * zone_cfg.user_zones[i].x_centre -
zone_cfg.user_zones[i].width) >> 1;
TopLeftY = (2 * zone_cfg.user_zones[i].y_centre +
zone_cfg.user_zones[i].height) >> 1;
BotRightX = (2 * zone_cfg.user_zones[i].x_centre +
zone_cfg.user_zones[i].width) >> 1;
BotRightY = (2 * zone_cfg.user_zones[i].y_centre -
zone_cfg.user_zones[i].height) >> 1;
pRoiConfig->UserRois[i].TopLeftX = TopLeftX;
pRoiConfig->UserRois[i].TopLeftY = TopLeftY;
pRoiConfig->UserRois[i].BotRightX = BotRightX;
pRoiConfig->UserRois[i].BotRightY = BotRightY;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetNumberOfSequenceSteps(VL53L1_DEV Dev,
uint8_t *pNumberOfSequenceSteps)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
SUPPRESS_UNUSED_WARNING(Dev);
LOG_FUNCTION_START("");
*pNumberOfSequenceSteps = VL53L1_SEQUENCESTEP_NUMBER_OF_ITEMS;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetSequenceStepsInfo(VL53L1_SequenceStepId SequenceStepId,
char *pSequenceStepsString)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_sequence_steps_info(
SequenceStepId,
pSequenceStepsString);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetSequenceStepEnable(VL53L1_DEV Dev,
VL53L1_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint32_t MeasurementTimingBudgetMicroSeconds;
LOG_FUNCTION_START("");
Status = VL53L1_set_sequence_config_bit(Dev,
(VL53L1_DeviceSequenceConfig)SequenceStepId,
SequenceStepEnabled);
if (Status == VL53L1_ERROR_NONE) {
MeasurementTimingBudgetMicroSeconds = VL53L1DevDataGet(Dev,
CurrentParameters.MeasurementTimingBudgetMicroSeconds);
VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev,
MeasurementTimingBudgetMicroSeconds);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetSequenceStepEnable(VL53L1_DEV Dev,
VL53L1_SequenceStepId SequenceStepId, uint8_t *pSequenceStepEnabled)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_sequence_config_bit(Dev,
(VL53L1_DeviceSequenceConfig)SequenceStepId,
pSequenceStepEnabled);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_StartMeasurement(VL53L1_DEV Dev)
{
#define TIMED_MODE_TIMING_GUARD_MILLISECONDS 4
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t DeviceMeasurementMode;
VL53L1_State CurrPalState;
VL53L1_Error lStatus;
uint32_t MTBus, IMPms;
uint8_t i;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
LOG_FUNCTION_START("");
DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
if (DeviceMeasurementMode != VL53L1_DEVICEMEASUREMENTMODE_TIMED)
VL53L1_load_patch(Dev);
for (i = 0; i < VL53L1_MAX_USER_ZONES; i++) {
pdev->PreviousRangeMilliMeter[i] = 0;
pdev->PreviousRangeStatus[i] = 255;
pdev->PreviousExtendedRange[i] = 0;
pdev->PreviousRangeActiveResults[i] = 0;
pdev->smudge_corrector_internals.previous_xtalk[i] = 0;
pdev->smudge_corrector_internals.current_samples[i] = 0;
pdev->smudge_corrector_internals.accumulator[i] = 0;
pdev->smudge_corrector_internals.nodetect_counter[i] = 0;
}
pdev->PreviousStreamCount = 0;
CurrPalState = VL53L1DevDataGet(Dev, PalState);
switch (CurrPalState) {
case VL53L1_STATE_IDLE:
Status = VL53L1_ERROR_NONE;
break;
case VL53L1_STATE_POWERDOWN:
case VL53L1_STATE_WAIT_STATICINIT:
case VL53L1_STATE_STANDBY:
case VL53L1_STATE_RUNNING:
case VL53L1_STATE_RESET:
case VL53L1_STATE_UNKNOWN:
case VL53L1_STATE_ERROR:
Status = VL53L1_ERROR_INVALID_COMMAND;
break;
default:
Status = VL53L1_ERROR_UNDEFINED;
}
if ((Status == VL53L1_ERROR_NONE) &&
(DeviceMeasurementMode == VL53L1_DEVICEMEASUREMENTMODE_TIMED)) {
lStatus = VL53L1_GetMeasurementTimingBudgetMicroSeconds(Dev,
&MTBus);
MTBus /= 1000;
lStatus = VL53L1_GetInterMeasurementPeriodMilliSeconds(Dev,
&IMPms);
SUPPRESS_UNUSED_WARNING(lStatus);
if (IMPms < MTBus + TIMED_MODE_TIMING_GUARD_MILLISECONDS)
Status = VL53L1_ERROR_INVALID_PARAMS;
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_init_and_start_range(
Dev,
DeviceMeasurementMode,
VL53L1_DEVICECONFIGLEVEL_FULL);
if (Status == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_RUNNING);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_StopMeasurement(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t DeviceMeasurementMode;
LOG_FUNCTION_START("");
Status = VL53L1_stop_range(Dev);
DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
if (DeviceMeasurementMode != VL53L1_DEVICEMEASUREMENTMODE_TIMED)
VL53L1_unload_patch(Dev);
if (Status == VL53L1_ERROR_NONE)
VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_IDLE);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_ClearInterruptAndStartMeasurement(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t DeviceMeasurementMode;
LOG_FUNCTION_START("");
DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
Status = VL53L1_clear_interrupt_and_enable_next_range(Dev,
DeviceMeasurementMode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetMeasurementDataReady(VL53L1_DEV Dev,
uint8_t *pMeasurementDataReady)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_is_new_data_ready(Dev, pMeasurementDataReady);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_WaitMeasurementDataReady(VL53L1_DEV Dev)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_poll_for_range_completion(Dev,
VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS);
LOG_FUNCTION_END(Status);
return Status;
}
static void GenNewPresetMode(int16_t RefRange,
VL53L1_DistanceModes InternalDistanceMode,
VL53L1_DistanceModes *pNewDistanceMode)
{
uint16_t HRLI = 600;
uint16_t HRLH = 700;
uint16_t MRLI = 1400;
uint16_t MRLH = 1500;
switch (InternalDistanceMode) {
case VL53L1_DISTANCEMODE_SHORT:
if (RefRange > MRLH)
*pNewDistanceMode = VL53L1_DISTANCEMODE_LONG;
else if (RefRange > HRLH)
*pNewDistanceMode = VL53L1_DISTANCEMODE_MEDIUM;
break;
case VL53L1_DISTANCEMODE_MEDIUM:
if (RefRange > MRLH)
*pNewDistanceMode = VL53L1_DISTANCEMODE_LONG;
else if (RefRange < HRLI)
*pNewDistanceMode = VL53L1_DISTANCEMODE_SHORT;
break;
default:
if (RefRange < HRLI)
*pNewDistanceMode = VL53L1_DISTANCEMODE_SHORT;
else if (RefRange < MRLI)
*pNewDistanceMode = VL53L1_DISTANCEMODE_MEDIUM;
break;
}
}
static void CheckAndChangeDistanceMode(VL53L1_DEV Dev,
VL53L1_TargetRangeData_t *pRangeData,
int16_t Ambient100DmaxMm,
VL53L1_DistanceModes *pNewDistanceMode
)
{
VL53L1_DistanceModes DistanceMode;
uint8_t RangeStatus = pRangeData->RangeStatus;
uint8_t DmaxValid;
int32_t MinAmbient = BDTable[VL53L1_TUNING_MIN_AMBIENT_DMAX_VALID];
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
int32_t tmpint32;
switch (RangeStatus) {
case VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL:
case VL53L1_RANGESTATUS_WRAP_TARGET_FAIL:
case VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE:
case VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL:
case VL53L1_RANGESTATUS_SYNCRONISATION_INT:
case VL53L1_RANGESTATUS_NONE:
return;
default:
break;
}
DmaxValid = 1;
tmpint32 = pdev->hist_data.VL53L1_p_004;
if ((tmpint32 < MinAmbient) || (Ambient100DmaxMm == 0))
DmaxValid = 0;
DistanceMode = VL53L1DevDataGet(Dev,
CurrentParameters.DistanceMode);
*pNewDistanceMode = DistanceMode;
if (RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID)
GenNewPresetMode(pRangeData->RangeMilliMeter,
DistanceMode, pNewDistanceMode);
else {
if (DmaxValid)
GenNewPresetMode(Ambient100DmaxMm,
DistanceMode, pNewDistanceMode);
else
*pNewDistanceMode = VL53L1_DISTANCEMODE_LONG;
}
}
static uint8_t ComputeRQL(uint8_t active_results,
uint8_t FilteredRangeStatus,
VL53L1_range_data_t *presults_data)
{
int16_t T_Wide = 150;
int16_t SRL = 300;
uint16_t SRAS = 30;
FixPoint1616_t RAS;
FixPoint1616_t SRQL;
FixPoint1616_t GI = 7713587;
FixPoint1616_t GGm = 3198157;
FixPoint1616_t LRAP = 6554;
FixPoint1616_t partial;
uint8_t finalvalue;
uint8_t returnvalue;
if (active_results == 0)
returnvalue = 0;
else if (((presults_data->max_range_mm -
presults_data->min_range_mm) >= T_Wide) ||
(FilteredRangeStatus == VL53L1_DEVICEERROR_PHASECONSISTENCY))
returnvalue = 50;
else {
if (presults_data->median_range_mm < SRL)
RAS = SRAS * 65536;
else
RAS = LRAP * presults_data->median_range_mm;
if (RAS != 0) {
partial = (GGm * presults_data->VL53L1_p_005);
partial = partial + (RAS >> 1);
partial = partial / RAS;
partial = partial * 65536;
if (partial <= GI)
SRQL = GI - partial;
else
SRQL = 50 * 65536;
} else
SRQL = 100 * 65536;
finalvalue = (uint8_t)(SRQL >> 16);
returnvalue = MAX(50, MIN(100, finalvalue));
}
return returnvalue;
}
static uint8_t ConvertStatusLite(uint8_t FilteredRangeStatus)
{
uint8_t RangeStatus;
switch (FilteredRangeStatus) {
case VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY:
RangeStatus = VL53L1_RANGESTATUS_SYNCRONISATION_INT;
break;
case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
break;
case VL53L1_DEVICEERROR_RANGEPHASECHECK:
RangeStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
break;
case VL53L1_DEVICEERROR_MSRCNOTARGET:
RangeStatus = VL53L1_RANGESTATUS_SIGNAL_FAIL;
break;
case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
RangeStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
break;
case VL53L1_DEVICEERROR_PHASECONSISTENCY:
RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
break;
case VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD:
RangeStatus = VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL;
break;
case VL53L1_DEVICEERROR_MINCLIP:
RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED;
break;
case VL53L1_DEVICEERROR_RANGECOMPLETE:
RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
break;
default:
RangeStatus = VL53L1_RANGESTATUS_NONE;
}
return RangeStatus;
}
static uint8_t ConvertStatusHisto(uint8_t FilteredRangeStatus)
{
uint8_t RangeStatus;
switch (FilteredRangeStatus) {
case VL53L1_DEVICEERROR_RANGEPHASECHECK:
RangeStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
break;
case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
RangeStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
break;
case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
break;
case VL53L1_DEVICEERROR_PHASECONSISTENCY:
RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
break;
case VL53L1_DEVICEERROR_PREV_RANGE_NO_TARGETS:
RangeStatus = VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL;
break;
case VL53L1_DEVICEERROR_EVENTCONSISTENCY:
RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
break;
case VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE:
RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE;
break;
case VL53L1_DEVICEERROR_RANGECOMPLETE:
RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
break;
default:
RangeStatus = VL53L1_RANGESTATUS_NONE;
}
return RangeStatus;
}
static VL53L1_Error SetSimpleData(VL53L1_DEV Dev,
uint8_t active_results, uint8_t device_status,
VL53L1_range_data_t *presults_data,
VL53L1_RangingMeasurementData_t *pRangeData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t FilteredRangeStatus;
uint8_t SigmaLimitflag;
uint8_t SignalLimitflag;
uint8_t Temp8Enable;
uint8_t Temp8;
FixPoint1616_t AmbientRate;
FixPoint1616_t SignalRate;
FixPoint1616_t TempFix1616;
FixPoint1616_t LimitCheckValue;
VL53L1_PresetModes PresetMode;
int16_t Range;
pRangeData->TimeStamp = presults_data->time_stamp;
FilteredRangeStatus = presults_data->range_status & 0x1F;
pRangeData->RangeQualityLevel = ComputeRQL(active_results,
FilteredRangeStatus,
presults_data);
SignalRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->peak_signal_count_rate_mcps);
pRangeData->SignalRateRtnMegaCps
= SignalRate;
AmbientRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->ambient_count_rate_mcps);
pRangeData->AmbientRateRtnMegaCps = AmbientRate;
pRangeData->EffectiveSpadRtnCount =
presults_data->VL53L1_p_006;
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->VL53L1_p_005);
pRangeData->SigmaMilliMeter = TempFix1616;
pRangeData->RangeMilliMeter = presults_data->median_range_mm;
pRangeData->RangeFractionalPart = 0;
switch (device_status) {
case VL53L1_DEVICEERROR_MULTCLIPFAIL:
case VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
case VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
case VL53L1_DEVICEERROR_NOVHVVALUEFOUND:
pRangeData->RangeStatus = VL53L1_RANGESTATUS_HARDWARE_FAIL;
break;
case VL53L1_DEVICEERROR_USERROICLIP:
pRangeData->RangeStatus = VL53L1_RANGESTATUS_MIN_RANGE_FAIL;
break;
default:
pRangeData->RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
}
if (pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
PresetMode = VL53L1DevDataGet(Dev,
CurrentParameters.PresetMode);
if ((PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) ||
(PresetMode == VL53L1_PRESETMODE_RANGING) ||
(PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE))
pRangeData->RangeStatus =
ConvertStatusHisto(FilteredRangeStatus);
else
pRangeData->RangeStatus =
ConvertStatusLite(FilteredRangeStatus);
}
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->VL53L1_p_005);
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksCurrent, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
TempFix1616);
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->peak_signal_count_rate_mcps);
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksCurrent, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
TempFix1616);
VL53L1_GetLimitCheckValue(Dev,
VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
&LimitCheckValue);
SigmaLimitflag = (FilteredRangeStatus ==
VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK)
? 1 : 0;
VL53L1_GetLimitCheckEnable(Dev,
VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
&Temp8Enable);
Temp8 = ((Temp8Enable == 1) && (SigmaLimitflag == 1)) ? 1 : 0;
VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
VL53L1_GetLimitCheckValue(Dev,
VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&LimitCheckValue);
SignalLimitflag = (FilteredRangeStatus ==
VL53L1_DEVICEERROR_MSRCNOTARGET)
? 1 : 0;
VL53L1_GetLimitCheckEnable(Dev,
VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&Temp8Enable);
Temp8 = ((Temp8Enable == 1) && (SignalLimitflag == 1)) ? 1 : 0;
VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, Temp8);
Range = pRangeData->RangeMilliMeter;
if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
(Range < 0)) {
if (Range < BDTable[VL53L1_TUNING_PROXY_MIN])
pRangeData->RangeStatus =
VL53L1_RANGESTATUS_RANGE_INVALID;
else
pRangeData->RangeMilliMeter = 0;
}
return Status;
}
static VL53L1_Error SetTargetData(VL53L1_DEV Dev,
uint8_t active_results, uint8_t iteration,
uint8_t device_status, VL53L1_range_data_t *presults_data,
VL53L1_TargetRangeData_t *pRangeData)
{
#define MAX_AMBIENT 2 * 65536
#define MAX_RANGE_DIFF 500
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_tuning_parm_storage_t *tp =
&(pdev->tuning_parms);
uint8_t roi = pdev->ll_state.rd_zone_id;
uint8_t timing = pdev->hist_data.number_of_ambient_bins / 4;
uint8_t sequency;
uint8_t FilteredRangeStatus;
uint8_t SigmaLimitflag;
uint8_t SignalLimitflag;
uint8_t Temp8Enable;
uint8_t Temp8;
FixPoint1616_t AmbientRate;
FixPoint1616_t SignalRate;
FixPoint1616_t TempFix1616;
FixPoint1616_t LimitCheckValue;
VL53L1_PresetModes PresetMode;
int16_t Range, RangeDiff, RangeMillimeterInit;
int32_t ExtendedRangeEnabled = 0;
uint8_t uwr_status;
int16_t AddOffset;
int32_t TuningMinValidSignal;
FixPoint1616_t MinValidSignal;
uint8_t histo_merge_nb;
FilteredRangeStatus = presults_data->range_status & 0x1F;
pRangeData->RangeQualityLevel = ComputeRQL(active_results,
FilteredRangeStatus,
presults_data);
SignalRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->peak_signal_count_rate_mcps);
pRangeData->SignalRateRtnMegaCps
= SignalRate;
AmbientRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->ambient_count_rate_mcps);
pRangeData->AmbientRateRtnMegaCps = AmbientRate;
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->VL53L1_p_005);
pRangeData->SigmaMilliMeter = TempFix1616;
pRangeData->RangeMilliMeter = presults_data->median_range_mm;
pRangeData->RangeMaxMilliMeter = presults_data->max_range_mm;
pRangeData->RangeMinMilliMeter = presults_data->min_range_mm;
pRangeData->RangeFractionalPart = 0;
switch (device_status) {
case VL53L1_DEVICEERROR_MULTCLIPFAIL:
case VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
case VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
case VL53L1_DEVICEERROR_NOVHVVALUEFOUND:
pRangeData->RangeStatus = VL53L1_RANGESTATUS_HARDWARE_FAIL;
break;
case VL53L1_DEVICEERROR_USERROICLIP:
pRangeData->RangeStatus = VL53L1_RANGESTATUS_MIN_RANGE_FAIL;
break;
default:
pRangeData->RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
}
if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
(active_results == 0)) {
pRangeData->RangeStatus = VL53L1_RANGESTATUS_NONE;
pRangeData->SignalRateRtnMegaCps = 0;
pRangeData->SigmaMilliMeter = 0;
pRangeData->RangeMilliMeter = 8191;
pRangeData->RangeMaxMilliMeter = 8191;
pRangeData->RangeMinMilliMeter = 8191;
}
if (pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
PresetMode = VL53L1DevDataGet(Dev,
CurrentParameters.PresetMode);
if ((PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) ||
(PresetMode == VL53L1_PRESETMODE_RANGING) ||
(PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE))
pRangeData->RangeStatus =
ConvertStatusHisto(FilteredRangeStatus);
else
pRangeData->RangeStatus =
ConvertStatusLite(FilteredRangeStatus);
}
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->VL53L1_p_005);
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksCurrent, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
TempFix1616);
TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
presults_data->peak_signal_count_rate_mcps);
VL53L1_SETARRAYPARAMETERFIELD(Dev,
LimitChecksCurrent, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
TempFix1616);
VL53L1_GetLimitCheckValue(Dev,
VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
&LimitCheckValue);
SigmaLimitflag = (FilteredRangeStatus ==
VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK)
? 1 : 0;
VL53L1_GetLimitCheckEnable(Dev,
VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
&Temp8Enable);
Temp8 = ((Temp8Enable == 1) && (SigmaLimitflag == 1)) ? 1 : 0;
VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
VL53L1_GetLimitCheckValue(Dev,
VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&LimitCheckValue);
SignalLimitflag = (FilteredRangeStatus ==
VL53L1_DEVICEERROR_MSRCNOTARGET)
? 1 : 0;
VL53L1_GetLimitCheckEnable(Dev,
VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&Temp8Enable);
Temp8 = ((Temp8Enable == 1) && (SignalLimitflag == 1)) ? 1 : 0;
VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, Temp8);
Range = pRangeData->RangeMilliMeter;
if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
(Range < 0)) {
if (Range < BDTable[VL53L1_TUNING_PROXY_MIN])
pRangeData->RangeStatus =
VL53L1_RANGESTATUS_RANGE_INVALID;
else
pRangeData->RangeMilliMeter = 0;
}
VL53L1_get_tuning_parm(Dev,
VL53L1_TUNINGPARM_MIN_SIGNAL_SECONDARY_TARGETS,
&TuningMinValidSignal);
MinValidSignal = (FixPoint1616_t)TuningMinValidSignal;
if ((iteration > 0) &&
(pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
(pRangeData->SignalRateRtnMegaCps < MinValidSignal))
pRangeData->RangeStatus =
VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL;
VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_UWR_ENABLE,
&ExtendedRangeEnabled);
sequency = 1 - pdev->hist_data.number_of_ambient_bins / 4;
uwr_status = 0;
RangeMillimeterInit = pRangeData->RangeMilliMeter;
AddOffset = 0;
pRangeData->ExtendedRange = 0;
if (pRangeData->AmbientRateRtnMegaCps > MAX_AMBIENT)
ExtendedRangeEnabled = 0;
if ((active_results != 1) ||
(pdev->PreviousRangeActiveResults[roi] != 1))
ExtendedRangeEnabled = 0;
Status = VL53L1_compute_nb_frame_in_buffer(Dev,
&histo_merge_nb, roi, timing);
if (histo_merge_nb < 2)
{
ExtendedRangeEnabled = 0;
}
Status = VL53L1_compute_nb_frame_in_buffer(Dev,
&histo_merge_nb, roi, 1 - timing);
if (histo_merge_nb < 2)
{
ExtendedRangeEnabled = 0;
}
if ((pRangeData->RangeMaxMilliMeter -
pRangeData->RangeMinMilliMeter) > MAX_RANGE_DIFF)
{
if (pRangeData->RangeStatus != 0)
{
pRangeData->RangeStatus = 255;
ExtendedRangeEnabled = 0;
}
}
if (ExtendedRangeEnabled &&
(pRangeData->RangeStatus ==
VL53L1_RANGESTATUS_WRAP_TARGET_FAIL ||
pRangeData->RangeStatus ==
VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL)
&& (pdev->PreviousRangeStatus[roi] ==
VL53L1_RANGESTATUS_WRAP_TARGET_FAIL ||
pdev->PreviousRangeStatus[roi] ==
VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL ||
pdev->PreviousRangeStatus[roi] ==
VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL ||
(pdev->PreviousRangeStatus[roi] ==
VL53L1_RANGESTATUS_RANGE_VALID &&
pdev->PreviousExtendedRange[roi] == 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[roi];
uwr_status = 1;
switch (pdev->preset_mode) {
case VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_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
if (RangeDiff > tp->tp_uwr_lng_z_4_min &&
RangeDiff < tp->tp_uwr_lng_z_4_max &&
sequency == 1) {
AddOffset =
tp->tp_uwr_lng_corr_z_4_rangeb;
}
else
if (RangeDiff < -tp->tp_uwr_lng_z_4_min &&
RangeDiff > -tp->tp_uwr_lng_z_4_max &&
sequency == 0) {
AddOffset =
tp->tp_uwr_lng_corr_z_4_rangea;
}
else
uwr_status = 0;
break;
case VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE:
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
if (RangeDiff > tp->tp_uwr_med_z_6_min &&
RangeDiff < tp->tp_uwr_med_z_6_max &&
sequency == 1) {
AddOffset =
tp->tp_uwr_med_corr_z_6_rangeb;
}
else
if (RangeDiff < -tp->tp_uwr_med_z_6_min &&
RangeDiff > -tp->tp_uwr_med_z_6_max &&
sequency == 0) {
AddOffset =
tp->tp_uwr_med_corr_z_6_rangea;
}
else
uwr_status = 0;
break;
case VL53L1_DEVICEPRESETMODE_HISTOGRAM_SHORT_RANGE:
uwr_status = 0;
break;
case VL53L1_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
if (RangeDiff > tp->tp_uwr_med_z_6_min &&
RangeDiff < tp->tp_uwr_med_z_6_max &&
sequency == 1) {
AddOffset =
tp->tp_uwr_med_corr_z_6_rangeb;
}
else
if (RangeDiff < -tp->tp_uwr_med_z_6_min &&
RangeDiff > -tp->tp_uwr_med_z_6_max &&
sequency == 0) {
AddOffset =
tp->tp_uwr_med_corr_z_6_rangea;
}
else
uwr_status = 0;
break;
case VL53L1_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
if (RangeDiff > tp->tp_uwr_lng_z_4_min &&
RangeDiff < tp->tp_uwr_lng_z_4_max &&
sequency == 1) {
AddOffset =
tp->tp_uwr_lng_corr_z_4_rangeb;
}
else
if (RangeDiff < -tp->tp_uwr_lng_z_4_min &&
RangeDiff > -tp->tp_uwr_lng_z_4_max &&
sequency == 0) {
AddOffset =
tp->tp_uwr_lng_corr_z_4_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[roi] = RangeMillimeterInit;
pdev->PreviousRangeStatus[roi] = pRangeData->RangeStatus;
pdev->PreviousExtendedRange[roi] = pRangeData->ExtendedRange;
pdev->PreviousRangeActiveResults[roi] = active_results;
return Status;
}
static uint8_t GetOutputDataIndex(VL53L1_DEV Dev,
VL53L1_range_results_t *presults)
{
uint8_t i;
uint8_t index = 0;
VL53L1_OutputModes OutputMode;
OutputMode = VL53L1DevDataGet(Dev, CurrentParameters.OutputMode);
if (OutputMode == VL53L1_OUTPUTMODE_NEAREST)
return 0;
for (i = 1; i < presults->active_results; i++) {
if (presults->VL53L1_p_002[i].peak_signal_count_rate_mcps >
presults->VL53L1_p_002[index].peak_signal_count_rate_mcps)
index = i;
}
return index;
}
VL53L1_Error VL53L1_GetRangingMeasurementData(VL53L1_DEV Dev,
VL53L1_RangingMeasurementData_t *pRangingMeasurementData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_range_results_t *presults =
(VL53L1_range_results_t *) pdev->wArea1;
VL53L1_range_data_t *presults_data;
VL53L1_PresetModes PresetMode;
uint8_t index = 0;
LOG_FUNCTION_START("");
PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
if (PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) {
Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
LOG_FUNCTION_END(Status);
return Status;
}
memset(pRangingMeasurementData, 0xFF,
sizeof(VL53L1_RangingMeasurementData_t));
Status = VL53L1_get_device_results(
Dev,
VL53L1_DEVICERESULTSLEVEL_FULL,
presults);
if (Status == VL53L1_ERROR_NONE) {
pRangingMeasurementData->StreamCount = presults->stream_count;
index = GetOutputDataIndex(Dev, presults);
presults_data = &(presults->VL53L1_p_002[index]);
Status = SetSimpleData(Dev, presults->active_results,
presults->device_status,
presults_data,
pRangingMeasurementData);
}
LOG_FUNCTION_END(Status);
return Status;
}
static VL53L1_Error SetMeasurementData(VL53L1_DEV Dev,
VL53L1_range_results_t *presults,
VL53L1_MultiRangingData_t *pMultiRangingData)
{
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
uint8_t i;
uint8_t iteration;
VL53L1_TargetRangeData_t *pRangeData;
VL53L1_range_data_t *presults_data;
int16_t dmax_min;
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t Furthest_idx = 0;
int16_t Furthest_range = 0;
uint8_t ActiveResults, amb_idx;
pMultiRangingData->NumberOfObjectsFound = presults->active_results;
pMultiRangingData->RoiNumber = presults->zone_id;
pMultiRangingData->HasXtalkValueChanged =
presults->smudge_corrector_data.new_xtalk_applied_flag;
dmax_min = MIN(presults->wrap_dmax_mm,
presults->VL53L1_p_007[DMAX_REFLECTANCE_IDX]);
pMultiRangingData->DmaxMilliMeter = dmax_min;
pMultiRangingData->TimeStamp = 0;
pMultiRangingData->StreamCount = presults->stream_count;
pMultiRangingData->RecommendedDistanceMode =
VL53L1DevDataGet(Dev, CurrentParameters.DistanceMode);
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->VL53L1_p_002[i]);
if (Status == VL53L1_ERROR_NONE)
Status = SetTargetData(Dev, ActiveResults,
i,
presults->device_status,
presults_data,
pRangeData);
pMultiRangingData->EffectiveSpadRtnCount =
presults_data->VL53L1_p_006;
if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID)
&& (pRangeData->RangeMilliMeter > Furthest_range)) {
Furthest_range = pRangeData->RangeMilliMeter;
Furthest_idx = i;
}
}
pdev->PreviousStreamCount = pdev->hist_data.result__stream_count;
if ((Status == VL53L1_ERROR_NONE) && (ActiveResults > 0)) {
pRangeData = &(pMultiRangingData->RangeData[Furthest_idx]);
amb_idx = VL53L1_MAX_AMBIENT_DMAX_VALUES-1;
CheckAndChangeDistanceMode(Dev, pRangeData,
presults->VL53L1_p_007[amb_idx],
&pMultiRangingData->RecommendedDistanceMode);
}
return Status;
}
VL53L1_Error VL53L1_GetMultiRangingData(VL53L1_DEV Dev,
VL53L1_MultiRangingData_t *pMultiRangingData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_range_results_t *presults =
(VL53L1_range_results_t *) pdev->wArea1;
LOG_FUNCTION_START("");
memset(pMultiRangingData, 0xFF,
sizeof(VL53L1_MultiRangingData_t));
Status = VL53L1_get_device_results(
Dev,
VL53L1_DEVICERESULTSLEVEL_FULL,
presults);
if (Status == VL53L1_ERROR_NONE) {
switch (presults->rd_device_state) {
case VL53L1_DEVICESTATE_RANGING_GATHER_DATA:
pMultiRangingData->RoiStatus =
VL53L1_ROISTATUS_VALID_NOT_LAST;
break;
case VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA:
pMultiRangingData->RoiStatus =
VL53L1_ROISTATUS_VALID_LAST;
break;
default:
pMultiRangingData->RoiStatus =
VL53L1_ROISTATUS_NOT_VALID;
}
Status = SetMeasurementData(Dev,
presults,
pMultiRangingData);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetAdditionalData(VL53L1_DEV Dev,
VL53L1_AdditionalData_t *pAdditionalData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_additional_data(Dev, pAdditionalData);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetTuningParameter(VL53L1_DEV Dev,
uint16_t TuningParameterId, int32_t TuningParameterValue)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if (TuningParameterId ==
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS)
return VL53L1_ERROR_INVALID_PARAMS;
if (TuningParameterId >= 32768)
Status = VL53L1_set_tuning_parm(Dev,
TuningParameterId,
TuningParameterValue);
else {
if (TuningParameterId < VL53L1_TUNING_MAX_TUNABLE_KEY)
BDTable[TuningParameterId] = TuningParameterValue;
else
Status = VL53L1_ERROR_INVALID_PARAMS;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetTuningParameter(VL53L1_DEV Dev,
uint16_t TuningParameterId, int32_t *pTuningParameterValue)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if (TuningParameterId >= 32768)
Status = VL53L1_get_tuning_parm(Dev,
TuningParameterId,
pTuningParameterValue);
else {
if (TuningParameterId < VL53L1_TUNING_MAX_TUNABLE_KEY)
*pTuningParameterValue = BDTable[TuningParameterId];
else
Status = VL53L1_ERROR_INVALID_PARAMS;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_PerformRefSpadManagement(VL53L1_DEV Dev)
{
#ifdef VL53L1_NOCALIB
VL53L1_Error Status = VL53L1_ERROR_NOT_SUPPORTED;
SUPPRESS_UNUSED_WARNING(Dev);
LOG_FUNCTION_START("");
#else
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_Error RawStatus;
uint8_t dcrbuffer[24];
uint8_t *commbuf;
uint8_t numloc[2] = {5, 3};
VL53L1_LLDriverData_t *pdev;
VL53L1_customer_nvm_managed_t *pc;
VL53L1_PresetModes PresetMode;
LOG_FUNCTION_START("");
pdev = VL53L1DevStructGetLLDriverHandle(Dev);
pc = &pdev->customer;
PresetMode = VL53L1DevDataGet(Dev,
CurrentParameters.PresetMode);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_run_ref_spad_char(Dev, &RawStatus);
if (Status == VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH) {
Status = VL53L1_read_nvm_raw_data(Dev,
(uint8_t)(0xA0 >> 2),
(uint8_t)(24 >> 2),
dcrbuffer);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_WriteMulti(Dev,
VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
numloc, 2);
if (Status == VL53L1_ERROR_NONE) {
pc->ref_spad_man__num_requested_ref_spads = numloc[0];
pc->ref_spad_man__ref_location = numloc[1];
}
commbuf = &dcrbuffer[16];
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_WriteMulti(Dev,
VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
commbuf, 6);
if (Status == VL53L1_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];
}
}
VL53L1_SetPresetMode(Dev, PresetMode);
#endif
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SmudgeCorrectionEnable(VL53L1_DEV Dev,
VL53L1_SmudgeCorrectionModes Mode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_Error s1 = VL53L1_ERROR_NONE;
VL53L1_Error s2 = VL53L1_ERROR_NONE;
VL53L1_Error s3 = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
switch (Mode) {
case VL53L1_SMUDGE_CORRECTION_NONE:
s1 = VL53L1_dynamic_xtalk_correction_disable(Dev);
s2 = VL53L1_dynamic_xtalk_correction_apply_disable(Dev);
s3 = VL53L1_dynamic_xtalk_correction_single_apply_disable(Dev);
break;
case VL53L1_SMUDGE_CORRECTION_CONTINUOUS:
s1 = VL53L1_dynamic_xtalk_correction_enable(Dev);
s2 = VL53L1_dynamic_xtalk_correction_apply_enable(Dev);
s3 = VL53L1_dynamic_xtalk_correction_single_apply_disable(Dev);
break;
case VL53L1_SMUDGE_CORRECTION_SINGLE:
s1 = VL53L1_dynamic_xtalk_correction_enable(Dev);
s2 = VL53L1_dynamic_xtalk_correction_apply_enable(Dev);
s3 = VL53L1_dynamic_xtalk_correction_single_apply_enable(Dev);
break;
case VL53L1_SMUDGE_CORRECTION_DEBUG:
s1 = VL53L1_dynamic_xtalk_correction_enable(Dev);
s2 = VL53L1_dynamic_xtalk_correction_apply_disable(Dev);
s3 = VL53L1_dynamic_xtalk_correction_single_apply_disable(Dev);
break;
default:
Status = VL53L1_ERROR_INVALID_PARAMS;
break;
}
if (Status == VL53L1_ERROR_NONE) {
Status = s1;
if (Status == VL53L1_ERROR_NONE)
Status = s2;
if (Status == VL53L1_ERROR_NONE)
Status = s3;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetXTalkCompensationEnable(VL53L1_DEV Dev,
uint8_t XTalkCompensationEnable)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
if (XTalkCompensationEnable == 0)
Status = VL53L1_disable_xtalk_compensation(Dev);
else
Status = VL53L1_enable_xtalk_compensation(Dev);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetXTalkCompensationEnable(VL53L1_DEV Dev,
uint8_t *pXTalkCompensationEnable)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
VL53L1_get_xtalk_compensation_enable(
Dev,
pXTalkCompensationEnable);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_PerformXTalkCalibration(VL53L1_DEV Dev,
uint8_t CalibrationOption)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_Error UStatus;
int16_t CalDistanceMm;
VL53L1_xtalk_calibration_results_t xtalk;
VL53L1_CalibrationData_t caldata;
VL53L1_LLDriverData_t *pLLData;
int i;
uint32_t *pPlaneOffsetKcps;
uint32_t Margin =
BDTable[VL53L1_TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN];
uint32_t DefaultOffset =
BDTable[VL53L1_TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET];
uint32_t *pLLDataPlaneOffsetKcps;
uint32_t sum = 0;
uint8_t binok = 0;
int32_t merge;
VL53L1_DistanceModes DistanceMode;
uint32_t TimingBudgetMicroSeconds;
LOG_FUNCTION_START("");
Status = VL53L1_GetDistanceMode(Dev, &DistanceMode);
Status = VL53L1_GetMeasurementTimingBudgetMicroSeconds(Dev, &TimingBudgetMicroSeconds);
pPlaneOffsetKcps =
&caldata.customer.algo__crosstalk_compensation_plane_offset_kcps;
pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
pLLDataPlaneOffsetKcps =
&pLLData->xtalk_cal.algo__crosstalk_compensation_plane_offset_kcps;
VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, &merge);
VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, 0);
switch (CalibrationOption) {
case VL53L1_XTALKCALIBRATIONMODE_NO_TARGET:
Status = VL53L1_run_xtalk_extraction(Dev, &UStatus);
if (Status == VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL)
VL53L1_xtalk_cal_data_init(Dev);
break;
case VL53L1_XTALKCALIBRATIONMODE_SINGLE_TARGET:
Status = SingleTargetXTalkCalibration(Dev);
break;
case VL53L1_XTALKCALIBRATIONMODE_FULL_ROI:
CalDistanceMm = (int16_t)
BDTable[VL53L1_TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM];
VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
merge);
Status = VL53L1_run_hist_xtalk_extraction(Dev, CalDistanceMm,
&UStatus);
VL53L1_GetCalibrationData(Dev, &caldata);
for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++) {
sum += caldata.xtalkhisto.xtalk_shape.bin_data[i];
if (caldata.xtalkhisto.xtalk_shape.bin_data[i] > 0)
binok++;
}
if ((UStatus ==
VL53L1_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 < VL53L1_XTALK_HISTO_BINS; i++)
caldata.xtalkhisto.xtalk_shape.bin_data[i] = 0;
for (i = 0; i < VL53L1_BIN_REC_SIZE; i++)
caldata.algo__xtalk_cpo_HistoMerge_kcps[i] =
DefaultOffset + DefaultOffset * i;
VL53L1_SetCalibrationData(Dev, &caldata);
}
break;
default:
Status = VL53L1_ERROR_INVALID_PARAMS;
}
VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, merge);
if (Status == VL53L1_ERROR_NONE) {
Status = VL53L1_get_current_xtalk_settings(Dev, &xtalk);
Status = VL53L1_set_tuning_parm(Dev,
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS,
xtalk.algo__crosstalk_compensation_plane_offset_kcps);
}
VL53L1_SetDistanceMode(Dev, DistanceMode);
VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev, TimingBudgetMicroSeconds);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetOffsetCalibrationMode(VL53L1_DEV Dev,
VL53L1_OffsetCalibrationModes OffsetCalibrationMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_OffsetCalibrationMode offset_cal_mode;
LOG_FUNCTION_START("");
offset_cal_mode =
VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD;
if (OffsetCalibrationMode == VL53L1_OFFSETCALIBRATIONMODE_STANDARD) {
offset_cal_mode =
VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD;
} else if (OffsetCalibrationMode ==
VL53L1_OFFSETCALIBRATIONMODE_PRERANGE_ONLY) {
offset_cal_mode =
VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY;
} else if (OffsetCalibrationMode ==
VL53L1_OFFSETCALIBRATIONMODE_MULTI_ZONE) {
offset_cal_mode =
VL53L1_OFFSETCALIBRATIONMODE__PER_ZONE;
} else {
Status = VL53L1_ERROR_INVALID_PARAMS;
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_set_offset_calibration_mode(Dev,
offset_cal_mode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetOffsetCorrectionMode(VL53L1_DEV Dev,
VL53L1_OffsetCorrectionModes OffsetCorrectionMode)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_OffsetCorrectionMode offset_cor_mode;
LOG_FUNCTION_START("");
offset_cor_mode =
VL53L1_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS;
if (OffsetCorrectionMode == VL53L1_OFFSETCORRECTIONMODE_STANDARD) {
offset_cor_mode =
VL53L1_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS;
} else if (OffsetCorrectionMode ==
VL53L1_OFFSETCORRECTIONMODE_PERZONE) {
offset_cor_mode =
VL53L1_OFFSETCORRECTIONMODE__PER_ZONE_OFFSETS;
} else if (OffsetCorrectionMode ==
VL53L1_OFFSETCORRECTIONMODE_PERVCSEL) {
offset_cor_mode =
VL53L1_OFFSETCORRECTIONMODE__PER_VCSEL_OFFSETS;
} else {
Status = VL53L1_ERROR_INVALID_PARAMS;
}
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_set_offset_correction_mode(Dev,
offset_cor_mode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_PerformOffsetCalibration(VL53L1_DEV Dev,
int32_t CalDistanceMilliMeter, FixPoint1616_t CalReflectancePercent)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_Error UnfilteredStatus;
VL53L1_OffsetCalibrationMode offset_cal_mode;
uint16_t CalReflectancePercent_int;
VL53L1_DevicePresetModes device_preset_mode;
VL53L1_DeviceZonePreset zone_preset;
VL53L1_zone_config_t zone_cfg;
int32_t MergeEnabled;
LOG_FUNCTION_START("");
CalReflectancePercent_int =
VL53L1_FIXPOINT1616TOFIXPOINT72(CalReflectancePercent);
Status = VL53L1_get_offset_calibration_mode(Dev,
&offset_cal_mode);
if (Status != VL53L1_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
if ((offset_cal_mode ==
VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD) ||
(offset_cal_mode ==
VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY
)) {
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_run_offset_calibration(
Dev,
(int16_t)CalDistanceMilliMeter,
CalReflectancePercent_int,
&UnfilteredStatus);
} else if (offset_cal_mode ==
VL53L1_OFFSETCALIBRATIONMODE__PER_ZONE) {
VL53L1_get_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
&MergeEnabled);
VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE, 0);
device_preset_mode =
VL53L1_DEVICEPRESETMODE_HISTOGRAM_MULTIZONE_LONG_RANGE;
zone_preset = VL53L1_DEVICEZONEPRESET_CUSTOM;
Status = VL53L1_get_zone_config(Dev, &zone_cfg);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_run_zone_calibration(
Dev,
device_preset_mode,
zone_preset,
&zone_cfg,
(int16_t)CalDistanceMilliMeter,
CalReflectancePercent_int,
&UnfilteredStatus);
VL53L1_set_tuning_parm(Dev, VL53L1_TUNINGPARM_HIST_MERGE,
MergeEnabled);
} else {
Status = VL53L1_ERROR_INVALID_PARAMS;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_PerformOffsetSimpleCalibration(VL53L1_DEV Dev,
int32_t CalDistanceMilliMeter)
{
VL53L1_Error Status = VL53L1_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;
VL53L1_RangingMeasurementData_t RangingMeasurementData;
VL53L1_LLDriverData_t *pdev;
uint8_t goodmeas;
VL53L1_Error SmudgeStatus = VL53L1_ERROR_NONE;
uint8_t smudge_corr_en;
LOG_FUNCTION_START("");
pdev = VL53L1DevStructGetLLDriverHandle(Dev);
smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
SmudgeStatus = VL53L1_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[VL53L1_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
Max = BDTable[
VL53L1_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 == VL53L1_ERROR_NONE)) {
Status = VL53L1_StartMeasurement(Dev);
if (Status == VL53L1_ERROR_NONE) {
VL53L1_WaitMeasurementDataReady(Dev);
VL53L1_GetRangingMeasurementData(Dev,
&RangingMeasurementData);
VL53L1_ClearInterruptAndStartMeasurement(Dev);
}
inloopcount = 0;
offset_meas = 0;
while ((Status == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
(offset_meas < OverMax)) {
Status = VL53L1_WaitMeasurementDataReady(Dev);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_GetRangingMeasurementData(Dev,
&RangingMeasurementData);
goodmeas = (RangingMeasurementData.RangeStatus ==
VL53L1_RANGESTATUS_RANGE_VALID);
if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
sum_ranging = sum_ranging +
RangingMeasurementData.RangeMilliMeter;
inloopcount++;
}
Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
offset_meas++;
}
total_count += inloopcount;
if (inloopcount < UnderMax)
Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
VL53L1_StopMeasurement(Dev);
Repeat--;
}
if ((SmudgeStatus == VL53L1_ERROR_NONE) && (smudge_corr_en == 1))
SmudgeStatus = VL53L1_dynamic_xtalk_correction_enable(Dev);
if ((sum_ranging < 0) ||
(sum_ranging > ((int32_t) total_count * 0xffff)))
Status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
if ((Status == VL53L1_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 = VL53L1_set_customer_nvm_managed(Dev,
&(pdev->customer));
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_PerformOffsetZeroDistanceCalibration(VL53L1_DEV Dev)
{
#define START_OFFSET 50
VL53L1_Error Status = VL53L1_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;
VL53L1_RangingMeasurementData_t RangingMeasurementData;
VL53L1_LLDriverData_t *pdev;
uint8_t goodmeas;
VL53L1_Error SmudgeStatus = VL53L1_ERROR_NONE;
uint8_t smudge_corr_en;
LOG_FUNCTION_START("");
pdev = VL53L1DevStructGetLLDriverHandle(Dev);
smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
SmudgeStatus = VL53L1_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[
VL53L1_TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR];
Repeat = BDTable[VL53L1_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
Max = BDTable[
VL53L1_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 == VL53L1_ERROR_NONE)) {
Status = VL53L1_StartMeasurement(Dev);
if (Status == VL53L1_ERROR_NONE) {
VL53L1_WaitMeasurementDataReady(Dev);
VL53L1_GetRangingMeasurementData(Dev,
&RangingMeasurementData);
VL53L1_ClearInterruptAndStartMeasurement(Dev);
}
inloopcount = 0;
offset_meas = 0;
while ((Status == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
(offset_meas < OverMax)) {
Status = VL53L1_WaitMeasurementDataReady(Dev);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_GetRangingMeasurementData(Dev,
&RangingMeasurementData);
goodmeas = (RangingMeasurementData.RangeStatus ==
VL53L1_RANGESTATUS_RANGE_VALID);
if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
sum_ranging = sum_ranging +
RangingMeasurementData.RangeMilliMeter;
inloopcount++;
}
Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
offset_meas++;
}
total_count += inloopcount;
if (inloopcount < UnderMax)
Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
VL53L1_StopMeasurement(Dev);
Repeat--;
}
if ((SmudgeStatus == VL53L1_ERROR_NONE) && (smudge_corr_en == 1))
SmudgeStatus = VL53L1_dynamic_xtalk_correction_enable(Dev);
if ((sum_ranging < 0) ||
(sum_ranging > ((int32_t) total_count * 0xffff)))
Status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
if ((Status == VL53L1_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 = VL53L1_set_customer_nvm_managed(Dev,
&(pdev->customer));
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetCalibrationData(VL53L1_DEV Dev,
VL53L1_CalibrationData_t *pCalibrationData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_CustomerNvmManaged_t *pC;
VL53L1_calibration_data_t cal_data;
uint32_t x, IncomeVersion, CurrentVersion;
uint8_t CalStopsOn_cal_peak_rate_map = 0;
VL53L1_xtalk_calibration_results_t xtalk;
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
LOG_FUNCTION_START("");
cal_data.struct_version = pCalibrationData->struct_version -
VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
IncomeVersion = pCalibrationData->struct_version;
CurrentVersion = VL53L1_LL_CALIBRATION_DATA_STRUCT_VERSION +
VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
if ((IncomeVersion < CurrentVersion) &&
((IncomeVersion & 0xFFFFFF0F) ==
(CurrentVersion & 0xFFFFFF0F))) {
cal_data.struct_version =
VL53L1_LL_CALIBRATION_DATA_STRUCT_VERSION;
CalStopsOn_cal_peak_rate_map = 1;
pdev->tuning_parms.tp_hist_merge = 0;
}
memcpy(
&(cal_data.fmt_dmax_cal),
&(pCalibrationData->fmt_dmax_cal),
sizeof(VL53L1_dmax_calibration_data_t));
memcpy(
&(cal_data.cust_dmax_cal),
&(pCalibrationData->cust_dmax_cal),
sizeof(VL53L1_dmax_calibration_data_t));
memcpy(
&(cal_data.add_off_cal_data),
&(pCalibrationData->add_off_cal_data),
sizeof(VL53L1_additional_offset_cal_data_t));
memcpy(
&(cal_data.optical_centre),
&(pCalibrationData->optical_centre),
sizeof(VL53L1_optical_centre_t));
memcpy(
&(cal_data.xtalkhisto),
&(pCalibrationData->xtalkhisto),
sizeof(VL53L1_xtalk_histogram_data_t));
memcpy(
&(cal_data.gain_cal),
&(pCalibrationData->gain_cal),
sizeof(VL53L1_gain_calibration_data_t));
memcpy(
&(cal_data.cal_peak_rate_map),
&(pCalibrationData->cal_peak_rate_map),
sizeof(VL53L1_cal_peak_rate_map_t));
if (!CalStopsOn_cal_peak_rate_map)
memcpy(
&(cal_data.per_vcsel_cal_data),
&(pCalibrationData->per_vcsel_cal_data),
sizeof(VL53L1_per_vcsel_period_offset_cal_data_t));
else {
cal_data.per_vcsel_cal_data.short_a_offset_mm =
cal_data.per_vcsel_cal_data.short_b_offset_mm =
cal_data.per_vcsel_cal_data.medium_a_offset_mm =
cal_data.per_vcsel_cal_data.medium_b_offset_mm =
cal_data.per_vcsel_cal_data.long_a_offset_mm =
cal_data.per_vcsel_cal_data.long_b_offset_mm = 0;
}
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 = VL53L1_set_part_to_part_data(Dev, &cal_data);
if (Status != VL53L1_ERROR_NONE)
goto ENDFUNC;
Status = VL53L1_get_current_xtalk_settings(Dev, &xtalk);
if (Status != VL53L1_ERROR_NONE)
goto ENDFUNC;
xtalk.algo__crosstalk_compensation_plane_offset_kcps = x;
Status = VL53L1_set_tuning_parm(Dev,
VL53L1_TUNINGPARM_DYNXTALK_NODETECT_XTALK_OFFSET_KCPS,
x);
if (!CalStopsOn_cal_peak_rate_map)
memcpy(
&(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]),
&(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps[0]),
sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
else
memset(
&(xtalk.algo__xtalk_cpo_HistoMerge_kcps[0]), 0,
sizeof(pCalibrationData->algo__xtalk_cpo_HistoMerge_kcps));
Status = VL53L1_set_current_xtalk_settings(Dev, &xtalk);
ENDFUNC:
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetCalibrationData(VL53L1_DEV Dev,
VL53L1_CalibrationData_t *pCalibrationData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_calibration_data_t cal_data;
VL53L1_CustomerNvmManaged_t *pC;
VL53L1_customer_nvm_managed_t *pC2;
VL53L1_xtalk_calibration_results_t xtalk;
uint32_t tmp;
VL53L1_PresetModes PresetMode;
LOG_FUNCTION_START("");
Status = VL53L1_get_part_to_part_data(Dev, &cal_data);
pCalibrationData->struct_version = cal_data.struct_version +
VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
memcpy(
&(pCalibrationData->fmt_dmax_cal),
&(cal_data.fmt_dmax_cal),
sizeof(VL53L1_dmax_calibration_data_t));
memcpy(
&(pCalibrationData->cust_dmax_cal),
&(cal_data.cust_dmax_cal),
sizeof(VL53L1_dmax_calibration_data_t));
memcpy(
&(pCalibrationData->add_off_cal_data),
&(cal_data.add_off_cal_data),
sizeof(VL53L1_additional_offset_cal_data_t));
memcpy(
&(pCalibrationData->optical_centre),
&(cal_data.optical_centre),
sizeof(VL53L1_optical_centre_t));
memcpy(
&(pCalibrationData->xtalkhisto),
&(cal_data.xtalkhisto),
sizeof(VL53L1_xtalk_histogram_data_t));
memcpy(
&(pCalibrationData->gain_cal),
&(cal_data.gain_cal),
sizeof(VL53L1_gain_calibration_data_t));
memcpy(
&(pCalibrationData->cal_peak_rate_map),
&(cal_data.cal_peak_rate_map),
sizeof(VL53L1_cal_peak_rate_map_t));
memcpy(
&(pCalibrationData->per_vcsel_cal_data),
&(cal_data.per_vcsel_cal_data),
sizeof(VL53L1_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);
PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
if ((PresetMode == VL53L1_PRESETMODE_RANGING) ||
(PresetMode == VL53L1_PRESETMODE_MULTIZONES_SCANNING) ||
(PresetMode == VL53L1_PRESETMODE_PROXY_RANGING_MODE)
) {
Status = VL53L1_get_current_xtalk_settings(Dev, &xtalk);
if (Status != VL53L1_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;
}
VL53L1_Error VL53L1_SetZoneCalibrationData(VL53L1_DEV Dev,
VL53L1_ZoneCalibrationData_t *pZoneCalibrationData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_set_zone_calibration_data(Dev, pZoneCalibrationData);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetZoneCalibrationData(VL53L1_DEV Dev,
VL53L1_ZoneCalibrationData_t *pZoneCalibrationData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L1_get_zone_calibration_data(Dev, pZoneCalibrationData);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetOpticalCenter(VL53L1_DEV Dev,
FixPoint1616_t *pOpticalCenterX,
FixPoint1616_t *pOpticalCenterY)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_calibration_data_t CalibrationData;
LOG_FUNCTION_START("");
*pOpticalCenterX = 0;
*pOpticalCenterY = 0;
Status = VL53L1_get_part_to_part_data(Dev, &CalibrationData);
if (Status == VL53L1_ERROR_NONE) {
*pOpticalCenterX = VL53L1_FIXPOINT44TOFIXPOINT1616(
CalibrationData.optical_centre.x_centre);
*pOpticalCenterY = VL53L1_FIXPOINT44TOFIXPOINT1616(
CalibrationData.optical_centre.y_centre);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_SetThresholdConfig(VL53L1_DEV Dev,
VL53L1_DetectionConfig_t *pConfig)
{
#define BADTHRESBOUNDS(T) \
(((T.CrossMode == VL53L1_THRESHOLD_OUT_OF_WINDOW) || \
(T.CrossMode == VL53L1_THRESHOLD_IN_WINDOW)) && (T.Low > T.High))
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_GPIO_interrupt_config_t Cfg;
uint16_t g;
FixPoint1616_t gain, high1616, low1616;
VL53L1_LLDriverData_t *pdev;
LOG_FUNCTION_START("");
pdev = VL53L1DevStructGetLLDriverHandle(Dev);
Status = VL53L1_get_GPIO_interrupt_config(Dev, &Cfg);
if (Status != VL53L1_ERROR_NONE)
return Status;
if (pConfig->DetectionMode == VL53L1_DETECTION_NORMAL_RUN) {
Cfg.intr_new_measure_ready = 1;
Status = VL53L1_set_GPIO_interrupt_config_struct(Dev,
Cfg);
} else {
if (BADTHRESBOUNDS(pConfig->Distance))
Status = VL53L1_ERROR_INVALID_PARAMS;
if ((Status == VL53L1_ERROR_NONE) &&
(BADTHRESBOUNDS(pConfig->Rate)))
Status = VL53L1_ERROR_INVALID_PARAMS;
if (Status == VL53L1_ERROR_NONE) {
Cfg.intr_new_measure_ready = 0;
Cfg.intr_no_target = pConfig->IntrNoTarget;
g = pdev->gain_cal.standard_ranging_gain_factor;
if (g != 0) {
gain = (FixPoint1616_t) ((uint32_t)g << 5);
high1616 = (FixPoint1616_t) ((uint32_t)
pConfig->Distance.High << 16);
low1616 = (FixPoint1616_t) ((uint32_t)
pConfig->Distance.Low << 16);
high1616 = (high1616 + 32768) / gain;
low1616 = (low1616 + 32768) / gain;
Cfg.threshold_distance_high = (uint16_t)
(high1616 & 0xFFFF);
Cfg.threshold_distance_low = (uint16_t)
(low1616 & 0xFFFF);
}
Cfg.threshold_rate_high =
VL53L1_FIXPOINT1616TOFIXPOINT97(
pConfig->Rate.High);
Cfg.threshold_rate_low =
VL53L1_FIXPOINT1616TOFIXPOINT97(
pConfig->Rate.Low);
Cfg.intr_mode_distance = ConvertModeToLLD(
&Status,
pConfig->Distance.CrossMode);
if (Status == VL53L1_ERROR_NONE)
Cfg.intr_mode_rate = ConvertModeToLLD(
&Status,
pConfig->Rate.CrossMode);
}
if (Status == VL53L1_ERROR_NONE) {
Cfg.intr_combined_mode = 1;
switch (pConfig->DetectionMode) {
case VL53L1_DETECTION_DISTANCE_ONLY:
Cfg.threshold_rate_high = 0;
Cfg.threshold_rate_low = 0;
break;
case VL53L1_DETECTION_RATE_ONLY:
Cfg.threshold_distance_high = 0;
Cfg.threshold_distance_low = 0;
break;
case VL53L1_DETECTION_DISTANCE_OR_RATE:
break;
case VL53L1_DETECTION_DISTANCE_AND_RATE:
Cfg.intr_combined_mode = 0;
break;
default:
Status = VL53L1_ERROR_INVALID_PARAMS;
}
}
if (Status == VL53L1_ERROR_NONE)
Status =
VL53L1_set_GPIO_interrupt_config_struct(Dev, Cfg);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_GetThresholdConfig(VL53L1_DEV Dev,
VL53L1_DetectionConfig_t *pConfig)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_GPIO_interrupt_config_t Cfg;
LOG_FUNCTION_START("");
Status = VL53L1_get_GPIO_interrupt_config(Dev, &Cfg);
if (Status != VL53L1_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
pConfig->IntrNoTarget = Cfg.intr_no_target;
pConfig->Distance.High = Cfg.threshold_distance_high;
pConfig->Distance.Low = Cfg.threshold_distance_low;
pConfig->Rate.High =
VL53L1_FIXPOINT97TOFIXPOINT1616(
Cfg.threshold_rate_high);
pConfig->Rate.Low =
VL53L1_FIXPOINT97TOFIXPOINT1616(Cfg.threshold_rate_low);
pConfig->Distance.CrossMode =
ConvertModeFromLLD(&Status, Cfg.intr_mode_distance);
if (Status == VL53L1_ERROR_NONE)
pConfig->Rate.CrossMode =
ConvertModeFromLLD(&Status, Cfg.intr_mode_rate);
if (Cfg.intr_new_measure_ready == 1) {
pConfig->DetectionMode = VL53L1_DETECTION_NORMAL_RUN;
} else {
if (Status == VL53L1_ERROR_NONE) {
if (Cfg.intr_combined_mode == 0)
pConfig->DetectionMode =
VL53L1_DETECTION_DISTANCE_AND_RATE;
else {
if ((Cfg.threshold_distance_high == 0) &&
(Cfg.threshold_distance_low == 0))
pConfig->DetectionMode =
VL53L1_DETECTION_RATE_ONLY;
else if ((Cfg.threshold_rate_high == 0) &&
(Cfg.threshold_rate_low == 0))
pConfig->DetectionMode =
VL53L1_DETECTION_DISTANCE_ONLY;
else
pConfig->DetectionMode =
VL53L1_DETECTION_DISTANCE_OR_RATE;
}
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L1_Error VL53L1_PerformOffsetPerVcselCalibration(VL53L1_DEV Dev,
int32_t CalDistanceMilliMeter)
{
VL53L1_Error Status = VL53L1_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;
VL53L1_RangingMeasurementData_t RangingMeasurementData;
VL53L1_LLDriverData_t *pdev;
uint8_t goodmeas;
VL53L1_PresetModes currentMode;
VL53L1_DistanceModes currentDist;
VL53L1_DistanceModes DistMode[3] = {VL53L1_DISTANCEMODE_SHORT,
VL53L1_DISTANCEMODE_MEDIUM, VL53L1_DISTANCEMODE_LONG};
int16_t offsetA[3];
int16_t offsetB[3];
VL53L1_Error SmudgeStatus = VL53L1_ERROR_NONE;
uint8_t smudge_corr_en, isc;
LOG_FUNCTION_START("");
pdev = VL53L1DevStructGetLLDriverHandle(Dev);
smudge_corr_en = pdev->smudge_correct_config.smudge_corr_enabled;
SmudgeStatus = VL53L1_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;
Max = 2 * BDTable[
VL53L1_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
UnderMax = 1 + (Max / 2);
OverMax = Max + (Max / 2);
Status = VL53L1_GetPresetMode(Dev, &currentMode);
Status = VL53L1_GetDistanceMode(Dev, &currentDist);
while ((Repeat < 3) && (Status == VL53L1_ERROR_NONE)) {
Status = VL53L1_SetDistanceMode(Dev, DistMode[Repeat]);
Status = VL53L1_StartMeasurement(Dev);
if (Status == VL53L1_ERROR_NONE) {
VL53L1_WaitMeasurementDataReady(Dev);
VL53L1_GetRangingMeasurementData(Dev,
&RangingMeasurementData);
VL53L1_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 == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
(inloopcount < OverMax)) {
Status = VL53L1_WaitMeasurementDataReady(Dev);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_GetRangingMeasurementData(Dev,
&RangingMeasurementData);
goodmeas = (RangingMeasurementData.RangeStatus ==
VL53L1_RANGESTATUS_RANGE_VALID);
isc = pdev->ll_state.cfg_internal_stream_count;
if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
if (isc & 0x01) {
sum_ranging_range_A +=
RangingMeasurementData.RangeMilliMeter;
offset_meas_range_A++;
} else {
sum_ranging_range_B +=
RangingMeasurementData.RangeMilliMeter;
offset_meas_range_B++;
}
inloopcount = offset_meas_range_A +
offset_meas_range_B;
}
Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
}
if (inloopcount < UnderMax)
Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
VL53L1_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 = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
}
if ((Status == VL53L1_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 == VL53L1_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 == VL53L1_ERROR_NONE) && (smudge_corr_en == 1))
SmudgeStatus = VL53L1_dynamic_xtalk_correction_enable(Dev);
if (Status == VL53L1_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];
}
VL53L1_SetPresetMode(Dev, currentMode);
VL53L1_SetDistanceMode(Dev, currentDist);
LOG_FUNCTION_END(Status);
return Status;
}
Reference in New Issue View Git Blame Copy Permalink