Tarek Lule
Condensed Version of Public VL53L0X
VL53L0X.cpp
- Committer:
- sepp_nepp
- Date:
- 2019-04-08
- Revision:
- 11:c6f95a42d4d7
- Parent:
- 10:cd251e0fc2fd
- Child:
- 12:aa177f0e4c10
File content as of revision 11:c6f95a42d4d7:
/**
******************************************************************************
* @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"
// 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 */
DevSpecParams.OscFrequencyMHz = 618660;
DevSpecParams.RefSPADSInitialised = 0;
DevSpecParams.ReadDataFromDeviceDone = 0;
#ifdef USE_IQC_STATION
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;
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;
Get_info_from_device(2);
if (ErrState == VL53L0X_OK)
{ if (DevSpecParams.ModuleId == 0)
{ revision = 0;
strcpy(Device_Info.ProductId,""); }
else
{ revision = DevSpecParams.Revision;
strcpy(Device_Info.ProductId,DevSpecParams.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;
}
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;}
}
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 all NVM info used by the API */
Get_info_from_device(7);
/* 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);
DevSpecParams.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);
DevSpecParams.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 (DevSpecParams.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 = DevSpecParams.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 = DevSpecParams.FinalRangeTimeoutMicroSecs;
final_range_vcsel_pclks = DevSpecParams.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 = DevSpecParams.PreRangeTimeoutMicroSecs;
pre_range_vcsel_pclks = DevSpecParams.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 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 < 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,
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 < 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,
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, 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,
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,
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,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,SPAD_array_size);
}
} /* while */
}
if (ErrState == VL53L0X_OK) {
*ref_SPAD_count = ref_SPAD_count_int;
*is_aperture_SPADS = is_aperture_SPADS_int;
DevSpecParams.RefSPADSInitialised = 1;
DevSpecParams.ReferenceSPADCount = (uint8_t)(*ref_SPAD_count);
DevSpecParams.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 SPAD_array_size = 6;
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 < 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,
SPAD_array_size,
start_select,
current_SPAD_index,
count,
&last_SPAD_index);
if (ErrState == VL53L0X_OK) {
DevSpecParams.RefSPADSInitialised = 1;
DevSpecParams.ReferenceSPADCount = (uint8_t)(count);
DevSpecParams.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)
{DevSpecParams.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; }
DevSpecParams.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);
DevSpecParams.LastEncodedTimeout = pre_range_encoded_time_out;
Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out);
if (ErrState == VL53L0X_OK)
{DevSpecParams.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)
{ DevSpecParams.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;
Get_info_from_device(1);
/* set the ref SPAD from NVM */
count = (uint32_t)DevSpecParams.ReferenceSPADCount;
aperture_SPADS = DevSpecParams.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)
{ DevSpecParams.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)
{ DevSpecParams.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)
{ DevSpecParams.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)
{ DevSpecParams.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)
{ DevSpecParams.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'
}
void VL53L0X::Get_info_from_device(uint8_t option)
{ uint8_t byte;
uint32_t tmp_dword;
uint8_t module_id;
uint8_t revision;
uint8_t reference_SPAD_count = 0;
uint8_t reference_SPAD_type = 0;
uint32_t part_uid_upper = 0;
uint32_t part_uid_lower = 0;
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;
char product_id[19];
uint8_t read_data_from_device_done;
TFP1616 signal_rate_meas_fixed400_mm_fix = 0;
uint8_t nvm_Ref_good_SPAD_map[REF_SPAD_BUFFER_SIZE];
int i;
read_data_from_device_done = DevSpecParams.ReadDataFromDeviceDone;
/* This access is done only once after that a GetDeviceInfo or datainit is done*/
if (read_data_from_device_done != 7) {
Write_Byte(0x80,0x01);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x00);
Write_Byte(0xFF,0x06);
byte = Read_Byte(0x83);
Write_Byte(0x83,byte | 4);
Write_Byte(0xFF,0x07);
Write_Byte(0x81,0x01);
Polling_delay(); // warning, does nothing!!
Write_Byte(0x80,0x01);
if (((option & 1) == 1) &&
((read_data_from_device_done & 1) == 0)) {
Write_Byte(0x94,0x6b);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
reference_SPAD_count = (uint8_t)((tmp_dword >> 8) & 0x7f);
reference_SPAD_type = (uint8_t)((tmp_dword >> 15) & 0x01);
Write_Byte(0x94,0x24);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
nvm_Ref_good_SPAD_map[0] = (uint8_t)((tmp_dword >> 24)& 0xff);
nvm_Ref_good_SPAD_map[1] = (uint8_t)((tmp_dword >> 16)& 0xff);
nvm_Ref_good_SPAD_map[2] = (uint8_t)((tmp_dword >> 8)& 0xff);
nvm_Ref_good_SPAD_map[3] = (uint8_t)(tmp_dword & 0xff);
Write_Byte(0x94,0x25);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
nvm_Ref_good_SPAD_map[4] = (uint8_t)((tmp_dword >> 24)& 0xff);
nvm_Ref_good_SPAD_map[5] = (uint8_t)((tmp_dword >> 16)& 0xff);
}
if (((option & 2) == 2) && ((read_data_from_device_done & 2) == 0)) {
Write_Byte(0x94,0x02);
Wait_read_strobe();
module_id = Read_Byte(0x90);
Write_Byte(0x94,0x7B);
Wait_read_strobe();
revision = Read_Byte(0x90);
Write_Byte(0x94,0x77);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
product_id[0] = (char)((tmp_dword >> 25) & 0x07f);
product_id[1] = (char)((tmp_dword >> 18) & 0x07f);
product_id[2] = (char)((tmp_dword >> 11) & 0x07f);
product_id[3] = (char)((tmp_dword >> 4) & 0x07f);
byte = (uint8_t)((tmp_dword & 0x00f) << 3);
Write_Byte(0x94,0x78);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
product_id[4] = (char)(byte +((tmp_dword >> 29) & 0x07f));
product_id[5] = (char)((tmp_dword >> 22) & 0x07f);
product_id[6] = (char)((tmp_dword >> 15) & 0x07f);
product_id[7] = (char)((tmp_dword >> 8) & 0x07f);
product_id[8] = (char)((tmp_dword >> 1) & 0x07f);
byte = (uint8_t)((tmp_dword & 0x001) << 6);
Write_Byte(0x94,0x79);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
product_id[9] = (char)(byte +((tmp_dword >> 26) & 0x07f));
product_id[10] = (char)((tmp_dword >> 19) & 0x07f);
product_id[11] = (char)((tmp_dword >> 12) & 0x07f);
product_id[12] = (char)((tmp_dword >> 5) & 0x07f);
byte = (uint8_t)((tmp_dword & 0x01f) << 2);
Write_Byte(0x94,0x7A);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
product_id[13] = (char)(byte +((tmp_dword >> 30) & 0x07f));
product_id[14] = (char)((tmp_dword >> 23) & 0x07f);
product_id[15] = (char)((tmp_dword >> 16) & 0x07f);
product_id[16] = (char)((tmp_dword >> 9) & 0x07f);
product_id[17] = (char)((tmp_dword >> 2) & 0x07f);
product_id[18] = '\0';
}
if (((option & 4) == 4) && ((read_data_from_device_done & 4) == 0))
{ Write_Byte(0x94,0x7B);
Wait_read_strobe();
part_uid_upper = Read_DWord(0x90);
Write_Byte(0x94,0x7C);
Wait_read_strobe();
part_uid_lower = Read_DWord(0x90);
Write_Byte(0x94,0x73);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
signal_rate_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff) << 8;
Write_Byte(0x94,0x74);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
signal_rate_meas_fixed1104_400_mm |= ((tmp_dword &
0xff000000) >> 24);
Write_Byte(0x94,0x75);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)<< 8;
Write_Byte(0x94,0x76);
Wait_read_strobe();
tmp_dword = Read_DWord(0x90);
dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000) >> 24);
}
Write_Byte(0x81,0x00);
Write_Byte(0xFF,0x06);
Write_Byte(0x83,Read_Byte(0x83) & 0xfb);
Write_Byte(0xFF,0x01);
Write_Byte(0x00,0x01);
Write_Byte(0xFF,0x00);
Write_Byte(0x80,0x00);
}
if ((ErrState == VL53L0X_OK) && (read_data_from_device_done != 7)) {
/* Assign to variable if ErrState is ok */
if (((option & 1) == 1) && ((read_data_from_device_done & 1) == 0))
{ DevSpecParams.ReferenceSPADCount=reference_SPAD_count;
DevSpecParams.ReferenceSPADType =reference_SPAD_type;
for (i = 0; i < REF_SPAD_BUFFER_SIZE; i++)
{ SPADData.RefGoodSPADMap[i] = nvm_Ref_good_SPAD_map[i]; }
}
if (((option & 2) == 2) &&((read_data_from_device_done & 2) == 0))
{ DevSpecParams.ModuleId = module_id;
DevSpecParams.Revision = revision;
strcpy(DevSpecParams.ProductId, product_id);
}
if (((option & 4) == 4) && ((read_data_from_device_done & 4) == 0)) {
DevSpecParams.PartUIDUpper = part_uid_upper;
DevSpecParams.PartUIDLower = part_uid_lower;
signal_rate_meas_fixed400_mm_fix =
FP97_TO_FP1616(signal_rate_meas_fixed1104_400_mm);
DevSpecParams.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
DevSpecParams.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
offset_um = 0;
if (dist_meas_fixed1104_400_mm != 0) {
offset_fixed1104_mm = dist_meas_fixed1104_400_mm -
dist_meas_tgt_fixed1104_mm;
offset_um = (offset_fixed1104_mm * 1000) >> 4;
offset_um *= -1;
}
NVM_Offset_Cal_um = offset_um;
}
byte = (uint8_t)(read_data_from_device_done | option);
DevSpecParams.ReadDataFromDeviceDone = byte;
}
}
/******************************************************************************/
/****************** 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 *************************/
/******************************************************************************/
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; }
}
/******************************************************************************/