ケンタ ミヤザキ
/
X_NUCLEO_53L0A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
X_NUCLEO_53L0A1
by 
ST
Components/VL53L0X/vl53l0x_class.cpp
- Committer:
- johnAlexander
- Date:
- 2016-12-01
- Revision:
- 2:58b5e9097aa3
- Parent:
- 0:c523920bcc09
- Child:
- 7:35ecf3e060c3
File content as of revision 2:58b5e9097aa3:
/**
******************************************************************************
* @file vl53l0x_class.cpp
* @author IMG
* @version V0.0.1
* @date 28-June-2016
* @brief Implementation file for the VL53L0X driver class
******************************************************************************
* @attention
*
* <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
*
* 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 STMicroelectronics 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.
*
******************************************************************************
*/
/* Includes */
#include <stdlib.h>
#include "vl53l0x_class.h"
//#include "vl53l0x_api_core.h"
//#include "vl53l0x_api_calibration.h"
//#include "vl53l0x_api_strings.h"
#include "vl53l0x_interrupt_threshold_settings.h"
#include "vl53l0x_tuning.h"
#include "vl53l0x_types.h"
/****************** define for i2c configuration *******************************/
#define TEMP_BUF_SIZE 64
/** Maximum buffer size to be used in i2c */
#define VL53L0X_MAX_I2C_XFER_SIZE 64 /* Maximum buffer size to be used in i2c */
#define VL53L0X_I2C_USER_VAR /* none but could be for a flag var to get/pass to mutex interruptible return flags and try again */
#define LOG_FUNCTION_START(fmt, ...) \
_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
#ifdef VL53L0X_LOG_ENABLE
#define trace_print(level, ...) trace_print_module_function(TRACE_MODULE_API, \
level, TRACE_FUNCTION_NONE, ##__VA_ARGS__)
#endif
#define REF_ARRAY_SPAD_0 0
#define REF_ARRAY_SPAD_5 5
#define REF_ARRAY_SPAD_10 10
uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5,
REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 };
VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t strobe;
uint32_t LoopNb;
LOG_FUNCTION_START("");
Status |= VL53L0X_WrByte(Dev, 0x83, 0x00);
/* polling
* use timeout to avoid deadlock*/
if (Status == VL53L0X_ERROR_NONE) {
LoopNb = 0;
do {
Status = VL53L0X_RdByte(Dev, 0x83, &strobe);
if ((strobe != 0x00) || Status != VL53L0X_ERROR_NONE)
break;
LoopNb = LoopNb + 1;
} while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
Status = VL53L0X_ERROR_TIME_OUT;
}
Status |= VL53L0X_WrByte(Dev, 0x83, 0x01);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t byte;
uint32_t TmpDWord;
uint8_t ModuleId;
uint8_t Revision;
uint8_t ReferenceSpadCount = 0;
uint8_t ReferenceSpadType = 0;
uint32_t PartUIDUpper = 0;
uint32_t PartUIDLower = 0;
uint32_t OffsetFixed1104_mm = 0;
int16_t OffsetMicroMeters = 0;
uint32_t DistMeasTgtFixed1104_mm = 400 << 4;
uint32_t DistMeasFixed1104_400_mm = 0;
uint32_t SignalRateMeasFixed1104_400_mm = 0;
char ProductId[19];
char *ProductId_tmp;
uint8_t ReadDataFromDeviceDone;
FixPoint1616_t SignalRateMeasFixed400mmFix = 0;
uint8_t NvmRefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
int i;
LOG_FUNCTION_START("");
ReadDataFromDeviceDone = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
ReadDataFromDeviceDone);
/* This access is done only once after that a GetDeviceInfo or
* datainit is done*/
if (ReadDataFromDeviceDone != 7) {
Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
Status |= VL53L0X_WrByte(Dev, 0x83, byte|4);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x07);
Status |= VL53L0X_WrByte(Dev, 0x81, 0x01);
Status |= VL53L0X_PollingDelay(Dev);
Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
if (((option & 1) == 1) &&
((ReadDataFromDeviceDone & 1) == 0)) {
Status |= VL53L0X_WrByte(Dev, 0x94, 0x6b);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
ReferenceSpadCount = (uint8_t)((TmpDWord >> 8) & 0x07f);
ReferenceSpadType = (uint8_t)((TmpDWord >> 15) & 0x01);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x24);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
NvmRefGoodSpadMap[0] = (uint8_t)((TmpDWord >> 24)
& 0xff);
NvmRefGoodSpadMap[1] = (uint8_t)((TmpDWord >> 16)
& 0xff);
NvmRefGoodSpadMap[2] = (uint8_t)((TmpDWord >> 8)
& 0xff);
NvmRefGoodSpadMap[3] = (uint8_t)(TmpDWord & 0xff);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x25);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
NvmRefGoodSpadMap[4] = (uint8_t)((TmpDWord >> 24)
& 0xff);
NvmRefGoodSpadMap[5] = (uint8_t)((TmpDWord >> 16)
& 0xff);
}
if (((option & 2) == 2) &&
((ReadDataFromDeviceDone & 2) == 0)) {
Status |= VL53L0X_WrByte(Dev, 0x94, 0x02);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdByte(Dev, 0x90, &ModuleId);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdByte(Dev, 0x90, &Revision);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x77);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
ProductId[0] = (char)((TmpDWord >> 25) & 0x07f);
ProductId[1] = (char)((TmpDWord >> 18) & 0x07f);
ProductId[2] = (char)((TmpDWord >> 11) & 0x07f);
ProductId[3] = (char)((TmpDWord >> 4) & 0x07f);
byte = (uint8_t)((TmpDWord & 0x00f) << 3);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x78);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
ProductId[4] = (char)(byte +
((TmpDWord >> 29) & 0x07f));
ProductId[5] = (char)((TmpDWord >> 22) & 0x07f);
ProductId[6] = (char)((TmpDWord >> 15) & 0x07f);
ProductId[7] = (char)((TmpDWord >> 8) & 0x07f);
ProductId[8] = (char)((TmpDWord >> 1) & 0x07f);
byte = (uint8_t)((TmpDWord & 0x001) << 6);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x79);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
ProductId[9] = (char)(byte +
((TmpDWord >> 26) & 0x07f));
ProductId[10] = (char)((TmpDWord >> 19) & 0x07f);
ProductId[11] = (char)((TmpDWord >> 12) & 0x07f);
ProductId[12] = (char)((TmpDWord >> 5) & 0x07f);
byte = (uint8_t)((TmpDWord & 0x01f) << 2);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x7A);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
ProductId[13] = (char)(byte +
((TmpDWord >> 30) & 0x07f));
ProductId[14] = (char)((TmpDWord >> 23) & 0x07f);
ProductId[15] = (char)((TmpDWord >> 16) & 0x07f);
ProductId[16] = (char)((TmpDWord >> 9) & 0x07f);
ProductId[17] = (char)((TmpDWord >> 2) & 0x07f);
ProductId[18] = '\0';
}
if (((option & 4) == 4) &&
((ReadDataFromDeviceDone & 4) == 0)) {
Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDUpper);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x7C);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDLower);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x73);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
SignalRateMeasFixed1104_400_mm = (TmpDWord &
0x0000000ff) << 8;
Status |= VL53L0X_WrByte(Dev, 0x94, 0x74);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
SignalRateMeasFixed1104_400_mm |= ((TmpDWord &
0xff000000) >> 24);
Status |= VL53L0X_WrByte(Dev, 0x94, 0x75);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
DistMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff)
<< 8;
Status |= VL53L0X_WrByte(Dev, 0x94, 0x76);
Status |= VL53L0X_device_read_strobe(Dev);
Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
DistMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000)
>> 24);
}
Status |= VL53L0X_WrByte(Dev, 0x81, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
Status |= VL53L0X_WrByte(Dev, 0x83, byte&0xfb);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
}
if ((Status == VL53L0X_ERROR_NONE) &&
(ReadDataFromDeviceDone != 7)) {
/* Assign to variable if status is ok */
if (((option & 1) == 1) &&
((ReadDataFromDeviceDone & 1) == 0)) {
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadCount, ReferenceSpadCount);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadType, ReferenceSpadType);
for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
Dev->Data.SpadData.RefGoodSpadMap[i] =
NvmRefGoodSpadMap[i];
}
}
if (((option & 2) == 2) &&
((ReadDataFromDeviceDone & 2) == 0)) {
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ModuleId, ModuleId);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
Revision, Revision);
ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
ProductId);
VL53L0X_COPYSTRING(ProductId_tmp, ProductId);
}
if (((option & 4) == 4) &&
((ReadDataFromDeviceDone & 4) == 0)) {
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
PartUIDUpper, PartUIDUpper);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
PartUIDLower, PartUIDLower);
SignalRateMeasFixed400mmFix =
VL53L0X_FIXPOINT97TOFIXPOINT1616(
SignalRateMeasFixed1104_400_mm);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
SignalRateMeasFixed400mm,
SignalRateMeasFixed400mmFix);
OffsetMicroMeters = 0;
if (DistMeasFixed1104_400_mm != 0) {
OffsetFixed1104_mm =
DistMeasFixed1104_400_mm -
DistMeasTgtFixed1104_mm;
OffsetMicroMeters = (OffsetFixed1104_mm
* 1000) >> 4;
OffsetMicroMeters *= -1;
}
PALDevDataSet(Dev,
Part2PartOffsetAdjustmentNVMMicroMeter,
OffsetMicroMeters);
}
byte = (uint8_t)(ReadDataFromDeviceDone|option);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone,
byte);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
int32_t *pOffsetCalibrationDataMicroMeter)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint16_t RangeOffsetRegister;
int16_t cMaxOffset = 2047;
int16_t cOffsetRange = 4096;
/* Note that offset has 10.2 format */
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
&RangeOffsetRegister);
if (Status == VL53L0X_ERROR_NONE) {
RangeOffsetRegister = (RangeOffsetRegister & 0x0fff);
/* Apply 12 bit 2's compliment conversion */
if (RangeOffsetRegister > cMaxOffset)
*pOffsetCalibrationDataMicroMeter =
(int16_t)(RangeOffsetRegister - cOffsetRange)
* 250;
else
*pOffsetCalibrationDataMicroMeter =
(int16_t)RangeOffsetRegister * 250;
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev,
int32_t *pOffsetCalibrationDataMicroMeter)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_get_offset_calibration_data_micro_meter(Dev,
pOffsetCalibrationDataMicroMeter);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
int32_t OffsetCalibrationDataMicroMeter)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
int32_t cMaxOffsetMicroMeter = 511000;
int32_t cMinOffsetMicroMeter = -512000;
int16_t cOffsetRange = 4096;
uint32_t encodedOffsetVal;
LOG_FUNCTION_START("");
if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter)
OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter;
else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter)
OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter;
/* The offset register is 10.2 format and units are mm
* therefore conversion is applied by a division of
* 250.
*/
if (OffsetCalibrationDataMicroMeter >= 0) {
encodedOffsetVal =
OffsetCalibrationDataMicroMeter/250;
} else {
encodedOffsetVal =
cOffsetRange +
OffsetCalibrationDataMicroMeter/250;
}
Status = VL53L0X_WrWord(Dev,
VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
encodedOffsetVal);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev,
int32_t OffsetCalibrationDataMicroMeter)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_set_offset_calibration_data_micro_meter(Dev,
OffsetCalibrationDataMicroMeter);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
int32_t CorrectedOffsetMicroMeters;
int32_t CurrentOffsetMicroMeters;
/* if we run on this function we can read all the NVM info
* used by the API */
Status = VL53L0X_get_info_from_device(Dev, 7);
/* Read back current device offset */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
&CurrentOffsetMicroMeters);
}
/* Apply Offset Adjustment derived from 400mm measurements */
if (Status == VL53L0X_ERROR_NONE) {
/* Store initial device offset */
PALDevDataSet(Dev, Part2PartOffsetNVMMicroMeter,
CurrentOffsetMicroMeters);
CorrectedOffsetMicroMeters = CurrentOffsetMicroMeters +
(int32_t)PALDevDataGet(Dev,
Part2PartOffsetAdjustmentNVMMicroMeter);
Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
CorrectedOffsetMicroMeters);
/* store current, adjusted offset */
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
CorrectedOffsetMicroMeters);
}
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetDeviceMode(VL53L0X_DEV Dev,
VL53L0X_DeviceModes *pDeviceMode)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
VL53L0X_GETPARAMETERFIELD(Dev, DeviceMode, *pDeviceMode);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetInterMeasurementPeriodMilliSeconds(VL53L0X_DEV Dev,
uint32_t *pInterMeasurementPeriodMilliSeconds)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint16_t osc_calibrate_val;
uint32_t IMPeriodMilliSeconds;
LOG_FUNCTION_START("");
Status = VL53L0X_RdWord(Dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
&osc_calibrate_val);
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdDWord(Dev,
VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
&IMPeriodMilliSeconds);
}
if (Status == VL53L0X_ERROR_NONE) {
if (osc_calibrate_val != 0) {
*pInterMeasurementPeriodMilliSeconds =
IMPeriodMilliSeconds / osc_calibrate_val;
}
VL53L0X_SETPARAMETERFIELD(Dev,
InterMeasurementPeriodMilliSeconds,
*pInterMeasurementPeriodMilliSeconds);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationRateMegaCps(VL53L0X_DEV Dev,
FixPoint1616_t *pXTalkCompensationRateMegaCps)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint16_t Value;
FixPoint1616_t TempFix1616;
LOG_FUNCTION_START("");
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&Value);
if (Status == VL53L0X_ERROR_NONE) {
if (Value == 0) {
/* the Xtalk is disabled return value from memory */
VL53L0X_GETPARAMETERFIELD(Dev,
XTalkCompensationRateMegaCps, TempFix1616);
*pXTalkCompensationRateMegaCps = TempFix1616;
VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable,
0);
} else {
TempFix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(Value);
*pXTalkCompensationRateMegaCps = TempFix1616;
VL53L0X_SETPARAMETERFIELD(Dev,
XTalkCompensationRateMegaCps, TempFix1616);
VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable,
1);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId,
FixPoint1616_t *pLimitCheckValue)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t EnableZeroValue = 0;
uint16_t Temp16;
FixPoint1616_t TempFix1616;
LOG_FUNCTION_START("");
switch (LimitCheckId) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, TempFix1616);
EnableZeroValue = 0;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
&Temp16);
if (Status == VL53L0X_ERROR_NONE)
TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16);
EnableZeroValue = 1;
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, TempFix1616);
EnableZeroValue = 0;
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, TempFix1616);
EnableZeroValue = 0;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
&Temp16);
if (Status == VL53L0X_ERROR_NONE)
TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16);
EnableZeroValue = 0;
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
if (Status == VL53L0X_ERROR_NONE) {
if (EnableZeroValue == 1) {
if (TempFix1616 == 0) {
/* disabled: return value from memory */
VL53L0X_GETARRAYPARAMETERFIELD(Dev,
LimitChecksValue, LimitCheckId,
TempFix1616);
*pLimitCheckValue = TempFix1616;
VL53L0X_SETARRAYPARAMETERFIELD(Dev,
LimitChecksEnable, LimitCheckId, 0);
} else {
*pLimitCheckValue = TempFix1616;
VL53L0X_SETARRAYPARAMETERFIELD(Dev,
LimitChecksValue, LimitCheckId,
TempFix1616);
VL53L0X_SETARRAYPARAMETERFIELD(Dev,
LimitChecksEnable, LimitCheckId, 1);
}
} else {
*pLimitCheckValue = TempFix1616;
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId,
uint8_t *pLimitCheckEnable)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t Temp8;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L0X_ERROR_INVALID_PARAMS;
*pLimitCheckEnable = 0;
} else {
VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
LimitCheckId, Temp8);
*pLimitCheckEnable = Temp8;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetWrapAroundCheckEnable(VL53L0X_DEV Dev,
uint8_t *pWrapAroundCheckEnable)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t data;
LOG_FUNCTION_START("");
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data);
if (Status == VL53L0X_ERROR_NONE) {
PALDevDataSet(Dev, SequenceConfig, data);
if (data & (0x01 << 7))
*pWrapAroundCheckEnable = 0x01;
else
*pWrapAroundCheckEnable = 0x00;
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETPARAMETERFIELD(Dev, WrapAroundCheckEnable,
*pWrapAroundCheckEnable);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_DEV Dev,
VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceConfig,
uint8_t *pSequenceStepEnabled)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
*pSequenceStepEnabled = 0;
LOG_FUNCTION_START("");
switch (SequenceStepId) {
case VL53L0X_SEQUENCESTEP_TCC:
*pSequenceStepEnabled = (SequenceConfig & 0x10) >> 4;
break;
case VL53L0X_SEQUENCESTEP_DSS:
*pSequenceStepEnabled = (SequenceConfig & 0x08) >> 3;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
*pSequenceStepEnabled = (SequenceConfig & 0x04) >> 2;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
*pSequenceStepEnabled = (SequenceConfig & 0x40) >> 6;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
*pSequenceStepEnabled = (SequenceConfig & 0x80) >> 7;
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetSequenceStepEnables(VL53L0X_DEV Dev,
VL53L0X_SchedulerSequenceSteps_t *pSchedulerSequenceSteps)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t SequenceConfig = 0;
LOG_FUNCTION_START("");
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
&SequenceConfig);
if (Status == VL53L0X_ERROR_NONE) {
Status = sequence_step_enabled(Dev,
VL53L0X_SEQUENCESTEP_TCC, SequenceConfig,
&pSchedulerSequenceSteps->TccOn);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = sequence_step_enabled(Dev,
VL53L0X_SEQUENCESTEP_DSS, SequenceConfig,
&pSchedulerSequenceSteps->DssOn);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = sequence_step_enabled(Dev,
VL53L0X_SEQUENCESTEP_MSRC, SequenceConfig,
&pSchedulerSequenceSteps->MsrcOn);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = sequence_step_enabled(Dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE, SequenceConfig,
&pSchedulerSequenceSteps->PreRangeOn);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = sequence_step_enabled(Dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE, SequenceConfig,
&pSchedulerSequenceSteps->FinalRangeOn);
}
LOG_FUNCTION_END(Status);
return Status;
}
uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
{
/*!
* Converts the encoded VCSEL period register value into the real
* period in PLL clocks
*/
uint8_t vcsel_period_pclks = 0;
vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
return vcsel_period_pclks;
}
uint8_t VL53L0X::VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks)
{
/*!
* Converts the encoded VCSEL period register value into the real period
* in PLL clocks
*/
uint8_t vcsel_period_reg = 0;
vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
return vcsel_period_reg;
}
VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t vcsel_period_reg;
uint8_t MinPreVcselPeriodPCLK = 12;
uint8_t MaxPreVcselPeriodPCLK = 18;
uint8_t MinFinalVcselPeriodPCLK = 8;
uint8_t MaxFinalVcselPeriodPCLK = 14;
uint32_t MeasurementTimingBudgetMicroSeconds;
uint32_t FinalRangeTimeoutMicroSeconds;
uint32_t PreRangeTimeoutMicroSeconds;
uint32_t MsrcTimeoutMicroSeconds;
uint8_t PhaseCalInt = 0;
/* Check if valid clock period requested */
if ((VCSELPulsePeriodPCLK % 2) != 0) {
/* Value must be an even number */
Status = VL53L0X_ERROR_INVALID_PARAMS;
} else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
(VCSELPulsePeriodPCLK < MinPreVcselPeriodPCLK ||
VCSELPulsePeriodPCLK > MaxPreVcselPeriodPCLK)) {
Status = VL53L0X_ERROR_INVALID_PARAMS;
} else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
(VCSELPulsePeriodPCLK < MinFinalVcselPeriodPCLK ||
VCSELPulsePeriodPCLK > MaxFinalVcselPeriodPCLK)) {
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
/* Apply specific settings for the requested clock period */
if (Status != VL53L0X_ERROR_NONE)
return Status;
if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
/* Set phase check limits */
if (VCSELPulsePeriodPCLK == 12) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x18);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
} else if (VCSELPulsePeriodPCLK == 14) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x30);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
} else if (VCSELPulsePeriodPCLK == 16) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x40);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
} else if (VCSELPulsePeriodPCLK == 18) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
0x50);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
}
} else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
if (VCSELPulsePeriodPCLK == 8) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x10);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_LIM,
0x30);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
} else if (VCSELPulsePeriodPCLK == 10) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x28);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_LIM,
0x20);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
} else if (VCSELPulsePeriodPCLK == 12) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x38);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_LIM,
0x20);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
} else if (VCSELPulsePeriodPCLK == 14) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
0x048);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
0x08);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_ALGO_PHASECAL_LIM,
0x20);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
}
}
/* Re-calculate and apply timeouts, in macro periods */
if (Status == VL53L0X_ERROR_NONE) {
vcsel_period_reg = VL53L0X_encode_vcsel_period((uint8_t)
VCSELPulsePeriodPCLK);
/* When the VCSEL period for the pre or final range is changed,
* the corresponding timeout must be read from the device using
* the current VCSEL period, then the new VCSEL period can be
* applied. The timeout then must be written back to the device
* using the new VCSEL period.
*
* For the MSRC timeout, the same applies - this timeout being
* dependant on the pre-range vcsel period.
*/
switch (VcselPeriodType) {
case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE,
&PreRangeTimeoutMicroSeconds);
if (Status == VL53L0X_ERROR_NONE)
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_MSRC,
&MsrcTimeoutMicroSeconds);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
vcsel_period_reg);
if (Status == VL53L0X_ERROR_NONE)
Status = set_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE,
PreRangeTimeoutMicroSeconds);
if (Status == VL53L0X_ERROR_NONE)
Status = set_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_MSRC,
MsrcTimeoutMicroSeconds);
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
PreRangeVcselPulsePeriod,
VCSELPulsePeriodPCLK);
break;
case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&FinalRangeTimeoutMicroSeconds);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
vcsel_period_reg);
if (Status == VL53L0X_ERROR_NONE)
Status = set_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
FinalRangeTimeoutMicroSeconds);
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
FinalRangeVcselPulsePeriod,
VCSELPulsePeriodPCLK);
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
/* Finally, the timing budget must be re-applied */
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_GETPARAMETERFIELD(Dev,
MeasurementTimingBudgetMicroSeconds,
MeasurementTimingBudgetMicroSeconds);
Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
MeasurementTimingBudgetMicroSeconds);
}
/* Perform the phase calibration. This is needed after changing on
* vcsel period.
* get_data_enable = 0, restore_config = 1 */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_perform_phase_calibration(
Dev, &PhaseCalInt, 0, 1);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetVcselPulsePeriod(VL53L0X_DEV Dev,
VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_set_vcsel_pulse_period(Dev, VcselPeriodType,
VCSELPulsePeriodPCLK);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t vcsel_period_reg;
switch (VcselPeriodType) {
case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
&vcsel_period_reg);
break;
case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
&vcsel_period_reg);
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
if (Status == VL53L0X_ERROR_NONE)
*pVCSELPulsePeriodPCLK =
VL53L0X_decode_vcsel_period(vcsel_period_reg);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetVcselPulsePeriod(VL53L0X_DEV Dev,
VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_get_vcsel_pulse_period(Dev, VcselPeriodType,
pVCSELPulsePeriodPCLK);
LOG_FUNCTION_END(Status);
return Status;
}
uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout)
{
/*!
* Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
*/
uint32_t timeout_macro_clks = 0;
timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
<< (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1;
return timeout_macro_clks;
}
uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev, uint8_t vcsel_period_pclks)
{
uint64_t PLL_period_ps;
uint32_t macro_period_vclks;
uint32_t macro_period_ps;
LOG_FUNCTION_START("");
/* The above calculation will produce rounding errors,
therefore set fixed value
*/
PLL_period_ps = 1655;
macro_period_vclks = 2304;
macro_period_ps = (uint32_t)(macro_period_vclks
* vcsel_period_pclks * PLL_period_ps);
LOG_FUNCTION_END("");
return macro_period_ps;
}
/* To convert register value into us */
uint32_t VL53L0X::VL53L0X_calc_timeout_us(VL53L0X_DEV Dev,
uint16_t timeout_period_mclks,
uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ps;
uint32_t macro_period_ns;
uint32_t actual_timeout_period_us = 0;
macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
macro_period_ns = (macro_period_ps + 500) / 1000;
actual_timeout_period_us =
((timeout_period_mclks * macro_period_ns) + 500) / 1000;
return actual_timeout_period_us;
}
VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV Dev,
VL53L0X_SequenceStepId SequenceStepId,
uint32_t *pTimeOutMicroSecs)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t CurrentVCSELPulsePeriodPClk;
uint8_t EncodedTimeOutByte = 0;
uint32_t TimeoutMicroSeconds = 0;
uint16_t PreRangeEncodedTimeOut = 0;
uint16_t MsrcTimeOutMClks;
uint16_t PreRangeTimeOutMClks;
uint16_t FinalRangeTimeOutMClks = 0;
uint16_t FinalRangeEncodedTimeOut;
VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) ||
(SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) ||
(SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
&EncodedTimeOutByte);
}
MsrcTimeOutMClks = VL53L0X_decode_timeout(EncodedTimeOutByte);
TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
MsrcTimeOutMClks,
CurrentVCSELPulsePeriodPClk);
} else if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
/* Retrieve PRE-RANGE VCSEL Period */
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
if (Status == VL53L0X_ERROR_NONE) {
/* Retrieve PRE-RANGE VCSEL Period */
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
&PreRangeEncodedTimeOut);
}
PreRangeTimeOutMClks = VL53L0X_decode_timeout(
PreRangeEncodedTimeOut);
TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
PreRangeTimeOutMClks,
CurrentVCSELPulsePeriodPClk);
}
} else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
PreRangeTimeOutMClks = 0;
if (SchedulerSequenceSteps.PreRangeOn) {
/* Retrieve PRE-RANGE VCSEL Period */
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
/* Retrieve PRE-RANGE Timeout in Macro periods
* (MCLKS) */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
&PreRangeEncodedTimeOut);
PreRangeTimeOutMClks = VL53L0X_decode_timeout(
PreRangeEncodedTimeOut);
}
}
if (Status == VL53L0X_ERROR_NONE) {
/* Retrieve FINAL-RANGE VCSEL Period */
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
&CurrentVCSELPulsePeriodPClk);
}
/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
&FinalRangeEncodedTimeOut);
FinalRangeTimeOutMClks = VL53L0X_decode_timeout(
FinalRangeEncodedTimeOut);
}
FinalRangeTimeOutMClks -= PreRangeTimeOutMClks;
TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
FinalRangeTimeOutMClks,
CurrentVCSELPulsePeriodPClk);
}
*pTimeOutMicroSecs = TimeoutMicroSeconds;
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
uint32_t *pMeasurementTimingBudgetMicroSeconds)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
uint32_t FinalRangeTimeoutMicroSeconds;
uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
uint32_t StartOverheadMicroSeconds = 1910;
uint32_t EndOverheadMicroSeconds = 960;
uint32_t MsrcOverheadMicroSeconds = 660;
uint32_t TccOverheadMicroSeconds = 590;
uint32_t DssOverheadMicroSeconds = 690;
uint32_t PreRangeOverheadMicroSeconds = 660;
uint32_t FinalRangeOverheadMicroSeconds = 550;
uint32_t PreRangeTimeoutMicroSeconds = 0;
LOG_FUNCTION_START("");
/* Start and end overhead times always present */
*pMeasurementTimingBudgetMicroSeconds
= StartOverheadMicroSeconds + EndOverheadMicroSeconds;
Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
if (Status != VL53L0X_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
if (SchedulerSequenceSteps.TccOn ||
SchedulerSequenceSteps.MsrcOn ||
SchedulerSequenceSteps.DssOn) {
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_MSRC,
&MsrcDccTccTimeoutMicroSeconds);
if (Status == VL53L0X_ERROR_NONE) {
if (SchedulerSequenceSteps.TccOn) {
*pMeasurementTimingBudgetMicroSeconds +=
MsrcDccTccTimeoutMicroSeconds +
TccOverheadMicroSeconds;
}
if (SchedulerSequenceSteps.DssOn) {
*pMeasurementTimingBudgetMicroSeconds +=
2 * (MsrcDccTccTimeoutMicroSeconds +
DssOverheadMicroSeconds);
} else if (SchedulerSequenceSteps.MsrcOn) {
*pMeasurementTimingBudgetMicroSeconds +=
MsrcDccTccTimeoutMicroSeconds +
MsrcOverheadMicroSeconds;
}
}
}
if (Status == VL53L0X_ERROR_NONE) {
if (SchedulerSequenceSteps.PreRangeOn) {
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE,
&PreRangeTimeoutMicroSeconds);
*pMeasurementTimingBudgetMicroSeconds +=
PreRangeTimeoutMicroSeconds +
PreRangeOverheadMicroSeconds;
}
}
if (Status == VL53L0X_ERROR_NONE) {
if (SchedulerSequenceSteps.FinalRangeOn) {
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&FinalRangeTimeoutMicroSeconds);
*pMeasurementTimingBudgetMicroSeconds +=
(FinalRangeTimeoutMicroSeconds +
FinalRangeOverheadMicroSeconds);
}
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETPARAMETERFIELD(Dev,
MeasurementTimingBudgetMicroSeconds,
*pMeasurementTimingBudgetMicroSeconds);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev,
uint32_t *pMeasurementTimingBudgetMicroSeconds)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_get_measurement_timing_budget_micro_seconds(Dev,
pMeasurementTimingBudgetMicroSeconds);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetDeviceParameters(VL53L0X_DEV Dev,
VL53L0X_DeviceParameters_t *pDeviceParameters)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
int i;
LOG_FUNCTION_START("");
Status = VL53L0X_GetDeviceMode(Dev, &(pDeviceParameters->DeviceMode));
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetInterMeasurementPeriodMilliSeconds(Dev,
&(pDeviceParameters->InterMeasurementPeriodMilliSeconds));
if (Status == VL53L0X_ERROR_NONE)
pDeviceParameters->XTalkCompensationEnable = 0;
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetXTalkCompensationRateMegaCps(Dev,
&(pDeviceParameters->XTalkCompensationRateMegaCps));
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
&(pDeviceParameters->RangeOffsetMicroMeters));
if (Status == VL53L0X_ERROR_NONE) {
for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
/* get first the values, then the enables.
* VL53L0X_GetLimitCheckValue will modify the enable
* flags
*/
if (Status == VL53L0X_ERROR_NONE) {
Status |= VL53L0X_GetLimitCheckValue(Dev, i,
&(pDeviceParameters->LimitChecksValue[i]));
} else {
break;
}
if (Status == VL53L0X_ERROR_NONE) {
Status |= VL53L0X_GetLimitCheckEnable(Dev, i,
&(pDeviceParameters->LimitChecksEnable[i]));
} else {
break;
}
}
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetWrapAroundCheckEnable(Dev,
&(pDeviceParameters->WrapAroundCheckEnable));
}
/* Need to be done at the end as it uses VCSELPulsePeriod */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(Dev,
&(pDeviceParameters->MeasurementTimingBudgetMicroSeconds));
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId,
FixPoint1616_t LimitCheckValue)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t Temp8;
LOG_FUNCTION_START("");
VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId,
Temp8);
if (Temp8 == 0) { /* disabled write only internal value */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
LimitCheckId, LimitCheckValue);
} else {
switch (LimitCheckId) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
LimitCheckValue);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
Status = VL53L0X_WrWord(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
VL53L0X_FIXPOINT1616TOFIXPOINT97(
LimitCheckValue));
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
LimitCheckValue);
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
LimitCheckValue);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
Status = VL53L0X_WrWord(Dev,
VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
VL53L0X_FIXPOINT1616TOFIXPOINT97(
LimitCheckValue));
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
LimitCheckId, LimitCheckValue);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_DataInit(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceParameters_t CurrentParameters;
int i;
uint8_t StopVariable;
LOG_FUNCTION_START("");
/* by default the I2C is running at 1V8 if you want to change it you
* need to include this define at compilation level. */
#ifdef USE_I2C_2V8
Status = VL53L0X_UpdateByte(Dev,
VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
0xFE,
0x01);
#endif
/* Set I2C standard mode */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0x88, 0x00);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone, 0);
#ifdef USE_IQC_STATION
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_apply_offset_adjustment(Dev);
#endif
/* Default value is 1000 for Linearity Corrective Gain */
PALDevDataSet(Dev, LinearityCorrectiveGain, 1000);
/* Dmax default Parameter */
PALDevDataSet(Dev, DmaxCalRangeMilliMeter, 400);
PALDevDataSet(Dev, DmaxCalSignalRateRtnMegaCps,
(FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/
/* Set Default static parameters
*set first temporary values 9.44MHz * 65536 = 618660 */
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, OscFrequencyMHz, 618660);
/* Set Default XTalkCompensationRateMegaCps to 0 */
VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, 0);
/* Get default parameters */
Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters);
if (Status == VL53L0X_ERROR_NONE) {
/* initialize PAL values */
CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
PALDevDataSet(Dev, CurrentParameters, CurrentParameters);
}
/* Sigma estimator variable */
PALDevDataSet(Dev, SigmaEstRefArray, 100);
PALDevDataSet(Dev, SigmaEstEffPulseWidth, 900);
PALDevDataSet(Dev, SigmaEstEffAmbWidth, 500);
PALDevDataSet(Dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */
/* Use internal default settings */
PALDevDataSet(Dev, UseInternalTuningSettings, 1);
Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
Status |= VL53L0X_RdByte(Dev, 0x91, &StopVariable);
PALDevDataSet(Dev, StopVariable, StopVariable);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
/* Enable all check */
for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
if (Status == VL53L0X_ERROR_NONE)
Status |= VL53L0X_SetLimitCheckEnable(Dev, i, 1);
else
break;
}
/* Disable the following checks */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_SetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_SetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_SetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_SetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
/* Limit default values */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
(FixPoint1616_t)(18 * 65536));
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
(FixPoint1616_t)(25 * 65536 / 100));
/* 0.25 * 65536 */
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
(FixPoint1616_t)(35 * 65536));
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
(FixPoint1616_t)(0 * 65536));
}
if (Status == VL53L0X_ERROR_NONE) {
PALDevDataSet(Dev, SequenceConfig, 0xFF);
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
0xFF);
/* Set PAL state to tell that we are waiting for call to
* VL53L0X_StaticInit */
PALDevDataSet(Dev, PalState, VL53L0X_STATE_WAIT_STATICINIT);
}
if (Status == VL53L0X_ERROR_NONE)
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 0);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV Dev,
uint8_t *Revision,
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t ModuleIdInt;
char *ProductId_tmp;
LOG_FUNCTION_START("");
Status = VL53L0X_get_info_from_device(Dev, 2);
if (Status == VL53L0X_ERROR_NONE) {
ModuleIdInt = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ModuleId);
if (ModuleIdInt == 0) {
*Revision = 0;
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, "");
} else {
*Revision = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Revision);
ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
ProductId);
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, ProductId_tmp);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV Dev,
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t revision_id;
uint8_t Revision;
Status = VL53L0X_check_part_used(Dev, &Revision, pVL53L0X_DeviceInfo);
if (Status == VL53L0X_ERROR_NONE) {
if (Revision == 0) {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
} else if ((Revision <= 34) && (Revision != 32)) {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
} else if (Revision < 39) {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
} else {
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
}
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Type,
VL53L0X_STRING_DEVICE_INFO_TYPE);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
&pVL53L0X_DeviceInfo->ProductType);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_IDENTIFICATION_REVISION_ID,
&revision_id);
pVL53L0X_DeviceInfo->ProductRevisionMajor = 1;
pVL53L0X_DeviceInfo->ProductRevisionMinor =
(revision_id & 0xF0) >> 4;
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetDeviceInfo(VL53L0X_DEV Dev,
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_get_device_info(Dev, pVL53L0X_DeviceInfo);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetInterruptMaskStatus(VL53L0X_DEV Dev,
uint32_t *pInterruptMaskStatus)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t Byte;
LOG_FUNCTION_START("");
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte);
*pInterruptMaskStatus = Byte & 0x07;
if (Byte & 0x18)
Status = VL53L0X_ERROR_RANGE_ERROR;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementDataReady(VL53L0X_DEV Dev,
uint8_t *pMeasurementDataReady)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t SysRangeStatusRegister;
uint8_t InterruptConfig;
uint32_t InterruptMask;
LOG_FUNCTION_START("");
InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
Pin0GpioFunctionality);
if (InterruptConfig ==
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
Status = VL53L0X_GetInterruptMaskStatus(Dev, &InterruptMask);
if (InterruptMask ==
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY)
*pMeasurementDataReady = 1;
else
*pMeasurementDataReady = 0;
} else {
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_RANGE_STATUS,
&SysRangeStatusRegister);
if (Status == VL53L0X_ERROR_NONE) {
if (SysRangeStatusRegister & 0x01)
*pMeasurementDataReady = 1;
else
*pMeasurementDataReady = 0;
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_PollingDelay(VL53L0X_DEV Dev) {
VL53L0X_Error status = VL53L0X_ERROR_NONE;
// do nothing
VL53L0X_OsDelay();
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t NewDataReady = 0;
uint32_t LoopNb;
LOG_FUNCTION_START("");
LoopNb = 0;
do {
Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDataReady);
if (Status != 0)
break; /* the error is set */
if (NewDataReady == 1)
break; /* done note that status == 0 */
LoopNb++;
if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
Status = VL53L0X_ERROR_TIME_OUT;
break;
}
VL53L0X_PollingDelay(Dev);
} while (1);
LOG_FUNCTION_END(Status);
return Status;
}
/* Group PAL Interrupt Functions */
VL53L0X_Error VL53L0X::VL53L0X_ClearInterruptMask(VL53L0X_DEV Dev, uint32_t InterruptMask)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t LoopCount;
uint8_t Byte;
LOG_FUNCTION_START("");
/* clear bit 0 range interrupt, bit 1 error interrupt */
LoopCount = 0;
do {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
Status |= VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
Status |= VL53L0X_RdByte(Dev,
VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte);
LoopCount++;
} while (((Byte & 0x07) != 0x00)
&& (LoopCount < 3)
&& (Status == VL53L0X_ERROR_NONE));
if (LoopCount >= 3)
Status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(VL53L0X_DEV Dev,
uint8_t vhv_init_byte)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_START_STOP |
vhv_init_byte);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_measurement_poll_for_completion(Dev);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_ClearInterruptMask(Dev, 0);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x00);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(VL53L0X_DEV Dev, uint8_t read_not_write,
uint8_t VhvSettings, uint8_t PhaseCal,
uint8_t *pVhvSettings, uint8_t *pPhaseCal,
const uint8_t vhv_enable, const uint8_t phase_enable)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t PhaseCalint = 0;
/* Read VHV from device */
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
if (read_not_write) {
if (vhv_enable)
Status |= VL53L0X_RdByte(Dev, 0xCB, pVhvSettings);
if (phase_enable)
Status |= VL53L0X_RdByte(Dev, 0xEE, &PhaseCalint);
} else {
if (vhv_enable)
Status |= VL53L0X_WrByte(Dev, 0xCB, VhvSettings);
if (phase_enable)
Status |= VL53L0X_UpdateByte(Dev, 0xEE, 0x80, PhaseCal);
}
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
*pPhaseCal = (uint8_t)(PhaseCalint&0xEF);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev,
uint8_t *pVhvSettings, const uint8_t get_data_enable,
const uint8_t restore_config)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t SequenceConfig = 0;
uint8_t VhvSettings = 0;
uint8_t PhaseCal = 0;
uint8_t PhaseCalInt = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
if (restore_config)
SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
/* Run VHV */
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_perform_single_ref_calibration(Dev, 0x40);
/* Read VHV from device */
if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
Status = VL53L0X_ref_calibration_io(Dev, 1,
VhvSettings, PhaseCal, /* Not used here */
pVhvSettings, &PhaseCalInt,
1, 0);
} else
*pVhvSettings = 0;
if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
/* restore the previous Sequence Config */
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
SequenceConfig);
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
uint8_t *pPhaseCal, const uint8_t get_data_enable,
const uint8_t restore_config)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t SequenceConfig = 0;
uint8_t VhvSettings = 0;
uint8_t PhaseCal = 0;
uint8_t VhvSettingsint;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
if (restore_config)
SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
/* Run PhaseCal */
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_perform_single_ref_calibration(Dev, 0x0);
/* Read PhaseCal from device */
if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
Status = VL53L0X_ref_calibration_io(Dev, 1,
VhvSettings, PhaseCal, /* Not used here */
&VhvSettingsint, pPhaseCal,
0, 1);
} else
*pPhaseCal = 0;
if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
/* restore the previous Sequence Config */
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
SequenceConfig);
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t SequenceConfig = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
/* In the following function we don't save the config to optimize
* writes on device. Config is saved and restored only once. */
Status = VL53L0X_perform_vhv_calibration(
Dev, pVhvSettings, get_data_enable, 0);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_perform_phase_calibration(
Dev, pPhaseCal, get_data_enable, 0);
if (Status == VL53L0X_ERROR_NONE) {
/* restore the previous Sequence Config */
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
SequenceConfig);
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
}
return Status;
}
void VL53L0X::get_next_good_spad(uint8_t goodSpadArray[], uint32_t size,
uint32_t curr, int32_t *next)
{
uint32_t startIndex;
uint32_t fineOffset;
uint32_t cSpadsPerByte = 8;
uint32_t coarseIndex;
uint32_t fineIndex;
uint8_t dataByte;
uint8_t success = 0;
/*
* Starting with the current good spad, loop through the array to find
* the next. i.e. the next bit set in the sequence.
*
* The coarse index is the byte index of the array and the fine index is
* the index of the bit within each byte.
*/
*next = -1;
startIndex = curr / cSpadsPerByte;
fineOffset = curr % cSpadsPerByte;
for (coarseIndex = startIndex; ((coarseIndex < size) && !success);
coarseIndex++) {
fineIndex = 0;
dataByte = goodSpadArray[coarseIndex];
if (coarseIndex == startIndex) {
/* locate the bit position of the provided current
* spad bit before iterating */
dataByte >>= fineOffset;
fineIndex = fineOffset;
}
while (fineIndex < cSpadsPerByte) {
if ((dataByte & 0x1) == 1) {
success = 1;
*next = coarseIndex * cSpadsPerByte + fineIndex;
break;
}
dataByte >>= 1;
fineIndex++;
}
}
}
uint8_t VL53L0X::is_aperture(uint32_t spadIndex)
{
/*
* This function reports if a given spad index is an aperture SPAD by
* deriving the quadrant.
*/
uint32_t quadrant;
uint8_t isAperture = 1;
quadrant = spadIndex >> 6;
if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0)
isAperture = 0;
return isAperture;
}
VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spadArray[], uint32_t size,
uint32_t spadIndex)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t cSpadsPerByte = 8;
uint32_t coarseIndex;
uint32_t fineIndex;
coarseIndex = spadIndex / cSpadsPerByte;
fineIndex = spadIndex % cSpadsPerByte;
if (coarseIndex >= size)
status = VL53L0X_ERROR_REF_SPAD_INIT;
else
spadArray[coarseIndex] |= (1 << fineIndex);
return status;
}
VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
{
VL53L0X_Error status = VL53L0X_WriteMulti(Dev,
VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
refSpadArray, 6);
return status;
}
VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
{
VL53L0X_Error status = VL53L0X_ReadMulti(Dev,
VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
refSpadArray,
6);
// VL53L0X_Error status = VL53L0X_ERROR_NONE;
// uint8_t count=0;
// for (count = 0; count < 6; count++)
// status = VL53L0X_RdByte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]);
return status;
}
VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV Dev,
uint8_t apertureSpads,
uint8_t goodSpadArray[],
uint8_t spadArray[],
uint32_t size,
uint32_t start,
uint32_t offset,
uint32_t spadCount,
uint32_t *lastSpad)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t index;
uint32_t i;
int32_t nextGoodSpad = offset;
uint32_t currentSpad;
uint8_t checkSpadArray[6];
/*
* This function takes in a spad array which may or may not have SPADS
* already enabled and appends from a given offset a requested number
* of new SPAD enables. The 'good spad map' is applied to
* determine the next SPADs to enable.
*
* This function applies to only aperture or only non-aperture spads.
* Checks are performed to ensure this.
*/
currentSpad = offset;
for (index = 0; index < spadCount; index++) {
get_next_good_spad(goodSpadArray, size, currentSpad,
&nextGoodSpad);
if (nextGoodSpad == -1) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
/* Confirm that the next good SPAD is non-aperture */
if (is_aperture(start + nextGoodSpad) != apertureSpads) {
/* if we can't get the required number of good aperture
* spads from the current quadrant then this is an error
*/
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
currentSpad = (uint32_t)nextGoodSpad;
enable_spad_bit(spadArray, size, currentSpad);
currentSpad++;
}
*lastSpad = currentSpad;
if (status == VL53L0X_ERROR_NONE)
status = set_ref_spad_map(Dev, spadArray);
if (status == VL53L0X_ERROR_NONE) {
status = get_ref_spad_map(Dev, checkSpadArray);
i = 0;
/* Compare spad maps. If not equal report error. */
while (i < size) {
if (spadArray[i] != checkSpadArray[i]) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
i++;
}
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetDeviceMode(VL53L0X_DEV Dev, VL53L0X_DeviceModes DeviceMode)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("%d", (int)DeviceMode);
switch (DeviceMode) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
case VL53L0X_DEVICEMODE_GPIO_DRIVE:
case VL53L0X_DEVICEMODE_GPIO_OSC:
/* Supported modes */
VL53L0X_SETPARAMETERFIELD(Dev, DeviceMode, DeviceMode);
break;
default:
/* Unsupported mode */
Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetInterruptThresholds(VL53L0X_DEV Dev,
VL53L0X_DeviceModes DeviceMode, FixPoint1616_t *pThresholdLow,
FixPoint1616_t *pThresholdHigh)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint16_t Threshold16;
LOG_FUNCTION_START("");
/* no dependency on DeviceMode for Ewok */
Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &Threshold16);
/* Need to multiply by 2 because the FW will apply a x2 */
*pThresholdLow = (FixPoint1616_t)((0x00fff & Threshold16) << 17);
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
&Threshold16);
/* Need to multiply by 2 because the FW will apply a x2 */
*pThresholdHigh =
(FixPoint1616_t)((0x00fff & Threshold16) << 17);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
uint8_t *pTuningSettingBuffer)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
int i;
int Index;
uint8_t msb;
uint8_t lsb;
uint8_t SelectParam;
uint8_t NumberOfWrites;
uint8_t Address;
uint8_t localBuffer[4]; /* max */
uint16_t Temp16;
LOG_FUNCTION_START("");
Index = 0;
while ((*(pTuningSettingBuffer + Index) != 0) &&
(Status == VL53L0X_ERROR_NONE)) {
NumberOfWrites = *(pTuningSettingBuffer + Index);
Index++;
if (NumberOfWrites == 0xFF) {
/* internal parameters */
SelectParam = *(pTuningSettingBuffer + Index);
Index++;
switch (SelectParam) {
case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
msb = *(pTuningSettingBuffer + Index);
Index++;
lsb = *(pTuningSettingBuffer + Index);
Index++;
Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
PALDevDataSet(Dev, SigmaEstRefArray, Temp16);
break;
case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
msb = *(pTuningSettingBuffer + Index);
Index++;
lsb = *(pTuningSettingBuffer + Index);
Index++;
Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
PALDevDataSet(Dev, SigmaEstEffPulseWidth,
Temp16);
break;
case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
msb = *(pTuningSettingBuffer + Index);
Index++;
lsb = *(pTuningSettingBuffer + Index);
Index++;
Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
PALDevDataSet(Dev, SigmaEstEffAmbWidth, Temp16);
break;
case 3: /* uint16_t targetRefRate -> 2 bytes */
msb = *(pTuningSettingBuffer + Index);
Index++;
lsb = *(pTuningSettingBuffer + Index);
Index++;
Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
PALDevDataSet(Dev, targetRefRate, Temp16);
break;
default: /* invalid parameter */
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
} else if (NumberOfWrites <= 4) {
Address = *(pTuningSettingBuffer + Index);
Index++;
for (i = 0; i < NumberOfWrites; i++) {
localBuffer[i] = *(pTuningSettingBuffer +
Index);
Index++;
}
Status = VL53L0X_WriteMulti(Dev, Address, localBuffer,
NumberOfWrites);
} else {
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_CheckAndLoadInterruptSettings(VL53L0X_DEV Dev,
uint8_t StartNotStopFlag)
{
uint8_t InterruptConfig;
FixPoint1616_t ThresholdLow;
FixPoint1616_t ThresholdHigh;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
Pin0GpioFunctionality);
if ((InterruptConfig ==
VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) ||
(InterruptConfig ==
VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) ||
(InterruptConfig ==
VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
Status = VL53L0X_GetInterruptThresholds(Dev,
VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
&ThresholdLow, &ThresholdHigh);
if (((ThresholdLow > 255*65536) ||
(ThresholdHigh > 255*65536)) &&
(Status == VL53L0X_ERROR_NONE)) {
if (StartNotStopFlag != 0) {
Status = VL53L0X_load_tuning_settings(Dev,
InterruptThresholdSettings);
} else {
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x04);
Status |= VL53L0X_WrByte(Dev, 0x70, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
}
}
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_StartMeasurement(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceModes DeviceMode;
uint8_t Byte;
uint8_t StartStopByte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
uint32_t LoopNb;
LOG_FUNCTION_START("");
/* Get Current DeviceMode */
VL53L0X_GetDeviceMode(Dev, &DeviceMode);
Status = VL53L0X_WrByte(Dev, 0x80, 0x01);
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
Status = VL53L0X_WrByte(Dev, 0x91, PALDevDataGet(Dev, StopVariable));
Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
Status = VL53L0X_WrByte(Dev, 0x80, 0x00);
switch (DeviceMode) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x01);
Byte = StartStopByte;
if (Status == VL53L0X_ERROR_NONE) {
/* Wait until start bit has been cleared */
LoopNb = 0;
do {
if (LoopNb > 0)
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_SYSRANGE_START, &Byte);
LoopNb = LoopNb + 1;
} while (((Byte & StartStopByte) == StartStopByte)
&& (Status == VL53L0X_ERROR_NONE)
&& (LoopNb < VL53L0X_DEFAULT_MAX_LOOP));
if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
Status = VL53L0X_ERROR_TIME_OUT;
}
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
/* Back-to-back mode */
/* Check if need to apply interrupt settings */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
if (Status == VL53L0X_ERROR_NONE) {
/* Set PAL State to Running */
PALDevDataSet(Dev, PalState, VL53L0X_STATE_RUNNING);
}
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
/* Continuous mode */
/* Check if need to apply interrupt settings */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_TIMED);
if (Status == VL53L0X_ERROR_NONE) {
/* Set PAL State to Running */
PALDevDataSet(Dev, PalState, VL53L0X_STATE_RUNNING);
}
break;
default:
/* Selected mode not supported */
Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
LOG_FUNCTION_END(Status);
return Status;
}
/* Group PAL Measurement Functions */
VL53L0X_Error VL53L0X::VL53L0X_PerformSingleMeasurement(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceModes DeviceMode;
LOG_FUNCTION_START("");
/* Get Current DeviceMode */
Status = VL53L0X_GetDeviceMode(Dev, &DeviceMode);
/* Start immediately to run a single ranging measurement in case of
* single ranging or single histogram */
if (Status == VL53L0X_ERROR_NONE
&& DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
Status = VL53L0X_StartMeasurement(Dev);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_measurement_poll_for_completion(Dev);
/* Change PAL State in case of single ranging or single histogram */
if (Status == VL53L0X_ERROR_NONE
&& DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationEnable(VL53L0X_DEV Dev,
uint8_t *pXTalkCompensationEnable)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t Temp8;
LOG_FUNCTION_START("");
VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable, Temp8);
*pXTalkCompensationEnable = Temp8;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *ptotal_xtalk_rate_mcps)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t xtalkCompEnable;
FixPoint1616_t totalXtalkMegaCps;
FixPoint1616_t xtalkPerSpadMegaCps;
*ptotal_xtalk_rate_mcps = 0;
Status = VL53L0X_GetXTalkCompensationEnable(Dev, &xtalkCompEnable);
if (Status == VL53L0X_ERROR_NONE) {
if (xtalkCompEnable) {
VL53L0X_GETPARAMETERFIELD(
Dev,
XTalkCompensationRateMegaCps,
xtalkPerSpadMegaCps);
/* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
totalXtalkMegaCps =
pRangingMeasurementData->EffectiveSpadRtnCount *
xtalkPerSpadMegaCps;
/* FixPoint0824 >> 8 = FixPoint1616 */
*ptotal_xtalk_rate_mcps =
(totalXtalkMegaCps + 0x80) >> 8;
}
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *ptotal_signal_rate_mcps)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
FixPoint1616_t totalXtalkMegaCps;
LOG_FUNCTION_START("");
*ptotal_signal_rate_mcps =
pRangingMeasurementData->SignalRateRtnMegaCps;
Status = VL53L0X_get_total_xtalk_rate(
Dev, pRangingMeasurementData, &totalXtalkMegaCps);
if (Status == VL53L0X_ERROR_NONE)
*ptotal_signal_rate_mcps += totalXtalkMegaCps;
return Status;
}
/* To convert ms into register value */
uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
uint32_t timeout_period_us,
uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ps;
uint32_t macro_period_ns;
uint32_t timeout_period_mclks = 0;
macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
macro_period_ns = (macro_period_ps + 500) / 1000;
timeout_period_mclks =
(uint32_t) (((timeout_period_us * 1000)
+ (macro_period_ns / 2)) / macro_period_ns);
return timeout_period_mclks;
}
uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
{
/*
* Implements an integer square root
*
* From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
*/
uint32_t res = 0;
uint32_t bit = 1 << 30;
/* The second-to-top bit is set:
* 1 << 14 for 16-bits, 1 << 30 for 32 bits */
/* "bit" starts at the highest power of four <= the argument. */
while (bit > num)
bit >>= 2;
while (bit != 0) {
if (num >= res + bit) {
num -= res + bit;
res = (res >> 1) + bit;
} else
res >>= 1;
bit >>= 2;
}
return res;
}
VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(
VL53L0X_DEV Dev,
FixPoint1616_t totalSignalRate_mcps,
FixPoint1616_t totalCorrSignalRate_mcps,
FixPoint1616_t pwMult,
uint32_t sigmaEstimateP1,
FixPoint1616_t sigmaEstimateP2,
uint32_t peakVcselDuration_us,
uint32_t *pdmax_mm)
{
const uint32_t cSigmaLimit = 18;
const FixPoint1616_t cSignalLimit = 0x4000; /* 0.25 */
const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
const uint32_t cAmbEffWidthSigmaEst_ns = 6;
const uint32_t cAmbEffWidthDMax_ns = 7;
uint32_t dmaxCalRange_mm;
FixPoint1616_t dmaxCalSignalRateRtn_mcps;
FixPoint1616_t minSignalNeeded;
FixPoint1616_t minSignalNeeded_p1;
FixPoint1616_t minSignalNeeded_p2;
FixPoint1616_t minSignalNeeded_p3;
FixPoint1616_t minSignalNeeded_p4;
FixPoint1616_t sigmaLimitTmp;
FixPoint1616_t sigmaEstSqTmp;
FixPoint1616_t signalLimitTmp;
FixPoint1616_t SignalAt0mm;
FixPoint1616_t dmaxDark;
FixPoint1616_t dmaxAmbient;
FixPoint1616_t dmaxDarkTmp;
FixPoint1616_t sigmaEstP2Tmp;
uint32_t signalRateTemp_mcps;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
dmaxCalRange_mm =
PALDevDataGet(Dev, DmaxCalRangeMilliMeter);
dmaxCalSignalRateRtn_mcps =
PALDevDataGet(Dev, DmaxCalSignalRateRtnMegaCps);
/* uint32 * FixPoint1616 = FixPoint1616 */
SignalAt0mm = dmaxCalRange_mm * dmaxCalSignalRateRtn_mcps;
/* FixPoint1616 >> 8 = FixPoint2408 */
SignalAt0mm = (SignalAt0mm + 0x80) >> 8;
SignalAt0mm *= dmaxCalRange_mm;
minSignalNeeded_p1 = 0;
if (totalCorrSignalRate_mcps > 0) {
/* Shift by 10 bits to increase resolution prior to the
* division */
signalRateTemp_mcps = totalSignalRate_mcps << 10;
/* Add rounding value prior to division */
minSignalNeeded_p1 = signalRateTemp_mcps +
(totalCorrSignalRate_mcps/2);
/* FixPoint0626/FixPoint1616 = FixPoint2210 */
minSignalNeeded_p1 /= totalCorrSignalRate_mcps;
/* Apply a factored version of the speed of light.
Correction to be applied at the end */
minSignalNeeded_p1 *= 3;
/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
minSignalNeeded_p1 *= minSignalNeeded_p1;
/* FixPoint1220 >> 16 = FixPoint2804 */
minSignalNeeded_p1 = (minSignalNeeded_p1 + 0x8000) >> 16;
}
minSignalNeeded_p2 = pwMult * sigmaEstimateP1;
/* FixPoint1616 >> 16 = uint32 */
minSignalNeeded_p2 = (minSignalNeeded_p2 + 0x8000) >> 16;
/* uint32 * uint32 = uint32 */
minSignalNeeded_p2 *= minSignalNeeded_p2;
/* Check sigmaEstimateP2
* If this value is too high there is not enough signal rate
* to calculate dmax value so set a suitable value to ensure
* a very small dmax.
*/
sigmaEstP2Tmp = (sigmaEstimateP2 + 0x8000) >> 16;
sigmaEstP2Tmp = (sigmaEstP2Tmp + cAmbEffWidthSigmaEst_ns/2)/
cAmbEffWidthSigmaEst_ns;
sigmaEstP2Tmp *= cAmbEffWidthDMax_ns;
if (sigmaEstP2Tmp > 0xffff) {
minSignalNeeded_p3 = 0xfff00000;
} else {
/* DMAX uses a different ambient width from sigma, so apply
* correction.
* Perform division before multiplication to prevent overflow.
*/
sigmaEstimateP2 = (sigmaEstimateP2 + cAmbEffWidthSigmaEst_ns/2)/
cAmbEffWidthSigmaEst_ns;
sigmaEstimateP2 *= cAmbEffWidthDMax_ns;
/* FixPoint1616 >> 16 = uint32 */
minSignalNeeded_p3 = (sigmaEstimateP2 + 0x8000) >> 16;
minSignalNeeded_p3 *= minSignalNeeded_p3;
}
/* FixPoint1814 / uint32 = FixPoint1814 */
sigmaLimitTmp = ((cSigmaLimit << 14) + 500) / 1000;
/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
sigmaLimitTmp *= sigmaLimitTmp;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sigmaEstSqTmp = cSigmaEstRef * cSigmaEstRef;
/* FixPoint3232 >> 4 = FixPoint0428 */
sigmaEstSqTmp = (sigmaEstSqTmp + 0x08) >> 4;
/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
sigmaLimitTmp -= sigmaEstSqTmp;
/* uint32_t * FixPoint0428 = FixPoint0428 */
minSignalNeeded_p4 = 4 * 12 * sigmaLimitTmp;
/* FixPoint0428 >> 14 = FixPoint1814 */
minSignalNeeded_p4 = (minSignalNeeded_p4 + 0x2000) >> 14;
/* uint32 + uint32 = uint32 */
minSignalNeeded = (minSignalNeeded_p2 + minSignalNeeded_p3);
/* uint32 / uint32 = uint32 */
minSignalNeeded += (peakVcselDuration_us/2);
minSignalNeeded /= peakVcselDuration_us;
/* uint32 << 14 = FixPoint1814 */
minSignalNeeded <<= 14;
/* FixPoint1814 / FixPoint1814 = uint32 */
minSignalNeeded += (minSignalNeeded_p4/2);
minSignalNeeded /= minSignalNeeded_p4;
/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
minSignalNeeded *= minSignalNeeded_p1;
/* Apply correction by dividing by 1000000.
* This assumes 10E16 on the numerator of the equation
* and 10E-22 on the denominator.
* We do this because 32bit fix point calculation can't
* handle the larger and smaller elements of this equation,
* i.e. speed of light and pulse widths.
*/
minSignalNeeded = (minSignalNeeded + 500) / 1000;
minSignalNeeded <<= 4;
minSignalNeeded = (minSignalNeeded + 500) / 1000;
/* FixPoint1616 >> 8 = FixPoint2408 */
signalLimitTmp = (cSignalLimit + 0x80) >> 8;
/* FixPoint2408/FixPoint2408 = uint32 */
if (signalLimitTmp != 0)
dmaxDarkTmp = (SignalAt0mm + (signalLimitTmp / 2))
/ signalLimitTmp;
else
dmaxDarkTmp = 0;
dmaxDark = VL53L0X_isqrt(dmaxDarkTmp);
/* FixPoint2408/FixPoint2408 = uint32 */
if (minSignalNeeded != 0)
dmaxAmbient = (SignalAt0mm + minSignalNeeded/2)
/ minSignalNeeded;
else
dmaxAmbient = 0;
dmaxAmbient = VL53L0X_isqrt(dmaxAmbient);
*pdmax_mm = dmaxDark;
if (dmaxDark > dmaxAmbient)
*pdmax_mm = dmaxAmbient;
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *pSigmaEstimate,
uint32_t *pDmax_mm)
{
/* Expressed in 100ths of a ns, i.e. centi-ns */
const uint32_t cPulseEffectiveWidth_centi_ns = 800;
/* Expressed in 100ths of a ns, i.e. centi-ns */
const uint32_t cAmbientEffectiveWidth_centi_ns = 600;
const FixPoint1616_t cDfltFinalRangeIntegrationTimeMilliSecs = 0x00190000; /* 25ms */
const uint32_t cVcselPulseWidth_ps = 4700; /* pico secs */
const FixPoint1616_t cSigmaEstMax = 0x028F87AE;
const FixPoint1616_t cSigmaEstRtnMax = 0xF000;
const FixPoint1616_t cAmbToSignalRatioMax = 0xF0000000/
cAmbientEffectiveWidth_centi_ns;
/* Time Of Flight per mm (6.6 pico secs) */
const FixPoint1616_t cTOF_per_mm_ps = 0x0006999A;
const uint32_t c16BitRoundingParam = 0x00008000;
const FixPoint1616_t cMaxXTalk_kcps = 0x00320000;
const uint32_t cPllPeriod_ps = 1655;
uint32_t vcselTotalEventsRtn;
uint32_t finalRangeTimeoutMicroSecs;
uint32_t preRangeTimeoutMicroSecs;
uint32_t finalRangeIntegrationTimeMilliSecs;
FixPoint1616_t sigmaEstimateP1;
FixPoint1616_t sigmaEstimateP2;
FixPoint1616_t sigmaEstimateP3;
FixPoint1616_t deltaT_ps;
FixPoint1616_t pwMult;
FixPoint1616_t sigmaEstRtn;
FixPoint1616_t sigmaEstimate;
FixPoint1616_t xTalkCorrection;
FixPoint1616_t ambientRate_kcps;
FixPoint1616_t peakSignalRate_kcps;
FixPoint1616_t xTalkCompRate_mcps;
uint32_t xTalkCompRate_kcps;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
FixPoint1616_t diff1_mcps;
FixPoint1616_t diff2_mcps;
FixPoint1616_t sqr1;
FixPoint1616_t sqr2;
FixPoint1616_t sqrSum;
FixPoint1616_t sqrtResult_centi_ns;
FixPoint1616_t sqrtResult;
FixPoint1616_t totalSignalRate_mcps;
FixPoint1616_t correctedSignalRate_mcps;
FixPoint1616_t sigmaEstRef;
uint32_t vcselWidth;
uint32_t finalRangeMacroPCLKS;
uint32_t preRangeMacroPCLKS;
uint32_t peakVcselDuration_us;
uint8_t finalRangeVcselPCLKS;
uint8_t preRangeVcselPCLKS;
/*! \addtogroup calc_sigma_estimate
* @{
*
* Estimates the range sigma
*/
LOG_FUNCTION_START("");
VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
xTalkCompRate_mcps);
/*
* We work in kcps rather than mcps as this helps keep within the
* confines of the 32 Fix1616 type.
*/
ambientRate_kcps =
(pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16;
correctedSignalRate_mcps =
pRangingMeasurementData->SignalRateRtnMegaCps;
Status = VL53L0X_get_total_signal_rate(
Dev, pRangingMeasurementData, &totalSignalRate_mcps);
Status = VL53L0X_get_total_xtalk_rate(
Dev, pRangingMeasurementData, &xTalkCompRate_mcps);
/* Signal rate measurement provided by device is the
* peak signal rate, not average.
*/
peakSignalRate_kcps = (totalSignalRate_mcps * 1000);
peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16;
xTalkCompRate_kcps = xTalkCompRate_mcps * 1000;
if (xTalkCompRate_kcps > cMaxXTalk_kcps)
xTalkCompRate_kcps = cMaxXTalk_kcps;
if (Status == VL53L0X_ERROR_NONE) {
/* Calculate final range macro periods */
finalRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, FinalRangeTimeoutMicroSecs);
finalRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, FinalRangeVcselPulsePeriod);
finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS);
/* Calculate pre-range macro periods */
preRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, PreRangeTimeoutMicroSecs);
preRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, PreRangeVcselPulsePeriod);
preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS);
vcselWidth = 3;
if (finalRangeVcselPCLKS == 8)
vcselWidth = 2;
peakVcselDuration_us = vcselWidth * 2048 *
(preRangeMacroPCLKS + finalRangeMacroPCLKS);
peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
peakVcselDuration_us *= cPllPeriod_ps;
peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
/* Fix1616 >> 8 = Fix2408 */
totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8;
/* Fix2408 * uint32 = Fix2408 */
vcselTotalEventsRtn = totalSignalRate_mcps *
peakVcselDuration_us;
/* Fix2408 >> 8 = uint32 */
vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8;
/* Fix2408 << 8 = Fix1616 = */
totalSignalRate_mcps <<= 8;
}
if (Status != VL53L0X_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
if (peakSignalRate_kcps == 0) {
*pSigmaEstimate = cSigmaEstMax;
PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax);
*pDmax_mm = 0;
} else {
if (vcselTotalEventsRtn < 1)
vcselTotalEventsRtn = 1;
sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns;
/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sigmaEstimateP2 = (ambientRate_kcps << 16)/peakSignalRate_kcps;
if (sigmaEstimateP2 > cAmbToSignalRatioMax) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigmaEstimateP2 = cAmbToSignalRatioMax;
}
sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns;
sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12);
/* uint32 * FixPoint1616 = FixPoint1616 */
deltaT_ps = pRangingMeasurementData->RangeMilliMeter *
cTOF_per_mm_ps;
/*
* vcselRate - xtalkCompRate
* (uint32 << 16) - FixPoint1616 = FixPoint1616.
* Divide result by 1000 to convert to mcps.
* 500 is added to ensure rounding when integer division
* truncates.
*/
diff1_mcps = (((peakSignalRate_kcps << 16) -
2 * xTalkCompRate_kcps) + 500)/1000;
/* vcselRate + xtalkCompRate */
diff2_mcps = ((peakSignalRate_kcps << 16) + 500)/1000;
/* Shift by 8 bits to increase resolution prior to the
* division */
diff1_mcps <<= 8;
/* FixPoint0824/FixPoint1616 = FixPoint2408 */
// xTalkCorrection = abs(diff1_mcps/diff2_mcps);
// abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
xTalkCorrection = diff1_mcps/diff2_mcps;
/* FixPoint2408 << 8 = FixPoint1616 */
xTalkCorrection <<= 8;
if(pRangingMeasurementData->RangeStatus != 0){
pwMult = 1 << 16;
} else {
/* FixPoint1616/uint32 = FixPoint1616 */
pwMult = deltaT_ps/cVcselPulseWidth_ps; /* smaller than 1.0f */
/*
* FixPoint1616 * FixPoint1616 = FixPoint3232, however both
* values are small enough such that32 bits will not be
* exceeded.
*/
pwMult *= ((1 << 16) - xTalkCorrection);
/* (FixPoint3232 >> 16) = FixPoint1616 */
pwMult = (pwMult + c16BitRoundingParam) >> 16;
/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
pwMult += (1 << 16);
/*
* At this point the value will be 1.xx, therefore if we square
* the value this will exceed 32 bits. To address this perform
* a single shift to the right before the multiplication.
*/
pwMult >>= 1;
/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
pwMult = pwMult * pwMult;
/* (FixPoint3430 >> 14) = Fix1616 */
pwMult >>= 14;
}
/* FixPoint1616 * uint32 = FixPoint1616 */
sqr1 = pwMult * sigmaEstimateP1;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr1 = (sqr1 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr1 *= sqr1;
sqr2 = sigmaEstimateP2;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr2 = (sqr2 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr2 *= sqr2;
/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
sqrSum = sqr1 + sqr2;
/* SQRT(FixPoin6400) = FixPoint3200 */
sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum);
/* (FixPoint3200 << 16) = FixPoint1616 */
sqrtResult_centi_ns <<= 16;
/*
* Note that the Speed Of Light is expressed in um per 1E-10
* seconds (2997) Therefore to get mm/ns we have to divide by
* 10000
*/
sigmaEstRtn = (((sqrtResult_centi_ns+50)/100) /
sigmaEstimateP3);
sigmaEstRtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
/* Add 5000 before dividing by 10000 to ensure rounding. */
sigmaEstRtn += 5000;
sigmaEstRtn /= 10000;
if (sigmaEstRtn > cSigmaEstRtnMax) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigmaEstRtn = cSigmaEstRtnMax;
}
finalRangeIntegrationTimeMilliSecs =
(finalRangeTimeoutMicroSecs + preRangeTimeoutMicroSecs + 500)/1000;
/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
* sqrt(FixPoint1616/int) = FixPoint2408)
*/
sigmaEstRef =
VL53L0X_isqrt((cDfltFinalRangeIntegrationTimeMilliSecs +
finalRangeIntegrationTimeMilliSecs/2)/
finalRangeIntegrationTimeMilliSecs);
/* FixPoint2408 << 8 = FixPoint1616 */
sigmaEstRef <<= 8;
sigmaEstRef = (sigmaEstRef + 500)/1000;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr1 = sigmaEstRtn * sigmaEstRtn;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr2 = sigmaEstRef * sigmaEstRef;
/* sqrt(FixPoint3232) = FixPoint1616 */
sqrtResult = VL53L0X_isqrt((sqr1 + sqr2));
/*
* Note that the Shift by 4 bits increases resolution prior to
* the sqrt, therefore the result must be shifted by 2 bits to
* the right to revert back to the FixPoint1616 format.
*/
sigmaEstimate = 1000 * sqrtResult;
if ((peakSignalRate_kcps < 1) || (vcselTotalEventsRtn < 1) ||
(sigmaEstimate > cSigmaEstMax)) {
sigmaEstimate = cSigmaEstMax;
}
*pSigmaEstimate = (uint32_t)(sigmaEstimate);
PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate);
Status = VL53L0X_calc_dmax(
Dev,
totalSignalRate_mcps,
correctedSignalRate_mcps,
pwMult,
sigmaEstimateP1,
sigmaEstimateP2,
peakVcselDuration_us,
pDmax_mm);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
uint8_t DeviceRangeStatus,
FixPoint1616_t SignalRate,
uint16_t EffectiveSpadRtnCount,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
uint8_t *pPalRangeStatus)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t NoneFlag;
uint8_t SigmaLimitflag = 0;
uint8_t SignalRefClipflag = 0;
uint8_t RangeIgnoreThresholdflag = 0;
uint8_t SigmaLimitCheckEnable = 0;
uint8_t SignalRateFinalRangeLimitCheckEnable = 0;
uint8_t SignalRefClipLimitCheckEnable = 0;
uint8_t RangeIgnoreThresholdLimitCheckEnable = 0;
FixPoint1616_t SigmaEstimate;
FixPoint1616_t SigmaLimitValue;
FixPoint1616_t SignalRefClipValue;
FixPoint1616_t RangeIgnoreThresholdValue;
FixPoint1616_t SignalRatePerSpad;
uint8_t DeviceRangeStatusInternal = 0;
uint16_t tmpWord = 0;
uint8_t Temp8;
uint32_t Dmax_mm = 0;
FixPoint1616_t LastSignalRefMcps;
LOG_FUNCTION_START("");
/*
* VL53L0X has a good ranging when the value of the
* DeviceRangeStatus = 11. This function will replace the value 0 with
* the value 11 in the DeviceRangeStatus.
* In addition, the SigmaEstimator is not included in the VL53L0X
* DeviceRangeStatus, this will be added in the PalRangeStatus.
*/
DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3);
if (DeviceRangeStatusInternal == 0 ||
DeviceRangeStatusInternal == 5 ||
DeviceRangeStatusInternal == 7 ||
DeviceRangeStatusInternal == 12 ||
DeviceRangeStatusInternal == 13 ||
DeviceRangeStatusInternal == 14 ||
DeviceRangeStatusInternal == 15
) {
NoneFlag = 1;
} else {
NoneFlag = 0;
}
/*
* Check if Sigma limit is enabled, if yes then do comparison with limit
* value and put the result back into pPalRangeStatus.
*/
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
&SigmaLimitCheckEnable);
if ((SigmaLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) {
/*
* compute the Sigma and check with limit
*/
Status = VL53L0X_calc_sigma_estimate(
Dev,
pRangingMeasurementData,
&SigmaEstimate,
&Dmax_mm);
if (Status == VL53L0X_ERROR_NONE)
pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm;
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
&SigmaLimitValue);
if ((SigmaLimitValue > 0) &&
(SigmaEstimate > SigmaLimitValue))
/* Limit Fail */
SigmaLimitflag = 1;
}
}
/*
* Check if Signal ref clip limit is enabled, if yes then do comparison
* with limit value and put the result back into pPalRangeStatus.
*/
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
&SignalRefClipLimitCheckEnable);
if ((SignalRefClipLimitCheckEnable != 0) &&
(Status == VL53L0X_ERROR_NONE)) {
Status = VL53L0X_GetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
&SignalRefClipValue);
/* Read LastSignalRefMcps from device */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
&tmpWord);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord);
PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps);
if ((SignalRefClipValue > 0) &&
(LastSignalRefMcps > SignalRefClipValue)) {
/* Limit Fail */
SignalRefClipflag = 1;
}
}
/*
* Check if Signal ref clip limit is enabled, if yes then do comparison
* with limit value and put the result back into pPalRangeStatus.
* EffectiveSpadRtnCount has a format 8.8
* If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
*/
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
&RangeIgnoreThresholdLimitCheckEnable);
if ((RangeIgnoreThresholdLimitCheckEnable != 0) &&
(Status == VL53L0X_ERROR_NONE)) {
/* Compute the signal rate per spad */
if (EffectiveSpadRtnCount == 0) {
SignalRatePerSpad = 0;
} else {
SignalRatePerSpad = (FixPoint1616_t)((256 * SignalRate)
/ EffectiveSpadRtnCount);
}
Status = VL53L0X_GetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
&RangeIgnoreThresholdValue);
if ((RangeIgnoreThresholdValue > 0) &&
(SignalRatePerSpad < RangeIgnoreThresholdValue)) {
/* Limit Fail add 2^6 to range status */
RangeIgnoreThresholdflag = 1;
}
}
if (Status == VL53L0X_ERROR_NONE) {
if (NoneFlag == 1) {
*pPalRangeStatus = 255; /* NONE */
} else if (DeviceRangeStatusInternal == 1 ||
DeviceRangeStatusInternal == 2 ||
DeviceRangeStatusInternal == 3) {
*pPalRangeStatus = 5; /* HW fail */
} else if (DeviceRangeStatusInternal == 6 ||
DeviceRangeStatusInternal == 9) {
*pPalRangeStatus = 4; /* Phase fail */
} else if (DeviceRangeStatusInternal == 8 ||
DeviceRangeStatusInternal == 10 ||
SignalRefClipflag == 1) {
*pPalRangeStatus = 3; /* Min range */
} else if (DeviceRangeStatusInternal == 4 ||
RangeIgnoreThresholdflag == 1) {
*pPalRangeStatus = 2; /* Signal Fail */
} else if (SigmaLimitflag == 1) {
*pPalRangeStatus = 1; /* Sigma Fail */
} else {
*pPalRangeStatus = 0; /* Range Valid */
}
}
/* DMAX only relevant during range error */
if (*pPalRangeStatus == 0)
pRangingMeasurementData->RangeDMaxMilliMeter = 0;
/* fill the Limit Check Status */
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&SignalRateFinalRangeLimitCheckEnable);
if (Status == VL53L0X_ERROR_NONE) {
if ((SigmaLimitCheckEnable == 0) || (SigmaLimitflag == 1))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
if ((DeviceRangeStatusInternal == 4) ||
(SignalRateFinalRangeLimitCheckEnable == 0))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Temp8);
if ((SignalRefClipLimitCheckEnable == 0) ||
(SignalRefClipflag == 1))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8);
if ((RangeIgnoreThresholdLimitCheckEnable == 0) ||
(RangeIgnoreThresholdflag == 1))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
Temp8);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetRangingMeasurementData(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t DeviceRangeStatus;
uint8_t RangeFractionalEnable;
uint8_t PalRangeStatus;
uint8_t XTalkCompensationEnable;
uint16_t AmbientRate;
FixPoint1616_t SignalRate;
uint16_t XTalkCompensationRateMegaCps;
uint16_t EffectiveSpadRtnCount;
uint16_t tmpuint16;
uint16_t XtalkRangeMilliMeter;
uint16_t LinearityCorrectiveGain;
uint8_t localBuffer[12];
VL53L0X_RangingMeasurementData_t LastRangeDataBuffer;
LOG_FUNCTION_START("");
/*
* use multi read even if some registers are not useful, result will
* be more efficient
* start reading at 0x14 dec20
* end reading at 0x21 dec33 total 14 bytes to read
*/
Status = VL53L0X_ReadMulti(Dev, 0x14, localBuffer, 12);
if (Status == VL53L0X_ERROR_NONE) {
pRangingMeasurementData->ZoneId = 0; /* Only one zone */
pRangingMeasurementData->TimeStamp = 0; /* Not Implemented */
tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]);
/* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
*(format 11.2) else no fractional
*/
pRangingMeasurementData->MeasurementTimeUsec = 0;
SignalRate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
/* peak_signal_count_rate_rtn_mcps */
pRangingMeasurementData->SignalRateRtnMegaCps = SignalRate;
AmbientRate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
pRangingMeasurementData->AmbientRateRtnMegaCps =
VL53L0X_FIXPOINT97TOFIXPOINT1616(AmbientRate);
EffectiveSpadRtnCount = VL53L0X_MAKEUINT16(localBuffer[3],
localBuffer[2]);
/* EffectiveSpadRtnCount is 8.8 format */
pRangingMeasurementData->EffectiveSpadRtnCount =
EffectiveSpadRtnCount;
DeviceRangeStatus = localBuffer[0];
/* Get Linearity Corrective Gain */
LinearityCorrectiveGain = PALDevDataGet(Dev,
LinearityCorrectiveGain);
/* Get ranging configuration */
RangeFractionalEnable = PALDevDataGet(Dev,
RangeFractionalEnable);
if (LinearityCorrectiveGain != 1000) {
tmpuint16 = (uint16_t)((LinearityCorrectiveGain
* tmpuint16 + 500) / 1000);
/* Implement Xtalk */
VL53L0X_GETPARAMETERFIELD(Dev,
XTalkCompensationRateMegaCps,
XTalkCompensationRateMegaCps);
VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable,
XTalkCompensationEnable);
if (XTalkCompensationEnable) {
if ((SignalRate
- ((XTalkCompensationRateMegaCps
* EffectiveSpadRtnCount) >> 8))
<= 0) {
if (RangeFractionalEnable)
XtalkRangeMilliMeter = 8888;
else
XtalkRangeMilliMeter = 8888
<< 2;
} else {
XtalkRangeMilliMeter =
(tmpuint16 * SignalRate)
/ (SignalRate
- ((XTalkCompensationRateMegaCps
* EffectiveSpadRtnCount)
>> 8));
}
tmpuint16 = XtalkRangeMilliMeter;
}
}
if (RangeFractionalEnable) {
pRangingMeasurementData->RangeMilliMeter =
(uint16_t)((tmpuint16) >> 2);
pRangingMeasurementData->RangeFractionalPart =
(uint8_t)((tmpuint16 & 0x03) << 6);
} else {
pRangingMeasurementData->RangeMilliMeter = tmpuint16;
pRangingMeasurementData->RangeFractionalPart = 0;
}
/*
* For a standard definition of RangeStatus, this should
* return 0 in case of good result after a ranging
* The range status depends on the device so call a device
* specific function to obtain the right Status.
*/
Status |= VL53L0X_get_pal_range_status(Dev, DeviceRangeStatus,
SignalRate, EffectiveSpadRtnCount,
pRangingMeasurementData, &PalRangeStatus);
if (Status == VL53L0X_ERROR_NONE)
pRangingMeasurementData->RangeStatus = PalRangeStatus;
}
if (Status == VL53L0X_ERROR_NONE) {
/* Copy last read data into Dev buffer */
LastRangeDataBuffer = PALDevDataGet(Dev, LastRangeMeasure);
LastRangeDataBuffer.RangeMilliMeter =
pRangingMeasurementData->RangeMilliMeter;
LastRangeDataBuffer.RangeFractionalPart =
pRangingMeasurementData->RangeFractionalPart;
LastRangeDataBuffer.RangeDMaxMilliMeter =
pRangingMeasurementData->RangeDMaxMilliMeter;
LastRangeDataBuffer.MeasurementTimeUsec =
pRangingMeasurementData->MeasurementTimeUsec;
LastRangeDataBuffer.SignalRateRtnMegaCps =
pRangingMeasurementData->SignalRateRtnMegaCps;
LastRangeDataBuffer.AmbientRateRtnMegaCps =
pRangingMeasurementData->AmbientRateRtnMegaCps;
LastRangeDataBuffer.EffectiveSpadRtnCount =
pRangingMeasurementData->EffectiveSpadRtnCount;
LastRangeDataBuffer.RangeStatus =
pRangingMeasurementData->RangeStatus;
PALDevDataSet(Dev, LastRangeMeasure, LastRangeDataBuffer);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_PerformSingleRangingMeasurement(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
/* This function will do a complete single ranging
* Here we fix the mode! */
Status = VL53L0X_SetDeviceMode(Dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_PerformSingleMeasurement(Dev);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetRangingMeasurementData(Dev,
pRangingMeasurementData);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_ClearInterruptMask(Dev, 0);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV Dev,
uint16_t *refSignalRate)
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_RangingMeasurementData_t rangingMeasurementData;
uint8_t SequenceConfig = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function
*/
SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
/*
* This function performs a reference signal rate measurement.
*/
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_PerformSingleRangingMeasurement(Dev,
&rangingMeasurementData);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_RdWord(Dev,
VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
refSignalRate);
if (status == VL53L0X_ERROR_NONE)
status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
if (status == VL53L0X_ERROR_NONE) {
/* restore the previous Sequence Config */
status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
SequenceConfig);
if (status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
uint32_t *refSpadCount,
uint8_t *isApertureSpads)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t lastSpadArray[6];
uint8_t startSelect = 0xB4;
uint32_t minimumSpadCount = 3;
uint32_t maxSpadCount = 44;
uint32_t currentSpadIndex = 0;
uint32_t lastSpadIndex = 0;
int32_t nextGoodSpad = 0;
uint16_t targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
uint16_t peakSignalRateRef;
uint32_t needAptSpads = 0;
uint32_t index = 0;
uint32_t spadArraySize = 6;
uint32_t signalRateDiff = 0;
uint32_t lastSignalRateDiff = 0;
uint8_t complete = 0;
uint8_t VhvSettings = 0;
uint8_t PhaseCal = 0;
uint32_t refSpadCount_int = 0;
uint8_t isApertureSpads_int = 0;
/*
* The reference SPAD initialization procedure determines the minimum
* amount of reference spads to be enables to achieve a target reference
* signal rate and should be performed once during initialization.
*
* Either aperture or non-aperture spads are applied but never both.
* Firstly non-aperture spads are set, begining with 5 spads, and
* increased one spad at a time until the closest measurement to the
* target rate is achieved.
*
* If the target rate is exceeded when 5 non-aperture spads are enabled,
* initialization is performed instead with aperture spads.
*
* When setting spads, a 'Good Spad Map' is applied.
*
* This procedure operates within a SPAD window of interest of a maximum
* 44 spads.
* The start point is currently fixed to 180, which lies towards the end
* of the non-aperture quadrant and runs in to the adjacent aperture
* quadrant.
*/
targetRefRate = PALDevDataGet(Dev, targetRefRate);
/*
* Initialize Spad arrays.
* Currently the good spad map is initialised to 'All good'.
* This is a short term implementation. The good spad map will be
* provided as an input.
* Note that there are 6 bytes. Only the first 44 bits will be used to
* represent spads.
*/
for (index = 0; index < spadArraySize; index++)
Dev->Data.SpadData.RefSpadEnables[index] = 0;
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
startSelect);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
/* Perform ref calibration */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_perform_ref_calibration(Dev, &VhvSettings,
&PhaseCal, 0);
if (Status == VL53L0X_ERROR_NONE) {
/* Enable Minimum NON-APERTURE Spads */
currentSpadIndex = 0;
lastSpadIndex = currentSpadIndex;
needAptSpads = 0;
Status = enable_ref_spads(Dev,
needAptSpads,
Dev->Data.SpadData.RefGoodSpadMap,
Dev->Data.SpadData.RefSpadEnables,
spadArraySize,
startSelect,
currentSpadIndex,
minimumSpadCount,
&lastSpadIndex);
}
if (Status == VL53L0X_ERROR_NONE) {
currentSpadIndex = lastSpadIndex;
Status = perform_ref_signal_measurement(Dev,
&peakSignalRateRef);
if ((Status == VL53L0X_ERROR_NONE) &&
(peakSignalRateRef > targetRefRate)) {
/* Signal rate measurement too high,
* switch to APERTURE SPADs */
for (index = 0; index < spadArraySize; index++)
Dev->Data.SpadData.RefSpadEnables[index] = 0;
/* Increment to the first APERTURE spad */
while ((is_aperture(startSelect + currentSpadIndex)
== 0) && (currentSpadIndex < maxSpadCount)) {
currentSpadIndex++;
}
needAptSpads = 1;
Status = enable_ref_spads(Dev,
needAptSpads,
Dev->Data.SpadData.RefGoodSpadMap,
Dev->Data.SpadData.RefSpadEnables,
spadArraySize,
startSelect,
currentSpadIndex,
minimumSpadCount,
&lastSpadIndex);
if (Status == VL53L0X_ERROR_NONE) {
currentSpadIndex = lastSpadIndex;
Status = perform_ref_signal_measurement(Dev,
&peakSignalRateRef);
if ((Status == VL53L0X_ERROR_NONE) &&
(peakSignalRateRef > targetRefRate)) {
/* Signal rate still too high after
* setting the minimum number of
* APERTURE spads. Can do no more
* therefore set the min number of
* aperture spads as the result.
*/
isApertureSpads_int = 1;
refSpadCount_int = minimumSpadCount;
}
}
} else {
needAptSpads = 0;
}
}
if ((Status == VL53L0X_ERROR_NONE) &&
(peakSignalRateRef < targetRefRate)) {
/* At this point, the minimum number of either aperture
* or non-aperture spads have been set. Proceed to add
* spads and perform measurements until the target
* reference is reached.
*/
isApertureSpads_int = needAptSpads;
refSpadCount_int = minimumSpadCount;
memcpy(lastSpadArray, Dev->Data.SpadData.RefSpadEnables,
spadArraySize);
lastSignalRateDiff = abs(peakSignalRateRef -
targetRefRate);
complete = 0;
while (!complete) {
get_next_good_spad(
Dev->Data.SpadData.RefGoodSpadMap,
spadArraySize, currentSpadIndex,
&nextGoodSpad);
if (nextGoodSpad == -1) {
Status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
/* Cannot combine Aperture and Non-Aperture spads, so
* ensure the current spad is of the correct type.
*/
if (is_aperture((uint32_t)startSelect + nextGoodSpad) !=
needAptSpads) {
/* At this point we have enabled the maximum
* number of Aperture spads.
*/
complete = 1;
break;
}
(refSpadCount_int)++;
currentSpadIndex = nextGoodSpad;
Status = enable_spad_bit(
Dev->Data.SpadData.RefSpadEnables,
spadArraySize, currentSpadIndex);
if (Status == VL53L0X_ERROR_NONE) {
currentSpadIndex++;
/* Proceed to apply the additional spad and
* perform measurement. */
Status = set_ref_spad_map(Dev,
Dev->Data.SpadData.RefSpadEnables);
}
if (Status != VL53L0X_ERROR_NONE)
break;
Status = perform_ref_signal_measurement(Dev,
&peakSignalRateRef);
if (Status != VL53L0X_ERROR_NONE)
break;
signalRateDiff = abs(peakSignalRateRef - targetRefRate);
if (peakSignalRateRef > targetRefRate) {
/* Select the spad map that provides the
* measurement closest to the target rate,
* either above or below it.
*/
if (signalRateDiff > lastSignalRateDiff) {
/* Previous spad map produced a closer
* measurement, so choose this. */
Status = set_ref_spad_map(Dev,
lastSpadArray);
memcpy(
Dev->Data.SpadData.RefSpadEnables,
lastSpadArray, spadArraySize);
(refSpadCount_int)--;
}
complete = 1;
} else {
/* Continue to add spads */
lastSignalRateDiff = signalRateDiff;
memcpy(lastSpadArray,
Dev->Data.SpadData.RefSpadEnables,
spadArraySize);
}
} /* while */
}
if (Status == VL53L0X_ERROR_NONE) {
*refSpadCount = refSpadCount_int;
*isApertureSpads = isApertureSpads_int;
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadCount, (uint8_t)(*refSpadCount));
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadType, *isApertureSpads);
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
uint32_t count, uint8_t isApertureSpads)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint32_t currentSpadIndex = 0;
uint8_t startSelect = 0xB4;
uint32_t spadArraySize = 6;
uint32_t maxSpadCount = 44;
uint32_t lastSpadIndex;
uint32_t index;
/*
* This function applies a requested number of reference spads, either
* aperture or
* non-aperture, as requested.
* The good spad map will be applied.
*/
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
startSelect);
for (index = 0; index < spadArraySize; index++)
Dev->Data.SpadData.RefSpadEnables[index] = 0;
if (isApertureSpads) {
/* Increment to the first APERTURE spad */
while ((is_aperture(startSelect + currentSpadIndex) == 0) &&
(currentSpadIndex < maxSpadCount)) {
currentSpadIndex++;
}
}
Status = enable_ref_spads(Dev,
isApertureSpads,
Dev->Data.SpadData.RefGoodSpadMap,
Dev->Data.SpadData.RefSpadEnables,
spadArraySize,
startSelect,
currentSpadIndex,
count,
&lastSpadIndex);
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadCount, (uint8_t)(count));
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadType, isApertureSpads);
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_WaitDeviceBooted(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NOT_IMPLEMENTED;
LOG_FUNCTION_START("");
/* not implemented on VL53L0X */
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_PerformRefCalibration(VL53L0X_DEV Dev, uint8_t *pVhvSettings,
uint8_t *pPhaseCal)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_perform_ref_calibration(Dev, pVhvSettings,
pPhaseCal, 1);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_PerformRefSpadManagement(VL53L0X_DEV Dev,
uint32_t *refSpadCount, uint8_t *isApertureSpads)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_perform_ref_spad_management(Dev, refSpadCount,
isApertureSpads);
LOG_FUNCTION_END(Status);
return Status;
}
/* Group PAL Init Functions */
VL53L0X_Error VL53L0X::VL53L0X_SetDeviceAddress(VL53L0X_DEV Dev, uint8_t DeviceAddress)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
DeviceAddress / 2);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetGpioConfig(VL53L0X_DEV Dev, uint8_t Pin,
VL53L0X_DeviceModes DeviceMode, VL53L0X_GpioFunctionality Functionality,
VL53L0X_InterruptPolarity Polarity)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t data;
LOG_FUNCTION_START("");
if (Pin != 0) {
Status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
} else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
if (Polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
data = 0x10;
else
data = 1;
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
} else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_OSC) {
Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x85, 0x02);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x04);
Status |= VL53L0X_WrByte(Dev, 0xcd, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xcc, 0x11);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x07);
Status |= VL53L0X_WrByte(Dev, 0xbe, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x06);
Status |= VL53L0X_WrByte(Dev, 0xcc, 0x09);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
} else {
if (Status == VL53L0X_ERROR_NONE) {
switch (Functionality) {
case VL53L0X_GPIOFUNCTIONALITY_OFF:
data = 0x00;
break;
case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
data = 0x01;
break;
case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
data = 0x02;
break;
case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
data = 0x03;
break;
case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
data = 0x04;
break;
default:
Status =
VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
}
}
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
if (Status == VL53L0X_ERROR_NONE) {
if (Polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
data = 0;
else
data = (uint8_t)(1 << 4);
Status = VL53L0X_UpdateByte(Dev,
VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
}
if (Status == VL53L0X_ERROR_NONE)
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
Pin0GpioFunctionality, Functionality);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_ClearInterruptMask(Dev, 0);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetFractionEnable(VL53L0X_DEV Dev, uint8_t *pEnabled)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, pEnabled);
if (Status == VL53L0X_ERROR_NONE)
*pEnabled = (*pEnabled & 1);
LOG_FUNCTION_END(Status);
return Status;
}
uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
{
/*!
* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
*/
uint16_t encoded_timeout = 0;
uint32_t ls_byte = 0;
uint16_t ms_byte = 0;
if (timeout_macro_clks > 0) {
ls_byte = timeout_macro_clks - 1;
while ((ls_byte & 0xFFFFFF00) > 0) {
ls_byte = ls_byte >> 1;
ms_byte++;
}
encoded_timeout = (ms_byte << 8)
+ (uint16_t) (ls_byte & 0x000000FF);
}
return encoded_timeout;
}
VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV Dev,
VL53L0X_SequenceStepId SequenceStepId,
uint32_t TimeOutMicroSecs)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t CurrentVCSELPulsePeriodPClk;
uint8_t MsrcEncodedTimeOut;
uint16_t PreRangeEncodedTimeOut;
uint16_t PreRangeTimeOutMClks;
uint16_t MsrcRangeTimeOutMClks;
uint32_t FinalRangeTimeOutMClks;
uint16_t FinalRangeEncodedTimeOut;
VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) ||
(SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) ||
(SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
if (Status == VL53L0X_ERROR_NONE) {
MsrcRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev,
TimeOutMicroSecs,
(uint8_t)CurrentVCSELPulsePeriodPClk);
if (MsrcRangeTimeOutMClks > 256)
MsrcEncodedTimeOut = 255;
else
MsrcEncodedTimeOut =
(uint8_t)MsrcRangeTimeOutMClks - 1;
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
LastEncodedTimeout,
MsrcEncodedTimeOut);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
MsrcEncodedTimeOut);
}
} else {
if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
PreRangeTimeOutMClks =
VL53L0X_calc_timeout_mclks(Dev,
TimeOutMicroSecs,
(uint8_t)CurrentVCSELPulsePeriodPClk);
PreRangeEncodedTimeOut = VL53L0X_encode_timeout(
PreRangeTimeOutMClks);
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
LastEncodedTimeout,
PreRangeEncodedTimeOut);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_WrWord(Dev,
VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
PreRangeEncodedTimeOut);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
PreRangeTimeoutMicroSecs,
TimeOutMicroSecs);
}
} else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
/* For the final range timeout, the pre-range timeout
* must be added. To do this both final and pre-range
* timeouts must be expressed in macro periods MClks
* because they have different vcsel periods.
*/
VL53L0X_GetSequenceStepEnables(Dev,
&SchedulerSequenceSteps);
PreRangeTimeOutMClks = 0;
if (SchedulerSequenceSteps.PreRangeOn) {
/* Retrieve PRE-RANGE VCSEL Period */
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&CurrentVCSELPulsePeriodPClk);
/* Retrieve PRE-RANGE Timeout in Macro periods
* (MCLKS) */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdWord(Dev, 0x51,
&PreRangeEncodedTimeOut);
PreRangeTimeOutMClks =
VL53L0X_decode_timeout(
PreRangeEncodedTimeOut);
}
}
/* Calculate FINAL RANGE Timeout in Macro Periods
* (MCLKS) and add PRE-RANGE value
*/
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetVcselPulsePeriod(Dev,
VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
&CurrentVCSELPulsePeriodPClk);
}
if (Status == VL53L0X_ERROR_NONE) {
FinalRangeTimeOutMClks =
VL53L0X_calc_timeout_mclks(Dev,
TimeOutMicroSecs,
(uint8_t) CurrentVCSELPulsePeriodPClk);
FinalRangeTimeOutMClks += PreRangeTimeOutMClks;
FinalRangeEncodedTimeOut =
VL53L0X_encode_timeout(FinalRangeTimeOutMClks);
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_WrWord(Dev, 0x71,
FinalRangeEncodedTimeOut);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
FinalRangeTimeoutMicroSecs,
TimeOutMicroSecs);
}
}
} else
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
uint32_t MeasurementTimingBudgetMicroSeconds)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint32_t FinalRangeTimingBudgetMicroSeconds;
VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
uint32_t StartOverheadMicroSeconds = 1910;
uint32_t EndOverheadMicroSeconds = 960;
uint32_t MsrcOverheadMicroSeconds = 660;
uint32_t TccOverheadMicroSeconds = 590;
uint32_t DssOverheadMicroSeconds = 690;
uint32_t PreRangeOverheadMicroSeconds = 660;
uint32_t FinalRangeOverheadMicroSeconds = 550;
uint32_t PreRangeTimeoutMicroSeconds = 0;
uint32_t cMinTimingBudgetMicroSeconds = 20000;
uint32_t SubTimeout = 0;
LOG_FUNCTION_START("");
if (MeasurementTimingBudgetMicroSeconds
< cMinTimingBudgetMicroSeconds) {
Status = VL53L0X_ERROR_INVALID_PARAMS;
return Status;
}
FinalRangeTimingBudgetMicroSeconds =
MeasurementTimingBudgetMicroSeconds -
(StartOverheadMicroSeconds + EndOverheadMicroSeconds);
Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
if (Status == VL53L0X_ERROR_NONE &&
(SchedulerSequenceSteps.TccOn ||
SchedulerSequenceSteps.MsrcOn ||
SchedulerSequenceSteps.DssOn)) {
/* TCC, MSRC and DSS all share the same timeout */
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_MSRC,
&MsrcDccTccTimeoutMicroSeconds);
/* Subtract the TCC, MSRC and DSS timeouts if they are
* enabled. */
if (Status != VL53L0X_ERROR_NONE)
return Status;
/* TCC */
if (SchedulerSequenceSteps.TccOn) {
SubTimeout = MsrcDccTccTimeoutMicroSeconds
+ TccOverheadMicroSeconds;
if (SubTimeout <
FinalRangeTimingBudgetMicroSeconds) {
FinalRangeTimingBudgetMicroSeconds -=
SubTimeout;
} else {
/* Requested timeout too big. */
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (Status != VL53L0X_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
/* DSS */
if (SchedulerSequenceSteps.DssOn) {
SubTimeout = 2 * (MsrcDccTccTimeoutMicroSeconds +
DssOverheadMicroSeconds);
if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
FinalRangeTimingBudgetMicroSeconds
-= SubTimeout;
} else {
/* Requested timeout too big. */
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
} else if (SchedulerSequenceSteps.MsrcOn) {
/* MSRC */
SubTimeout = MsrcDccTccTimeoutMicroSeconds +
MsrcOverheadMicroSeconds;
if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
FinalRangeTimingBudgetMicroSeconds
-= SubTimeout;
} else {
/* Requested timeout too big. */
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
}
if (Status != VL53L0X_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
if (SchedulerSequenceSteps.PreRangeOn) {
/* Subtract the Pre-range timeout if enabled. */
Status = get_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE,
&PreRangeTimeoutMicroSeconds);
SubTimeout = PreRangeTimeoutMicroSeconds +
PreRangeOverheadMicroSeconds;
if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
} else {
/* Requested timeout too big. */
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (Status == VL53L0X_ERROR_NONE &&
SchedulerSequenceSteps.FinalRangeOn) {
FinalRangeTimingBudgetMicroSeconds -=
FinalRangeOverheadMicroSeconds;
/* Final Range Timeout
* Note that the final range timeout is determined by the timing
* budget and the sum of all other timeouts within the sequence.
* If there is no room for the final range timeout, then an error
* will be set. Otherwise the remaining time will be applied to
* the final range.
*/
Status = set_sequence_step_timeout(Dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
FinalRangeTimingBudgetMicroSeconds);
VL53L0X_SETPARAMETERFIELD(Dev,
MeasurementTimingBudgetMicroSeconds,
MeasurementTimingBudgetMicroSeconds);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev,
uint32_t MeasurementTimingBudgetMicroSeconds)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_set_measurement_timing_budget_micro_seconds(Dev,
MeasurementTimingBudgetMicroSeconds);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetSequenceStepEnable(VL53L0X_DEV Dev,
VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t SequenceConfig = 0;
uint8_t SequenceConfigNew = 0;
uint32_t MeasurementTimingBudgetMicroSeconds;
LOG_FUNCTION_START("");
Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
&SequenceConfig);
SequenceConfigNew = SequenceConfig;
if (Status == VL53L0X_ERROR_NONE) {
if (SequenceStepEnabled == 1) {
/* Enable requested sequence step
*/
switch (SequenceStepId) {
case VL53L0X_SEQUENCESTEP_TCC:
SequenceConfigNew |= 0x10;
break;
case VL53L0X_SEQUENCESTEP_DSS:
SequenceConfigNew |= 0x28;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
SequenceConfigNew |= 0x04;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
SequenceConfigNew |= 0x40;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
SequenceConfigNew |= 0x80;
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
} else {
/* Disable requested sequence step
*/
switch (SequenceStepId) {
case VL53L0X_SEQUENCESTEP_TCC:
SequenceConfigNew &= 0xef;
break;
case VL53L0X_SEQUENCESTEP_DSS:
SequenceConfigNew &= 0xd7;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
SequenceConfigNew &= 0xfb;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
SequenceConfigNew &= 0xbf;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
SequenceConfigNew &= 0x7f;
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
}
if (SequenceConfigNew != SequenceConfig) {
/* Apply New Setting */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_WrByte(Dev,
VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, SequenceConfigNew);
}
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, SequenceConfig, SequenceConfigNew);
/* Recalculate timing budget */
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_GETPARAMETERFIELD(Dev,
MeasurementTimingBudgetMicroSeconds,
MeasurementTimingBudgetMicroSeconds);
VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
MeasurementTimingBudgetMicroSeconds);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId,
uint8_t LimitCheckEnable)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
FixPoint1616_t TempFix1616 = 0;
uint8_t LimitCheckEnableInt = 0;
uint8_t LimitCheckDisable = 0;
uint8_t Temp8;
LOG_FUNCTION_START("");
if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
Status = VL53L0X_ERROR_INVALID_PARAMS;
} else {
if (LimitCheckEnable == 0) {
TempFix1616 = 0;
LimitCheckEnableInt = 0;
LimitCheckDisable = 1;
} else {
VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
LimitCheckId, TempFix1616);
LimitCheckDisable = 0;
/* this to be sure to have either 0 or 1 */
LimitCheckEnableInt = 1;
}
switch (LimitCheckId) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
LimitCheckEnableInt);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
Status = VL53L0X_WrWord(Dev,
VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
VL53L0X_FIXPOINT1616TOFIXPOINT97(TempFix1616));
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
LimitCheckEnableInt);
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
LimitCheckEnableInt);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
Temp8 = (uint8_t)(LimitCheckDisable << 1);
Status = VL53L0X_UpdateByte(Dev,
VL53L0X_REG_MSRC_CONFIG_CONTROL,
0xFE, Temp8);
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
Temp8 = (uint8_t)(LimitCheckDisable << 4);
Status = VL53L0X_UpdateByte(Dev,
VL53L0X_REG_MSRC_CONFIG_CONTROL,
0xEF, Temp8);
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (Status == VL53L0X_ERROR_NONE) {
if (LimitCheckEnable == 0) {
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
LimitCheckId, 0);
} else {
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
LimitCheckId, 1);
}
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_StaticInit(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceParameters_t CurrentParameters = {0};
uint8_t *pTuningSettingBuffer;
uint16_t tempword = 0;
uint8_t tempbyte = 0;
uint8_t UseInternalTuningSettings = 0;
uint32_t count = 0;
uint8_t isApertureSpads = 0;
uint32_t refSpadCount = 0;
uint8_t ApertureSpads = 0;
uint8_t vcselPulsePeriodPCLK;
uint32_t seqTimeoutMicroSecs;
LOG_FUNCTION_START("");
Status = VL53L0X_get_info_from_device(Dev, 1);
/* set the ref spad from NVM */
count = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadCount);
ApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
ReferenceSpadType);
/* NVM value invalid */
if ((ApertureSpads > 1) ||
((ApertureSpads == 1) && (count > 32)) ||
((ApertureSpads == 0) && (count > 12)))
Status = VL53L0X_perform_ref_spad_management(Dev, &refSpadCount,
&isApertureSpads);
else
Status = VL53L0X_set_reference_spads(Dev, count, ApertureSpads);
/* Initialize tuning settings buffer to prevent compiler warning. */
pTuningSettingBuffer = DefaultTuningSettings;
if (Status == VL53L0X_ERROR_NONE) {
UseInternalTuningSettings = PALDevDataGet(Dev,
UseInternalTuningSettings);
if (UseInternalTuningSettings == 0)
pTuningSettingBuffer = PALDevDataGet(Dev,
pTuningSettingsPointer);
else
pTuningSettingBuffer = DefaultTuningSettings;
}
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_load_tuning_settings(Dev, pTuningSettingBuffer);
/* Set interrupt config to new sample ready */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetGpioConfig(Dev, 0, 0,
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
VL53L0X_INTERRUPTPOLARITY_LOW);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status |= VL53L0X_RdWord(Dev, 0x84, &tempword);
Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, OscFrequencyMHz,
VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword));
}
/* After static init, some device parameters may be changed,
* so update them */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters);
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetFractionEnable(Dev, &tempbyte);
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, RangeFractionalEnable, tempbyte);
}
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, CurrentParameters, CurrentParameters);
/* read the sequence config and save it */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_RdByte(Dev,
VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, SequenceConfig, tempbyte);
}
/* Disable MSRC and TCC by default */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_SetSequenceStepEnable(Dev,
VL53L0X_SEQUENCESTEP_TCC, 0);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_SetSequenceStepEnable(Dev,
VL53L0X_SEQUENCESTEP_MSRC, 0);
/* Set PAL State to standby */
if (Status == VL53L0X_ERROR_NONE)
PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
/* Store pre-range vcsel period */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetVcselPulsePeriod(
Dev,
VL53L0X_VCSEL_PERIOD_PRE_RANGE,
&vcselPulsePeriodPCLK);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
PreRangeVcselPulsePeriod,
vcselPulsePeriodPCLK);
}
/* Store final-range vcsel period */
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetVcselPulsePeriod(
Dev,
VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
&vcselPulsePeriodPCLK);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
FinalRangeVcselPulsePeriod,
vcselPulsePeriodPCLK);
}
/* Store pre-range timeout */
if (Status == VL53L0X_ERROR_NONE) {
Status = get_sequence_step_timeout(
Dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE,
&seqTimeoutMicroSecs);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
PreRangeTimeoutMicroSecs,
seqTimeoutMicroSecs);
}
/* Store final-range timeout */
if (Status == VL53L0X_ERROR_NONE) {
Status = get_sequence_step_timeout(
Dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&seqTimeoutMicroSecs);
}
if (Status == VL53L0X_ERROR_NONE) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
Dev,
FinalRangeTimeoutMicroSecs,
seqTimeoutMicroSecs);
}
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_StopMeasurement(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
LOG_FUNCTION_START("");
Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
Status = VL53L0X_WrByte(Dev, 0x91, 0x00);
Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
if (Status == VL53L0X_ERROR_NONE) {
/* Set PAL State to Idle */
PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
}
/* Check if need to apply interrupt settings */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 0);
LOG_FUNCTION_END(Status);
return Status;
}
VL53L0X_Error VL53L0X::VL53L0X_GetStopCompletedStatus(VL53L0X_DEV Dev,
uint32_t *pStopStatus)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t Byte = 0;
LOG_FUNCTION_START("");
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_RdByte(Dev, 0x04, &Byte);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x0);
*pStopStatus = Byte;
if (Byte == 0) {
Status = VL53L0X_WrByte(Dev, 0x80, 0x01);
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
Status = VL53L0X_WrByte(Dev, 0x91,
PALDevDataGet(Dev, StopVariable));
Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
Status = VL53L0X_WrByte(Dev, 0x80, 0x00);
}
LOG_FUNCTION_END(Status);
return Status;
}
/****************** Write and read functions from I2C *************************/
VL53L0X_Error VL53L0X::VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
{
int status;
status = VL53L0X_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
{
int status;
if (count>=VL53L0X_MAX_I2C_XFER_SIZE){
status = VL53L0X_ERROR_INVALID_PARAMS;
}
status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
{
int status;
status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, &data, 1);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
{
int status;
uint8_t buffer[2];
buffer[0] = data >> 8;
buffer[1] = data & 0x00FF;
status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
{
int status;
uint8_t buffer[4];
buffer[0] = (data >> 24) & 0xFF;
buffer[1] = (data >> 16) & 0xFF;
buffer[2] = (data >> 8) & 0xFF;
buffer[3] = (data >> 0) & 0xFF;
status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
{
int status;
status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, data, 1);
if(status)
return -1;
return 0;
}
VL53L0X_Error VL53L0X::VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
{
int status;
uint8_t buffer[2] = {0,0};
status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, buffer, 2);
if (!status)
{
*data = (buffer[0] << 8) + buffer[1];
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
{
int status;
uint8_t buffer[4] = {0,0,0,0};
status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, buffer, 4);
if(!status)
{
*data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData)
{
int status;
uint8_t buffer = 0;
/* read data direct onto buffer */
status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, &buffer,1);
if (!status)
{
buffer = (buffer & AndData) | OrData;
status = VL53L0X_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint8_t)1);
}
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_I2CWrite(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
{
int ret;
ret = dev_i2c.i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
if(ret)
return -1;
return 0;
}
VL53L0X_Error VL53L0X::VL53L0X_I2CRead(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
{
int ret;
ret = dev_i2c.i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
if(ret)
return -1;
return 0;
}
int VL53L0X::ReadID(uint8_t *id)
{
int status = 0;
uint16_t rl_id=0;
status = VL53L0X_RdWord(Device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id);
if (rl_id == 0xEEAA)
return status;
return -1;
}
VL53L0X_Error VL53L0X::WaitMeasurementDataReady(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t NewDatReady=0;
uint32_t LoopNb;
// Wait until it finished
// use timeout to avoid deadlock
if (Status == VL53L0X_ERROR_NONE) {
LoopNb = 0;
do {
Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) {
break;
}
LoopNb = LoopNb + 1;
VL53L0X_PollingDelay(Dev);
} while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
Status = VL53L0X_ERROR_TIME_OUT;
}
}
return Status;
}
VL53L0X_Error VL53L0X::WaitStopCompleted(VL53L0X_DEV Dev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint32_t StopCompleted=0;
uint32_t LoopNb;
// Wait until it finished
// use timeout to avoid deadlock
if (Status == VL53L0X_ERROR_NONE) {
LoopNb = 0;
do {
Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) {
break;
}
LoopNb = LoopNb + 1;
VL53L0X_PollingDelay(Dev);
} while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
Status = VL53L0X_ERROR_TIME_OUT;
}
}
return Status;
}
int VL53L0X::InitSensor(uint8_t NewAddr)
{
int status;
VL53L0X_Off();
VL53L0X_On();
// status=VL53L0X_WaitDeviceBooted(Device);
// if(status)
// printf("WaitDeviceBooted fail\n\r");
status=IsPresent();
if(!status)
{
VL53L0X_DataInit(Device);
if(status != VL53L0X_ERROR_NONE)
{
printf("Failed to init VL53L0X sensor!\n\r");
return status;
}
// deduce silicon version
status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
status=Prepare();
if(status != VL53L0X_ERROR_NONE)
{
printf("Failed to prepare VL53L0X!\n\r");
return status;
}
/*
if(NewAddr!=DEFAULT_DEVICE_ADDRESS)
{
status=SetDeviceAddress(NewAddr);
if(status)
{
printf("Failed to change I2C address!\n\r");
return status;
}
}
else
{
printf("Invalid new address!\n\r");
return VL53L0X_ERROR_INVALID_PARAMS;
}
*/
// Device->Ready=1;
}
return status;
}
int VL53L0X::StartMeasurement(OperatingMode operating_mode, void (*fptr)(void))
{
int Status = VL53L0X_ERROR_NONE;
uint8_t VhvSettings;
uint8_t PhaseCal;
// *** from mass market cube expansion v1.1, ranging with satellites.
// default settings, for normal range.
FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536);
FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536);
uint32_t timingBudget = 33000;
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
if (operating_mode == range_single_shot_polling)
{
// singelshot, polled ranging
if(Status == VL53L0X_ERROR_NONE)
{
// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
}
// Enable/Disable Sigma and Signal check
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckEnable(Device,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckEnable(Device,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
}
// *** from mass market cube expansion v1.1, ranging with satellites.
/* Ranging configuration */
//*
// switch(rangingConfig) {
// case LONG_RANGE:
signalLimit = (FixPoint1616_t)(0.1*65536);
sigmaLimit = (FixPoint1616_t)(60*65536);
timingBudget = 33000;
preRangeVcselPeriod = 18;
finalRangeVcselPeriod = 14;
/* break;
case HIGH_ACCURACY:
signalLimit = (FixPoint1616_t)(0.25*65536);
sigmaLimit = (FixPoint1616_t)(18*65536);
timingBudget = 200000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
case HIGH_SPEED:
signalLimit = (FixPoint1616_t)(0.25*65536);
sigmaLimit = (FixPoint1616_t)(32*65536);
timingBudget = 20000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
default:
debug_printf("Not Supported");
}
*/
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckValue(Device,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetLimitCheckValue(Device,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Device, timingBudget);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetVcselPulsePeriod(Device,
VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_SetVcselPulsePeriod(Device,
VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
}
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_PerformRefCalibration(Device, &VhvSettings, &PhaseCal);
}
}
if (operating_mode == range_continuous_polling)
{
if(Status == VL53L0X_ERROR_NONE)
{
printf ("Call of VL53L0X_SetDeviceMode\n");
Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
}
if(Status == VL53L0X_ERROR_NONE)
{
printf ("Call of VL53L0X_StartMeasurement\n");
Status = VL53L0X_StartMeasurement(Device);
}
}
return Status;
}
int VL53L0X::GetMeasurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *Data)
{
int Status = VL53L0X_ERROR_NONE;
if (operating_mode == range_single_shot_polling)
{
Status = VL53L0X_PerformSingleRangingMeasurement(Device, Data);
}
if (operating_mode == range_continuous_polling)
{
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_measurement_poll_for_completion(Device);
if(Status == VL53L0X_ERROR_NONE)
{
Status = VL53L0X_GetRangingMeasurementData(Device, Data);
// Clear the interrupt
VL53L0X_ClearInterruptMask(Device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
VL53L0X_PollingDelay(Device);
}
}
return Status;
}
int VL53L0X::StopMeasurement(OperatingMode operating_mode)
{
int status = VL53L0X_ERROR_NONE;
// don't need to stop for a singleshot range!
if (operating_mode==range_single_shot_polling)
{
}
if (operating_mode==range_continuous_interrupt || operating_mode==range_continuous_polling)
{
// continuous mode
if(status == VL53L0X_ERROR_NONE)
{
printf ("Call of VL53L0X_StopMeasurement\n");
status = VL53L0X_StopMeasurement(Device);
}
if(status == VL53L0X_ERROR_NONE)
{
printf ("Wait Stop to be competed\n");
status = WaitStopCompleted(Device);
}
if(status == VL53L0X_ERROR_NONE)
status = VL53L0X_ClearInterruptMask(Device,
VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
}
return status;
}
int VL53L0X::HandleIRQ(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *Data)
{
int status;
EnableInterruptMeasureDetectionIRQ();
status=GetMeasurement(operating_mode, Data);
return status;
}
/******************************************************************************/