Tarek Lule
Condensed Version of Public VL53L0X
VL53L0X.cpp
- Committer:
- sepp_nepp
- Date:
- 2019-04-10
- Revision:
- 12:aa177f0e4c10
- Parent:
- 11:c6f95a42d4d7
File content as of revision 12:aa177f0e4c10:
/**
******************************************************************************
* @file 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.
*
******************************************************************************
*/
// Some example regex that were used to replace useless macros
// \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E
// _device->CurrParams.\1[\3] = \5;
// to replace this "#define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value)" by "_device->CurrParams.field[index] = value"
// to replace "Read_Byte(0x90,&module_id);" by "module_id = Read_Byte(0x90);" search and replace
// \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E
// \3 = Read_Byte\(\1\);
/* Includes */
#include <stdlib.h>
#include "VL53L0X.h"
#include "VL53L0X_tuning.h"
/******************************************************************************/
/******************************************************************************/
/****************** All initialization functions *************************/
/******************************************************************************/
/******************************************************************************/
// Function Data_init and Init_Sensor is united into Start_Sensor
VL53L0X_Error VL53L0X::Start_Sensor(uint8_t new_addr)
{ ErrState = VL53L0X_OK;
if (_gpio0) { // Can the shutdown pin be controlled?
*_gpio0 = 0; wait_ms(1); // quick shutdown
*_gpio0 = 1; wait_ms(10); // and back ON again
}
/* Setup the I2C bus. 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
VL53L0X_UpdateByte(REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01);
#endif
/* Set I2C standard mode */
Write_Byte(0x88,0x00);
// read and check the device ID from the ID register
Device_Info.ProductType = Read_Byte(REG_IDENTIFICATION_MODEL_ID);
if ( (ErrState == VL53L0X_OK) && (Device_Info.ProductType != 0xEEAA) )
{return VL53L0X_ERROR_I2C_WRONG_DEV_ID; }
// reconfigure the address with a new address if requested
if ( (ErrState == VL53L0X_OK) && (new_addr != VL53L0X_DEFAULT_ADDRESS) )
{ Write_Byte(REG_I2C_SLAVE_DEVICE_ADDRESS, new_addr / 2);
I2cDevAddr = new_addr;
}
// quite if an error was raised
if (ErrState != VL53L0X_OK) {return ErrState; }
/* Set Default static parameters
*set first temporary values 9.44MHz * 65536 = 618660 */
OscFrequencyMHz = 618660;
RefSPADSInitialised = 0;
// Read All NVM from device
ReadNVMDataFromDeviceDone = 0;
Get_all_NVM_info_from_device();
#ifdef USE_IQC_STATION
// must make sure that first your read all NVM info used by the API */
VL53L0X_Apply_Offset_Cal();
#endif
/* Default value is 1000 for Linearity Corrective Gain */
LinearityCorrectiveGain = 1000;
/* Dmax default Parameter */
DmaxCalRangeMilliMeter = 400;
DmaxCalSignalRateRtnMHz = (TFP1616)((0x00016B85)); /* 1.42 No Cover Glass*/
/* Get default parameters */
CurrParams = Get_device_parameters();
/* Set Default Xtalk_CompRate_MHz to 0 */
CurrParams.Xtalk_CompRate_MHz = 0;
/* initialize CurrParams values */
CurrParams.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
CurrParams.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
/* Sigma estimator variable */
SigmaEstRefArray = 100;
SigmaEstEffPulseWidth = 900;
SigmaEstEffAmbWidth = 500;
targetRefRate = 0x0A00; /* 20 MHz in 9:7 format */
/* Use internal default settings */
UseInternalTuningSettings = 1;
Write_Byte(0x80,0x01);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
StopVariable = Read_Byte(0x91);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
Write_Byte(0x80,0x00);
// quite if an error was raised
if (ErrState != VL53L0X_OK) {return ErrState; }
/* Disable the following SW-internal checks plaus set some values */
CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_REF_CLIP] = 0;
CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIG_REF_CLIP] = (35 * 65536);
CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
/* Disable the following Device-Internal Checks: */
CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_MSRC] = 0;
CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE] = 0;
Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEE, 0);
/* Only enable this internal Check : */
CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = 1;
CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = (18 * 65536);
/* Plus Enable VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE check */
Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1);
if (ErrState == VL53L0X_OK) { /* 0.25 in FP1616 notation 65536 */
Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,
(TFP1616)(25 * 65536 / 100)); }
// quit if an error was raised
if (ErrState != VL53L0X_OK) {return ErrState; }
// Preset the Config States
SequenceConfig = 0xFF ;
/* Set Device state to tell that we are waiting for call to VL53L0X_StaticInit */
Current_State = VL53L0X_STATE_WAIT_STATICINIT ;
Fill_device_info(); // Retrieve Silicon version, stored in Device_Info
uint32_t ref_SPAD_count;
uint8_t is_aperture_SPADS;
uint8_t vhv_settings;
uint8_t phase_cal;
// make sure the Get_all_NVM_info_from_device was called before calling static init
if (ErrState == VL53L0X_OK) { Static_init(); } // Device Initialization
if (ErrState == VL53L0X_OK) { // Device Calibration
Perf_Ref_calibration( &vhv_settings, &phase_cal, 1); }
if (ErrState == VL53L0X_OK) { // SPAD Configuration
Perf_Ref_SPAD_management( &ref_SPAD_count, &is_aperture_SPADS); }
return ErrState;
}
void VL53L0X::Fill_device_info()
{ uint8_t revision;
if (ErrState == VL53L0X_OK)
{ if (ModuleId == 0)
{ revision = 0;
strcpy(Device_Info.ProductId,""); }
else
{ revision = Revision;
strcpy(Device_Info.ProductId,ProductId);
}
if (revision == 0)
{ strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS0); }
else if ((revision <= 34) && (revision != 32))
{ strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS1); }
else if (revision < 39)
{ strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS2); }
else { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_ES1); }
strcpy(Device_Info.Type,VL53L0X_STRING_DEVICE_INFO_TYPE);
}
Device_Info.ProductRevisionMajor = 1;
Device_Info.ProductRevisionMinor =
(Read_Byte(REG_IDENTIFICATION_REVISION_ID) & 0xF0) >> 4;
}
/* That was 'Get_info_from_device( unit8_t option)' where option was composed of
1, 2, 4 depending what info was requested.
It was decided to combine all that into a single Get ALL infos from the device
that is called once at Sensor Init so all the job is done.
In addition, originally values would first be read into local variables and then
only copied to class fields if no errors.
However, if at device initialization an error is raised the whole class instance
cannot be used anyway, so decided that we directly read into class fields. */
void VL53L0X::Get_all_NVM_info_from_device()
{ uint32_t tmp_dword;
uint32_t offset_fixed1104_mm = 0;
int16_t offset_um = 0;
uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
uint32_t dist_meas_fixed1104_400_mm = 0;
uint32_t signal_rate_meas_fixed1104_400_mm = 0;
int i;
/* This access is done only once after that a GetDeviceInfo or datainit is done*/
if (ReadNVMDataFromDeviceDone == 7) { return ; }
Write_Byte(0x80,0x01);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0xFF,0x06);
Register_BitMask(0x83,0xFF,0x04)
Write_Byte(0xFF,0x07);
Write_Byte(0x81,0x01);
Polling_delay(); // warning, does nothing!!
Write_Byte(0x80,0x01);
/* ************* First Block of NVM data is read: ************/
tmp_dword = Get_NVM_DWord(0x6b);
ReferenceSPADCount = (uint8_t)((tmp_dword >> 8) & 0x7f);
ReferenceSPADType = (uint8_t)((tmp_dword >> 15) & 0x01);
tmp_dword = Get_NVM_DWord(0x24);
SPADData.RefGoodSPADMap[0] = (uint8_t)((tmp_dword >> 24)& 0xff);
SPADData.RefGoodSPADMap[1] = (uint8_t)((tmp_dword >> 16)& 0xff);
SPADData.RefGoodSPADMap[2] = (uint8_t)((tmp_dword >> 8)& 0xff);
SPADData.RefGoodSPADMap[3] = (uint8_t)(tmp_dword & 0xff);
tmp_dword = Get_NVM_DWord(0x25);
SPADData.RefGoodSPADMap[4] = (uint8_t)((tmp_dword >> 24)& 0xff);
SPADData.RefGoodSPADMap[5] = (uint8_t)((tmp_dword >> 16)& 0xff);
/* ************* Second Block of NVM data is read: ************/
ModuleId = Get_NVM_Byte(0x02);
Revision = Get_NVM_Byte(0x02);
tmp_dword = Get_NVM_DWord(0x77);
ProductId[0] = (char)((tmp_dword >> 25) & 0x07f);
ProductId[1] = (char)((tmp_dword >> 18) & 0x07f);
ProductId[2] = (char)((tmp_dword >> 11) & 0x07f);
ProductId[3] = (char)((tmp_dword >> 4) & 0x07f);
ProductId[4] = (char)((tmp_dword << 3) & 0x07f);
tmp_dword = Get_NVM_DWord(0x78);
ProductId[4] = ProductId[4] |
(char)((tmp_dword >> 29) & 0x07f));
ProductId[5] = (char)((tmp_dword >> 22) & 0x07f);
ProductId[6] = (char)((tmp_dword >> 15) & 0x07f);
ProductId[7] = (char)((tmp_dword >> 8) & 0x07f);
ProductId[8] = (char)((tmp_dword >> 1) & 0x07f);
ProductId[9] = (char)((tmp_dword << 6) & 0x07f);
tmp_dword = Get_NVM_DWord(0x79);
ProductId[9] = ProductId[9] |
(char)((tmp_dword >> 26) & 0x07f);
ProductId[10] = (char)((tmp_dword >> 19) & 0x07f);
ProductId[11] = (char)((tmp_dword >> 12) & 0x07f);
ProductId[12] = (char)((tmp_dword >> 5) & 0x07f);
ProductId[13] = (char)((tmp_dword << 2) & 0x07f);
tmp_dword = Get_NVM_DWord(0x7A);
ProductId[13] = ProductId[13] |
(char)((tmp_dword >> 30) & 0x07f));
ProductId[14] = (char)((tmp_dword >> 23) & 0x07f);
ProductId[15] = (char)((tmp_dword >> 16) & 0x07f);
ProductId[16] = (char)((tmp_dword >> 9) & 0x07f);
ProductId[17] = (char)((tmp_dword >> 2) & 0x07f);
ProductId[18] = '\0';
/* ************* Third Block of NVM data is read: ************/
PartUIDUpper = Get_NVM_DWord(0x7B);
PartUIDLower = Get_NVM_DWord(0x7C);
SignalRateMeasFixed400mm = // convert from FP97_TO_FP1616
( (( Get_NVM_DWord(0x73) << 17) & 0x1fE0000) |
(( Get_NVM_DWord(0x74) >> 15) & 0x001fE00) ) ;
dist_meas_fixed1104_400_mm =
(( Get_NVM_DWord(0x75) << 8) & 0xff00) |
(( Get_NVM_DWord(0x76) >> 24) & 0x00ff);
if (dist_meas_fixed1104_400_mm != 0) {
offset_fixed1104_mm = dist_meas_fixed1104_400_mm -
dist_meas_tgt_fixed1104_mm;
NVM_Offset_Cal_um = (offset_fixed1104_mm * 1000) >> 4;
NVM_Offset_Cal_um *= -1; }
else { NVM_Offset_Cal_um = 0; }
Write_Byte(0x81,0x00);
Write_Byte(0xFF,0x06);
Register_BitMask(0x83,0xfb,0x00)
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
Write_Byte(0x80,0x00);
ReadNVMDataFromDeviceDone = 7;
}
uint32_t VL53L0X::Get_distance()
{ ErrState = VL53L0X_OK;
TRangeResults p_ranging_results;
Start_Measurement(op_single_shot_poll, NULL);
if (ErrState==VL53L0X_OK)
{ p_ranging_results = Get_Measurement(op_single_shot_poll); }
Stop_Measurement(op_single_shot_poll);
if (p_ranging_results.RangeStatus == 0) // we have a valid range ?
{ return p_ranging_results.RangeMilliMeter; }
else
{ ErrState = VL53L0X_ERROR_RANGE_ERROR; return 0;}
}
/******************************************************************************/
/******************************************************************************/
/****************** Actual Measurement functions *************************/
/******************************************************************************/
/******************************************************************************/
TRangeResults VL53L0X::Get_Measurement(TOperatingMode operating_mode)
{ TRangeResults p_data;
switch (operating_mode) {
case op_single_shot_poll:
Perf_single_ranging_measurement(&p_data);
break;
case op_poll:
Poll_Measure_Completion();
Get_ranging_results(&p_data);
if (ErrState == VL53L0X_OK) { // Clear the interrupt
Clear_interrupt_mask(REG_SYSINT_GPIO_NEW_SAMPLE_READY);
Polling_delay();
}
break;
case op_INT:
Get_ranging_results(&p_data);
Clear_interrupt_mask(REG_SYSINT_CLEAR | REG_RESULT_INTERRUPT_STATUS);
} // switch
return p_data;
}
/** Get part to part calibration offset; Should only be used after a
successful call to @a VL53L0X_DataInit to backup device NVM value **/
int32_t VL53L0X::Get_Offset_Cal_um()
{ uint16_t range_offset_register;
int16_t c_max_offset = 2047;
int16_t c_offset_range = 4096;
/* Note that offset has 10.2 format */
range_offset_register = Read_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM);
if (ErrState == VL53L0X_OK) {
range_offset_register = (range_offset_register & 0x0fff);
/* Apply 12 bit 2's complement conversion */
if (range_offset_register > c_max_offset)
{ return (int16_t)(range_offset_register - c_offset_range) * 250; }
else
{ return (int16_t)range_offset_register * 250;}
}
else return 0;
}
void VL53L0X::Set_Offset_Cal_um(int32_t Offset_Cal_um)
{ int32_t c_max_offset_um = 511000;
int32_t c_min_offset_um = -512000;
int16_t c_offset_range = 4096;
uint32_t encoded_offset_val;
if (Offset_Cal_um > c_max_offset_um) { Offset_Cal_um = c_max_offset_um; }
else
if (Offset_Cal_um < c_min_offset_um) { Offset_Cal_um = c_min_offset_um; }
/* The offset register is 10.2 format and units are mm
* therefore conversion is applied by a division of 250. */
if (Offset_Cal_um >= 0) { encoded_offset_val = Offset_Cal_um / 250; }
else { encoded_offset_val = c_offset_range + Offset_Cal_um / 250; }
Write_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, encoded_offset_val);
}
void VL53L0X::VL53L0X_Apply_Offset_Cal()
{ int32_t Summed_Offset_Cal_um;
/* Read back current device offset, and remember in case later someone wants to use it */
if (ErrState == VL53L0X_OK) { Last_Offset_Cal_um = Get_Offset_Cal_um(); }
/* Apply Offset Adjustment derived from 400mm measurements */
if (ErrState == VL53L0X_OK)
{ Summed_Offset_Cal_um = Last_Offset_Cal_um + (int32_t) NVM_Offset_Cal_um;
Set_Offset_Cal_um(Summed_Offset_Cal_um);
/* remember current,adjusted offset */
if (ErrState == VL53L0X_OK) { CurrParams.Offset_Cal_um = Summed_Offset_Cal_um; }
}
}
void VL53L0X::Get_measure_period_ms(uint32_t *p_measure_period_ms)
{ uint16_t osc_calibrate_val;
uint32_t im_period_ms;
osc_calibrate_val = Read_Word(REG_OSC_CALIBRATE_VAL);
if (ErrState == VL53L0X_OK) { im_period_ms = Read_DWord(REG_SYSTEM_MEASURE_PERIOD); }
if (ErrState == VL53L0X_OK) {
if (osc_calibrate_val != 0)
{*p_measure_period_ms = im_period_ms / osc_calibrate_val; }
CurrParams.Measure_Period_ms = *p_measure_period_ms;
}
}
void VL53L0X::Get_Xtalk_CompRate_MHz( TFP1616 *p_Xtalk_CompRate_MHz)
{ uint16_t value;
TFP1616 temp_fix1616;
value = Read_Word(REG_XTALK_COMPENS_RATE_MHz);
if (ErrState == VL53L0X_OK) {
if (value == 0) {
/* the Xtalk is disabled return value from memory */
temp_fix1616 = CurrParams.Xtalk_CompRate_MHz;
*p_Xtalk_CompRate_MHz = temp_fix1616;
CurrParams.XTalk_Compens_En = 0;
} else {
temp_fix1616 = FP313_TO_FP1616(value);
*p_Xtalk_CompRate_MHz = temp_fix1616;
CurrParams.Xtalk_CompRate_MHz = temp_fix1616;
CurrParams.XTalk_Compens_En = 1;
}
}
}
TFP1616 VL53L0X::Get_limit_chk_val( uint16_t limit_check_id )
{ uint16_t temp16;
TFP1616 temp_fix1616;
switch (limit_check_id) {
case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* only internal computations: */
case VL53L0X_CHECKEN_SIG_REF_CLIP:
case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD:
return CurrParams.Limit_Chk_Val[limit_check_id];// need no more 'break';
case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
temp16 = Read_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT);
temp_fix1616 = FP97_TO_FP1616(temp16);
if (temp_fix1616 == 0) /* disabled: return value from memory instead*/
{ temp_fix1616 = CurrParams.Limit_Chk_Val[limit_check_id];
CurrParams.Limit_Chk_En[limit_check_id] = 0; }
else
{ CurrParams.Limit_Chk_Val[limit_check_id] = temp_fix1616;
CurrParams.Limit_Chk_En[limit_check_id] = 1; }
return temp_fix1616; // need no more 'break';
case VL53L0X_CHECKEN_SIG_RATE_MSRC:
case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
temp16 = Read_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT);
return FP97_TO_FP1616(temp16); // need no more break;
default:
ErrState = VL53L0X_ERROR_INVALID_PARAMS;
return 0;
}
}
uint8_t VL53L0X::Get_limit_chk_en(uint16_t limit_check_id )
{ if (limit_check_id >= VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
{ ErrState = VL53L0X_ERROR_INVALID_PARAMS;
return 0; }
else { return CurrParams.Limit_Chk_En[limit_check_id]; }
}
uint8_t VL53L0X::Get_Wrap_Around_Chk_En()
{ /* Now using the private state field SequenceConfig instead of reading from device:
uint8_t SequenceConfig;
SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
Set_SequenceConfig( SequenceConfig ); // checks for ErrState
if (ErrState == VL53L0X_OK) {
*/
CurrParams.Wrap_Around_Chk_En = (SequenceConfig >> 7) & 0x01;
return CurrParams.Wrap_Around_Chk_En;
// } else return 0;
}
VL53L0X_Sequence_Steps_t VL53L0X::Get_sequence_step_enables()
{ VL53L0X_Sequence_Steps_t p_sequence_steps;
/* Now using the private state field SequenceConfig instead of reading from device:
uint8_t SequenceConfig;
SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
if (ErrState == VL53L0X_OK) {
*/
p_sequence_steps.TccOn = (SequenceConfig & 0x10) >> 4;
p_sequence_steps.DssOn = (SequenceConfig & 0x08) >> 3;
p_sequence_steps.MsrcOn = (SequenceConfig & 0x04) >> 2;
p_sequence_steps.PreRangeOn = (SequenceConfig & 0x40) >> 6;
p_sequence_steps.FinalRangeOn = (SequenceConfig & 0x80) >> 7;
// }
return p_sequence_steps;
}
void VL53L0X::Set_vcsel_PPeriod(VL53L0X_Range_Phase Vcsel_Range_Phase, uint8_t vcsel_PPeriod_pclk)
{ uint8_t vcsel_period_reg;
uint8_t min_pre_vcsel_period_pclk = 12;
uint8_t max_pre_vcsel_period_pclk = 18;
uint8_t min_final_vcsel_period_pclk = 8;
uint8_t max_final_vcsel_period_pclk = 14;
uint32_t final_range_timeout_us;
uint32_t pre_range_timeout_us;
uint32_t msrc_timeout_us;
uint8_t phase_cal_int = 0;
/* Check if valid clock period requested */
if ( ((vcsel_PPeriod_pclk % 2) != 0 ) /* Value must be an even number */
||
( Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE &&
((vcsel_PPeriod_pclk < min_pre_vcsel_period_pclk)||
(vcsel_PPeriod_pclk > max_pre_vcsel_period_pclk) ) )
||
( Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE &&
(vcsel_PPeriod_pclk < min_final_vcsel_period_pclk ||
vcsel_PPeriod_pclk > max_final_vcsel_period_pclk) ) )
{ ErrState = VL53L0X_ERROR_INVALID_PARAMS;
return;}
/* Apply specific settings for the requested clock period */
if (Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE) {
/* Set phase check limits for pre-ranging*/
if (vcsel_PPeriod_pclk == 12) {
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x18);
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
} else if (vcsel_PPeriod_pclk == 14) {
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x30);
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
} else if (vcsel_PPeriod_pclk == 16) {
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x40);
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
} else if (vcsel_PPeriod_pclk == 18) {
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x50);
Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
}
} else if (Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE) {
if (vcsel_PPeriod_pclk == 8) {
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10);
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH ,0x02);
Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C);
Write_Byte(0xff,0x01);
Write_Byte(REG_ALGO_PHASECAL_LIM,0x30);
Write_Byte(0xff,0x00);
} else if (vcsel_PPeriod_pclk == 10) {
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28);
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09);
Write_Byte(0xff,0x01);
Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
Write_Byte(0xff,0x00);
} else if (vcsel_PPeriod_pclk == 12) {
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x38);
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08);
Write_Byte(0xff,0x01);
Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
Write_Byte(0xff,0x00);
} else if (vcsel_PPeriod_pclk == 14) {
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048);
Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07);
Write_Byte(0xff,0x01);
Write_Byte(REG_ALGO_PHASECAL_LIM, 0x20);
Write_Byte(0xff,0x00);
}
}
/* Re-calculate and apply timeouts,in macro periods */
if (ErrState == VL53L0X_OK) {
/* Converts the encoded VCSEL period register value into the real period in PLL clocks */
/* Flattened from procedure called Encode_vcsel_period */
vcsel_period_reg = (vcsel_PPeriod_pclk >> 1) - 1;
/* 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 (Vcsel_Range_Phase) {
case VL53L0X_VCSEL_PRE_RANGE:
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&pre_range_timeout_us);
if (ErrState == VL53L0X_OK)
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,&msrc_timeout_us);
Write_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
if (ErrState == VL53L0X_OK)
Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,pre_range_timeout_us);
if (ErrState == VL53L0X_OK)
Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,msrc_timeout_us);
PreRangeVcselPPeriod = vcsel_PPeriod_pclk;
break;
case VL53L0X_VCSEL_FINAL_RANGE:
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&final_range_timeout_us);
Write_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
if (ErrState == VL53L0X_OK)
Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,final_range_timeout_us);
FinalRangeVcselPPeriod = vcsel_PPeriod_pclk;
break;
default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
}
}
/* Finally,the timing budget is re-applied */
if (ErrState == VL53L0X_OK)
{ Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
/* Perform the phase calibration. This is needed after changing on vcsel period.
* get_data_enable = 0,restore_config = 1 */
Perf_phase_calibration(&phase_cal_int,0,1);
}
#define VL53L0X_MACRO_PERIOD_NS 3813; // = ( VL53L0X_PLL_PERIOD_PS * VL53L0X_MACRO_PERIOD_VCLKS / 1000 )
/* To convert register value into us */
uint32_t VL53L0X::Calc_timeout_us(uint16_t timeout_period_mclks,
uint8_t vcsel_period_pclks)
{
uint32_t macro_period_ns;
uint32_t actual_timeout_period_us = 0;
macro_period_ns = (uint32_t) (vcsel_period_pclks ) * VL53L0X_MACRO_PERIOD_NS;
actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
return actual_timeout_period_us;
}
void VL53L0X::Get_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
uint32_t *p_time_out_micro_secs)
{ uint8_t current_vcsel_PPeriod_p_clk;
uint8_t encoded_time_out_byte = 0;
uint32_t timeout_us = 0;
uint16_t pre_range_encoded_time_out = 0;
uint16_t msrc_time_out_m_clks;
uint16_t pre_range_time_out_m_clks;
uint16_t final_range_time_out_m_clks = 0;
uint16_t final_range_encoded_time_out;
VL53L0X_Sequence_Steps_t sequence_steps;
if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) ||
(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) ||
(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC) ) {
current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
if (ErrState == VL53L0X_OK) {
encoded_time_out_byte = Read_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP);
}
msrc_time_out_m_clks = Decode_timeout(encoded_time_out_byte);
timeout_us = Calc_timeout_us(msrc_time_out_m_clks,
current_vcsel_PPeriod_p_clk);
} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
if (ErrState == VL53L0X_OK) {
pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
timeout_us = Calc_timeout_us(pre_range_time_out_m_clks,
current_vcsel_PPeriod_p_clk);
}
} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
sequence_steps = Get_sequence_step_enables();
pre_range_time_out_m_clks = 0;
if (sequence_steps.PreRangeOn) {
current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
if (ErrState == VL53L0X_OK) {
pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
}
}
if (ErrState == VL53L0X_OK) {
current_vcsel_PPeriod_p_clk = /* Get and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
}
/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
if (ErrState == VL53L0X_OK) {
final_range_encoded_time_out = Read_Word(REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
final_range_time_out_m_clks = Decode_timeout(final_range_encoded_time_out);
}
final_range_time_out_m_clks -= pre_range_time_out_m_clks;
timeout_us = Calc_timeout_us(final_range_time_out_m_clks,current_vcsel_PPeriod_p_clk);
}
*p_time_out_micro_secs = timeout_us;
}
uint32_t VL53L0X::Get_Measure_Time_Budget_us()
{ VL53L0X_Sequence_Steps_t sequence_steps;
uint32_t p_Measure_Time_Budget_us;
uint32_t final_range_timeout_us;
uint32_t msrc_dcc_tcc_timeout_us= 2000;
uint32_t start_overhead_us = 1910;
uint32_t end_overhead_us = 960;
uint32_t msrc_overhead_us = 660;
uint32_t tcc_overhead_us = 590;
uint32_t dss_overhead_us = 690;
uint32_t pre_range_overhead_us = 660;
uint32_t final_range_overhead_us= 550;
uint32_t pre_range_timeout_us = 0;
if (ErrState != VL53L0X_OK) {return 0; } // do nothing while in Error State!!!!
/* Start and end overhead times always present */
p_Measure_Time_Budget_us = start_overhead_us + end_overhead_us;
sequence_steps = Get_sequence_step_enables();
if (sequence_steps.TccOn || sequence_steps.MsrcOn || sequence_steps.DssOn)
{ Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us);
if (ErrState == VL53L0X_OK) {
if (sequence_steps.TccOn)
{ p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + tcc_overhead_us; }
if (sequence_steps.DssOn) {
p_Measure_Time_Budget_us += 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
} else if (sequence_steps.MsrcOn) {
p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + msrc_overhead_us;
}
}
}
if ( (ErrState == VL53L0X_OK) && sequence_steps.PreRangeOn) {
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us);
p_Measure_Time_Budget_us += pre_range_timeout_us + pre_range_overhead_us;
}
if (ErrState == VL53L0X_OK) {
if (sequence_steps.FinalRangeOn) {
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, &final_range_timeout_us);
p_Measure_Time_Budget_us += (final_range_timeout_us + final_range_overhead_us);
}
}
if (ErrState == VL53L0X_OK)
{ CurrParams.Measure_Time_Budget_us = p_Measure_Time_Budget_us; }
return p_Measure_Time_Budget_us;
}
VL53L0X_DeviceParams_t VL53L0X::Get_device_parameters()
{ VL53L0X_DeviceParams_t device_params = {0};
int i;
if (ErrState != VL53L0X_OK) {return device_params; } // do nothing while in Error State!!!!
device_params.DeviceMode = CurrParams.DeviceMode;
device_params.XTalk_Compens_En = 0;
device_params.Offset_Cal_um = Get_Offset_Cal_um();
Get_measure_period_ms(&(device_params.Measure_Period_ms));
if (ErrState == VL53L0X_OK)
Get_Xtalk_CompRate_MHz(&(device_params.Xtalk_CompRate_MHz));
if (ErrState == VL53L0X_OK) {
for (i = 0; i < VL53L0X_CHECKEN_NUMBER_OF_CHECKS; i++)
{/* get first the values,then the enables. GetLimitCheckValue will
modify the enable flags */
if (ErrState == VL53L0X_OK)
{ device_params.Limit_Chk_Val[i] = Get_limit_chk_val(i); }
else { break; }
if (ErrState == VL53L0X_OK)
{ device_params.Limit_Chk_En[i]= Get_limit_chk_en(i);}
else { break; }
}
}
if (ErrState == VL53L0X_OK) {
device_params.Wrap_Around_Chk_En = Get_Wrap_Around_Chk_En();}
/* Need to be done at the end as it uses VCSELPPeriod */
if (ErrState == VL53L0X_OK) {
device_params.Measure_Time_Budget_us = Get_Measure_Time_Budget_us(); }
return device_params;
}
void VL53L0X::Set_limit_chk_val(uint16_t limit_check_id, TFP1616 limit_chk_val)
{ /* first verify that the ID is within bounds .. */
if (limit_check_id>=VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
{ ErrState = VL53L0X_ERROR_INVALID_PARAMS; return; }
/* Under all other circumstances store value in local array: */
CurrParams.Limit_Chk_Val[limit_check_id] = limit_chk_val;
/* in addition, if enabled, then write the external ones also to the Registers */
if (CurrParams.Limit_Chk_En[ limit_check_id ])
switch (limit_check_id) {
case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
FP1616_TO_FP97(limit_chk_val));
break;
case VL53L0X_CHECKEN_SIG_RATE_MSRC:
case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
Write_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
FP1616_TO_FP97(limit_chk_val));
break;
} // switch
}
void VL53L0X::Get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
{ uint8_t intStat;
intStat = Read_Byte(REG_RESULT_INTERRUPT_STATUS);
*p_interrupt_mask_status = intStat & 0x07;
if (intStat & 0x18) { ErrState = VL53L0X_ERROR_RANGE_ERROR; }
}
uint8_t VL53L0X::Get_Measurement_Ready()
{ uint8_t sys_range_status_register;
uint32_t interrupt_mask;
if (GpioFunctionality == REG_SYSINT_GPIO_NEW_SAMPLE_READY)
{ Get_interrupt_mask_status(&interrupt_mask);
if (interrupt_mask == REG_SYSINT_GPIO_NEW_SAMPLE_READY)
{ return 1; } else { return 0; }
}
else
{ sys_range_status_register = Read_Byte(REG_RESULT_RANGE_STATUS);
if ( ( ErrState == VL53L0X_OK ) & (sys_range_status_register & 0x01) )
{ return 1; } else { return 0; }
}
}
void VL53L0X::Polling_delay()
{
// do nothing VL53L0X_OsDelay();
}
void VL53L0X::Poll_Measure_Completion()
{ uint8_t new_data_ready;
uint32_t loop_nb = 0;
if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
new_data_ready = Get_Measurement_Ready();
while ((ErrState==0) && (new_data_ready != 1) )
{ Polling_delay();
new_data_ready = Get_Measurement_Ready();
if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) ErrState=VL53L0X_ERROR_TIME_OUT;
} // while ;
}
/* Group Device Interrupt Functions */
void VL53L0X::Clear_interrupt_mask(uint32_t interrupt_mask)
{ uint8_t loop_count = 0;
uint8_t byte;
if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
/* clear bit 0 range interrupt,bit 1 error interrupt */
do {
Write_Byte(REG_SYSINT_CLEAR,0x01);
Write_Byte(REG_SYSINT_CLEAR,0x00);
byte = Read_Byte(REG_RESULT_INTERRUPT_STATUS);
if (loop_count++ > 3) {ErrState =VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;}
} while (((byte & 0x07) != 0x00) && (ErrState == VL53L0X_OK));
}
void VL53L0X::Perf_single_Ref_calibration(uint8_t vhv_init_byte)
{ if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_START_STOP | vhv_init_byte);
Poll_Measure_Completion();
Clear_interrupt_mask(0);
Write_Byte(REG_SYSRANGE_START,0x00);
}
void VL53L0X::Ref_calibration_io(uint8_t read_not_write,
uint8_t vhv_settings,uint8_t phase_cal,
uint8_t *p_vhv_settings,uint8_t *p_phase_cal,
const uint8_t vhv_enable,const uint8_t phase_enable)
{ uint8_t phase_calint = 0;
/* Read VHV from device */
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0xFF,0x00);
if (read_not_write) {
if (vhv_enable ) { *p_vhv_settings = Read_Byte(0xCB); }
if (phase_enable) { phase_calint = Read_Byte(0xEE); }
}
else {
if (vhv_enable ) { Write_Byte(0xCB,vhv_settings); }
if (phase_enable) { Register_BitMask(0xEE,0x80,phase_cal); }
}
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
*p_phase_cal = (uint8_t)(phase_calint & 0xEF);
}
void VL53L0X::Perf_vhv_calibration(uint8_t *p_vhv_settings,
const uint8_t get_data_enable, const uint8_t restore_config)
{ uint8_t orig_sequence_config = 0;
uint8_t vhv_settings = 0;
uint8_t phase_cal = 0;
uint8_t phase_cal_int = 0;
/* store the value of the sequence config,
* this will be reset before the end of the function */
orig_sequence_config = SequenceConfig;
/* Run VHV */
Set_SequenceConfig( 0x01 );
Perf_single_Ref_calibration(0x40);
/* Read VHV from device */
if ((ErrState == VL53L0X_OK) && (get_data_enable == 1))
{ Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
p_vhv_settings,&phase_cal_int, 1,0); }
else { *p_vhv_settings = 0; }
if (restore_config) { /* restore the previous Sequence Config */
Set_SequenceConfig( orig_sequence_config ); } // checks for ErrState
}
void VL53L0X::Perf_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable,
const uint8_t restore_config)
{ uint8_t orig_sequence_config;
uint8_t vhv_settings = 0;
uint8_t phase_cal = 0;
uint8_t vhv_settingsint;
if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
/* store the value of the sequence config, this will be reset before the end of the function */
orig_sequence_config = SequenceConfig;
/* Run PhaseCal: */
Set_SequenceConfig( 0x02 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
Perf_single_Ref_calibration(0x0);
/* Read PhaseCal from device */
if ((ErrState == VL53L0X_OK) && (get_data_enable == 1))
{ Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
&vhv_settingsint,p_phase_cal, 0,1); }
else { *p_phase_cal = 0; }
if (restore_config) { /* restore the previous Sequence Config */
Set_SequenceConfig( orig_sequence_config ); }
}
void VL53L0X::Perf_Ref_calibration(uint8_t *p_vhv_settings,
uint8_t *p_phase_cal, uint8_t get_data_enable)
{ uint8_t orig_sequence_config;
/* store the value of the sequence config,
* this will be reset before the end of the function */
orig_sequence_config = SequenceConfig;
/* In the following function we don't save the config to optimize
* writes on device. Config is saved and restored only once. */
Perf_vhv_calibration(p_vhv_settings,get_data_enable,0);
Perf_phase_calibration(p_phase_cal,get_data_enable,0);
/* restore the previous Sequence Config */
Set_SequenceConfig( orig_sequence_config ); // sets REG_SYSTEM_SEQUENCE_CONFIG
}
void VL53L0X::Get_Next_Good_SPAD(uint8_t good_SPAD_array[],uint32_t size,
uint32_t curr,int32_t *p_next)
{ uint32_t start_index;
uint32_t fine_offset;
uint32_t c_SPADS_per_byte = 8;
uint32_t coarse_index;
uint32_t fine_index;
uint8_t data_byte;
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. */
*p_next = -1;
start_index = curr / c_SPADS_per_byte;
fine_offset = curr % c_SPADS_per_byte;
for (coarse_index = start_index; ((coarse_index < size) && !success);
coarse_index++) {
fine_index = 0;
data_byte = good_SPAD_array[coarse_index];
if (coarse_index == start_index) {
/* locate the bit position of the provided current
* SPAD bit before iterating */
data_byte >>= fine_offset;
fine_index = fine_offset;
}
while (fine_index < c_SPADS_per_byte) {
if ((data_byte & 0x1) == 1) {
success = 1;
*p_next = coarse_index * c_SPADS_per_byte + fine_index;
break;
}
data_byte >>= 1;
fine_index++;
}
}
}
void VL53L0X::Enable_SPAD_bit(uint8_t SPAD_array[],uint32_t size,uint32_t SPAD_index)
{ uint32_t c_SPADS_per_byte = 8;
uint32_t coarse_index;
uint32_t fine_index;
coarse_index = SPAD_index / c_SPADS_per_byte;
fine_index = SPAD_index % c_SPADS_per_byte;
if (coarse_index >= size) { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; }
else { SPAD_array[coarse_index] |= (1 << fine_index); }
}
void VL53L0X::Enable_Ref_SPADS( uint8_t aperture_SPADS, uint8_t good_SPAD_array[],
uint8_t SPAD_array[], uint32_t size, uint32_t start, uint32_t offset,
uint32_t SPAD_count, uint32_t *p_last_SPAD )
{ uint32_t index;
uint32_t i;
int32_t next_good_SPAD = offset;
uint32_t current_SPAD;
uint8_t check_SPAD_array[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.
*/
current_SPAD = offset;
for (index = 0; index < SPAD_count; index++) {
Get_Next_Good_SPAD(good_SPAD_array,size,current_SPAD, &next_good_SPAD);
if (next_good_SPAD == -1)
{ ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
break; }
/* Confirm that the next good SPAD is non-aperture */
if (Is_ApertureSPAD(start + next_good_SPAD) != aperture_SPADS) {
/* if we can't get the required number of good aperture
* SPADS from the current quadrant then this is an error */
ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
break;}
current_SPAD = (uint32_t)next_good_SPAD;
Enable_SPAD_bit(SPAD_array,size,current_SPAD);
current_SPAD++;
}
*p_last_SPAD = current_SPAD;
if (ErrState == VL53L0X_OK)
{ I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPAD_array,6); } // set_Ref_SPAD_map()
if (ErrState == VL53L0X_OK) {
// Get the ref_SPAD_map from the device
I2c_Read(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,check_SPAD_array,6);
/* Compare SPAD maps. If not equal report error. */
i = 0;
while (i < size) {
if (SPAD_array[i] != check_SPAD_array[i]) {
ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
i++;
}
}
}
void VL53L0X::Set_device_mode(VL53L0X_DeviceModes device_mode)
{ if (ErrState != VL53L0X_OK) {return; } // no reaction while in Error State!!!!
switch (device_mode) {
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 */
CurrParams.DeviceMode = device_mode;
break;
default: /* Unsupported mode */
ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
}
void VL53L0X::Set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 threshold_low,
TFP1616 threshold_high)
{ uint16_t threshold16;
/* no dependency on DeviceMode for FlightSense */
/* Need to divide by 2 because the FW will apply a x2 */
threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
Write_Word(REG_SYSTEM_THRESH_LOW,threshold16);
/* Need to divide by 2 because the FW will apply a x2 */
threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
Write_Word(REG_SYSTEM_THRESH_HIGH,threshold16);
}
void VL53L0X::Get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 *p_threshold_low,
TFP1616 *p_threshold_high)
{ uint16_t threshold16;
/* no dependency on DeviceMode for FlightSense */
threshold16 = Read_Word(REG_SYSTEM_THRESH_LOW);
/* Need to multiply by 2 because the FW will apply a x2 */
*p_threshold_low = (TFP1616)((0x00fff & threshold16) << 17);
if (ErrState == VL53L0X_OK) {
threshold16 = Read_Word(REG_SYSTEM_THRESH_HIGH);
/* Need to multiply by 2 because the FW will apply a x2 */
*p_threshold_high = (TFP1616)((0x00fff & threshold16) << 17);
}
}
void VL53L0X::Load_tuning_settings(uint8_t *p_tuning_setting_buffer)
{ int i;
int index;
uint8_t msb;
uint8_t lsb;
uint8_t select_param;
uint8_t number_of_writes;
uint8_t address;
uint8_t local_buffer[4]; /* max */
uint16_t temp16;
index = 0;
while ((*(p_tuning_setting_buffer + index) != 0) &&
(ErrState == VL53L0X_OK)) {
number_of_writes = *(p_tuning_setting_buffer + index);
index++;
if (number_of_writes == 0xFF) {
/* internal parameters */
select_param = *(p_tuning_setting_buffer + index);
index++;
switch (select_param) {
case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
SigmaEstRefArray = temp16;
break;
case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
SigmaEstEffPulseWidth = temp16;
break;
case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
SigmaEstEffAmbWidth = temp16;
break;
case 3: /* uint16_t targetRefRate -> 2 bytes */
msb = *(p_tuning_setting_buffer + index);
index++;
lsb = *(p_tuning_setting_buffer + index);
index++;
temp16 = VL53L0X_MAKEUINT16(lsb,msb);
targetRefRate = temp16;
break;
default: /* invalid parameter */
ErrState = VL53L0X_ERROR_INVALID_PARAMS;
}
} else if (number_of_writes <= 4) {
address = *(p_tuning_setting_buffer + index);
index++;
for (i = 0; i < number_of_writes; i++) {
local_buffer[i] = *(p_tuning_setting_buffer + index);
index++;
}
I2c_Write(address,local_buffer,number_of_writes);
} else {
ErrState = VL53L0X_ERROR_INVALID_PARAMS;
}
}
}
void VL53L0X::Check_and_load_interrupt_settings(uint8_t start_not_stopflag)
{ uint8_t interrupt_config;
TFP1616 threshold_low;
TFP1616 threshold_high;
if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
interrupt_config = GpioFunctionality;
if ((interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_LOW ) ||
(interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_HIGH) ||
(interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_OUT )) {
Get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
&threshold_low,&threshold_high);
if (((threshold_low > 255 * 65536) || (threshold_high > 255 * 65536)) &&
(ErrState == VL53L0X_OK))
{ if (start_not_stopflag != 0)
{Load_tuning_settings(InterruptThresholdSettings); }
else
{Write_Byte(0xFF,0x04);
Write_Byte(0x70,0x00);
Write_Byte(0xFF,0x00);
Write_Byte(0x80,0x00);
}
}
}
}
void VL53L0X::Start_Measurement()
{ VL53L0X_DeviceModes device_mode;
uint8_t byte;
uint8_t start_stop_byte = REG_SYSRANGE_MODE_START_STOP;
uint32_t loop_nb;
if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
/* Get Current DeviceMode */
device_mode = CurrParams.DeviceMode;
Write_Byte(0x80,0x01);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0x91,StopVariable);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
Write_Byte(0x80,0x00);
switch (device_mode) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
Write_Byte(REG_SYSRANGE_START,0x01);
byte = start_stop_byte;
if (ErrState == VL53L0X_OK) {
/* Wait until start bit has been cleared */
loop_nb = 0;
do {
if (loop_nb > 0)
byte = Read_Byte(REG_SYSRANGE_START);
loop_nb = loop_nb + 1;
} while (((byte & start_stop_byte) == start_stop_byte)
&& (ErrState == VL53L0X_OK)
&& (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
ErrState = VL53L0X_ERROR_TIME_OUT;
}
}
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
/* Back-to-back mode, Check if need to apply interrupt settings */
Check_and_load_interrupt_settings(1);
Write_Byte(REG_SYSRANGE_START,REG_SYSRANGE_MODE_BACKTOBACK);
Set_Current_State( VL53L0X_STATE_RUNNING );
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
/* Continuous mode; Check if need to apply interrupt settings */
Check_and_load_interrupt_settings(1);
Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_TIMED);
Set_Current_State( VL53L0X_STATE_RUNNING );
break;
default:
/* Selected mode not supported */
ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
}
/* Group Device Measurement Functions */
void VL53L0X::Perf_single_measurement()
{ VL53L0X_DeviceModes device_mode;
if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
/* Get Current DeviceMode */
device_mode = CurrParams.DeviceMode;
/* Start immediately to run a single ranging measurement in case of
* single ranging or single histogram */
if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {Start_Measurement();}
Poll_Measure_Completion();
/* Change Device State in case of single ranging or single histogram */
if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
{ Set_Current_State( VL53L0X_STATE_IDLE ); }
}
TFP1616 VL53L0X::Get_total_xtalk_rate(TRangeResults *p_ranging_results)
{ TFP1616 total_xtalk_MHz;
// CurrParams.XTalk_Compens_En was Get_xtalk_compensation_enable
if ( (ErrState == VL53L0X_OK) & (CurrParams.XTalk_Compens_En ) )
{ /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
total_xtalk_MHz = p_ranging_results->EffectiveSPADRtnCount *
CurrParams.Xtalk_CompRate_MHz;
/* FixPoint0824 >> 8 = FixPoint1616 */
return (total_xtalk_MHz + 0x80) >> 8;
}
else { return 0; }
}
void VL53L0X::Get_total_SIG_rate(TRangeResults *p_ranging_results,
TFP1616 *p_total_SIG_rate_mcps)
{ TFP1616 total_xtalk_MHz;
*p_total_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
total_xtalk_MHz = Get_total_xtalk_rate(p_ranging_results);
if (ErrState == VL53L0X_OK) { *p_total_SIG_rate_mcps += total_xtalk_MHz;}
}
/* To convert ms into register value */
uint32_t VL53L0X::Calc_timeout_mclks(uint32_t timeout_period_us,
uint8_t vcsel_period_pclks)
{ uint32_t macro_period_ns;
macro_period_ns = (uint32_t)(vcsel_period_pclks) * VL53L0X_MACRO_PERIOD_NS;
return (uint32_t)(((timeout_period_us * 1000)
+ (macro_period_ns / 2)) / macro_period_ns);
}
uint32_t 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;
}
void VL53L0X::Calc_dmax(TFP1616 total_SIG_rate_mcps,
TFP1616 total_corr_SIG_rate_mcps,
TFP1616 pw_mult,
uint32_t sigma_estimate_p1,
TFP1616 sigma_estimate_p2,
uint32_t peak_vcsel_duration_us,
uint32_t *pd_max_mm)
{ const uint32_t c_sigma_limit = 18;
const TFP1616 c_SIG_limit = 0x4000; /* 0.25 */
const TFP1616 c_sigma_est_Ref = 0x00000042; /* 0.001 */
const uint32_t c_amb_eff_width_sigma_est_ns = 6;
const uint32_t c_amb_eff_width_d_max_ns = 7;
uint32_t dmax_cal_range_mm;
TFP1616 dmax_cal_SIG_rate_rtn_mcps;
TFP1616 min_SIG_needed;
TFP1616 min_SIG_needed_p1;
TFP1616 min_SIG_needed_p2;
TFP1616 min_SIG_needed_p3;
TFP1616 min_SIG_needed_p4;
TFP1616 sigma_limit_tmp;
TFP1616 sigma_est_sq_tmp;
TFP1616 signal_limit_tmp;
TFP1616 signal_at0_mm;
TFP1616 dmax_dark;
TFP1616 dmax_ambient;
TFP1616 dmax_dark_tmp;
TFP1616 sigma_est_p2_tmp;
uint32_t signal_rate_temp_mcps;
dmax_cal_range_mm = DmaxCalRangeMilliMeter;
dmax_cal_SIG_rate_rtn_mcps = DmaxCalSignalRateRtnMHz;
/* uint32 * FixPoint1616 = FixPoint1616 */
signal_at0_mm = dmax_cal_range_mm * dmax_cal_SIG_rate_rtn_mcps;
/* FixPoint1616 >> 8 = FixPoint2408 */
signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
signal_at0_mm *= dmax_cal_range_mm;
min_SIG_needed_p1 = 0;
if (total_corr_SIG_rate_mcps > 0) {
/* Shift by 10 bits to increase resolution prior to the division */
signal_rate_temp_mcps = total_SIG_rate_mcps << 10;
/* Add rounding value prior to division */
min_SIG_needed_p1 = signal_rate_temp_mcps + (total_corr_SIG_rate_mcps / 2);
/* FixPoint0626/FixPoint1616 = FixPoint2210 */
min_SIG_needed_p1 /= total_corr_SIG_rate_mcps;
/* Apply a factored version of the speed of light.
Correction to be applied at the end */
min_SIG_needed_p1 *= 3;
/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
min_SIG_needed_p1 *= min_SIG_needed_p1;
/* FixPoint1220 >> 16 = FixPoint2804 */
min_SIG_needed_p1 = (min_SIG_needed_p1 + 0x8000) >> 16;
}
min_SIG_needed_p2 = pw_mult * sigma_estimate_p1;
/* FixPoint1616 >> 16 = uint32 */
min_SIG_needed_p2 = (min_SIG_needed_p2 + 0x8000) >> 16;
/* uint32 * uint32 = uint32 */
min_SIG_needed_p2 *= min_SIG_needed_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. */
sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
c_amb_eff_width_sigma_est_ns;
sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
if (sigma_est_p2_tmp > 0xffff) {
min_SIG_needed_p3 = 0xfff00000;
} else {
/* DMAX uses a different ambient width from sigma,so apply correction.
* Perform division before multiplication to prevent overflow. */
sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
c_amb_eff_width_sigma_est_ns;
sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
/* FixPoint1616 >> 16 = uint32 */
min_SIG_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
min_SIG_needed_p3 *= min_SIG_needed_p3;
}
/* FixPoint1814 / uint32 = FixPoint1814 */
sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
sigma_limit_tmp *= sigma_limit_tmp;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sigma_est_sq_tmp = c_sigma_est_Ref * c_sigma_est_Ref;
/* FixPoint3232 >> 4 = FixPoint0428 */
sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
sigma_limit_tmp -= sigma_est_sq_tmp;
/* uint32_t * FixPoint0428 = FixPoint0428 */
min_SIG_needed_p4 = 4 * 12 * sigma_limit_tmp;
/* FixPoint0428 >> 14 = FixPoint1814 */
min_SIG_needed_p4 = (min_SIG_needed_p4 + 0x2000) >> 14;
/* uint32 + uint32 = uint32 */
min_SIG_needed = (min_SIG_needed_p2 + min_SIG_needed_p3);
/* uint32 / uint32 = uint32 */
min_SIG_needed += (peak_vcsel_duration_us / 2);
min_SIG_needed /= peak_vcsel_duration_us;
/* uint32 << 14 = FixPoint1814 */
min_SIG_needed <<= 14;
/* FixPoint1814 / FixPoint1814 = uint32 */
min_SIG_needed += (min_SIG_needed_p4 / 2);
min_SIG_needed /= min_SIG_needed_p4;
/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
min_SIG_needed *= min_SIG_needed_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.
*/
min_SIG_needed = (min_SIG_needed + 500) / 1000;
min_SIG_needed <<= 4;
min_SIG_needed = (min_SIG_needed + 500) / 1000;
/* FixPoint1616 >> 8 = FixPoint2408 */
signal_limit_tmp = (c_SIG_limit + 0x80) >> 8;
/* FixPoint2408/FixPoint2408 = uint32 */
if (signal_limit_tmp != 0) {
dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
/ signal_limit_tmp;
} else { dmax_dark_tmp = 0; }
dmax_dark = ISQRT(dmax_dark_tmp);
/* FixPoint2408/FixPoint2408 = uint32 */
if (min_SIG_needed != 0)
{ dmax_ambient = (signal_at0_mm + min_SIG_needed / 2) / min_SIG_needed; }
else { dmax_ambient = 0; }
dmax_ambient = ISQRT(dmax_ambient);
*pd_max_mm = dmax_dark;
if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; }
}
void VL53L0X::Calc_sigma_estimate(TRangeResults *p_ranging_results,
TFP1616 *p_sigma_estimate, uint32_t *p_dmax_mm)
{ /* Expressed in 100ths of a ns,i.e. centi-ns */
const uint32_t c_pulse_effective_width_centi_ns = 800;
/* Expressed in 100ths of a ns,i.e. centi-ns */
const uint32_t c_ambient_effective_width_centi_ns = 600;
const TFP1616 c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
const TFP1616 c_sigma_est_max = 0x028F87AE;
const TFP1616 c_sigma_est_rtn_max = 0xF000;
const TFP1616 c_amb_to_SIG_ratio_max = 0xF0000000 /
c_ambient_effective_width_centi_ns;
/* Time Of Flight per mm (6.6 pico secs) */
const TFP1616 c_tof_per_mm_ps = 0x0006999A;
const uint32_t c_16bit_rounding_param = 0x00008000;
const TFP1616 c_max_xtalk_kcps = 0x00320000;
const uint32_t c_pll_period_ps = 1655;
uint32_t vcsel_total_events_rtn;
uint32_t final_range_timeout_micro_secs;
uint32_t pre_range_timeout_micro_secs;
uint32_t final_range_integration_time_milli_secs;
TFP1616 sigma_estimate_p1;
TFP1616 sigma_estimate_p2;
TFP1616 sigma_estimate_p3;
TFP1616 delta_t_ps;
TFP1616 pw_mult;
TFP1616 sigma_est_rtn;
TFP1616 sigma_estimate;
TFP1616 xtalk_correction;
TFP1616 ambient_rate_kcps;
TFP1616 peak_SIG_rate_kcps;
TFP1616 xtalk_comp_rate_mcps;
uint32_t xtalk_comp_rate_kcps;
TFP1616 diff1_mcps;
TFP1616 diff2_mcps;
TFP1616 sqr1;
TFP1616 sqr2;
TFP1616 sqr_sum;
TFP1616 sqrt_result_centi_ns;
TFP1616 sqrt_result;
TFP1616 total_SIG_rate_mcps;
TFP1616 corrected_SIG_rate_mcps;
TFP1616 sigma_est_Ref;
uint32_t vcsel_width;
uint32_t final_range_macro_pclks;
uint32_t pre_range_macro_pclks;
uint32_t peak_vcsel_duration_us;
uint8_t final_range_vcsel_pclks;
uint8_t pre_range_vcsel_pclks;
/*! \addtogroup calc_sigma_estimate
* @{
* Estimates the range sigma */
xtalk_comp_rate_mcps = CurrParams.Xtalk_CompRate_MHz;
/* We work in kcps rather than mcps as this helps keep within the
* confines of the 32 Fix1616 type. */
ambient_rate_kcps = (p_ranging_results->AmbientRateRtnMHz * 1000) >> 16;
corrected_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
Get_total_SIG_rate(p_ranging_results,&total_SIG_rate_mcps);
xtalk_comp_rate_mcps = Get_total_xtalk_rate(p_ranging_results);
/* Signal rate measurement provided by device is the
* peak signal rate,not average. */
peak_SIG_rate_kcps = (total_SIG_rate_mcps * 1000);
peak_SIG_rate_kcps = (peak_SIG_rate_kcps + 0x8000) >> 16;
xtalk_comp_rate_kcps = xtalk_comp_rate_mcps * 1000;
if (xtalk_comp_rate_kcps > c_max_xtalk_kcps)
{ xtalk_comp_rate_kcps = c_max_xtalk_kcps; }
if (ErrState == VL53L0X_OK) {
/* Calculate final range macro periods */
final_range_timeout_micro_secs = FinalRangeTimeoutMicroSecs;
final_range_vcsel_pclks = FinalRangeVcselPPeriod;
final_range_macro_pclks = Calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks);
/* Calculate pre-range macro periods */
pre_range_timeout_micro_secs = PreRangeTimeoutMicroSecs;
pre_range_vcsel_pclks = PreRangeVcselPPeriod;
pre_range_macro_pclks = Calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks);
vcsel_width = 3;
if (final_range_vcsel_pclks == 8) { vcsel_width = 2; }
peak_vcsel_duration_us = vcsel_width * 2048 *
(pre_range_macro_pclks + final_range_macro_pclks);
peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
peak_vcsel_duration_us *= c_pll_period_ps;
peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
/* Fix1616 >> 8 = Fix2408 */
total_SIG_rate_mcps = (total_SIG_rate_mcps + 0x80) >> 8;
/* Fix2408 * uint32 = Fix2408 */
vcsel_total_events_rtn = total_SIG_rate_mcps * peak_vcsel_duration_us;
/* Fix2408 >> 8 = uint32 */
vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
/* Fix2408 << 8 = Fix1616 = */
total_SIG_rate_mcps <<= 8;
}
if (ErrState != VL53L0X_OK) { return ; }
if (peak_SIG_rate_kcps == 0) {
*p_sigma_estimate = c_sigma_est_max;
SigmaEstimate = c_sigma_est_max;
*p_dmax_mm = 0;
} else {
if (vcsel_total_events_rtn < 1) {vcsel_total_events_rtn = 1; }
sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_SIG_rate_kcps;
if (sigma_estimate_p2 > c_amb_to_SIG_ratio_max)
/* Clip to prevent overflow. Will ensure safe max result. */
{ sigma_estimate_p2 = c_amb_to_SIG_ratio_max; }
sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
sigma_estimate_p3 = 2 * ISQRT(vcsel_total_events_rtn * 12);
/* uint32 * FixPoint1616 = FixPoint1616 */
delta_t_ps = p_ranging_results->RangeMilliMeter * c_tof_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 = (((peak_SIG_rate_kcps << 16) - 2 * xtalk_comp_rate_kcps) + 500) / 1000;
/* vcselRate + xtalkCompRate */
diff2_mcps = ((peak_SIG_rate_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!
xtalk_correction = diff1_mcps / diff2_mcps;
/* FixPoint2408 << 8 = FixPoint1616 */
xtalk_correction <<= 8;
if (p_ranging_results->RangeStatus != 0)
{ pw_mult = 1 << 16; }
else {
/* FixPoint1616/uint32 = FixPoint1616 */
pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
/* FixPoint1616 * FixPoint1616 = FixPoint3232,however both
* values are small enough such that32 bits will not be
* exceeded. */
pw_mult *= ((1 << 16) - xtalk_correction);
/* (FixPoint3232 >> 16) = FixPoint1616 */
pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
pw_mult += (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. */
pw_mult >>= 1;
/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
pw_mult = pw_mult * pw_mult;
/* (FixPoint3430 >> 14) = Fix1616 */
pw_mult >>= 14;
}
/* FixPoint1616 * uint32 = FixPoint1616 */
sqr1 = pw_mult * sigma_estimate_p1;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr1 = (sqr1 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr1 *= sqr1;
sqr2 = sigma_estimate_p2;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr2 = (sqr2 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr2 *= sqr2;
/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
sqr_sum = sqr1 + sqr2;
/* SQRT(FixPoin6400) = FixPoint3200 */
sqrt_result_centi_ns = ISQRT(sqr_sum);
/* (FixPoint3200 << 16) = FixPoint1616 */
sqrt_result_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 */
sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
sigma_estimate_p3);
sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
/* Add 5000 before dividing by 10000 to ensure rounding. */
sigma_est_rtn = (sigma_est_rtn + 5000) / 10000;
if (sigma_est_rtn > c_sigma_est_rtn_max)
/* Clip to prevent overflow. Will ensure safe max result. */
{ sigma_est_rtn = c_sigma_est_rtn_max; }
final_range_integration_time_milli_secs =
(final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
* sqrt(FixPoint1616/int) = FixPoint2408) */
sigma_est_Ref =
ISQRT((c_dflt_final_range_integration_time_milli_secs +
final_range_integration_time_milli_secs / 2) /
final_range_integration_time_milli_secs);
/* FixPoint2408 << 8 = FixPoint1616 */
sigma_est_Ref <<= 8;
sigma_est_Ref = (sigma_est_Ref + 500) / 1000;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr1 = sigma_est_rtn * sigma_est_rtn;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr2 = sigma_est_Ref * sigma_est_Ref;
/* sqrt(FixPoint3232) = FixPoint1616 */
sqrt_result = 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. */
sigma_estimate = 1000 * sqrt_result;
if ((peak_SIG_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
(sigma_estimate > c_sigma_est_max)) {
sigma_estimate = c_sigma_est_max; }
*p_sigma_estimate = (uint32_t)(sigma_estimate);
SigmaEstimate = *p_sigma_estimate;
Calc_dmax(total_SIG_rate_mcps,
corrected_SIG_rate_mcps,
pw_mult,
sigma_estimate_p1,
sigma_estimate_p2,
peak_vcsel_duration_us,
p_dmax_mm);
}
}
void VL53L0X::Get_Device_range_status(uint8_t device_range_status,
TFP1616 signal_rate,
uint16_t effective_SPAD_rtn_count,
TRangeResults *p_ranging_results,
uint8_t *p_Device_range_status)
{ uint8_t none_flag;
uint8_t sigma_limitflag = 0;
uint8_t signal_Ref_clipflag = 0;
uint8_t range_ignore_thresholdflag = 0;
uint8_t sigma_limit_chk_en = 0;
uint8_t signal_rate_final_range_limit_chk_en = 0;
uint8_t signal_Ref_clip_limit_chk_en = 0;
uint8_t range_ignore_threshold_chk_en = 0;
TFP1616 sigma_estimate;
TFP1616 sigma_limit_value;
TFP1616 signal_Ref_clip_value;
TFP1616 range_ignore_threshold;
TFP1616 signal_rate_per_SPAD;
uint8_t device_range_status_internal = 0;
uint8_t temp8;
uint32_t dmax_mm = 0;
/* 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 DeviceRangeStatus. */
device_range_status_internal = ((device_range_status & 0x78) >> 3);
if ( device_range_status_internal == 0 ||
device_range_status_internal == 5 ||
device_range_status_internal == 7 ||
device_range_status_internal == 12 ||
device_range_status_internal == 13 ||
device_range_status_internal == 14 ||
device_range_status_internal == 15 )
{ none_flag = 1; }
else { none_flag = 0; }
/* Check if Sigma limit is enabled,if yes then do comparison with limit
* value and put the result back into pDeviceRangeStatus. */
if (ErrState == VL53L0X_OK)
{ sigma_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); }
if ((sigma_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) {
/* compute the Sigma and check with limit */
Calc_sigma_estimate(p_ranging_results, &sigma_estimate, &dmax_mm);
if (ErrState == VL53L0X_OK)
{ p_ranging_results->RangeDMaxMilliMeter = dmax_mm; }
if (ErrState == VL53L0X_OK)
{ sigma_limit_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE);
if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value))
{ sigma_limitflag = 1; }/* Limit Fail */
}
}
/* Check if Signal ref clip limit is enabled,if yes then do comparison
* with limit value and put the result back into pDeviceRangeStatus. */
if (ErrState == VL53L0X_OK)
{signal_Ref_clip_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_REF_CLIP);}
if ((signal_Ref_clip_limit_chk_en != 0) && (ErrState == VL53L0X_OK))
{ signal_Ref_clip_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIG_REF_CLIP);
/* Read LastSignalRefMcps from device */
Write_Byte(0xFF,0x01);
LastSignalRefMcps = FP97_TO_FP1616( Read_Word(REG_RESULT_PEAK_SIG_RATE_REF));
Write_Byte(0xFF,0x00);
if ((signal_Ref_clip_value > 0) && (LastSignalRefMcps > signal_Ref_clip_value)) \
{ signal_Ref_clipflag = 1; /* Limit Fail */ }
}
/* Check if Signal ref clip limit is enabled,if yes then do comparison
* with limit value and put the result back into pDeviceRangeStatus.
* EffectiveSPADRtnCount has a format 8.8
* If (Return signal rate < (1.5 x Xtalk x number of SPADS)) : FAIL */
if (ErrState == VL53L0X_OK)
{ range_ignore_threshold_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); }
if ((range_ignore_threshold_chk_en != 0) && (ErrState == VL53L0X_OK))
{/* Compute the signal rate per SPAD */
if (effective_SPAD_rtn_count == 0) { signal_rate_per_SPAD = 0; }
else { signal_rate_per_SPAD =
(TFP1616)((256 * signal_rate) / effective_SPAD_rtn_count); }
range_ignore_threshold=Get_limit_chk_val(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD);
if ((range_ignore_threshold > 0) && (signal_rate_per_SPAD < range_ignore_threshold)) {
/* Limit Fail add 2^6 to range ErrState */
range_ignore_thresholdflag = 1;
}
}
if (ErrState == VL53L0X_OK) {
if (none_flag == 1) {
*p_Device_range_status = 255; /* NONE */
} else if (device_range_status_internal == 1 ||
device_range_status_internal == 2 ||
device_range_status_internal == 3) {
*p_Device_range_status = 5; /* HW fail */
} else if (device_range_status_internal == 6 ||
device_range_status_internal == 9) {
*p_Device_range_status = 4; /* Phase fail */
} else if (device_range_status_internal == 8 ||
device_range_status_internal == 10 ||
signal_Ref_clipflag == 1) {
*p_Device_range_status = 3; /* Min range */
} else if (device_range_status_internal == 4 ||
range_ignore_thresholdflag == 1) {
*p_Device_range_status = 2; /* Signal Fail */
} else if (sigma_limitflag == 1) {
*p_Device_range_status = 1; /* Sigma Fail */
} else {
*p_Device_range_status = 0; /* Range Valid */
}
}
/* DMAX only relevant during range error */
if (*p_Device_range_status == 0) { p_ranging_results->RangeDMaxMilliMeter = 0; }
/* fill the Limit Check ErrState */
signal_rate_final_range_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE);
if (ErrState == VL53L0X_OK) {
if ((sigma_limit_chk_en == 0) || (sigma_limitflag == 1))
{ temp8 = 1; } else { temp8 = 0; }
CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = temp8;
if ((device_range_status_internal == 4) || (signal_rate_final_range_limit_chk_en == 0))
{ temp8 = 1; } else { temp8 = 0; }
CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE] = temp8;
if ((signal_Ref_clip_limit_chk_en == 0) || (signal_Ref_clipflag == 1))
{ temp8 = 1; } else { temp8 = 0; }
CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_REF_CLIP] = temp8;
if ((range_ignore_threshold_chk_en == 0) || (range_ignore_thresholdflag == 1))
{ temp8 = 1; } else { temp8 = 0;}
CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = temp8;
}
}
void VL53L0X::Get_ranging_results(TRangeResults *p_ranging_results)
{ uint8_t device_range_status;
uint8_t range_fractional_enable;
uint8_t Device_range_status;
uint8_t xtalk_compensation_enable;
uint16_t ambient_rate;
TFP1616 signal_rate;
uint16_t Xtalk_CompRate_MHz;
uint16_t effective_SPAD_rtn_count;
uint16_t tmpuint16;
uint16_t xtalk_range_milli_meter;
uint16_t linearity_corrective_gain;
uint8_t localBuffer[12];
TRangeResults last_range_data_buffer;
if (ErrState != VL53L0X_OK) { return; } // Do nothing while in error state
/* use multi read even if some registers are not useful,result will
* be more efficient start reading at REG_RESULT_RANGE_STATUS = 0x14
* end reading at 0x21 dec33 total 14 bytes to read */
I2c_Read(REG_RESULT_RANGE_STATUS, localBuffer,12);
if (ErrState == VL53L0X_OK) {
p_ranging_results->ZoneId = 0; /* Only one zone */
p_ranging_results->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 */
p_ranging_results->MeasurementTimeUsec = 0;
signal_rate = FP97_TO_FP1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6]));
/* peak_SIG_count_rate_rtn_mcps */
p_ranging_results->SignalRateRtnMHz = signal_rate;
ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]);
p_ranging_results->AmbientRateRtnMHz = FP97_TO_FP1616(ambient_rate);
effective_SPAD_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]);
/* EffectiveSPADRtnCount is 8.8 format */
p_ranging_results->EffectiveSPADRtnCount = effective_SPAD_rtn_count;
device_range_status = localBuffer[0];
/* Get Linearity Corrective Gain */
linearity_corrective_gain = LinearityCorrectiveGain;
/* Get ranging configuration */
range_fractional_enable = RangeFractionalEnable;
if (linearity_corrective_gain != 1000) {
tmpuint16 = (uint16_t)((linearity_corrective_gain
* tmpuint16 + 500) / 1000);
/* Implement Xtalk */
Xtalk_CompRate_MHz = CurrParams.Xtalk_CompRate_MHz;
xtalk_compensation_enable = CurrParams.XTalk_Compens_En;
if (xtalk_compensation_enable) {
if ((signal_rate - ((Xtalk_CompRate_MHz
* effective_SPAD_rtn_count) >> 8)) <= 0) {
if (range_fractional_enable) { xtalk_range_milli_meter = 8888;
} else { xtalk_range_milli_meter = 8888 << 2; }
} else {
xtalk_range_milli_meter = (tmpuint16 * signal_rate)
/ (signal_rate - ((Xtalk_CompRate_MHz * effective_SPAD_rtn_count) >> 8));
}
tmpuint16 = xtalk_range_milli_meter;
}
}
if (range_fractional_enable) {
p_ranging_results->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2);
p_ranging_results->RangeFractionalPart =
(uint8_t)((tmpuint16 & 0x03) << 6);
} else {
p_ranging_results->RangeMilliMeter = tmpuint16;
p_ranging_results->RangeFractionalPart = 0;
}
/* For a standard definition of RangeStatus,this should
* return 0 in case of good result after a ranging
* The range ErrState depends on the device so call a device
* specific function to obtain the right ErrState. */
Get_Device_range_status(device_range_status,signal_rate,effective_SPAD_rtn_count,
p_ranging_results,&Device_range_status);
if (ErrState == VL53L0X_OK) {
p_ranging_results->RangeStatus = Device_range_status;
}
}
if (ErrState == VL53L0X_OK) {
/* Copy last read data into device+ buffer */
last_range_data_buffer = LastRangeMeasure;
last_range_data_buffer.RangeMilliMeter =
p_ranging_results->RangeMilliMeter;
last_range_data_buffer.RangeFractionalPart =
p_ranging_results->RangeFractionalPart;
last_range_data_buffer.RangeDMaxMilliMeter =
p_ranging_results->RangeDMaxMilliMeter;
last_range_data_buffer.MeasurementTimeUsec =
p_ranging_results->MeasurementTimeUsec;
last_range_data_buffer.SignalRateRtnMHz =
p_ranging_results->SignalRateRtnMHz;
last_range_data_buffer.AmbientRateRtnMHz =
p_ranging_results->AmbientRateRtnMHz;
last_range_data_buffer.EffectiveSPADRtnCount =
p_ranging_results->EffectiveSPADRtnCount;
last_range_data_buffer.RangeStatus =
p_ranging_results->RangeStatus;
LastRangeMeasure = last_range_data_buffer;
}
}
void VL53L0X::Perf_single_ranging_measurement(
TRangeResults *p_ranging_results)
{ if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
/* This function will do a complete single ranging Here we fix the mode! */
Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING);
Perf_single_measurement();
Get_ranging_results(p_ranging_results);
Clear_interrupt_mask(0);
}
uint16_t VL53L0X::Get_Perf_Ref_SIG_measurement()
{ TRangeResults ranging_results;
uint8_t orig_sequence_config;
uint16_t Ref_SIG_rate ;
/* store the value of the sequence config,
* this will be reset before the end of the function*/
orig_sequence_config = SequenceConfig;
/* This function performs a reference signal rate measurement.*/
Set_SequenceConfig( 0xC0 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
Perf_single_ranging_measurement(&ranging_results);
Write_Byte(0xFF,0x01);
Ref_SIG_rate = Read_Word(REG_RESULT_PEAK_SIG_RATE_REF);
Write_Byte(0xFF,0x00);
/* restore the previous Sequence Config */
Set_SequenceConfig( orig_sequence_config ); // resets REG_SYSTEM_SEQUENCE_CONFIG
return Ref_SIG_rate;
}
void VL53L0X::Perf_Ref_SPAD_management(uint32_t *ref_SPAD_count,
uint8_t *is_aperture_SPADS)
{ uint8_t last_SPAD_array[6];
uint8_t start_select = 0xB4;
uint32_t minimum_SPAD_count = 3;
uint32_t max_SPAD_count = 44;
uint32_t current_SPAD_index = 0;
uint32_t last_SPAD_index = 0;
int32_t next_good_SPAD = 0;
uint16_t target_Ref_rate = 0x0A00; /* 20 MHz in 9:7 format */
uint16_t peak_SIG_rate_Ref;
uint32_t need_apt_SPADS = 0;
uint32_t index = 0;
uint32_t REF_SPAD_ARRAY_SIZE = 6;
uint32_t signal_rate_diff = 0;
uint32_t last_SIG_rate_diff = 0;
uint8_t complete = 0;
uint8_t vhv_settings = 0;
uint8_t phase_cal = 0;
uint32_t ref_SPAD_count_int = 0;
uint8_t is_aperture_SPADS_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. */
target_Ref_rate = 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 < REF_SPAD_ARRAY_SIZE; index++) {
SPADData.RefSPADEnables[index] = 0; }
Write_Byte(0xFF,0x01);
Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
Write_Byte(0xFF,0x00);
Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT,start_select);
Write_Byte(REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0);
/* Perform ref calibration */
if (ErrState == VL53L0X_OK)
{Perf_Ref_calibration(&vhv_settings, &phase_cal, 0);}
if (ErrState == VL53L0X_OK) {
/* Enable Minimum NON-APERTURE SPADS */
current_SPAD_index = 0;
last_SPAD_index = current_SPAD_index;
need_apt_SPADS = 0;
Enable_Ref_SPADS(need_apt_SPADS,
SPADData.RefGoodSPADMap,
SPADData.RefSPADEnables,
REF_SPAD_ARRAY_SIZE,
start_select,
current_SPAD_index,
minimum_SPAD_count,
&last_SPAD_index);
}
if (ErrState == VL53L0X_OK) {
current_SPAD_index = last_SPAD_index;
peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate))
{ /* Signal rate measurement too high, switch to APERTURE SPADS */
for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++)
{ SPADData.RefSPADEnables[index] = 0; }
/* Increment to the first APERTURE SPAD */
while ((Is_ApertureSPAD(start_select + current_SPAD_index)
== 0) && (current_SPAD_index < max_SPAD_count))
{ current_SPAD_index++; }
need_apt_SPADS = 1;
Enable_Ref_SPADS(need_apt_SPADS,
SPADData.RefGoodSPADMap,
SPADData.RefSPADEnables,
REF_SPAD_ARRAY_SIZE,
start_select,
current_SPAD_index,
minimum_SPAD_count,
&last_SPAD_index);
if (ErrState == VL53L0X_OK) {
current_SPAD_index = last_SPAD_index;
peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate))
{ /* 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. */
is_aperture_SPADS_int = 1;
ref_SPAD_count_int = minimum_SPAD_count;
}
}
} else { need_apt_SPADS = 0;}
}
if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref < target_Ref_rate))
{ /* 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.*/
is_aperture_SPADS_int = need_apt_SPADS;
ref_SPAD_count_int = minimum_SPAD_count;
memcpy(last_SPAD_array,SPADData.RefSPADEnables, REF_SPAD_ARRAY_SIZE);
last_SIG_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
complete = 0;
while (!complete) {
Get_Next_Good_SPAD(SPADData.RefGoodSPADMap,
REF_SPAD_ARRAY_SIZE,current_SPAD_index, &next_good_SPAD);
if (next_good_SPAD == -1) {
ErrState = 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_ApertureSPAD((uint32_t)start_select + next_good_SPAD) !=
need_apt_SPADS) {
/* At this point we have enabled the maximum number of Aperture SPADS. */
complete = 1;
break;
}
(ref_SPAD_count_int)++;
current_SPAD_index = next_good_SPAD;
Enable_SPAD_bit(SPADData.RefSPADEnables,
REF_SPAD_ARRAY_SIZE,current_SPAD_index);
if (ErrState == VL53L0X_OK) {
current_SPAD_index++;
/* Proceed to apply the additional SPAD and perform measurement. */
I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPADData.RefSPADEnables,6); //Set_Ref_SPAD_map
}
if (ErrState != VL53L0X_OK) { break; }
peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
if (ErrState != VL53L0X_OK) { break; }
signal_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
if (peak_SIG_rate_Ref > target_Ref_rate) {
/* Select the SPAD map that provides the
* measurement closest to the target rate,
* either above or below it. */
if (signal_rate_diff > last_SIG_rate_diff) {
/* Previous SPAD map produced a closer measurement,so choose this. */
I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, last_SPAD_array,6); // Set_Ref_SPAD_map();
memcpy(SPADData.RefSPADEnables,last_SPAD_array,REF_SPAD_ARRAY_SIZE);
(ref_SPAD_count_int)--;
}
complete = 1;
} else {
/* Continue to add SPADS */
last_SIG_rate_diff = signal_rate_diff;
memcpy(last_SPAD_array, SPADData.RefSPADEnables,REF_SPAD_ARRAY_SIZE);
}
} /* while */
}
if (ErrState == VL53L0X_OK) {
*ref_SPAD_count = ref_SPAD_count_int;
*is_aperture_SPADS = is_aperture_SPADS_int;
RefSPADSInitialised = 1;
ReferenceSPADCount = (uint8_t)(*ref_SPAD_count);
ReferenceSPADType = *is_aperture_SPADS;
}
}
void VL53L0X::Set_Reference_SPADS(uint32_t count,uint8_t is_aperture_SPADS)
{ uint32_t current_SPAD_index = 0;
uint8_t start_select = 0xB4;
uint32_t max_SPAD_count = 44;
uint32_t last_SPAD_index;
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.*/
Write_Byte(0xFF,0x01);
Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
Write_Byte(0xFF,0x00);
Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT, start_select);
for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
SPADData.RefSPADEnables[index] = 0; }
if (is_aperture_SPADS) {
/* Increment to the first APERTURE SPAD */
while ((Is_ApertureSPAD(start_select + current_SPAD_index) == 0) &&
(current_SPAD_index < max_SPAD_count)) {
current_SPAD_index++;
}
}
Enable_Ref_SPADS(is_aperture_SPADS, SPADData.RefGoodSPADMap,
SPADData.RefSPADEnables,
REF_SPAD_ARRAY_SIZE,
start_select,
current_SPAD_index,
count,
&last_SPAD_index);
if (ErrState == VL53L0X_OK) {
RefSPADSInitialised = 1;
ReferenceSPADCount = (uint8_t)(count);
ReferenceSPADType = is_aperture_SPADS;
}
}
void VL53L0X::Set_GPIO_config(VL53L0X_DeviceModes device_mode,
TGPIO_Func functionality, VL53L0X_InterruptPolarity polarity)
{ uint8_t pol_data;
if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
{ pol_data = 0x00;} else { pol_data = 0x10;}
switch ( device_mode ) {
case VL53L0X_DEVICEMODE_GPIO_DRIVE:
Write_Byte(REG_GPIO_HV_MUX_ACTIVE_HIGH,pol_data);
break;
case VL53L0X_DEVICEMODE_GPIO_OSC:
Write_Byte(0xff,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0xff,0x00);
Write_Byte(0x80,0x01);
Write_Byte(0x85,0x02);
Write_Byte(0xff,0x04);
Write_Byte(0xcd,0x00);
Write_Byte(0xcc,0x11);
Write_Byte(0xff,0x07);
Write_Byte(0xbe,0x00);
Write_Byte(0xff,0x06);
Write_Byte(0xcc,0x09);
Write_Byte(0xff,0x00);
Write_Byte(0xff,0x01);
Write_Byte(0x00,0x00);
break;
default:
if (functionality>GPIO_FUNC_NEW_MEASURE_READY)
{ ErrState = VL53L0X_ERROR_GPIO_FUNC_NOT_SUPPORTED; }
else { Write_Byte(REG_SYSINT_CONFIG_GPIO,functionality); }
if (ErrState == VL53L0X_OK)
{ Register_BitMask(REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,pol_data); }
if (ErrState == VL53L0X_OK)
{GpioFunctionality = functionality; }
Clear_interrupt_mask(0);
} // switch
} // Set_GPIO_config
/* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */
uint16_t VL53L0X::Encode_timeout(uint32_t timeout_macro_clks)
{ uint16_t encoded_timeout = 0;
uint16_t ms_byte = 0;
if (timeout_macro_clks > 0) {
timeout_macro_clks = timeout_macro_clks - 1;
while ((timeout_macro_clks & 0xFFFFFF00) > 0) {
timeout_macro_clks = timeout_macro_clks >> 1;
ms_byte++;
} // while
encoded_timeout = (ms_byte << 8) + (uint16_t)(timeout_macro_clks & 0x000000FF);
}
return encoded_timeout;
}
void VL53L0X::Set_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
uint32_t timeout_micro_secs)
{ uint8_t current_vcsel_PPeriod_p_clk;
uint8_t msrc_encoded_time_out;
uint16_t pre_range_encoded_time_out;
uint16_t pre_range_time_out_m_clks;
uint16_t msrc_range_time_out_m_clks;
uint32_t final_range_time_out_m_clks;
uint16_t final_range_encoded_time_out;
VL53L0X_Sequence_Steps_t sequence_steps;
switch (sequence_step_id) {
case VL53L0X_SEQUENCESTEP_TCC:
case VL53L0X_SEQUENCESTEP_DSS:
case VL53L0X_SEQUENCESTEP_MSRC:
current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
if (ErrState == VL53L0X_OK) {
msrc_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
(uint8_t)current_vcsel_PPeriod_p_clk);
if (msrc_range_time_out_m_clks > 256) { msrc_encoded_time_out = 255;}
else {msrc_encoded_time_out = (uint8_t)msrc_range_time_out_m_clks - 1; }
LastEncodedTimeout = msrc_encoded_time_out;
}
Write_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP,msrc_encoded_time_out);
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
pre_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
(uint8_t)current_vcsel_PPeriod_p_clk);
pre_range_encoded_time_out = Encode_timeout(pre_range_time_out_m_clks);
LastEncodedTimeout = pre_range_encoded_time_out;
Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out);
if (ErrState == VL53L0X_OK)
{PreRangeTimeoutMicroSecs=timeout_micro_secs; }
break;
case 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.*/
sequence_steps = Get_sequence_step_enables();
pre_range_time_out_m_clks = 0;
if (sequence_steps.PreRangeOn)
{ current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
if (ErrState == VL53L0X_OK)
{ pre_range_encoded_time_out = Read_Word(0x51);
pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
}
}
/* Calculate FINAL RANGE Timeout in Macro Periode (MCLKS) and add PRE-RANGE value */
if (ErrState == VL53L0X_OK)
{ current_vcsel_PPeriod_p_clk /* Get and converts the VCSEL period register into actual clock periods */
= ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
if (ErrState == VL53L0X_OK)
{ final_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
(uint8_t) current_vcsel_PPeriod_p_clk);
final_range_time_out_m_clks += pre_range_time_out_m_clks;
final_range_encoded_time_out = Encode_timeout(final_range_time_out_m_clks);
Write_Word(0x71,final_range_encoded_time_out);
}
if (ErrState == VL53L0X_OK)
{ FinalRangeTimeoutMicroSecs = timeout_micro_secs; }
break;
default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
} // switch (sequence_step_id)
}
void VL53L0X::Set_Measure_Time_Budget_us (uint32_t Measure_Time_Budget_us)
{ uint32_t final_range_timing_budget_us;
VL53L0X_Sequence_Steps_t sequence_steps;
uint32_t msrc_dcc_tcc_timeout_us= 2000;
uint32_t start_overhead_us = 1910;
uint32_t end_overhead_us = 960;
uint32_t msrc_overhead_us = 660;
uint32_t tcc_overhead_us = 590;
uint32_t dss_overhead_us = 690;
uint32_t pre_range_overhead_us = 660;
uint32_t final_range_overhead_us= 550;
uint32_t pre_range_timeout_us = 0;
uint32_t c_min_timing_budget_us = 20000;
uint32_t sub_timeout = 0;
if (Measure_Time_Budget_us < c_min_timing_budget_us)
{ ErrState = VL53L0X_ERROR_INVALID_PARAMS;
return ; }
final_range_timing_budget_us = Measure_Time_Budget_us -
(start_overhead_us + end_overhead_us);
sequence_steps = Get_sequence_step_enables();
if (ErrState == VL53L0X_OK &&
(sequence_steps.TccOn ||
sequence_steps.MsrcOn ||
sequence_steps.DssOn)) {
/* TCC,MSRC and DSS all share the same timeout */
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,
&msrc_dcc_tcc_timeout_us);
/* Subtract the TCC,MSRC and DSS timeouts if they are enabled. */
if (ErrState != VL53L0X_OK) {return ; }
/* TCC */
if (sequence_steps.TccOn) {
sub_timeout = msrc_dcc_tcc_timeout_us + tcc_overhead_us;
if (sub_timeout < final_range_timing_budget_us) {
final_range_timing_budget_us -= sub_timeout;
} else { /* Requested timeout too big. */
ErrState = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (ErrState != VL53L0X_OK) {return; }
/* DSS */
if (sequence_steps.DssOn)
{ sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
if (sub_timeout < final_range_timing_budget_us)
{ final_range_timing_budget_us -= sub_timeout; }
else { /* Requested timeout too big. */
ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
}
else if (sequence_steps.MsrcOn) /* MSRC */
{ sub_timeout = msrc_dcc_tcc_timeout_us + msrc_overhead_us;
if (sub_timeout < final_range_timing_budget_us)
{ final_range_timing_budget_us -= sub_timeout; }
else /* Requested timeout too big. */
{ ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
}
}
if (ErrState != VL53L0X_OK) {return; }
if (sequence_steps.PreRangeOn) {
/* Subtract the Pre-range timeout if enabled. */
Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
&pre_range_timeout_us);
sub_timeout = pre_range_timeout_us + pre_range_overhead_us;
if (sub_timeout < final_range_timing_budget_us) {
final_range_timing_budget_us -= sub_timeout;
} else {
/* Requested timeout too big. */
ErrState = VL53L0X_ERROR_INVALID_PARAMS;
}
}
if (ErrState == VL53L0X_OK && sequence_steps.FinalRangeOn)
{ final_range_timing_budget_us -= final_range_overhead_us;
/* 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.
*/
Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
final_range_timing_budget_us);
CurrParams.Measure_Time_Budget_us = Measure_Time_Budget_us;
}
}
const uint8_t SEQUENCESTEP_MASK[] =
{ 0x10, //VL53L0X_SEQUENCESTEP_TCC = 0
0x28, //VL53L0X_SEQUENCESTEP_DSS = 1
0x04, //VL53L0X_SEQUENCESTEP_MSRC= 2
0x40, //VL53L0X_SEQUENCESTEP_PRE_RANGE= 3
0x80}; //VL53L0X_SEQUENCESTEP_FINAL_RANGE = 4
void VL53L0X::Set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id,
uint8_t sequence_step_enabled)
{ uint8_t new_config = 0;
// SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
// instead of reading from the device, use the SequenceConfig local data field!!
if (sequence_step_enabled == 1) /* Enable requested sequence step */
{ new_config = SequenceConfig | SEQUENCESTEP_MASK[sequence_step_id]; }
else /* Disable requested sequence step */
{ new_config = SequenceConfig & (0xff - SEQUENCESTEP_MASK[sequence_step_id]); }
if (new_config != SequenceConfig) { /* Apply New Setting */
Set_SequenceConfig( new_config );
if (ErrState == VL53L0X_OK) /* Recalculate timing budget */
{ Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
} // if (new_config != sequence_config)
}
void VL53L0X::Set_limit_chk_en(uint16_t limit_check_id, uint8_t limit_chk_en)
{ TFP1616 temp_fix1616 = 0;
if (limit_chk_en!=0) {limit_chk_en=1;} // make sure we only have 0 or 1 as values!!!
switch (limit_check_id) {
case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* internal computation: */
case VL53L0X_CHECKEN_SIG_REF_CLIP: /* internal computation: */
case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: /* internal computation: */
CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en;
break;
case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
temp_fix1616 = limit_chk_en * CurrParams.Limit_Chk_Val[limit_check_id];
Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
FP1616_TO_FP97(temp_fix1616));
break;
case VL53L0X_CHECKEN_SIG_RATE_MSRC:
Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xFE, (1-limit_chk_en)<< 1);
break;
case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEF, (1-limit_chk_en)<< 4);
break;
default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
} // switch
if (ErrState == VL53L0X_OK) { CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en; }
}
void VL53L0X::Static_init()
{ VL53L0X_DeviceParams_t new_curr_parameters;
uint8_t *p_tuning_setting_buffer;
uint16_t tempword = 0;
uint8_t tempbyte = 0;
uint32_t count = 0;
uint8_t is_aperture_SPADS = 0;
uint32_t ref_SPAD_count = 0;
uint8_t aperture_SPADS = 0;
uint8_t vcsel_PPeriod_pclk;
uint32_t seq_timeout_micro_secs;
/* set the ref SPAD from NVM */
count = (uint32_t)ReferenceSPADCount;
aperture_SPADS = ReferenceSPADType;
/* NVM value invalid */
if ((aperture_SPADS > 1) || ((aperture_SPADS == 1) && (count > 32)) ||
((aperture_SPADS == 0) && (count > 12)))
{ Perf_Ref_SPAD_management(&ref_SPAD_count, &is_aperture_SPADS); }
else
{ Set_Reference_SPADS(count,aperture_SPADS); }
/* Initialize tuning settings buffer to prevent compiler warning. */
p_tuning_setting_buffer = DefaultTuningSettings;
if (ErrState == VL53L0X_OK) {
if (UseInternalTuningSettings == 0)
{ p_tuning_setting_buffer = pTuningSettingsPointer; }
else
{ p_tuning_setting_buffer = DefaultTuningSettings; }
}
if (ErrState == VL53L0X_OK)
{ Load_tuning_settings(p_tuning_setting_buffer); }
/* Set interrupt config to new sample ready */
if (ErrState == VL53L0X_OK)
{ Set_GPIO_config(0,GPIO_FUNC_NEW_MEASURE_READY,VL53L0X_INTERRUPTPOLARITY_LOW); }
Write_Byte(0xFF,0x01);
tempword = Read_Word(0x84);
Write_Byte(0xFF,0x00);
if (ErrState == VL53L0X_OK)
{ OscFrequencyMHz=FP412_TO_FP1616(tempword); }
/* After static init,some device parameters may be changed, so update them */
new_curr_parameters = Get_device_parameters();
if (ErrState == VL53L0X_OK) { tempbyte = Read_Byte(REG_SYSTEM_RANGE_CONFIG); }
if (ErrState == VL53L0X_OK) { RangeFractionalEnable = (tempbyte & 1); }
if (ErrState == VL53L0X_OK) { CurrParams = new_curr_parameters; }
/* read the sequence config and save it */
Set_SequenceConfig( Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG) ); // checks for ErrState
/* Disable MSRC and TCC by default */
if (ErrState == VL53L0X_OK)
{ Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0); }
if (ErrState == VL53L0X_OK)
{ Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0); }
/* Set State to standby */
Set_Current_State( VL53L0X_STATE_IDLE) ;
/* Store pre-range vcsel period */
if (ErrState == VL53L0X_OK)/* Gets and converts the VCSEL period register into actual clock periods */
{ vcsel_PPeriod_pclk = (Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
if ( ErrState == VL53L0X_OK)
{ PreRangeVcselPPeriod = vcsel_PPeriod_pclk; }
/* Store final-range vcsel period */
if (ErrState == VL53L0X_OK)
{ vcsel_PPeriod_pclk /* Get and convert the VCSEL period register into actual clock periods */
= ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
if (ErrState == VL53L0X_OK)
{ FinalRangeVcselPPeriod = vcsel_PPeriod_pclk; }
/* Store pre-range timeout */
if (ErrState == VL53L0X_OK)
{ Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_PRE_RANGE,
&seq_timeout_micro_secs); }
if (ErrState == VL53L0X_OK)
{ PreRangeTimeoutMicroSecs = seq_timeout_micro_secs; }
/* Store final-range timeout */
if (ErrState == VL53L0X_OK)
{ Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_FINAL_RANGE,
&seq_timeout_micro_secs);}
if (ErrState == VL53L0X_OK)
{ FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs; }
}
void VL53L0X::Stop_Measurement()
{ Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_SINGLESHOT);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0x91,0x00);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
Set_Current_State( VL53L0X_STATE_IDLE );
/* Check if need to apply interrupt settings */
Check_and_load_interrupt_settings(0);
}
uint8_t VL53L0X::Get_Stop_Completed()
{ uint8_t Abyte = 0;
Write_Byte(0xFF,0x01);
Abyte = Read_Byte(0x04);
Write_Byte(0xFF,0x0);
if ((ErrState == VL53L0X_OK) & (Abyte == 0))
{ Write_Byte(0x80,0x01);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0x91,StopVariable);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
Write_Byte(0x80,0x00);
}
return Abyte;
}
void VL53L0X::Wait_Measurement_Ready()
{ uint32_t loop_nb = 0;
// Wait until it finished, or loopo count reached = avoids deadlock
while ( !Get_Measurement_Ready() & (ErrState == VL53L0X_OK) )
{ if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP)
{ ErrState = VL53L0X_ERROR_TIME_OUT;}
else { Polling_delay(); }
} // while ends
}
void VL53L0X::Wait_Stop_Completed()
{ uint32_t loop_nb = 0;
// Wait until Stop_Completed, or loopo count reached = avoids deadlock
while ( (ErrState == VL53L0X_OK) & !Get_Stop_Completed() )
{ if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP)
{ ErrState = VL53L0X_ERROR_TIME_OUT;}
else { Polling_delay(); }
} // while ends
}
void VL53L0X::Range_meas_int_continuous_mode(void (*fptr)(void))
{ Stop_Measurement(); // it is safer to do this while sensor is stopped
Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH);
if (ErrState==VL53L0X_OK) {
Attach_interrupt_measure_detection_irq(fptr);
Enable_interrupt_measure_detection_irq(); }
Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS);
// NB: return value was previously only passed to logging macro,but did not get passed back
if (ErrState==VL53L0X_OK) { Range_start_continuous_mode(); }
}
VL53L0X_Error VL53L0X::Start_Measurement(TOperatingMode operating_mode, void (*fptr)(void))
{ uint8_t VhvSettings;
uint8_t PhaseCal;
// *** from mass market cube expansion v1.1,ranging with satellites.
// default settings,for normal range.
TFP1616 signalLimit = (TFP1616)(0.25 * 65536);
TFP1616 sigmaLimit = (TFP1616)(18 * 65536);
uint32_t timingBudget = 33000;
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
switch (operating_mode) {
case op_INT:
if (_gpio1Int == NULL) { ErrState=1; return ErrState; }
Stop_Measurement(); // it is safer to do this while sensor is stopped
Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH);
if (ErrState == VL53L0X_OK)
{ Attach_interrupt_measure_detection_irq(fptr);
Enable_interrupt_measure_detection_irq(); }
Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS);
// NB: return value was previously only passed to logging macro, but did not get passed back
// Setup in continuous ranging mode
Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
Start_Measurement();
break;
case op_single_shot_poll:
// singelshot,polled ranging; no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
// Enable/Disable Sigma and Signal check
if (ErrState == VL53L0X_OK)
{ Set_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,1); }
if (ErrState == VL53L0X_OK)
{ Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); }
/* Ranging configuration */
// *** from mass market cube expansion v1.1,ranging with satellites.
// switch(rangingConfig) {
// case LONG_RANGE:
signalLimit = (TFP1616)(0.1 * 65536);
sigmaLimit = (TFP1616)(60 * 65536);
timingBudget = 33000;
preRangeVcselPeriod = 18;
finalRangeVcselPeriod = 14;
/* break;
case HIGH_ACCURACY:
signalLimit = (TFP1616)(0.25*65536);
sigmaLimit = (TFP1616)(18*65536);
timingBudget = 200000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
case HIGH_SPEED:
signalLimit = (TFP1616)(0.25*65536);
sigmaLimit = (TFP1616)(32*65536);
timingBudget = 20000;
preRangeVcselPeriod = 14;
finalRangeVcselPeriod = 10;
break;
default:
debug_printf("Not Supported");
}
*/
if (ErrState == VL53L0X_OK)
{ Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,signalLimit);}
if (ErrState == VL53L0X_OK)
{ Set_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,sigmaLimit);}
if (ErrState == VL53L0X_OK)
{ Set_Measure_Time_Budget_us(timingBudget);}
if (ErrState == VL53L0X_OK)
{ Set_vcsel_PPeriod(VL53L0X_VCSEL_PRE_RANGE,preRangeVcselPeriod); }
if (ErrState == VL53L0X_OK)
{ Set_vcsel_PPeriod(VL53L0X_VCSEL_FINAL_RANGE,finalRangeVcselPeriod);}
if (ErrState == VL53L0X_OK)
{ Perf_Ref_calibration(&VhvSettings,&PhaseCal,1); }
break;
case op_poll: // Setup in continuous ranging mode
Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
Start_Measurement();
} // switch
return ErrState;
}
VL53L0X_Error VL53L0X::Stop_Measurement(TOperatingMode operating_mode)
{ if ((ErrState == VL53L0X_OK) &
(operating_mode == op_INT || operating_mode == op_poll) )
{ // only stop if in one of the continuous modes !!!!
Stop_Measurement();
Wait_Stop_Completed();
Clear_interrupt_mask( REG_SYSINT_GPIO_NEW_SAMPLE_READY);
}
return ErrState;
}
TRangeResults VL53L0X::Handle_irq(TOperatingMode operating_mode)
{ TRangeResults RangeResults;
RangeResults = Get_Measurement(operating_mode);
Enable_interrupt_measure_detection_irq();
return RangeResults;
}
/****************** Private device functions *************************/
void VL53L0X::Wait_read_strobe()
{ uint32_t loop_nb = 0;
Write_Byte(0x83,0x00); // set strobe register to 0
/* polling while no error, no strobe, and not reached max number of loop to avoid deadlock*/
while ((ErrState == VL53L0X_OK) && (Read_Byte(0x83) == 0x00) )
{ if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP)
{ ErrState = VL53L0X_ERROR_TIME_OUT; } }
Write_Byte(0x83,0x01); // set strobe register back to 1 'manually'
}
/******************************************************************************/
/****************** Small Service and wrapper functions **********************/
/******************************************************************************/
uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10,REF_ARRAY_SPAD_5,
REF_ARRAY_SPAD_0,REF_ARRAY_SPAD_5 };
uint32_t VL53L0X::Decode_timeout(uint16_t encoded_timeout)
{ /*Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 */
return ((uint32_t) (encoded_timeout & 0x00FF)
<< (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
}
uint8_t VL53L0X::Is_ApertureSPAD(uint32_t SPAD_index)
{ /* This function reports if a given SPAD index is an aperture SPAD by
* deriving the quadrant = SPAD_index >> 6. */
if (refArrayQuadrants[SPAD_index >> 6] == REF_ARRAY_SPAD_0)
{ return 0; } else { return 1; }
}
/******************************************************************************/
/****************** Write and read functions from I2C *************************/
/******************************************************************************/
uint32_t VL53L0X::Get_NVM_DWord(uint8_t NVM_Address)
{ Write_Byte(0x94,NVM_Address);
Wait_read_strobe();
return Read_DWord(0x90);
}
uint16_t VL53L0X::Get_NVM_Word(uint8_t NVM_Address)
{ Write_Byte(0x94,NVM_Address);
Wait_read_strobe();
return Read_Word(0x90);
}
uint8_t VL53L0X::Get_NVM_Byte(uint8_t NVM_Address)
{ Write_Byte(0x94,NVM_Address);
Wait_read_strobe();
return Read_Byte(0x90);
}
void VL53L0X::Write_Byte(uint8_t index, uint8_t data)
{ I2c_Write(index,&data,1); }
void VL53L0X::Write_Word(uint8_t index,uint16_t data)
{ uint8_t buffer[2];
buffer[0] = data >> 8;
buffer[1] = data & 0x00FF;
I2c_Write(index,(uint8_t *)buffer,2);
}
void VL53L0X::Write_DWord(uint8_t index, uint32_t data)
{ uint8_t buffer[4];
buffer[0] = (data >> 24) & 0xFF;
buffer[1] = (data >> 16) & 0xFF;
buffer[2] = (data >> 8) & 0xFF;
buffer[3] = (data >> 0) & 0xFF;
I2c_Write(index,(uint8_t *)buffer,4);
}
uint8_t VL53L0X::Read_Byte(uint8_t index)
{ uint8_t result;
I2c_Read(index,&result,1);
return result;
}
uint16_t VL53L0X::Read_Word(uint8_t index)
{ uint8_t buffer[2] = {0,0};
I2c_Read(index, &buffer[0], 2);
return (buffer[0] << 8) + buffer[1];
}
uint32_t VL53L0X::Read_DWord(uint8_t index)
{ uint8_t buffer[4] = {0,0,0,0};
I2c_Read(index,buffer,4);
return (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
}
void VL53L0X::Register_BitMask(uint8_t index,uint8_t and_mask,uint8_t or_mask)
{ uint8_t buffer = 0;
/* read data direct onto buffer */
I2c_Read(index,&buffer,1);
if (ErrState==VL53L0X_OK) {
buffer = (buffer & and_mask) | or_mask;
I2c_Write(index,&buffer,(uint8_t)1);
}
}
/**
* @brief Writes a buffer towards the I2C peripheral device.
* @param RegisterAddr specifies the internal address register
* @param p_data pointer to the byte-array data to send
* where to start writing to (must be correctly masked).
* @param NumByteToWrite number of bytes to be written.
* @note On some devices if NumByteToWrite is greater
* than one, the RegisterAddr must be masked correctly! */
void VL53L0X::I2c_Write( uint8_t RegisterAddr, uint8_t *p_data,
uint16_t NumByteToWrite )
{ int ret;
uint8_t tmp[TEMP_BUF_SIZE];
if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State!!!!
if (NumByteToWrite >= TEMP_BUF_SIZE)
{ErrState = VL53L0X_ERROR_I2C_BUF_OVERFLOW; return; };
/* First, send device address. Then, send data and terminate with STOP condition */
tmp[0] = RegisterAddr;
memcpy(tmp+1, p_data, NumByteToWrite);
ret = _dev_i2c->write(I2cDevAddr, (const char*)tmp, NumByteToWrite+1, false);
if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; }
}
/**@brief Reads a buffer from the I2C peripheral device.
* @param pBuffer pointer to the byte-array to read data in to
* @param I2cDevAddr specifies the peripheral device slave address.
* @param RegisterAddr specifies the internal address register
* where to start reading from (must be correctly masked).
* @param NumByteToRead number of bytes to be read, maximum VL53L0X_MAX_I2C_XFER_SIZE
* @note On some devices if NumByteToWrite is greater
* than one, the RegisterAddr must be masked correctly!
*/
void VL53L0X::I2c_Read(uint8_t RegisterAddr, uint8_t *p_data,
uint16_t NumByteToRead)
{ int ret;
if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State, return value undefined!!!!
/* Send device address, without STOP condition */
ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true);
if(!ret) /* Read data, with STOP condition */
{ ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); }
if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; }
}
/******************************************************************************/