VL53L0X_Condensed - Condensed Version of Public VL53L0X

Users » sepp_nepp » Code » VL53L0X_Condensed

Condensed Version of Public VL53L0X

VL53L0X.cpp@12:aa177f0e4c10, 2019-04-10 (annotated)

Committer:: sepp_nepp
Date:: Wed Apr 10 19:03:16 2019 +0000
Revision:: 12:aa177f0e4c10
Parent:: 11:c6f95a42d4d7

Troy a gain

Who changed what in which revision?

User	Revision	Line number	New contents of line
nikapov	0:a1a69d32f310	1	/**
nikapov	0:a1a69d32f310	2	******************************************************************************
sepp_nepp	11:c6f95a42d4d7	3	* @file Class.cpp
nikapov	0:a1a69d32f310	4	* @author IMG
nikapov	0:a1a69d32f310	5	* @version V0.0.1
nikapov	0:a1a69d32f310	6	* @date 28-June-2016
nikapov	0:a1a69d32f310	7	* @brief Implementation file for the VL53L0X driver class
nikapov	0:a1a69d32f310	8	******************************************************************************
nikapov	0:a1a69d32f310	9	* @attention
nikapov	0:a1a69d32f310	10	*
nikapov	0:a1a69d32f310	11	* <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
nikapov	0:a1a69d32f310	12	*
sepp_nepp	7:41cbc431e1f4	13	* Redistribution and use in source and binary forms,with or without modification,
nikapov	0:a1a69d32f310	14	* are permitted provided that the following conditions are met:
nikapov	0:a1a69d32f310	15	* 1. Redistributions of source code must retain the above copyright notice,
nikapov	0:a1a69d32f310	16	* this list of conditions and the following disclaimer.
nikapov	0:a1a69d32f310	17	* 2. Redistributions in binary form must reproduce the above copyright notice,
nikapov	0:a1a69d32f310	18	* this list of conditions and the following disclaimer in the documentation
nikapov	0:a1a69d32f310	19	* and/or other materials provided with the distribution.
nikapov	0:a1a69d32f310	20	* 3. Neither the name of STMicroelectronics nor the names of its contributors
nikapov	0:a1a69d32f310	21	* may be used to endorse or promote products derived from this software
nikapov	0:a1a69d32f310	22	* without specific prior written permission.
nikapov	0:a1a69d32f310	23	*
nikapov	0:a1a69d32f310	24	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
sepp_nepp	7:41cbc431e1f4	25	* AND ANY EXPRESS OR IMPLIED WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE
nikapov	0:a1a69d32f310	26	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
nikapov	0:a1a69d32f310	27	* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
sepp_nepp	7:41cbc431e1f4	28	* FOR ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL
sepp_nepp	7:41cbc431e1f4	29	* DAMAGES (INCLUDING,BUT NOT LIMITED TO,PROCUREMENT OF SUBSTITUTE GOODS OR
sepp_nepp	7:41cbc431e1f4	30	* SERVICES; LOSS OF USE,DATA,OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
sepp_nepp	7:41cbc431e1f4	31	* CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,
nikapov	0:a1a69d32f310	32	* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
sepp_nepp	7:41cbc431e1f4	33	* OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
nikapov	0:a1a69d32f310	34	*
nikapov	0:a1a69d32f310	35	******************************************************************************
nikapov	0:a1a69d32f310	36	*/
nikapov	0:a1a69d32f310	37
sepp_nepp	7:41cbc431e1f4	38	// Some example regex that were used to replace useless macros
sepp_nepp	7:41cbc431e1f4	39	// \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s)([A-Z\d_]+)[[:punct:]](\s)([A-Z\d_]+)\Q);\E
sepp_nepp	11:c6f95a42d4d7	40	// _device->CurrParams.\1[\3] = \5;
sepp_nepp	11:c6f95a42d4d7	41	// to replace this "#define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value)" by "_device->CurrParams.field[index] = value"
sepp_nepp	11:c6f95a42d4d7	42
sepp_nepp	11:c6f95a42d4d7	43	// to replace "Read_Byte(0x90,&module_id);" by "module_id = Read_Byte(0x90);" search and replace
sepp_nepp	11:c6f95a42d4d7	44	// \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E
sepp_nepp	11:c6f95a42d4d7	45	// \3 = Read_Byte\(\1\);
sepp_nepp	7:41cbc431e1f4	46
nikapov	0:a1a69d32f310	47	/* Includes */
nikapov	0:a1a69d32f310	48	#include <stdlib.h>
nikapov	0:a1a69d32f310	49	#include "VL53L0X.h"
sepp_nepp	10:cd251e0fc2fd	50	#include "VL53L0X_tuning.h"
sepp_nepp	7:41cbc431e1f4	51
sepp_nepp	12:aa177f0e4c10	52	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	53	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	54	/**************** All initialization functions ***********************/
sepp_nepp	12:aa177f0e4c10	55	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	56	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	57
sepp_nepp	11:c6f95a42d4d7	58	// Function Data_init and Init_Sensor is united into Start_Sensor
sepp_nepp	11:c6f95a42d4d7	59	VL53L0X_Error VL53L0X::Start_Sensor(uint8_t new_addr)
sepp_nepp	11:c6f95a42d4d7	60	{ ErrState = VL53L0X_OK;
sepp_nepp	11:c6f95a42d4d7	61
sepp_nepp	11:c6f95a42d4d7	62	if (_gpio0) { // Can the shutdown pin be controlled?
sepp_nepp	11:c6f95a42d4d7	63	*_gpio0 = 0; wait_ms(1); // quick shutdown
sepp_nepp	11:c6f95a42d4d7	64	*_gpio0 = 1; wait_ms(10); // and back ON again
sepp_nepp	11:c6f95a42d4d7	65	}
sepp_nepp	11:c6f95a42d4d7	66
sepp_nepp	11:c6f95a42d4d7	67	/* Setup the I2C bus. By default the I2C is running at 1V8 if you
sepp_nepp	11:c6f95a42d4d7	68	* want to change it you need to include this define at compilation level. */
sepp_nepp	11:c6f95a42d4d7	69	#ifdef USE_I2C_2V8
sepp_nepp	11:c6f95a42d4d7	70	VL53L0X_UpdateByte(REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01);
sepp_nepp	11:c6f95a42d4d7	71	#endif
sepp_nepp	11:c6f95a42d4d7	72	/* Set I2C standard mode */
sepp_nepp	11:c6f95a42d4d7	73	Write_Byte(0x88,0x00);
sepp_nepp	11:c6f95a42d4d7	74
sepp_nepp	11:c6f95a42d4d7	75	// read and check the device ID from the ID register
sepp_nepp	11:c6f95a42d4d7	76	Device_Info.ProductType = Read_Byte(REG_IDENTIFICATION_MODEL_ID);
sepp_nepp	11:c6f95a42d4d7	77	if ( (ErrState == VL53L0X_OK) && (Device_Info.ProductType != 0xEEAA) )
sepp_nepp	11:c6f95a42d4d7	78	{return VL53L0X_ERROR_I2C_WRONG_DEV_ID; }
sepp_nepp	11:c6f95a42d4d7	79
sepp_nepp	11:c6f95a42d4d7	80	// reconfigure the address with a new address if requested
sepp_nepp	11:c6f95a42d4d7	81	if ( (ErrState == VL53L0X_OK) && (new_addr != VL53L0X_DEFAULT_ADDRESS) )
sepp_nepp	11:c6f95a42d4d7	82	{ Write_Byte(REG_I2C_SLAVE_DEVICE_ADDRESS, new_addr / 2);
sepp_nepp	11:c6f95a42d4d7	83	I2cDevAddr = new_addr;
sepp_nepp	11:c6f95a42d4d7	84	}
sepp_nepp	11:c6f95a42d4d7	85	// quite if an error was raised
sepp_nepp	11:c6f95a42d4d7	86	if (ErrState != VL53L0X_OK) {return ErrState; }
sepp_nepp	11:c6f95a42d4d7	87
sepp_nepp	11:c6f95a42d4d7	88	/* Set Default static parameters
sepp_nepp	11:c6f95a42d4d7	89	set first temporary values 9.44MHz 65536 = 618660 */
sepp_nepp	12:aa177f0e4c10	90	OscFrequencyMHz = 618660;
sepp_nepp	12:aa177f0e4c10	91	RefSPADSInitialised = 0;
sepp_nepp	12:aa177f0e4c10	92
sepp_nepp	12:aa177f0e4c10	93	// Read All NVM from device
sepp_nepp	12:aa177f0e4c10	94	ReadNVMDataFromDeviceDone = 0;
sepp_nepp	12:aa177f0e4c10	95	Get_all_NVM_info_from_device();
sepp_nepp	11:c6f95a42d4d7	96
sepp_nepp	11:c6f95a42d4d7	97	#ifdef USE_IQC_STATION
sepp_nepp	12:aa177f0e4c10	98	// must make sure that first your read all NVM info used by the API */
sepp_nepp	11:c6f95a42d4d7	99	VL53L0X_Apply_Offset_Cal();
sepp_nepp	11:c6f95a42d4d7	100	#endif
sepp_nepp	11:c6f95a42d4d7	101
sepp_nepp	11:c6f95a42d4d7	102	/* Default value is 1000 for Linearity Corrective Gain */
sepp_nepp	11:c6f95a42d4d7	103	LinearityCorrectiveGain = 1000;
sepp_nepp	11:c6f95a42d4d7	104
sepp_nepp	11:c6f95a42d4d7	105	/* Dmax default Parameter */
sepp_nepp	11:c6f95a42d4d7	106	DmaxCalRangeMilliMeter = 400;
sepp_nepp	11:c6f95a42d4d7	107	DmaxCalSignalRateRtnMHz = (TFP1616)((0x00016B85)); /* 1.42 No Cover Glass*/
sepp_nepp	11:c6f95a42d4d7	108
sepp_nepp	11:c6f95a42d4d7	109	/* Get default parameters */
sepp_nepp	11:c6f95a42d4d7	110	CurrParams = Get_device_parameters();
sepp_nepp	11:c6f95a42d4d7	111
sepp_nepp	11:c6f95a42d4d7	112	/* Set Default Xtalk_CompRate_MHz to 0 */
sepp_nepp	11:c6f95a42d4d7	113	CurrParams.Xtalk_CompRate_MHz = 0;
sepp_nepp	11:c6f95a42d4d7	114
sepp_nepp	11:c6f95a42d4d7	115	/* initialize CurrParams values */
sepp_nepp	11:c6f95a42d4d7	116	CurrParams.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
sepp_nepp	11:c6f95a42d4d7	117	CurrParams.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
sepp_nepp	11:c6f95a42d4d7	118
sepp_nepp	11:c6f95a42d4d7	119	/* Sigma estimator variable */
sepp_nepp	11:c6f95a42d4d7	120	SigmaEstRefArray = 100;
sepp_nepp	11:c6f95a42d4d7	121	SigmaEstEffPulseWidth = 900;
sepp_nepp	11:c6f95a42d4d7	122	SigmaEstEffAmbWidth = 500;
sepp_nepp	11:c6f95a42d4d7	123	targetRefRate = 0x0A00; /* 20 MHz in 9:7 format */
sepp_nepp	11:c6f95a42d4d7	124
sepp_nepp	11:c6f95a42d4d7	125	/* Use internal default settings */
sepp_nepp	11:c6f95a42d4d7	126	UseInternalTuningSettings = 1;
sepp_nepp	11:c6f95a42d4d7	127	Write_Byte(0x80,0x01);
sepp_nepp	11:c6f95a42d4d7	128	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	129	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	130	StopVariable = Read_Byte(0x91);
sepp_nepp	11:c6f95a42d4d7	131	Write_Byte(0x00,0x01);
sepp_nepp	11:c6f95a42d4d7	132	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	133	Write_Byte(0x80,0x00);
sepp_nepp	11:c6f95a42d4d7	134
sepp_nepp	11:c6f95a42d4d7	135	// quite if an error was raised
sepp_nepp	11:c6f95a42d4d7	136	if (ErrState != VL53L0X_OK) {return ErrState; }
sepp_nepp	11:c6f95a42d4d7	137
sepp_nepp	11:c6f95a42d4d7	138	/* Disable the following SW-internal checks plaus set some values */
sepp_nepp	11:c6f95a42d4d7	139	CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_REF_CLIP] = 0;
sepp_nepp	11:c6f95a42d4d7	140	CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIG_REF_CLIP] = (35 * 65536);
sepp_nepp	11:c6f95a42d4d7	141	CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
sepp_nepp	11:c6f95a42d4d7	142	CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
sepp_nepp	11:c6f95a42d4d7	143
sepp_nepp	11:c6f95a42d4d7	144	/* Disable the following Device-Internal Checks: */
sepp_nepp	11:c6f95a42d4d7	145	CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_MSRC] = 0;
sepp_nepp	11:c6f95a42d4d7	146	CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE] = 0;
sepp_nepp	11:c6f95a42d4d7	147	Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEE, 0);
sepp_nepp	11:c6f95a42d4d7	148
sepp_nepp	11:c6f95a42d4d7	149	/* Only enable this internal Check : */
sepp_nepp	11:c6f95a42d4d7	150	CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = 1;
sepp_nepp	11:c6f95a42d4d7	151	CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = (18 * 65536);
sepp_nepp	11:c6f95a42d4d7	152
sepp_nepp	11:c6f95a42d4d7	153	/* Plus Enable VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE check */
sepp_nepp	11:c6f95a42d4d7	154	Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1);
sepp_nepp	11:c6f95a42d4d7	155
sepp_nepp	11:c6f95a42d4d7	156	if (ErrState == VL53L0X_OK) { /* 0.25 in FP1616 notation 65536 */
sepp_nepp	11:c6f95a42d4d7	157	Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,
sepp_nepp	11:c6f95a42d4d7	158	(TFP1616)(25 * 65536 / 100)); }
sepp_nepp	11:c6f95a42d4d7	159
sepp_nepp	11:c6f95a42d4d7	160	// quit if an error was raised
sepp_nepp	11:c6f95a42d4d7	161	if (ErrState != VL53L0X_OK) {return ErrState; }
sepp_nepp	11:c6f95a42d4d7	162
sepp_nepp	11:c6f95a42d4d7	163	// Preset the Config States
sepp_nepp	11:c6f95a42d4d7	164	SequenceConfig = 0xFF ;
sepp_nepp	11:c6f95a42d4d7	165
sepp_nepp	11:c6f95a42d4d7	166	/* Set Device state to tell that we are waiting for call to VL53L0X_StaticInit */
sepp_nepp	11:c6f95a42d4d7	167	Current_State = VL53L0X_STATE_WAIT_STATICINIT ;
sepp_nepp	11:c6f95a42d4d7	168
sepp_nepp	11:c6f95a42d4d7	169	Fill_device_info(); // Retrieve Silicon version, stored in Device_Info
sepp_nepp	11:c6f95a42d4d7	170
sepp_nepp	11:c6f95a42d4d7	171	uint32_t ref_SPAD_count;
sepp_nepp	11:c6f95a42d4d7	172	uint8_t is_aperture_SPADS;
sepp_nepp	11:c6f95a42d4d7	173	uint8_t vhv_settings;
sepp_nepp	11:c6f95a42d4d7	174	uint8_t phase_cal;
sepp_nepp	11:c6f95a42d4d7	175
sepp_nepp	12:aa177f0e4c10	176	// make sure the Get_all_NVM_info_from_device was called before calling static init
sepp_nepp	11:c6f95a42d4d7	177	if (ErrState == VL53L0X_OK) { Static_init(); } // Device Initialization
sepp_nepp	11:c6f95a42d4d7	178
sepp_nepp	11:c6f95a42d4d7	179	if (ErrState == VL53L0X_OK) { // Device Calibration
sepp_nepp	11:c6f95a42d4d7	180	Perf_Ref_calibration( &vhv_settings, &phase_cal, 1); }
sepp_nepp	11:c6f95a42d4d7	181
sepp_nepp	11:c6f95a42d4d7	182	if (ErrState == VL53L0X_OK) { // SPAD Configuration
sepp_nepp	11:c6f95a42d4d7	183	Perf_Ref_SPAD_management( &ref_SPAD_count, &is_aperture_SPADS); }
sepp_nepp	11:c6f95a42d4d7	184
sepp_nepp	11:c6f95a42d4d7	185	return ErrState;
sepp_nepp	11:c6f95a42d4d7	186	}
sepp_nepp	11:c6f95a42d4d7	187
sepp_nepp	11:c6f95a42d4d7	188	void VL53L0X::Fill_device_info()
sepp_nepp	11:c6f95a42d4d7	189	{ uint8_t revision;
sepp_nepp	11:c6f95a42d4d7	190
sepp_nepp	11:c6f95a42d4d7	191	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	192	{ if (ModuleId == 0)
sepp_nepp	11:c6f95a42d4d7	193	{ revision = 0;
sepp_nepp	11:c6f95a42d4d7	194	strcpy(Device_Info.ProductId,""); }
sepp_nepp	11:c6f95a42d4d7	195	else
sepp_nepp	12:aa177f0e4c10	196	{ revision = Revision;
sepp_nepp	12:aa177f0e4c10	197	strcpy(Device_Info.ProductId,ProductId);
sepp_nepp	11:c6f95a42d4d7	198	}
sepp_nepp	11:c6f95a42d4d7	199	if (revision == 0)
sepp_nepp	11:c6f95a42d4d7	200	{ strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS0); }
sepp_nepp	11:c6f95a42d4d7	201	else if ((revision <= 34) && (revision != 32))
sepp_nepp	11:c6f95a42d4d7	202	{ strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS1); }
sepp_nepp	11:c6f95a42d4d7	203	else if (revision < 39)
sepp_nepp	11:c6f95a42d4d7	204	{ strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS2); }
sepp_nepp	11:c6f95a42d4d7	205	else { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_ES1); }
sepp_nepp	11:c6f95a42d4d7	206	strcpy(Device_Info.Type,VL53L0X_STRING_DEVICE_INFO_TYPE);
sepp_nepp	11:c6f95a42d4d7	207	}
sepp_nepp	11:c6f95a42d4d7	208
sepp_nepp	11:c6f95a42d4d7	209	Device_Info.ProductRevisionMajor = 1;
sepp_nepp	11:c6f95a42d4d7	210	Device_Info.ProductRevisionMinor =
sepp_nepp	11:c6f95a42d4d7	211	(Read_Byte(REG_IDENTIFICATION_REVISION_ID) & 0xF0) >> 4;
nikapov	0:a1a69d32f310	212	}
sepp_nepp	11:c6f95a42d4d7	213
sepp_nepp	12:aa177f0e4c10	214	/* That was 'Get_info_from_device( unit8_t option)' where option was composed of
sepp_nepp	12:aa177f0e4c10	215	1, 2, 4 depending what info was requested.
sepp_nepp	12:aa177f0e4c10	216	It was decided to combine all that into a single Get ALL infos from the device
sepp_nepp	12:aa177f0e4c10	217	that is called once at Sensor Init so all the job is done.
sepp_nepp	12:aa177f0e4c10	218	In addition, originally values would first be read into local variables and then
sepp_nepp	12:aa177f0e4c10	219	only copied to class fields if no errors.
sepp_nepp	12:aa177f0e4c10	220	However, if at device initialization an error is raised the whole class instance
sepp_nepp	12:aa177f0e4c10	221	cannot be used anyway, so decided that we directly read into class fields. */
sepp_nepp	12:aa177f0e4c10	222
sepp_nepp	12:aa177f0e4c10	223	void VL53L0X::Get_all_NVM_info_from_device()
sepp_nepp	12:aa177f0e4c10	224	{ uint32_t tmp_dword;
sepp_nepp	12:aa177f0e4c10	225	uint32_t offset_fixed1104_mm = 0;
sepp_nepp	12:aa177f0e4c10	226	int16_t offset_um = 0;
sepp_nepp	12:aa177f0e4c10	227	uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
sepp_nepp	12:aa177f0e4c10	228	uint32_t dist_meas_fixed1104_400_mm = 0;
sepp_nepp	12:aa177f0e4c10	229	uint32_t signal_rate_meas_fixed1104_400_mm = 0;
sepp_nepp	12:aa177f0e4c10	230	int i;
sepp_nepp	12:aa177f0e4c10	231
sepp_nepp	12:aa177f0e4c10	232	/* This access is done only once after that a GetDeviceInfo or datainit is done*/
sepp_nepp	12:aa177f0e4c10	233	if (ReadNVMDataFromDeviceDone == 7) { return ; }
sepp_nepp	12:aa177f0e4c10	234
sepp_nepp	12:aa177f0e4c10	235	Write_Byte(0x80,0x01);
sepp_nepp	12:aa177f0e4c10	236	Write_Byte(0xFF,0x01);
sepp_nepp	12:aa177f0e4c10	237	Write_Byte(0x00,0x00);
sepp_nepp	12:aa177f0e4c10	238	Write_Byte(0xFF,0x06);
sepp_nepp	12:aa177f0e4c10	239	Register_BitMask(0x83,0xFF,0x04)
sepp_nepp	12:aa177f0e4c10	240	Write_Byte(0xFF,0x07);
sepp_nepp	12:aa177f0e4c10	241	Write_Byte(0x81,0x01);
sepp_nepp	12:aa177f0e4c10	242	Polling_delay(); // warning, does nothing!!
sepp_nepp	12:aa177f0e4c10	243	Write_Byte(0x80,0x01);
sepp_nepp	12:aa177f0e4c10	244
sepp_nepp	12:aa177f0e4c10	245	/* *********** First Block of NVM data is read: **********/
sepp_nepp	12:aa177f0e4c10	246	tmp_dword = Get_NVM_DWord(0x6b);
sepp_nepp	12:aa177f0e4c10	247	ReferenceSPADCount = (uint8_t)((tmp_dword >> 8) & 0x7f);
sepp_nepp	12:aa177f0e4c10	248	ReferenceSPADType = (uint8_t)((tmp_dword >> 15) & 0x01);
sepp_nepp	12:aa177f0e4c10	249
sepp_nepp	12:aa177f0e4c10	250	tmp_dword = Get_NVM_DWord(0x24);
sepp_nepp	12:aa177f0e4c10	251	SPADData.RefGoodSPADMap[0] = (uint8_t)((tmp_dword >> 24)& 0xff);
sepp_nepp	12:aa177f0e4c10	252	SPADData.RefGoodSPADMap[1] = (uint8_t)((tmp_dword >> 16)& 0xff);
sepp_nepp	12:aa177f0e4c10	253	SPADData.RefGoodSPADMap[2] = (uint8_t)((tmp_dword >> 8)& 0xff);
sepp_nepp	12:aa177f0e4c10	254	SPADData.RefGoodSPADMap[3] = (uint8_t)(tmp_dword & 0xff);
sepp_nepp	12:aa177f0e4c10	255
sepp_nepp	12:aa177f0e4c10	256	tmp_dword = Get_NVM_DWord(0x25);
sepp_nepp	12:aa177f0e4c10	257	SPADData.RefGoodSPADMap[4] = (uint8_t)((tmp_dword >> 24)& 0xff);
sepp_nepp	12:aa177f0e4c10	258	SPADData.RefGoodSPADMap[5] = (uint8_t)((tmp_dword >> 16)& 0xff);
sepp_nepp	12:aa177f0e4c10	259
sepp_nepp	12:aa177f0e4c10	260	/* *********** Second Block of NVM data is read: **********/
sepp_nepp	12:aa177f0e4c10	261	ModuleId = Get_NVM_Byte(0x02);
sepp_nepp	12:aa177f0e4c10	262	Revision = Get_NVM_Byte(0x02);
sepp_nepp	12:aa177f0e4c10	263
sepp_nepp	12:aa177f0e4c10	264	tmp_dword = Get_NVM_DWord(0x77);
sepp_nepp	12:aa177f0e4c10	265	ProductId[0] = (char)((tmp_dword >> 25) & 0x07f);
sepp_nepp	12:aa177f0e4c10	266	ProductId[1] = (char)((tmp_dword >> 18) & 0x07f);
sepp_nepp	12:aa177f0e4c10	267	ProductId[2] = (char)((tmp_dword >> 11) & 0x07f);
sepp_nepp	12:aa177f0e4c10	268	ProductId[3] = (char)((tmp_dword >> 4) & 0x07f);
sepp_nepp	12:aa177f0e4c10	269	ProductId[4] = (char)((tmp_dword << 3) & 0x07f);
sepp_nepp	12:aa177f0e4c10	270
sepp_nepp	12:aa177f0e4c10	271	tmp_dword = Get_NVM_DWord(0x78);
sepp_nepp	12:aa177f0e4c10	272	ProductId[4] = ProductId[4] \|
sepp_nepp	12:aa177f0e4c10	273	(char)((tmp_dword >> 29) & 0x07f));
sepp_nepp	12:aa177f0e4c10	274	ProductId[5] = (char)((tmp_dword >> 22) & 0x07f);
sepp_nepp	12:aa177f0e4c10	275	ProductId[6] = (char)((tmp_dword >> 15) & 0x07f);
sepp_nepp	12:aa177f0e4c10	276	ProductId[7] = (char)((tmp_dword >> 8) & 0x07f);
sepp_nepp	12:aa177f0e4c10	277	ProductId[8] = (char)((tmp_dword >> 1) & 0x07f);
sepp_nepp	12:aa177f0e4c10	278	ProductId[9] = (char)((tmp_dword << 6) & 0x07f);
sepp_nepp	12:aa177f0e4c10	279
sepp_nepp	12:aa177f0e4c10	280	tmp_dword = Get_NVM_DWord(0x79);
sepp_nepp	12:aa177f0e4c10	281	ProductId[9] = ProductId[9] \|
sepp_nepp	12:aa177f0e4c10	282	(char)((tmp_dword >> 26) & 0x07f);
sepp_nepp	12:aa177f0e4c10	283	ProductId[10] = (char)((tmp_dword >> 19) & 0x07f);
sepp_nepp	12:aa177f0e4c10	284	ProductId[11] = (char)((tmp_dword >> 12) & 0x07f);
sepp_nepp	12:aa177f0e4c10	285	ProductId[12] = (char)((tmp_dword >> 5) & 0x07f);
sepp_nepp	12:aa177f0e4c10	286	ProductId[13] = (char)((tmp_dword << 2) & 0x07f);
sepp_nepp	12:aa177f0e4c10	287
sepp_nepp	12:aa177f0e4c10	288	tmp_dword = Get_NVM_DWord(0x7A);
sepp_nepp	12:aa177f0e4c10	289	ProductId[13] = ProductId[13] \|
sepp_nepp	12:aa177f0e4c10	290	(char)((tmp_dword >> 30) & 0x07f));
sepp_nepp	12:aa177f0e4c10	291	ProductId[14] = (char)((tmp_dword >> 23) & 0x07f);
sepp_nepp	12:aa177f0e4c10	292	ProductId[15] = (char)((tmp_dword >> 16) & 0x07f);
sepp_nepp	12:aa177f0e4c10	293	ProductId[16] = (char)((tmp_dword >> 9) & 0x07f);
sepp_nepp	12:aa177f0e4c10	294	ProductId[17] = (char)((tmp_dword >> 2) & 0x07f);
sepp_nepp	12:aa177f0e4c10	295	ProductId[18] = '\0';
sepp_nepp	12:aa177f0e4c10	296
sepp_nepp	12:aa177f0e4c10	297	/* *********** Third Block of NVM data is read: **********/
sepp_nepp	12:aa177f0e4c10	298	PartUIDUpper = Get_NVM_DWord(0x7B);
sepp_nepp	12:aa177f0e4c10	299	PartUIDLower = Get_NVM_DWord(0x7C);
sepp_nepp	12:aa177f0e4c10	300	SignalRateMeasFixed400mm = // convert from FP97_TO_FP1616
sepp_nepp	12:aa177f0e4c10	301	( (( Get_NVM_DWord(0x73) << 17) & 0x1fE0000) \|
sepp_nepp	12:aa177f0e4c10	302	(( Get_NVM_DWord(0x74) >> 15) & 0x001fE00) ) ;
sepp_nepp	12:aa177f0e4c10	303
sepp_nepp	12:aa177f0e4c10	304	dist_meas_fixed1104_400_mm =
sepp_nepp	12:aa177f0e4c10	305	(( Get_NVM_DWord(0x75) << 8) & 0xff00) \|
sepp_nepp	12:aa177f0e4c10	306	(( Get_NVM_DWord(0x76) >> 24) & 0x00ff);
sepp_nepp	12:aa177f0e4c10	307
sepp_nepp	12:aa177f0e4c10	308	if (dist_meas_fixed1104_400_mm != 0) {
sepp_nepp	12:aa177f0e4c10	309	offset_fixed1104_mm = dist_meas_fixed1104_400_mm -
sepp_nepp	12:aa177f0e4c10	310	dist_meas_tgt_fixed1104_mm;
sepp_nepp	12:aa177f0e4c10	311	NVM_Offset_Cal_um = (offset_fixed1104_mm * 1000) >> 4;
sepp_nepp	12:aa177f0e4c10	312	NVM_Offset_Cal_um *= -1; }
sepp_nepp	12:aa177f0e4c10	313	else { NVM_Offset_Cal_um = 0; }
sepp_nepp	12:aa177f0e4c10	314
sepp_nepp	12:aa177f0e4c10	315	Write_Byte(0x81,0x00);
sepp_nepp	12:aa177f0e4c10	316	Write_Byte(0xFF,0x06);
sepp_nepp	12:aa177f0e4c10	317	Register_BitMask(0x83,0xfb,0x00)
sepp_nepp	12:aa177f0e4c10	318	Write_Byte(0xFF,0x01);
sepp_nepp	12:aa177f0e4c10	319	Write_Byte(0x00,0x01);
sepp_nepp	12:aa177f0e4c10	320	Write_Byte(0xFF,0x00);
sepp_nepp	12:aa177f0e4c10	321	Write_Byte(0x80,0x00);
sepp_nepp	12:aa177f0e4c10	322	ReadNVMDataFromDeviceDone = 7;
sepp_nepp	12:aa177f0e4c10	323	}
sepp_nepp	11:c6f95a42d4d7	324
sepp_nepp	11:c6f95a42d4d7	325	uint32_t VL53L0X::Get_distance()
sepp_nepp	11:c6f95a42d4d7	326	{ ErrState = VL53L0X_OK;
sepp_nepp	11:c6f95a42d4d7	327	TRangeResults p_ranging_results;
sepp_nepp	11:c6f95a42d4d7	328
sepp_nepp	11:c6f95a42d4d7	329	Start_Measurement(op_single_shot_poll, NULL);
sepp_nepp	11:c6f95a42d4d7	330	if (ErrState==VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	331	{ p_ranging_results = Get_Measurement(op_single_shot_poll); }
sepp_nepp	11:c6f95a42d4d7	332
sepp_nepp	11:c6f95a42d4d7	333	Stop_Measurement(op_single_shot_poll);
sepp_nepp	11:c6f95a42d4d7	334
sepp_nepp	11:c6f95a42d4d7	335	if (p_ranging_results.RangeStatus == 0) // we have a valid range ?
sepp_nepp	11:c6f95a42d4d7	336	{ return p_ranging_results.RangeMilliMeter; }
sepp_nepp	11:c6f95a42d4d7	337	else
sepp_nepp	11:c6f95a42d4d7	338	{ ErrState = VL53L0X_ERROR_RANGE_ERROR; return 0;}
nikapov	0:a1a69d32f310	339	}
nikapov	0:a1a69d32f310	340
sepp_nepp	12:aa177f0e4c10	341	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	342	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	343	/**************** Actual Measurement functions ***********************/
sepp_nepp	12:aa177f0e4c10	344	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	345	/******************************************************************************/
sepp_nepp	12:aa177f0e4c10	346
sepp_nepp	11:c6f95a42d4d7	347	TRangeResults VL53L0X::Get_Measurement(TOperatingMode operating_mode)
sepp_nepp	11:c6f95a42d4d7	348	{ TRangeResults p_data;
sepp_nepp	11:c6f95a42d4d7	349
sepp_nepp	11:c6f95a42d4d7	350	switch (operating_mode) {
sepp_nepp	11:c6f95a42d4d7	351	case op_single_shot_poll:
sepp_nepp	11:c6f95a42d4d7	352	Perf_single_ranging_measurement(&p_data);
sepp_nepp	11:c6f95a42d4d7	353	break;
sepp_nepp	11:c6f95a42d4d7	354	case op_poll:
sepp_nepp	11:c6f95a42d4d7	355	Poll_Measure_Completion();
sepp_nepp	11:c6f95a42d4d7	356	Get_ranging_results(&p_data);
sepp_nepp	11:c6f95a42d4d7	357	if (ErrState == VL53L0X_OK) { // Clear the interrupt
sepp_nepp	11:c6f95a42d4d7	358	Clear_interrupt_mask(REG_SYSINT_GPIO_NEW_SAMPLE_READY);
sepp_nepp	11:c6f95a42d4d7	359	Polling_delay();
sepp_nepp	11:c6f95a42d4d7	360	}
sepp_nepp	11:c6f95a42d4d7	361	break;
sepp_nepp	11:c6f95a42d4d7	362	case op_INT:
sepp_nepp	11:c6f95a42d4d7	363	Get_ranging_results(&p_data);
sepp_nepp	11:c6f95a42d4d7	364	Clear_interrupt_mask(REG_SYSINT_CLEAR \| REG_RESULT_INTERRUPT_STATUS);
sepp_nepp	11:c6f95a42d4d7	365	} // switch
sepp_nepp	11:c6f95a42d4d7	366	return p_data;
sepp_nepp	11:c6f95a42d4d7	367	}
sepp_nepp	11:c6f95a42d4d7	368
sepp_nepp	11:c6f95a42d4d7	369	/** Get part to part calibration offset; Should only be used after a
sepp_nepp	11:c6f95a42d4d7	370	successful call to @a VL53L0X_DataInit to backup device NVM value **/
sepp_nepp	11:c6f95a42d4d7	371	int32_t VL53L0X::Get_Offset_Cal_um()
sepp_nepp	11:c6f95a42d4d7	372	{ uint16_t range_offset_register;
nikapov	0:a1a69d32f310	373	int16_t c_max_offset = 2047;
nikapov	0:a1a69d32f310	374	int16_t c_offset_range = 4096;
nikapov	0:a1a69d32f310	375
nikapov	0:a1a69d32f310	376	/* Note that offset has 10.2 format */
sepp_nepp	11:c6f95a42d4d7	377	range_offset_register = Read_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM);
sepp_nepp	11:c6f95a42d4d7	378
sepp_nepp	11:c6f95a42d4d7	379	if (ErrState == VL53L0X_OK) {
nikapov	0:a1a69d32f310	380	range_offset_register = (range_offset_register & 0x0fff);
nikapov	0:a1a69d32f310	381
sepp_nepp	11:c6f95a42d4d7	382	/* Apply 12 bit 2's complement conversion */
sepp_nepp	11:c6f95a42d4d7	383	if (range_offset_register > c_max_offset)
sepp_nepp	11:c6f95a42d4d7	384	{ return (int16_t)(range_offset_register - c_offset_range) * 250; }
sepp_nepp	11:c6f95a42d4d7	385	else
sepp_nepp	11:c6f95a42d4d7	386	{ return (int16_t)range_offset_register * 250;}
nikapov	0:a1a69d32f310	387	}
sepp_nepp	11:c6f95a42d4d7	388	else return 0;
nikapov	0:a1a69d32f310	389	}
nikapov	0:a1a69d32f310	390
sepp_nepp	11:c6f95a42d4d7	391	void VL53L0X::Set_Offset_Cal_um(int32_t Offset_Cal_um)
sepp_nepp	11:c6f95a42d4d7	392	{ int32_t c_max_offset_um = 511000;
sepp_nepp	10:cd251e0fc2fd	393	int32_t c_min_offset_um = -512000;
sepp_nepp	11:c6f95a42d4d7	394	int16_t c_offset_range = 4096;
nikapov	0:a1a69d32f310	395	uint32_t encoded_offset_val;
nikapov	0:a1a69d32f310	396
sepp_nepp	11:c6f95a42d4d7	397	if (Offset_Cal_um > c_max_offset_um) { Offset_Cal_um = c_max_offset_um; }
sepp_nepp	11:c6f95a42d4d7	398	else
sepp_nepp	11:c6f95a42d4d7	399	if (Offset_Cal_um < c_min_offset_um) { Offset_Cal_um = c_min_offset_um; }
nikapov	0:a1a69d32f310	400
nikapov	0:a1a69d32f310	401	/* The offset register is 10.2 format and units are mm
sepp_nepp	10:cd251e0fc2fd	402	* therefore conversion is applied by a division of 250. */
sepp_nepp	11:c6f95a42d4d7	403	if (Offset_Cal_um >= 0) { encoded_offset_val = Offset_Cal_um / 250; }
sepp_nepp	11:c6f95a42d4d7	404	else { encoded_offset_val = c_offset_range + Offset_Cal_um / 250; }
sepp_nepp	11:c6f95a42d4d7	405
sepp_nepp	11:c6f95a42d4d7	406	Write_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, encoded_offset_val);
nikapov	0:a1a69d32f310	407	}
nikapov	0:a1a69d32f310	408
sepp_nepp	11:c6f95a42d4d7	409	void VL53L0X::VL53L0X_Apply_Offset_Cal()
sepp_nepp	11:c6f95a42d4d7	410	{ int32_t Summed_Offset_Cal_um;
sepp_nepp	11:c6f95a42d4d7	411
sepp_nepp	11:c6f95a42d4d7	412	/* Read back current device offset, and remember in case later someone wants to use it */
sepp_nepp	11:c6f95a42d4d7	413	if (ErrState == VL53L0X_OK) { Last_Offset_Cal_um = Get_Offset_Cal_um(); }
nikapov	0:a1a69d32f310	414
nikapov	0:a1a69d32f310	415	/* Apply Offset Adjustment derived from 400mm measurements */
sepp_nepp	11:c6f95a42d4d7	416	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	417	{ Summed_Offset_Cal_um = Last_Offset_Cal_um + (int32_t) NVM_Offset_Cal_um;
sepp_nepp	11:c6f95a42d4d7	418	Set_Offset_Cal_um(Summed_Offset_Cal_um);
sepp_nepp	11:c6f95a42d4d7	419	/* remember current,adjusted offset */
sepp_nepp	11:c6f95a42d4d7	420	if (ErrState == VL53L0X_OK) { CurrParams.Offset_Cal_um = Summed_Offset_Cal_um; }
sepp_nepp	11:c6f95a42d4d7	421	}
sepp_nepp	11:c6f95a42d4d7	422	}
sepp_nepp	11:c6f95a42d4d7	423
sepp_nepp	11:c6f95a42d4d7	424	void VL53L0X::Get_measure_period_ms(uint32_t *p_measure_period_ms)
sepp_nepp	11:c6f95a42d4d7	425	{ uint16_t osc_calibrate_val;
sepp_nepp	11:c6f95a42d4d7	426	uint32_t im_period_ms;
sepp_nepp	11:c6f95a42d4d7	427
sepp_nepp	11:c6f95a42d4d7	428	osc_calibrate_val = Read_Word(REG_OSC_CALIBRATE_VAL);
sepp_nepp	11:c6f95a42d4d7	429
sepp_nepp	11:c6f95a42d4d7	430	if (ErrState == VL53L0X_OK) { im_period_ms = Read_DWord(REG_SYSTEM_MEASURE_PERIOD); }
sepp_nepp	11:c6f95a42d4d7	431
sepp_nepp	11:c6f95a42d4d7	432	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	433	if (osc_calibrate_val != 0)
sepp_nepp	11:c6f95a42d4d7	434	{*p_measure_period_ms = im_period_ms / osc_calibrate_val; }
sepp_nepp	11:c6f95a42d4d7	435	CurrParams.Measure_Period_ms = *p_measure_period_ms;
sepp_nepp	11:c6f95a42d4d7	436	}
sepp_nepp	11:c6f95a42d4d7	437	}
sepp_nepp	11:c6f95a42d4d7	438
sepp_nepp	11:c6f95a42d4d7	439	void VL53L0X::Get_Xtalk_CompRate_MHz( TFP1616 *p_Xtalk_CompRate_MHz)
sepp_nepp	11:c6f95a42d4d7	440	{ uint16_t value;
sepp_nepp	11:c6f95a42d4d7	441	TFP1616 temp_fix1616;
sepp_nepp	11:c6f95a42d4d7	442
sepp_nepp	11:c6f95a42d4d7	443	value = Read_Word(REG_XTALK_COMPENS_RATE_MHz);
sepp_nepp	11:c6f95a42d4d7	444
sepp_nepp	11:c6f95a42d4d7	445	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	446	if (value == 0) {
sepp_nepp	11:c6f95a42d4d7	447	/* the Xtalk is disabled return value from memory */
sepp_nepp	11:c6f95a42d4d7	448	temp_fix1616 = CurrParams.Xtalk_CompRate_MHz;
sepp_nepp	11:c6f95a42d4d7	449	*p_Xtalk_CompRate_MHz = temp_fix1616;
sepp_nepp	11:c6f95a42d4d7	450	CurrParams.XTalk_Compens_En = 0;
sepp_nepp	11:c6f95a42d4d7	451	} else {
sepp_nepp	11:c6f95a42d4d7	452	temp_fix1616 = FP313_TO_FP1616(value);
sepp_nepp	11:c6f95a42d4d7	453	*p_Xtalk_CompRate_MHz = temp_fix1616;
sepp_nepp	11:c6f95a42d4d7	454	CurrParams.Xtalk_CompRate_MHz = temp_fix1616;
sepp_nepp	11:c6f95a42d4d7	455	CurrParams.XTalk_Compens_En = 1;
nikapov	0:a1a69d32f310	456	}
nikapov	0:a1a69d32f310	457	}
nikapov	0:a1a69d32f310	458	}
nikapov	0:a1a69d32f310	459
sepp_nepp	11:c6f95a42d4d7	460	TFP1616 VL53L0X::Get_limit_chk_val( uint16_t limit_check_id )
sepp_nepp	11:c6f95a42d4d7	461	{ uint16_t temp16;
sepp_nepp	11:c6f95a42d4d7	462	TFP1616 temp_fix1616;
nikapov	0:a1a69d32f310	463
nikapov	0:a1a69d32f310	464	switch (limit_check_id) {
sepp_nepp	11:c6f95a42d4d7	465	case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* only internal computations: */
sepp_nepp	11:c6f95a42d4d7	466	case VL53L0X_CHECKEN_SIG_REF_CLIP:
sepp_nepp	11:c6f95a42d4d7	467	case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD:
sepp_nepp	11:c6f95a42d4d7	468	return CurrParams.Limit_Chk_Val[limit_check_id];// need no more 'break';
sepp_nepp	11:c6f95a42d4d7	469
sepp_nepp	11:c6f95a42d4d7	470	case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
sepp_nepp	11:c6f95a42d4d7	471	temp16 = Read_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT);
sepp_nepp	11:c6f95a42d4d7	472	temp_fix1616 = FP97_TO_FP1616(temp16);
sepp_nepp	11:c6f95a42d4d7	473	if (temp_fix1616 == 0) /* disabled: return value from memory instead*/
sepp_nepp	11:c6f95a42d4d7	474	{ temp_fix1616 = CurrParams.Limit_Chk_Val[limit_check_id];
sepp_nepp	11:c6f95a42d4d7	475	CurrParams.Limit_Chk_En[limit_check_id] = 0; }
sepp_nepp	11:c6f95a42d4d7	476	else
sepp_nepp	11:c6f95a42d4d7	477	{ CurrParams.Limit_Chk_Val[limit_check_id] = temp_fix1616;
sepp_nepp	11:c6f95a42d4d7	478	CurrParams.Limit_Chk_En[limit_check_id] = 1; }
sepp_nepp	11:c6f95a42d4d7	479	return temp_fix1616; // need no more 'break';
sepp_nepp	11:c6f95a42d4d7	480
sepp_nepp	11:c6f95a42d4d7	481	case VL53L0X_CHECKEN_SIG_RATE_MSRC:
sepp_nepp	11:c6f95a42d4d7	482	case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
sepp_nepp	11:c6f95a42d4d7	483	temp16 = Read_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT);
sepp_nepp	11:c6f95a42d4d7	484	return FP97_TO_FP1616(temp16); // need no more break;
nikapov	0:a1a69d32f310	485
nikapov	0:a1a69d32f310	486	default:
sepp_nepp	11:c6f95a42d4d7	487	ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp	11:c6f95a42d4d7	488	return 0;
nikapov	0:a1a69d32f310	489	}
nikapov	0:a1a69d32f310	490	}
nikapov	0:a1a69d32f310	491
sepp_nepp	11:c6f95a42d4d7	492	uint8_t VL53L0X::Get_limit_chk_en(uint16_t limit_check_id )
sepp_nepp	11:c6f95a42d4d7	493	{ if (limit_check_id >= VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
sepp_nepp	11:c6f95a42d4d7	494	{ ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp	11:c6f95a42d4d7	495	return 0; }
sepp_nepp	11:c6f95a42d4d7	496	else { return CurrParams.Limit_Chk_En[limit_check_id]; }
nikapov	0:a1a69d32f310	497	}
nikapov	0:a1a69d32f310	498
sepp_nepp	11:c6f95a42d4d7	499	uint8_t VL53L0X::Get_Wrap_Around_Chk_En()
sepp_nepp	11:c6f95a42d4d7	500	{ /* Now using the private state field SequenceConfig instead of reading from device:
sepp_nepp	11:c6f95a42d4d7	501	uint8_t SequenceConfig;
sepp_nepp	11:c6f95a42d4d7	502	SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
sepp_nepp	11:c6f95a42d4d7	503	Set_SequenceConfig( SequenceConfig ); // checks for ErrState
sepp_nepp	5:b95f6951f7d5	504
sepp_nepp	11:c6f95a42d4d7	505	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	506	*/
sepp_nepp	11:c6f95a42d4d7	507	CurrParams.Wrap_Around_Chk_En = (SequenceConfig >> 7) & 0x01;
sepp_nepp	11:c6f95a42d4d7	508	return CurrParams.Wrap_Around_Chk_En;
sepp_nepp	11:c6f95a42d4d7	509	// } else return 0;
nikapov	0:a1a69d32f310	510	}
nikapov	0:a1a69d32f310	511
sepp_nepp	11:c6f95a42d4d7	512	VL53L0X_Sequence_Steps_t VL53L0X::Get_sequence_step_enables()
sepp_nepp	11:c6f95a42d4d7	513	{ VL53L0X_Sequence_Steps_t p_sequence_steps;
sepp_nepp	11:c6f95a42d4d7	514	/* Now using the private state field SequenceConfig instead of reading from device:
sepp_nepp	11:c6f95a42d4d7	515	uint8_t SequenceConfig;
sepp_nepp	11:c6f95a42d4d7	516
sepp_nepp	11:c6f95a42d4d7	517	SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
sepp_nepp	11:c6f95a42d4d7	518
sepp_nepp	11:c6f95a42d4d7	519	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	520	*/
sepp_nepp	11:c6f95a42d4d7	521	p_sequence_steps.TccOn = (SequenceConfig & 0x10) >> 4;
sepp_nepp	11:c6f95a42d4d7	522	p_sequence_steps.DssOn = (SequenceConfig & 0x08) >> 3;
sepp_nepp	11:c6f95a42d4d7	523	p_sequence_steps.MsrcOn = (SequenceConfig & 0x04) >> 2;
sepp_nepp	11:c6f95a42d4d7	524	p_sequence_steps.PreRangeOn = (SequenceConfig & 0x40) >> 6;
sepp_nepp	11:c6f95a42d4d7	525	p_sequence_steps.FinalRangeOn = (SequenceConfig & 0x80) >> 7;
sepp_nepp	11:c6f95a42d4d7	526	// }
sepp_nepp	11:c6f95a42d4d7	527	return p_sequence_steps;
nikapov	0:a1a69d32f310	528	}
nikapov	0:a1a69d32f310	529
sepp_nepp	11:c6f95a42d4d7	530	void VL53L0X::Set_vcsel_PPeriod(VL53L0X_Range_Phase Vcsel_Range_Phase, uint8_t vcsel_PPeriod_pclk)
sepp_nepp	11:c6f95a42d4d7	531	{ uint8_t vcsel_period_reg;
nikapov	0:a1a69d32f310	532	uint8_t min_pre_vcsel_period_pclk = 12;
nikapov	0:a1a69d32f310	533	uint8_t max_pre_vcsel_period_pclk = 18;
nikapov	0:a1a69d32f310	534	uint8_t min_final_vcsel_period_pclk = 8;
nikapov	0:a1a69d32f310	535	uint8_t max_final_vcsel_period_pclk = 14;
sepp_nepp	10:cd251e0fc2fd	536	uint32_t final_range_timeout_us;
sepp_nepp	10:cd251e0fc2fd	537	uint32_t pre_range_timeout_us;
sepp_nepp	10:cd251e0fc2fd	538	uint32_t msrc_timeout_us;
nikapov	0:a1a69d32f310	539	uint8_t phase_cal_int = 0;
nikapov	0:a1a69d32f310	540
nikapov	0:a1a69d32f310	541	/* Check if valid clock period requested */
sepp_nepp	11:c6f95a42d4d7	542	if ( ((vcsel_PPeriod_pclk % 2) != 0 ) /* Value must be an even number */
sepp_nepp	11:c6f95a42d4d7	543	\|\|
sepp_nepp	11:c6f95a42d4d7	544	( Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE &&
sepp_nepp	11:c6f95a42d4d7	545	((vcsel_PPeriod_pclk < min_pre_vcsel_period_pclk)\|\|
sepp_nepp	11:c6f95a42d4d7	546	(vcsel_PPeriod_pclk > max_pre_vcsel_period_pclk) ) )
sepp_nepp	11:c6f95a42d4d7	547	\|\|
sepp_nepp	11:c6f95a42d4d7	548	( Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE &&
sepp_nepp	11:c6f95a42d4d7	549	(vcsel_PPeriod_pclk < min_final_vcsel_period_pclk \|\|
sepp_nepp	11:c6f95a42d4d7	550	vcsel_PPeriod_pclk > max_final_vcsel_period_pclk) ) )
sepp_nepp	11:c6f95a42d4d7	551	{ ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp	11:c6f95a42d4d7	552	return;}
nikapov	0:a1a69d32f310	553
nikapov	0:a1a69d32f310	554	/* Apply specific settings for the requested clock period */
sepp_nepp	11:c6f95a42d4d7	555	if (Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE) {
sepp_nepp	11:c6f95a42d4d7	556	/* Set phase check limits for pre-ranging*/
sepp_nepp	11:c6f95a42d4d7	557	if (vcsel_PPeriod_pclk == 12) {
sepp_nepp	11:c6f95a42d4d7	558	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x18);
sepp_nepp	11:c6f95a42d4d7	559	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp	11:c6f95a42d4d7	560	} else if (vcsel_PPeriod_pclk == 14) {
sepp_nepp	11:c6f95a42d4d7	561	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x30);
sepp_nepp	11:c6f95a42d4d7	562	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp	11:c6f95a42d4d7	563	} else if (vcsel_PPeriod_pclk == 16) {
sepp_nepp	11:c6f95a42d4d7	564	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x40);
sepp_nepp	11:c6f95a42d4d7	565	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp	11:c6f95a42d4d7	566	} else if (vcsel_PPeriod_pclk == 18) {
sepp_nepp	11:c6f95a42d4d7	567	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x50);
sepp_nepp	11:c6f95a42d4d7	568	Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
nikapov	0:a1a69d32f310	569	}
sepp_nepp	11:c6f95a42d4d7	570	} else if (Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE) {
sepp_nepp	11:c6f95a42d4d7	571	if (vcsel_PPeriod_pclk == 8) {
sepp_nepp	11:c6f95a42d4d7	572	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10);
sepp_nepp	11:c6f95a42d4d7	573	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp	11:c6f95a42d4d7	574	Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH ,0x02);
sepp_nepp	11:c6f95a42d4d7	575	Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C);
sepp_nepp	11:c6f95a42d4d7	576	Write_Byte(0xff,0x01);
sepp_nepp	11:c6f95a42d4d7	577	Write_Byte(REG_ALGO_PHASECAL_LIM,0x30);
sepp_nepp	11:c6f95a42d4d7	578	Write_Byte(0xff,0x00);
sepp_nepp	11:c6f95a42d4d7	579	} else if (vcsel_PPeriod_pclk == 10) {
sepp_nepp	11:c6f95a42d4d7	580	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28);
sepp_nepp	11:c6f95a42d4d7	581	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
sepp_nepp	11:c6f95a42d4d7	582	Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp	11:c6f95a42d4d7	583	Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09);
sepp_nepp	11:c6f95a42d4d7	584	Write_Byte(0xff,0x01);
sepp_nepp	11:c6f95a42d4d7	585	Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
sepp_nepp	11:c6f95a42d4d7	586	Write_Byte(0xff,0x00);
sepp_nepp	11:c6f95a42d4d7	587	} else if (vcsel_PPeriod_pclk == 12) {
sepp_nepp	11:c6f95a42d4d7	588	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x38);
sepp_nepp	11:c6f95a42d4d7	589	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
sepp_nepp	11:c6f95a42d4d7	590	Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp	11:c6f95a42d4d7	591	Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08);
sepp_nepp	11:c6f95a42d4d7	592	Write_Byte(0xff,0x01);
sepp_nepp	11:c6f95a42d4d7	593	Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
sepp_nepp	11:c6f95a42d4d7	594	Write_Byte(0xff,0x00);
sepp_nepp	11:c6f95a42d4d7	595	} else if (vcsel_PPeriod_pclk == 14) {
sepp_nepp	11:c6f95a42d4d7	596	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048);
sepp_nepp	11:c6f95a42d4d7	597	Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
sepp_nepp	11:c6f95a42d4d7	598	Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp	11:c6f95a42d4d7	599	Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07);
sepp_nepp	11:c6f95a42d4d7	600	Write_Byte(0xff,0x01);
sepp_nepp	11:c6f95a42d4d7	601	Write_Byte(REG_ALGO_PHASECAL_LIM, 0x20);
sepp_nepp	11:c6f95a42d4d7	602	Write_Byte(0xff,0x00);
nikapov	0:a1a69d32f310	603	}
nikapov	0:a1a69d32f310	604	}
nikapov	0:a1a69d32f310	605
sepp_nepp	7:41cbc431e1f4	606	/* Re-calculate and apply timeouts,in macro periods */
sepp_nepp	11:c6f95a42d4d7	607	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	608	/* Converts the encoded VCSEL period register value into the real period in PLL clocks */
sepp_nepp	11:c6f95a42d4d7	609	/* Flattened from procedure called Encode_vcsel_period */
sepp_nepp	11:c6f95a42d4d7	610	vcsel_period_reg = (vcsel_PPeriod_pclk >> 1) - 1;
sepp_nepp	11:c6f95a42d4d7	611
nikapov	0:a1a69d32f310	612	/* When the VCSEL period for the pre or final range is changed,
nikapov	0:a1a69d32f310	613	* the corresponding timeout must be read from the device using
sepp_nepp	7:41cbc431e1f4	614	* the current VCSEL period,then the new VCSEL period can be
nikapov	0:a1a69d32f310	615	* applied. The timeout then must be written back to the device
nikapov	0:a1a69d32f310	616	* using the new VCSEL period.
sepp_nepp	7:41cbc431e1f4	617	* For the MSRC timeout,the same applies - this timeout being
nikapov	0:a1a69d32f310	618	* dependant on the pre-range vcsel period.
nikapov	0:a1a69d32f310	619	*/
sepp_nepp	11:c6f95a42d4d7	620	switch (Vcsel_Range_Phase) {
sepp_nepp	11:c6f95a42d4d7	621	case VL53L0X_VCSEL_PRE_RANGE:
sepp_nepp	11:c6f95a42d4d7	622	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&pre_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	623
sepp_nepp	11:c6f95a42d4d7	624	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	625	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,&msrc_timeout_us);
sepp_nepp	11:c6f95a42d4d7	626
sepp_nepp	11:c6f95a42d4d7	627	Write_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
sepp_nepp	11:c6f95a42d4d7	628
sepp_nepp	11:c6f95a42d4d7	629	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	630	Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,pre_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	631
sepp_nepp	11:c6f95a42d4d7	632	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	633	Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,msrc_timeout_us);
sepp_nepp	11:c6f95a42d4d7	634
sepp_nepp	12:aa177f0e4c10	635	PreRangeVcselPPeriod = vcsel_PPeriod_pclk;
sepp_nepp	11:c6f95a42d4d7	636	break;
sepp_nepp	11:c6f95a42d4d7	637
sepp_nepp	11:c6f95a42d4d7	638	case VL53L0X_VCSEL_FINAL_RANGE:
sepp_nepp	11:c6f95a42d4d7	639	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&final_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	640
sepp_nepp	11:c6f95a42d4d7	641	Write_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
sepp_nepp	11:c6f95a42d4d7	642
sepp_nepp	11:c6f95a42d4d7	643	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	644	Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,final_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	645
sepp_nepp	12:aa177f0e4c10	646	FinalRangeVcselPPeriod = vcsel_PPeriod_pclk;
sepp_nepp	11:c6f95a42d4d7	647	break;
sepp_nepp	11:c6f95a42d4d7	648	default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	649	}
nikapov	0:a1a69d32f310	650	}
nikapov	0:a1a69d32f310	651
sepp_nepp	11:c6f95a42d4d7	652	/* Finally,the timing budget is re-applied */
sepp_nepp	11:c6f95a42d4d7	653	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	654	{ Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
sepp_nepp	11:c6f95a42d4d7	655
sepp_nepp	11:c6f95a42d4d7	656	/* Perform the phase calibration. This is needed after changing on vcsel period.
sepp_nepp	7:41cbc431e1f4	657	* get_data_enable = 0,restore_config = 1 */
sepp_nepp	11:c6f95a42d4d7	658	Perf_phase_calibration(&phase_cal_int,0,1);
nikapov	0:a1a69d32f310	659	}
nikapov	0:a1a69d32f310	660
sepp_nepp	11:c6f95a42d4d7	661	#define VL53L0X_MACRO_PERIOD_NS 3813; // = ( VL53L0X_PLL_PERIOD_PS * VL53L0X_MACRO_PERIOD_VCLKS / 1000 )
nikapov	0:a1a69d32f310	662
nikapov	0:a1a69d32f310	663	/* To convert register value into us */
sepp_nepp	11:c6f95a42d4d7	664	uint32_t VL53L0X::Calc_timeout_us(uint16_t timeout_period_mclks,
nikapov	0:a1a69d32f310	665	uint8_t vcsel_period_pclks)
nikapov	0:a1a69d32f310	666	{
nikapov	0:a1a69d32f310	667	uint32_t macro_period_ns;
nikapov	0:a1a69d32f310	668	uint32_t actual_timeout_period_us = 0;
nikapov	0:a1a69d32f310	669
sepp_nepp	11:c6f95a42d4d7	670	macro_period_ns = (uint32_t) (vcsel_period_pclks ) * VL53L0X_MACRO_PERIOD_NS;
sepp_nepp	11:c6f95a42d4d7	671
sepp_nepp	11:c6f95a42d4d7	672	actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
nikapov	0:a1a69d32f310	673
nikapov	0:a1a69d32f310	674	return actual_timeout_period_us;
nikapov	0:a1a69d32f310	675	}
nikapov	0:a1a69d32f310	676
sepp_nepp	11:c6f95a42d4d7	677	void VL53L0X::Get_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
nikapov	0:a1a69d32f310	678	uint32_t *p_time_out_micro_secs)
sepp_nepp	11:c6f95a42d4d7	679	{ uint8_t current_vcsel_PPeriod_p_clk;
nikapov	0:a1a69d32f310	680	uint8_t encoded_time_out_byte = 0;
sepp_nepp	10:cd251e0fc2fd	681	uint32_t timeout_us = 0;
nikapov	0:a1a69d32f310	682	uint16_t pre_range_encoded_time_out = 0;
nikapov	0:a1a69d32f310	683	uint16_t msrc_time_out_m_clks;
nikapov	0:a1a69d32f310	684	uint16_t pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	685	uint16_t final_range_time_out_m_clks = 0;
nikapov	0:a1a69d32f310	686	uint16_t final_range_encoded_time_out;
sepp_nepp	11:c6f95a42d4d7	687	VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp	11:c6f95a42d4d7	688
sepp_nepp	11:c6f95a42d4d7	689	if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) \|\|
sepp_nepp	11:c6f95a42d4d7	690	(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) \|\|
sepp_nepp	11:c6f95a42d4d7	691	(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC) ) {
sepp_nepp	11:c6f95a42d4d7	692
sepp_nepp	11:c6f95a42d4d7	693	current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	694	( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp	11:c6f95a42d4d7	695
sepp_nepp	11:c6f95a42d4d7	696	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	697	encoded_time_out_byte = Read_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP);
nikapov	0:a1a69d32f310	698	}
sepp_nepp	11:c6f95a42d4d7	699	msrc_time_out_m_clks = Decode_timeout(encoded_time_out_byte);
sepp_nepp	11:c6f95a42d4d7	700
sepp_nepp	11:c6f95a42d4d7	701	timeout_us = Calc_timeout_us(msrc_time_out_m_clks,
sepp_nepp	11:c6f95a42d4d7	702	current_vcsel_PPeriod_p_clk);
nikapov	0:a1a69d32f310	703	} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
sepp_nepp	11:c6f95a42d4d7	704
sepp_nepp	11:c6f95a42d4d7	705	current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	706	( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
nikapov	0:a1a69d32f310	707
nikapov	0:a1a69d32f310	708	/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp	11:c6f95a42d4d7	709	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	710
sepp_nepp	11:c6f95a42d4d7	711	pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
sepp_nepp	11:c6f95a42d4d7	712
sepp_nepp	11:c6f95a42d4d7	713	pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
sepp_nepp	11:c6f95a42d4d7	714
sepp_nepp	11:c6f95a42d4d7	715	timeout_us = Calc_timeout_us(pre_range_time_out_m_clks,
sepp_nepp	11:c6f95a42d4d7	716	current_vcsel_PPeriod_p_clk);
nikapov	0:a1a69d32f310	717	}
nikapov	0:a1a69d32f310	718	} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov	0:a1a69d32f310	719
sepp_nepp	11:c6f95a42d4d7	720	sequence_steps = Get_sequence_step_enables();
nikapov	0:a1a69d32f310	721	pre_range_time_out_m_clks = 0;
nikapov	0:a1a69d32f310	722
sepp_nepp	11:c6f95a42d4d7	723	if (sequence_steps.PreRangeOn) {
sepp_nepp	11:c6f95a42d4d7	724	current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	725	( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp	11:c6f95a42d4d7	726
sepp_nepp	11:c6f95a42d4d7	727	/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp	11:c6f95a42d4d7	728	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	729	pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
sepp_nepp	11:c6f95a42d4d7	730	pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	731	}
nikapov	0:a1a69d32f310	732	}
nikapov	0:a1a69d32f310	733
sepp_nepp	11:c6f95a42d4d7	734	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	735	current_vcsel_PPeriod_p_clk = /* Get and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	736	( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp	11:c6f95a42d4d7	737
nikapov	0:a1a69d32f310	738	}
nikapov	0:a1a69d32f310	739
nikapov	0:a1a69d32f310	740	/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp	11:c6f95a42d4d7	741	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	742	final_range_encoded_time_out = Read_Word(REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
sepp_nepp	11:c6f95a42d4d7	743	final_range_time_out_m_clks = Decode_timeout(final_range_encoded_time_out);
nikapov	0:a1a69d32f310	744	}
nikapov	0:a1a69d32f310	745
nikapov	0:a1a69d32f310	746	final_range_time_out_m_clks -= pre_range_time_out_m_clks;
sepp_nepp	11:c6f95a42d4d7	747	timeout_us = Calc_timeout_us(final_range_time_out_m_clks,current_vcsel_PPeriod_p_clk);
nikapov	0:a1a69d32f310	748	}
nikapov	0:a1a69d32f310	749
sepp_nepp	10:cd251e0fc2fd	750	*p_time_out_micro_secs = timeout_us;
nikapov	0:a1a69d32f310	751	}
nikapov	0:a1a69d32f310	752
sepp_nepp	11:c6f95a42d4d7	753	uint32_t VL53L0X::Get_Measure_Time_Budget_us()
sepp_nepp	11:c6f95a42d4d7	754	{ VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp	11:c6f95a42d4d7	755	uint32_t p_Measure_Time_Budget_us;
sepp_nepp	10:cd251e0fc2fd	756	uint32_t final_range_timeout_us;
sepp_nepp	11:c6f95a42d4d7	757	uint32_t msrc_dcc_tcc_timeout_us= 2000;
sepp_nepp	10:cd251e0fc2fd	758	uint32_t start_overhead_us = 1910;
sepp_nepp	11:c6f95a42d4d7	759	uint32_t end_overhead_us = 960;
sepp_nepp	10:cd251e0fc2fd	760	uint32_t msrc_overhead_us = 660;
sepp_nepp	11:c6f95a42d4d7	761	uint32_t tcc_overhead_us = 590;
sepp_nepp	11:c6f95a42d4d7	762	uint32_t dss_overhead_us = 690;
sepp_nepp	10:cd251e0fc2fd	763	uint32_t pre_range_overhead_us = 660;
sepp_nepp	11:c6f95a42d4d7	764	uint32_t final_range_overhead_us= 550;
sepp_nepp	10:cd251e0fc2fd	765	uint32_t pre_range_timeout_us = 0;
nikapov	0:a1a69d32f310	766
sepp_nepp	11:c6f95a42d4d7	767	if (ErrState != VL53L0X_OK) {return 0; } // do nothing while in Error State!!!!
sepp_nepp	11:c6f95a42d4d7	768
nikapov	0:a1a69d32f310	769	/* Start and end overhead times always present */
sepp_nepp	11:c6f95a42d4d7	770	p_Measure_Time_Budget_us = start_overhead_us + end_overhead_us;
sepp_nepp	11:c6f95a42d4d7	771
sepp_nepp	11:c6f95a42d4d7	772	sequence_steps = Get_sequence_step_enables();
sepp_nepp	11:c6f95a42d4d7	773
sepp_nepp	11:c6f95a42d4d7	774	if (sequence_steps.TccOn \|\| sequence_steps.MsrcOn \|\| sequence_steps.DssOn)
sepp_nepp	11:c6f95a42d4d7	775	{ Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us);
sepp_nepp	11:c6f95a42d4d7	776
sepp_nepp	11:c6f95a42d4d7	777	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	778	if (sequence_steps.TccOn)
sepp_nepp	11:c6f95a42d4d7	779	{ p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + tcc_overhead_us; }
sepp_nepp	11:c6f95a42d4d7	780
sepp_nepp	11:c6f95a42d4d7	781	if (sequence_steps.DssOn) {
sepp_nepp	11:c6f95a42d4d7	782	p_Measure_Time_Budget_us += 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
sepp_nepp	11:c6f95a42d4d7	783	} else if (sequence_steps.MsrcOn) {
sepp_nepp	11:c6f95a42d4d7	784	p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + msrc_overhead_us;
nikapov	0:a1a69d32f310	785	}
nikapov	0:a1a69d32f310	786	}
nikapov	0:a1a69d32f310	787	}
nikapov	0:a1a69d32f310	788
sepp_nepp	11:c6f95a42d4d7	789	if ( (ErrState == VL53L0X_OK) && sequence_steps.PreRangeOn) {
sepp_nepp	11:c6f95a42d4d7	790	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	791	p_Measure_Time_Budget_us += pre_range_timeout_us + pre_range_overhead_us;
sepp_nepp	11:c6f95a42d4d7	792	}
sepp_nepp	11:c6f95a42d4d7	793
sepp_nepp	11:c6f95a42d4d7	794	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	795	if (sequence_steps.FinalRangeOn) {
sepp_nepp	11:c6f95a42d4d7	796	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, &final_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	797	p_Measure_Time_Budget_us += (final_range_timeout_us + final_range_overhead_us);
nikapov	0:a1a69d32f310	798	}
nikapov	0:a1a69d32f310	799	}
nikapov	0:a1a69d32f310	800
sepp_nepp	11:c6f95a42d4d7	801	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	802	{ CurrParams.Measure_Time_Budget_us = p_Measure_Time_Budget_us; }
sepp_nepp	11:c6f95a42d4d7	803
sepp_nepp	11:c6f95a42d4d7	804	return p_Measure_Time_Budget_us;
nikapov	0:a1a69d32f310	805	}
nikapov	0:a1a69d32f310	806
sepp_nepp	11:c6f95a42d4d7	807	VL53L0X_DeviceParams_t VL53L0X::Get_device_parameters()
sepp_nepp	11:c6f95a42d4d7	808	{ VL53L0X_DeviceParams_t device_params = {0};
sepp_nepp	11:c6f95a42d4d7	809	int i;
sepp_nepp	11:c6f95a42d4d7	810
sepp_nepp	11:c6f95a42d4d7	811	if (ErrState != VL53L0X_OK) {return device_params; } // do nothing while in Error State!!!!
sepp_nepp	11:c6f95a42d4d7	812
sepp_nepp	11:c6f95a42d4d7	813	device_params.DeviceMode = CurrParams.DeviceMode;
sepp_nepp	11:c6f95a42d4d7	814	device_params.XTalk_Compens_En = 0;
sepp_nepp	11:c6f95a42d4d7	815	device_params.Offset_Cal_um = Get_Offset_Cal_um();
sepp_nepp	11:c6f95a42d4d7	816
sepp_nepp	11:c6f95a42d4d7	817	Get_measure_period_ms(&(device_params.Measure_Period_ms));
sepp_nepp	11:c6f95a42d4d7	818
sepp_nepp	11:c6f95a42d4d7	819	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	820	Get_Xtalk_CompRate_MHz(&(device_params.Xtalk_CompRate_MHz));
sepp_nepp	11:c6f95a42d4d7	821
sepp_nepp	11:c6f95a42d4d7	822	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	823	for (i = 0; i < VL53L0X_CHECKEN_NUMBER_OF_CHECKS; i++)
sepp_nepp	11:c6f95a42d4d7	824	{/* get first the values,then the enables. GetLimitCheckValue will
sepp_nepp	11:c6f95a42d4d7	825	modify the enable flags */
sepp_nepp	11:c6f95a42d4d7	826	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	827	{ device_params.Limit_Chk_Val[i] = Get_limit_chk_val(i); }
sepp_nepp	11:c6f95a42d4d7	828	else { break; }
sepp_nepp	11:c6f95a42d4d7	829	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	830	{ device_params.Limit_Chk_En[i]= Get_limit_chk_en(i);}
sepp_nepp	10:cd251e0fc2fd	831	else { break; }
nikapov	0:a1a69d32f310	832	}
nikapov	0:a1a69d32f310	833	}
nikapov	0:a1a69d32f310	834
sepp_nepp	11:c6f95a42d4d7	835	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	836	device_params.Wrap_Around_Chk_En = Get_Wrap_Around_Chk_En();}
sepp_nepp	11:c6f95a42d4d7	837
sepp_nepp	11:c6f95a42d4d7	838	/* Need to be done at the end as it uses VCSELPPeriod */
sepp_nepp	11:c6f95a42d4d7	839	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	840	device_params.Measure_Time_Budget_us = Get_Measure_Time_Budget_us(); }
sepp_nepp	11:c6f95a42d4d7	841
sepp_nepp	11:c6f95a42d4d7	842	return device_params;
nikapov	0:a1a69d32f310	843	}
nikapov	0:a1a69d32f310	844
sepp_nepp	11:c6f95a42d4d7	845	void VL53L0X::Set_limit_chk_val(uint16_t limit_check_id, TFP1616 limit_chk_val)
sepp_nepp	11:c6f95a42d4d7	846	{ /* first verify that the ID is within bounds .. */
sepp_nepp	11:c6f95a42d4d7	847	if (limit_check_id>=VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
sepp_nepp	11:c6f95a42d4d7	848	{ ErrState = VL53L0X_ERROR_INVALID_PARAMS; return; }
sepp_nepp	11:c6f95a42d4d7	849
sepp_nepp	11:c6f95a42d4d7	850	/* Under all other circumstances store value in local array: */
sepp_nepp	11:c6f95a42d4d7	851	CurrParams.Limit_Chk_Val[limit_check_id] = limit_chk_val;
sepp_nepp	11:c6f95a42d4d7	852
sepp_nepp	11:c6f95a42d4d7	853	/* in addition, if enabled, then write the external ones also to the Registers */
sepp_nepp	11:c6f95a42d4d7	854	if (CurrParams.Limit_Chk_En[ limit_check_id ])
nikapov	0:a1a69d32f310	855	switch (limit_check_id) {
sepp_nepp	11:c6f95a42d4d7	856	case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
sepp_nepp	11:c6f95a42d4d7	857	Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp	11:c6f95a42d4d7	858	FP1616_TO_FP97(limit_chk_val));
nikapov	0:a1a69d32f310	859	break;
sepp_nepp	11:c6f95a42d4d7	860	case VL53L0X_CHECKEN_SIG_RATE_MSRC:
sepp_nepp	11:c6f95a42d4d7	861	case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
sepp_nepp	11:c6f95a42d4d7	862	Write_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp	11:c6f95a42d4d7	863	FP1616_TO_FP97(limit_chk_val));
sepp_nepp	11:c6f95a42d4d7	864	break;
sepp_nepp	11:c6f95a42d4d7	865	} // switch
nikapov	0:a1a69d32f310	866	}
nikapov	0:a1a69d32f310	867
sepp_nepp	11:c6f95a42d4d7	868
sepp_nepp	11:c6f95a42d4d7	869
sepp_nepp	11:c6f95a42d4d7	870
sepp_nepp	11:c6f95a42d4d7	871	void VL53L0X::Get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
sepp_nepp	11:c6f95a42d4d7	872	{ uint8_t intStat;
sepp_nepp	11:c6f95a42d4d7	873
sepp_nepp	11:c6f95a42d4d7	874	intStat = Read_Byte(REG_RESULT_INTERRUPT_STATUS);
sepp_nepp	11:c6f95a42d4d7	875	*p_interrupt_mask_status = intStat & 0x07;
sepp_nepp	11:c6f95a42d4d7	876	if (intStat & 0x18) { ErrState = VL53L0X_ERROR_RANGE_ERROR; }
sepp_nepp	11:c6f95a42d4d7	877	}
sepp_nepp	11:c6f95a42d4d7	878
sepp_nepp	11:c6f95a42d4d7	879	uint8_t VL53L0X::Get_Measurement_Ready()
sepp_nepp	11:c6f95a42d4d7	880	{ uint8_t sys_range_status_register;
sepp_nepp	11:c6f95a42d4d7	881	uint32_t interrupt_mask;
sepp_nepp	11:c6f95a42d4d7	882
sepp_nepp	12:aa177f0e4c10	883	if (GpioFunctionality == REG_SYSINT_GPIO_NEW_SAMPLE_READY)
sepp_nepp	11:c6f95a42d4d7	884	{ Get_interrupt_mask_status(&interrupt_mask);
sepp_nepp	11:c6f95a42d4d7	885	if (interrupt_mask == REG_SYSINT_GPIO_NEW_SAMPLE_READY)
sepp_nepp	11:c6f95a42d4d7	886	{ return 1; } else { return 0; }
sepp_nepp	11:c6f95a42d4d7	887	}
sepp_nepp	11:c6f95a42d4d7	888	else
sepp_nepp	11:c6f95a42d4d7	889	{ sys_range_status_register = Read_Byte(REG_RESULT_RANGE_STATUS);
sepp_nepp	11:c6f95a42d4d7	890	if ( ( ErrState == VL53L0X_OK ) & (sys_range_status_register & 0x01) )
sepp_nepp	11:c6f95a42d4d7	891	{ return 1; } else { return 0; }
sepp_nepp	10:cd251e0fc2fd	892	}
nikapov	0:a1a69d32f310	893	}
nikapov	0:a1a69d32f310	894
sepp_nepp	11:c6f95a42d4d7	895	void VL53L0X::Polling_delay()
nikapov	0:a1a69d32f310	896	{
sepp_nepp	11:c6f95a42d4d7	897	// do nothing VL53L0X_OsDelay();
nikapov	0:a1a69d32f310	898	}
nikapov	0:a1a69d32f310	899
sepp_nepp	11:c6f95a42d4d7	900	void VL53L0X::Poll_Measure_Completion()
sepp_nepp	11:c6f95a42d4d7	901	{ uint8_t new_data_ready;
sepp_nepp	11:c6f95a42d4d7	902	uint32_t loop_nb = 0;
sepp_nepp	11:c6f95a42d4d7	903
sepp_nepp	11:c6f95a42d4d7	904	if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp	11:c6f95a42d4d7	905
sepp_nepp	11:c6f95a42d4d7	906	new_data_ready = Get_Measurement_Ready();
sepp_nepp	11:c6f95a42d4d7	907
sepp_nepp	11:c6f95a42d4d7	908	while ((ErrState==0) && (new_data_ready != 1) )
sepp_nepp	11:c6f95a42d4d7	909	{ Polling_delay();
sepp_nepp	11:c6f95a42d4d7	910	new_data_ready = Get_Measurement_Ready();
sepp_nepp	11:c6f95a42d4d7	911	if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) ErrState=VL53L0X_ERROR_TIME_OUT;
sepp_nepp	11:c6f95a42d4d7	912	} // while ;
nikapov	0:a1a69d32f310	913	}
nikapov	0:a1a69d32f310	914
sepp_nepp	11:c6f95a42d4d7	915	/* Group Device Interrupt Functions */
sepp_nepp	11:c6f95a42d4d7	916	void VL53L0X::Clear_interrupt_mask(uint32_t interrupt_mask)
sepp_nepp	11:c6f95a42d4d7	917	{ uint8_t loop_count = 0;
nikapov	0:a1a69d32f310	918	uint8_t byte;
nikapov	0:a1a69d32f310	919
sepp_nepp	11:c6f95a42d4d7	920	if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp	11:c6f95a42d4d7	921
sepp_nepp	7:41cbc431e1f4	922	/* clear bit 0 range interrupt,bit 1 error interrupt */
nikapov	0:a1a69d32f310	923	do {
sepp_nepp	11:c6f95a42d4d7	924	Write_Byte(REG_SYSINT_CLEAR,0x01);
sepp_nepp	11:c6f95a42d4d7	925	Write_Byte(REG_SYSINT_CLEAR,0x00);
sepp_nepp	11:c6f95a42d4d7	926	byte = Read_Byte(REG_RESULT_INTERRUPT_STATUS);
sepp_nepp	11:c6f95a42d4d7	927	if (loop_count++ > 3) {ErrState =VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;}
sepp_nepp	11:c6f95a42d4d7	928	} while (((byte & 0x07) != 0x00) && (ErrState == VL53L0X_OK));
sepp_nepp	11:c6f95a42d4d7	929
nikapov	0:a1a69d32f310	930	}
nikapov	0:a1a69d32f310	931
sepp_nepp	11:c6f95a42d4d7	932	void VL53L0X::Perf_single_Ref_calibration(uint8_t vhv_init_byte)
sepp_nepp	11:c6f95a42d4d7	933	{ if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
sepp_nepp	11:c6f95a42d4d7	934	Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_START_STOP \| vhv_init_byte);
sepp_nepp	11:c6f95a42d4d7	935	Poll_Measure_Completion();
sepp_nepp	11:c6f95a42d4d7	936	Clear_interrupt_mask(0);
sepp_nepp	11:c6f95a42d4d7	937	Write_Byte(REG_SYSRANGE_START,0x00);
nikapov	0:a1a69d32f310	938	}
nikapov	0:a1a69d32f310	939
sepp_nepp	11:c6f95a42d4d7	940	void VL53L0X::Ref_calibration_io(uint8_t read_not_write,
sepp_nepp	7:41cbc431e1f4	941	uint8_t vhv_settings,uint8_t phase_cal,
sepp_nepp	7:41cbc431e1f4	942	uint8_t p_vhv_settings,uint8_t p_phase_cal,
sepp_nepp	7:41cbc431e1f4	943	const uint8_t vhv_enable,const uint8_t phase_enable)
sepp_nepp	11:c6f95a42d4d7	944	{ uint8_t phase_calint = 0;
nikapov	0:a1a69d32f310	945
nikapov	0:a1a69d32f310	946	/* Read VHV from device */
sepp_nepp	11:c6f95a42d4d7	947	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	948	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	949	Write_Byte(0xFF,0x00);
nikapov	0:a1a69d32f310	950
nikapov	0:a1a69d32f310	951	if (read_not_write) {
sepp_nepp	11:c6f95a42d4d7	952	if (vhv_enable ) { *p_vhv_settings = Read_Byte(0xCB); }
sepp_nepp	11:c6f95a42d4d7	953	if (phase_enable) { phase_calint = Read_Byte(0xEE); }
sepp_nepp	11:c6f95a42d4d7	954	}
sepp_nepp	11:c6f95a42d4d7	955	else {
sepp_nepp	11:c6f95a42d4d7	956	if (vhv_enable ) { Write_Byte(0xCB,vhv_settings); }
sepp_nepp	11:c6f95a42d4d7	957	if (phase_enable) { Register_BitMask(0xEE,0x80,phase_cal); }
sepp_nepp	11:c6f95a42d4d7	958	}
sepp_nepp	11:c6f95a42d4d7	959
sepp_nepp	11:c6f95a42d4d7	960	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	961	Write_Byte(0x00,0x01);
sepp_nepp	11:c6f95a42d4d7	962	Write_Byte(0xFF,0x00);
nikapov	0:a1a69d32f310	963
nikapov	0:a1a69d32f310	964	*p_phase_cal = (uint8_t)(phase_calint & 0xEF);
nikapov	0:a1a69d32f310	965	}
nikapov	0:a1a69d32f310	966
sepp_nepp	11:c6f95a42d4d7	967	void VL53L0X::Perf_vhv_calibration(uint8_t *p_vhv_settings,
sepp_nepp	11:c6f95a42d4d7	968	const uint8_t get_data_enable, const uint8_t restore_config)
sepp_nepp	11:c6f95a42d4d7	969	{ uint8_t orig_sequence_config = 0;
nikapov	0:a1a69d32f310	970	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	971	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	972	uint8_t phase_cal_int = 0;
nikapov	0:a1a69d32f310	973
nikapov	0:a1a69d32f310	974	/* store the value of the sequence config,
sepp_nepp	11:c6f95a42d4d7	975	* this will be reset before the end of the function */
sepp_nepp	11:c6f95a42d4d7	976	orig_sequence_config = SequenceConfig;
nikapov	0:a1a69d32f310	977
nikapov	0:a1a69d32f310	978	/* Run VHV */
sepp_nepp	11:c6f95a42d4d7	979	Set_SequenceConfig( 0x01 );
sepp_nepp	11:c6f95a42d4d7	980	Perf_single_Ref_calibration(0x40);
nikapov	0:a1a69d32f310	981
nikapov	0:a1a69d32f310	982	/* Read VHV from device */
sepp_nepp	11:c6f95a42d4d7	983	if ((ErrState == VL53L0X_OK) && (get_data_enable == 1))
sepp_nepp	11:c6f95a42d4d7	984	{ Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
sepp_nepp	11:c6f95a42d4d7	985	p_vhv_settings,&phase_cal_int, 1,0); }
sepp_nepp	11:c6f95a42d4d7	986	else { *p_vhv_settings = 0; }
sepp_nepp	11:c6f95a42d4d7	987
sepp_nepp	11:c6f95a42d4d7	988	if (restore_config) { /* restore the previous Sequence Config */
sepp_nepp	11:c6f95a42d4d7	989	Set_SequenceConfig( orig_sequence_config ); } // checks for ErrState
nikapov	0:a1a69d32f310	990	}
nikapov	0:a1a69d32f310	991
sepp_nepp	11:c6f95a42d4d7	992	void VL53L0X::Perf_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable,
nikapov	0:a1a69d32f310	993	const uint8_t restore_config)
sepp_nepp	11:c6f95a42d4d7	994	{ uint8_t orig_sequence_config;
nikapov	0:a1a69d32f310	995	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	996	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	997	uint8_t vhv_settingsint;
nikapov	0:a1a69d32f310	998
sepp_nepp	11:c6f95a42d4d7	999	if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp	11:c6f95a42d4d7	1000
sepp_nepp	11:c6f95a42d4d7	1001	/* store the value of the sequence config, this will be reset before the end of the function */
sepp_nepp	11:c6f95a42d4d7	1002	orig_sequence_config = SequenceConfig;
sepp_nepp	11:c6f95a42d4d7	1003
sepp_nepp	11:c6f95a42d4d7	1004	/* Run PhaseCal: */
sepp_nepp	11:c6f95a42d4d7	1005	Set_SequenceConfig( 0x02 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
sepp_nepp	11:c6f95a42d4d7	1006	Perf_single_Ref_calibration(0x0);
nikapov	0:a1a69d32f310	1007
nikapov	0:a1a69d32f310	1008	/* Read PhaseCal from device */
sepp_nepp	11:c6f95a42d4d7	1009	if ((ErrState == VL53L0X_OK) && (get_data_enable == 1))
sepp_nepp	11:c6f95a42d4d7	1010	{ Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
sepp_nepp	11:c6f95a42d4d7	1011	&vhv_settingsint,p_phase_cal, 0,1); }
sepp_nepp	11:c6f95a42d4d7	1012	else { *p_phase_cal = 0; }
sepp_nepp	11:c6f95a42d4d7	1013
sepp_nepp	11:c6f95a42d4d7	1014	if (restore_config) { /* restore the previous Sequence Config */
sepp_nepp	11:c6f95a42d4d7	1015	Set_SequenceConfig( orig_sequence_config ); }
nikapov	0:a1a69d32f310	1016	}
nikapov	0:a1a69d32f310	1017
sepp_nepp	11:c6f95a42d4d7	1018	void VL53L0X::Perf_Ref_calibration(uint8_t *p_vhv_settings,
sepp_nepp	10:cd251e0fc2fd	1019	uint8_t *p_phase_cal, uint8_t get_data_enable)
sepp_nepp	11:c6f95a42d4d7	1020	{ uint8_t orig_sequence_config;
nikapov	0:a1a69d32f310	1021
nikapov	0:a1a69d32f310	1022	/* store the value of the sequence config,
sepp_nepp	10:cd251e0fc2fd	1023	* this will be reset before the end of the function */
sepp_nepp	11:c6f95a42d4d7	1024	orig_sequence_config = SequenceConfig;
nikapov	0:a1a69d32f310	1025
nikapov	0:a1a69d32f310	1026	/* In the following function we don't save the config to optimize
nikapov	0:a1a69d32f310	1027	* writes on device. Config is saved and restored only once. */
sepp_nepp	11:c6f95a42d4d7	1028	Perf_vhv_calibration(p_vhv_settings,get_data_enable,0);
sepp_nepp	11:c6f95a42d4d7	1029	Perf_phase_calibration(p_phase_cal,get_data_enable,0);
sepp_nepp	11:c6f95a42d4d7	1030
sepp_nepp	11:c6f95a42d4d7	1031	/* restore the previous Sequence Config */
sepp_nepp	11:c6f95a42d4d7	1032	Set_SequenceConfig( orig_sequence_config ); // sets REG_SYSTEM_SEQUENCE_CONFIG
nikapov	0:a1a69d32f310	1033	}
nikapov	0:a1a69d32f310	1034
sepp_nepp	11:c6f95a42d4d7	1035	void VL53L0X::Get_Next_Good_SPAD(uint8_t good_SPAD_array[],uint32_t size,
sepp_nepp	7:41cbc431e1f4	1036	uint32_t curr,int32_t *p_next)
sepp_nepp	10:cd251e0fc2fd	1037	{ uint32_t start_index;
nikapov	0:a1a69d32f310	1038	uint32_t fine_offset;
sepp_nepp	11:c6f95a42d4d7	1039	uint32_t c_SPADS_per_byte = 8;
nikapov	0:a1a69d32f310	1040	uint32_t coarse_index;
nikapov	0:a1a69d32f310	1041	uint32_t fine_index;
nikapov	0:a1a69d32f310	1042	uint8_t data_byte;
nikapov	0:a1a69d32f310	1043	uint8_t success = 0;
nikapov	0:a1a69d32f310	1044
sepp_nepp	11:c6f95a42d4d7	1045	/* Starting with the current good SPAD,loop through the array to find
nikapov	0:a1a69d32f310	1046	* the next. i.e. the next bit set in the sequence.
nikapov	0:a1a69d32f310	1047	* The coarse index is the byte index of the array and the fine index is
sepp_nepp	11:c6f95a42d4d7	1048	* the index of the bit within each byte. */
nikapov	0:a1a69d32f310	1049	*p_next = -1;
nikapov	0:a1a69d32f310	1050
sepp_nepp	11:c6f95a42d4d7	1051	start_index = curr / c_SPADS_per_byte;
sepp_nepp	11:c6f95a42d4d7	1052	fine_offset = curr % c_SPADS_per_byte;
nikapov	0:a1a69d32f310	1053
nikapov	0:a1a69d32f310	1054	for (coarse_index = start_index; ((coarse_index < size) && !success);
nikapov	0:a1a69d32f310	1055	coarse_index++) {
nikapov	0:a1a69d32f310	1056	fine_index = 0;
sepp_nepp	11:c6f95a42d4d7	1057	data_byte = good_SPAD_array[coarse_index];
nikapov	0:a1a69d32f310	1058
nikapov	0:a1a69d32f310	1059	if (coarse_index == start_index) {
nikapov	0:a1a69d32f310	1060	/* locate the bit position of the provided current
sepp_nepp	11:c6f95a42d4d7	1061	* SPAD bit before iterating */
nikapov	0:a1a69d32f310	1062	data_byte >>= fine_offset;
nikapov	0:a1a69d32f310	1063	fine_index = fine_offset;
nikapov	0:a1a69d32f310	1064	}
nikapov	0:a1a69d32f310	1065
sepp_nepp	11:c6f95a42d4d7	1066	while (fine_index < c_SPADS_per_byte) {
nikapov	0:a1a69d32f310	1067	if ((data_byte & 0x1) == 1) {
nikapov	0:a1a69d32f310	1068	success = 1;
sepp_nepp	11:c6f95a42d4d7	1069	p_next = coarse_index c_SPADS_per_byte + fine_index;
nikapov	0:a1a69d32f310	1070	break;
nikapov	0:a1a69d32f310	1071	}
nikapov	0:a1a69d32f310	1072	data_byte >>= 1;
nikapov	0:a1a69d32f310	1073	fine_index++;
nikapov	0:a1a69d32f310	1074	}
nikapov	0:a1a69d32f310	1075	}
nikapov	0:a1a69d32f310	1076	}
nikapov	0:a1a69d32f310	1077
sepp_nepp	11:c6f95a42d4d7	1078	void VL53L0X::Enable_SPAD_bit(uint8_t SPAD_array[],uint32_t size,uint32_t SPAD_index)
sepp_nepp	11:c6f95a42d4d7	1079	{ uint32_t c_SPADS_per_byte = 8;
nikapov	0:a1a69d32f310	1080	uint32_t coarse_index;
nikapov	0:a1a69d32f310	1081	uint32_t fine_index;
nikapov	0:a1a69d32f310	1082
sepp_nepp	11:c6f95a42d4d7	1083	coarse_index = SPAD_index / c_SPADS_per_byte;
sepp_nepp	11:c6f95a42d4d7	1084	fine_index = SPAD_index % c_SPADS_per_byte;
sepp_nepp	11:c6f95a42d4d7	1085	if (coarse_index >= size) { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; }
sepp_nepp	11:c6f95a42d4d7	1086	else { SPAD_array[coarse_index] \|= (1 << fine_index); }
nikapov	0:a1a69d32f310	1087	}
nikapov	0:a1a69d32f310	1088
sepp_nepp	11:c6f95a42d4d7	1089	void VL53L0X::Enable_Ref_SPADS( uint8_t aperture_SPADS, uint8_t good_SPAD_array[],
sepp_nepp	11:c6f95a42d4d7	1090	uint8_t SPAD_array[], uint32_t size, uint32_t start, uint32_t offset,
sepp_nepp	11:c6f95a42d4d7	1091	uint32_t SPAD_count, uint32_t *p_last_SPAD )
sepp_nepp	11:c6f95a42d4d7	1092	{ uint32_t index;
nikapov	0:a1a69d32f310	1093	uint32_t i;
sepp_nepp	11:c6f95a42d4d7	1094	int32_t next_good_SPAD = offset;
sepp_nepp	11:c6f95a42d4d7	1095	uint32_t current_SPAD;
sepp_nepp	11:c6f95a42d4d7	1096	uint8_t check_SPAD_array[6];
sepp_nepp	11:c6f95a42d4d7	1097
sepp_nepp	11:c6f95a42d4d7	1098	/* This function takes in a SPAD array which may or may not have SPADS
nikapov	0:a1a69d32f310	1099	* already enabled and appends from a given offset a requested number
sepp_nepp	11:c6f95a42d4d7	1100	* of new SPAD enables. The 'good SPAD map' is applied to
sepp_nepp	11:c6f95a42d4d7	1101	* determine the next SPADS to enable.
nikapov	0:a1a69d32f310	1102	*
sepp_nepp	11:c6f95a42d4d7	1103	* This function applies to only aperture or only non-aperture SPADS.
nikapov	0:a1a69d32f310	1104	* Checks are performed to ensure this.
nikapov	0:a1a69d32f310	1105	*/
nikapov	0:a1a69d32f310	1106
sepp_nepp	11:c6f95a42d4d7	1107	current_SPAD = offset;
sepp_nepp	11:c6f95a42d4d7	1108	for (index = 0; index < SPAD_count; index++) {
sepp_nepp	11:c6f95a42d4d7	1109	Get_Next_Good_SPAD(good_SPAD_array,size,current_SPAD, &next_good_SPAD);
sepp_nepp	11:c6f95a42d4d7	1110
sepp_nepp	11:c6f95a42d4d7	1111	if (next_good_SPAD == -1)
sepp_nepp	11:c6f95a42d4d7	1112	{ ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp	11:c6f95a42d4d7	1113	break; }
sepp_nepp	11:c6f95a42d4d7	1114
nikapov	0:a1a69d32f310	1115
nikapov	0:a1a69d32f310	1116	/* Confirm that the next good SPAD is non-aperture */
sepp_nepp	11:c6f95a42d4d7	1117	if (Is_ApertureSPAD(start + next_good_SPAD) != aperture_SPADS) {
nikapov	0:a1a69d32f310	1118	/* if we can't get the required number of good aperture
sepp_nepp	11:c6f95a42d4d7	1119	* SPADS from the current quadrant then this is an error */
sepp_nepp	11:c6f95a42d4d7	1120	ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp	11:c6f95a42d4d7	1121	break;}
sepp_nepp	11:c6f95a42d4d7	1122
sepp_nepp	11:c6f95a42d4d7	1123	current_SPAD = (uint32_t)next_good_SPAD;
sepp_nepp	11:c6f95a42d4d7	1124	Enable_SPAD_bit(SPAD_array,size,current_SPAD);
sepp_nepp	11:c6f95a42d4d7	1125	current_SPAD++;
nikapov	0:a1a69d32f310	1126	}
sepp_nepp	11:c6f95a42d4d7	1127	*p_last_SPAD = current_SPAD;
sepp_nepp	11:c6f95a42d4d7	1128
sepp_nepp	11:c6f95a42d4d7	1129	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	1130	{ I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPAD_array,6); } // set_Ref_SPAD_map()
sepp_nepp	11:c6f95a42d4d7	1131
sepp_nepp	11:c6f95a42d4d7	1132	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	1133	// Get the ref_SPAD_map from the device
sepp_nepp	11:c6f95a42d4d7	1134	I2c_Read(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,check_SPAD_array,6);
sepp_nepp	11:c6f95a42d4d7	1135
sepp_nepp	11:c6f95a42d4d7	1136	/* Compare SPAD maps. If not equal report error. */
nikapov	0:a1a69d32f310	1137	i = 0;
nikapov	0:a1a69d32f310	1138	while (i < size) {
sepp_nepp	11:c6f95a42d4d7	1139	if (SPAD_array[i] != check_SPAD_array[i]) {
sepp_nepp	11:c6f95a42d4d7	1140	ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	1141	break;
nikapov	0:a1a69d32f310	1142	}
nikapov	0:a1a69d32f310	1143	i++;
nikapov	0:a1a69d32f310	1144	}
nikapov	0:a1a69d32f310	1145	}
nikapov	0:a1a69d32f310	1146	}
nikapov	0:a1a69d32f310	1147
sepp_nepp	11:c6f95a42d4d7	1148	void VL53L0X::Set_device_mode(VL53L0X_DeviceModes device_mode)
sepp_nepp	11:c6f95a42d4d7	1149	{ if (ErrState != VL53L0X_OK) {return; } // no reaction while in Error State!!!!
nikapov	0:a1a69d32f310	1150
nikapov	0:a1a69d32f310	1151	switch (device_mode) {
nikapov	0:a1a69d32f310	1152	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov	0:a1a69d32f310	1153	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov	0:a1a69d32f310	1154	case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov	0:a1a69d32f310	1155	case VL53L0X_DEVICEMODE_GPIO_DRIVE:
sepp_nepp	11:c6f95a42d4d7	1156	case VL53L0X_DEVICEMODE_GPIO_OSC: /* Supported modes */
sepp_nepp	11:c6f95a42d4d7	1157	CurrParams.DeviceMode = device_mode;
nikapov	0:a1a69d32f310	1158	break;
sepp_nepp	11:c6f95a42d4d7	1159	default: /* Unsupported mode */
sepp_nepp	11:c6f95a42d4d7	1160	ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov	0:a1a69d32f310	1161	}
nikapov	0:a1a69d32f310	1162	}
nikapov	0:a1a69d32f310	1163
sepp_nepp	11:c6f95a42d4d7	1164	void VL53L0X::Set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 threshold_low,
sepp_nepp	11:c6f95a42d4d7	1165	TFP1616 threshold_high)
sepp_nepp	11:c6f95a42d4d7	1166	{ uint16_t threshold16;
sepp_nepp	5:b95f6951f7d5	1167
sepp_nepp	5:b95f6951f7d5	1168	/* no dependency on DeviceMode for FlightSense */
nikapov	0:a1a69d32f310	1169	/* Need to divide by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	1170	threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
sepp_nepp	11:c6f95a42d4d7	1171	Write_Word(REG_SYSTEM_THRESH_LOW,threshold16);
sepp_nepp	11:c6f95a42d4d7	1172
sepp_nepp	11:c6f95a42d4d7	1173	/* Need to divide by 2 because the FW will apply a x2 */
sepp_nepp	11:c6f95a42d4d7	1174	threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
sepp_nepp	11:c6f95a42d4d7	1175	Write_Word(REG_SYSTEM_THRESH_HIGH,threshold16);
nikapov	0:a1a69d32f310	1176	}
nikapov	0:a1a69d32f310	1177
sepp_nepp	11:c6f95a42d4d7	1178	void VL53L0X::Get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 *p_threshold_low,
sepp_nepp	11:c6f95a42d4d7	1179	TFP1616 *p_threshold_high)
sepp_nepp	11:c6f95a42d4d7	1180	{ uint16_t threshold16;
sepp_nepp	5:b95f6951f7d5	1181
sepp_nepp	5:b95f6951f7d5	1182	/* no dependency on DeviceMode for FlightSense */
sepp_nepp	11:c6f95a42d4d7	1183	threshold16 = Read_Word(REG_SYSTEM_THRESH_LOW);
nikapov	0:a1a69d32f310	1184	/* Need to multiply by 2 because the FW will apply a x2 */
sepp_nepp	11:c6f95a42d4d7	1185	*p_threshold_low = (TFP1616)((0x00fff & threshold16) << 17);
sepp_nepp	11:c6f95a42d4d7	1186
sepp_nepp	11:c6f95a42d4d7	1187	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	1188	threshold16 = Read_Word(REG_SYSTEM_THRESH_HIGH);
nikapov	0:a1a69d32f310	1189	/* Need to multiply by 2 because the FW will apply a x2 */
sepp_nepp	11:c6f95a42d4d7	1190	*p_threshold_high = (TFP1616)((0x00fff & threshold16) << 17);
nikapov	0:a1a69d32f310	1191	}
nikapov	0:a1a69d32f310	1192	}
nikapov	0:a1a69d32f310	1193
sepp_nepp	11:c6f95a42d4d7	1194	void VL53L0X::Load_tuning_settings(uint8_t *p_tuning_setting_buffer)
sepp_nepp	11:c6f95a42d4d7	1195	{ int i;
nikapov	0:a1a69d32f310	1196	int index;
nikapov	0:a1a69d32f310	1197	uint8_t msb;
nikapov	0:a1a69d32f310	1198	uint8_t lsb;
nikapov	0:a1a69d32f310	1199	uint8_t select_param;
nikapov	0:a1a69d32f310	1200	uint8_t number_of_writes;
nikapov	0:a1a69d32f310	1201	uint8_t address;
nikapov	0:a1a69d32f310	1202	uint8_t local_buffer[4]; /* max */
nikapov	0:a1a69d32f310	1203	uint16_t temp16;
nikapov	0:a1a69d32f310	1204
nikapov	0:a1a69d32f310	1205	index = 0;
nikapov	0:a1a69d32f310	1206
nikapov	0:a1a69d32f310	1207	while ((*(p_tuning_setting_buffer + index) != 0) &&
sepp_nepp	11:c6f95a42d4d7	1208	(ErrState == VL53L0X_OK)) {
nikapov	0:a1a69d32f310	1209	number_of_writes = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1210	index++;
nikapov	0:a1a69d32f310	1211	if (number_of_writes == 0xFF) {
nikapov	0:a1a69d32f310	1212	/* internal parameters */
nikapov	0:a1a69d32f310	1213	select_param = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1214	index++;
nikapov	0:a1a69d32f310	1215	switch (select_param) {
nikapov	0:a1a69d32f310	1216	case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
nikapov	0:a1a69d32f310	1217	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1218	index++;
nikapov	0:a1a69d32f310	1219	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1220	index++;
sepp_nepp	7:41cbc431e1f4	1221	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	10:cd251e0fc2fd	1222	SigmaEstRefArray = temp16;
nikapov	0:a1a69d32f310	1223	break;
nikapov	0:a1a69d32f310	1224	case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
nikapov	0:a1a69d32f310	1225	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1226	index++;
nikapov	0:a1a69d32f310	1227	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1228	index++;
sepp_nepp	7:41cbc431e1f4	1229	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	10:cd251e0fc2fd	1230	SigmaEstEffPulseWidth = temp16;
nikapov	0:a1a69d32f310	1231	break;
nikapov	0:a1a69d32f310	1232	case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
nikapov	0:a1a69d32f310	1233	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1234	index++;
nikapov	0:a1a69d32f310	1235	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1236	index++;
sepp_nepp	7:41cbc431e1f4	1237	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	10:cd251e0fc2fd	1238	SigmaEstEffAmbWidth = temp16;
nikapov	0:a1a69d32f310	1239	break;
nikapov	0:a1a69d32f310	1240	case 3: /* uint16_t targetRefRate -> 2 bytes */
nikapov	0:a1a69d32f310	1241	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1242	index++;
nikapov	0:a1a69d32f310	1243	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1244	index++;
sepp_nepp	7:41cbc431e1f4	1245	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	10:cd251e0fc2fd	1246	targetRefRate = temp16;
nikapov	0:a1a69d32f310	1247	break;
nikapov	0:a1a69d32f310	1248	default: /* invalid parameter */
sepp_nepp	11:c6f95a42d4d7	1249	ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	1250	}
nikapov	0:a1a69d32f310	1251	} else if (number_of_writes <= 4) {
nikapov	0:a1a69d32f310	1252	address = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1253	index++;
nikapov	0:a1a69d32f310	1254	for (i = 0; i < number_of_writes; i++) {
sepp_nepp	11:c6f95a42d4d7	1255	local_buffer[i] = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	1256	index++;
nikapov	0:a1a69d32f310	1257	}
sepp_nepp	11:c6f95a42d4d7	1258	I2c_Write(address,local_buffer,number_of_writes);
nikapov	0:a1a69d32f310	1259	} else {
sepp_nepp	11:c6f95a42d4d7	1260	ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	1261	}
nikapov	0:a1a69d32f310	1262	}
sepp_nepp	11:c6f95a42d4d7	1263	}
sepp_nepp	11:c6f95a42d4d7	1264
sepp_nepp	11:c6f95a42d4d7	1265	void VL53L0X::Check_and_load_interrupt_settings(uint8_t start_not_stopflag)
sepp_nepp	11:c6f95a42d4d7	1266	{ uint8_t interrupt_config;
sepp_nepp	11:c6f95a42d4d7	1267	TFP1616 threshold_low;
sepp_nepp	11:c6f95a42d4d7	1268	TFP1616 threshold_high;
sepp_nepp	5:b95f6951f7d5	1269
sepp_nepp	11:c6f95a42d4d7	1270	if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp	11:c6f95a42d4d7	1271
sepp_nepp	12:aa177f0e4c10	1272	interrupt_config = GpioFunctionality;
sepp_nepp	11:c6f95a42d4d7	1273
sepp_nepp	11:c6f95a42d4d7	1274	if ((interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_LOW ) \|\|
sepp_nepp	11:c6f95a42d4d7	1275	(interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_HIGH) \|\|
sepp_nepp	11:c6f95a42d4d7	1276	(interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_OUT )) {
sepp_nepp	11:c6f95a42d4d7	1277	Get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp	7:41cbc431e1f4	1278	&threshold_low,&threshold_high);
nikapov	0:a1a69d32f310	1279
sepp_nepp	11:c6f95a42d4d7	1280	if (((threshold_low > 255 * 65536) \|\| (threshold_high > 255 * 65536)) &&
sepp_nepp	11:c6f95a42d4d7	1281	(ErrState == VL53L0X_OK))
sepp_nepp	11:c6f95a42d4d7	1282	{ if (start_not_stopflag != 0)
sepp_nepp	11:c6f95a42d4d7	1283	{Load_tuning_settings(InterruptThresholdSettings); }
sepp_nepp	11:c6f95a42d4d7	1284	else
sepp_nepp	11:c6f95a42d4d7	1285	{Write_Byte(0xFF,0x04);
sepp_nepp	11:c6f95a42d4d7	1286	Write_Byte(0x70,0x00);
sepp_nepp	11:c6f95a42d4d7	1287	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	1288	Write_Byte(0x80,0x00);
sepp_nepp	11:c6f95a42d4d7	1289	}
nikapov	0:a1a69d32f310	1290	}
nikapov	0:a1a69d32f310	1291	}
nikapov	0:a1a69d32f310	1292	}
nikapov	0:a1a69d32f310	1293
sepp_nepp	11:c6f95a42d4d7	1294	void VL53L0X::Start_Measurement()
sepp_nepp	11:c6f95a42d4d7	1295	{ VL53L0X_DeviceModes device_mode;
nikapov	0:a1a69d32f310	1296	uint8_t byte;
sepp_nepp	11:c6f95a42d4d7	1297	uint8_t start_stop_byte = REG_SYSRANGE_MODE_START_STOP;
nikapov	0:a1a69d32f310	1298	uint32_t loop_nb;
nikapov	0:a1a69d32f310	1299
sepp_nepp	11:c6f95a42d4d7	1300	if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
sepp_nepp	11:c6f95a42d4d7	1301
nikapov	0:a1a69d32f310	1302	/* Get Current DeviceMode */
sepp_nepp	11:c6f95a42d4d7	1303	device_mode = CurrParams.DeviceMode;
sepp_nepp	11:c6f95a42d4d7	1304
sepp_nepp	11:c6f95a42d4d7	1305	Write_Byte(0x80,0x01);
sepp_nepp	11:c6f95a42d4d7	1306	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	1307	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	1308	Write_Byte(0x91,StopVariable);
sepp_nepp	11:c6f95a42d4d7	1309	Write_Byte(0x00,0x01);
sepp_nepp	11:c6f95a42d4d7	1310	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	1311	Write_Byte(0x80,0x00);
nikapov	0:a1a69d32f310	1312
nikapov	0:a1a69d32f310	1313	switch (device_mode) {
nikapov	0:a1a69d32f310	1314	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
sepp_nepp	11:c6f95a42d4d7	1315	Write_Byte(REG_SYSRANGE_START,0x01);
nikapov	0:a1a69d32f310	1316
nikapov	0:a1a69d32f310	1317	byte = start_stop_byte;
sepp_nepp	11:c6f95a42d4d7	1318	if (ErrState == VL53L0X_OK) {
nikapov	0:a1a69d32f310	1319	/* Wait until start bit has been cleared */
nikapov	0:a1a69d32f310	1320	loop_nb = 0;
nikapov	0:a1a69d32f310	1321	do {
nikapov	0:a1a69d32f310	1322	if (loop_nb > 0)
sepp_nepp	11:c6f95a42d4d7	1323	byte = Read_Byte(REG_SYSRANGE_START);
nikapov	0:a1a69d32f310	1324	loop_nb = loop_nb + 1;
nikapov	0:a1a69d32f310	1325	} while (((byte & start_stop_byte) == start_stop_byte)
sepp_nepp	11:c6f95a42d4d7	1326	&& (ErrState == VL53L0X_OK)
nikapov	0:a1a69d32f310	1327	&& (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
nikapov	0:a1a69d32f310	1328
Davidroid	3:e9269ff624ed	1329	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp	11:c6f95a42d4d7	1330	ErrState = VL53L0X_ERROR_TIME_OUT;
Davidroid	3:e9269ff624ed	1331	}
nikapov	0:a1a69d32f310	1332	}
nikapov	0:a1a69d32f310	1333	break;
sepp_nepp	11:c6f95a42d4d7	1334
sepp_nepp	11:c6f95a42d4d7	1335	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
sepp_nepp	11:c6f95a42d4d7	1336	/* Back-to-back mode, Check if need to apply interrupt settings */
sepp_nepp	11:c6f95a42d4d7	1337	Check_and_load_interrupt_settings(1);
sepp_nepp	11:c6f95a42d4d7	1338	Write_Byte(REG_SYSRANGE_START,REG_SYSRANGE_MODE_BACKTOBACK);
sepp_nepp	11:c6f95a42d4d7	1339	Set_Current_State( VL53L0X_STATE_RUNNING );
sepp_nepp	11:c6f95a42d4d7	1340	break;
nikapov	0:a1a69d32f310	1341	case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
sepp_nepp	11:c6f95a42d4d7	1342	/* Continuous mode; Check if need to apply interrupt settings */
sepp_nepp	11:c6f95a42d4d7	1343	Check_and_load_interrupt_settings(1);
sepp_nepp	11:c6f95a42d4d7	1344	Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_TIMED);
sepp_nepp	11:c6f95a42d4d7	1345	Set_Current_State( VL53L0X_STATE_RUNNING );
nikapov	0:a1a69d32f310	1346	break;
nikapov	0:a1a69d32f310	1347	default:
nikapov	0:a1a69d32f310	1348	/* Selected mode not supported */
sepp_nepp	11:c6f95a42d4d7	1349	ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov	0:a1a69d32f310	1350	}
nikapov	0:a1a69d32f310	1351	}
nikapov	0:a1a69d32f310	1352
sepp_nepp	11:c6f95a42d4d7	1353	/* Group Device Measurement Functions */
sepp_nepp	11:c6f95a42d4d7	1354	void VL53L0X::Perf_single_measurement()
sepp_nepp	11:c6f95a42d4d7	1355	{ VL53L0X_DeviceModes device_mode;
sepp_nepp	11:c6f95a42d4d7	1356
sepp_nepp	11:c6f95a42d4d7	1357	if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
nikapov	0:a1a69d32f310	1358
nikapov	0:a1a69d32f310	1359	/* Get Current DeviceMode */
sepp_nepp	11:c6f95a42d4d7	1360	device_mode = CurrParams.DeviceMode;
nikapov	0:a1a69d32f310	1361
nikapov	0:a1a69d32f310	1362	/* Start immediately to run a single ranging measurement in case of
nikapov	0:a1a69d32f310	1363	* single ranging or single histogram */
sepp_nepp	11:c6f95a42d4d7	1364	if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {Start_Measurement();}
sepp_nepp	11:c6f95a42d4d7	1365
sepp_nepp	11:c6f95a42d4d7	1366	Poll_Measure_Completion();
sepp_nepp	11:c6f95a42d4d7	1367
sepp_nepp	11:c6f95a42d4d7	1368	/* Change Device State in case of single ranging or single histogram */
sepp_nepp	11:c6f95a42d4d7	1369	if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
sepp_nepp	11:c6f95a42d4d7	1370	{ Set_Current_State( VL53L0X_STATE_IDLE ); }
nikapov	0:a1a69d32f310	1371	}
nikapov	0:a1a69d32f310	1372
sepp_nepp	11:c6f95a42d4d7	1373	TFP1616 VL53L0X::Get_total_xtalk_rate(TRangeResults *p_ranging_results)
sepp_nepp	11:c6f95a42d4d7	1374	{ TFP1616 total_xtalk_MHz;
sepp_nepp	11:c6f95a42d4d7	1375
sepp_nepp	11:c6f95a42d4d7	1376	// CurrParams.XTalk_Compens_En was Get_xtalk_compensation_enable
sepp_nepp	11:c6f95a42d4d7	1377	if ( (ErrState == VL53L0X_OK) & (CurrParams.XTalk_Compens_En ) )
sepp_nepp	11:c6f95a42d4d7	1378	{ /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
sepp_nepp	11:c6f95a42d4d7	1379	total_xtalk_MHz = p_ranging_results->EffectiveSPADRtnCount *
sepp_nepp	11:c6f95a42d4d7	1380	CurrParams.Xtalk_CompRate_MHz;
sepp_nepp	11:c6f95a42d4d7	1381
sepp_nepp	11:c6f95a42d4d7	1382	/* FixPoint0824 >> 8 = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1383	return (total_xtalk_MHz + 0x80) >> 8;
sepp_nepp	11:c6f95a42d4d7	1384	}
sepp_nepp	11:c6f95a42d4d7	1385	else { return 0; }
nikapov	0:a1a69d32f310	1386	}
nikapov	0:a1a69d32f310	1387
sepp_nepp	11:c6f95a42d4d7	1388	void VL53L0X::Get_total_SIG_rate(TRangeResults *p_ranging_results,
sepp_nepp	11:c6f95a42d4d7	1389	TFP1616 *p_total_SIG_rate_mcps)
sepp_nepp	11:c6f95a42d4d7	1390	{ TFP1616 total_xtalk_MHz;
sepp_nepp	11:c6f95a42d4d7	1391
sepp_nepp	11:c6f95a42d4d7	1392	*p_total_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
sepp_nepp	11:c6f95a42d4d7	1393	total_xtalk_MHz = Get_total_xtalk_rate(p_ranging_results);
sepp_nepp	5:b95f6951f7d5	1394
sepp_nepp	11:c6f95a42d4d7	1395	if (ErrState == VL53L0X_OK) { *p_total_SIG_rate_mcps += total_xtalk_MHz;}
nikapov	0:a1a69d32f310	1396	}
nikapov	0:a1a69d32f310	1397
nikapov	0:a1a69d32f310	1398	/* To convert ms into register value */
sepp_nepp	11:c6f95a42d4d7	1399	uint32_t VL53L0X::Calc_timeout_mclks(uint32_t timeout_period_us,
nikapov	0:a1a69d32f310	1400	uint8_t vcsel_period_pclks)
sepp_nepp	11:c6f95a42d4d7	1401	{ uint32_t macro_period_ns;
sepp_nepp	11:c6f95a42d4d7	1402
sepp_nepp	11:c6f95a42d4d7	1403	macro_period_ns = (uint32_t)(vcsel_period_pclks) * VL53L0X_MACRO_PERIOD_NS;
sepp_nepp	11:c6f95a42d4d7	1404
sepp_nepp	11:c6f95a42d4d7	1405	return (uint32_t)(((timeout_period_us * 1000)
sepp_nepp	11:c6f95a42d4d7	1406	+ (macro_period_ns / 2)) / macro_period_ns);
nikapov	0:a1a69d32f310	1407	}
nikapov	0:a1a69d32f310	1408
sepp_nepp	11:c6f95a42d4d7	1409	uint32_t VL53L0X::ISQRT(uint32_t num)
sepp_nepp	11:c6f95a42d4d7	1410	{ /* Implements an integer square root
sepp_nepp	11:c6f95a42d4d7	1411	* From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */
nikapov	0:a1a69d32f310	1412	uint32_t res = 0;
nikapov	0:a1a69d32f310	1413	uint32_t bit = 1 << 30;
sepp_nepp	11:c6f95a42d4d7	1414	/* The second-to-top bit is set: 1 << 14 for 16-bits,1 << 30 for 32 bits */
nikapov	0:a1a69d32f310	1415
nikapov	0:a1a69d32f310	1416	/* "bit" starts at the highest power of four <= the argument. */
sepp_nepp	10:cd251e0fc2fd	1417	while (bit > num) { bit >>= 2; }
nikapov	0:a1a69d32f310	1418
nikapov	0:a1a69d32f310	1419	while (bit != 0) {
nikapov	0:a1a69d32f310	1420	if (num >= res + bit) {
nikapov	0:a1a69d32f310	1421	num -= res + bit;
nikapov	0:a1a69d32f310	1422	res = (res >> 1) + bit;
sepp_nepp	11:c6f95a42d4d7	1423	} else { res >>= 1; }
nikapov	0:a1a69d32f310	1424	bit >>= 2;
nikapov	0:a1a69d32f310	1425	}
nikapov	0:a1a69d32f310	1426	return res;
nikapov	0:a1a69d32f310	1427	}
nikapov	0:a1a69d32f310	1428
sepp_nepp	11:c6f95a42d4d7	1429	void VL53L0X::Calc_dmax(TFP1616 total_SIG_rate_mcps,
sepp_nepp	11:c6f95a42d4d7	1430	TFP1616 total_corr_SIG_rate_mcps,
sepp_nepp	11:c6f95a42d4d7	1431	TFP1616 pw_mult,
nikapov	0:a1a69d32f310	1432	uint32_t sigma_estimate_p1,
sepp_nepp	11:c6f95a42d4d7	1433	TFP1616 sigma_estimate_p2,
nikapov	0:a1a69d32f310	1434	uint32_t peak_vcsel_duration_us,
nikapov	0:a1a69d32f310	1435	uint32_t *pd_max_mm)
sepp_nepp	11:c6f95a42d4d7	1436	{ const uint32_t c_sigma_limit = 18;
sepp_nepp	11:c6f95a42d4d7	1437	const TFP1616 c_SIG_limit = 0x4000; /* 0.25 */
sepp_nepp	11:c6f95a42d4d7	1438	const TFP1616 c_sigma_est_Ref = 0x00000042; /* 0.001 */
nikapov	0:a1a69d32f310	1439	const uint32_t c_amb_eff_width_sigma_est_ns = 6;
nikapov	0:a1a69d32f310	1440	const uint32_t c_amb_eff_width_d_max_ns = 7;
nikapov	0:a1a69d32f310	1441	uint32_t dmax_cal_range_mm;
sepp_nepp	11:c6f95a42d4d7	1442	TFP1616 dmax_cal_SIG_rate_rtn_mcps;
sepp_nepp	11:c6f95a42d4d7	1443	TFP1616 min_SIG_needed;
sepp_nepp	11:c6f95a42d4d7	1444	TFP1616 min_SIG_needed_p1;
sepp_nepp	11:c6f95a42d4d7	1445	TFP1616 min_SIG_needed_p2;
sepp_nepp	11:c6f95a42d4d7	1446	TFP1616 min_SIG_needed_p3;
sepp_nepp	11:c6f95a42d4d7	1447	TFP1616 min_SIG_needed_p4;
sepp_nepp	11:c6f95a42d4d7	1448	TFP1616 sigma_limit_tmp;
sepp_nepp	11:c6f95a42d4d7	1449	TFP1616 sigma_est_sq_tmp;
sepp_nepp	11:c6f95a42d4d7	1450	TFP1616 signal_limit_tmp;
sepp_nepp	11:c6f95a42d4d7	1451	TFP1616 signal_at0_mm;
sepp_nepp	11:c6f95a42d4d7	1452	TFP1616 dmax_dark;
sepp_nepp	11:c6f95a42d4d7	1453	TFP1616 dmax_ambient;
sepp_nepp	11:c6f95a42d4d7	1454	TFP1616 dmax_dark_tmp;
sepp_nepp	11:c6f95a42d4d7	1455	TFP1616 sigma_est_p2_tmp;
nikapov	0:a1a69d32f310	1456	uint32_t signal_rate_temp_mcps;
nikapov	0:a1a69d32f310	1457
sepp_nepp	10:cd251e0fc2fd	1458	dmax_cal_range_mm = DmaxCalRangeMilliMeter;
sepp_nepp	10:cd251e0fc2fd	1459
sepp_nepp	11:c6f95a42d4d7	1460	dmax_cal_SIG_rate_rtn_mcps = DmaxCalSignalRateRtnMHz;
nikapov	0:a1a69d32f310	1461
nikapov	0:a1a69d32f310	1462	/* uint32 * FixPoint1616 = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1463	signal_at0_mm = dmax_cal_range_mm * dmax_cal_SIG_rate_rtn_mcps;
nikapov	0:a1a69d32f310	1464
nikapov	0:a1a69d32f310	1465	/* FixPoint1616 >> 8 = FixPoint2408 */
nikapov	0:a1a69d32f310	1466	signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
nikapov	0:a1a69d32f310	1467	signal_at0_mm *= dmax_cal_range_mm;
nikapov	0:a1a69d32f310	1468
sepp_nepp	11:c6f95a42d4d7	1469	min_SIG_needed_p1 = 0;
sepp_nepp	11:c6f95a42d4d7	1470	if (total_corr_SIG_rate_mcps > 0) {
sepp_nepp	11:c6f95a42d4d7	1471	/* Shift by 10 bits to increase resolution prior to the division */
sepp_nepp	11:c6f95a42d4d7	1472	signal_rate_temp_mcps = total_SIG_rate_mcps << 10;
nikapov	0:a1a69d32f310	1473
nikapov	0:a1a69d32f310	1474	/* Add rounding value prior to division */
sepp_nepp	11:c6f95a42d4d7	1475	min_SIG_needed_p1 = signal_rate_temp_mcps + (total_corr_SIG_rate_mcps / 2);
nikapov	0:a1a69d32f310	1476
nikapov	0:a1a69d32f310	1477	/* FixPoint0626/FixPoint1616 = FixPoint2210 */
sepp_nepp	11:c6f95a42d4d7	1478	min_SIG_needed_p1 /= total_corr_SIG_rate_mcps;
nikapov	0:a1a69d32f310	1479
nikapov	0:a1a69d32f310	1480	/* Apply a factored version of the speed of light.
nikapov	0:a1a69d32f310	1481	Correction to be applied at the end */
sepp_nepp	11:c6f95a42d4d7	1482	min_SIG_needed_p1 *= 3;
nikapov	0:a1a69d32f310	1483
nikapov	0:a1a69d32f310	1484	/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
sepp_nepp	11:c6f95a42d4d7	1485	min_SIG_needed_p1 *= min_SIG_needed_p1;
nikapov	0:a1a69d32f310	1486
nikapov	0:a1a69d32f310	1487	/* FixPoint1220 >> 16 = FixPoint2804 */
sepp_nepp	11:c6f95a42d4d7	1488	min_SIG_needed_p1 = (min_SIG_needed_p1 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	1489	}
nikapov	0:a1a69d32f310	1490
sepp_nepp	11:c6f95a42d4d7	1491	min_SIG_needed_p2 = pw_mult * sigma_estimate_p1;
nikapov	0:a1a69d32f310	1492
nikapov	0:a1a69d32f310	1493	/* FixPoint1616 >> 16 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1494	min_SIG_needed_p2 = (min_SIG_needed_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	1495
nikapov	0:a1a69d32f310	1496	/* uint32 * uint32 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1497	min_SIG_needed_p2 *= min_SIG_needed_p2;
sepp_nepp	11:c6f95a42d4d7	1498
sepp_nepp	11:c6f95a42d4d7	1499	/* Check sigmaEstimateP2; If this value is too high, there is not enough
sepp_nepp	11:c6f95a42d4d7	1500	* signal rate to calculate dmax value so set a suitable value to ensure
sepp_nepp	11:c6f95a42d4d7	1501	* a very small dmax. */
nikapov	0:a1a69d32f310	1502	sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	1503	sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
nikapov	0:a1a69d32f310	1504	c_amb_eff_width_sigma_est_ns;
nikapov	0:a1a69d32f310	1505	sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
nikapov	0:a1a69d32f310	1506
nikapov	0:a1a69d32f310	1507	if (sigma_est_p2_tmp > 0xffff) {
sepp_nepp	11:c6f95a42d4d7	1508	min_SIG_needed_p3 = 0xfff00000;
nikapov	0:a1a69d32f310	1509	} else {
sepp_nepp	11:c6f95a42d4d7	1510	/* DMAX uses a different ambient width from sigma,so apply correction.
sepp_nepp	11:c6f95a42d4d7	1511	* Perform division before multiplication to prevent overflow. */
nikapov	0:a1a69d32f310	1512	sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
nikapov	0:a1a69d32f310	1513	c_amb_eff_width_sigma_est_ns;
nikapov	0:a1a69d32f310	1514	sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
nikapov	0:a1a69d32f310	1515
nikapov	0:a1a69d32f310	1516	/* FixPoint1616 >> 16 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1517	min_SIG_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
sepp_nepp	11:c6f95a42d4d7	1518	min_SIG_needed_p3 *= min_SIG_needed_p3;
nikapov	0:a1a69d32f310	1519	}
nikapov	0:a1a69d32f310	1520
nikapov	0:a1a69d32f310	1521	/* FixPoint1814 / uint32 = FixPoint1814 */
nikapov	0:a1a69d32f310	1522	sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
nikapov	0:a1a69d32f310	1523
nikapov	0:a1a69d32f310	1524	/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
nikapov	0:a1a69d32f310	1525	sigma_limit_tmp *= sigma_limit_tmp;
nikapov	0:a1a69d32f310	1526
nikapov	0:a1a69d32f310	1527	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp	11:c6f95a42d4d7	1528	sigma_est_sq_tmp = c_sigma_est_Ref * c_sigma_est_Ref;
nikapov	0:a1a69d32f310	1529
nikapov	0:a1a69d32f310	1530	/* FixPoint3232 >> 4 = FixPoint0428 */
nikapov	0:a1a69d32f310	1531	sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
nikapov	0:a1a69d32f310	1532
nikapov	0:a1a69d32f310	1533	/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
nikapov	0:a1a69d32f310	1534	sigma_limit_tmp -= sigma_est_sq_tmp;
nikapov	0:a1a69d32f310	1535
nikapov	0:a1a69d32f310	1536	/* uint32_t * FixPoint0428 = FixPoint0428 */
sepp_nepp	11:c6f95a42d4d7	1537	min_SIG_needed_p4 = 4 * 12 * sigma_limit_tmp;
nikapov	0:a1a69d32f310	1538
nikapov	0:a1a69d32f310	1539	/* FixPoint0428 >> 14 = FixPoint1814 */
sepp_nepp	11:c6f95a42d4d7	1540	min_SIG_needed_p4 = (min_SIG_needed_p4 + 0x2000) >> 14;
nikapov	0:a1a69d32f310	1541
nikapov	0:a1a69d32f310	1542	/* uint32 + uint32 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1543	min_SIG_needed = (min_SIG_needed_p2 + min_SIG_needed_p3);
nikapov	0:a1a69d32f310	1544
nikapov	0:a1a69d32f310	1545	/* uint32 / uint32 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1546	min_SIG_needed += (peak_vcsel_duration_us / 2);
sepp_nepp	11:c6f95a42d4d7	1547	min_SIG_needed /= peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	1548
nikapov	0:a1a69d32f310	1549	/* uint32 << 14 = FixPoint1814 */
sepp_nepp	11:c6f95a42d4d7	1550	min_SIG_needed <<= 14;
nikapov	0:a1a69d32f310	1551
nikapov	0:a1a69d32f310	1552	/* FixPoint1814 / FixPoint1814 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1553	min_SIG_needed += (min_SIG_needed_p4 / 2);
sepp_nepp	11:c6f95a42d4d7	1554	min_SIG_needed /= min_SIG_needed_p4;
nikapov	0:a1a69d32f310	1555
nikapov	0:a1a69d32f310	1556	/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
sepp_nepp	11:c6f95a42d4d7	1557	min_SIG_needed *= min_SIG_needed_p1;
nikapov	0:a1a69d32f310	1558
nikapov	0:a1a69d32f310	1559	/* Apply correction by dividing by 1000000.
sepp_nepp	11:c6f95a42d4d7	1560	* This assumes 10E16 on the numerator of the equation and 10E-22 on the denominator.
nikapov	0:a1a69d32f310	1561	* We do this because 32bit fix point calculation can't
nikapov	0:a1a69d32f310	1562	* handle the larger and smaller elements of this equation,
nikapov	0:a1a69d32f310	1563	* i.e. speed of light and pulse widths.
nikapov	0:a1a69d32f310	1564	*/
sepp_nepp	11:c6f95a42d4d7	1565	min_SIG_needed = (min_SIG_needed + 500) / 1000;
sepp_nepp	11:c6f95a42d4d7	1566	min_SIG_needed <<= 4;
sepp_nepp	11:c6f95a42d4d7	1567
sepp_nepp	11:c6f95a42d4d7	1568	min_SIG_needed = (min_SIG_needed + 500) / 1000;
nikapov	0:a1a69d32f310	1569
nikapov	0:a1a69d32f310	1570	/* FixPoint1616 >> 8 = FixPoint2408 */
sepp_nepp	11:c6f95a42d4d7	1571	signal_limit_tmp = (c_SIG_limit + 0x80) >> 8;
nikapov	0:a1a69d32f310	1572
nikapov	0:a1a69d32f310	1573	/* FixPoint2408/FixPoint2408 = uint32 */
nikapov	0:a1a69d32f310	1574	if (signal_limit_tmp != 0) {
nikapov	0:a1a69d32f310	1575	dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
nikapov	0:a1a69d32f310	1576	/ signal_limit_tmp;
sepp_nepp	11:c6f95a42d4d7	1577	} else { dmax_dark_tmp = 0; }
sepp_nepp	11:c6f95a42d4d7	1578
sepp_nepp	11:c6f95a42d4d7	1579	dmax_dark = ISQRT(dmax_dark_tmp);
nikapov	0:a1a69d32f310	1580
nikapov	0:a1a69d32f310	1581	/* FixPoint2408/FixPoint2408 = uint32 */
sepp_nepp	11:c6f95a42d4d7	1582	if (min_SIG_needed != 0)
sepp_nepp	11:c6f95a42d4d7	1583	{ dmax_ambient = (signal_at0_mm + min_SIG_needed / 2) / min_SIG_needed; }
sepp_nepp	11:c6f95a42d4d7	1584	else { dmax_ambient = 0; }
sepp_nepp	11:c6f95a42d4d7	1585
sepp_nepp	11:c6f95a42d4d7	1586	dmax_ambient = ISQRT(dmax_ambient);
nikapov	0:a1a69d32f310	1587
nikapov	0:a1a69d32f310	1588	*pd_max_mm = dmax_dark;
sepp_nepp	11:c6f95a42d4d7	1589	if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; }
nikapov	0:a1a69d32f310	1590	}
nikapov	0:a1a69d32f310	1591
sepp_nepp	11:c6f95a42d4d7	1592	void VL53L0X::Calc_sigma_estimate(TRangeResults *p_ranging_results,
sepp_nepp	11:c6f95a42d4d7	1593	TFP1616 p_sigma_estimate, uint32_t p_dmax_mm)
sepp_nepp	11:c6f95a42d4d7	1594	{ /* Expressed in 100ths of a ns,i.e. centi-ns */
nikapov	0:a1a69d32f310	1595	const uint32_t c_pulse_effective_width_centi_ns = 800;
sepp_nepp	7:41cbc431e1f4	1596	/* Expressed in 100ths of a ns,i.e. centi-ns */
nikapov	0:a1a69d32f310	1597	const uint32_t c_ambient_effective_width_centi_ns = 600;
sepp_nepp	11:c6f95a42d4d7	1598	const TFP1616 c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
nikapov	0:a1a69d32f310	1599	const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
sepp_nepp	11:c6f95a42d4d7	1600	const TFP1616 c_sigma_est_max = 0x028F87AE;
sepp_nepp	11:c6f95a42d4d7	1601	const TFP1616 c_sigma_est_rtn_max = 0xF000;
sepp_nepp	11:c6f95a42d4d7	1602	const TFP1616 c_amb_to_SIG_ratio_max = 0xF0000000 /
nikapov	0:a1a69d32f310	1603	c_ambient_effective_width_centi_ns;
nikapov	0:a1a69d32f310	1604	/* Time Of Flight per mm (6.6 pico secs) */
sepp_nepp	11:c6f95a42d4d7	1605	const TFP1616 c_tof_per_mm_ps = 0x0006999A;
nikapov	0:a1a69d32f310	1606	const uint32_t c_16bit_rounding_param = 0x00008000;
sepp_nepp	11:c6f95a42d4d7	1607	const TFP1616 c_max_xtalk_kcps = 0x00320000;
nikapov	0:a1a69d32f310	1608	const uint32_t c_pll_period_ps = 1655;
nikapov	0:a1a69d32f310	1609
nikapov	0:a1a69d32f310	1610	uint32_t vcsel_total_events_rtn;
nikapov	0:a1a69d32f310	1611	uint32_t final_range_timeout_micro_secs;
nikapov	0:a1a69d32f310	1612	uint32_t pre_range_timeout_micro_secs;
nikapov	0:a1a69d32f310	1613	uint32_t final_range_integration_time_milli_secs;
sepp_nepp	11:c6f95a42d4d7	1614	TFP1616 sigma_estimate_p1;
sepp_nepp	11:c6f95a42d4d7	1615	TFP1616 sigma_estimate_p2;
sepp_nepp	11:c6f95a42d4d7	1616	TFP1616 sigma_estimate_p3;
sepp_nepp	11:c6f95a42d4d7	1617	TFP1616 delta_t_ps;
sepp_nepp	11:c6f95a42d4d7	1618	TFP1616 pw_mult;
sepp_nepp	11:c6f95a42d4d7	1619	TFP1616 sigma_est_rtn;
sepp_nepp	11:c6f95a42d4d7	1620	TFP1616 sigma_estimate;
sepp_nepp	11:c6f95a42d4d7	1621	TFP1616 xtalk_correction;
sepp_nepp	11:c6f95a42d4d7	1622	TFP1616 ambient_rate_kcps;
sepp_nepp	11:c6f95a42d4d7	1623	TFP1616 peak_SIG_rate_kcps;
sepp_nepp	11:c6f95a42d4d7	1624	TFP1616 xtalk_comp_rate_mcps;
sepp_nepp	11:c6f95a42d4d7	1625	uint32_t xtalk_comp_rate_kcps;
sepp_nepp	11:c6f95a42d4d7	1626
sepp_nepp	11:c6f95a42d4d7	1627	TFP1616 diff1_mcps;
sepp_nepp	11:c6f95a42d4d7	1628	TFP1616 diff2_mcps;
sepp_nepp	11:c6f95a42d4d7	1629	TFP1616 sqr1;
sepp_nepp	11:c6f95a42d4d7	1630	TFP1616 sqr2;
sepp_nepp	11:c6f95a42d4d7	1631	TFP1616 sqr_sum;
sepp_nepp	11:c6f95a42d4d7	1632	TFP1616 sqrt_result_centi_ns;
sepp_nepp	11:c6f95a42d4d7	1633	TFP1616 sqrt_result;
sepp_nepp	11:c6f95a42d4d7	1634	TFP1616 total_SIG_rate_mcps;
sepp_nepp	11:c6f95a42d4d7	1635	TFP1616 corrected_SIG_rate_mcps;
sepp_nepp	11:c6f95a42d4d7	1636	TFP1616 sigma_est_Ref;
nikapov	0:a1a69d32f310	1637	uint32_t vcsel_width;
nikapov	0:a1a69d32f310	1638	uint32_t final_range_macro_pclks;
nikapov	0:a1a69d32f310	1639	uint32_t pre_range_macro_pclks;
nikapov	0:a1a69d32f310	1640	uint32_t peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	1641	uint8_t final_range_vcsel_pclks;
nikapov	0:a1a69d32f310	1642	uint8_t pre_range_vcsel_pclks;
nikapov	0:a1a69d32f310	1643	/*! \addtogroup calc_sigma_estimate
nikapov	0:a1a69d32f310	1644	* @{
sepp_nepp	11:c6f95a42d4d7	1645	* Estimates the range sigma */
sepp_nepp	11:c6f95a42d4d7	1646
sepp_nepp	11:c6f95a42d4d7	1647	xtalk_comp_rate_mcps = CurrParams.Xtalk_CompRate_MHz;
sepp_nepp	10:cd251e0fc2fd	1648
sepp_nepp	10:cd251e0fc2fd	1649	/* We work in kcps rather than mcps as this helps keep within the
sepp_nepp	11:c6f95a42d4d7	1650	* confines of the 32 Fix1616 type. */
sepp_nepp	11:c6f95a42d4d7	1651	ambient_rate_kcps = (p_ranging_results->AmbientRateRtnMHz * 1000) >> 16;
sepp_nepp	11:c6f95a42d4d7	1652
sepp_nepp	11:c6f95a42d4d7	1653	corrected_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
sepp_nepp	11:c6f95a42d4d7	1654
sepp_nepp	11:c6f95a42d4d7	1655	Get_total_SIG_rate(p_ranging_results,&total_SIG_rate_mcps);
sepp_nepp	11:c6f95a42d4d7	1656	xtalk_comp_rate_mcps = Get_total_xtalk_rate(p_ranging_results);
nikapov	0:a1a69d32f310	1657
nikapov	0:a1a69d32f310	1658	/* Signal rate measurement provided by device is the
sepp_nepp	11:c6f95a42d4d7	1659	* peak signal rate,not average. */
sepp_nepp	11:c6f95a42d4d7	1660	peak_SIG_rate_kcps = (total_SIG_rate_mcps * 1000);
sepp_nepp	11:c6f95a42d4d7	1661	peak_SIG_rate_kcps = (peak_SIG_rate_kcps + 0x8000) >> 16;
sepp_nepp	11:c6f95a42d4d7	1662
sepp_nepp	11:c6f95a42d4d7	1663	xtalk_comp_rate_kcps = xtalk_comp_rate_mcps * 1000;
sepp_nepp	11:c6f95a42d4d7	1664
sepp_nepp	11:c6f95a42d4d7	1665	if (xtalk_comp_rate_kcps > c_max_xtalk_kcps)
sepp_nepp	11:c6f95a42d4d7	1666	{ xtalk_comp_rate_kcps = c_max_xtalk_kcps; }
sepp_nepp	11:c6f95a42d4d7	1667
sepp_nepp	11:c6f95a42d4d7	1668	if (ErrState == VL53L0X_OK) {
nikapov	0:a1a69d32f310	1669	/* Calculate final range macro periods */
sepp_nepp	12:aa177f0e4c10	1670	final_range_timeout_micro_secs = FinalRangeTimeoutMicroSecs;
sepp_nepp	12:aa177f0e4c10	1671	final_range_vcsel_pclks = FinalRangeVcselPPeriod;
sepp_nepp	11:c6f95a42d4d7	1672	final_range_macro_pclks = Calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks);
nikapov	0:a1a69d32f310	1673
nikapov	0:a1a69d32f310	1674	/* Calculate pre-range macro periods */
sepp_nepp	12:aa177f0e4c10	1675	pre_range_timeout_micro_secs = PreRangeTimeoutMicroSecs;
sepp_nepp	12:aa177f0e4c10	1676	pre_range_vcsel_pclks = PreRangeVcselPPeriod;
sepp_nepp	11:c6f95a42d4d7	1677	pre_range_macro_pclks = Calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks);
nikapov	0:a1a69d32f310	1678	vcsel_width = 3;
sepp_nepp	11:c6f95a42d4d7	1679	if (final_range_vcsel_pclks == 8) { vcsel_width = 2; }
nikapov	0:a1a69d32f310	1680
nikapov	0:a1a69d32f310	1681	peak_vcsel_duration_us = vcsel_width * 2048 *
nikapov	0:a1a69d32f310	1682	(pre_range_macro_pclks + final_range_macro_pclks);
nikapov	0:a1a69d32f310	1683	peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov	0:a1a69d32f310	1684	peak_vcsel_duration_us *= c_pll_period_ps;
nikapov	0:a1a69d32f310	1685	peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov	0:a1a69d32f310	1686
nikapov	0:a1a69d32f310	1687	/* Fix1616 >> 8 = Fix2408 */
sepp_nepp	11:c6f95a42d4d7	1688	total_SIG_rate_mcps = (total_SIG_rate_mcps + 0x80) >> 8;
nikapov	0:a1a69d32f310	1689
nikapov	0:a1a69d32f310	1690	/* Fix2408 * uint32 = Fix2408 */
sepp_nepp	11:c6f95a42d4d7	1691	vcsel_total_events_rtn = total_SIG_rate_mcps * peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	1692
nikapov	0:a1a69d32f310	1693	/* Fix2408 >> 8 = uint32 */
nikapov	0:a1a69d32f310	1694	vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
nikapov	0:a1a69d32f310	1695
nikapov	0:a1a69d32f310	1696	/* Fix2408 << 8 = Fix1616 = */
sepp_nepp	11:c6f95a42d4d7	1697	total_SIG_rate_mcps <<= 8;
nikapov	0:a1a69d32f310	1698	}
nikapov	0:a1a69d32f310	1699
sepp_nepp	11:c6f95a42d4d7	1700	if (ErrState != VL53L0X_OK) { return ; }
sepp_nepp	11:c6f95a42d4d7	1701
sepp_nepp	11:c6f95a42d4d7	1702	if (peak_SIG_rate_kcps == 0) {
nikapov	0:a1a69d32f310	1703	*p_sigma_estimate = c_sigma_est_max;
sepp_nepp	10:cd251e0fc2fd	1704	SigmaEstimate = c_sigma_est_max;
nikapov	0:a1a69d32f310	1705	*p_dmax_mm = 0;
nikapov	0:a1a69d32f310	1706	} else {
sepp_nepp	11:c6f95a42d4d7	1707	if (vcsel_total_events_rtn < 1) {vcsel_total_events_rtn = 1; }
nikapov	0:a1a69d32f310	1708
nikapov	0:a1a69d32f310	1709	sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
nikapov	0:a1a69d32f310	1710
nikapov	0:a1a69d32f310	1711	/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1712	sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_SIG_rate_kcps;
sepp_nepp	11:c6f95a42d4d7	1713	if (sigma_estimate_p2 > c_amb_to_SIG_ratio_max)
sepp_nepp	11:c6f95a42d4d7	1714	/* Clip to prevent overflow. Will ensure safe max result. */
sepp_nepp	11:c6f95a42d4d7	1715	{ sigma_estimate_p2 = c_amb_to_SIG_ratio_max; }
nikapov	0:a1a69d32f310	1716	sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
sepp_nepp	11:c6f95a42d4d7	1717	sigma_estimate_p3 = 2 * ISQRT(vcsel_total_events_rtn * 12);
nikapov	0:a1a69d32f310	1718
nikapov	0:a1a69d32f310	1719	/* uint32 * FixPoint1616 = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1720	delta_t_ps = p_ranging_results->RangeMilliMeter * c_tof_per_mm_ps;
sepp_nepp	11:c6f95a42d4d7	1721
sepp_nepp	11:c6f95a42d4d7	1722	/* vcselRate - xtalkCompRate
nikapov	0:a1a69d32f310	1723	* (uint32 << 16) - FixPoint1616 = FixPoint1616.
nikapov	0:a1a69d32f310	1724	* Divide result by 1000 to convert to mcps.
sepp_nepp	11:c6f95a42d4d7	1725	* 500 is added to ensure rounding when integer division truncates. */
sepp_nepp	11:c6f95a42d4d7	1726	diff1_mcps = (((peak_SIG_rate_kcps << 16) - 2 * xtalk_comp_rate_kcps) + 500) / 1000;
nikapov	0:a1a69d32f310	1727
nikapov	0:a1a69d32f310	1728	/* vcselRate + xtalkCompRate */
sepp_nepp	11:c6f95a42d4d7	1729	diff2_mcps = ((peak_SIG_rate_kcps << 16) + 500) / 1000;
sepp_nepp	11:c6f95a42d4d7	1730
sepp_nepp	11:c6f95a42d4d7	1731	/* Shift by 8 bits to increase resolution prior to the division */
nikapov	0:a1a69d32f310	1732	diff1_mcps <<= 8;
nikapov	0:a1a69d32f310	1733
nikapov	0:a1a69d32f310	1734	/* FixPoint0824/FixPoint1616 = FixPoint2408 */
nikapov	0:a1a69d32f310	1735	// xTalkCorrection = abs(diff1_mcps/diff2_mcps);
sepp_nepp	11:c6f95a42d4d7	1736	// abs is causing compiler overloading isue in C++, but unsigned types. So,redundant call anyway!
sepp_nepp	11:c6f95a42d4d7	1737	xtalk_correction = diff1_mcps / diff2_mcps;
nikapov	0:a1a69d32f310	1738
nikapov	0:a1a69d32f310	1739	/* FixPoint2408 << 8 = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1740	xtalk_correction <<= 8;
sepp_nepp	11:c6f95a42d4d7	1741
sepp_nepp	11:c6f95a42d4d7	1742	if (p_ranging_results->RangeStatus != 0)
sepp_nepp	11:c6f95a42d4d7	1743	{ pw_mult = 1 << 16; }
sepp_nepp	11:c6f95a42d4d7	1744	else {
nikapov	0:a1a69d32f310	1745	/* FixPoint1616/uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	1746	pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
nikapov	0:a1a69d32f310	1747
sepp_nepp	10:cd251e0fc2fd	1748	/* FixPoint1616 * FixPoint1616 = FixPoint3232,however both
nikapov	0:a1a69d32f310	1749	* values are small enough such that32 bits will not be
sepp_nepp	11:c6f95a42d4d7	1750	* exceeded. */
sepp_nepp	11:c6f95a42d4d7	1751	pw_mult *= ((1 << 16) - xtalk_correction);
nikapov	0:a1a69d32f310	1752
nikapov	0:a1a69d32f310	1753	/* (FixPoint3232 >> 16) = FixPoint1616 */
nikapov	0:a1a69d32f310	1754	pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
nikapov	0:a1a69d32f310	1755
nikapov	0:a1a69d32f310	1756	/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	1757	pw_mult += (1 << 16);
nikapov	0:a1a69d32f310	1758
sepp_nepp	10:cd251e0fc2fd	1759	/* At this point the value will be 1.xx,therefore if we square
nikapov	0:a1a69d32f310	1760	* the value this will exceed 32 bits. To address this perform
sepp_nepp	11:c6f95a42d4d7	1761	* a single shift to the right before the multiplication. */
nikapov	0:a1a69d32f310	1762	pw_mult >>= 1;
nikapov	0:a1a69d32f310	1763	/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
nikapov	0:a1a69d32f310	1764	pw_mult = pw_mult * pw_mult;
nikapov	0:a1a69d32f310	1765
nikapov	0:a1a69d32f310	1766	/* (FixPoint3430 >> 14) = Fix1616 */
nikapov	0:a1a69d32f310	1767	pw_mult >>= 14;
nikapov	0:a1a69d32f310	1768	}
nikapov	0:a1a69d32f310	1769
nikapov	0:a1a69d32f310	1770	/* FixPoint1616 * uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	1771	sqr1 = pw_mult * sigma_estimate_p1;
nikapov	0:a1a69d32f310	1772
nikapov	0:a1a69d32f310	1773	/* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov	0:a1a69d32f310	1774	sqr1 = (sqr1 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	1775
nikapov	0:a1a69d32f310	1776	/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov	0:a1a69d32f310	1777	sqr1 *= sqr1;
nikapov	0:a1a69d32f310	1778	sqr2 = sigma_estimate_p2;
nikapov	0:a1a69d32f310	1779
nikapov	0:a1a69d32f310	1780	/* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov	0:a1a69d32f310	1781	sqr2 = (sqr2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	1782
nikapov	0:a1a69d32f310	1783	/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov	0:a1a69d32f310	1784	sqr2 *= sqr2;
nikapov	0:a1a69d32f310	1785
nikapov	0:a1a69d32f310	1786	/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
nikapov	0:a1a69d32f310	1787	sqr_sum = sqr1 + sqr2;
nikapov	0:a1a69d32f310	1788
nikapov	0:a1a69d32f310	1789	/* SQRT(FixPoin6400) = FixPoint3200 */
sepp_nepp	11:c6f95a42d4d7	1790	sqrt_result_centi_ns = ISQRT(sqr_sum);
nikapov	0:a1a69d32f310	1791
nikapov	0:a1a69d32f310	1792	/* (FixPoint3200 << 16) = FixPoint1616 */
nikapov	0:a1a69d32f310	1793	sqrt_result_centi_ns <<= 16;
nikapov	0:a1a69d32f310	1794
sepp_nepp	11:c6f95a42d4d7	1795	/* Note that the Speed Of Light is expressed in um per 1E-10
sepp_nepp	11:c6f95a42d4d7	1796	* seconds (2997) Therefore to get mm/ns we have to divide by 10000 */
nikapov	0:a1a69d32f310	1797	sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
nikapov	0:a1a69d32f310	1798	sigma_estimate_p3);
nikapov	0:a1a69d32f310	1799	sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
nikapov	0:a1a69d32f310	1800
nikapov	0:a1a69d32f310	1801	/* Add 5000 before dividing by 10000 to ensure rounding. */
sepp_nepp	11:c6f95a42d4d7	1802	sigma_est_rtn = (sigma_est_rtn + 5000) / 10000;
sepp_nepp	11:c6f95a42d4d7	1803
sepp_nepp	11:c6f95a42d4d7	1804	if (sigma_est_rtn > c_sigma_est_rtn_max)
sepp_nepp	11:c6f95a42d4d7	1805	/* Clip to prevent overflow. Will ensure safe max result. */
sepp_nepp	11:c6f95a42d4d7	1806	{ sigma_est_rtn = c_sigma_est_rtn_max; }
sepp_nepp	11:c6f95a42d4d7	1807
nikapov	0:a1a69d32f310	1808	final_range_integration_time_milli_secs =
nikapov	0:a1a69d32f310	1809	(final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
nikapov	0:a1a69d32f310	1810
nikapov	0:a1a69d32f310	1811	/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
sepp_nepp	11:c6f95a42d4d7	1812	* sqrt(FixPoint1616/int) = FixPoint2408) */
sepp_nepp	11:c6f95a42d4d7	1813	sigma_est_Ref =
sepp_nepp	11:c6f95a42d4d7	1814	ISQRT((c_dflt_final_range_integration_time_milli_secs +
nikapov	0:a1a69d32f310	1815	final_range_integration_time_milli_secs / 2) /
nikapov	0:a1a69d32f310	1816	final_range_integration_time_milli_secs);
nikapov	0:a1a69d32f310	1817
nikapov	0:a1a69d32f310	1818	/* FixPoint2408 << 8 = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1819	sigma_est_Ref <<= 8;
sepp_nepp	11:c6f95a42d4d7	1820	sigma_est_Ref = (sigma_est_Ref + 500) / 1000;
nikapov	0:a1a69d32f310	1821
nikapov	0:a1a69d32f310	1822	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	1823	sqr1 = sigma_est_rtn * sigma_est_rtn;
nikapov	0:a1a69d32f310	1824	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp	11:c6f95a42d4d7	1825	sqr2 = sigma_est_Ref * sigma_est_Ref;
nikapov	0:a1a69d32f310	1826
nikapov	0:a1a69d32f310	1827	/* sqrt(FixPoint3232) = FixPoint1616 */
sepp_nepp	11:c6f95a42d4d7	1828	sqrt_result = ISQRT((sqr1 + sqr2));
sepp_nepp	11:c6f95a42d4d7	1829	/* Note that the Shift by 4 bits increases resolution prior to
sepp_nepp	7:41cbc431e1f4	1830	* the sqrt,therefore the result must be shifted by 2 bits to
sepp_nepp	11:c6f95a42d4d7	1831	* the right to revert back to the FixPoint1616 format. */
nikapov	0:a1a69d32f310	1832	sigma_estimate = 1000 * sqrt_result;
nikapov	0:a1a69d32f310	1833
sepp_nepp	11:c6f95a42d4d7	1834	if ((peak_SIG_rate_kcps < 1) \|\| (vcsel_total_events_rtn < 1) \|\|
nikapov	0:a1a69d32f310	1835	(sigma_estimate > c_sigma_est_max)) {
sepp_nepp	11:c6f95a42d4d7	1836	sigma_estimate = c_sigma_est_max; }
nikapov	0:a1a69d32f310	1837
nikapov	0:a1a69d32f310	1838	*p_sigma_estimate = (uint32_t)(sigma_estimate);
sepp_nepp	10:cd251e0fc2fd	1839	SigmaEstimate = *p_sigma_estimate;
sepp_nepp	11:c6f95a42d4d7	1840	Calc_dmax(total_SIG_rate_mcps,
sepp_nepp	11:c6f95a42d4d7	1841	corrected_SIG_rate_mcps,
nikapov	0:a1a69d32f310	1842	pw_mult,
nikapov	0:a1a69d32f310	1843	sigma_estimate_p1,
nikapov	0:a1a69d32f310	1844	sigma_estimate_p2,
nikapov	0:a1a69d32f310	1845	peak_vcsel_duration_us,
nikapov	0:a1a69d32f310	1846	p_dmax_mm);
nikapov	0:a1a69d32f310	1847	}
nikapov	0:a1a69d32f310	1848	}
nikapov	0:a1a69d32f310	1849
sepp_nepp	11:c6f95a42d4d7	1850	void VL53L0X::Get_Device_range_status(uint8_t device_range_status,
sepp_nepp	11:c6f95a42d4d7	1851	TFP1616 signal_rate,
sepp_nepp	11:c6f95a42d4d7	1852	uint16_t effective_SPAD_rtn_count,
sepp_nepp	11:c6f95a42d4d7	1853	TRangeResults *p_ranging_results,
sepp_nepp	11:c6f95a42d4d7	1854	uint8_t *p_Device_range_status)
sepp_nepp	11:c6f95a42d4d7	1855	{ uint8_t none_flag;
nikapov	0:a1a69d32f310	1856	uint8_t sigma_limitflag = 0;
sepp_nepp	11:c6f95a42d4d7	1857	uint8_t signal_Ref_clipflag = 0;
nikapov	0:a1a69d32f310	1858	uint8_t range_ignore_thresholdflag = 0;
sepp_nepp	11:c6f95a42d4d7	1859	uint8_t sigma_limit_chk_en = 0;
sepp_nepp	11:c6f95a42d4d7	1860	uint8_t signal_rate_final_range_limit_chk_en = 0;
sepp_nepp	11:c6f95a42d4d7	1861	uint8_t signal_Ref_clip_limit_chk_en = 0;
sepp_nepp	11:c6f95a42d4d7	1862	uint8_t range_ignore_threshold_chk_en = 0;
sepp_nepp	11:c6f95a42d4d7	1863	TFP1616 sigma_estimate;
sepp_nepp	11:c6f95a42d4d7	1864	TFP1616 sigma_limit_value;
sepp_nepp	11:c6f95a42d4d7	1865	TFP1616 signal_Ref_clip_value;
sepp_nepp	11:c6f95a42d4d7	1866	TFP1616 range_ignore_threshold;
sepp_nepp	11:c6f95a42d4d7	1867	TFP1616 signal_rate_per_SPAD;
nikapov	0:a1a69d32f310	1868	uint8_t device_range_status_internal = 0;
nikapov	0:a1a69d32f310	1869	uint8_t temp8;
nikapov	0:a1a69d32f310	1870	uint32_t dmax_mm = 0;
sepp_nepp	11:c6f95a42d4d7	1871
sepp_nepp	11:c6f95a42d4d7	1872	/* VL53L0X has a good ranging when the value of the
nikapov	0:a1a69d32f310	1873	* DeviceRangeStatus = 11. This function will replace the value 0 with
nikapov	0:a1a69d32f310	1874	* the value 11 in the DeviceRangeStatus.
sepp_nepp	7:41cbc431e1f4	1875	* In addition,the SigmaEstimator is not included in the VL53L0X
sepp_nepp	11:c6f95a42d4d7	1876	* DeviceRangeStatus,this will be added in the DeviceRangeStatus. */
nikapov	0:a1a69d32f310	1877
nikapov	0:a1a69d32f310	1878	device_range_status_internal = ((device_range_status & 0x78) >> 3);
nikapov	0:a1a69d32f310	1879
sepp_nepp	11:c6f95a42d4d7	1880	if ( device_range_status_internal == 0 \|\|
sepp_nepp	11:c6f95a42d4d7	1881	device_range_status_internal == 5 \|\|
sepp_nepp	11:c6f95a42d4d7	1882	device_range_status_internal == 7 \|\|
sepp_nepp	11:c6f95a42d4d7	1883	device_range_status_internal == 12 \|\|
sepp_nepp	11:c6f95a42d4d7	1884	device_range_status_internal == 13 \|\|
sepp_nepp	11:c6f95a42d4d7	1885	device_range_status_internal == 14 \|\|
sepp_nepp	11:c6f95a42d4d7	1886	device_range_status_internal == 15 )
sepp_nepp	11:c6f95a42d4d7	1887	{ none_flag = 1; }
sepp_nepp	11:c6f95a42d4d7	1888	else { none_flag = 0; }
sepp_nepp	11:c6f95a42d4d7	1889
sepp_nepp	11:c6f95a42d4d7	1890	/* Check if Sigma limit is enabled,if yes then do comparison with limit
sepp_nepp	11:c6f95a42d4d7	1891	* value and put the result back into pDeviceRangeStatus. */
sepp_nepp	11:c6f95a42d4d7	1892	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	1893	{ sigma_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); }
sepp_nepp	11:c6f95a42d4d7	1894
sepp_nepp	11:c6f95a42d4d7	1895	if ((sigma_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) {
sepp_nepp	11:c6f95a42d4d7	1896	/* compute the Sigma and check with limit */
sepp_nepp	11:c6f95a42d4d7	1897	Calc_sigma_estimate(p_ranging_results, &sigma_estimate, &dmax_mm);
sepp_nepp	11:c6f95a42d4d7	1898	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	1899	{ p_ranging_results->RangeDMaxMilliMeter = dmax_mm; }
sepp_nepp	11:c6f95a42d4d7	1900
sepp_nepp	11:c6f95a42d4d7	1901	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	1902	{ sigma_limit_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE);
sepp_nepp	11:c6f95a42d4d7	1903
sepp_nepp	11:c6f95a42d4d7	1904	if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value))
sepp_nepp	11:c6f95a42d4d7	1905	{ sigma_limitflag = 1; }/* Limit Fail */
sepp_nepp	11:c6f95a42d4d7	1906	}
sepp_nepp	11:c6f95a42d4d7	1907	}
sepp_nepp	11:c6f95a42d4d7	1908
sepp_nepp	11:c6f95a42d4d7	1909	/* Check if Signal ref clip limit is enabled,if yes then do comparison
sepp_nepp	11:c6f95a42d4d7	1910	* with limit value and put the result back into pDeviceRangeStatus. */
sepp_nepp	11:c6f95a42d4d7	1911	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	1912	{signal_Ref_clip_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_REF_CLIP);}
sepp_nepp	11:c6f95a42d4d7	1913
sepp_nepp	11:c6f95a42d4d7	1914	if ((signal_Ref_clip_limit_chk_en != 0) && (ErrState == VL53L0X_OK))
sepp_nepp	11:c6f95a42d4d7	1915	{ signal_Ref_clip_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIG_REF_CLIP);
nikapov	0:a1a69d32f310	1916
nikapov	0:a1a69d32f310	1917	/* Read LastSignalRefMcps from device */
sepp_nepp	11:c6f95a42d4d7	1918	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	1919	LastSignalRefMcps = FP97_TO_FP1616( Read_Word(REG_RESULT_PEAK_SIG_RATE_REF));
sepp_nepp	11:c6f95a42d4d7	1920	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	1921
sepp_nepp	11:c6f95a42d4d7	1922	if ((signal_Ref_clip_value > 0) && (LastSignalRefMcps > signal_Ref_clip_value)) \
sepp_nepp	11:c6f95a42d4d7	1923	{ signal_Ref_clipflag = 1; /* Limit Fail */ }
sepp_nepp	11:c6f95a42d4d7	1924	}
sepp_nepp	11:c6f95a42d4d7	1925
sepp_nepp	11:c6f95a42d4d7	1926	/* Check if Signal ref clip limit is enabled,if yes then do comparison
sepp_nepp	11:c6f95a42d4d7	1927	* with limit value and put the result back into pDeviceRangeStatus.
sepp_nepp	11:c6f95a42d4d7	1928	* EffectiveSPADRtnCount has a format 8.8
sepp_nepp	11:c6f95a42d4d7	1929	* If (Return signal rate < (1.5 x Xtalk x number of SPADS)) : FAIL */
sepp_nepp	11:c6f95a42d4d7	1930	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	1931	{ range_ignore_threshold_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); }
sepp_nepp	11:c6f95a42d4d7	1932
sepp_nepp	11:c6f95a42d4d7	1933	if ((range_ignore_threshold_chk_en != 0) && (ErrState == VL53L0X_OK))
sepp_nepp	11:c6f95a42d4d7	1934	{/* Compute the signal rate per SPAD */
sepp_nepp	11:c6f95a42d4d7	1935	if (effective_SPAD_rtn_count == 0) { signal_rate_per_SPAD = 0; }
sepp_nepp	11:c6f95a42d4d7	1936	else { signal_rate_per_SPAD =
sepp_nepp	11:c6f95a42d4d7	1937	(TFP1616)((256 * signal_rate) / effective_SPAD_rtn_count); }
sepp_nepp	11:c6f95a42d4d7	1938
sepp_nepp	11:c6f95a42d4d7	1939	range_ignore_threshold=Get_limit_chk_val(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD);
sepp_nepp	11:c6f95a42d4d7	1940
sepp_nepp	11:c6f95a42d4d7	1941	if ((range_ignore_threshold > 0) && (signal_rate_per_SPAD < range_ignore_threshold)) {
sepp_nepp	11:c6f95a42d4d7	1942	/* Limit Fail add 2^6 to range ErrState */
nikapov	0:a1a69d32f310	1943	range_ignore_thresholdflag = 1;
nikapov	0:a1a69d32f310	1944	}
nikapov	0:a1a69d32f310	1945	}
nikapov	0:a1a69d32f310	1946
sepp_nepp	11:c6f95a42d4d7	1947	if (ErrState == VL53L0X_OK) {
nikapov	0:a1a69d32f310	1948	if (none_flag == 1) {
sepp_nepp	11:c6f95a42d4d7	1949	p_Device_range_status = 255; / NONE */
nikapov	0:a1a69d32f310	1950	} else if (device_range_status_internal == 1 \|\|
nikapov	0:a1a69d32f310	1951	device_range_status_internal == 2 \|\|
nikapov	0:a1a69d32f310	1952	device_range_status_internal == 3) {
sepp_nepp	11:c6f95a42d4d7	1953	p_Device_range_status = 5; / HW fail */
nikapov	0:a1a69d32f310	1954	} else if (device_range_status_internal == 6 \|\|
nikapov	0:a1a69d32f310	1955	device_range_status_internal == 9) {
sepp_nepp	11:c6f95a42d4d7	1956	p_Device_range_status = 4; / Phase fail */
nikapov	0:a1a69d32f310	1957	} else if (device_range_status_internal == 8 \|\|
nikapov	0:a1a69d32f310	1958	device_range_status_internal == 10 \|\|
sepp_nepp	11:c6f95a42d4d7	1959	signal_Ref_clipflag == 1) {
sepp_nepp	11:c6f95a42d4d7	1960	p_Device_range_status = 3; / Min range */
nikapov	0:a1a69d32f310	1961	} else if (device_range_status_internal == 4 \|\|
nikapov	0:a1a69d32f310	1962	range_ignore_thresholdflag == 1) {
sepp_nepp	11:c6f95a42d4d7	1963	p_Device_range_status = 2; / Signal Fail */
nikapov	0:a1a69d32f310	1964	} else if (sigma_limitflag == 1) {
sepp_nepp	11:c6f95a42d4d7	1965	p_Device_range_status = 1; / Sigma Fail */
nikapov	0:a1a69d32f310	1966	} else {
sepp_nepp	11:c6f95a42d4d7	1967	p_Device_range_status = 0; / Range Valid */
nikapov	0:a1a69d32f310	1968	}
nikapov	0:a1a69d32f310	1969	}
nikapov	0:a1a69d32f310	1970
nikapov	0:a1a69d32f310	1971	/* DMAX only relevant during range error */
sepp_nepp	11:c6f95a42d4d7	1972	if (*p_Device_range_status == 0) { p_ranging_results->RangeDMaxMilliMeter = 0; }
sepp_nepp	11:c6f95a42d4d7	1973
sepp_nepp	11:c6f95a42d4d7	1974	/* fill the Limit Check ErrState */
sepp_nepp	11:c6f95a42d4d7	1975	signal_rate_final_range_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE);
sepp_nepp	11:c6f95a42d4d7	1976
sepp_nepp	11:c6f95a42d4d7	1977	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	1978	if ((sigma_limit_chk_en == 0) \|\| (sigma_limitflag == 1))
sepp_nepp	11:c6f95a42d4d7	1979	{ temp8 = 1; } else { temp8 = 0; }
sepp_nepp	11:c6f95a42d4d7	1980	CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = temp8;
sepp_nepp	11:c6f95a42d4d7	1981
sepp_nepp	11:c6f95a42d4d7	1982	if ((device_range_status_internal == 4) \|\| (signal_rate_final_range_limit_chk_en == 0))
sepp_nepp	11:c6f95a42d4d7	1983	{ temp8 = 1; } else { temp8 = 0; }
sepp_nepp	11:c6f95a42d4d7	1984	CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE] = temp8;
sepp_nepp	11:c6f95a42d4d7	1985
sepp_nepp	11:c6f95a42d4d7	1986	if ((signal_Ref_clip_limit_chk_en == 0) \|\| (signal_Ref_clipflag == 1))
sepp_nepp	11:c6f95a42d4d7	1987	{ temp8 = 1; } else { temp8 = 0; }
sepp_nepp	11:c6f95a42d4d7	1988	CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_REF_CLIP] = temp8;
sepp_nepp	11:c6f95a42d4d7	1989
sepp_nepp	11:c6f95a42d4d7	1990	if ((range_ignore_threshold_chk_en == 0) \|\| (range_ignore_thresholdflag == 1))
sepp_nepp	11:c6f95a42d4d7	1991	{ temp8 = 1; } else { temp8 = 0;}
sepp_nepp	11:c6f95a42d4d7	1992	CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = temp8;
Davidroid	3:e9269ff624ed	1993	}
nikapov	0:a1a69d32f310	1994	}
nikapov	0:a1a69d32f310	1995
sepp_nepp	11:c6f95a42d4d7	1996	void VL53L0X::Get_ranging_results(TRangeResults *p_ranging_results)
sepp_nepp	11:c6f95a42d4d7	1997	{ uint8_t device_range_status;
nikapov	0:a1a69d32f310	1998	uint8_t range_fractional_enable;
sepp_nepp	11:c6f95a42d4d7	1999	uint8_t Device_range_status;
sepp_nepp	11:c6f95a42d4d7	2000	uint8_t xtalk_compensation_enable;
nikapov	0:a1a69d32f310	2001	uint16_t ambient_rate;
sepp_nepp	11:c6f95a42d4d7	2002	TFP1616 signal_rate;
sepp_nepp	11:c6f95a42d4d7	2003	uint16_t Xtalk_CompRate_MHz;
sepp_nepp	11:c6f95a42d4d7	2004	uint16_t effective_SPAD_rtn_count;
nikapov	0:a1a69d32f310	2005	uint16_t tmpuint16;
nikapov	0:a1a69d32f310	2006	uint16_t xtalk_range_milli_meter;
nikapov	0:a1a69d32f310	2007	uint16_t linearity_corrective_gain;
nikapov	0:a1a69d32f310	2008	uint8_t localBuffer[12];
sepp_nepp	11:c6f95a42d4d7	2009	TRangeResults last_range_data_buffer;
sepp_nepp	11:c6f95a42d4d7	2010
sepp_nepp	11:c6f95a42d4d7	2011	if (ErrState != VL53L0X_OK) { return; } // Do nothing while in error state
sepp_nepp	11:c6f95a42d4d7	2012
sepp_nepp	11:c6f95a42d4d7	2013	/* use multi read even if some registers are not useful,result will
sepp_nepp	11:c6f95a42d4d7	2014	* be more efficient start reading at REG_RESULT_RANGE_STATUS = 0x14
sepp_nepp	11:c6f95a42d4d7	2015	* end reading at 0x21 dec33 total 14 bytes to read */
sepp_nepp	11:c6f95a42d4d7	2016	I2c_Read(REG_RESULT_RANGE_STATUS, localBuffer,12);
sepp_nepp	11:c6f95a42d4d7	2017
sepp_nepp	11:c6f95a42d4d7	2018	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2019	p_ranging_results->ZoneId = 0; /* Only one zone */
sepp_nepp	11:c6f95a42d4d7	2020	p_ranging_results->TimeStamp = 0; /* Not Implemented */
nikapov	0:a1a69d32f310	2021
sepp_nepp	7:41cbc431e1f4	2022	tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]);
nikapov	0:a1a69d32f310	2023	/* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
sepp_nepp	11:c6f95a42d4d7	2024	(format 11.2) else no fractional /
sepp_nepp	11:c6f95a42d4d7	2025
sepp_nepp	11:c6f95a42d4d7	2026	p_ranging_results->MeasurementTimeUsec = 0;
sepp_nepp	11:c6f95a42d4d7	2027
sepp_nepp	11:c6f95a42d4d7	2028	signal_rate = FP97_TO_FP1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6]));
sepp_nepp	11:c6f95a42d4d7	2029	/* peak_SIG_count_rate_rtn_mcps */
sepp_nepp	11:c6f95a42d4d7	2030	p_ranging_results->SignalRateRtnMHz = signal_rate;
nikapov	0:a1a69d32f310	2031
sepp_nepp	7:41cbc431e1f4	2032	ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]);
sepp_nepp	11:c6f95a42d4d7	2033	p_ranging_results->AmbientRateRtnMHz = FP97_TO_FP1616(ambient_rate);
sepp_nepp	11:c6f95a42d4d7	2034
sepp_nepp	11:c6f95a42d4d7	2035	effective_SPAD_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]);
sepp_nepp	11:c6f95a42d4d7	2036	/* EffectiveSPADRtnCount is 8.8 format */
sepp_nepp	11:c6f95a42d4d7	2037	p_ranging_results->EffectiveSPADRtnCount = effective_SPAD_rtn_count;
nikapov	0:a1a69d32f310	2038
nikapov	0:a1a69d32f310	2039	device_range_status = localBuffer[0];
nikapov	0:a1a69d32f310	2040
nikapov	0:a1a69d32f310	2041	/* Get Linearity Corrective Gain */
sepp_nepp	10:cd251e0fc2fd	2042	linearity_corrective_gain = LinearityCorrectiveGain;
nikapov	0:a1a69d32f310	2043
nikapov	0:a1a69d32f310	2044	/* Get ranging configuration */
sepp_nepp	10:cd251e0fc2fd	2045	range_fractional_enable = RangeFractionalEnable;
nikapov	0:a1a69d32f310	2046
nikapov	0:a1a69d32f310	2047	if (linearity_corrective_gain != 1000) {
nikapov	0:a1a69d32f310	2048	tmpuint16 = (uint16_t)((linearity_corrective_gain
nikapov	0:a1a69d32f310	2049	* tmpuint16 + 500) / 1000);
nikapov	0:a1a69d32f310	2050
nikapov	0:a1a69d32f310	2051	/* Implement Xtalk */
sepp_nepp	11:c6f95a42d4d7	2052	Xtalk_CompRate_MHz = CurrParams.Xtalk_CompRate_MHz;
sepp_nepp	11:c6f95a42d4d7	2053	xtalk_compensation_enable = CurrParams.XTalk_Compens_En;
sepp_nepp	11:c6f95a42d4d7	2054
sepp_nepp	11:c6f95a42d4d7	2055	if (xtalk_compensation_enable) {
sepp_nepp	11:c6f95a42d4d7	2056	if ((signal_rate - ((Xtalk_CompRate_MHz
sepp_nepp	11:c6f95a42d4d7	2057	* effective_SPAD_rtn_count) >> 8)) <= 0) {
sepp_nepp	11:c6f95a42d4d7	2058	if (range_fractional_enable) { xtalk_range_milli_meter = 8888;
sepp_nepp	11:c6f95a42d4d7	2059	} else { xtalk_range_milli_meter = 8888 << 2; }
nikapov	0:a1a69d32f310	2060	} else {
sepp_nepp	11:c6f95a42d4d7	2061	xtalk_range_milli_meter = (tmpuint16 * signal_rate)
sepp_nepp	11:c6f95a42d4d7	2062	/ (signal_rate - ((Xtalk_CompRate_MHz * effective_SPAD_rtn_count) >> 8));
nikapov	0:a1a69d32f310	2063	}
nikapov	0:a1a69d32f310	2064	tmpuint16 = xtalk_range_milli_meter;
nikapov	0:a1a69d32f310	2065	}
nikapov	0:a1a69d32f310	2066	}
nikapov	0:a1a69d32f310	2067
nikapov	0:a1a69d32f310	2068	if (range_fractional_enable) {
sepp_nepp	11:c6f95a42d4d7	2069	p_ranging_results->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2);
sepp_nepp	11:c6f95a42d4d7	2070	p_ranging_results->RangeFractionalPart =
nikapov	0:a1a69d32f310	2071	(uint8_t)((tmpuint16 & 0x03) << 6);
nikapov	0:a1a69d32f310	2072	} else {
sepp_nepp	11:c6f95a42d4d7	2073	p_ranging_results->RangeMilliMeter = tmpuint16;
sepp_nepp	11:c6f95a42d4d7	2074	p_ranging_results->RangeFractionalPart = 0;
nikapov	0:a1a69d32f310	2075	}
nikapov	0:a1a69d32f310	2076
sepp_nepp	11:c6f95a42d4d7	2077	/* For a standard definition of RangeStatus,this should
nikapov	0:a1a69d32f310	2078	* return 0 in case of good result after a ranging
sepp_nepp	11:c6f95a42d4d7	2079	* The range ErrState depends on the device so call a device
sepp_nepp	11:c6f95a42d4d7	2080	* specific function to obtain the right ErrState. */
sepp_nepp	11:c6f95a42d4d7	2081	Get_Device_range_status(device_range_status,signal_rate,effective_SPAD_rtn_count,
sepp_nepp	11:c6f95a42d4d7	2082	p_ranging_results,&Device_range_status);
sepp_nepp	11:c6f95a42d4d7	2083	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2084	p_ranging_results->RangeStatus = Device_range_status;
nikapov	0:a1a69d32f310	2085	}
nikapov	0:a1a69d32f310	2086	}
nikapov	0:a1a69d32f310	2087
sepp_nepp	11:c6f95a42d4d7	2088	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2089	/* Copy last read data into device+ buffer */
sepp_nepp	10:cd251e0fc2fd	2090	last_range_data_buffer = LastRangeMeasure;
nikapov	0:a1a69d32f310	2091	last_range_data_buffer.RangeMilliMeter =
sepp_nepp	11:c6f95a42d4d7	2092	p_ranging_results->RangeMilliMeter;
nikapov	0:a1a69d32f310	2093	last_range_data_buffer.RangeFractionalPart =
sepp_nepp	11:c6f95a42d4d7	2094	p_ranging_results->RangeFractionalPart;
nikapov	0:a1a69d32f310	2095	last_range_data_buffer.RangeDMaxMilliMeter =
sepp_nepp	11:c6f95a42d4d7	2096	p_ranging_results->RangeDMaxMilliMeter;
nikapov	0:a1a69d32f310	2097	last_range_data_buffer.MeasurementTimeUsec =
sepp_nepp	11:c6f95a42d4d7	2098	p_ranging_results->MeasurementTimeUsec;
sepp_nepp	11:c6f95a42d4d7	2099	last_range_data_buffer.SignalRateRtnMHz =
sepp_nepp	11:c6f95a42d4d7	2100	p_ranging_results->SignalRateRtnMHz;
sepp_nepp	11:c6f95a42d4d7	2101	last_range_data_buffer.AmbientRateRtnMHz =
sepp_nepp	11:c6f95a42d4d7	2102	p_ranging_results->AmbientRateRtnMHz;
sepp_nepp	11:c6f95a42d4d7	2103	last_range_data_buffer.EffectiveSPADRtnCount =
sepp_nepp	11:c6f95a42d4d7	2104	p_ranging_results->EffectiveSPADRtnCount;
nikapov	0:a1a69d32f310	2105	last_range_data_buffer.RangeStatus =
sepp_nepp	11:c6f95a42d4d7	2106	p_ranging_results->RangeStatus;
sepp_nepp	10:cd251e0fc2fd	2107	LastRangeMeasure = last_range_data_buffer;
nikapov	0:a1a69d32f310	2108	}
nikapov	0:a1a69d32f310	2109	}
nikapov	0:a1a69d32f310	2110
sepp_nepp	11:c6f95a42d4d7	2111	void VL53L0X::Perf_single_ranging_measurement(
sepp_nepp	11:c6f95a42d4d7	2112	TRangeResults *p_ranging_results)
sepp_nepp	11:c6f95a42d4d7	2113	{ if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
sepp_nepp	11:c6f95a42d4d7	2114
sepp_nepp	11:c6f95a42d4d7	2115	/* This function will do a complete single ranging Here we fix the mode! */
sepp_nepp	11:c6f95a42d4d7	2116	Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING);
sepp_nepp	11:c6f95a42d4d7	2117
sepp_nepp	11:c6f95a42d4d7	2118	Perf_single_measurement();
sepp_nepp	11:c6f95a42d4d7	2119
sepp_nepp	11:c6f95a42d4d7	2120	Get_ranging_results(p_ranging_results);
sepp_nepp	11:c6f95a42d4d7	2121
sepp_nepp	11:c6f95a42d4d7	2122	Clear_interrupt_mask(0);
nikapov	0:a1a69d32f310	2123	}
nikapov	0:a1a69d32f310	2124
sepp_nepp	11:c6f95a42d4d7	2125	uint16_t VL53L0X::Get_Perf_Ref_SIG_measurement()
sepp_nepp	11:c6f95a42d4d7	2126	{ TRangeResults ranging_results;
sepp_nepp	11:c6f95a42d4d7	2127	uint8_t orig_sequence_config;
sepp_nepp	11:c6f95a42d4d7	2128	uint16_t Ref_SIG_rate ;
nikapov	0:a1a69d32f310	2129
nikapov	0:a1a69d32f310	2130	/* store the value of the sequence config,
sepp_nepp	10:cd251e0fc2fd	2131	* this will be reset before the end of the function*/
sepp_nepp	11:c6f95a42d4d7	2132	orig_sequence_config = SequenceConfig;
sepp_nepp	10:cd251e0fc2fd	2133
sepp_nepp	10:cd251e0fc2fd	2134	/* This function performs a reference signal rate measurement.*/
sepp_nepp	11:c6f95a42d4d7	2135	Set_SequenceConfig( 0xC0 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
sepp_nepp	11:c6f95a42d4d7	2136
sepp_nepp	11:c6f95a42d4d7	2137	Perf_single_ranging_measurement(&ranging_results);
sepp_nepp	11:c6f95a42d4d7	2138
sepp_nepp	11:c6f95a42d4d7	2139	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2140	Ref_SIG_rate = Read_Word(REG_RESULT_PEAK_SIG_RATE_REF);
sepp_nepp	11:c6f95a42d4d7	2141	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	2142
sepp_nepp	11:c6f95a42d4d7	2143	/* restore the previous Sequence Config */
sepp_nepp	11:c6f95a42d4d7	2144	Set_SequenceConfig( orig_sequence_config ); // resets REG_SYSTEM_SEQUENCE_CONFIG
sepp_nepp	11:c6f95a42d4d7	2145
sepp_nepp	11:c6f95a42d4d7	2146	return Ref_SIG_rate;
nikapov	0:a1a69d32f310	2147	}
nikapov	0:a1a69d32f310	2148
sepp_nepp	11:c6f95a42d4d7	2149	void VL53L0X::Perf_Ref_SPAD_management(uint32_t *ref_SPAD_count,
sepp_nepp	11:c6f95a42d4d7	2150	uint8_t *is_aperture_SPADS)
sepp_nepp	11:c6f95a42d4d7	2151	{ uint8_t last_SPAD_array[6];
nikapov	0:a1a69d32f310	2152	uint8_t start_select = 0xB4;
sepp_nepp	11:c6f95a42d4d7	2153	uint32_t minimum_SPAD_count = 3;
sepp_nepp	11:c6f95a42d4d7	2154	uint32_t max_SPAD_count = 44;
sepp_nepp	11:c6f95a42d4d7	2155	uint32_t current_SPAD_index = 0;
sepp_nepp	11:c6f95a42d4d7	2156	uint32_t last_SPAD_index = 0;
sepp_nepp	11:c6f95a42d4d7	2157	int32_t next_good_SPAD = 0;
sepp_nepp	11:c6f95a42d4d7	2158	uint16_t target_Ref_rate = 0x0A00; /* 20 MHz in 9:7 format */
sepp_nepp	11:c6f95a42d4d7	2159	uint16_t peak_SIG_rate_Ref;
sepp_nepp	11:c6f95a42d4d7	2160	uint32_t need_apt_SPADS = 0;
nikapov	0:a1a69d32f310	2161	uint32_t index = 0;
sepp_nepp	12:aa177f0e4c10	2162	uint32_t REF_SPAD_ARRAY_SIZE = 6;
nikapov	0:a1a69d32f310	2163	uint32_t signal_rate_diff = 0;
sepp_nepp	11:c6f95a42d4d7	2164	uint32_t last_SIG_rate_diff = 0;
nikapov	0:a1a69d32f310	2165	uint8_t complete = 0;
nikapov	0:a1a69d32f310	2166	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	2167	uint8_t phase_cal = 0;
sepp_nepp	11:c6f95a42d4d7	2168	uint32_t ref_SPAD_count_int = 0;
sepp_nepp	11:c6f95a42d4d7	2169	uint8_t is_aperture_SPADS_int = 0;
nikapov	0:a1a69d32f310	2170
nikapov	0:a1a69d32f310	2171	/*
nikapov	0:a1a69d32f310	2172	* The reference SPAD initialization procedure determines the minimum
sepp_nepp	11:c6f95a42d4d7	2173	* amount of reference SPADS to be enables to achieve a target reference
nikapov	0:a1a69d32f310	2174	* signal rate and should be performed once during initialization.
nikapov	0:a1a69d32f310	2175	*
sepp_nepp	11:c6f95a42d4d7	2176	* Either aperture or non-aperture SPADS are applied but never both.
sepp_nepp	11:c6f95a42d4d7	2177	* Firstly non-aperture SPADS are set,begining with 5 SPADS,and
sepp_nepp	11:c6f95a42d4d7	2178	* increased one SPAD at a time until the closest measurement to the
nikapov	0:a1a69d32f310	2179	* target rate is achieved.
nikapov	0:a1a69d32f310	2180	*
sepp_nepp	11:c6f95a42d4d7	2181	* If the target rate is exceeded when 5 non-aperture SPADS are enabled,
sepp_nepp	11:c6f95a42d4d7	2182	* initialization is performed instead with aperture SPADS.
nikapov	0:a1a69d32f310	2183	*
sepp_nepp	11:c6f95a42d4d7	2184	* When setting SPADS,a 'Good SPAD Map' is applied.
nikapov	0:a1a69d32f310	2185	*
nikapov	0:a1a69d32f310	2186	* This procedure operates within a SPAD window of interest of a maximum
sepp_nepp	11:c6f95a42d4d7	2187	* 44 SPADS.
sepp_nepp	7:41cbc431e1f4	2188	* The start point is currently fixed to 180,which lies towards the end
nikapov	0:a1a69d32f310	2189	* of the non-aperture quadrant and runs in to the adjacent aperture
sepp_nepp	11:c6f95a42d4d7	2190	* quadrant. */
sepp_nepp	11:c6f95a42d4d7	2191	target_Ref_rate = targetRefRate;
sepp_nepp	11:c6f95a42d4d7	2192
sepp_nepp	11:c6f95a42d4d7	2193	/* Initialize SPAD arrays.
sepp_nepp	11:c6f95a42d4d7	2194	* Currently the good SPAD map is initialised to 'All good'.
sepp_nepp	11:c6f95a42d4d7	2195	* This is a short term implementation. The good SPAD map will be
nikapov	0:a1a69d32f310	2196	* provided as an input.
nikapov	0:a1a69d32f310	2197	* Note that there are 6 bytes. Only the first 44 bits will be used to
sepp_nepp	11:c6f95a42d4d7	2198	* represent SPADS. */
sepp_nepp	12:aa177f0e4c10	2199	for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
sepp_nepp	11:c6f95a42d4d7	2200	SPADData.RefSPADEnables[index] = 0; }
sepp_nepp	11:c6f95a42d4d7	2201
sepp_nepp	11:c6f95a42d4d7	2202	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2203	Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
sepp_nepp	11:c6f95a42d4d7	2204	Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
sepp_nepp	11:c6f95a42d4d7	2205	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	2206	Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT,start_select);
sepp_nepp	11:c6f95a42d4d7	2207	Write_Byte(REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0);
nikapov	0:a1a69d32f310	2208
nikapov	0:a1a69d32f310	2209	/* Perform ref calibration */
sepp_nepp	11:c6f95a42d4d7	2210	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2211	{Perf_Ref_calibration(&vhv_settings, &phase_cal, 0);}
sepp_nepp	11:c6f95a42d4d7	2212
sepp_nepp	11:c6f95a42d4d7	2213	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2214	/* Enable Minimum NON-APERTURE SPADS */
sepp_nepp	11:c6f95a42d4d7	2215	current_SPAD_index = 0;
sepp_nepp	11:c6f95a42d4d7	2216	last_SPAD_index = current_SPAD_index;
sepp_nepp	11:c6f95a42d4d7	2217	need_apt_SPADS = 0;
sepp_nepp	11:c6f95a42d4d7	2218	Enable_Ref_SPADS(need_apt_SPADS,
sepp_nepp	11:c6f95a42d4d7	2219	SPADData.RefGoodSPADMap,
sepp_nepp	11:c6f95a42d4d7	2220	SPADData.RefSPADEnables,
sepp_nepp	12:aa177f0e4c10	2221	REF_SPAD_ARRAY_SIZE,
nikapov	0:a1a69d32f310	2222	start_select,
sepp_nepp	11:c6f95a42d4d7	2223	current_SPAD_index,
sepp_nepp	11:c6f95a42d4d7	2224	minimum_SPAD_count,
sepp_nepp	11:c6f95a42d4d7	2225	&last_SPAD_index);
nikapov	0:a1a69d32f310	2226	}
nikapov	0:a1a69d32f310	2227
sepp_nepp	11:c6f95a42d4d7	2228	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2229	current_SPAD_index = last_SPAD_index;
sepp_nepp	11:c6f95a42d4d7	2230
sepp_nepp	11:c6f95a42d4d7	2231	peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
sepp_nepp	11:c6f95a42d4d7	2232	if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate))
sepp_nepp	11:c6f95a42d4d7	2233	{ /* Signal rate measurement too high, switch to APERTURE SPADS */
sepp_nepp	12:aa177f0e4c10	2234	for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++)
sepp_nepp	11:c6f95a42d4d7	2235	{ SPADData.RefSPADEnables[index] = 0; }
sepp_nepp	11:c6f95a42d4d7	2236
sepp_nepp	11:c6f95a42d4d7	2237	/* Increment to the first APERTURE SPAD */
sepp_nepp	11:c6f95a42d4d7	2238	while ((Is_ApertureSPAD(start_select + current_SPAD_index)
sepp_nepp	11:c6f95a42d4d7	2239	== 0) && (current_SPAD_index < max_SPAD_count))
sepp_nepp	11:c6f95a42d4d7	2240	{ current_SPAD_index++; }
sepp_nepp	11:c6f95a42d4d7	2241
sepp_nepp	11:c6f95a42d4d7	2242	need_apt_SPADS = 1;
sepp_nepp	11:c6f95a42d4d7	2243
sepp_nepp	11:c6f95a42d4d7	2244	Enable_Ref_SPADS(need_apt_SPADS,
sepp_nepp	11:c6f95a42d4d7	2245	SPADData.RefGoodSPADMap,
sepp_nepp	11:c6f95a42d4d7	2246	SPADData.RefSPADEnables,
sepp_nepp	12:aa177f0e4c10	2247	REF_SPAD_ARRAY_SIZE,
nikapov	0:a1a69d32f310	2248	start_select,
sepp_nepp	11:c6f95a42d4d7	2249	current_SPAD_index,
sepp_nepp	11:c6f95a42d4d7	2250	minimum_SPAD_count,
sepp_nepp	11:c6f95a42d4d7	2251	&last_SPAD_index);
sepp_nepp	11:c6f95a42d4d7	2252
sepp_nepp	11:c6f95a42d4d7	2253	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2254	current_SPAD_index = last_SPAD_index;
sepp_nepp	11:c6f95a42d4d7	2255	peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
sepp_nepp	11:c6f95a42d4d7	2256
sepp_nepp	11:c6f95a42d4d7	2257	if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate))
sepp_nepp	11:c6f95a42d4d7	2258	{ /* Signal rate still too high after setting the minimum number of
sepp_nepp	11:c6f95a42d4d7	2259	* APERTURE SPADS. Can do no more therefore set the min number of
sepp_nepp	11:c6f95a42d4d7	2260	* aperture SPADS as the result. */
sepp_nepp	11:c6f95a42d4d7	2261	is_aperture_SPADS_int = 1;
sepp_nepp	11:c6f95a42d4d7	2262	ref_SPAD_count_int = minimum_SPAD_count;
nikapov	0:a1a69d32f310	2263	}
nikapov	0:a1a69d32f310	2264	}
sepp_nepp	11:c6f95a42d4d7	2265	} else { need_apt_SPADS = 0;}
nikapov	0:a1a69d32f310	2266	}
nikapov	0:a1a69d32f310	2267
sepp_nepp	11:c6f95a42d4d7	2268	if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref < target_Ref_rate))
sepp_nepp	11:c6f95a42d4d7	2269	{ /* At this point,the minimum number of either aperture
sepp_nepp	11:c6f95a42d4d7	2270	* or non-aperture SPADS have been set. Proceed to add
sepp_nepp	11:c6f95a42d4d7	2271	* SPADS and perform measurements until the target reference is reached.*/
sepp_nepp	11:c6f95a42d4d7	2272	is_aperture_SPADS_int = need_apt_SPADS;
sepp_nepp	11:c6f95a42d4d7	2273	ref_SPAD_count_int = minimum_SPAD_count;
sepp_nepp	11:c6f95a42d4d7	2274
sepp_nepp	12:aa177f0e4c10	2275	memcpy(last_SPAD_array,SPADData.RefSPADEnables, REF_SPAD_ARRAY_SIZE);
sepp_nepp	11:c6f95a42d4d7	2276	last_SIG_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
nikapov	0:a1a69d32f310	2277	complete = 0;
nikapov	0:a1a69d32f310	2278
nikapov	0:a1a69d32f310	2279	while (!complete) {
sepp_nepp	11:c6f95a42d4d7	2280	Get_Next_Good_SPAD(SPADData.RefGoodSPADMap,
sepp_nepp	12:aa177f0e4c10	2281	REF_SPAD_ARRAY_SIZE,current_SPAD_index, &next_good_SPAD);
sepp_nepp	11:c6f95a42d4d7	2282
sepp_nepp	11:c6f95a42d4d7	2283	if (next_good_SPAD == -1) {
sepp_nepp	11:c6f95a42d4d7	2284	ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2285	break;
nikapov	0:a1a69d32f310	2286	}
nikapov	0:a1a69d32f310	2287
sepp_nepp	11:c6f95a42d4d7	2288	/* Cannot combine Aperture and Non-Aperture SPADS,so
sepp_nepp	11:c6f95a42d4d7	2289	* ensure the current SPAD is of the correct type. */
sepp_nepp	11:c6f95a42d4d7	2290	if (Is_ApertureSPAD((uint32_t)start_select + next_good_SPAD) !=
sepp_nepp	11:c6f95a42d4d7	2291	need_apt_SPADS) {
sepp_nepp	11:c6f95a42d4d7	2292	/* At this point we have enabled the maximum number of Aperture SPADS. */
nikapov	0:a1a69d32f310	2293	complete = 1;
nikapov	0:a1a69d32f310	2294	break;
nikapov	0:a1a69d32f310	2295	}
nikapov	0:a1a69d32f310	2296
sepp_nepp	11:c6f95a42d4d7	2297	(ref_SPAD_count_int)++;
sepp_nepp	11:c6f95a42d4d7	2298
sepp_nepp	11:c6f95a42d4d7	2299	current_SPAD_index = next_good_SPAD;
sepp_nepp	11:c6f95a42d4d7	2300	Enable_SPAD_bit(SPADData.RefSPADEnables,
sepp_nepp	12:aa177f0e4c10	2301	REF_SPAD_ARRAY_SIZE,current_SPAD_index);
sepp_nepp	11:c6f95a42d4d7	2302
sepp_nepp	11:c6f95a42d4d7	2303	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2304	current_SPAD_index++;
sepp_nepp	11:c6f95a42d4d7	2305	/* Proceed to apply the additional SPAD and perform measurement. */
sepp_nepp	11:c6f95a42d4d7	2306	I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPADData.RefSPADEnables,6); //Set_Ref_SPAD_map
nikapov	0:a1a69d32f310	2307	}
nikapov	0:a1a69d32f310	2308
sepp_nepp	11:c6f95a42d4d7	2309	if (ErrState != VL53L0X_OK) { break; }
sepp_nepp	11:c6f95a42d4d7	2310
sepp_nepp	11:c6f95a42d4d7	2311	peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
sepp_nepp	11:c6f95a42d4d7	2312
sepp_nepp	11:c6f95a42d4d7	2313	if (ErrState != VL53L0X_OK) { break; }
sepp_nepp	11:c6f95a42d4d7	2314
sepp_nepp	11:c6f95a42d4d7	2315	signal_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
sepp_nepp	11:c6f95a42d4d7	2316
sepp_nepp	11:c6f95a42d4d7	2317	if (peak_SIG_rate_Ref > target_Ref_rate) {
sepp_nepp	11:c6f95a42d4d7	2318	/* Select the SPAD map that provides the
nikapov	0:a1a69d32f310	2319	* measurement closest to the target rate,
sepp_nepp	11:c6f95a42d4d7	2320	* either above or below it. */
sepp_nepp	11:c6f95a42d4d7	2321	if (signal_rate_diff > last_SIG_rate_diff) {
sepp_nepp	11:c6f95a42d4d7	2322	/* Previous SPAD map produced a closer measurement,so choose this. */
sepp_nepp	11:c6f95a42d4d7	2323	I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, last_SPAD_array,6); // Set_Ref_SPAD_map();
sepp_nepp	12:aa177f0e4c10	2324	memcpy(SPADData.RefSPADEnables,last_SPAD_array,REF_SPAD_ARRAY_SIZE);
sepp_nepp	11:c6f95a42d4d7	2325	(ref_SPAD_count_int)--;
nikapov	0:a1a69d32f310	2326	}
nikapov	0:a1a69d32f310	2327	complete = 1;
nikapov	0:a1a69d32f310	2328	} else {
sepp_nepp	11:c6f95a42d4d7	2329	/* Continue to add SPADS */
sepp_nepp	11:c6f95a42d4d7	2330	last_SIG_rate_diff = signal_rate_diff;
sepp_nepp	12:aa177f0e4c10	2331	memcpy(last_SPAD_array, SPADData.RefSPADEnables,REF_SPAD_ARRAY_SIZE);
nikapov	0:a1a69d32f310	2332	}
nikapov	0:a1a69d32f310	2333	} /* while */
nikapov	0:a1a69d32f310	2334	}
nikapov	0:a1a69d32f310	2335
sepp_nepp	11:c6f95a42d4d7	2336	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2337	*ref_SPAD_count = ref_SPAD_count_int;
sepp_nepp	11:c6f95a42d4d7	2338	*is_aperture_SPADS = is_aperture_SPADS_int;
sepp_nepp	12:aa177f0e4c10	2339	RefSPADSInitialised = 1;
sepp_nepp	12:aa177f0e4c10	2340	ReferenceSPADCount = (uint8_t)(*ref_SPAD_count);
sepp_nepp	12:aa177f0e4c10	2341	ReferenceSPADType = *is_aperture_SPADS;
nikapov	0:a1a69d32f310	2342	}
nikapov	0:a1a69d32f310	2343	}
nikapov	0:a1a69d32f310	2344
sepp_nepp	11:c6f95a42d4d7	2345	void VL53L0X::Set_Reference_SPADS(uint32_t count,uint8_t is_aperture_SPADS)
sepp_nepp	11:c6f95a42d4d7	2346	{ uint32_t current_SPAD_index = 0;
sepp_nepp	11:c6f95a42d4d7	2347	uint8_t start_select = 0xB4;
sepp_nepp	11:c6f95a42d4d7	2348	uint32_t max_SPAD_count = 44;
sepp_nepp	11:c6f95a42d4d7	2349	uint32_t last_SPAD_index;
nikapov	0:a1a69d32f310	2350	uint32_t index;
nikapov	0:a1a69d32f310	2351
sepp_nepp	11:c6f95a42d4d7	2352	/* This function applies a requested number of reference SPADS,either
sepp_nepp	11:c6f95a42d4d7	2353	* aperture or non-aperture,as requested. The good SPAD map will be applied.*/
sepp_nepp	11:c6f95a42d4d7	2354	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2355	Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
sepp_nepp	11:c6f95a42d4d7	2356	Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
sepp_nepp	11:c6f95a42d4d7	2357	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	2358	Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT, start_select);
sepp_nepp	11:c6f95a42d4d7	2359
sepp_nepp	12:aa177f0e4c10	2360	for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
sepp_nepp	11:c6f95a42d4d7	2361	SPADData.RefSPADEnables[index] = 0; }
sepp_nepp	11:c6f95a42d4d7	2362
sepp_nepp	11:c6f95a42d4d7	2363	if (is_aperture_SPADS) {
sepp_nepp	11:c6f95a42d4d7	2364	/* Increment to the first APERTURE SPAD */
sepp_nepp	11:c6f95a42d4d7	2365	while ((Is_ApertureSPAD(start_select + current_SPAD_index) == 0) &&
sepp_nepp	11:c6f95a42d4d7	2366	(current_SPAD_index < max_SPAD_count)) {
sepp_nepp	11:c6f95a42d4d7	2367	current_SPAD_index++;
nikapov	0:a1a69d32f310	2368	}
nikapov	0:a1a69d32f310	2369	}
sepp_nepp	12:aa177f0e4c10	2370	Enable_Ref_SPADS(is_aperture_SPADS, SPADData.RefGoodSPADMap,
sepp_nepp	11:c6f95a42d4d7	2371	SPADData.RefSPADEnables,
sepp_nepp	12:aa177f0e4c10	2372	REF_SPAD_ARRAY_SIZE,
nikapov	0:a1a69d32f310	2373	start_select,
sepp_nepp	11:c6f95a42d4d7	2374	current_SPAD_index,
nikapov	0:a1a69d32f310	2375	count,
sepp_nepp	11:c6f95a42d4d7	2376	&last_SPAD_index);
sepp_nepp	11:c6f95a42d4d7	2377
sepp_nepp	11:c6f95a42d4d7	2378	if (ErrState == VL53L0X_OK) {
sepp_nepp	12:aa177f0e4c10	2379	RefSPADSInitialised = 1;
sepp_nepp	12:aa177f0e4c10	2380	ReferenceSPADCount = (uint8_t)(count);
sepp_nepp	12:aa177f0e4c10	2381	ReferenceSPADType = is_aperture_SPADS;
nikapov	0:a1a69d32f310	2382	}
nikapov	0:a1a69d32f310	2383	}
nikapov	0:a1a69d32f310	2384
sepp_nepp	11:c6f95a42d4d7	2385	void VL53L0X::Set_GPIO_config(VL53L0X_DeviceModes device_mode,
sepp_nepp	11:c6f95a42d4d7	2386	TGPIO_Func functionality, VL53L0X_InterruptPolarity polarity)
sepp_nepp	11:c6f95a42d4d7	2387	{ uint8_t pol_data;
sepp_nepp	11:c6f95a42d4d7	2388
sepp_nepp	11:c6f95a42d4d7	2389	if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
sepp_nepp	11:c6f95a42d4d7	2390	{ pol_data = 0x00;} else { pol_data = 0x10;}
sepp_nepp	11:c6f95a42d4d7	2391
sepp_nepp	11:c6f95a42d4d7	2392	switch ( device_mode ) {
sepp_nepp	11:c6f95a42d4d7	2393	case VL53L0X_DEVICEMODE_GPIO_DRIVE:
sepp_nepp	11:c6f95a42d4d7	2394	Write_Byte(REG_GPIO_HV_MUX_ACTIVE_HIGH,pol_data);
sepp_nepp	11:c6f95a42d4d7	2395	break;
sepp_nepp	11:c6f95a42d4d7	2396	case VL53L0X_DEVICEMODE_GPIO_OSC:
sepp_nepp	11:c6f95a42d4d7	2397	Write_Byte(0xff,0x01);
sepp_nepp	11:c6f95a42d4d7	2398	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	2399	Write_Byte(0xff,0x00);
sepp_nepp	11:c6f95a42d4d7	2400	Write_Byte(0x80,0x01);
sepp_nepp	11:c6f95a42d4d7	2401	Write_Byte(0x85,0x02);
sepp_nepp	11:c6f95a42d4d7	2402	Write_Byte(0xff,0x04);
sepp_nepp	11:c6f95a42d4d7	2403	Write_Byte(0xcd,0x00);
sepp_nepp	11:c6f95a42d4d7	2404	Write_Byte(0xcc,0x11);
sepp_nepp	11:c6f95a42d4d7	2405	Write_Byte(0xff,0x07);
sepp_nepp	11:c6f95a42d4d7	2406	Write_Byte(0xbe,0x00);
sepp_nepp	11:c6f95a42d4d7	2407	Write_Byte(0xff,0x06);
sepp_nepp	11:c6f95a42d4d7	2408	Write_Byte(0xcc,0x09);
sepp_nepp	11:c6f95a42d4d7	2409	Write_Byte(0xff,0x00);
sepp_nepp	11:c6f95a42d4d7	2410	Write_Byte(0xff,0x01);
sepp_nepp	11:c6f95a42d4d7	2411	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	2412	break;
sepp_nepp	11:c6f95a42d4d7	2413	default:
sepp_nepp	11:c6f95a42d4d7	2414	if (functionality>GPIO_FUNC_NEW_MEASURE_READY)
sepp_nepp	11:c6f95a42d4d7	2415	{ ErrState = VL53L0X_ERROR_GPIO_FUNC_NOT_SUPPORTED; }
sepp_nepp	11:c6f95a42d4d7	2416	else { Write_Byte(REG_SYSINT_CONFIG_GPIO,functionality); }
sepp_nepp	11:c6f95a42d4d7	2417
sepp_nepp	11:c6f95a42d4d7	2418	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2419	{ Register_BitMask(REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,pol_data); }
sepp_nepp	11:c6f95a42d4d7	2420
sepp_nepp	11:c6f95a42d4d7	2421	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2422	{GpioFunctionality = functionality; }
sepp_nepp	11:c6f95a42d4d7	2423
sepp_nepp	11:c6f95a42d4d7	2424	Clear_interrupt_mask(0);
sepp_nepp	11:c6f95a42d4d7	2425	} // switch
sepp_nepp	11:c6f95a42d4d7	2426	} // Set_GPIO_config
sepp_nepp	11:c6f95a42d4d7	2427
sepp_nepp	11:c6f95a42d4d7	2428	/* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */
sepp_nepp	11:c6f95a42d4d7	2429	uint16_t VL53L0X::Encode_timeout(uint32_t timeout_macro_clks)
sepp_nepp	11:c6f95a42d4d7	2430	{ uint16_t encoded_timeout = 0;
nikapov	0:a1a69d32f310	2431	uint16_t ms_byte = 0;
nikapov	0:a1a69d32f310	2432
nikapov	0:a1a69d32f310	2433	if (timeout_macro_clks > 0) {
sepp_nepp	11:c6f95a42d4d7	2434	timeout_macro_clks = timeout_macro_clks - 1;
sepp_nepp	11:c6f95a42d4d7	2435	while ((timeout_macro_clks & 0xFFFFFF00) > 0) {
sepp_nepp	11:c6f95a42d4d7	2436	timeout_macro_clks = timeout_macro_clks >> 1;
nikapov	0:a1a69d32f310	2437	ms_byte++;
sepp_nepp	11:c6f95a42d4d7	2438	} // while
sepp_nepp	11:c6f95a42d4d7	2439	encoded_timeout = (ms_byte << 8) + (uint16_t)(timeout_macro_clks & 0x000000FF);
sepp_nepp	11:c6f95a42d4d7	2440	}
nikapov	0:a1a69d32f310	2441	return encoded_timeout;
nikapov	0:a1a69d32f310	2442	}
nikapov	0:a1a69d32f310	2443
sepp_nepp	11:c6f95a42d4d7	2444	void VL53L0X::Set_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
nikapov	0:a1a69d32f310	2445	uint32_t timeout_micro_secs)
sepp_nepp	11:c6f95a42d4d7	2446	{ uint8_t current_vcsel_PPeriod_p_clk;
nikapov	0:a1a69d32f310	2447	uint8_t msrc_encoded_time_out;
nikapov	0:a1a69d32f310	2448	uint16_t pre_range_encoded_time_out;
nikapov	0:a1a69d32f310	2449	uint16_t pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	2450	uint16_t msrc_range_time_out_m_clks;
nikapov	0:a1a69d32f310	2451	uint32_t final_range_time_out_m_clks;
nikapov	0:a1a69d32f310	2452	uint16_t final_range_encoded_time_out;
sepp_nepp	11:c6f95a42d4d7	2453	VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp	11:c6f95a42d4d7	2454
sepp_nepp	11:c6f95a42d4d7	2455	switch (sequence_step_id) {
sepp_nepp	11:c6f95a42d4d7	2456	case VL53L0X_SEQUENCESTEP_TCC:
sepp_nepp	11:c6f95a42d4d7	2457	case VL53L0X_SEQUENCESTEP_DSS:
sepp_nepp	11:c6f95a42d4d7	2458	case VL53L0X_SEQUENCESTEP_MSRC:
sepp_nepp	11:c6f95a42d4d7	2459	current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	2460	( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp	11:c6f95a42d4d7	2461
sepp_nepp	11:c6f95a42d4d7	2462	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2463	msrc_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
sepp_nepp	11:c6f95a42d4d7	2464	(uint8_t)current_vcsel_PPeriod_p_clk);
sepp_nepp	11:c6f95a42d4d7	2465
sepp_nepp	11:c6f95a42d4d7	2466	if (msrc_range_time_out_m_clks > 256) { msrc_encoded_time_out = 255;}
sepp_nepp	11:c6f95a42d4d7	2467	else {msrc_encoded_time_out = (uint8_t)msrc_range_time_out_m_clks - 1; }
nikapov	0:a1a69d32f310	2468
sepp_nepp	12:aa177f0e4c10	2469	LastEncodedTimeout = msrc_encoded_time_out;
sepp_nepp	11:c6f95a42d4d7	2470	}
sepp_nepp	11:c6f95a42d4d7	2471	Write_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP,msrc_encoded_time_out);
sepp_nepp	11:c6f95a42d4d7	2472	break;
sepp_nepp	11:c6f95a42d4d7	2473
sepp_nepp	11:c6f95a42d4d7	2474	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sepp_nepp	11:c6f95a42d4d7	2475	current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	2476	( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp	11:c6f95a42d4d7	2477
sepp_nepp	11:c6f95a42d4d7	2478	pre_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
sepp_nepp	11:c6f95a42d4d7	2479	(uint8_t)current_vcsel_PPeriod_p_clk);
sepp_nepp	11:c6f95a42d4d7	2480	pre_range_encoded_time_out = Encode_timeout(pre_range_time_out_m_clks);
sepp_nepp	11:c6f95a42d4d7	2481
sepp_nepp	12:aa177f0e4c10	2482	LastEncodedTimeout = pre_range_encoded_time_out;
sepp_nepp	11:c6f95a42d4d7	2483
sepp_nepp	11:c6f95a42d4d7	2484	Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out);
sepp_nepp	11:c6f95a42d4d7	2485
sepp_nepp	11:c6f95a42d4d7	2486	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2487	{PreRangeTimeoutMicroSecs=timeout_micro_secs; }
sepp_nepp	11:c6f95a42d4d7	2488	break;
sepp_nepp	11:c6f95a42d4d7	2489
sepp_nepp	11:c6f95a42d4d7	2490	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sepp_nepp	11:c6f95a42d4d7	2491	/* For the final range timeout,the pre-range timeout must be added.
sepp_nepp	11:c6f95a42d4d7	2492	* To do this both final and pre-range timeouts must be expressed in
sepp_nepp	11:c6f95a42d4d7	2493	* macro periods MClks because they have different vcsel periods.*/
sepp_nepp	11:c6f95a42d4d7	2494	sequence_steps = Get_sequence_step_enables();
nikapov	0:a1a69d32f310	2495	pre_range_time_out_m_clks = 0;
sepp_nepp	11:c6f95a42d4d7	2496
sepp_nepp	11:c6f95a42d4d7	2497	if (sequence_steps.PreRangeOn)
sepp_nepp	11:c6f95a42d4d7	2498	{ current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	2499	( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp	11:c6f95a42d4d7	2500
sepp_nepp	11:c6f95a42d4d7	2501	/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp	11:c6f95a42d4d7	2502	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2503	{ pre_range_encoded_time_out = Read_Word(0x51);
sepp_nepp	11:c6f95a42d4d7	2504	pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
sepp_nepp	11:c6f95a42d4d7	2505	}
sepp_nepp	11:c6f95a42d4d7	2506	}
sepp_nepp	11:c6f95a42d4d7	2507
sepp_nepp	11:c6f95a42d4d7	2508	/* Calculate FINAL RANGE Timeout in Macro Periode (MCLKS) and add PRE-RANGE value */
sepp_nepp	11:c6f95a42d4d7	2509	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2510	{ current_vcsel_PPeriod_p_clk /* Get and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	2511	= ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
sepp_nepp	11:c6f95a42d4d7	2512
sepp_nepp	11:c6f95a42d4d7	2513	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2514	{ final_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
sepp_nepp	11:c6f95a42d4d7	2515	(uint8_t) current_vcsel_PPeriod_p_clk);
nikapov	0:a1a69d32f310	2516
nikapov	0:a1a69d32f310	2517	final_range_time_out_m_clks += pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	2518
sepp_nepp	11:c6f95a42d4d7	2519	final_range_encoded_time_out = Encode_timeout(final_range_time_out_m_clks);
sepp_nepp	11:c6f95a42d4d7	2520
sepp_nepp	11:c6f95a42d4d7	2521	Write_Word(0x71,final_range_encoded_time_out);
sepp_nepp	11:c6f95a42d4d7	2522	}
sepp_nepp	11:c6f95a42d4d7	2523
sepp_nepp	11:c6f95a42d4d7	2524	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2525	{ FinalRangeTimeoutMicroSecs = timeout_micro_secs; }
sepp_nepp	11:c6f95a42d4d7	2526	break;
sepp_nepp	11:c6f95a42d4d7	2527
sepp_nepp	11:c6f95a42d4d7	2528	default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp	11:c6f95a42d4d7	2529	} // switch (sequence_step_id)
sepp_nepp	11:c6f95a42d4d7	2530	}
sepp_nepp	11:c6f95a42d4d7	2531
sepp_nepp	11:c6f95a42d4d7	2532	void VL53L0X::Set_Measure_Time_Budget_us (uint32_t Measure_Time_Budget_us)
sepp_nepp	11:c6f95a42d4d7	2533	{ uint32_t final_range_timing_budget_us;
sepp_nepp	11:c6f95a42d4d7	2534	VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp	11:c6f95a42d4d7	2535	uint32_t msrc_dcc_tcc_timeout_us= 2000;
sepp_nepp	10:cd251e0fc2fd	2536	uint32_t start_overhead_us = 1910;
sepp_nepp	10:cd251e0fc2fd	2537	uint32_t end_overhead_us = 960;
sepp_nepp	10:cd251e0fc2fd	2538	uint32_t msrc_overhead_us = 660;
sepp_nepp	10:cd251e0fc2fd	2539	uint32_t tcc_overhead_us = 590;
sepp_nepp	10:cd251e0fc2fd	2540	uint32_t dss_overhead_us = 690;
sepp_nepp	10:cd251e0fc2fd	2541	uint32_t pre_range_overhead_us = 660;
sepp_nepp	11:c6f95a42d4d7	2542	uint32_t final_range_overhead_us= 550;
sepp_nepp	10:cd251e0fc2fd	2543	uint32_t pre_range_timeout_us = 0;
sepp_nepp	10:cd251e0fc2fd	2544	uint32_t c_min_timing_budget_us = 20000;
nikapov	0:a1a69d32f310	2545	uint32_t sub_timeout = 0;
nikapov	0:a1a69d32f310	2546
sepp_nepp	11:c6f95a42d4d7	2547	if (Measure_Time_Budget_us < c_min_timing_budget_us)
sepp_nepp	11:c6f95a42d4d7	2548	{ ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp	11:c6f95a42d4d7	2549	return ; }
sepp_nepp	11:c6f95a42d4d7	2550
sepp_nepp	11:c6f95a42d4d7	2551	final_range_timing_budget_us = Measure_Time_Budget_us -
sepp_nepp	11:c6f95a42d4d7	2552	(start_overhead_us + end_overhead_us);
sepp_nepp	11:c6f95a42d4d7	2553
sepp_nepp	11:c6f95a42d4d7	2554	sequence_steps = Get_sequence_step_enables();
sepp_nepp	11:c6f95a42d4d7	2555
sepp_nepp	11:c6f95a42d4d7	2556	if (ErrState == VL53L0X_OK &&
sepp_nepp	11:c6f95a42d4d7	2557	(sequence_steps.TccOn \|\|
sepp_nepp	11:c6f95a42d4d7	2558	sequence_steps.MsrcOn \|\|
sepp_nepp	11:c6f95a42d4d7	2559	sequence_steps.DssOn)) {
nikapov	0:a1a69d32f310	2560
sepp_nepp	7:41cbc431e1f4	2561	/* TCC,MSRC and DSS all share the same timeout */
sepp_nepp	11:c6f95a42d4d7	2562	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,
sepp_nepp	10:cd251e0fc2fd	2563	&msrc_dcc_tcc_timeout_us);
nikapov	0:a1a69d32f310	2564
sepp_nepp	11:c6f95a42d4d7	2565	/* Subtract the TCC,MSRC and DSS timeouts if they are enabled. */
sepp_nepp	11:c6f95a42d4d7	2566	if (ErrState != VL53L0X_OK) {return ; }
nikapov	0:a1a69d32f310	2567
nikapov	0:a1a69d32f310	2568	/* TCC */
sepp_nepp	11:c6f95a42d4d7	2569	if (sequence_steps.TccOn) {
sepp_nepp	11:c6f95a42d4d7	2570
sepp_nepp	11:c6f95a42d4d7	2571	sub_timeout = msrc_dcc_tcc_timeout_us + tcc_overhead_us;
sepp_nepp	11:c6f95a42d4d7	2572
sepp_nepp	11:c6f95a42d4d7	2573	if (sub_timeout < final_range_timing_budget_us) {
sepp_nepp	11:c6f95a42d4d7	2574	final_range_timing_budget_us -= sub_timeout;
sepp_nepp	11:c6f95a42d4d7	2575	} else { /* Requested timeout too big. */
sepp_nepp	11:c6f95a42d4d7	2576	ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	2577	}
nikapov	0:a1a69d32f310	2578	}
nikapov	0:a1a69d32f310	2579
sepp_nepp	11:c6f95a42d4d7	2580	if (ErrState != VL53L0X_OK) {return; }
nikapov	0:a1a69d32f310	2581
nikapov	0:a1a69d32f310	2582	/* DSS */
sepp_nepp	11:c6f95a42d4d7	2583	if (sequence_steps.DssOn)
sepp_nepp	11:c6f95a42d4d7	2584	{ sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
sepp_nepp	11:c6f95a42d4d7	2585
sepp_nepp	11:c6f95a42d4d7	2586	if (sub_timeout < final_range_timing_budget_us)
sepp_nepp	11:c6f95a42d4d7	2587	{ final_range_timing_budget_us -= sub_timeout; }
sepp_nepp	11:c6f95a42d4d7	2588	else { /* Requested timeout too big. */
sepp_nepp	11:c6f95a42d4d7	2589	ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
sepp_nepp	11:c6f95a42d4d7	2590	}
sepp_nepp	11:c6f95a42d4d7	2591	else if (sequence_steps.MsrcOn) /* MSRC */
sepp_nepp	11:c6f95a42d4d7	2592	{ sub_timeout = msrc_dcc_tcc_timeout_us + msrc_overhead_us;
sepp_nepp	11:c6f95a42d4d7	2593
sepp_nepp	11:c6f95a42d4d7	2594	if (sub_timeout < final_range_timing_budget_us)
sepp_nepp	11:c6f95a42d4d7	2595	{ final_range_timing_budget_us -= sub_timeout; }
sepp_nepp	11:c6f95a42d4d7	2596	else /* Requested timeout too big. */
sepp_nepp	11:c6f95a42d4d7	2597	{ ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
sepp_nepp	11:c6f95a42d4d7	2598	}
nikapov	0:a1a69d32f310	2599	}
nikapov	0:a1a69d32f310	2600
sepp_nepp	11:c6f95a42d4d7	2601	if (ErrState != VL53L0X_OK) {return; }
sepp_nepp	11:c6f95a42d4d7	2602
sepp_nepp	11:c6f95a42d4d7	2603	if (sequence_steps.PreRangeOn) {
nikapov	0:a1a69d32f310	2604	/* Subtract the Pre-range timeout if enabled. */
sepp_nepp	11:c6f95a42d4d7	2605	Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp	10:cd251e0fc2fd	2606	&pre_range_timeout_us);
sepp_nepp	11:c6f95a42d4d7	2607	sub_timeout = pre_range_timeout_us + pre_range_overhead_us;
sepp_nepp	10:cd251e0fc2fd	2608
sepp_nepp	10:cd251e0fc2fd	2609	if (sub_timeout < final_range_timing_budget_us) {
sepp_nepp	10:cd251e0fc2fd	2610	final_range_timing_budget_us -= sub_timeout;
nikapov	0:a1a69d32f310	2611	} else {
nikapov	0:a1a69d32f310	2612	/* Requested timeout too big. */
sepp_nepp	11:c6f95a42d4d7	2613	ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	2614	}
nikapov	0:a1a69d32f310	2615	}
nikapov	0:a1a69d32f310	2616
sepp_nepp	11:c6f95a42d4d7	2617	if (ErrState == VL53L0X_OK && sequence_steps.FinalRangeOn)
sepp_nepp	11:c6f95a42d4d7	2618	{ final_range_timing_budget_us -= final_range_overhead_us;
nikapov	0:a1a69d32f310	2619
nikapov	0:a1a69d32f310	2620	/* Final Range Timeout
nikapov	0:a1a69d32f310	2621	* Note that the final range timeout is determined by the timing
nikapov	0:a1a69d32f310	2622	* budget and the sum of all other timeouts within the sequence.
sepp_nepp	7:41cbc431e1f4	2623	* If there is no room for the final range timeout,then an error
nikapov	0:a1a69d32f310	2624	* will be set. Otherwise the remaining time will be applied to
nikapov	0:a1a69d32f310	2625	* the final range.
nikapov	0:a1a69d32f310	2626	*/
sepp_nepp	11:c6f95a42d4d7	2627	Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp	10:cd251e0fc2fd	2628	final_range_timing_budget_us);
sepp_nepp	10:cd251e0fc2fd	2629
sepp_nepp	11:c6f95a42d4d7	2630	CurrParams.Measure_Time_Budget_us = Measure_Time_Budget_us;
nikapov	0:a1a69d32f310	2631	}
nikapov	0:a1a69d32f310	2632	}
nikapov	0:a1a69d32f310	2633
sepp_nepp	11:c6f95a42d4d7	2634	const uint8_t SEQUENCESTEP_MASK[] =
sepp_nepp	11:c6f95a42d4d7	2635	{ 0x10, //VL53L0X_SEQUENCESTEP_TCC = 0
sepp_nepp	11:c6f95a42d4d7	2636	0x28, //VL53L0X_SEQUENCESTEP_DSS = 1
sepp_nepp	11:c6f95a42d4d7	2637	0x04, //VL53L0X_SEQUENCESTEP_MSRC= 2
sepp_nepp	11:c6f95a42d4d7	2638	0x40, //VL53L0X_SEQUENCESTEP_PRE_RANGE= 3
sepp_nepp	11:c6f95a42d4d7	2639	0x80}; //VL53L0X_SEQUENCESTEP_FINAL_RANGE = 4
sepp_nepp	11:c6f95a42d4d7	2640
sepp_nepp	11:c6f95a42d4d7	2641	void VL53L0X::Set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id,
sepp_nepp	11:c6f95a42d4d7	2642	uint8_t sequence_step_enabled)
sepp_nepp	11:c6f95a42d4d7	2643	{ uint8_t new_config = 0;
sepp_nepp	11:c6f95a42d4d7	2644
sepp_nepp	11:c6f95a42d4d7	2645	// SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
sepp_nepp	11:c6f95a42d4d7	2646	// instead of reading from the device, use the SequenceConfig local data field!!
sepp_nepp	11:c6f95a42d4d7	2647
sepp_nepp	11:c6f95a42d4d7	2648	if (sequence_step_enabled == 1) /* Enable requested sequence step */
sepp_nepp	11:c6f95a42d4d7	2649	{ new_config = SequenceConfig \| SEQUENCESTEP_MASK[sequence_step_id]; }
sepp_nepp	11:c6f95a42d4d7	2650	else /* Disable requested sequence step */
sepp_nepp	11:c6f95a42d4d7	2651	{ new_config = SequenceConfig & (0xff - SEQUENCESTEP_MASK[sequence_step_id]); }
sepp_nepp	11:c6f95a42d4d7	2652
sepp_nepp	11:c6f95a42d4d7	2653	if (new_config != SequenceConfig) { /* Apply New Setting */
sepp_nepp	11:c6f95a42d4d7	2654	Set_SequenceConfig( new_config );
sepp_nepp	11:c6f95a42d4d7	2655	if (ErrState == VL53L0X_OK) /* Recalculate timing budget */
sepp_nepp	11:c6f95a42d4d7	2656	{ Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
sepp_nepp	11:c6f95a42d4d7	2657	} // if (new_config != sequence_config)
nikapov	0:a1a69d32f310	2658	}
nikapov	0:a1a69d32f310	2659
sepp_nepp	11:c6f95a42d4d7	2660	void VL53L0X::Set_limit_chk_en(uint16_t limit_check_id, uint8_t limit_chk_en)
sepp_nepp	11:c6f95a42d4d7	2661	{ TFP1616 temp_fix1616 = 0;
sepp_nepp	11:c6f95a42d4d7	2662	if (limit_chk_en!=0) {limit_chk_en=1;} // make sure we only have 0 or 1 as values!!!
sepp_nepp	11:c6f95a42d4d7	2663
sepp_nepp	11:c6f95a42d4d7	2664	switch (limit_check_id) {
sepp_nepp	11:c6f95a42d4d7	2665	case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* internal computation: */
sepp_nepp	11:c6f95a42d4d7	2666	case VL53L0X_CHECKEN_SIG_REF_CLIP: /* internal computation: */
sepp_nepp	11:c6f95a42d4d7	2667	case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: /* internal computation: */
sepp_nepp	11:c6f95a42d4d7	2668	CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en;
sepp_nepp	11:c6f95a42d4d7	2669	break;
sepp_nepp	11:c6f95a42d4d7	2670
sepp_nepp	11:c6f95a42d4d7	2671	case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
sepp_nepp	11:c6f95a42d4d7	2672	temp_fix1616 = limit_chk_en * CurrParams.Limit_Chk_Val[limit_check_id];
sepp_nepp	11:c6f95a42d4d7	2673	Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp	11:c6f95a42d4d7	2674	FP1616_TO_FP97(temp_fix1616));
sepp_nepp	11:c6f95a42d4d7	2675	break;
sepp_nepp	11:c6f95a42d4d7	2676
sepp_nepp	11:c6f95a42d4d7	2677	case VL53L0X_CHECKEN_SIG_RATE_MSRC:
sepp_nepp	11:c6f95a42d4d7	2678	Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xFE, (1-limit_chk_en)<< 1);
sepp_nepp	11:c6f95a42d4d7	2679	break;
sepp_nepp	11:c6f95a42d4d7	2680
sepp_nepp	11:c6f95a42d4d7	2681	case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
sepp_nepp	11:c6f95a42d4d7	2682	Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEF, (1-limit_chk_en)<< 4);
sepp_nepp	11:c6f95a42d4d7	2683	break;
sepp_nepp	11:c6f95a42d4d7	2684
sepp_nepp	11:c6f95a42d4d7	2685	default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp	11:c6f95a42d4d7	2686	} // switch
sepp_nepp	11:c6f95a42d4d7	2687
sepp_nepp	11:c6f95a42d4d7	2688	if (ErrState == VL53L0X_OK) { CurrParams.Limit_Chk_En[limit_check_id] = limit_chk_en; }
nikapov	0:a1a69d32f310	2689	}
nikapov	0:a1a69d32f310	2690
sepp_nepp	11:c6f95a42d4d7	2691	void VL53L0X::Static_init()
sepp_nepp	11:c6f95a42d4d7	2692	{ VL53L0X_DeviceParams_t new_curr_parameters;
sepp_nepp	11:c6f95a42d4d7	2693	uint8_t *p_tuning_setting_buffer;
nikapov	0:a1a69d32f310	2694	uint16_t tempword = 0;
sepp_nepp	11:c6f95a42d4d7	2695	uint8_t tempbyte = 0;
nikapov	0:a1a69d32f310	2696	uint32_t count = 0;
sepp_nepp	11:c6f95a42d4d7	2697	uint8_t is_aperture_SPADS = 0;
sepp_nepp	11:c6f95a42d4d7	2698	uint32_t ref_SPAD_count = 0;
sepp_nepp	11:c6f95a42d4d7	2699	uint8_t aperture_SPADS = 0;
sepp_nepp	11:c6f95a42d4d7	2700	uint8_t vcsel_PPeriod_pclk;
nikapov	0:a1a69d32f310	2701	uint32_t seq_timeout_micro_secs;
nikapov	0:a1a69d32f310	2702
sepp_nepp	11:c6f95a42d4d7	2703	/* set the ref SPAD from NVM */
sepp_nepp	12:aa177f0e4c10	2704	count = (uint32_t)ReferenceSPADCount;
sepp_nepp	12:aa177f0e4c10	2705	aperture_SPADS = ReferenceSPADType;
nikapov	0:a1a69d32f310	2706
nikapov	0:a1a69d32f310	2707	/* NVM value invalid */
sepp_nepp	11:c6f95a42d4d7	2708	if ((aperture_SPADS > 1) \|\| ((aperture_SPADS == 1) && (count > 32)) \|\|
sepp_nepp	11:c6f95a42d4d7	2709	((aperture_SPADS == 0) && (count > 12)))
sepp_nepp	11:c6f95a42d4d7	2710	{ Perf_Ref_SPAD_management(&ref_SPAD_count, &is_aperture_SPADS); }
sepp_nepp	11:c6f95a42d4d7	2711	else
sepp_nepp	11:c6f95a42d4d7	2712	{ Set_Reference_SPADS(count,aperture_SPADS); }
nikapov	0:a1a69d32f310	2713
nikapov	0:a1a69d32f310	2714	/* Initialize tuning settings buffer to prevent compiler warning. */
nikapov	0:a1a69d32f310	2715	p_tuning_setting_buffer = DefaultTuningSettings;
nikapov	0:a1a69d32f310	2716
sepp_nepp	11:c6f95a42d4d7	2717	if (ErrState == VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2718	if (UseInternalTuningSettings == 0)
sepp_nepp	11:c6f95a42d4d7	2719	{ p_tuning_setting_buffer = pTuningSettingsPointer; }
sepp_nepp	11:c6f95a42d4d7	2720	else
sepp_nepp	11:c6f95a42d4d7	2721	{ p_tuning_setting_buffer = DefaultTuningSettings; }
sepp_nepp	11:c6f95a42d4d7	2722	}
sepp_nepp	11:c6f95a42d4d7	2723
sepp_nepp	11:c6f95a42d4d7	2724	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2725	{ Load_tuning_settings(p_tuning_setting_buffer); }
sepp_nepp	11:c6f95a42d4d7	2726
sepp_nepp	11:c6f95a42d4d7	2727	/* Set interrupt config to new sample ready */
sepp_nepp	11:c6f95a42d4d7	2728	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2729	{ Set_GPIO_config(0,GPIO_FUNC_NEW_MEASURE_READY,VL53L0X_INTERRUPTPOLARITY_LOW); }
sepp_nepp	11:c6f95a42d4d7	2730
sepp_nepp	11:c6f95a42d4d7	2731	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2732	tempword = Read_Word(0x84);
sepp_nepp	11:c6f95a42d4d7	2733	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	2734
sepp_nepp	11:c6f95a42d4d7	2735	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2736	{ OscFrequencyMHz=FP412_TO_FP1616(tempword); }
sepp_nepp	11:c6f95a42d4d7	2737
sepp_nepp	11:c6f95a42d4d7	2738	/* After static init,some device parameters may be changed, so update them */
sepp_nepp	11:c6f95a42d4d7	2739	new_curr_parameters = Get_device_parameters();
sepp_nepp	11:c6f95a42d4d7	2740
sepp_nepp	11:c6f95a42d4d7	2741	if (ErrState == VL53L0X_OK) { tempbyte = Read_Byte(REG_SYSTEM_RANGE_CONFIG); }
sepp_nepp	11:c6f95a42d4d7	2742	if (ErrState == VL53L0X_OK) { RangeFractionalEnable = (tempbyte & 1); }
sepp_nepp	11:c6f95a42d4d7	2743	if (ErrState == VL53L0X_OK) { CurrParams = new_curr_parameters; }
nikapov	0:a1a69d32f310	2744
nikapov	0:a1a69d32f310	2745	/* read the sequence config and save it */
sepp_nepp	11:c6f95a42d4d7	2746	Set_SequenceConfig( Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG) ); // checks for ErrState
nikapov	0:a1a69d32f310	2747
nikapov	0:a1a69d32f310	2748	/* Disable MSRC and TCC by default */
sepp_nepp	11:c6f95a42d4d7	2749	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2750	{ Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0); }
sepp_nepp	11:c6f95a42d4d7	2751
sepp_nepp	11:c6f95a42d4d7	2752	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2753	{ Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0); }
sepp_nepp	11:c6f95a42d4d7	2754
sepp_nepp	11:c6f95a42d4d7	2755	/* Set State to standby */
sepp_nepp	11:c6f95a42d4d7	2756	Set_Current_State( VL53L0X_STATE_IDLE) ;
nikapov	0:a1a69d32f310	2757
nikapov	0:a1a69d32f310	2758	/* Store pre-range vcsel period */
sepp_nepp	11:c6f95a42d4d7	2759	if (ErrState == VL53L0X_OK)/* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	2760	{ vcsel_PPeriod_pclk = (Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
sepp_nepp	11:c6f95a42d4d7	2761
sepp_nepp	11:c6f95a42d4d7	2762	if ( ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2763	{ PreRangeVcselPPeriod = vcsel_PPeriod_pclk; }
nikapov	0:a1a69d32f310	2764
nikapov	0:a1a69d32f310	2765	/* Store final-range vcsel period */
sepp_nepp	11:c6f95a42d4d7	2766	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2767	{ vcsel_PPeriod_pclk /* Get and convert the VCSEL period register into actual clock periods */
sepp_nepp	11:c6f95a42d4d7	2768	= ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
sepp_nepp	11:c6f95a42d4d7	2769
sepp_nepp	11:c6f95a42d4d7	2770	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2771	{ FinalRangeVcselPPeriod = vcsel_PPeriod_pclk; }
nikapov	0:a1a69d32f310	2772
nikapov	0:a1a69d32f310	2773	/* Store pre-range timeout */
sepp_nepp	11:c6f95a42d4d7	2774	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2775	{ Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp	11:c6f95a42d4d7	2776	&seq_timeout_micro_secs); }
sepp_nepp	11:c6f95a42d4d7	2777
sepp_nepp	11:c6f95a42d4d7	2778	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2779	{ PreRangeTimeoutMicroSecs = seq_timeout_micro_secs; }
nikapov	0:a1a69d32f310	2780
nikapov	0:a1a69d32f310	2781	/* Store final-range timeout */
sepp_nepp	11:c6f95a42d4d7	2782	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2783	{ Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp	11:c6f95a42d4d7	2784	&seq_timeout_micro_secs);}
sepp_nepp	11:c6f95a42d4d7	2785
sepp_nepp	11:c6f95a42d4d7	2786	if (ErrState == VL53L0X_OK)
sepp_nepp	12:aa177f0e4c10	2787	{ FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs; }
nikapov	0:a1a69d32f310	2788	}
nikapov	0:a1a69d32f310	2789
sepp_nepp	11:c6f95a42d4d7	2790	void VL53L0X::Stop_Measurement()
sepp_nepp	11:c6f95a42d4d7	2791	{ Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_SINGLESHOT);
sepp_nepp	11:c6f95a42d4d7	2792	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2793	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	2794	Write_Byte(0x91,0x00);
sepp_nepp	11:c6f95a42d4d7	2795	Write_Byte(0x00,0x01);
sepp_nepp	11:c6f95a42d4d7	2796	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	2797
sepp_nepp	11:c6f95a42d4d7	2798	Set_Current_State( VL53L0X_STATE_IDLE );
nikapov	0:a1a69d32f310	2799
nikapov	0:a1a69d32f310	2800	/* Check if need to apply interrupt settings */
sepp_nepp	11:c6f95a42d4d7	2801	Check_and_load_interrupt_settings(0);
nikapov	0:a1a69d32f310	2802	}
nikapov	0:a1a69d32f310	2803
sepp_nepp	11:c6f95a42d4d7	2804	uint8_t VL53L0X::Get_Stop_Completed()
sepp_nepp	11:c6f95a42d4d7	2805	{ uint8_t Abyte = 0;
sepp_nepp	11:c6f95a42d4d7	2806
sepp_nepp	11:c6f95a42d4d7	2807	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2808	Abyte = Read_Byte(0x04);
sepp_nepp	11:c6f95a42d4d7	2809	Write_Byte(0xFF,0x0);
sepp_nepp	11:c6f95a42d4d7	2810
sepp_nepp	11:c6f95a42d4d7	2811	if ((ErrState == VL53L0X_OK) & (Abyte == 0))
sepp_nepp	11:c6f95a42d4d7	2812	{ Write_Byte(0x80,0x01);
sepp_nepp	11:c6f95a42d4d7	2813	Write_Byte(0xFF,0x01);
sepp_nepp	11:c6f95a42d4d7	2814	Write_Byte(0x00,0x00);
sepp_nepp	11:c6f95a42d4d7	2815	Write_Byte(0x91,StopVariable);
sepp_nepp	11:c6f95a42d4d7	2816	Write_Byte(0x00,0x01);
sepp_nepp	11:c6f95a42d4d7	2817	Write_Byte(0xFF,0x00);
sepp_nepp	11:c6f95a42d4d7	2818	Write_Byte(0x80,0x00);
sepp_nepp	11:c6f95a42d4d7	2819	}
sepp_nepp	11:c6f95a42d4d7	2820	return Abyte;
nikapov	0:a1a69d32f310	2821	}
nikapov	0:a1a69d32f310	2822
sepp_nepp	11:c6f95a42d4d7	2823	void VL53L0X::Wait_Measurement_Ready()
sepp_nepp	11:c6f95a42d4d7	2824	{ uint32_t loop_nb = 0;
sepp_nepp	11:c6f95a42d4d7	2825
sepp_nepp	11:c6f95a42d4d7	2826	// Wait until it finished, or loopo count reached = avoids deadlock
sepp_nepp	11:c6f95a42d4d7	2827	while ( !Get_Measurement_Ready() & (ErrState == VL53L0X_OK) )
sepp_nepp	11:c6f95a42d4d7	2828	{ if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP)
sepp_nepp	11:c6f95a42d4d7	2829	{ ErrState = VL53L0X_ERROR_TIME_OUT;}
sepp_nepp	11:c6f95a42d4d7	2830	else { Polling_delay(); }
sepp_nepp	11:c6f95a42d4d7	2831	} // while ends
nikapov	0:a1a69d32f310	2832	}
nikapov	0:a1a69d32f310	2833
sepp_nepp	11:c6f95a42d4d7	2834	void VL53L0X::Wait_Stop_Completed()
sepp_nepp	11:c6f95a42d4d7	2835	{ uint32_t loop_nb = 0;
sepp_nepp	11:c6f95a42d4d7	2836
sepp_nepp	11:c6f95a42d4d7	2837	// Wait until Stop_Completed, or loopo count reached = avoids deadlock
sepp_nepp	11:c6f95a42d4d7	2838	while ( (ErrState == VL53L0X_OK) & !Get_Stop_Completed() )
sepp_nepp	11:c6f95a42d4d7	2839	{ if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP)
sepp_nepp	11:c6f95a42d4d7	2840	{ ErrState = VL53L0X_ERROR_TIME_OUT;}
sepp_nepp	11:c6f95a42d4d7	2841	else { Polling_delay(); }
sepp_nepp	11:c6f95a42d4d7	2842	} // while ends
nikapov	0:a1a69d32f310	2843	}
nikapov	0:a1a69d32f310	2844
sepp_nepp	11:c6f95a42d4d7	2845	void VL53L0X::Range_meas_int_continuous_mode(void (*fptr)(void))
sepp_nepp	11:c6f95a42d4d7	2846	{ Stop_Measurement(); // it is safer to do this while sensor is stopped
sepp_nepp	11:c6f95a42d4d7	2847
sepp_nepp	11:c6f95a42d4d7	2848	Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp	11:c6f95a42d4d7	2849	GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH);
sepp_nepp	11:c6f95a42d4d7	2850	if (ErrState==VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	2851	Attach_interrupt_measure_detection_irq(fptr);
sepp_nepp	11:c6f95a42d4d7	2852	Enable_interrupt_measure_detection_irq(); }
sepp_nepp	11:c6f95a42d4d7	2853
sepp_nepp	11:c6f95a42d4d7	2854	Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS \| REG_RESULT_RANGE_STATUS);
sepp_nepp	11:c6f95a42d4d7	2855	// NB: return value was previously only passed to logging macro,but did not get passed back
sepp_nepp	11:c6f95a42d4d7	2856
sepp_nepp	11:c6f95a42d4d7	2857	if (ErrState==VL53L0X_OK) { Range_start_continuous_mode(); }
nikapov	0:a1a69d32f310	2858	}
nikapov	0:a1a69d32f310	2859
sepp_nepp	11:c6f95a42d4d7	2860
sepp_nepp	11:c6f95a42d4d7	2861	VL53L0X_Error VL53L0X::Start_Measurement(TOperatingMode operating_mode, void (*fptr)(void))
sepp_nepp	11:c6f95a42d4d7	2862	{ uint8_t VhvSettings;
nikapov	0:a1a69d32f310	2863	uint8_t PhaseCal;
sepp_nepp	7:41cbc431e1f4	2864	// *** from mass market cube expansion v1.1,ranging with satellites.
sepp_nepp	7:41cbc431e1f4	2865	// default settings,for normal range.
sepp_nepp	11:c6f95a42d4d7	2866	TFP1616 signalLimit = (TFP1616)(0.25 * 65536);
sepp_nepp	11:c6f95a42d4d7	2867	TFP1616 sigmaLimit = (TFP1616)(18 * 65536);
nikapov	0:a1a69d32f310	2868	uint32_t timingBudget = 33000;
nikapov	0:a1a69d32f310	2869	uint8_t preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	2870	uint8_t finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	2871
sepp_nepp	11:c6f95a42d4d7	2872	switch (operating_mode) {
sepp_nepp	11:c6f95a42d4d7	2873	case op_INT:
sepp_nepp	11:c6f95a42d4d7	2874	if (_gpio1Int == NULL) { ErrState=1; return ErrState; }
sepp_nepp	11:c6f95a42d4d7	2875	Stop_Measurement(); // it is safer to do this while sensor is stopped
sepp_nepp	11:c6f95a42d4d7	2876
sepp_nepp	11:c6f95a42d4d7	2877	Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp	11:c6f95a42d4d7	2878	GPIO_FUNC_NEW_MEASURE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH);
sepp_nepp	11:c6f95a42d4d7	2879
sepp_nepp	11:c6f95a42d4d7	2880	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2881	{ Attach_interrupt_measure_detection_irq(fptr);
sepp_nepp	11:c6f95a42d4d7	2882	Enable_interrupt_measure_detection_irq(); }
sepp_nepp	11:c6f95a42d4d7	2883
sepp_nepp	11:c6f95a42d4d7	2884	Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS \| REG_RESULT_RANGE_STATUS);
sepp_nepp	11:c6f95a42d4d7	2885	// NB: return value was previously only passed to logging macro, but did not get passed back
sepp_nepp	11:c6f95a42d4d7	2886
sepp_nepp	11:c6f95a42d4d7	2887	// Setup in continuous ranging mode
sepp_nepp	11:c6f95a42d4d7	2888	Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
sepp_nepp	11:c6f95a42d4d7	2889	Start_Measurement();
sepp_nepp	11:c6f95a42d4d7	2890	break;
sepp_nepp	11:c6f95a42d4d7	2891
sepp_nepp	11:c6f95a42d4d7	2892	case op_single_shot_poll:
sepp_nepp	11:c6f95a42d4d7	2893	// singelshot,polled ranging; no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
sepp_nepp	11:c6f95a42d4d7	2894	Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
nikapov	0:a1a69d32f310	2895
nikapov	0:a1a69d32f310	2896	// Enable/Disable Sigma and Signal check
sepp_nepp	11:c6f95a42d4d7	2897	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2898	{ Set_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,1); }
sepp_nepp	11:c6f95a42d4d7	2899
sepp_nepp	11:c6f95a42d4d7	2900	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2901	{ Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); }
sepp_nepp	11:c6f95a42d4d7	2902
sepp_nepp	11:c6f95a42d4d7	2903
sepp_nepp	11:c6f95a42d4d7	2904	/* Ranging configuration */
sepp_nepp	11:c6f95a42d4d7	2905	// *** from mass market cube expansion v1.1,ranging with satellites.
sepp_nepp	11:c6f95a42d4d7	2906	// switch(rangingConfig) {
sepp_nepp	11:c6f95a42d4d7	2907	// case LONG_RANGE:
sepp_nepp	11:c6f95a42d4d7	2908	signalLimit = (TFP1616)(0.1 * 65536);
sepp_nepp	11:c6f95a42d4d7	2909	sigmaLimit = (TFP1616)(60 * 65536);
nikapov	0:a1a69d32f310	2910	timingBudget = 33000;
nikapov	0:a1a69d32f310	2911	preRangeVcselPeriod = 18;
nikapov	0:a1a69d32f310	2912	finalRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	2913	/* break;
nikapov	0:a1a69d32f310	2914	case HIGH_ACCURACY:
sepp_nepp	11:c6f95a42d4d7	2915	signalLimit = (TFP1616)(0.25*65536);
sepp_nepp	11:c6f95a42d4d7	2916	sigmaLimit = (TFP1616)(18*65536);
nikapov	0:a1a69d32f310	2917	timingBudget = 200000;
nikapov	0:a1a69d32f310	2918	preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	2919	finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	2920	break;
nikapov	0:a1a69d32f310	2921	case HIGH_SPEED:
sepp_nepp	11:c6f95a42d4d7	2922	signalLimit = (TFP1616)(0.25*65536);
sepp_nepp	11:c6f95a42d4d7	2923	sigmaLimit = (TFP1616)(32*65536);
nikapov	0:a1a69d32f310	2924	timingBudget = 20000;
nikapov	0:a1a69d32f310	2925	preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	2926	finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	2927	break;
nikapov	0:a1a69d32f310	2928	default:
nikapov	0:a1a69d32f310	2929	debug_printf("Not Supported");
nikapov	0:a1a69d32f310	2930	}
nikapov	0:a1a69d32f310	2931	*/
nikapov	0:a1a69d32f310	2932
sepp_nepp	11:c6f95a42d4d7	2933	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2934	{ Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,signalLimit);}
sepp_nepp	11:c6f95a42d4d7	2935
sepp_nepp	11:c6f95a42d4d7	2936	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2937	{ Set_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,sigmaLimit);}
sepp_nepp	11:c6f95a42d4d7	2938
sepp_nepp	11:c6f95a42d4d7	2939	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2940	{ Set_Measure_Time_Budget_us(timingBudget);}
sepp_nepp	11:c6f95a42d4d7	2941
sepp_nepp	11:c6f95a42d4d7	2942	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2943	{ Set_vcsel_PPeriod(VL53L0X_VCSEL_PRE_RANGE,preRangeVcselPeriod); }
sepp_nepp	11:c6f95a42d4d7	2944
sepp_nepp	11:c6f95a42d4d7	2945	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2946	{ Set_vcsel_PPeriod(VL53L0X_VCSEL_FINAL_RANGE,finalRangeVcselPeriod);}
sepp_nepp	11:c6f95a42d4d7	2947
sepp_nepp	11:c6f95a42d4d7	2948	if (ErrState == VL53L0X_OK)
sepp_nepp	11:c6f95a42d4d7	2949	{ Perf_Ref_calibration(&VhvSettings,&PhaseCal,1); }
sepp_nepp	11:c6f95a42d4d7	2950	break;
sepp_nepp	11:c6f95a42d4d7	2951	case op_poll: // Setup in continuous ranging mode
sepp_nepp	11:c6f95a42d4d7	2952	Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
sepp_nepp	11:c6f95a42d4d7	2953	Start_Measurement();
sepp_nepp	11:c6f95a42d4d7	2954	} // switch
sepp_nepp	11:c6f95a42d4d7	2955	return ErrState;
sepp_nepp	11:c6f95a42d4d7	2956	}
sepp_nepp	11:c6f95a42d4d7	2957
sepp_nepp	11:c6f95a42d4d7	2958	VL53L0X_Error VL53L0X::Stop_Measurement(TOperatingMode operating_mode)
sepp_nepp	11:c6f95a42d4d7	2959	{ if ((ErrState == VL53L0X_OK) &
sepp_nepp	11:c6f95a42d4d7	2960	(operating_mode == op_INT \|\| operating_mode == op_poll) )
sepp_nepp	11:c6f95a42d4d7	2961	{ // only stop if in one of the continuous modes !!!!
sepp_nepp	11:c6f95a42d4d7	2962	Stop_Measurement();
sepp_nepp	11:c6f95a42d4d7	2963	Wait_Stop_Completed();
sepp_nepp	11:c6f95a42d4d7	2964	Clear_interrupt_mask( REG_SYSINT_GPIO_NEW_SAMPLE_READY);
sepp_nepp	11:c6f95a42d4d7	2965	}
sepp_nepp	11:c6f95a42d4d7	2966	return ErrState;
sepp_nepp	11:c6f95a42d4d7	2967	}
sepp_nepp	11:c6f95a42d4d7	2968
sepp_nepp	11:c6f95a42d4d7	2969	TRangeResults VL53L0X::Handle_irq(TOperatingMode operating_mode)
sepp_nepp	11:c6f95a42d4d7	2970	{ TRangeResults RangeResults;
sepp_nepp	11:c6f95a42d4d7	2971	RangeResults = Get_Measurement(operating_mode);
sepp_nepp	11:c6f95a42d4d7	2972	Enable_interrupt_measure_detection_irq();
sepp_nepp	11:c6f95a42d4d7	2973	return RangeResults;
sepp_nepp	11:c6f95a42d4d7	2974	}
sepp_nepp	11:c6f95a42d4d7	2975
sepp_nepp	11:c6f95a42d4d7	2976	/**************** Private device functions ***********************/
sepp_nepp	11:c6f95a42d4d7	2977
sepp_nepp	11:c6f95a42d4d7	2978	void VL53L0X::Wait_read_strobe()
sepp_nepp	11:c6f95a42d4d7	2979	{ uint32_t loop_nb = 0;
sepp_nepp	11:c6f95a42d4d7	2980
sepp_nepp	11:c6f95a42d4d7	2981	Write_Byte(0x83,0x00); // set strobe register to 0
sepp_nepp	11:c6f95a42d4d7	2982
sepp_nepp	11:c6f95a42d4d7	2983	/* polling while no error, no strobe, and not reached max number of loop to avoid deadlock*/
sepp_nepp	11:c6f95a42d4d7	2984	while ((ErrState == VL53L0X_OK) && (Read_Byte(0x83) == 0x00) )
sepp_nepp	11:c6f95a42d4d7	2985	{ if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP)
sepp_nepp	11:c6f95a42d4d7	2986	{ ErrState = VL53L0X_ERROR_TIME_OUT; } }
sepp_nepp	11:c6f95a42d4d7	2987
sepp_nepp	11:c6f95a42d4d7	2988	Write_Byte(0x83,0x01); // set strobe register back to 1 'manually'
nikapov	0:a1a69d32f310	2989	}
nikapov	0:a1a69d32f310	2990
nikapov	0:a1a69d32f310	2991	/******************************************************************************/
sepp_nepp	11:c6f95a42d4d7	2992	/**************** Small Service and wrapper functions ********************/
sepp_nepp	11:c6f95a42d4d7	2993	/******************************************************************************/
sepp_nepp	11:c6f95a42d4d7	2994	uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10,REF_ARRAY_SPAD_5,
sepp_nepp	11:c6f95a42d4d7	2995	REF_ARRAY_SPAD_0,REF_ARRAY_SPAD_5 };
sepp_nepp	11:c6f95a42d4d7	2996
sepp_nepp	11:c6f95a42d4d7	2997
sepp_nepp	11:c6f95a42d4d7	2998	uint32_t VL53L0X::Decode_timeout(uint16_t encoded_timeout)
sepp_nepp	11:c6f95a42d4d7	2999	{ /Decode 16-bit timeout register value - format (LSByte 2^MSByte) + 1 */
sepp_nepp	11:c6f95a42d4d7	3000	return ((uint32_t) (encoded_timeout & 0x00FF)
sepp_nepp	11:c6f95a42d4d7	3001	<< (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
sepp_nepp	11:c6f95a42d4d7	3002	}
sepp_nepp	11:c6f95a42d4d7	3003
sepp_nepp	11:c6f95a42d4d7	3004	uint8_t VL53L0X::Is_ApertureSPAD(uint32_t SPAD_index)
sepp_nepp	11:c6f95a42d4d7	3005	{ /* This function reports if a given SPAD index is an aperture SPAD by
sepp_nepp	11:c6f95a42d4d7	3006	* deriving the quadrant = SPAD_index >> 6. */
sepp_nepp	11:c6f95a42d4d7	3007	if (refArrayQuadrants[SPAD_index >> 6] == REF_ARRAY_SPAD_0)
sepp_nepp	11:c6f95a42d4d7	3008	{ return 0; } else { return 1; }
sepp_nepp	11:c6f95a42d4d7	3009	}
sepp_nepp	11:c6f95a42d4d7	3010
sepp_nepp	11:c6f95a42d4d7	3011	/******************************************************************************/
sepp_nepp	11:c6f95a42d4d7	3012	/**************** Write and read functions from I2C ***********************/
sepp_nepp	11:c6f95a42d4d7	3013	/******************************************************************************/
sepp_nepp	11:c6f95a42d4d7	3014
sepp_nepp	12:aa177f0e4c10	3015
sepp_nepp	12:aa177f0e4c10	3016	uint32_t VL53L0X::Get_NVM_DWord(uint8_t NVM_Address)
sepp_nepp	12:aa177f0e4c10	3017	{ Write_Byte(0x94,NVM_Address);
sepp_nepp	12:aa177f0e4c10	3018	Wait_read_strobe();
sepp_nepp	12:aa177f0e4c10	3019	return Read_DWord(0x90);
sepp_nepp	12:aa177f0e4c10	3020	}
sepp_nepp	12:aa177f0e4c10	3021	uint16_t VL53L0X::Get_NVM_Word(uint8_t NVM_Address)
sepp_nepp	12:aa177f0e4c10	3022	{ Write_Byte(0x94,NVM_Address);
sepp_nepp	12:aa177f0e4c10	3023	Wait_read_strobe();
sepp_nepp	12:aa177f0e4c10	3024	return Read_Word(0x90);
sepp_nepp	12:aa177f0e4c10	3025	}
sepp_nepp	12:aa177f0e4c10	3026	uint8_t VL53L0X::Get_NVM_Byte(uint8_t NVM_Address)
sepp_nepp	12:aa177f0e4c10	3027	{ Write_Byte(0x94,NVM_Address);
sepp_nepp	12:aa177f0e4c10	3028	Wait_read_strobe();
sepp_nepp	12:aa177f0e4c10	3029	return Read_Byte(0x90);
sepp_nepp	12:aa177f0e4c10	3030	}
sepp_nepp	11:c6f95a42d4d7	3031	void VL53L0X::Write_Byte(uint8_t index, uint8_t data)
sepp_nepp	11:c6f95a42d4d7	3032	{ I2c_Write(index,&data,1); }
sepp_nepp	11:c6f95a42d4d7	3033
sepp_nepp	11:c6f95a42d4d7	3034	void VL53L0X::Write_Word(uint8_t index,uint16_t data)
sepp_nepp	11:c6f95a42d4d7	3035	{ uint8_t buffer[2];
sepp_nepp	11:c6f95a42d4d7	3036	buffer[0] = data >> 8;
sepp_nepp	11:c6f95a42d4d7	3037	buffer[1] = data & 0x00FF;
sepp_nepp	11:c6f95a42d4d7	3038	I2c_Write(index,(uint8_t *)buffer,2);
sepp_nepp	11:c6f95a42d4d7	3039	}
sepp_nepp	11:c6f95a42d4d7	3040
sepp_nepp	11:c6f95a42d4d7	3041	void VL53L0X::Write_DWord(uint8_t index, uint32_t data)
sepp_nepp	11:c6f95a42d4d7	3042	{ uint8_t buffer[4];
sepp_nepp	11:c6f95a42d4d7	3043	buffer[0] = (data >> 24) & 0xFF;
sepp_nepp	11:c6f95a42d4d7	3044	buffer[1] = (data >> 16) & 0xFF;
sepp_nepp	11:c6f95a42d4d7	3045	buffer[2] = (data >> 8) & 0xFF;
sepp_nepp	11:c6f95a42d4d7	3046	buffer[3] = (data >> 0) & 0xFF;
sepp_nepp	11:c6f95a42d4d7	3047	I2c_Write(index,(uint8_t *)buffer,4);
sepp_nepp	11:c6f95a42d4d7	3048	}
sepp_nepp	11:c6f95a42d4d7	3049
sepp_nepp	11:c6f95a42d4d7	3050	uint8_t VL53L0X::Read_Byte(uint8_t index)
sepp_nepp	11:c6f95a42d4d7	3051	{ uint8_t result;
sepp_nepp	11:c6f95a42d4d7	3052	I2c_Read(index,&result,1);
sepp_nepp	11:c6f95a42d4d7	3053	return result;
sepp_nepp	11:c6f95a42d4d7	3054	}
sepp_nepp	11:c6f95a42d4d7	3055
sepp_nepp	11:c6f95a42d4d7	3056	uint16_t VL53L0X::Read_Word(uint8_t index)
sepp_nepp	11:c6f95a42d4d7	3057	{ uint8_t buffer[2] = {0,0};
sepp_nepp	11:c6f95a42d4d7	3058	I2c_Read(index, &buffer[0], 2);
sepp_nepp	11:c6f95a42d4d7	3059	return (buffer[0] << 8) + buffer[1];
sepp_nepp	11:c6f95a42d4d7	3060	}
sepp_nepp	11:c6f95a42d4d7	3061
sepp_nepp	11:c6f95a42d4d7	3062	uint32_t VL53L0X::Read_DWord(uint8_t index)
sepp_nepp	11:c6f95a42d4d7	3063	{ uint8_t buffer[4] = {0,0,0,0};
sepp_nepp	11:c6f95a42d4d7	3064	I2c_Read(index,buffer,4);
sepp_nepp	11:c6f95a42d4d7	3065	return (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
sepp_nepp	11:c6f95a42d4d7	3066	}
sepp_nepp	11:c6f95a42d4d7	3067
sepp_nepp	11:c6f95a42d4d7	3068	void VL53L0X::Register_BitMask(uint8_t index,uint8_t and_mask,uint8_t or_mask)
sepp_nepp	11:c6f95a42d4d7	3069	{ uint8_t buffer = 0;
sepp_nepp	11:c6f95a42d4d7	3070	/* read data direct onto buffer */
sepp_nepp	11:c6f95a42d4d7	3071	I2c_Read(index,&buffer,1);
sepp_nepp	11:c6f95a42d4d7	3072	if (ErrState==VL53L0X_OK) {
sepp_nepp	11:c6f95a42d4d7	3073	buffer = (buffer & and_mask) \| or_mask;
sepp_nepp	11:c6f95a42d4d7	3074	I2c_Write(index,&buffer,(uint8_t)1);
sepp_nepp	11:c6f95a42d4d7	3075	}
sepp_nepp	11:c6f95a42d4d7	3076	}
sepp_nepp	11:c6f95a42d4d7	3077
sepp_nepp	11:c6f95a42d4d7	3078	/**
sepp_nepp	11:c6f95a42d4d7	3079	* @brief Writes a buffer towards the I2C peripheral device.
sepp_nepp	11:c6f95a42d4d7	3080	* @param RegisterAddr specifies the internal address register
sepp_nepp	11:c6f95a42d4d7	3081	* @param p_data pointer to the byte-array data to send
sepp_nepp	11:c6f95a42d4d7	3082	* where to start writing to (must be correctly masked).
sepp_nepp	11:c6f95a42d4d7	3083	* @param NumByteToWrite number of bytes to be written.
sepp_nepp	11:c6f95a42d4d7	3084	* @note On some devices if NumByteToWrite is greater
sepp_nepp	11:c6f95a42d4d7	3085	* than one, the RegisterAddr must be masked correctly! */
sepp_nepp	11:c6f95a42d4d7	3086	void VL53L0X::I2c_Write( uint8_t RegisterAddr, uint8_t *p_data,
sepp_nepp	11:c6f95a42d4d7	3087	uint16_t NumByteToWrite )
sepp_nepp	11:c6f95a42d4d7	3088	{ int ret;
sepp_nepp	11:c6f95a42d4d7	3089	uint8_t tmp[TEMP_BUF_SIZE];
sepp_nepp	11:c6f95a42d4d7	3090
sepp_nepp	11:c6f95a42d4d7	3091	if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State!!!!
sepp_nepp	11:c6f95a42d4d7	3092
sepp_nepp	11:c6f95a42d4d7	3093	if (NumByteToWrite >= TEMP_BUF_SIZE)
sepp_nepp	11:c6f95a42d4d7	3094	{ErrState = VL53L0X_ERROR_I2C_BUF_OVERFLOW; return; };
sepp_nepp	11:c6f95a42d4d7	3095
sepp_nepp	11:c6f95a42d4d7	3096	/* First, send device address. Then, send data and terminate with STOP condition */
sepp_nepp	11:c6f95a42d4d7	3097	tmp[0] = RegisterAddr;
sepp_nepp	11:c6f95a42d4d7	3098	memcpy(tmp+1, p_data, NumByteToWrite);
sepp_nepp	11:c6f95a42d4d7	3099	ret = _dev_i2c->write(I2cDevAddr, (const char*)tmp, NumByteToWrite+1, false);
sepp_nepp	11:c6f95a42d4d7	3100
sepp_nepp	11:c6f95a42d4d7	3101	if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; }
sepp_nepp	11:c6f95a42d4d7	3102	}
sepp_nepp	11:c6f95a42d4d7	3103
sepp_nepp	11:c6f95a42d4d7	3104	/**@brief Reads a buffer from the I2C peripheral device.
sepp_nepp	11:c6f95a42d4d7	3105	* @param pBuffer pointer to the byte-array to read data in to
sepp_nepp	11:c6f95a42d4d7	3106	* @param I2cDevAddr specifies the peripheral device slave address.
sepp_nepp	11:c6f95a42d4d7	3107	* @param RegisterAddr specifies the internal address register
sepp_nepp	11:c6f95a42d4d7	3108	* where to start reading from (must be correctly masked).
sepp_nepp	11:c6f95a42d4d7	3109	* @param NumByteToRead number of bytes to be read, maximum VL53L0X_MAX_I2C_XFER_SIZE
sepp_nepp	11:c6f95a42d4d7	3110	* @note On some devices if NumByteToWrite is greater
sepp_nepp	11:c6f95a42d4d7	3111	* than one, the RegisterAddr must be masked correctly!
sepp_nepp	11:c6f95a42d4d7	3112	*/
sepp_nepp	11:c6f95a42d4d7	3113	void VL53L0X::I2c_Read(uint8_t RegisterAddr, uint8_t *p_data,
sepp_nepp	11:c6f95a42d4d7	3114	uint16_t NumByteToRead)
sepp_nepp	11:c6f95a42d4d7	3115	{ int ret;
sepp_nepp	11:c6f95a42d4d7	3116	if (ErrState != VL53L0X_OK) {return; } // no comms while in Error State, return value undefined!!!!
sepp_nepp	11:c6f95a42d4d7	3117
sepp_nepp	11:c6f95a42d4d7	3118	/* Send device address, without STOP condition */
sepp_nepp	11:c6f95a42d4d7	3119	ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true);
sepp_nepp	11:c6f95a42d4d7	3120	if(!ret) /* Read data, with STOP condition */
sepp_nepp	11:c6f95a42d4d7	3121	{ ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); }
sepp_nepp	11:c6f95a42d4d7	3122
sepp_nepp	11:c6f95a42d4d7	3123	if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE; }
sepp_nepp	11:c6f95a42d4d7	3124	}
sepp_nepp	11:c6f95a42d4d7	3125
sepp_nepp	11:c6f95a42d4d7	3126	/******************************************************************************/

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	23 Mar 2019
Imports:	20
Forks:	0
Commits:	13
Dependents:	1
Dependencies:	0
Followers:	2

VL53L0X.cpp@12:aa177f0e4c10, 2019-04-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning