VL53L0X_Condensed - Condensed Version of Public VL53L0X

Users » sepp_nepp » Code » VL53L0X_Condensed

Condensed Version of Public VL53L0X

VL53L0X.cpp@11:c6f95a42d4d7, 2019-04-08 (annotated)

Committer:: sepp_nepp
Date:: Mon Apr 08 16:26:19 2019 +0000
Revision:: 11:c6f95a42d4d7
Parent:: 10:cd251e0fc2fd
Child:: 12:aa177f0e4c10

Latest refactoring changes compile.; But now stuck with "Warning: L3912W: Option 'lecacyalign' is deprecated."

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

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@11:c6f95a42d4d7, 2019-04-08 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning