ST Expansion SW Team
The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2
Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.
src/vl53l1_api.c
- Committer:
- charlesmn
- Date:
- 2020-11-08
- Revision:
- 1:76429facbf6f
- Parent:
- 0:3ac96e360672
- Child:
- 7:1add29d51e72
File content as of revision 1:76429facbf6f:
/*******************************************************************************
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
This file is part of VL53L1 Core and is dual licensed,
either 'STMicroelectronics
Proprietary license'
or 'BSD 3-clause "New" or "Revised" License' , at your option.
********************************************************************************
'STMicroelectronics Proprietary license'
********************************************************************************
License terms: STMicroelectronics Proprietary in accordance with licensing
terms at www.st.com/sla0081
STMicroelectronics confidential
Reproduction and Communication of this document is strictly prohibited unless
specifically authorized in writing by STMicroelectronics.
********************************************************************************
Alternatively, VL53L1 Core may be distributed under the terms of
'BSD 3-clause "New" or "Revised" License', in which case the following
provisions apply instead of the ones
mentioned above :
********************************************************************************
License terms: BSD 3-clause "New" or "Revised" License.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
********************************************************************************
*/
#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"
#include "mbed_wait_api.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,
};
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;
LOG_FUNCTION_START("");
Status = VL53L1_WrByte(Dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS,
DeviceAddress / 2);
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);
VL53L1DevDataSet(Dev, CurrentParameters.PresetMode,
VL53L1_PRESETMODE_RANGING);
}
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);
VL53L1DevDataSet(Dev, CurrentParameters.DistanceMode,
VL53L1_DISTANCEMODE_LONG);
VL53L1DevDataSet(Dev, CurrentParameters.OutputMode,
VL53L1_OUTPUTMODE_NEAREST);
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;
uint32_t phasecal_config_timeout_us;
uint32_t mm_config_timeout_us;
uint32_t lld_range_config_timeout_us;
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;
uint32_t MmTimeoutUs;
uint32_t PhaseCalTimeoutUs;
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;
uint8_t Mm2Enabled;
uint32_t TimingGuard;
uint32_t divisor;
uint32_t TimingBudget;
uint32_t MmTimeoutUs;
VL53L1_PresetModes PresetMode;
uint32_t PhaseCalTimeoutUs;
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:
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;
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("");
VL53L1_load_patch(Dev);
for (i = 0; i < VL53L1_MAX_RANGE_RESULTS; i++) {
pdev->PreviousRangeMilliMeter[i] = 0;
pdev->PreviousRangeStatus[i] = 255;
pdev->PreviousExtendedRange[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;
}
DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
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;
LOG_FUNCTION_START("");
Status = VL53L1_stop_range(Dev);
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 streamcount, uint8_t iteration,
uint8_t device_status, VL53L1_range_data_t *presults_data,
VL53L1_TargetRangeData_t *pRangeData)
{
VL53L1_Error Status = VL53L1_ERROR_NONE;
VL53L1_LLDriverData_t *pdev =
VL53L1DevStructGetLLDriverHandle(Dev);
VL53L1_tuning_parm_storage_t *tp =
&(pdev->tuning_parms);
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;
uint8_t ExtendedRangeEnabled = 0;
uint8_t uwr_status;
int16_t AddOffset;
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_UWR_ENABLE,
(int32_t *)&ExtendedRangeEnabled);
sequency = streamcount % 2;
uwr_status = 1;
RangeMillimeterInit = pRangeData->RangeMilliMeter;
AddOffset = 0;
pRangeData->ExtendedRange = 0;
if (ExtendedRangeEnabled &&
(pRangeData->RangeStatus ==
VL53L1_RANGESTATUS_WRAP_TARGET_FAIL ||
pRangeData->RangeStatus ==
VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL)
&& (pdev->PreviousRangeStatus[iteration] ==
VL53L1_RANGESTATUS_WRAP_TARGET_FAIL ||
pdev->PreviousRangeStatus[iteration] ==
VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL ||
(pdev->PreviousRangeStatus[iteration] ==
VL53L1_RANGESTATUS_RANGE_VALID &&
pdev->PreviousExtendedRange[iteration] == 1)))
{
if (((pdev->PreviousStreamCount) ==
(pdev->hist_data.result__stream_count - 1 ))
|| ((pdev->PreviousStreamCount) ==
(pdev->hist_data.result__stream_count + 127)))
{
RangeDiff = pRangeData->RangeMilliMeter -
pdev->PreviousRangeMilliMeter[iteration];
switch (pdev->preset_mode) {
case 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[iteration] = RangeMillimeterInit;
pdev->PreviousRangeStatus[iteration] = pRangeData->RangeStatus;
pdev->PreviousExtendedRange[iteration] = pRangeData->ExtendedRange;
pdev->PreviousStreamCount = pdev->hist_data.result__stream_count;
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,
pMultiRangingData->StreamCount,
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;
}
}
for (i = iteration; i < VL53L1_MAX_RANGE_RESULTS; i++) {
pdev->PreviousRangeMilliMeter[i] = 0;
pdev->PreviousRangeStatus[i] = 255;
pdev->PreviousExtendedRange[i] = 0;
}
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;
if (Status == VL53L1_ERROR_NONE) {
PresetMode = VL53L1DevDataGet(Dev,
CurrentParameters.PresetMode);
Status = VL53L1_run_ref_spad_char(Dev, &RawStatus);
if (Status == VL53L1_ERROR_NONE)
Status = VL53L1_SetPresetMode(Dev, PresetMode);
}
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];
}
if (Status == VL53L1_ERROR_NONE)
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];
}
}
#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;
LOG_FUNCTION_START("");
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);
}
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("");
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("");
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);
if (Status == VL53L1_ERROR_NONE)
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, ¤tMode);
Status = VL53L1_GetDistanceMode(Dev, ¤tDist);
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;
}