VL53L0X_Condensed - Condensed Version of Public VL53L0X

Users » sepp_nepp » Code » VL53L0X_Condensed

Condensed Version of Public VL53L0X

VL53L0X.cpp@7:41cbc431e1f4, 2019-03-24 (annotated)

Committer:: sepp_nepp
Date:: Sun Mar 24 18:18:54 2019 +0000
Revision:: 7:41cbc431e1f4
Parent:: 6:1976f4afed97
Child:: 8:abea9638127a

Many edits, but bizzarre compile error;

Who changed what in which revision?

User	Revision	Line number	New contents of line
nikapov	0:a1a69d32f310	1	/**
nikapov	0:a1a69d32f310	2	******************************************************************************
nikapov	0:a1a69d32f310	3	* @file VL53L0X_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_SETDEVICESPECIFICPARAMETER(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	40	// _device->DeviceSpecificParameters.\1 = \3;
sepp_nepp	7:41cbc431e1f4	41
sepp_nepp	7:41cbc431e1f4	42	// \QVL53L0X_GETDEVICESPECIFICPARAMETER(\E([A-Z\d]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	43	// _device->DeviceSpecificParameters.\1;
sepp_nepp	7:41cbc431e1f4	44
sepp_nepp	7:41cbc431e1f4	45	// \QVL53L0X_SETPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	46	// _device->CurrentParameters.\1 = \3;
sepp_nepp	7:41cbc431e1f4	47
sepp_nepp	7:41cbc431e1f4	48	// \QVL53L0X_GETPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	49	// \3 = _device->CurrentParameters.\1
sepp_nepp	7:41cbc431e1f4	50
sepp_nepp	7:41cbc431e1f4	51	// \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s)([A-Z\d_]+)[[:punct:]](\s)([A-Z\d_]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	52	// _device->CurrentParameters.\1[\3] = \5;
sepp_nepp	7:41cbc431e1f4	53	// is used to replace following macro for ex:
sepp_nepp	7:41cbc431e1f4	54	// #define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value) \
sepp_nepp	7:41cbc431e1f4	55	//_device->CurrentParameters.field[index] = value
sepp_nepp	7:41cbc431e1f4	56
sepp_nepp	7:41cbc431e1f4	57	// \QVL53L0X_GETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s)([A-Z\d_]+)[[:punct:]](\s)([A-Z\d_]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	58	// \5 = _device->CurrentParameters.\1[\3]
sepp_nepp	7:41cbc431e1f4	59	// is used to replace following macro for ex:
sepp_nepp	7:41cbc431e1f4	60	// #define VL53L0X_GETARRAYPARAMETERFIELD(field, index, variable) \
sepp_nepp	7:41cbc431e1f4	61	// variable = _device-> CurrentParameters.field[index]
sepp_nepp	7:41cbc431e1f4	62
sepp_nepp	7:41cbc431e1f4	63	// \QPALDevDataSet(\E([A-Z\d]+)[[:punct:]]([[:space:]]*)([A-Z\d_]+)\Q);\E
sepp_nepp	7:41cbc431e1f4	64	// _device->\1 = \3;
sepp_nepp	7:41cbc431e1f4	65	// is used to replace:
sepp_nepp	7:41cbc431e1f4	66	// #define PALDevDataSet(field, data) (Dev->field)=(data)
sepp_nepp	7:41cbc431e1f4	67
sepp_nepp	7:41cbc431e1f4	68
sepp_nepp	7:41cbc431e1f4	69
nikapov	0:a1a69d32f310	70	/* Includes */
nikapov	0:a1a69d32f310	71	#include <stdlib.h>
nikapov	0:a1a69d32f310	72	#include "VL53L0X.h"
sepp_nepp	7:41cbc431e1f4	73
sepp_nepp	7:41cbc431e1f4	74	VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe()
nikapov	0:a1a69d32f310	75	{
nikapov	0:a1a69d32f310	76	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	77	uint8_t strobe;
nikapov	0:a1a69d32f310	78	uint32_t loop_nb;
nikapov	0:a1a69d32f310	79
sepp_nepp	7:41cbc431e1f4	80	status \|= VL53L0X_write_byte(0x83,0x00);
nikapov	0:a1a69d32f310	81
nikapov	0:a1a69d32f310	82	/* polling
nikapov	0:a1a69d32f310	83	* use timeout to avoid deadlock*/
nikapov	0:a1a69d32f310	84	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	85	loop_nb = 0;
nikapov	0:a1a69d32f310	86	do {
sepp_nepp	7:41cbc431e1f4	87	status = VL53L0X_read_byte(0x83,&strobe);
nikapov	0:a1a69d32f310	88	if ((strobe != 0x00) \|\| status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	89	break;
nikapov	0:a1a69d32f310	90	}
nikapov	0:a1a69d32f310	91
nikapov	0:a1a69d32f310	92	loop_nb = loop_nb + 1;
nikapov	0:a1a69d32f310	93	} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov	0:a1a69d32f310	94
nikapov	0:a1a69d32f310	95	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	96	status = VL53L0X_ERROR_TIME_OUT;
nikapov	0:a1a69d32f310	97	}
nikapov	0:a1a69d32f310	98	}
nikapov	0:a1a69d32f310	99
sepp_nepp	7:41cbc431e1f4	100	status \|= VL53L0X_write_byte(0x83,0x01);
nikapov	0:a1a69d32f310	101
nikapov	0:a1a69d32f310	102	return status;
nikapov	0:a1a69d32f310	103	}
nikapov	0:a1a69d32f310	104
sepp_nepp	7:41cbc431e1f4	105	VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(uint8_t option)
nikapov	0:a1a69d32f310	106	{
nikapov	0:a1a69d32f310	107	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	108	uint8_t byte;
nikapov	0:a1a69d32f310	109	uint32_t tmp_dword;
nikapov	0:a1a69d32f310	110	uint8_t module_id;
nikapov	0:a1a69d32f310	111	uint8_t revision;
nikapov	0:a1a69d32f310	112	uint8_t reference_spad_count = 0;
nikapov	0:a1a69d32f310	113	uint8_t reference_spad_type = 0;
nikapov	0:a1a69d32f310	114	uint32_t part_uid_upper = 0;
nikapov	0:a1a69d32f310	115	uint32_t part_uid_lower = 0;
nikapov	0:a1a69d32f310	116	uint32_t offset_fixed1104_mm = 0;
nikapov	0:a1a69d32f310	117	int16_t offset_micro_meters = 0;
nikapov	0:a1a69d32f310	118	uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
nikapov	0:a1a69d32f310	119	uint32_t dist_meas_fixed1104_400_mm = 0;
nikapov	0:a1a69d32f310	120	uint32_t signal_rate_meas_fixed1104_400_mm = 0;
nikapov	0:a1a69d32f310	121	char product_id[19];
nikapov	0:a1a69d32f310	122	char *product_id_tmp;
nikapov	0:a1a69d32f310	123	uint8_t read_data_from_device_done;
nikapov	0:a1a69d32f310	124	FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0;
nikapov	0:a1a69d32f310	125	uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE];
nikapov	0:a1a69d32f310	126	int i;
sepp_nepp	5:b95f6951f7d5	127
nikapov	0:a1a69d32f310	128
sepp_nepp	7:41cbc431e1f4	129	read_data_from_device_done = _device->DeviceSpecificParameters.ReadDataFromDeviceDone;
sepp_nepp	7:41cbc431e1f4	130	read_data_from_device_done = _device->DeviceSpecificParameters.ReadDataFromDeviceDone;
nikapov	0:a1a69d32f310	131
nikapov	0:a1a69d32f310	132	/* This access is done only once after that a GetDeviceInfo or
nikapov	0:a1a69d32f310	133	* datainit is done*/
nikapov	0:a1a69d32f310	134	if (read_data_from_device_done != 7) {
nikapov	0:a1a69d32f310	135
sepp_nepp	7:41cbc431e1f4	136	status \|= VL53L0X_write_byte(0x80,0x01);
sepp_nepp	7:41cbc431e1f4	137	status \|= VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	138	status \|= VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	139
sepp_nepp	7:41cbc431e1f4	140	status \|= VL53L0X_write_byte(0xFF,0x06);
sepp_nepp	7:41cbc431e1f4	141	status \|= VL53L0X_read_byte(0x83,&byte);
sepp_nepp	7:41cbc431e1f4	142	status \|= VL53L0X_write_byte(0x83,byte \| 4);
sepp_nepp	7:41cbc431e1f4	143	status \|= VL53L0X_write_byte(0xFF,0x07);
sepp_nepp	7:41cbc431e1f4	144	status \|= VL53L0X_write_byte(0x81,0x01);
sepp_nepp	7:41cbc431e1f4	145
sepp_nepp	7:41cbc431e1f4	146	status \|= VL53L0X_polling_delay();
sepp_nepp	7:41cbc431e1f4	147
sepp_nepp	7:41cbc431e1f4	148	status \|= VL53L0X_write_byte(0x80,0x01);
nikapov	0:a1a69d32f310	149
nikapov	0:a1a69d32f310	150	if (((option & 1) == 1) &&
nikapov	0:a1a69d32f310	151	((read_data_from_device_done & 1) == 0)) {
sepp_nepp	7:41cbc431e1f4	152	status \|= VL53L0X_write_byte(0x94,0x6b);
sepp_nepp	7:41cbc431e1f4	153	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	154	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	155
nikapov	0:a1a69d32f310	156	reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f);
nikapov	0:a1a69d32f310	157	reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01);
nikapov	0:a1a69d32f310	158
sepp_nepp	7:41cbc431e1f4	159	status \|= VL53L0X_write_byte(0x94,0x24);
sepp_nepp	7:41cbc431e1f4	160	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	161	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	162
nikapov	0:a1a69d32f310	163
nikapov	0:a1a69d32f310	164	nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24)
nikapov	0:a1a69d32f310	165	& 0xff);
nikapov	0:a1a69d32f310	166	nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16)
nikapov	0:a1a69d32f310	167	& 0xff);
nikapov	0:a1a69d32f310	168	nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8)
nikapov	0:a1a69d32f310	169	& 0xff);
nikapov	0:a1a69d32f310	170	nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff);
nikapov	0:a1a69d32f310	171
sepp_nepp	7:41cbc431e1f4	172	status \|= VL53L0X_write_byte(0x94,0x25);
sepp_nepp	7:41cbc431e1f4	173	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	174	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	175
nikapov	0:a1a69d32f310	176	nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24)
nikapov	0:a1a69d32f310	177	& 0xff);
nikapov	0:a1a69d32f310	178	nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16)
nikapov	0:a1a69d32f310	179	& 0xff);
nikapov	0:a1a69d32f310	180	}
nikapov	0:a1a69d32f310	181
nikapov	0:a1a69d32f310	182	if (((option & 2) == 2) &&
nikapov	0:a1a69d32f310	183	((read_data_from_device_done & 2) == 0)) {
nikapov	0:a1a69d32f310	184
sepp_nepp	7:41cbc431e1f4	185	status \|= VL53L0X_write_byte(0x94,0x02);
sepp_nepp	7:41cbc431e1f4	186	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	187	status \|= VL53L0X_read_byte(0x90,&module_id);
sepp_nepp	7:41cbc431e1f4	188
sepp_nepp	7:41cbc431e1f4	189	status \|= VL53L0X_write_byte(0x94,0x7B);
sepp_nepp	7:41cbc431e1f4	190	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	191	status \|= VL53L0X_read_byte(0x90,&revision);
sepp_nepp	7:41cbc431e1f4	192
sepp_nepp	7:41cbc431e1f4	193	status \|= VL53L0X_write_byte(0x94,0x77);
sepp_nepp	7:41cbc431e1f4	194	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	195	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	196
nikapov	0:a1a69d32f310	197	product_id[0] = (char)((tmp_dword >> 25) & 0x07f);
nikapov	0:a1a69d32f310	198	product_id[1] = (char)((tmp_dword >> 18) & 0x07f);
nikapov	0:a1a69d32f310	199	product_id[2] = (char)((tmp_dword >> 11) & 0x07f);
nikapov	0:a1a69d32f310	200	product_id[3] = (char)((tmp_dword >> 4) & 0x07f);
nikapov	0:a1a69d32f310	201
nikapov	0:a1a69d32f310	202	byte = (uint8_t)((tmp_dword & 0x00f) << 3);
nikapov	0:a1a69d32f310	203
sepp_nepp	7:41cbc431e1f4	204	status \|= VL53L0X_write_byte(0x94,0x78);
sepp_nepp	7:41cbc431e1f4	205	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	206	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	207
nikapov	0:a1a69d32f310	208	product_id[4] = (char)(byte +
nikapov	0:a1a69d32f310	209	((tmp_dword >> 29) & 0x07f));
nikapov	0:a1a69d32f310	210	product_id[5] = (char)((tmp_dword >> 22) & 0x07f);
nikapov	0:a1a69d32f310	211	product_id[6] = (char)((tmp_dword >> 15) & 0x07f);
nikapov	0:a1a69d32f310	212	product_id[7] = (char)((tmp_dword >> 8) & 0x07f);
nikapov	0:a1a69d32f310	213	product_id[8] = (char)((tmp_dword >> 1) & 0x07f);
nikapov	0:a1a69d32f310	214
nikapov	0:a1a69d32f310	215	byte = (uint8_t)((tmp_dword & 0x001) << 6);
nikapov	0:a1a69d32f310	216
sepp_nepp	7:41cbc431e1f4	217	status \|= VL53L0X_write_byte(0x94,0x79);
sepp_nepp	7:41cbc431e1f4	218
sepp_nepp	7:41cbc431e1f4	219	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	220
sepp_nepp	7:41cbc431e1f4	221	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	222
nikapov	0:a1a69d32f310	223	product_id[9] = (char)(byte +
nikapov	0:a1a69d32f310	224	((tmp_dword >> 26) & 0x07f));
nikapov	0:a1a69d32f310	225	product_id[10] = (char)((tmp_dword >> 19) & 0x07f);
nikapov	0:a1a69d32f310	226	product_id[11] = (char)((tmp_dword >> 12) & 0x07f);
nikapov	0:a1a69d32f310	227	product_id[12] = (char)((tmp_dword >> 5) & 0x07f);
nikapov	0:a1a69d32f310	228
nikapov	0:a1a69d32f310	229	byte = (uint8_t)((tmp_dword & 0x01f) << 2);
nikapov	0:a1a69d32f310	230
sepp_nepp	7:41cbc431e1f4	231	status \|= VL53L0X_write_byte(0x94,0x7A);
sepp_nepp	7:41cbc431e1f4	232
sepp_nepp	7:41cbc431e1f4	233	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	234
sepp_nepp	7:41cbc431e1f4	235	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	236
nikapov	0:a1a69d32f310	237	product_id[13] = (char)(byte +
nikapov	0:a1a69d32f310	238	((tmp_dword >> 30) & 0x07f));
nikapov	0:a1a69d32f310	239	product_id[14] = (char)((tmp_dword >> 23) & 0x07f);
nikapov	0:a1a69d32f310	240	product_id[15] = (char)((tmp_dword >> 16) & 0x07f);
nikapov	0:a1a69d32f310	241	product_id[16] = (char)((tmp_dword >> 9) & 0x07f);
nikapov	0:a1a69d32f310	242	product_id[17] = (char)((tmp_dword >> 2) & 0x07f);
nikapov	0:a1a69d32f310	243	product_id[18] = '\0';
nikapov	0:a1a69d32f310	244
nikapov	0:a1a69d32f310	245	}
nikapov	0:a1a69d32f310	246
nikapov	0:a1a69d32f310	247	if (((option & 4) == 4) &&
nikapov	0:a1a69d32f310	248	((read_data_from_device_done & 4) == 0)) {
nikapov	0:a1a69d32f310	249
sepp_nepp	7:41cbc431e1f4	250	status \|= VL53L0X_write_byte(0x94,0x7B);
sepp_nepp	7:41cbc431e1f4	251	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	252	status \|= VL53L0X_read_dword(0x90,&part_uid_upper);
sepp_nepp	7:41cbc431e1f4	253
sepp_nepp	7:41cbc431e1f4	254	status \|= VL53L0X_write_byte(0x94,0x7C);
sepp_nepp	7:41cbc431e1f4	255	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	256	status \|= VL53L0X_read_dword(0x90,&part_uid_lower);
sepp_nepp	7:41cbc431e1f4	257
sepp_nepp	7:41cbc431e1f4	258	status \|= VL53L0X_write_byte(0x94,0x73);
sepp_nepp	7:41cbc431e1f4	259	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	260	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	261
nikapov	0:a1a69d32f310	262	signal_rate_meas_fixed1104_400_mm = (tmp_dword &
nikapov	0:a1a69d32f310	263	0x0000000ff) << 8;
nikapov	0:a1a69d32f310	264
sepp_nepp	7:41cbc431e1f4	265	status \|= VL53L0X_write_byte(0x94,0x74);
sepp_nepp	7:41cbc431e1f4	266	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	267	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	268
nikapov	0:a1a69d32f310	269	signal_rate_meas_fixed1104_400_mm \|= ((tmp_dword &
nikapov	0:a1a69d32f310	270	0xff000000) >> 24);
nikapov	0:a1a69d32f310	271
sepp_nepp	7:41cbc431e1f4	272	status \|= VL53L0X_write_byte(0x94,0x75);
sepp_nepp	7:41cbc431e1f4	273	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	274	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	275
nikapov	0:a1a69d32f310	276	dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)
nikapov	0:a1a69d32f310	277	<< 8;
nikapov	0:a1a69d32f310	278
sepp_nepp	7:41cbc431e1f4	279	status \|= VL53L0X_write_byte(0x94,0x76);
sepp_nepp	7:41cbc431e1f4	280	status \|= VL53L0X_device_read_strobe();
sepp_nepp	7:41cbc431e1f4	281	status \|= VL53L0X_read_dword(0x90,&tmp_dword);
nikapov	0:a1a69d32f310	282
nikapov	0:a1a69d32f310	283	dist_meas_fixed1104_400_mm \|= ((tmp_dword & 0xff000000)
nikapov	0:a1a69d32f310	284	>> 24);
nikapov	0:a1a69d32f310	285	}
nikapov	0:a1a69d32f310	286
sepp_nepp	7:41cbc431e1f4	287	status \|= VL53L0X_write_byte(0x81,0x00);
sepp_nepp	7:41cbc431e1f4	288	status \|= VL53L0X_write_byte(0xFF,0x06);
sepp_nepp	7:41cbc431e1f4	289	status \|= VL53L0X_read_byte(0x83,&byte);
sepp_nepp	7:41cbc431e1f4	290	status \|= VL53L0X_write_byte(0x83,byte & 0xfb);
sepp_nepp	7:41cbc431e1f4	291	status \|= VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	292	status \|= VL53L0X_write_byte(0x00,0x01);
sepp_nepp	7:41cbc431e1f4	293
sepp_nepp	7:41cbc431e1f4	294	status \|= VL53L0X_write_byte(0xFF,0x00);
sepp_nepp	7:41cbc431e1f4	295	status \|= VL53L0X_write_byte(0x80,0x00);
nikapov	0:a1a69d32f310	296	}
nikapov	0:a1a69d32f310	297
nikapov	0:a1a69d32f310	298	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	299	(read_data_from_device_done != 7)) {
nikapov	0:a1a69d32f310	300	/* Assign to variable if status is ok */
nikapov	0:a1a69d32f310	301	if (((option & 1) == 1) &&
nikapov	0:a1a69d32f310	302	((read_data_from_device_done & 1) == 0)) {
sepp_nepp	7:41cbc431e1f4	303	_device->DeviceSpecificParameters.ReferenceSpadCount=reference_spad_count;
sepp_nepp	7:41cbc431e1f4	304	_device->DeviceSpecificParameters.ReferenceSpadType=reference_spad_type;
nikapov	0:a1a69d32f310	305
nikapov	0:a1a69d32f310	306	for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
sepp_nepp	7:41cbc431e1f4	307	_device->SpadData.RefGoodSpadMap[i] =
nikapov	0:a1a69d32f310	308	nvm_ref_good_spad_map[i];
nikapov	0:a1a69d32f310	309	}
nikapov	0:a1a69d32f310	310	}
nikapov	0:a1a69d32f310	311
nikapov	0:a1a69d32f310	312	if (((option & 2) == 2) &&
nikapov	0:a1a69d32f310	313	((read_data_from_device_done & 2) == 0)) {
sepp_nepp	7:41cbc431e1f4	314	_device->DeviceSpecificParameters.ModuleId = module_id;
sepp_nepp	7:41cbc431e1f4	315
sepp_nepp	7:41cbc431e1f4	316	_device->DeviceSpecificParameters.Revision = revision;
sepp_nepp	7:41cbc431e1f4	317
sepp_nepp	7:41cbc431e1f4	318	product_id_tmp = _device->DeviceSpecificParameters.ProductId;
sepp_nepp	7:41cbc431e1f4	319	strcpy(product_id_tmp,product_id);
nikapov	0:a1a69d32f310	320
nikapov	0:a1a69d32f310	321	}
nikapov	0:a1a69d32f310	322
nikapov	0:a1a69d32f310	323	if (((option & 4) == 4) &&
nikapov	0:a1a69d32f310	324	((read_data_from_device_done & 4) == 0)) {
sepp_nepp	7:41cbc431e1f4	325	_device->DeviceSpecificParameters.PartUIDUpper = part_uid_upper;
sepp_nepp	7:41cbc431e1f4	326
sepp_nepp	7:41cbc431e1f4	327	_device->DeviceSpecificParameters.PartUIDLower = part_uid_lower;
nikapov	0:a1a69d32f310	328
nikapov	0:a1a69d32f310	329	signal_rate_meas_fixed400_mm_fix =
sepp_nepp	7:41cbc431e1f4	330	VL53L0X_FIXPOINT97TOFIXPOINT1616(signal_rate_meas_fixed1104_400_mm);
sepp_nepp	7:41cbc431e1f4	331
sepp_nepp	7:41cbc431e1f4	332	_device->DeviceSpecificParameters.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
sepp_nepp	7:41cbc431e1f4	333	_device->DeviceSpecificParameters.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix;
nikapov	0:a1a69d32f310	334
nikapov	0:a1a69d32f310	335	offset_micro_meters = 0;
nikapov	0:a1a69d32f310	336	if (dist_meas_fixed1104_400_mm != 0) {
nikapov	0:a1a69d32f310	337	offset_fixed1104_mm =
nikapov	0:a1a69d32f310	338	dist_meas_fixed1104_400_mm -
nikapov	0:a1a69d32f310	339	dist_meas_tgt_fixed1104_mm;
nikapov	0:a1a69d32f310	340	offset_micro_meters = (offset_fixed1104_mm
nikapov	0:a1a69d32f310	341	* 1000) >> 4;
nikapov	0:a1a69d32f310	342	offset_micro_meters *= -1;
nikapov	0:a1a69d32f310	343	}
nikapov	0:a1a69d32f310	344
sepp_nepp	7:41cbc431e1f4	345	_device->Part2PartOffsetAdjustmentNVMMicroMeter = offset_micro_meters;
nikapov	0:a1a69d32f310	346	}
nikapov	0:a1a69d32f310	347	byte = (uint8_t)(read_data_from_device_done \| option);
sepp_nepp	7:41cbc431e1f4	348	_device->DeviceSpecificParameters.ReadDataFromDeviceDone = byte;
nikapov	0:a1a69d32f310	349	}
nikapov	0:a1a69d32f310	350
sepp_nepp	5:b95f6951f7d5	351
nikapov	0:a1a69d32f310	352	return status;
nikapov	0:a1a69d32f310	353	}
nikapov	0:a1a69d32f310	354
sepp_nepp	7:41cbc431e1f4	355	VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter)
nikapov	0:a1a69d32f310	356	{
nikapov	0:a1a69d32f310	357	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	358	uint16_t range_offset_register;
nikapov	0:a1a69d32f310	359	int16_t c_max_offset = 2047;
nikapov	0:a1a69d32f310	360	int16_t c_offset_range = 4096;
nikapov	0:a1a69d32f310	361
nikapov	0:a1a69d32f310	362	/* Note that offset has 10.2 format */
nikapov	0:a1a69d32f310	363
sepp_nepp	7:41cbc431e1f4	364	status = VL53L0X_read_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
nikapov	0:a1a69d32f310	365	&range_offset_register);
nikapov	0:a1a69d32f310	366
nikapov	0:a1a69d32f310	367	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	368	range_offset_register = (range_offset_register & 0x0fff);
nikapov	0:a1a69d32f310	369
nikapov	0:a1a69d32f310	370	/* Apply 12 bit 2's compliment conversion */
nikapov	0:a1a69d32f310	371	if (range_offset_register > c_max_offset) {
nikapov	0:a1a69d32f310	372	*p_offset_calibration_data_micro_meter =
nikapov	0:a1a69d32f310	373	(int16_t)(range_offset_register - c_offset_range)
nikapov	0:a1a69d32f310	374	* 250;
nikapov	0:a1a69d32f310	375	} else {
nikapov	0:a1a69d32f310	376	*p_offset_calibration_data_micro_meter =
nikapov	0:a1a69d32f310	377	(int16_t)range_offset_register * 250;
nikapov	0:a1a69d32f310	378	}
nikapov	0:a1a69d32f310	379
nikapov	0:a1a69d32f310	380	}
nikapov	0:a1a69d32f310	381
nikapov	0:a1a69d32f310	382	return status;
nikapov	0:a1a69d32f310	383	}
nikapov	0:a1a69d32f310	384
sepp_nepp	7:41cbc431e1f4	385	VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter)
nikapov	0:a1a69d32f310	386	{
nikapov	0:a1a69d32f310	387	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	388
nikapov	0:a1a69d32f310	389
sepp_nepp	7:41cbc431e1f4	390	status = wrapped_VL53L0X_get_offset_calibration_data_micro_meter(p_offset_calibration_data_micro_meter);
nikapov	0:a1a69d32f310	391
sepp_nepp	5:b95f6951f7d5	392
nikapov	0:a1a69d32f310	393	return status;
nikapov	0:a1a69d32f310	394	}
nikapov	0:a1a69d32f310	395
sepp_nepp	7:41cbc431e1f4	396	VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter)
nikapov	0:a1a69d32f310	397	{
nikapov	0:a1a69d32f310	398	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	399	int32_t c_max_offset_micro_meter = 511000;
nikapov	0:a1a69d32f310	400	int32_t c_min_offset_micro_meter = -512000;
nikapov	0:a1a69d32f310	401	int16_t c_offset_range = 4096;
nikapov	0:a1a69d32f310	402	uint32_t encoded_offset_val;
nikapov	0:a1a69d32f310	403
sepp_nepp	5:b95f6951f7d5	404
nikapov	0:a1a69d32f310	405
nikapov	0:a1a69d32f310	406	if (offset_calibration_data_micro_meter > c_max_offset_micro_meter) {
nikapov	0:a1a69d32f310	407	offset_calibration_data_micro_meter = c_max_offset_micro_meter;
nikapov	0:a1a69d32f310	408	} else {
nikapov	0:a1a69d32f310	409	if (offset_calibration_data_micro_meter < c_min_offset_micro_meter) {
Davidroid	2:d07edeaff6f1	410	offset_calibration_data_micro_meter = c_min_offset_micro_meter;
Davidroid	2:d07edeaff6f1	411	}
Davidroid	2:d07edeaff6f1	412	}
nikapov	0:a1a69d32f310	413
nikapov	0:a1a69d32f310	414	/* The offset register is 10.2 format and units are mm
nikapov	0:a1a69d32f310	415	* therefore conversion is applied by a division of
nikapov	0:a1a69d32f310	416	* 250.
nikapov	0:a1a69d32f310	417	*/
nikapov	0:a1a69d32f310	418	if (offset_calibration_data_micro_meter >= 0) {
nikapov	0:a1a69d32f310	419	encoded_offset_val =
nikapov	0:a1a69d32f310	420	offset_calibration_data_micro_meter / 250;
nikapov	0:a1a69d32f310	421	} else {
nikapov	0:a1a69d32f310	422	encoded_offset_val =
nikapov	0:a1a69d32f310	423	c_offset_range +
nikapov	0:a1a69d32f310	424	offset_calibration_data_micro_meter / 250;
nikapov	0:a1a69d32f310	425	}
nikapov	0:a1a69d32f310	426
sepp_nepp	7:41cbc431e1f4	427	status = VL53L0X_write_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
nikapov	0:a1a69d32f310	428	encoded_offset_val);
nikapov	0:a1a69d32f310	429
sepp_nepp	5:b95f6951f7d5	430
nikapov	0:a1a69d32f310	431	return status;
nikapov	0:a1a69d32f310	432	}
nikapov	0:a1a69d32f310	433
sepp_nepp	7:41cbc431e1f4	434	VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter)
nikapov	0:a1a69d32f310	435	{
nikapov	0:a1a69d32f310	436	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	437
nikapov	0:a1a69d32f310	438
sepp_nepp	7:41cbc431e1f4	439	status = wrapped_VL53L0X_set_offset_calibration_data_micro_meter(offset_calibration_data_micro_meter);
nikapov	0:a1a69d32f310	440
sepp_nepp	5:b95f6951f7d5	441
nikapov	0:a1a69d32f310	442	return status;
nikapov	0:a1a69d32f310	443	}
nikapov	0:a1a69d32f310	444
sepp_nepp	7:41cbc431e1f4	445	VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment()
nikapov	0:a1a69d32f310	446	{
nikapov	0:a1a69d32f310	447	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	448	int32_t corrected_offset_micro_meters;
nikapov	0:a1a69d32f310	449	int32_t current_offset_micro_meters;
nikapov	0:a1a69d32f310	450
nikapov	0:a1a69d32f310	451	/* if we run on this function we can read all the NVM info
nikapov	0:a1a69d32f310	452	* used by the API */
sepp_nepp	7:41cbc431e1f4	453	status = VL53L0X_get_info_from_device(7);
nikapov	0:a1a69d32f310	454
nikapov	0:a1a69d32f310	455	/* Read back current device offset */
nikapov	0:a1a69d32f310	456	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	457	status = VL53L0X_get_offset_calibration_data_micro_meter(&current_offset_micro_meters);
nikapov	0:a1a69d32f310	458	}
nikapov	0:a1a69d32f310	459
nikapov	0:a1a69d32f310	460	/* Apply Offset Adjustment derived from 400mm measurements */
nikapov	0:a1a69d32f310	461	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	462
nikapov	0:a1a69d32f310	463	/* Store initial device offset */
sepp_nepp	7:41cbc431e1f4	464	_device->Part2PartOffsetNVMMicroMeter = current_offset_micro_meters;
nikapov	0:a1a69d32f310	465
nikapov	0:a1a69d32f310	466	corrected_offset_micro_meters = current_offset_micro_meters +
sepp_nepp	7:41cbc431e1f4	467	(int32_t) _device->Part2PartOffsetAdjustmentNVMMicroMeter;
sepp_nepp	7:41cbc431e1f4	468
sepp_nepp	7:41cbc431e1f4	469	status = VL53L0X_set_offset_calibration_data_micro_meter(corrected_offset_micro_meters);
sepp_nepp	7:41cbc431e1f4	470
sepp_nepp	7:41cbc431e1f4	471	/* store current,adjusted offset */
nikapov	0:a1a69d32f310	472	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	473	_device->CurrentParameters.RangeOffsetMicroMeters = corrected_offset_micro_meters;
nikapov	0:a1a69d32f310	474	}
nikapov	0:a1a69d32f310	475	}
nikapov	0:a1a69d32f310	476
nikapov	0:a1a69d32f310	477	return status;
nikapov	0:a1a69d32f310	478	}
nikapov	0:a1a69d32f310	479
sepp_nepp	7:41cbc431e1f4	480	VL53L0X_Error VL53L0X::VL53L0X_get_device_mode(VL53L0X_DeviceModes *p_device_mode)
nikapov	0:a1a69d32f310	481	{
nikapov	0:a1a69d32f310	482	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	483
sepp_nepp	7:41cbc431e1f4	484	*p_device_mode = _device->CurrentParameters.DeviceMode;
sepp_nepp	5:b95f6951f7d5	485
nikapov	0:a1a69d32f310	486	return status;
nikapov	0:a1a69d32f310	487	}
nikapov	0:a1a69d32f310	488
sepp_nepp	7:41cbc431e1f4	489	VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_milli_seconds(uint32_t *p_inter_measurement_period_milli_seconds)
nikapov	0:a1a69d32f310	490	{
nikapov	0:a1a69d32f310	491	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	492	uint16_t osc_calibrate_val;
nikapov	0:a1a69d32f310	493	uint32_t im_period_milli_seconds;
nikapov	0:a1a69d32f310	494
sepp_nepp	5:b95f6951f7d5	495
nikapov	0:a1a69d32f310	496
sepp_nepp	7:41cbc431e1f4	497	status = VL53L0X_read_word(VL53L0X_REG_OSC_CALIBRATE_VAL,
nikapov	0:a1a69d32f310	498	&osc_calibrate_val);
nikapov	0:a1a69d32f310	499
nikapov	0:a1a69d32f310	500	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	501	status = VL53L0X_read_dword(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
nikapov	0:a1a69d32f310	502	&im_period_milli_seconds);
nikapov	0:a1a69d32f310	503	}
nikapov	0:a1a69d32f310	504
nikapov	0:a1a69d32f310	505	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	506	if (osc_calibrate_val != 0) {
nikapov	0:a1a69d32f310	507	*p_inter_measurement_period_milli_seconds =
nikapov	0:a1a69d32f310	508	im_period_milli_seconds / osc_calibrate_val;
nikapov	0:a1a69d32f310	509	}
sepp_nepp	7:41cbc431e1f4	510	_device->CurrentParameters.InterMeasurementPeriodMilliSeconds=
sepp_nepp	7:41cbc431e1f4	511	*p_inter_measurement_period_milli_seconds;
nikapov	0:a1a69d32f310	512	}
nikapov	0:a1a69d32f310	513
sepp_nepp	5:b95f6951f7d5	514
nikapov	0:a1a69d32f310	515	return status;
nikapov	0:a1a69d32f310	516	}
nikapov	0:a1a69d32f310	517
sepp_nepp	7:41cbc431e1f4	518	VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_mega_cps(FixPoint1616_t *p_xtalk_compensation_rate_mega_cps)
nikapov	0:a1a69d32f310	519	{
nikapov	0:a1a69d32f310	520	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	521	uint16_t value;
nikapov	0:a1a69d32f310	522	FixPoint1616_t temp_fix1616;
nikapov	0:a1a69d32f310	523
sepp_nepp	5:b95f6951f7d5	524
nikapov	0:a1a69d32f310	525
sepp_nepp	7:41cbc431e1f4	526	status = VL53L0X_read_word(VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS,(uint16_t *)&value);
nikapov	0:a1a69d32f310	527	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	528	if (value == 0) {
nikapov	0:a1a69d32f310	529	/* the Xtalk is disabled return value from memory */
sepp_nepp	7:41cbc431e1f4	530	temp_fix1616 = _device->CurrentParameters.XTalkCompensationRateMegaCps;
nikapov	0:a1a69d32f310	531	*p_xtalk_compensation_rate_mega_cps = temp_fix1616;
sepp_nepp	7:41cbc431e1f4	532	_device->CurrentParameters.XTalkCompensationEnable = 0;
nikapov	0:a1a69d32f310	533	} else {
nikapov	0:a1a69d32f310	534	temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value);
nikapov	0:a1a69d32f310	535	*p_xtalk_compensation_rate_mega_cps = temp_fix1616;
sepp_nepp	7:41cbc431e1f4	536	_device->CurrentParameters.XTalkCompensationRateMegaCps = temp_fix1616;
sepp_nepp	7:41cbc431e1f4	537	_device->CurrentParameters.XTalkCompensationEnable = 1;
nikapov	0:a1a69d32f310	538	}
nikapov	0:a1a69d32f310	539	}
nikapov	0:a1a69d32f310	540
sepp_nepp	5:b95f6951f7d5	541
nikapov	0:a1a69d32f310	542	return status;
nikapov	0:a1a69d32f310	543	}
nikapov	0:a1a69d32f310	544
sepp_nepp	7:41cbc431e1f4	545	VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value(uint16_t limit_check_id,
nikapov	0:a1a69d32f310	546	FixPoint1616_t *p_limit_check_value)
nikapov	0:a1a69d32f310	547	{
nikapov	0:a1a69d32f310	548	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	549	uint8_t enable_zero_value = 0;
nikapov	0:a1a69d32f310	550	uint16_t temp16;
nikapov	0:a1a69d32f310	551	FixPoint1616_t temp_fix1616;
nikapov	0:a1a69d32f310	552
sepp_nepp	5:b95f6951f7d5	553
nikapov	0:a1a69d32f310	554
nikapov	0:a1a69d32f310	555	switch (limit_check_id) {
nikapov	0:a1a69d32f310	556
nikapov	0:a1a69d32f310	557	case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov	0:a1a69d32f310	558	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	559	temp_fix1616 = _device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE];
nikapov	0:a1a69d32f310	560	enable_zero_value = 0;
nikapov	0:a1a69d32f310	561	break;
nikapov	0:a1a69d32f310	562
nikapov	0:a1a69d32f310	563	case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
sepp_nepp	7:41cbc431e1f4	564	status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
nikapov	0:a1a69d32f310	565	&temp16);
Davidroid	3:e9269ff624ed	566	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	567	temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
Davidroid	3:e9269ff624ed	568	}
nikapov	0:a1a69d32f310	569
nikapov	0:a1a69d32f310	570
nikapov	0:a1a69d32f310	571	enable_zero_value = 1;
nikapov	0:a1a69d32f310	572	break;
nikapov	0:a1a69d32f310	573
nikapov	0:a1a69d32f310	574	case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov	0:a1a69d32f310	575	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	576	temp_fix1616 = _device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP];
nikapov	0:a1a69d32f310	577	enable_zero_value = 0;
nikapov	0:a1a69d32f310	578	break;
nikapov	0:a1a69d32f310	579
nikapov	0:a1a69d32f310	580	case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov	0:a1a69d32f310	581	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	582	temp_fix1616 = _device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD];
nikapov	0:a1a69d32f310	583	enable_zero_value = 0;
nikapov	0:a1a69d32f310	584	break;
nikapov	0:a1a69d32f310	585
nikapov	0:a1a69d32f310	586	case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov	0:a1a69d32f310	587	case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
sepp_nepp	7:41cbc431e1f4	588	status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
nikapov	0:a1a69d32f310	589	&temp16);
Davidroid	3:e9269ff624ed	590	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	591	temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
Davidroid	3:e9269ff624ed	592	}
nikapov	0:a1a69d32f310	593
nikapov	0:a1a69d32f310	594
nikapov	0:a1a69d32f310	595	enable_zero_value = 0;
nikapov	0:a1a69d32f310	596	break;
nikapov	0:a1a69d32f310	597
nikapov	0:a1a69d32f310	598	default:
nikapov	0:a1a69d32f310	599	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	600
nikapov	0:a1a69d32f310	601	}
nikapov	0:a1a69d32f310	602
nikapov	0:a1a69d32f310	603	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	604
nikapov	0:a1a69d32f310	605	if (enable_zero_value == 1) {
nikapov	0:a1a69d32f310	606
nikapov	0:a1a69d32f310	607	if (temp_fix1616 == 0) {
nikapov	0:a1a69d32f310	608	/* disabled: return value from memory */
sepp_nepp	7:41cbc431e1f4	609	temp_fix1616 = _device->CurrentParameters.LimitChecksValue[limit_check_id];
nikapov	0:a1a69d32f310	610	*p_limit_check_value = temp_fix1616;
sepp_nepp	7:41cbc431e1f4	611	_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
sepp_nepp	7:41cbc431e1f4	612	_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
nikapov	0:a1a69d32f310	613	} else {
nikapov	0:a1a69d32f310	614	*p_limit_check_value = temp_fix1616;
sepp_nepp	7:41cbc431e1f4	615	_device->CurrentParameters.LimitChecksValue[limit_check_id] = temp_fix1616;
sepp_nepp	7:41cbc431e1f4	616	_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 1;
nikapov	0:a1a69d32f310	617	}
nikapov	0:a1a69d32f310	618	} else {
nikapov	0:a1a69d32f310	619	*p_limit_check_value = temp_fix1616;
nikapov	0:a1a69d32f310	620	}
nikapov	0:a1a69d32f310	621	}
nikapov	0:a1a69d32f310	622
sepp_nepp	5:b95f6951f7d5	623
nikapov	0:a1a69d32f310	624	return status;
nikapov	0:a1a69d32f310	625
nikapov	0:a1a69d32f310	626	}
nikapov	0:a1a69d32f310	627
sepp_nepp	7:41cbc431e1f4	628	VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable(uint16_t limit_check_id,
nikapov	0:a1a69d32f310	629	uint8_t *p_limit_check_enable)
nikapov	0:a1a69d32f310	630	{
nikapov	0:a1a69d32f310	631	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	632	uint8_t temp8;
nikapov	0:a1a69d32f310	633
sepp_nepp	5:b95f6951f7d5	634
nikapov	0:a1a69d32f310	635
nikapov	0:a1a69d32f310	636	if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
nikapov	0:a1a69d32f310	637	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	638	*p_limit_check_enable = 0;
nikapov	0:a1a69d32f310	639	} else {
sepp_nepp	7:41cbc431e1f4	640	temp8 = _device->CurrentParameters.LimitChecksEnable[limit_check_id];
nikapov	0:a1a69d32f310	641	*p_limit_check_enable = temp8;
nikapov	0:a1a69d32f310	642	}
nikapov	0:a1a69d32f310	643
sepp_nepp	5:b95f6951f7d5	644
nikapov	0:a1a69d32f310	645	return status;
nikapov	0:a1a69d32f310	646	}
nikapov	0:a1a69d32f310	647
sepp_nepp	7:41cbc431e1f4	648	VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(uint8_t *p_wrap_around_check_enable)
nikapov	0:a1a69d32f310	649	{
nikapov	0:a1a69d32f310	650	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	651	uint8_t data;
nikapov	0:a1a69d32f310	652
sepp_nepp	5:b95f6951f7d5	653
nikapov	0:a1a69d32f310	654
sepp_nepp	7:41cbc431e1f4	655	status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,&data);
nikapov	0:a1a69d32f310	656	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	657	_device->SequenceConfig = data;
Davidroid	3:e9269ff624ed	658	if (data & (0x01 << 7)) {
nikapov	0:a1a69d32f310	659	*p_wrap_around_check_enable = 0x01;
Davidroid	3:e9269ff624ed	660	} else {
nikapov	0:a1a69d32f310	661	*p_wrap_around_check_enable = 0x00;
Davidroid	3:e9269ff624ed	662	}
nikapov	0:a1a69d32f310	663	}
nikapov	0:a1a69d32f310	664	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	665	_device->CurrentParameters.WrapAroundCheckEnable=
sepp_nepp	7:41cbc431e1f4	666	*p_wrap_around_check_enable;
nikapov	0:a1a69d32f310	667	}
nikapov	0:a1a69d32f310	668
sepp_nepp	5:b95f6951f7d5	669
nikapov	0:a1a69d32f310	670	return status;
nikapov	0:a1a69d32f310	671	}
nikapov	0:a1a69d32f310	672
sepp_nepp	7:41cbc431e1f4	673	VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_SequenceStepId sequence_step_id,uint8_t sequence_config,
nikapov	0:a1a69d32f310	674	uint8_t *p_sequence_step_enabled)
nikapov	0:a1a69d32f310	675	{
nikapov	0:a1a69d32f310	676	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	677	*p_sequence_step_enabled = 0;
sepp_nepp	5:b95f6951f7d5	678
nikapov	0:a1a69d32f310	679
nikapov	0:a1a69d32f310	680	switch (sequence_step_id) {
nikapov	0:a1a69d32f310	681	case VL53L0X_SEQUENCESTEP_TCC:
nikapov	0:a1a69d32f310	682	*p_sequence_step_enabled = (sequence_config & 0x10) >> 4;
nikapov	0:a1a69d32f310	683	break;
nikapov	0:a1a69d32f310	684	case VL53L0X_SEQUENCESTEP_DSS:
nikapov	0:a1a69d32f310	685	*p_sequence_step_enabled = (sequence_config & 0x08) >> 3;
nikapov	0:a1a69d32f310	686	break;
nikapov	0:a1a69d32f310	687	case VL53L0X_SEQUENCESTEP_MSRC:
nikapov	0:a1a69d32f310	688	*p_sequence_step_enabled = (sequence_config & 0x04) >> 2;
nikapov	0:a1a69d32f310	689	break;
nikapov	0:a1a69d32f310	690	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov	0:a1a69d32f310	691	*p_sequence_step_enabled = (sequence_config & 0x40) >> 6;
nikapov	0:a1a69d32f310	692	break;
nikapov	0:a1a69d32f310	693	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov	0:a1a69d32f310	694	*p_sequence_step_enabled = (sequence_config & 0x80) >> 7;
nikapov	0:a1a69d32f310	695	break;
nikapov	0:a1a69d32f310	696	default:
nikapov	0:a1a69d32f310	697	Status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	698	}
nikapov	0:a1a69d32f310	699
sepp_nepp	5:b95f6951f7d5	700
nikapov	0:a1a69d32f310	701	return Status;
nikapov	0:a1a69d32f310	702	}
nikapov	0:a1a69d32f310	703
sepp_nepp	7:41cbc431e1f4	704	VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps)
nikapov	0:a1a69d32f310	705	{
nikapov	0:a1a69d32f310	706	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	707	uint8_t sequence_config = 0;
sepp_nepp	5:b95f6951f7d5	708
nikapov	0:a1a69d32f310	709
sepp_nepp	7:41cbc431e1f4	710	status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	711	&sequence_config);
nikapov	0:a1a69d32f310	712
nikapov	0:a1a69d32f310	713	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	714	status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_TCC,sequence_config,
nikapov	0:a1a69d32f310	715	&p_scheduler_sequence_steps->TccOn);
nikapov	0:a1a69d32f310	716	}
nikapov	0:a1a69d32f310	717	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	718	status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_DSS,sequence_config,
nikapov	0:a1a69d32f310	719	&p_scheduler_sequence_steps->DssOn);
nikapov	0:a1a69d32f310	720	}
nikapov	0:a1a69d32f310	721	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	722	status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_MSRC,sequence_config,
nikapov	0:a1a69d32f310	723	&p_scheduler_sequence_steps->MsrcOn);
nikapov	0:a1a69d32f310	724	}
nikapov	0:a1a69d32f310	725	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	726	status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_PRE_RANGE,sequence_config,
nikapov	0:a1a69d32f310	727	&p_scheduler_sequence_steps->PreRangeOn);
nikapov	0:a1a69d32f310	728	}
nikapov	0:a1a69d32f310	729	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	730	status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_FINAL_RANGE,sequence_config,
nikapov	0:a1a69d32f310	731	&p_scheduler_sequence_steps->FinalRangeOn);
nikapov	0:a1a69d32f310	732	}
nikapov	0:a1a69d32f310	733
sepp_nepp	5:b95f6951f7d5	734
nikapov	0:a1a69d32f310	735	return status;
nikapov	0:a1a69d32f310	736	}
nikapov	0:a1a69d32f310	737
nikapov	0:a1a69d32f310	738	uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
nikapov	0:a1a69d32f310	739	{
nikapov	0:a1a69d32f310	740	/*!
nikapov	0:a1a69d32f310	741	* Converts the encoded VCSEL period register value into the real
nikapov	0:a1a69d32f310	742	* period in PLL clocks
nikapov	0:a1a69d32f310	743	*/
nikapov	0:a1a69d32f310	744
nikapov	0:a1a69d32f310	745	uint8_t vcsel_period_pclks = 0;
nikapov	0:a1a69d32f310	746
nikapov	0:a1a69d32f310	747	vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
nikapov	0:a1a69d32f310	748
nikapov	0:a1a69d32f310	749	return vcsel_period_pclks;
nikapov	0:a1a69d32f310	750	}
nikapov	0:a1a69d32f310	751
nikapov	0:a1a69d32f310	752	uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks)
nikapov	0:a1a69d32f310	753	{
nikapov	0:a1a69d32f310	754	/*!
nikapov	0:a1a69d32f310	755	* Converts the encoded VCSEL period register value into the real period
nikapov	0:a1a69d32f310	756	* in PLL clocks
nikapov	0:a1a69d32f310	757	*/
nikapov	0:a1a69d32f310	758
nikapov	0:a1a69d32f310	759	uint8_t vcsel_period_reg = 0;
nikapov	0:a1a69d32f310	760
nikapov	0:a1a69d32f310	761	vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
nikapov	0:a1a69d32f310	762
nikapov	0:a1a69d32f310	763	return vcsel_period_reg;
nikapov	0:a1a69d32f310	764	}
nikapov	0:a1a69d32f310	765
nikapov	0:a1a69d32f310	766
sepp_nepp	7:41cbc431e1f4	767	VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t vcsel_pulse_period_pclk)
nikapov	0:a1a69d32f310	768	{
nikapov	0:a1a69d32f310	769	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	770	uint8_t vcsel_period_reg;
nikapov	0:a1a69d32f310	771	uint8_t min_pre_vcsel_period_pclk = 12;
nikapov	0:a1a69d32f310	772	uint8_t max_pre_vcsel_period_pclk = 18;
nikapov	0:a1a69d32f310	773	uint8_t min_final_vcsel_period_pclk = 8;
nikapov	0:a1a69d32f310	774	uint8_t max_final_vcsel_period_pclk = 14;
nikapov	0:a1a69d32f310	775	uint32_t measurement_timing_budget_micro_seconds;
nikapov	0:a1a69d32f310	776	uint32_t final_range_timeout_micro_seconds;
nikapov	0:a1a69d32f310	777	uint32_t pre_range_timeout_micro_seconds;
nikapov	0:a1a69d32f310	778	uint32_t msrc_timeout_micro_seconds;
nikapov	0:a1a69d32f310	779	uint8_t phase_cal_int = 0;
nikapov	0:a1a69d32f310	780
nikapov	0:a1a69d32f310	781	/* Check if valid clock period requested */
nikapov	0:a1a69d32f310	782
nikapov	0:a1a69d32f310	783	if ((vcsel_pulse_period_pclk % 2) != 0) {
nikapov	0:a1a69d32f310	784	/* Value must be an even number */
nikapov	0:a1a69d32f310	785	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	786	} else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
nikapov	0:a1a69d32f310	787	(vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk \|\|
nikapov	0:a1a69d32f310	788	vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) {
nikapov	0:a1a69d32f310	789	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	790	} else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
nikapov	0:a1a69d32f310	791	(vcsel_pulse_period_pclk < min_final_vcsel_period_pclk \|\|
nikapov	0:a1a69d32f310	792	vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) {
nikapov	0:a1a69d32f310	793
nikapov	0:a1a69d32f310	794	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	795	}
nikapov	0:a1a69d32f310	796
nikapov	0:a1a69d32f310	797	/* Apply specific settings for the requested clock period */
nikapov	0:a1a69d32f310	798
Davidroid	3:e9269ff624ed	799	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	800	return status;
Davidroid	3:e9269ff624ed	801	}
nikapov	0:a1a69d32f310	802
nikapov	0:a1a69d32f310	803
nikapov	0:a1a69d32f310	804	if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
nikapov	0:a1a69d32f310	805
nikapov	0:a1a69d32f310	806	/* Set phase check limits */
nikapov	0:a1a69d32f310	807	if (vcsel_pulse_period_pclk == 12) {
nikapov	0:a1a69d32f310	808
sepp_nepp	7:41cbc431e1f4	809	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	810	0x18);
sepp_nepp	7:41cbc431e1f4	811	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	812	0x08);
nikapov	0:a1a69d32f310	813	} else if (vcsel_pulse_period_pclk == 14) {
nikapov	0:a1a69d32f310	814
sepp_nepp	7:41cbc431e1f4	815	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	816	0x30);
sepp_nepp	7:41cbc431e1f4	817	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	818	0x08);
nikapov	0:a1a69d32f310	819	} else if (vcsel_pulse_period_pclk == 16) {
nikapov	0:a1a69d32f310	820
sepp_nepp	7:41cbc431e1f4	821	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	822	0x40);
sepp_nepp	7:41cbc431e1f4	823	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	824	0x08);
nikapov	0:a1a69d32f310	825	} else if (vcsel_pulse_period_pclk == 18) {
nikapov	0:a1a69d32f310	826
sepp_nepp	7:41cbc431e1f4	827	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	828	0x50);
sepp_nepp	7:41cbc431e1f4	829	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	830	0x08);
nikapov	0:a1a69d32f310	831	}
nikapov	0:a1a69d32f310	832	} else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
nikapov	0:a1a69d32f310	833
nikapov	0:a1a69d32f310	834	if (vcsel_pulse_period_pclk == 8) {
nikapov	0:a1a69d32f310	835
sepp_nepp	7:41cbc431e1f4	836	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	837	0x10);
sepp_nepp	7:41cbc431e1f4	838	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	839	0x08);
nikapov	0:a1a69d32f310	840
sepp_nepp	7:41cbc431e1f4	841	status \|= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x02);
sepp_nepp	7:41cbc431e1f4	842	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C);
sepp_nepp	7:41cbc431e1f4	843
sepp_nepp	7:41cbc431e1f4	844	status \|= VL53L0X_write_byte(0xff,0x01);
sepp_nepp	7:41cbc431e1f4	845	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov	0:a1a69d32f310	846	0x30);
sepp_nepp	7:41cbc431e1f4	847	status \|= VL53L0X_write_byte(0xff,0x00);
nikapov	0:a1a69d32f310	848	} else if (vcsel_pulse_period_pclk == 10) {
nikapov	0:a1a69d32f310	849
sepp_nepp	7:41cbc431e1f4	850	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	851	0x28);
sepp_nepp	7:41cbc431e1f4	852	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	853	0x08);
nikapov	0:a1a69d32f310	854
sepp_nepp	7:41cbc431e1f4	855	status \|= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp	7:41cbc431e1f4	856	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09);
sepp_nepp	7:41cbc431e1f4	857
sepp_nepp	7:41cbc431e1f4	858	status \|= VL53L0X_write_byte(0xff,0x01);
sepp_nepp	7:41cbc431e1f4	859	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov	0:a1a69d32f310	860	0x20);
sepp_nepp	7:41cbc431e1f4	861	status \|= VL53L0X_write_byte(0xff,0x00);
nikapov	0:a1a69d32f310	862	} else if (vcsel_pulse_period_pclk == 12) {
nikapov	0:a1a69d32f310	863
sepp_nepp	7:41cbc431e1f4	864	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	865	0x38);
sepp_nepp	7:41cbc431e1f4	866	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	867	0x08);
nikapov	0:a1a69d32f310	868
sepp_nepp	7:41cbc431e1f4	869	status \|= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp	7:41cbc431e1f4	870	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08);
sepp_nepp	7:41cbc431e1f4	871
sepp_nepp	7:41cbc431e1f4	872	status \|= VL53L0X_write_byte(0xff,0x01);
sepp_nepp	7:41cbc431e1f4	873	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov	0:a1a69d32f310	874	0x20);
sepp_nepp	7:41cbc431e1f4	875	status \|= VL53L0X_write_byte(0xff,0x00);
nikapov	0:a1a69d32f310	876	} else if (vcsel_pulse_period_pclk == 14) {
nikapov	0:a1a69d32f310	877
sepp_nepp	7:41cbc431e1f4	878	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
nikapov	0:a1a69d32f310	879	0x048);
sepp_nepp	7:41cbc431e1f4	880	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
nikapov	0:a1a69d32f310	881	0x08);
nikapov	0:a1a69d32f310	882
sepp_nepp	7:41cbc431e1f4	883	status \|= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp	7:41cbc431e1f4	884	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07);
sepp_nepp	7:41cbc431e1f4	885
sepp_nepp	7:41cbc431e1f4	886	status \|= VL53L0X_write_byte(0xff,0x01);
sepp_nepp	7:41cbc431e1f4	887	status \|= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,
nikapov	0:a1a69d32f310	888	0x20);
sepp_nepp	7:41cbc431e1f4	889	status \|= VL53L0X_write_byte(0xff,0x00);
nikapov	0:a1a69d32f310	890	}
nikapov	0:a1a69d32f310	891	}
nikapov	0:a1a69d32f310	892
nikapov	0:a1a69d32f310	893
sepp_nepp	7:41cbc431e1f4	894	/* Re-calculate and apply timeouts,in macro periods */
nikapov	0:a1a69d32f310	895
nikapov	0:a1a69d32f310	896	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	897	vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t)
nikapov	0:a1a69d32f310	898	vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	899
nikapov	0:a1a69d32f310	900	/* When the VCSEL period for the pre or final range is changed,
nikapov	0:a1a69d32f310	901	* the corresponding timeout must be read from the device using
sepp_nepp	7:41cbc431e1f4	902	* the current VCSEL period,then the new VCSEL period can be
nikapov	0:a1a69d32f310	903	* applied. The timeout then must be written back to the device
nikapov	0:a1a69d32f310	904	* using the new VCSEL period.
nikapov	0:a1a69d32f310	905	*
sepp_nepp	7:41cbc431e1f4	906	* For the MSRC timeout,the same applies - this timeout being
nikapov	0:a1a69d32f310	907	* dependant on the pre-range vcsel period.
nikapov	0:a1a69d32f310	908	*/
nikapov	0:a1a69d32f310	909	switch (vcsel_period_type) {
nikapov	0:a1a69d32f310	910	case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
sepp_nepp	7:41cbc431e1f4	911	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	912	&pre_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	913
nikapov	0:a1a69d32f310	914	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	915	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
nikapov	0:a1a69d32f310	916	&msrc_timeout_micro_seconds);
nikapov	0:a1a69d32f310	917
nikapov	0:a1a69d32f310	918	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	919	status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
nikapov	0:a1a69d32f310	920	vcsel_period_reg);
nikapov	0:a1a69d32f310	921
nikapov	0:a1a69d32f310	922
nikapov	0:a1a69d32f310	923	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	924	status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	925	pre_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	926
nikapov	0:a1a69d32f310	927
nikapov	0:a1a69d32f310	928	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	929	status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
nikapov	0:a1a69d32f310	930	msrc_timeout_micro_seconds);
nikapov	0:a1a69d32f310	931
sepp_nepp	7:41cbc431e1f4	932	_device->DeviceSpecificParameters.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
nikapov	0:a1a69d32f310	933	break;
nikapov	0:a1a69d32f310	934	case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
sepp_nepp	7:41cbc431e1f4	935	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	936	&final_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	937
nikapov	0:a1a69d32f310	938	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	939	status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
nikapov	0:a1a69d32f310	940	vcsel_period_reg);
nikapov	0:a1a69d32f310	941
nikapov	0:a1a69d32f310	942
nikapov	0:a1a69d32f310	943	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	944	status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	945	final_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	946
sepp_nepp	7:41cbc431e1f4	947	_device->DeviceSpecificParameters.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
nikapov	0:a1a69d32f310	948	break;
nikapov	0:a1a69d32f310	949	default:
nikapov	0:a1a69d32f310	950	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	951	}
nikapov	0:a1a69d32f310	952	}
nikapov	0:a1a69d32f310	953
sepp_nepp	7:41cbc431e1f4	954	/* Finally,the timing budget must be re-applied */
nikapov	0:a1a69d32f310	955	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	956	measurement_timing_budget_micro_seconds = _device->CurrentParameters.MeasurementTimingBudgetMicroSeconds;
sepp_nepp	7:41cbc431e1f4	957
sepp_nepp	7:41cbc431e1f4	958	status = VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	959	}
nikapov	0:a1a69d32f310	960
nikapov	0:a1a69d32f310	961	/* Perform the phase calibration. This is needed after changing on
nikapov	0:a1a69d32f310	962	* vcsel period.
sepp_nepp	7:41cbc431e1f4	963	* get_data_enable = 0,restore_config = 1 */
nikapov	0:a1a69d32f310	964	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	965	status = VL53L0X_perform_phase_calibration(&phase_cal_int,0,1);
nikapov	0:a1a69d32f310	966
nikapov	0:a1a69d32f310	967	return status;
nikapov	0:a1a69d32f310	968	}
nikapov	0:a1a69d32f310	969
sepp_nepp	7:41cbc431e1f4	970	VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t vcsel_pulse_period)
nikapov	0:a1a69d32f310	971	{
nikapov	0:a1a69d32f310	972	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	973
nikapov	0:a1a69d32f310	974
sepp_nepp	7:41cbc431e1f4	975	status = wrapped_VL53L0X_set_vcsel_pulse_period(vcsel_period_type,
nikapov	0:a1a69d32f310	976	vcsel_pulse_period);
nikapov	0:a1a69d32f310	977
sepp_nepp	5:b95f6951f7d5	978
nikapov	0:a1a69d32f310	979	return status;
nikapov	0:a1a69d32f310	980	}
nikapov	0:a1a69d32f310	981
sepp_nepp	7:41cbc431e1f4	982	VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t *p_vcsel_pulse_period_pclk)
nikapov	0:a1a69d32f310	983	{
nikapov	0:a1a69d32f310	984	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	985	uint8_t vcsel_period_reg;
nikapov	0:a1a69d32f310	986
nikapov	0:a1a69d32f310	987	switch (vcsel_period_type) {
nikapov	0:a1a69d32f310	988	case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
sepp_nepp	7:41cbc431e1f4	989	status = VL53L0X_read_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
nikapov	0:a1a69d32f310	990	&vcsel_period_reg);
nikapov	0:a1a69d32f310	991	break;
nikapov	0:a1a69d32f310	992	case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
sepp_nepp	7:41cbc431e1f4	993	status = VL53L0X_read_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
nikapov	0:a1a69d32f310	994	&vcsel_period_reg);
nikapov	0:a1a69d32f310	995	break;
nikapov	0:a1a69d32f310	996	default:
nikapov	0:a1a69d32f310	997	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	998	}
nikapov	0:a1a69d32f310	999
nikapov	0:a1a69d32f310	1000	if (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	1001	*p_vcsel_pulse_period_pclk =
nikapov	0:a1a69d32f310	1002	VL53L0X_decode_vcsel_period(vcsel_period_reg);
nikapov	0:a1a69d32f310	1003
nikapov	0:a1a69d32f310	1004	return status;
nikapov	0:a1a69d32f310	1005	}
nikapov	0:a1a69d32f310	1006
sepp_nepp	7:41cbc431e1f4	1007	VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type,uint8_t *p_vcsel_pulse_period_pclk)
nikapov	0:a1a69d32f310	1008	{
nikapov	0:a1a69d32f310	1009	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	1010
nikapov	0:a1a69d32f310	1011
sepp_nepp	7:41cbc431e1f4	1012	status = wrapped_VL53L0X_get_vcsel_pulse_period(vcsel_period_type,
nikapov	0:a1a69d32f310	1013	p_vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	1014
sepp_nepp	5:b95f6951f7d5	1015
nikapov	0:a1a69d32f310	1016	return status;
nikapov	0:a1a69d32f310	1017	}
nikapov	0:a1a69d32f310	1018
nikapov	0:a1a69d32f310	1019	uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout)
nikapov	0:a1a69d32f310	1020	{
nikapov	0:a1a69d32f310	1021	/*!
nikapov	0:a1a69d32f310	1022	* Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
nikapov	0:a1a69d32f310	1023	*/
nikapov	0:a1a69d32f310	1024
nikapov	0:a1a69d32f310	1025	uint32_t timeout_macro_clks = 0;
nikapov	0:a1a69d32f310	1026
nikapov	0:a1a69d32f310	1027	timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF)
nikapov	0:a1a69d32f310	1028	<< (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
nikapov	0:a1a69d32f310	1029
nikapov	0:a1a69d32f310	1030	return timeout_macro_clks;
nikapov	0:a1a69d32f310	1031	}
nikapov	0:a1a69d32f310	1032
sepp_nepp	7:41cbc431e1f4	1033	uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(uint8_t vcsel_period_pclks)
nikapov	0:a1a69d32f310	1034	{
nikapov	0:a1a69d32f310	1035	uint64_t pll_period_ps;
nikapov	0:a1a69d32f310	1036	uint32_t macro_period_vclks;
nikapov	0:a1a69d32f310	1037	uint32_t macro_period_ps;
nikapov	0:a1a69d32f310	1038
sepp_nepp	5:b95f6951f7d5	1039
nikapov	0:a1a69d32f310	1040
nikapov	0:a1a69d32f310	1041	/* The above calculation will produce rounding errors,
nikapov	0:a1a69d32f310	1042	therefore set fixed value
nikapov	0:a1a69d32f310	1043	*/
nikapov	0:a1a69d32f310	1044	pll_period_ps = 1655;
nikapov	0:a1a69d32f310	1045
nikapov	0:a1a69d32f310	1046	macro_period_vclks = 2304;
nikapov	0:a1a69d32f310	1047	macro_period_ps = (uint32_t)(macro_period_vclks
nikapov	0:a1a69d32f310	1048	* vcsel_period_pclks * pll_period_ps);
nikapov	0:a1a69d32f310	1049	return macro_period_ps;
nikapov	0:a1a69d32f310	1050	}
nikapov	0:a1a69d32f310	1051
nikapov	0:a1a69d32f310	1052	/* To convert register value into us */
sepp_nepp	7:41cbc431e1f4	1053	uint32_t VL53L0X::VL53L0X_calc_timeout_us(uint16_t timeout_period_mclks,
nikapov	0:a1a69d32f310	1054	uint8_t vcsel_period_pclks)
nikapov	0:a1a69d32f310	1055	{
nikapov	0:a1a69d32f310	1056	uint32_t macro_period_ps;
nikapov	0:a1a69d32f310	1057	uint32_t macro_period_ns;
nikapov	0:a1a69d32f310	1058	uint32_t actual_timeout_period_us = 0;
nikapov	0:a1a69d32f310	1059
sepp_nepp	7:41cbc431e1f4	1060	macro_period_ps = VL53L0X_calc_macro_period_ps(vcsel_period_pclks);
nikapov	0:a1a69d32f310	1061	macro_period_ns = (macro_period_ps + 500) / 1000;
nikapov	0:a1a69d32f310	1062
nikapov	0:a1a69d32f310	1063	actual_timeout_period_us =
nikapov	0:a1a69d32f310	1064	((timeout_period_mclks * macro_period_ns) + 500) / 1000;
nikapov	0:a1a69d32f310	1065
nikapov	0:a1a69d32f310	1066	return actual_timeout_period_us;
nikapov	0:a1a69d32f310	1067	}
nikapov	0:a1a69d32f310	1068
sepp_nepp	7:41cbc431e1f4	1069	VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id,
nikapov	0:a1a69d32f310	1070	uint32_t *p_time_out_micro_secs)
nikapov	0:a1a69d32f310	1071	{
nikapov	0:a1a69d32f310	1072	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1073	uint8_t current_vcsel_pulse_period_p_clk;
nikapov	0:a1a69d32f310	1074	uint8_t encoded_time_out_byte = 0;
nikapov	0:a1a69d32f310	1075	uint32_t timeout_micro_seconds = 0;
nikapov	0:a1a69d32f310	1076	uint16_t pre_range_encoded_time_out = 0;
nikapov	0:a1a69d32f310	1077	uint16_t msrc_time_out_m_clks;
nikapov	0:a1a69d32f310	1078	uint16_t pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	1079	uint16_t final_range_time_out_m_clks = 0;
nikapov	0:a1a69d32f310	1080	uint16_t final_range_encoded_time_out;
nikapov	0:a1a69d32f310	1081	VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov	0:a1a69d32f310	1082
nikapov	0:a1a69d32f310	1083	if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) \|\|
nikapov	0:a1a69d32f310	1084	(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) \|\|
nikapov	0:a1a69d32f310	1085	(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
nikapov	0:a1a69d32f310	1086
sepp_nepp	7:41cbc431e1f4	1087	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	1088	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1089	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1090	status = VL53L0X_read_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
nikapov	0:a1a69d32f310	1091	&encoded_time_out_byte);
nikapov	0:a1a69d32f310	1092	}
nikapov	0:a1a69d32f310	1093	msrc_time_out_m_clks = VL53L0X_decode_timeout(encoded_time_out_byte);
nikapov	0:a1a69d32f310	1094
sepp_nepp	7:41cbc431e1f4	1095	timeout_micro_seconds = VL53L0X_calc_timeout_us(msrc_time_out_m_clks,
nikapov	0:a1a69d32f310	1096	current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1097	} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
nikapov	0:a1a69d32f310	1098	/* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp	7:41cbc431e1f4	1099	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	1100	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1101
nikapov	0:a1a69d32f310	1102	/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
nikapov	0:a1a69d32f310	1103	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1104
nikapov	0:a1a69d32f310	1105	/* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp	7:41cbc431e1f4	1106	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	1107	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1108
nikapov	0:a1a69d32f310	1109	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1110	status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov	0:a1a69d32f310	1111	&pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	1112	}
nikapov	0:a1a69d32f310	1113
sepp_nepp	7:41cbc431e1f4	1114	pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out);
sepp_nepp	7:41cbc431e1f4	1115
sepp_nepp	7:41cbc431e1f4	1116	timeout_micro_seconds = VL53L0X_calc_timeout_us(pre_range_time_out_m_clks,
nikapov	0:a1a69d32f310	1117	current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1118	}
nikapov	0:a1a69d32f310	1119	} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov	0:a1a69d32f310	1120
sepp_nepp	7:41cbc431e1f4	1121	VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
nikapov	0:a1a69d32f310	1122	pre_range_time_out_m_clks = 0;
nikapov	0:a1a69d32f310	1123
nikapov	0:a1a69d32f310	1124	if (scheduler_sequence_steps.PreRangeOn) {
nikapov	0:a1a69d32f310	1125	/* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp	7:41cbc431e1f4	1126	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	1127	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1128
nikapov	0:a1a69d32f310	1129	/* Retrieve PRE-RANGE Timeout in Macro periods
nikapov	0:a1a69d32f310	1130	* (MCLKS) */
nikapov	0:a1a69d32f310	1131	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1132	status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov	0:a1a69d32f310	1133	&pre_range_encoded_time_out);
sepp_nepp	7:41cbc431e1f4	1134	pre_range_time_out_m_clks = VL53L0X_decode_timeout(pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	1135	}
nikapov	0:a1a69d32f310	1136	}
nikapov	0:a1a69d32f310	1137
nikapov	0:a1a69d32f310	1138	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1139	/* Retrieve FINAL-RANGE VCSEL Period */
sepp_nepp	7:41cbc431e1f4	1140	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov	0:a1a69d32f310	1141	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1142	}
nikapov	0:a1a69d32f310	1143
nikapov	0:a1a69d32f310	1144	/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
nikapov	0:a1a69d32f310	1145	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1146	status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov	0:a1a69d32f310	1147	&final_range_encoded_time_out);
sepp_nepp	7:41cbc431e1f4	1148	final_range_time_out_m_clks = VL53L0X_decode_timeout(final_range_encoded_time_out);
nikapov	0:a1a69d32f310	1149	}
nikapov	0:a1a69d32f310	1150
nikapov	0:a1a69d32f310	1151	final_range_time_out_m_clks -= pre_range_time_out_m_clks;
sepp_nepp	7:41cbc431e1f4	1152	timeout_micro_seconds = VL53L0X_calc_timeout_us(final_range_time_out_m_clks,
nikapov	0:a1a69d32f310	1153	current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	1154	}
nikapov	0:a1a69d32f310	1155
nikapov	0:a1a69d32f310	1156	*p_time_out_micro_secs = timeout_micro_seconds;
nikapov	0:a1a69d32f310	1157
nikapov	0:a1a69d32f310	1158	return status;
nikapov	0:a1a69d32f310	1159	}
nikapov	0:a1a69d32f310	1160
sepp_nepp	7:41cbc431e1f4	1161	VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(uint32_t *p_measurement_timing_budget_micro_seconds)
nikapov	0:a1a69d32f310	1162	{
nikapov	0:a1a69d32f310	1163	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1164	VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov	0:a1a69d32f310	1165	uint32_t final_range_timeout_micro_seconds;
nikapov	0:a1a69d32f310	1166	uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
nikapov	0:a1a69d32f310	1167	uint32_t start_overhead_micro_seconds = 1910;
nikapov	0:a1a69d32f310	1168	uint32_t end_overhead_micro_seconds = 960;
nikapov	0:a1a69d32f310	1169	uint32_t msrc_overhead_micro_seconds = 660;
nikapov	0:a1a69d32f310	1170	uint32_t tcc_overhead_micro_seconds = 590;
nikapov	0:a1a69d32f310	1171	uint32_t dss_overhead_micro_seconds = 690;
nikapov	0:a1a69d32f310	1172	uint32_t pre_range_overhead_micro_seconds = 660;
nikapov	0:a1a69d32f310	1173	uint32_t final_range_overhead_micro_seconds = 550;
nikapov	0:a1a69d32f310	1174	uint32_t pre_range_timeout_micro_seconds = 0;
nikapov	0:a1a69d32f310	1175
sepp_nepp	5:b95f6951f7d5	1176
nikapov	0:a1a69d32f310	1177
nikapov	0:a1a69d32f310	1178	/* Start and end overhead times always present */
nikapov	0:a1a69d32f310	1179	*p_measurement_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	1180	= start_overhead_micro_seconds + end_overhead_micro_seconds;
nikapov	0:a1a69d32f310	1181
sepp_nepp	7:41cbc431e1f4	1182	status = VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
nikapov	0:a1a69d32f310	1183
nikapov	0:a1a69d32f310	1184	if (status != VL53L0X_ERROR_NONE) {
sepp_nepp	5:b95f6951f7d5	1185
nikapov	0:a1a69d32f310	1186	return status;
nikapov	0:a1a69d32f310	1187	}
nikapov	0:a1a69d32f310	1188
nikapov	0:a1a69d32f310	1189
nikapov	0:a1a69d32f310	1190	if (scheduler_sequence_steps.TccOn \|\|
nikapov	0:a1a69d32f310	1191	scheduler_sequence_steps.MsrcOn \|\|
nikapov	0:a1a69d32f310	1192	scheduler_sequence_steps.DssOn) {
nikapov	0:a1a69d32f310	1193
sepp_nepp	7:41cbc431e1f4	1194	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
nikapov	0:a1a69d32f310	1195	&msrc_dcc_tcc_timeout_micro_seconds);
nikapov	0:a1a69d32f310	1196
nikapov	0:a1a69d32f310	1197	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1198	if (scheduler_sequence_steps.TccOn) {
nikapov	0:a1a69d32f310	1199	*p_measurement_timing_budget_micro_seconds +=
nikapov	0:a1a69d32f310	1200	msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	1201	tcc_overhead_micro_seconds;
nikapov	0:a1a69d32f310	1202	}
nikapov	0:a1a69d32f310	1203
nikapov	0:a1a69d32f310	1204	if (scheduler_sequence_steps.DssOn) {
nikapov	0:a1a69d32f310	1205	*p_measurement_timing_budget_micro_seconds +=
nikapov	0:a1a69d32f310	1206	2 * (msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	1207	dss_overhead_micro_seconds);
nikapov	0:a1a69d32f310	1208	} else if (scheduler_sequence_steps.MsrcOn) {
nikapov	0:a1a69d32f310	1209	*p_measurement_timing_budget_micro_seconds +=
nikapov	0:a1a69d32f310	1210	msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	1211	msrc_overhead_micro_seconds;
nikapov	0:a1a69d32f310	1212	}
nikapov	0:a1a69d32f310	1213	}
nikapov	0:a1a69d32f310	1214	}
nikapov	0:a1a69d32f310	1215
nikapov	0:a1a69d32f310	1216	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1217	if (scheduler_sequence_steps.PreRangeOn) {
sepp_nepp	7:41cbc431e1f4	1218	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	1219	&pre_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	1220	*p_measurement_timing_budget_micro_seconds +=
nikapov	0:a1a69d32f310	1221	pre_range_timeout_micro_seconds +
nikapov	0:a1a69d32f310	1222	pre_range_overhead_micro_seconds;
nikapov	0:a1a69d32f310	1223	}
nikapov	0:a1a69d32f310	1224	}
nikapov	0:a1a69d32f310	1225
nikapov	0:a1a69d32f310	1226	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1227	if (scheduler_sequence_steps.FinalRangeOn) {
sepp_nepp	7:41cbc431e1f4	1228	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	1229	&final_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	1230	*p_measurement_timing_budget_micro_seconds +=
nikapov	0:a1a69d32f310	1231	(final_range_timeout_micro_seconds +
nikapov	0:a1a69d32f310	1232	final_range_overhead_micro_seconds);
nikapov	0:a1a69d32f310	1233	}
nikapov	0:a1a69d32f310	1234	}
nikapov	0:a1a69d32f310	1235
nikapov	0:a1a69d32f310	1236	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1237	_device->CurrentParameters.MeasurementTimingBudgetMicroSeconds =
sepp_nepp	7:41cbc431e1f4	1238	*p_measurement_timing_budget_micro_seconds;
nikapov	0:a1a69d32f310	1239	}
nikapov	0:a1a69d32f310	1240
sepp_nepp	5:b95f6951f7d5	1241
nikapov	0:a1a69d32f310	1242	return status;
nikapov	0:a1a69d32f310	1243	}
nikapov	0:a1a69d32f310	1244
sepp_nepp	7:41cbc431e1f4	1245	VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(uint32_t *p_measurement_timing_budget_micro_seconds)
nikapov	0:a1a69d32f310	1246	{
nikapov	0:a1a69d32f310	1247	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	1248
nikapov	0:a1a69d32f310	1249
sepp_nepp	7:41cbc431e1f4	1250	status = wrapped_VL53L0X_get_measurement_timing_budget_micro_seconds(p_measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	1251
sepp_nepp	5:b95f6951f7d5	1252
nikapov	0:a1a69d32f310	1253	return status;
nikapov	0:a1a69d32f310	1254	}
nikapov	0:a1a69d32f310	1255
sepp_nepp	7:41cbc431e1f4	1256	VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters(VL53L0X_DeviceParameters_t *p_device_parameters)
nikapov	0:a1a69d32f310	1257	{
nikapov	0:a1a69d32f310	1258	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1259	int i;
nikapov	0:a1a69d32f310	1260
sepp_nepp	5:b95f6951f7d5	1261
nikapov	0:a1a69d32f310	1262
sepp_nepp	7:41cbc431e1f4	1263	status = VL53L0X_get_device_mode(&(p_device_parameters->DeviceMode));
nikapov	0:a1a69d32f310	1264
nikapov	0:a1a69d32f310	1265	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1266	status = VL53L0X_get_inter_measurement_period_milli_seconds(&(p_device_parameters->InterMeasurementPeriodMilliSeconds));
nikapov	0:a1a69d32f310	1267
nikapov	0:a1a69d32f310	1268
Davidroid	3:e9269ff624ed	1269	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1270	p_device_parameters->XTalkCompensationEnable = 0;
Davidroid	3:e9269ff624ed	1271	}
nikapov	0:a1a69d32f310	1272
nikapov	0:a1a69d32f310	1273	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1274	status = VL53L0X_get_x_talk_compensation_rate_mega_cps(&(p_device_parameters->XTalkCompensationRateMegaCps));
nikapov	0:a1a69d32f310	1275
nikapov	0:a1a69d32f310	1276
nikapov	0:a1a69d32f310	1277	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1278	status = VL53L0X_get_offset_calibration_data_micro_meter(&(p_device_parameters->RangeOffsetMicroMeters));
nikapov	0:a1a69d32f310	1279
nikapov	0:a1a69d32f310	1280
nikapov	0:a1a69d32f310	1281	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1282	for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
sepp_nepp	7:41cbc431e1f4	1283	/* get first the values,then the enables.
nikapov	0:a1a69d32f310	1284	* VL53L0X_GetLimitCheckValue will modify the enable
nikapov	0:a1a69d32f310	1285	* flags
nikapov	0:a1a69d32f310	1286	*/
nikapov	0:a1a69d32f310	1287	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1288	status \|= VL53L0X_get_limit_check_value(i,
nikapov	0:a1a69d32f310	1289	&(p_device_parameters->LimitChecksValue[i]));
nikapov	0:a1a69d32f310	1290	} else {
nikapov	0:a1a69d32f310	1291	break;
nikapov	0:a1a69d32f310	1292	}
nikapov	0:a1a69d32f310	1293	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1294	status \|= VL53L0X_get_limit_check_enable(i,
nikapov	0:a1a69d32f310	1295	&(p_device_parameters->LimitChecksEnable[i]));
nikapov	0:a1a69d32f310	1296	} else {
nikapov	0:a1a69d32f310	1297	break;
nikapov	0:a1a69d32f310	1298	}
nikapov	0:a1a69d32f310	1299	}
nikapov	0:a1a69d32f310	1300	}
nikapov	0:a1a69d32f310	1301
nikapov	0:a1a69d32f310	1302	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1303	status = VL53L0X_get_wrap_around_check_enable(&(p_device_parameters->WrapAroundCheckEnable));
nikapov	0:a1a69d32f310	1304	}
nikapov	0:a1a69d32f310	1305
nikapov	0:a1a69d32f310	1306	/* Need to be done at the end as it uses VCSELPulsePeriod */
nikapov	0:a1a69d32f310	1307	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1308	status = VL53L0X_get_measurement_timing_budget_micro_seconds(&(p_device_parameters->MeasurementTimingBudgetMicroSeconds));
nikapov	0:a1a69d32f310	1309	}
nikapov	0:a1a69d32f310	1310
sepp_nepp	5:b95f6951f7d5	1311
nikapov	0:a1a69d32f310	1312	return status;
nikapov	0:a1a69d32f310	1313	}
nikapov	0:a1a69d32f310	1314
sepp_nepp	7:41cbc431e1f4	1315	VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value(uint16_t limit_check_id,
nikapov	0:a1a69d32f310	1316	FixPoint1616_t limit_check_value)
nikapov	0:a1a69d32f310	1317	{
nikapov	0:a1a69d32f310	1318	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1319	uint8_t temp8;
nikapov	0:a1a69d32f310	1320
sepp_nepp	5:b95f6951f7d5	1321
nikapov	0:a1a69d32f310	1322
sepp_nepp	7:41cbc431e1f4	1323	temp8 = _device->CurrentParameters.LimitChecksEnable[limit_check_id];
nikapov	0:a1a69d32f310	1324
nikapov	0:a1a69d32f310	1325	if (temp8 == 0) { /* disabled write only internal value */
sepp_nepp	7:41cbc431e1f4	1326	_device->CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value;
nikapov	0:a1a69d32f310	1327	} else {
nikapov	0:a1a69d32f310	1328
nikapov	0:a1a69d32f310	1329	switch (limit_check_id) {
nikapov	0:a1a69d32f310	1330
nikapov	0:a1a69d32f310	1331	case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov	0:a1a69d32f310	1332	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	1333	_device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_value;
nikapov	0:a1a69d32f310	1334	break;
nikapov	0:a1a69d32f310	1335
nikapov	0:a1a69d32f310	1336	case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
nikapov	0:a1a69d32f310	1337
sepp_nepp	7:41cbc431e1f4	1338	status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp	7:41cbc431e1f4	1339	VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value));
nikapov	0:a1a69d32f310	1340
nikapov	0:a1a69d32f310	1341	break;
nikapov	0:a1a69d32f310	1342
nikapov	0:a1a69d32f310	1343	case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov	0:a1a69d32f310	1344
nikapov	0:a1a69d32f310	1345	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	1346	_device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_value;
nikapov	0:a1a69d32f310	1347
nikapov	0:a1a69d32f310	1348	break;
nikapov	0:a1a69d32f310	1349
nikapov	0:a1a69d32f310	1350	case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov	0:a1a69d32f310	1351
nikapov	0:a1a69d32f310	1352	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	1353	_device->CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_value;
nikapov	0:a1a69d32f310	1354
nikapov	0:a1a69d32f310	1355	break;
nikapov	0:a1a69d32f310	1356
nikapov	0:a1a69d32f310	1357	case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov	0:a1a69d32f310	1358	case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
nikapov	0:a1a69d32f310	1359
sepp_nepp	7:41cbc431e1f4	1360	status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp	7:41cbc431e1f4	1361	VL53L0X_FIXPOINT1616TOFIXPOINT97(limit_check_value));
nikapov	0:a1a69d32f310	1362
nikapov	0:a1a69d32f310	1363	break;
nikapov	0:a1a69d32f310	1364
nikapov	0:a1a69d32f310	1365	default:
nikapov	0:a1a69d32f310	1366	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	1367
nikapov	0:a1a69d32f310	1368	}
nikapov	0:a1a69d32f310	1369
nikapov	0:a1a69d32f310	1370	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1371	_device->CurrentParameters.LimitChecksValue[limit_check_id] = limit_check_value;
nikapov	0:a1a69d32f310	1372	}
nikapov	0:a1a69d32f310	1373	}
nikapov	0:a1a69d32f310	1374
sepp_nepp	5:b95f6951f7d5	1375
nikapov	0:a1a69d32f310	1376	return status;
nikapov	0:a1a69d32f310	1377	}
nikapov	0:a1a69d32f310	1378
sepp_nepp	7:41cbc431e1f4	1379	VL53L0X_Error VL53L0X::VL53L0X_data_init()
nikapov	0:a1a69d32f310	1380	{
nikapov	0:a1a69d32f310	1381	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1382	VL53L0X_DeviceParameters_t CurrentParameters;
nikapov	0:a1a69d32f310	1383	int i;
nikapov	0:a1a69d32f310	1384	uint8_t StopVariable;
nikapov	0:a1a69d32f310	1385
sepp_nepp	5:b95f6951f7d5	1386
nikapov	0:a1a69d32f310	1387
nikapov	0:a1a69d32f310	1388	/* by default the I2C is running at 1V8 if you want to change it you
nikapov	0:a1a69d32f310	1389	* need to include this define at compilation level. */
nikapov	0:a1a69d32f310	1390	#ifdef USE_I2C_2V8
sepp_nepp	7:41cbc431e1f4	1391	Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
nikapov	0:a1a69d32f310	1392	0xFE,
nikapov	0:a1a69d32f310	1393	0x01);
nikapov	0:a1a69d32f310	1394	#endif
nikapov	0:a1a69d32f310	1395
nikapov	0:a1a69d32f310	1396	/* Set I2C standard mode */
Davidroid	3:e9269ff624ed	1397	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1398	status = VL53L0X_write_byte(0x88,0x00);
sepp_nepp	7:41cbc431e1f4	1399	}
sepp_nepp	7:41cbc431e1f4	1400
sepp_nepp	7:41cbc431e1f4	1401	_device->DeviceSpecificParameters.ReadDataFromDeviceDone = 0;
sepp_nepp	7:41cbc431e1f4	1402	_device->DeviceSpecificParameters.ReadDataFromDeviceDone = 0;
nikapov	0:a1a69d32f310	1403
nikapov	0:a1a69d32f310	1404	#ifdef USE_IQC_STATION
Davidroid	3:e9269ff624ed	1405	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1406	Status = VL53L0X_apply_offset_adjustment();
Davidroid	3:e9269ff624ed	1407	}
nikapov	0:a1a69d32f310	1408	#endif
nikapov	0:a1a69d32f310	1409
nikapov	0:a1a69d32f310	1410	/* Default value is 1000 for Linearity Corrective Gain */
sepp_nepp	7:41cbc431e1f4	1411	_device->LinearityCorrectiveGain = 1000;
nikapov	0:a1a69d32f310	1412
nikapov	0:a1a69d32f310	1413	/* Dmax default Parameter */
sepp_nepp	7:41cbc431e1f4	1414	_device->DmaxCalRangeMilliMeter = 400;
sepp_nepp	7:41cbc431e1f4	1415	_device->DmaxCalSignalRateRtnMegaCps = (FixPoint1616_t)((0x00016B85)); /* 1.42 No Cover Glass*/
nikapov	0:a1a69d32f310	1416
nikapov	0:a1a69d32f310	1417	/* Set Default static parameters
nikapov	0:a1a69d32f310	1418	set first temporary values 9.44MHz 65536 = 618660 */
sepp_nepp	7:41cbc431e1f4	1419	_device->DeviceSpecificParameters.OscFrequencyMHz = 618660;
nikapov	0:a1a69d32f310	1420
nikapov	0:a1a69d32f310	1421	/* Set Default XTalkCompensationRateMegaCps to 0 */
sepp_nepp	7:41cbc431e1f4	1422	_device->CurrentParameters.XTalkCompensationRateMegaCps = 0;
nikapov	0:a1a69d32f310	1423
nikapov	0:a1a69d32f310	1424	/* Get default parameters */
sepp_nepp	7:41cbc431e1f4	1425	status = VL53L0X_get_device_parameters(&CurrentParameters);
nikapov	0:a1a69d32f310	1426	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1427	/* initialize PAL values */
nikapov	0:a1a69d32f310	1428	CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
nikapov	0:a1a69d32f310	1429	CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
sepp_nepp	7:41cbc431e1f4	1430	_device->CurrentParameters = CurrentParameters;
nikapov	0:a1a69d32f310	1431	}
nikapov	0:a1a69d32f310	1432
nikapov	0:a1a69d32f310	1433	/* Sigma estimator variable */
sepp_nepp	7:41cbc431e1f4	1434	_device->SigmaEstRefArray = 100;
sepp_nepp	7:41cbc431e1f4	1435	_device->SigmaEstEffPulseWidth = 900;
sepp_nepp	7:41cbc431e1f4	1436	_device->SigmaEstEffAmbWidth = 500;
sepp_nepp	7:41cbc431e1f4	1437	_device->targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
nikapov	0:a1a69d32f310	1438
nikapov	0:a1a69d32f310	1439	/* Use internal default settings */
sepp_nepp	7:41cbc431e1f4	1440	_device->UseInternalTuningSettings = 1;
sepp_nepp	7:41cbc431e1f4	1441
sepp_nepp	7:41cbc431e1f4	1442	status \|= VL53L0X_write_byte(0x80,0x01);
sepp_nepp	7:41cbc431e1f4	1443	status \|= VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	1444	status \|= VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	1445	status \|= VL53L0X_read_byte(0x91,&StopVariable);
sepp_nepp	7:41cbc431e1f4	1446	_device->StopVariable = StopVariable;
sepp_nepp	7:41cbc431e1f4	1447	status \|= VL53L0X_write_byte(0x00,0x01);
sepp_nepp	7:41cbc431e1f4	1448	status \|= VL53L0X_write_byte(0xFF,0x00);
sepp_nepp	7:41cbc431e1f4	1449	status \|= VL53L0X_write_byte(0x80,0x00);
nikapov	0:a1a69d32f310	1450
nikapov	0:a1a69d32f310	1451	/* Enable all check */
nikapov	0:a1a69d32f310	1452	for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
Davidroid	3:e9269ff624ed	1453	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1454	status \|= VL53L0X_set_limit_check_enable(i,1);
Davidroid	3:e9269ff624ed	1455	} else {
nikapov	0:a1a69d32f310	1456	break;
Davidroid	3:e9269ff624ed	1457	}
nikapov	0:a1a69d32f310	1458
nikapov	0:a1a69d32f310	1459	}
nikapov	0:a1a69d32f310	1460
nikapov	0:a1a69d32f310	1461	/* Disable the following checks */
nikapov	0:a1a69d32f310	1462	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1463	status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,0);
nikapov	0:a1a69d32f310	1464
nikapov	0:a1a69d32f310	1465	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1466	status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,0);
nikapov	0:a1a69d32f310	1467
nikapov	0:a1a69d32f310	1468	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1469	status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC,0);
nikapov	0:a1a69d32f310	1470
nikapov	0:a1a69d32f310	1471	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	1472	status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE,0);
nikapov	0:a1a69d32f310	1473
nikapov	0:a1a69d32f310	1474	/* Limit default values */
nikapov	0:a1a69d32f310	1475	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1476	status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	1477	(FixPoint1616_t)(18 * 65536));
nikapov	0:a1a69d32f310	1478	}
nikapov	0:a1a69d32f310	1479	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1480	status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov	0:a1a69d32f310	1481	(FixPoint1616_t)(25 * 65536 / 100));
nikapov	0:a1a69d32f310	1482	/* 0.25 * 65536 */
nikapov	0:a1a69d32f310	1483	}
nikapov	0:a1a69d32f310	1484
nikapov	0:a1a69d32f310	1485	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1486	status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	1487	(FixPoint1616_t)(35 * 65536));
nikapov	0:a1a69d32f310	1488	}
nikapov	0:a1a69d32f310	1489
nikapov	0:a1a69d32f310	1490	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1491	status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	1492	(FixPoint1616_t)(0 * 65536));
nikapov	0:a1a69d32f310	1493	}
nikapov	0:a1a69d32f310	1494
nikapov	0:a1a69d32f310	1495	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1496
sepp_nepp	7:41cbc431e1f4	1497	_device->SequenceConfig = 0xFF;
sepp_nepp	7:41cbc431e1f4	1498	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	1499	0xFF);
nikapov	0:a1a69d32f310	1500
nikapov	0:a1a69d32f310	1501	/* Set PAL state to tell that we are waiting for call to
nikapov	0:a1a69d32f310	1502	* VL53L0X_StaticInit */
sepp_nepp	7:41cbc431e1f4	1503	_device->PalState = VL53L0X_STATE_WAIT_STATICINIT;
nikapov	0:a1a69d32f310	1504	}
nikapov	0:a1a69d32f310	1505
Davidroid	3:e9269ff624ed	1506	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1507	_device->DeviceSpecificParameters.RefSpadsInitialised = 0;
Davidroid	3:e9269ff624ed	1508	}
nikapov	0:a1a69d32f310	1509
nikapov	0:a1a69d32f310	1510
sepp_nepp	5:b95f6951f7d5	1511
nikapov	0:a1a69d32f310	1512	return status;
nikapov	0:a1a69d32f310	1513	}
nikapov	0:a1a69d32f310	1514
sepp_nepp	7:41cbc431e1f4	1515	VL53L0X_Error VL53L0X::VL53L0X_check_part_used(uint8_t *revision,
nikapov	0:a1a69d32f310	1516	VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov	0:a1a69d32f310	1517	{
nikapov	0:a1a69d32f310	1518	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1519	uint8_t module_id_int;
nikapov	0:a1a69d32f310	1520	char *product_id_tmp;
nikapov	0:a1a69d32f310	1521
sepp_nepp	5:b95f6951f7d5	1522
nikapov	0:a1a69d32f310	1523
sepp_nepp	7:41cbc431e1f4	1524	status = VL53L0X_get_info_from_device(2);
nikapov	0:a1a69d32f310	1525
nikapov	0:a1a69d32f310	1526	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1527	module_id_int = _device->DeviceSpecificParameters.ModuleId;
nikapov	0:a1a69d32f310	1528
nikapov	0:a1a69d32f310	1529	if (module_id_int == 0) {
nikapov	0:a1a69d32f310	1530	*revision = 0;
sepp_nepp	7:41cbc431e1f4	1531	strcpy(p_VL53L0X_device_info->ProductId,"");
nikapov	0:a1a69d32f310	1532	} else {
sepp_nepp	7:41cbc431e1f4	1533	*revision = _device->DeviceSpecificParameters.Revision;
sepp_nepp	7:41cbc431e1f4	1534	product_id_tmp = _device->DeviceSpecificParameters.ProductId;
sepp_nepp	7:41cbc431e1f4	1535	strcpy(p_VL53L0X_device_info->ProductId,product_id_tmp);
nikapov	0:a1a69d32f310	1536	}
nikapov	0:a1a69d32f310	1537	}
nikapov	0:a1a69d32f310	1538
sepp_nepp	5:b95f6951f7d5	1539
nikapov	0:a1a69d32f310	1540	return status;
nikapov	0:a1a69d32f310	1541	}
nikapov	0:a1a69d32f310	1542
sepp_nepp	7:41cbc431e1f4	1543	VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov	0:a1a69d32f310	1544	{
nikapov	0:a1a69d32f310	1545	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1546	uint8_t revision_id;
nikapov	0:a1a69d32f310	1547	uint8_t revision;
nikapov	0:a1a69d32f310	1548
sepp_nepp	7:41cbc431e1f4	1549	status = VL53L0X_check_part_used(&revision,p_VL53L0X_device_info);
nikapov	0:a1a69d32f310	1550
nikapov	0:a1a69d32f310	1551	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1552	if (revision == 0) {
sepp_nepp	5:b95f6951f7d5	1553	strcpy(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1554	VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
nikapov	0:a1a69d32f310	1555	} else if ((revision <= 34) && (revision != 32)) {
sepp_nepp	5:b95f6951f7d5	1556	strcpy(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1557	VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
nikapov	0:a1a69d32f310	1558	} else if (revision < 39) {
sepp_nepp	5:b95f6951f7d5	1559	strcpy(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1560	VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
nikapov	0:a1a69d32f310	1561	} else {
sepp_nepp	5:b95f6951f7d5	1562	strcpy(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1563	VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
nikapov	0:a1a69d32f310	1564	}
nikapov	0:a1a69d32f310	1565
sepp_nepp	7:41cbc431e1f4	1566	strcpy(p_VL53L0X_device_info->Type,VL53L0X_STRING_DEVICE_INFO_TYPE);
nikapov	0:a1a69d32f310	1567	}
nikapov	0:a1a69d32f310	1568
nikapov	0:a1a69d32f310	1569	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1570	status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_MODEL_ID,
nikapov	0:a1a69d32f310	1571	&p_VL53L0X_device_info->ProductType);
nikapov	0:a1a69d32f310	1572	}
nikapov	0:a1a69d32f310	1573
nikapov	0:a1a69d32f310	1574	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1575	status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_REVISION_ID,
nikapov	0:a1a69d32f310	1576	&revision_id);
nikapov	0:a1a69d32f310	1577	p_VL53L0X_device_info->ProductRevisionMajor = 1;
nikapov	0:a1a69d32f310	1578	p_VL53L0X_device_info->ProductRevisionMinor =
nikapov	0:a1a69d32f310	1579	(revision_id & 0xF0) >> 4;
nikapov	0:a1a69d32f310	1580	}
nikapov	0:a1a69d32f310	1581
nikapov	0:a1a69d32f310	1582	return status;
nikapov	0:a1a69d32f310	1583	}
nikapov	0:a1a69d32f310	1584
sepp_nepp	7:41cbc431e1f4	1585	VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
nikapov	0:a1a69d32f310	1586	{
nikapov	0:a1a69d32f310	1587	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1588	uint8_t byte;
sepp_nepp	5:b95f6951f7d5	1589
nikapov	0:a1a69d32f310	1590
sepp_nepp	7:41cbc431e1f4	1591	status = VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS,&byte);
nikapov	0:a1a69d32f310	1592	*p_interrupt_mask_status = byte & 0x07;
nikapov	0:a1a69d32f310	1593
nikapov	0:a1a69d32f310	1594	if (byte & 0x18) {
nikapov	0:a1a69d32f310	1595	status = VL53L0X_ERROR_RANGE_ERROR;
nikapov	0:a1a69d32f310	1596	}
nikapov	0:a1a69d32f310	1597
sepp_nepp	5:b95f6951f7d5	1598
nikapov	0:a1a69d32f310	1599	return status;
nikapov	0:a1a69d32f310	1600	}
nikapov	0:a1a69d32f310	1601
sepp_nepp	7:41cbc431e1f4	1602	VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(uint8_t *p_measurement_data_ready)
nikapov	0:a1a69d32f310	1603	{
nikapov	0:a1a69d32f310	1604	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1605	uint8_t sys_range_status_register;
nikapov	0:a1a69d32f310	1606	uint8_t interrupt_config;
nikapov	0:a1a69d32f310	1607	uint32_t interrupt_mask;
sepp_nepp	5:b95f6951f7d5	1608
nikapov	0:a1a69d32f310	1609
sepp_nepp	7:41cbc431e1f4	1610	interrupt_config = _device->DeviceSpecificParameters.Pin0GpioFunctionality;
nikapov	0:a1a69d32f310	1611
nikapov	0:a1a69d32f310	1612	if (interrupt_config ==
nikapov	0:a1a69d32f310	1613	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
sepp_nepp	7:41cbc431e1f4	1614	status = VL53L0X_get_interrupt_mask_status(&interrupt_mask);
nikapov	0:a1a69d32f310	1615	if (interrupt_mask ==
nikapov	0:a1a69d32f310	1616	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
nikapov	0:a1a69d32f310	1617	*p_measurement_data_ready = 1;
nikapov	0:a1a69d32f310	1618	} else {
nikapov	0:a1a69d32f310	1619	*p_measurement_data_ready = 0;
nikapov	0:a1a69d32f310	1620	}
nikapov	0:a1a69d32f310	1621	} else {
sepp_nepp	7:41cbc431e1f4	1622	status = VL53L0X_read_byte(VL53L0X_REG_RESULT_RANGE_STATUS,
nikapov	0:a1a69d32f310	1623	&sys_range_status_register);
nikapov	0:a1a69d32f310	1624	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1625	if (sys_range_status_register & 0x01) {
nikapov	0:a1a69d32f310	1626	*p_measurement_data_ready = 1;
nikapov	0:a1a69d32f310	1627	} else {
nikapov	0:a1a69d32f310	1628	*p_measurement_data_ready = 0;
nikapov	0:a1a69d32f310	1629	}
nikapov	0:a1a69d32f310	1630	}
nikapov	0:a1a69d32f310	1631	}
nikapov	0:a1a69d32f310	1632
nikapov	0:a1a69d32f310	1633	return status;
nikapov	0:a1a69d32f310	1634	}
nikapov	0:a1a69d32f310	1635
sepp_nepp	7:41cbc431e1f4	1636	VL53L0X_Error VL53L0X::VL53L0X_polling_delay()
sepp_nepp	6:1976f4afed97	1637	{
sepp_nepp	6:1976f4afed97	1638	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	6:1976f4afed97	1639
sepp_nepp	6:1976f4afed97	1640	// do nothing VL53L0X_OsDelay();
sepp_nepp	6:1976f4afed97	1641	return status;
sepp_nepp	6:1976f4afed97	1642	}
sepp_nepp	6:1976f4afed97	1643
sepp_nepp	7:41cbc431e1f4	1644	VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion()
nikapov	0:a1a69d32f310	1645	{
nikapov	0:a1a69d32f310	1646	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1647	uint8_t new_data_ready = 0;
nikapov	0:a1a69d32f310	1648	uint32_t loop_nb;
nikapov	0:a1a69d32f310	1649
nikapov	0:a1a69d32f310	1650	loop_nb = 0;
sepp_nepp	5:b95f6951f7d5	1651
sepp_nepp	7:41cbc431e1f4	1652	status = VL53L0X_get_measurement_data_ready(&new_data_ready);
sepp_nepp	7:41cbc431e1f4	1653
sepp_nepp	7:41cbc431e1f4	1654	while ((status==0) && (new_data_ready != 1) &&
sepp_nepp	5:b95f6951f7d5	1655	(loop_nb < VL53L0X_DEFAULT_MAX_LOOP) )
sepp_nepp	5:b95f6951f7d5	1656	{
sepp_nepp	7:41cbc431e1f4	1657	VL53L0X_polling_delay();
sepp_nepp	7:41cbc431e1f4	1658	status = VL53L0X_get_measurement_data_ready(&new_data_ready);
sepp_nepp	5:b95f6951f7d5	1659	loop_nb++;
sepp_nepp	5:b95f6951f7d5	1660	} // while ;
sepp_nepp	5:b95f6951f7d5	1661
sepp_nepp	5:b95f6951f7d5	1662	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) status = VL53L0X_ERROR_TIME_OUT;
sepp_nepp	5:b95f6951f7d5	1663
nikapov	0:a1a69d32f310	1664	return status;
nikapov	0:a1a69d32f310	1665	}
nikapov	0:a1a69d32f310	1666
nikapov	0:a1a69d32f310	1667	/* Group PAL Interrupt Functions */
sepp_nepp	7:41cbc431e1f4	1668	VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask(uint32_t interrupt_mask)
nikapov	0:a1a69d32f310	1669	{
nikapov	0:a1a69d32f310	1670	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1671	uint8_t loop_count;
nikapov	0:a1a69d32f310	1672	uint8_t byte;
sepp_nepp	5:b95f6951f7d5	1673
nikapov	0:a1a69d32f310	1674
sepp_nepp	7:41cbc431e1f4	1675	/* clear bit 0 range interrupt,bit 1 error interrupt */
nikapov	0:a1a69d32f310	1676	loop_count = 0;
nikapov	0:a1a69d32f310	1677	do {
sepp_nepp	7:41cbc431e1f4	1678	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR,0x01);
sepp_nepp	7:41cbc431e1f4	1679	status \|= VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR,0x00);
sepp_nepp	7:41cbc431e1f4	1680	status \|= VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS,&byte);
nikapov	0:a1a69d32f310	1681	loop_count++;
nikapov	0:a1a69d32f310	1682	} while (((byte & 0x07) != 0x00)
nikapov	0:a1a69d32f310	1683	&& (loop_count < 3)
nikapov	0:a1a69d32f310	1684	&& (status == VL53L0X_ERROR_NONE));
nikapov	0:a1a69d32f310	1685
nikapov	0:a1a69d32f310	1686
nikapov	0:a1a69d32f310	1687	if (loop_count >= 3) {
nikapov	0:a1a69d32f310	1688	status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
nikapov	0:a1a69d32f310	1689	}
nikapov	0:a1a69d32f310	1690
sepp_nepp	5:b95f6951f7d5	1691
nikapov	0:a1a69d32f310	1692	return status;
nikapov	0:a1a69d32f310	1693	}
nikapov	0:a1a69d32f310	1694
sepp_nepp	7:41cbc431e1f4	1695	VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(uint8_t vhv_init_byte)
nikapov	0:a1a69d32f310	1696	{
nikapov	0:a1a69d32f310	1697	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1698
nikapov	0:a1a69d32f310	1699	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1700	status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	1701	VL53L0X_REG_SYSRANGE_MODE_START_STOP \|
nikapov	0:a1a69d32f310	1702	vhv_init_byte);
nikapov	0:a1a69d32f310	1703	}
nikapov	0:a1a69d32f310	1704
nikapov	0:a1a69d32f310	1705	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1706	status = VL53L0X_measurement_poll_for_completion();
nikapov	0:a1a69d32f310	1707	}
nikapov	0:a1a69d32f310	1708
nikapov	0:a1a69d32f310	1709	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1710	status = VL53L0X_clear_interrupt_mask(0);
nikapov	0:a1a69d32f310	1711	}
nikapov	0:a1a69d32f310	1712
nikapov	0:a1a69d32f310	1713	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1714	status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,0x00);
nikapov	0:a1a69d32f310	1715	}
nikapov	0:a1a69d32f310	1716
nikapov	0:a1a69d32f310	1717	return status;
nikapov	0:a1a69d32f310	1718	}
nikapov	0:a1a69d32f310	1719
sepp_nepp	7:41cbc431e1f4	1720	VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(uint8_t read_not_write,
sepp_nepp	7:41cbc431e1f4	1721	uint8_t vhv_settings,uint8_t phase_cal,
sepp_nepp	7:41cbc431e1f4	1722	uint8_t p_vhv_settings,uint8_t p_phase_cal,
sepp_nepp	7:41cbc431e1f4	1723	const uint8_t vhv_enable,const uint8_t phase_enable)
nikapov	0:a1a69d32f310	1724	{
nikapov	0:a1a69d32f310	1725	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1726	uint8_t phase_calint = 0;
nikapov	0:a1a69d32f310	1727
nikapov	0:a1a69d32f310	1728	/* Read VHV from device */
sepp_nepp	7:41cbc431e1f4	1729	status \|= VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	1730	status \|= VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	1731	status \|= VL53L0X_write_byte(0xFF,0x00);
nikapov	0:a1a69d32f310	1732
nikapov	0:a1a69d32f310	1733	if (read_not_write) {
nikapov	0:a1a69d32f310	1734	if (vhv_enable) {
sepp_nepp	7:41cbc431e1f4	1735	status \|= VL53L0X_read_byte(0xCB,p_vhv_settings);
nikapov	0:a1a69d32f310	1736	}
nikapov	0:a1a69d32f310	1737	if (phase_enable) {
sepp_nepp	7:41cbc431e1f4	1738	status \|= VL53L0X_read_byte(0xEE,&phase_calint);
nikapov	0:a1a69d32f310	1739	}
nikapov	0:a1a69d32f310	1740	} else {
nikapov	0:a1a69d32f310	1741	if (vhv_enable) {
sepp_nepp	7:41cbc431e1f4	1742	status \|= VL53L0X_write_byte(0xCB,vhv_settings);
nikapov	0:a1a69d32f310	1743	}
nikapov	0:a1a69d32f310	1744	if (phase_enable) {
sepp_nepp	7:41cbc431e1f4	1745	status \|= VL53L0X_update_byte(0xEE,0x80,phase_cal);
nikapov	0:a1a69d32f310	1746	}
nikapov	0:a1a69d32f310	1747	}
nikapov	0:a1a69d32f310	1748
sepp_nepp	7:41cbc431e1f4	1749	status \|= VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	1750	status \|= VL53L0X_write_byte(0x00,0x01);
sepp_nepp	7:41cbc431e1f4	1751	status \|= VL53L0X_write_byte(0xFF,0x00);
nikapov	0:a1a69d32f310	1752
nikapov	0:a1a69d32f310	1753	*p_phase_cal = (uint8_t)(phase_calint & 0xEF);
nikapov	0:a1a69d32f310	1754
nikapov	0:a1a69d32f310	1755	return status;
nikapov	0:a1a69d32f310	1756	}
nikapov	0:a1a69d32f310	1757
sepp_nepp	7:41cbc431e1f4	1758	VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(uint8_t *p_vhv_settings,const uint8_t get_data_enable,
nikapov	0:a1a69d32f310	1759	const uint8_t restore_config)
nikapov	0:a1a69d32f310	1760	{
nikapov	0:a1a69d32f310	1761	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1762	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	1763	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	1764	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	1765	uint8_t phase_cal_int = 0;
nikapov	0:a1a69d32f310	1766
nikapov	0:a1a69d32f310	1767	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	1768	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	1769	*/
sepp_nepp	7:41cbc431e1f4	1770	if (restore_config) {sequence_config = _device->SequenceConfig;}
nikapov	0:a1a69d32f310	1771
nikapov	0:a1a69d32f310	1772	/* Run VHV */
sepp_nepp	7:41cbc431e1f4	1773	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0x01);
nikapov	0:a1a69d32f310	1774
nikapov	0:a1a69d32f310	1775	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1776	status = VL53L0X_perform_single_ref_calibration(0x40);
nikapov	0:a1a69d32f310	1777	}
nikapov	0:a1a69d32f310	1778
nikapov	0:a1a69d32f310	1779	/* Read VHV from device */
nikapov	0:a1a69d32f310	1780	if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
sepp_nepp	7:41cbc431e1f4	1781	status = VL53L0X_ref_calibration_io(1,
sepp_nepp	7:41cbc431e1f4	1782	vhv_settings,phase_cal,/* Not used here */
sepp_nepp	7:41cbc431e1f4	1783	p_vhv_settings,&phase_cal_int,
sepp_nepp	7:41cbc431e1f4	1784	1,0);
nikapov	0:a1a69d32f310	1785	} else {
nikapov	0:a1a69d32f310	1786	*p_vhv_settings = 0;
nikapov	0:a1a69d32f310	1787	}
nikapov	0:a1a69d32f310	1788
nikapov	0:a1a69d32f310	1789
nikapov	0:a1a69d32f310	1790	if ((status == VL53L0X_ERROR_NONE) && restore_config) {
nikapov	0:a1a69d32f310	1791	/* restore the previous Sequence Config */
sepp_nepp	7:41cbc431e1f4	1792	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	1793	sequence_config);
nikapov	0:a1a69d32f310	1794	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1795	_device->SequenceConfig = sequence_config;
nikapov	0:a1a69d32f310	1796	}
nikapov	0:a1a69d32f310	1797
nikapov	0:a1a69d32f310	1798	}
nikapov	0:a1a69d32f310	1799
nikapov	0:a1a69d32f310	1800	return status;
nikapov	0:a1a69d32f310	1801	}
nikapov	0:a1a69d32f310	1802
sepp_nepp	7:41cbc431e1f4	1803	VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable,
nikapov	0:a1a69d32f310	1804	const uint8_t restore_config)
nikapov	0:a1a69d32f310	1805	{
nikapov	0:a1a69d32f310	1806	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1807	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	1808	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	1809	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	1810	uint8_t vhv_settingsint;
nikapov	0:a1a69d32f310	1811
nikapov	0:a1a69d32f310	1812	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	1813	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	1814	*/
nikapov	0:a1a69d32f310	1815
Davidroid	3:e9269ff624ed	1816	if (restore_config) {
sepp_nepp	7:41cbc431e1f4	1817	sequence_config = _device-> SequenceConfig;
Davidroid	3:e9269ff624ed	1818	}
nikapov	0:a1a69d32f310	1819
nikapov	0:a1a69d32f310	1820	/* Run PhaseCal */
sepp_nepp	7:41cbc431e1f4	1821	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0x02);
nikapov	0:a1a69d32f310	1822
nikapov	0:a1a69d32f310	1823	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1824	status = VL53L0X_perform_single_ref_calibration(0x0);
nikapov	0:a1a69d32f310	1825	}
nikapov	0:a1a69d32f310	1826
nikapov	0:a1a69d32f310	1827	/* Read PhaseCal from device */
nikapov	0:a1a69d32f310	1828	if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
sepp_nepp	7:41cbc431e1f4	1829	status = VL53L0X_ref_calibration_io(1,
sepp_nepp	7:41cbc431e1f4	1830	vhv_settings,phase_cal,/* Not used here */
sepp_nepp	7:41cbc431e1f4	1831	&vhv_settingsint,p_phase_cal,
sepp_nepp	7:41cbc431e1f4	1832	0,1);
nikapov	0:a1a69d32f310	1833	} else {
nikapov	0:a1a69d32f310	1834	*p_phase_cal = 0;
nikapov	0:a1a69d32f310	1835	}
nikapov	0:a1a69d32f310	1836
nikapov	0:a1a69d32f310	1837
nikapov	0:a1a69d32f310	1838	if ((status == VL53L0X_ERROR_NONE) && restore_config) {
nikapov	0:a1a69d32f310	1839	/* restore the previous Sequence Config */
sepp_nepp	7:41cbc431e1f4	1840	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	1841	sequence_config);
nikapov	0:a1a69d32f310	1842	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1843	_device->SequenceConfig = sequence_config;
nikapov	0:a1a69d32f310	1844	}
nikapov	0:a1a69d32f310	1845
nikapov	0:a1a69d32f310	1846	}
nikapov	0:a1a69d32f310	1847
nikapov	0:a1a69d32f310	1848	return status;
nikapov	0:a1a69d32f310	1849	}
nikapov	0:a1a69d32f310	1850
sepp_nepp	7:41cbc431e1f4	1851	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t p_vhv_settings,uint8_t p_phase_cal,uint8_t get_data_enable)
nikapov	0:a1a69d32f310	1852	{
nikapov	0:a1a69d32f310	1853	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1854	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	1855
nikapov	0:a1a69d32f310	1856	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	1857	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	1858	*/
nikapov	0:a1a69d32f310	1859
sepp_nepp	7:41cbc431e1f4	1860	sequence_config = _device-> SequenceConfig;
nikapov	0:a1a69d32f310	1861
nikapov	0:a1a69d32f310	1862	/* In the following function we don't save the config to optimize
nikapov	0:a1a69d32f310	1863	* writes on device. Config is saved and restored only once. */
sepp_nepp	7:41cbc431e1f4	1864	status = VL53L0X_perform_vhv_calibration(p_vhv_settings,get_data_enable,0);
nikapov	0:a1a69d32f310	1865
nikapov	0:a1a69d32f310	1866	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1867	status = VL53L0X_perform_phase_calibration(p_phase_cal,get_data_enable,0);
Davidroid	2:d07edeaff6f1	1868	}
nikapov	0:a1a69d32f310	1869
nikapov	0:a1a69d32f310	1870
nikapov	0:a1a69d32f310	1871	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1872	/* restore the previous Sequence Config */
sepp_nepp	7:41cbc431e1f4	1873	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	1874	sequence_config);
nikapov	0:a1a69d32f310	1875	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	1876	_device->SequenceConfig = sequence_config;
nikapov	0:a1a69d32f310	1877	}
nikapov	0:a1a69d32f310	1878
nikapov	0:a1a69d32f310	1879	}
nikapov	0:a1a69d32f310	1880
nikapov	0:a1a69d32f310	1881	return status;
nikapov	0:a1a69d32f310	1882	}
nikapov	0:a1a69d32f310	1883
sepp_nepp	7:41cbc431e1f4	1884	void VL53L0X::get_next_good_spad(uint8_t good_spad_array[],uint32_t size,
sepp_nepp	7:41cbc431e1f4	1885	uint32_t curr,int32_t *p_next)
nikapov	0:a1a69d32f310	1886	{
nikapov	0:a1a69d32f310	1887	uint32_t start_index;
nikapov	0:a1a69d32f310	1888	uint32_t fine_offset;
nikapov	0:a1a69d32f310	1889	uint32_t c_spads_per_byte = 8;
nikapov	0:a1a69d32f310	1890	uint32_t coarse_index;
nikapov	0:a1a69d32f310	1891	uint32_t fine_index;
nikapov	0:a1a69d32f310	1892	uint8_t data_byte;
nikapov	0:a1a69d32f310	1893	uint8_t success = 0;
nikapov	0:a1a69d32f310	1894
nikapov	0:a1a69d32f310	1895	/*
sepp_nepp	7:41cbc431e1f4	1896	* Starting with the current good spad,loop through the array to find
nikapov	0:a1a69d32f310	1897	* the next. i.e. the next bit set in the sequence.
nikapov	0:a1a69d32f310	1898	*
nikapov	0:a1a69d32f310	1899	* The coarse index is the byte index of the array and the fine index is
nikapov	0:a1a69d32f310	1900	* the index of the bit within each byte.
nikapov	0:a1a69d32f310	1901	*/
nikapov	0:a1a69d32f310	1902
nikapov	0:a1a69d32f310	1903	*p_next = -1;
nikapov	0:a1a69d32f310	1904
nikapov	0:a1a69d32f310	1905	start_index = curr / c_spads_per_byte;
nikapov	0:a1a69d32f310	1906	fine_offset = curr % c_spads_per_byte;
nikapov	0:a1a69d32f310	1907
nikapov	0:a1a69d32f310	1908	for (coarse_index = start_index; ((coarse_index < size) && !success);
nikapov	0:a1a69d32f310	1909	coarse_index++) {
nikapov	0:a1a69d32f310	1910	fine_index = 0;
nikapov	0:a1a69d32f310	1911	data_byte = good_spad_array[coarse_index];
nikapov	0:a1a69d32f310	1912
nikapov	0:a1a69d32f310	1913	if (coarse_index == start_index) {
nikapov	0:a1a69d32f310	1914	/* locate the bit position of the provided current
nikapov	0:a1a69d32f310	1915	* spad bit before iterating */
nikapov	0:a1a69d32f310	1916	data_byte >>= fine_offset;
nikapov	0:a1a69d32f310	1917	fine_index = fine_offset;
nikapov	0:a1a69d32f310	1918	}
nikapov	0:a1a69d32f310	1919
nikapov	0:a1a69d32f310	1920	while (fine_index < c_spads_per_byte) {
nikapov	0:a1a69d32f310	1921	if ((data_byte & 0x1) == 1) {
nikapov	0:a1a69d32f310	1922	success = 1;
nikapov	0:a1a69d32f310	1923	p_next = coarse_index c_spads_per_byte + fine_index;
nikapov	0:a1a69d32f310	1924	break;
nikapov	0:a1a69d32f310	1925	}
nikapov	0:a1a69d32f310	1926	data_byte >>= 1;
nikapov	0:a1a69d32f310	1927	fine_index++;
nikapov	0:a1a69d32f310	1928	}
nikapov	0:a1a69d32f310	1929	}
nikapov	0:a1a69d32f310	1930	}
nikapov	0:a1a69d32f310	1931
nikapov	0:a1a69d32f310	1932	uint8_t VL53L0X::is_aperture(uint32_t spad_index)
nikapov	0:a1a69d32f310	1933	{
nikapov	0:a1a69d32f310	1934	/*
nikapov	0:a1a69d32f310	1935	* This function reports if a given spad index is an aperture SPAD by
nikapov	0:a1a69d32f310	1936	* deriving the quadrant.
nikapov	0:a1a69d32f310	1937	*/
nikapov	0:a1a69d32f310	1938	uint32_t quadrant;
nikapov	0:a1a69d32f310	1939	uint8_t is_aperture = 1;
nikapov	0:a1a69d32f310	1940	quadrant = spad_index >> 6;
nikapov	0:a1a69d32f310	1941	if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) {
nikapov	0:a1a69d32f310	1942	is_aperture = 0;
Davidroid	2:d07edeaff6f1	1943	}
nikapov	0:a1a69d32f310	1944
nikapov	0:a1a69d32f310	1945	return is_aperture;
nikapov	0:a1a69d32f310	1946	}
nikapov	0:a1a69d32f310	1947
sepp_nepp	7:41cbc431e1f4	1948	VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[],uint32_t size,
nikapov	0:a1a69d32f310	1949	uint32_t spad_index)
nikapov	0:a1a69d32f310	1950	{
nikapov	0:a1a69d32f310	1951	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1952	uint32_t c_spads_per_byte = 8;
nikapov	0:a1a69d32f310	1953	uint32_t coarse_index;
nikapov	0:a1a69d32f310	1954	uint32_t fine_index;
nikapov	0:a1a69d32f310	1955
nikapov	0:a1a69d32f310	1956	coarse_index = spad_index / c_spads_per_byte;
nikapov	0:a1a69d32f310	1957	fine_index = spad_index % c_spads_per_byte;
nikapov	0:a1a69d32f310	1958	if (coarse_index >= size) {
nikapov	0:a1a69d32f310	1959	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	1960	} else {
nikapov	0:a1a69d32f310	1961	spad_array[coarse_index] \|= (1 << fine_index);
nikapov	0:a1a69d32f310	1962	}
nikapov	0:a1a69d32f310	1963
nikapov	0:a1a69d32f310	1964	return status;
nikapov	0:a1a69d32f310	1965	}
nikapov	0:a1a69d32f310	1966
sepp_nepp	7:41cbc431e1f4	1967	VL53L0X_Error VL53L0X::set_ref_spad_map(uint8_t *p_ref_spad_array)
nikapov	0:a1a69d32f310	1968	{
sepp_nepp	7:41cbc431e1f4	1969	VL53L0X_Error status = VL53L0X_write_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
sepp_nepp	7:41cbc431e1f4	1970	p_ref_spad_array,6);
nikapov	0:a1a69d32f310	1971
nikapov	0:a1a69d32f310	1972	return status;
nikapov	0:a1a69d32f310	1973	}
nikapov	0:a1a69d32f310	1974
sepp_nepp	7:41cbc431e1f4	1975	VL53L0X_Error VL53L0X::get_ref_spad_map(uint8_t *p_ref_spad_array)
nikapov	0:a1a69d32f310	1976	{
sepp_nepp	7:41cbc431e1f4	1977	VL53L0X_Error status = VL53L0X_read_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
nikapov	0:a1a69d32f310	1978	p_ref_spad_array,
nikapov	0:a1a69d32f310	1979	6);
nikapov	0:a1a69d32f310	1980	// VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1981	// uint8_t count=0;
nikapov	0:a1a69d32f310	1982
nikapov	0:a1a69d32f310	1983	// for (count = 0; count < 6; count++)
sepp_nepp	7:41cbc431e1f4	1984	// status = VL53L0X_RdByte((VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count),&refSpadArray[count]);
nikapov	0:a1a69d32f310	1985	return status;
nikapov	0:a1a69d32f310	1986	}
nikapov	0:a1a69d32f310	1987
sepp_nepp	7:41cbc431e1f4	1988	VL53L0X_Error VL53L0X::enable_ref_spads(uint8_t aperture_spads,
nikapov	0:a1a69d32f310	1989	uint8_t good_spad_array[],
nikapov	0:a1a69d32f310	1990	uint8_t spad_array[],
nikapov	0:a1a69d32f310	1991	uint32_t size,
nikapov	0:a1a69d32f310	1992	uint32_t start,
nikapov	0:a1a69d32f310	1993	uint32_t offset,
nikapov	0:a1a69d32f310	1994	uint32_t spad_count,
nikapov	0:a1a69d32f310	1995	uint32_t *p_last_spad)
nikapov	0:a1a69d32f310	1996	{
nikapov	0:a1a69d32f310	1997	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1998	uint32_t index;
nikapov	0:a1a69d32f310	1999	uint32_t i;
nikapov	0:a1a69d32f310	2000	int32_t next_good_spad = offset;
nikapov	0:a1a69d32f310	2001	uint32_t current_spad;
nikapov	0:a1a69d32f310	2002	uint8_t check_spad_array[6];
nikapov	0:a1a69d32f310	2003
nikapov	0:a1a69d32f310	2004	/*
nikapov	0:a1a69d32f310	2005	* This function takes in a spad array which may or may not have SPADS
nikapov	0:a1a69d32f310	2006	* already enabled and appends from a given offset a requested number
nikapov	0:a1a69d32f310	2007	* of new SPAD enables. The 'good spad map' is applied to
nikapov	0:a1a69d32f310	2008	* determine the next SPADs to enable.
nikapov	0:a1a69d32f310	2009	*
nikapov	0:a1a69d32f310	2010	* This function applies to only aperture or only non-aperture spads.
nikapov	0:a1a69d32f310	2011	* Checks are performed to ensure this.
nikapov	0:a1a69d32f310	2012	*/
nikapov	0:a1a69d32f310	2013
nikapov	0:a1a69d32f310	2014	current_spad = offset;
nikapov	0:a1a69d32f310	2015	for (index = 0; index < spad_count; index++) {
sepp_nepp	7:41cbc431e1f4	2016	get_next_good_spad(good_spad_array,size,current_spad,
nikapov	0:a1a69d32f310	2017	&next_good_spad);
nikapov	0:a1a69d32f310	2018
nikapov	0:a1a69d32f310	2019	if (next_good_spad == -1) {
nikapov	0:a1a69d32f310	2020	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2021	break;
nikapov	0:a1a69d32f310	2022	}
nikapov	0:a1a69d32f310	2023
nikapov	0:a1a69d32f310	2024	/* Confirm that the next good SPAD is non-aperture */
nikapov	0:a1a69d32f310	2025	if (is_aperture(start + next_good_spad) != aperture_spads) {
nikapov	0:a1a69d32f310	2026	/* if we can't get the required number of good aperture
nikapov	0:a1a69d32f310	2027	* spads from the current quadrant then this is an error
nikapov	0:a1a69d32f310	2028	*/
nikapov	0:a1a69d32f310	2029	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2030	break;
nikapov	0:a1a69d32f310	2031	}
nikapov	0:a1a69d32f310	2032	current_spad = (uint32_t)next_good_spad;
sepp_nepp	7:41cbc431e1f4	2033	enable_spad_bit(spad_array,size,current_spad);
nikapov	0:a1a69d32f310	2034	current_spad++;
nikapov	0:a1a69d32f310	2035	}
nikapov	0:a1a69d32f310	2036	*p_last_spad = current_spad;
nikapov	0:a1a69d32f310	2037
nikapov	0:a1a69d32f310	2038	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	2039	status = set_ref_spad_map(spad_array);
nikapov	0:a1a69d32f310	2040	}
nikapov	0:a1a69d32f310	2041
nikapov	0:a1a69d32f310	2042
nikapov	0:a1a69d32f310	2043	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	2044	status = get_ref_spad_map(check_spad_array);
nikapov	0:a1a69d32f310	2045
nikapov	0:a1a69d32f310	2046	i = 0;
nikapov	0:a1a69d32f310	2047
nikapov	0:a1a69d32f310	2048	/* Compare spad maps. If not equal report error. */
nikapov	0:a1a69d32f310	2049	while (i < size) {
nikapov	0:a1a69d32f310	2050	if (spad_array[i] != check_spad_array[i]) {
nikapov	0:a1a69d32f310	2051	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2052	break;
nikapov	0:a1a69d32f310	2053	}
nikapov	0:a1a69d32f310	2054	i++;
nikapov	0:a1a69d32f310	2055	}
nikapov	0:a1a69d32f310	2056	}
nikapov	0:a1a69d32f310	2057	return status;
nikapov	0:a1a69d32f310	2058	}
nikapov	0:a1a69d32f310	2059
sepp_nepp	7:41cbc431e1f4	2060	VL53L0X_Error VL53L0X::VL53L0X_set_device_mode(VL53L0X_DeviceModes device_mode)
nikapov	0:a1a69d32f310	2061	{
nikapov	0:a1a69d32f310	2062	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2063
nikapov	0:a1a69d32f310	2064	switch (device_mode) {
nikapov	0:a1a69d32f310	2065	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov	0:a1a69d32f310	2066	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov	0:a1a69d32f310	2067	case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov	0:a1a69d32f310	2068	case VL53L0X_DEVICEMODE_GPIO_DRIVE:
nikapov	0:a1a69d32f310	2069	case VL53L0X_DEVICEMODE_GPIO_OSC:
nikapov	0:a1a69d32f310	2070	/* Supported modes */
sepp_nepp	7:41cbc431e1f4	2071	_device->CurrentParameters.DeviceMode = device_mode;
nikapov	0:a1a69d32f310	2072	break;
nikapov	0:a1a69d32f310	2073	default:
nikapov	0:a1a69d32f310	2074	/* Unsupported mode */
nikapov	0:a1a69d32f310	2075	status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov	0:a1a69d32f310	2076	}
nikapov	0:a1a69d32f310	2077
sepp_nepp	5:b95f6951f7d5	2078
nikapov	0:a1a69d32f310	2079	return status;
nikapov	0:a1a69d32f310	2080	}
nikapov	0:a1a69d32f310	2081
sepp_nepp	7:41cbc431e1f4	2082	VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,FixPoint1616_t threshold_low,
nikapov	0:a1a69d32f310	2083	FixPoint1616_t threshold_high)
nikapov	0:a1a69d32f310	2084	{
nikapov	0:a1a69d32f310	2085	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2086	uint16_t threshold16;
sepp_nepp	5:b95f6951f7d5	2087
sepp_nepp	5:b95f6951f7d5	2088
sepp_nepp	5:b95f6951f7d5	2089	/* no dependency on DeviceMode for FlightSense */
nikapov	0:a1a69d32f310	2090	/* Need to divide by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2091	threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
sepp_nepp	7:41cbc431e1f4	2092	status = VL53L0X_write_word(VL53L0X_REG_SYSTEM_THRESH_LOW,threshold16);
nikapov	0:a1a69d32f310	2093
nikapov	0:a1a69d32f310	2094	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2095	/* Need to divide by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2096	threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
sepp_nepp	7:41cbc431e1f4	2097	status = VL53L0X_write_word(VL53L0X_REG_SYSTEM_THRESH_HIGH,
nikapov	0:a1a69d32f310	2098	threshold16);
nikapov	0:a1a69d32f310	2099	}
nikapov	0:a1a69d32f310	2100
sepp_nepp	5:b95f6951f7d5	2101
nikapov	0:a1a69d32f310	2102	return status;
nikapov	0:a1a69d32f310	2103	}
nikapov	0:a1a69d32f310	2104
sepp_nepp	7:41cbc431e1f4	2105	VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,FixPoint1616_t *p_threshold_low,
nikapov	0:a1a69d32f310	2106	FixPoint1616_t *p_threshold_high)
nikapov	0:a1a69d32f310	2107	{
nikapov	0:a1a69d32f310	2108	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2109	uint16_t threshold16;
sepp_nepp	5:b95f6951f7d5	2110
sepp_nepp	5:b95f6951f7d5	2111
sepp_nepp	5:b95f6951f7d5	2112	/* no dependency on DeviceMode for FlightSense */
nikapov	0:a1a69d32f310	2113
sepp_nepp	7:41cbc431e1f4	2114	status = VL53L0X_read_word(VL53L0X_REG_SYSTEM_THRESH_LOW,&threshold16);
nikapov	0:a1a69d32f310	2115	/* Need to multiply by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2116	*p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17);
nikapov	0:a1a69d32f310	2117
nikapov	0:a1a69d32f310	2118	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	2119	status = VL53L0X_read_word(VL53L0X_REG_SYSTEM_THRESH_HIGH,
nikapov	0:a1a69d32f310	2120	&threshold16);
nikapov	0:a1a69d32f310	2121	/* Need to multiply by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2122	*p_threshold_high =
nikapov	0:a1a69d32f310	2123	(FixPoint1616_t)((0x00fff & threshold16) << 17);
nikapov	0:a1a69d32f310	2124	}
nikapov	0:a1a69d32f310	2125
sepp_nepp	5:b95f6951f7d5	2126
nikapov	0:a1a69d32f310	2127	return status;
nikapov	0:a1a69d32f310	2128	}
nikapov	0:a1a69d32f310	2129
sepp_nepp	7:41cbc431e1f4	2130	VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(uint8_t *p_tuning_setting_buffer)
nikapov	0:a1a69d32f310	2131	{
nikapov	0:a1a69d32f310	2132	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2133	int i;
nikapov	0:a1a69d32f310	2134	int index;
nikapov	0:a1a69d32f310	2135	uint8_t msb;
nikapov	0:a1a69d32f310	2136	uint8_t lsb;
nikapov	0:a1a69d32f310	2137	uint8_t select_param;
nikapov	0:a1a69d32f310	2138	uint8_t number_of_writes;
nikapov	0:a1a69d32f310	2139	uint8_t address;
nikapov	0:a1a69d32f310	2140	uint8_t local_buffer[4]; /* max */
nikapov	0:a1a69d32f310	2141	uint16_t temp16;
nikapov	0:a1a69d32f310	2142
sepp_nepp	5:b95f6951f7d5	2143
nikapov	0:a1a69d32f310	2144
nikapov	0:a1a69d32f310	2145	index = 0;
nikapov	0:a1a69d32f310	2146
nikapov	0:a1a69d32f310	2147	while ((*(p_tuning_setting_buffer + index) != 0) &&
nikapov	0:a1a69d32f310	2148	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	2149	number_of_writes = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2150	index++;
nikapov	0:a1a69d32f310	2151	if (number_of_writes == 0xFF) {
nikapov	0:a1a69d32f310	2152	/* internal parameters */
nikapov	0:a1a69d32f310	2153	select_param = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2154	index++;
nikapov	0:a1a69d32f310	2155	switch (select_param) {
nikapov	0:a1a69d32f310	2156	case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
nikapov	0:a1a69d32f310	2157	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2158	index++;
nikapov	0:a1a69d32f310	2159	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2160	index++;
sepp_nepp	7:41cbc431e1f4	2161	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	7:41cbc431e1f4	2162	_device->SigmaEstRefArray = temp16;
nikapov	0:a1a69d32f310	2163	break;
nikapov	0:a1a69d32f310	2164	case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
nikapov	0:a1a69d32f310	2165	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2166	index++;
nikapov	0:a1a69d32f310	2167	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2168	index++;
sepp_nepp	7:41cbc431e1f4	2169	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	7:41cbc431e1f4	2170	_device->SigmaEstEffPulseWidth = temp16;
nikapov	0:a1a69d32f310	2171	break;
nikapov	0:a1a69d32f310	2172	case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
nikapov	0:a1a69d32f310	2173	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2174	index++;
nikapov	0:a1a69d32f310	2175	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2176	index++;
sepp_nepp	7:41cbc431e1f4	2177	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	7:41cbc431e1f4	2178	_device->SigmaEstEffAmbWidth = temp16;
nikapov	0:a1a69d32f310	2179	break;
nikapov	0:a1a69d32f310	2180	case 3: /* uint16_t targetRefRate -> 2 bytes */
nikapov	0:a1a69d32f310	2181	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2182	index++;
nikapov	0:a1a69d32f310	2183	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2184	index++;
sepp_nepp	7:41cbc431e1f4	2185	temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp	7:41cbc431e1f4	2186	_device->targetRefRate = temp16;
nikapov	0:a1a69d32f310	2187	break;
nikapov	0:a1a69d32f310	2188	default: /* invalid parameter */
nikapov	0:a1a69d32f310	2189	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	2190	}
nikapov	0:a1a69d32f310	2191
nikapov	0:a1a69d32f310	2192	} else if (number_of_writes <= 4) {
nikapov	0:a1a69d32f310	2193	address = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2194	index++;
nikapov	0:a1a69d32f310	2195
nikapov	0:a1a69d32f310	2196	for (i = 0; i < number_of_writes; i++) {
nikapov	0:a1a69d32f310	2197	local_buffer[i] = *(p_tuning_setting_buffer +
nikapov	0:a1a69d32f310	2198	index);
nikapov	0:a1a69d32f310	2199	index++;
nikapov	0:a1a69d32f310	2200	}
nikapov	0:a1a69d32f310	2201
sepp_nepp	7:41cbc431e1f4	2202	status = VL53L0X_write_multi(address,local_buffer,
nikapov	0:a1a69d32f310	2203	number_of_writes);
nikapov	0:a1a69d32f310	2204
nikapov	0:a1a69d32f310	2205	} else {
nikapov	0:a1a69d32f310	2206	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	2207	}
nikapov	0:a1a69d32f310	2208	}
nikapov	0:a1a69d32f310	2209
sepp_nepp	5:b95f6951f7d5	2210
nikapov	0:a1a69d32f310	2211	return status;
nikapov	0:a1a69d32f310	2212	}
nikapov	0:a1a69d32f310	2213
sepp_nepp	7:41cbc431e1f4	2214	VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(uint8_t start_not_stopflag)
nikapov	0:a1a69d32f310	2215	{
nikapov	0:a1a69d32f310	2216	uint8_t interrupt_config;
nikapov	0:a1a69d32f310	2217	FixPoint1616_t threshold_low;
nikapov	0:a1a69d32f310	2218	FixPoint1616_t threshold_high;
nikapov	0:a1a69d32f310	2219	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2220
sepp_nepp	7:41cbc431e1f4	2221	interrupt_config = _device->DeviceSpecificParameters.Pin0GpioFunctionality;
nikapov	0:a1a69d32f310	2222
nikapov	0:a1a69d32f310	2223	if ((interrupt_config ==
nikapov	0:a1a69d32f310	2224	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) \|\|
nikapov	0:a1a69d32f310	2225	(interrupt_config ==
nikapov	0:a1a69d32f310	2226	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) \|\|
nikapov	0:a1a69d32f310	2227	(interrupt_config ==
nikapov	0:a1a69d32f310	2228	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
nikapov	0:a1a69d32f310	2229
sepp_nepp	7:41cbc431e1f4	2230	status = VL53L0X_get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp	7:41cbc431e1f4	2231	&threshold_low,&threshold_high);
nikapov	0:a1a69d32f310	2232
nikapov	0:a1a69d32f310	2233	if (((threshold_low > 255 * 65536) \|\|
nikapov	0:a1a69d32f310	2234	(threshold_high > 255 * 65536)) &&
nikapov	0:a1a69d32f310	2235	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	2236
nikapov	0:a1a69d32f310	2237	if (start_not_stopflag != 0) {
sepp_nepp	7:41cbc431e1f4	2238	status = VL53L0X_load_tuning_settings(InterruptThresholdSettings);
nikapov	0:a1a69d32f310	2239	} else {
sepp_nepp	7:41cbc431e1f4	2240	status \|= VL53L0X_write_byte(0xFF,0x04);
sepp_nepp	7:41cbc431e1f4	2241	status \|= VL53L0X_write_byte(0x70,0x00);
sepp_nepp	7:41cbc431e1f4	2242	status \|= VL53L0X_write_byte(0xFF,0x00);
sepp_nepp	7:41cbc431e1f4	2243	status \|= VL53L0X_write_byte(0x80,0x00);
nikapov	0:a1a69d32f310	2244	}
nikapov	0:a1a69d32f310	2245
nikapov	0:a1a69d32f310	2246	}
nikapov	0:a1a69d32f310	2247
nikapov	0:a1a69d32f310	2248
nikapov	0:a1a69d32f310	2249	}
nikapov	0:a1a69d32f310	2250
nikapov	0:a1a69d32f310	2251	return status;
nikapov	0:a1a69d32f310	2252
nikapov	0:a1a69d32f310	2253	}
nikapov	0:a1a69d32f310	2254
sepp_nepp	7:41cbc431e1f4	2255	VL53L0X_Error VL53L0X::VL53L0X_start_measurement()
nikapov	0:a1a69d32f310	2256	{
nikapov	0:a1a69d32f310	2257	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2258	VL53L0X_DeviceModes device_mode;
nikapov	0:a1a69d32f310	2259	uint8_t byte;
nikapov	0:a1a69d32f310	2260	uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
nikapov	0:a1a69d32f310	2261	uint32_t loop_nb;
sepp_nepp	5:b95f6951f7d5	2262
nikapov	0:a1a69d32f310	2263
nikapov	0:a1a69d32f310	2264	/* Get Current DeviceMode */
sepp_nepp	7:41cbc431e1f4	2265	VL53L0X_get_device_mode(&device_mode);
sepp_nepp	7:41cbc431e1f4	2266
sepp_nepp	7:41cbc431e1f4	2267	status = VL53L0X_write_byte(0x80,0x01);
sepp_nepp	7:41cbc431e1f4	2268	status = VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	2269	status = VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	2270	status = VL53L0X_write_byte(0x91,_device-> StopVariable);
sepp_nepp	7:41cbc431e1f4	2271	status = VL53L0X_write_byte(0x00,0x01);
sepp_nepp	7:41cbc431e1f4	2272	status = VL53L0X_write_byte(0xFF,0x00);
sepp_nepp	7:41cbc431e1f4	2273	status = VL53L0X_write_byte(0x80,0x00);
nikapov	0:a1a69d32f310	2274
nikapov	0:a1a69d32f310	2275	switch (device_mode) {
nikapov	0:a1a69d32f310	2276	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
sepp_nepp	7:41cbc431e1f4	2277	status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,0x01);
nikapov	0:a1a69d32f310	2278
nikapov	0:a1a69d32f310	2279	byte = start_stop_byte;
nikapov	0:a1a69d32f310	2280	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2281	/* Wait until start bit has been cleared */
nikapov	0:a1a69d32f310	2282	loop_nb = 0;
nikapov	0:a1a69d32f310	2283	do {
nikapov	0:a1a69d32f310	2284	if (loop_nb > 0)
sepp_nepp	7:41cbc431e1f4	2285	status = VL53L0X_read_byte(VL53L0X_REG_SYSRANGE_START,&byte);
nikapov	0:a1a69d32f310	2286	loop_nb = loop_nb + 1;
nikapov	0:a1a69d32f310	2287	} while (((byte & start_stop_byte) == start_stop_byte)
nikapov	0:a1a69d32f310	2288	&& (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	2289	&& (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
nikapov	0:a1a69d32f310	2290
Davidroid	3:e9269ff624ed	2291	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	2292	status = VL53L0X_ERROR_TIME_OUT;
Davidroid	3:e9269ff624ed	2293	}
nikapov	0:a1a69d32f310	2294
nikapov	0:a1a69d32f310	2295	}
nikapov	0:a1a69d32f310	2296
nikapov	0:a1a69d32f310	2297	break;
nikapov	0:a1a69d32f310	2298	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov	0:a1a69d32f310	2299	/* Back-to-back mode */
nikapov	0:a1a69d32f310	2300
nikapov	0:a1a69d32f310	2301	/* Check if need to apply interrupt settings */
Davidroid	3:e9269ff624ed	2302	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	2303	status = VL53L0X_check_and_load_interrupt_settings(1);
Davidroid	3:e9269ff624ed	2304	}
nikapov	0:a1a69d32f310	2305
sepp_nepp	7:41cbc431e1f4	2306	status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	2307	VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
nikapov	0:a1a69d32f310	2308	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2309	/* Set PAL State to Running */
sepp_nepp	7:41cbc431e1f4	2310	_device->PalState = VL53L0X_STATE_RUNNING;
nikapov	0:a1a69d32f310	2311	}
nikapov	0:a1a69d32f310	2312	break;
nikapov	0:a1a69d32f310	2313	case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov	0:a1a69d32f310	2314	/* Continuous mode */
nikapov	0:a1a69d32f310	2315	/* Check if need to apply interrupt settings */
Davidroid	3:e9269ff624ed	2316	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	2317	status = VL53L0X_check_and_load_interrupt_settings(1);
Davidroid	3:e9269ff624ed	2318	}
nikapov	0:a1a69d32f310	2319
sepp_nepp	7:41cbc431e1f4	2320	status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	2321	VL53L0X_REG_SYSRANGE_MODE_TIMED);
nikapov	0:a1a69d32f310	2322
nikapov	0:a1a69d32f310	2323	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2324	/* Set PAL State to Running */
sepp_nepp	7:41cbc431e1f4	2325	_device->PalState = VL53L0X_STATE_RUNNING;
nikapov	0:a1a69d32f310	2326	}
nikapov	0:a1a69d32f310	2327	break;
nikapov	0:a1a69d32f310	2328	default:
nikapov	0:a1a69d32f310	2329	/* Selected mode not supported */
nikapov	0:a1a69d32f310	2330	status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov	0:a1a69d32f310	2331	}
nikapov	0:a1a69d32f310	2332
nikapov	0:a1a69d32f310	2333
sepp_nepp	5:b95f6951f7d5	2334
nikapov	0:a1a69d32f310	2335	return status;
nikapov	0:a1a69d32f310	2336	}
nikapov	0:a1a69d32f310	2337
nikapov	0:a1a69d32f310	2338	/* Group PAL Measurement Functions */
sepp_nepp	7:41cbc431e1f4	2339	VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement()
nikapov	0:a1a69d32f310	2340	{
nikapov	0:a1a69d32f310	2341	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2342	VL53L0X_DeviceModes device_mode;
nikapov	0:a1a69d32f310	2343
sepp_nepp	5:b95f6951f7d5	2344
nikapov	0:a1a69d32f310	2345
nikapov	0:a1a69d32f310	2346	/* Get Current DeviceMode */
sepp_nepp	7:41cbc431e1f4	2347	status = VL53L0X_get_device_mode(&device_mode);
nikapov	0:a1a69d32f310	2348
nikapov	0:a1a69d32f310	2349	/* Start immediately to run a single ranging measurement in case of
nikapov	0:a1a69d32f310	2350	* single ranging or single histogram */
nikapov	0:a1a69d32f310	2351	if (status == VL53L0X_ERROR_NONE
nikapov	0:a1a69d32f310	2352	&& device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
sepp_nepp	7:41cbc431e1f4	2353	status = VL53L0X_start_measurement();
nikapov	0:a1a69d32f310	2354	}
nikapov	0:a1a69d32f310	2355
nikapov	0:a1a69d32f310	2356
nikapov	0:a1a69d32f310	2357	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	2358	status = VL53L0X_measurement_poll_for_completion();
nikapov	0:a1a69d32f310	2359	}
nikapov	0:a1a69d32f310	2360
nikapov	0:a1a69d32f310	2361
nikapov	0:a1a69d32f310	2362	/* Change PAL State in case of single ranging or single histogram */
nikapov	0:a1a69d32f310	2363	if (status == VL53L0X_ERROR_NONE
Davidroid	3:e9269ff624ed	2364	&& device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
sepp_nepp	7:41cbc431e1f4	2365	_device->PalState = VL53L0X_STATE_IDLE;
Davidroid	3:e9269ff624ed	2366	}
nikapov	0:a1a69d32f310	2367
nikapov	0:a1a69d32f310	2368
sepp_nepp	5:b95f6951f7d5	2369
nikapov	0:a1a69d32f310	2370	return status;
nikapov	0:a1a69d32f310	2371	}
nikapov	0:a1a69d32f310	2372
sepp_nepp	7:41cbc431e1f4	2373	VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(uint8_t *p_x_talk_compensation_enable)
nikapov	0:a1a69d32f310	2374	{
nikapov	0:a1a69d32f310	2375	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2376	uint8_t temp8;
sepp_nepp	5:b95f6951f7d5	2377
nikapov	0:a1a69d32f310	2378
sepp_nepp	7:41cbc431e1f4	2379	temp8 = _device->CurrentParameters.XTalkCompensationEnable;
nikapov	0:a1a69d32f310	2380	*p_x_talk_compensation_enable = temp8;
nikapov	0:a1a69d32f310	2381
sepp_nepp	5:b95f6951f7d5	2382
nikapov	0:a1a69d32f310	2383	return status;
nikapov	0:a1a69d32f310	2384	}
nikapov	0:a1a69d32f310	2385
sepp_nepp	7:41cbc431e1f4	2386	VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	2387	FixPoint1616_t *p_total_xtalk_rate_mcps)
nikapov	0:a1a69d32f310	2388	{
nikapov	0:a1a69d32f310	2389	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2390
nikapov	0:a1a69d32f310	2391	uint8_t xtalk_comp_enable;
nikapov	0:a1a69d32f310	2392	FixPoint1616_t total_xtalk_mega_cps;
nikapov	0:a1a69d32f310	2393	FixPoint1616_t xtalk_per_spad_mega_cps;
nikapov	0:a1a69d32f310	2394
nikapov	0:a1a69d32f310	2395	*p_total_xtalk_rate_mcps = 0;
nikapov	0:a1a69d32f310	2396
sepp_nepp	7:41cbc431e1f4	2397	status = VL53L0X_get_x_talk_compensation_enable(&xtalk_comp_enable);
nikapov	0:a1a69d32f310	2398	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2399
nikapov	0:a1a69d32f310	2400	if (xtalk_comp_enable) {
nikapov	0:a1a69d32f310	2401
sepp_nepp	7:41cbc431e1f4	2402	xtalk_per_spad_mega_cps = _device->CurrentParameters.XTalkCompensationRateMegaCps;
nikapov	0:a1a69d32f310	2403
nikapov	0:a1a69d32f310	2404	/* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
nikapov	0:a1a69d32f310	2405	total_xtalk_mega_cps =
nikapov	0:a1a69d32f310	2406	p_ranging_measurement_data->EffectiveSpadRtnCount *
nikapov	0:a1a69d32f310	2407	xtalk_per_spad_mega_cps;
nikapov	0:a1a69d32f310	2408
nikapov	0:a1a69d32f310	2409	/* FixPoint0824 >> 8 = FixPoint1616 */
nikapov	0:a1a69d32f310	2410	*p_total_xtalk_rate_mcps =
nikapov	0:a1a69d32f310	2411	(total_xtalk_mega_cps + 0x80) >> 8;
nikapov	0:a1a69d32f310	2412	}
nikapov	0:a1a69d32f310	2413	}
nikapov	0:a1a69d32f310	2414
nikapov	0:a1a69d32f310	2415	return status;
nikapov	0:a1a69d32f310	2416	}
nikapov	0:a1a69d32f310	2417
sepp_nepp	7:41cbc431e1f4	2418	VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	2419	FixPoint1616_t *p_total_signal_rate_mcps)
nikapov	0:a1a69d32f310	2420	{
nikapov	0:a1a69d32f310	2421	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2422	FixPoint1616_t total_xtalk_mega_cps;
nikapov	0:a1a69d32f310	2423
sepp_nepp	5:b95f6951f7d5	2424
nikapov	0:a1a69d32f310	2425
nikapov	0:a1a69d32f310	2426	*p_total_signal_rate_mcps =
nikapov	0:a1a69d32f310	2427	p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	2428
sepp_nepp	7:41cbc431e1f4	2429	status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data,&total_xtalk_mega_cps);
nikapov	0:a1a69d32f310	2430
nikapov	0:a1a69d32f310	2431	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2432	*p_total_signal_rate_mcps += total_xtalk_mega_cps;
nikapov	0:a1a69d32f310	2433	}
nikapov	0:a1a69d32f310	2434
nikapov	0:a1a69d32f310	2435	return status;
nikapov	0:a1a69d32f310	2436	}
nikapov	0:a1a69d32f310	2437
nikapov	0:a1a69d32f310	2438	/* To convert ms into register value */
sepp_nepp	7:41cbc431e1f4	2439	uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(uint32_t timeout_period_us,
nikapov	0:a1a69d32f310	2440	uint8_t vcsel_period_pclks)
nikapov	0:a1a69d32f310	2441	{
nikapov	0:a1a69d32f310	2442	uint32_t macro_period_ps;
nikapov	0:a1a69d32f310	2443	uint32_t macro_period_ns;
nikapov	0:a1a69d32f310	2444	uint32_t timeout_period_mclks = 0;
nikapov	0:a1a69d32f310	2445
sepp_nepp	7:41cbc431e1f4	2446	macro_period_ps = VL53L0X_calc_macro_period_ps(vcsel_period_pclks);
nikapov	0:a1a69d32f310	2447	macro_period_ns = (macro_period_ps + 500) / 1000;
nikapov	0:a1a69d32f310	2448
nikapov	0:a1a69d32f310	2449	timeout_period_mclks =
nikapov	0:a1a69d32f310	2450	(uint32_t)(((timeout_period_us * 1000)
nikapov	0:a1a69d32f310	2451	+ (macro_period_ns / 2)) / macro_period_ns);
nikapov	0:a1a69d32f310	2452
nikapov	0:a1a69d32f310	2453	return timeout_period_mclks;
nikapov	0:a1a69d32f310	2454	}
nikapov	0:a1a69d32f310	2455
nikapov	0:a1a69d32f310	2456	uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
nikapov	0:a1a69d32f310	2457	{
nikapov	0:a1a69d32f310	2458	/*
nikapov	0:a1a69d32f310	2459	* Implements an integer square root
nikapov	0:a1a69d32f310	2460	*
nikapov	0:a1a69d32f310	2461	* From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
nikapov	0:a1a69d32f310	2462	*/
nikapov	0:a1a69d32f310	2463
nikapov	0:a1a69d32f310	2464	uint32_t res = 0;
nikapov	0:a1a69d32f310	2465	uint32_t bit = 1 << 30;
nikapov	0:a1a69d32f310	2466	/* The second-to-top bit is set:
sepp_nepp	7:41cbc431e1f4	2467	* 1 << 14 for 16-bits,1 << 30 for 32 bits */
nikapov	0:a1a69d32f310	2468
nikapov	0:a1a69d32f310	2469	/* "bit" starts at the highest power of four <= the argument. */
nikapov	0:a1a69d32f310	2470	while (bit > num) {
nikapov	0:a1a69d32f310	2471	bit >>= 2;
nikapov	0:a1a69d32f310	2472	}
nikapov	0:a1a69d32f310	2473
nikapov	0:a1a69d32f310	2474
nikapov	0:a1a69d32f310	2475	while (bit != 0) {
nikapov	0:a1a69d32f310	2476	if (num >= res + bit) {
nikapov	0:a1a69d32f310	2477	num -= res + bit;
nikapov	0:a1a69d32f310	2478	res = (res >> 1) + bit;
Davidroid	3:e9269ff624ed	2479	} else {
nikapov	0:a1a69d32f310	2480	res >>= 1;
Davidroid	3:e9269ff624ed	2481	}
nikapov	0:a1a69d32f310	2482
nikapov	0:a1a69d32f310	2483	bit >>= 2;
nikapov	0:a1a69d32f310	2484	}
nikapov	0:a1a69d32f310	2485
nikapov	0:a1a69d32f310	2486	return res;
nikapov	0:a1a69d32f310	2487	}
nikapov	0:a1a69d32f310	2488
sepp_nepp	7:41cbc431e1f4	2489	VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(FixPoint1616_t total_signal_rate_mcps,
nikapov	0:a1a69d32f310	2490	FixPoint1616_t total_corr_signal_rate_mcps,
nikapov	0:a1a69d32f310	2491	FixPoint1616_t pw_mult,
nikapov	0:a1a69d32f310	2492	uint32_t sigma_estimate_p1,
nikapov	0:a1a69d32f310	2493	FixPoint1616_t sigma_estimate_p2,
nikapov	0:a1a69d32f310	2494	uint32_t peak_vcsel_duration_us,
nikapov	0:a1a69d32f310	2495	uint32_t *pd_max_mm)
nikapov	0:a1a69d32f310	2496	{
nikapov	0:a1a69d32f310	2497	const uint32_t c_sigma_limit = 18;
nikapov	0:a1a69d32f310	2498	const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
nikapov	0:a1a69d32f310	2499	const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */
nikapov	0:a1a69d32f310	2500	const uint32_t c_amb_eff_width_sigma_est_ns = 6;
nikapov	0:a1a69d32f310	2501	const uint32_t c_amb_eff_width_d_max_ns = 7;
nikapov	0:a1a69d32f310	2502	uint32_t dmax_cal_range_mm;
nikapov	0:a1a69d32f310	2503	FixPoint1616_t dmax_cal_signal_rate_rtn_mcps;
nikapov	0:a1a69d32f310	2504	FixPoint1616_t min_signal_needed;
nikapov	0:a1a69d32f310	2505	FixPoint1616_t min_signal_needed_p1;
nikapov	0:a1a69d32f310	2506	FixPoint1616_t min_signal_needed_p2;
nikapov	0:a1a69d32f310	2507	FixPoint1616_t min_signal_needed_p3;
nikapov	0:a1a69d32f310	2508	FixPoint1616_t min_signal_needed_p4;
nikapov	0:a1a69d32f310	2509	FixPoint1616_t sigma_limit_tmp;
nikapov	0:a1a69d32f310	2510	FixPoint1616_t sigma_est_sq_tmp;
nikapov	0:a1a69d32f310	2511	FixPoint1616_t signal_limit_tmp;
nikapov	0:a1a69d32f310	2512	FixPoint1616_t signal_at0_mm;
nikapov	0:a1a69d32f310	2513	FixPoint1616_t dmax_dark;
nikapov	0:a1a69d32f310	2514	FixPoint1616_t dmax_ambient;
nikapov	0:a1a69d32f310	2515	FixPoint1616_t dmax_dark_tmp;
nikapov	0:a1a69d32f310	2516	FixPoint1616_t sigma_est_p2_tmp;
nikapov	0:a1a69d32f310	2517	uint32_t signal_rate_temp_mcps;
nikapov	0:a1a69d32f310	2518
nikapov	0:a1a69d32f310	2519	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2520
sepp_nepp	5:b95f6951f7d5	2521
nikapov	0:a1a69d32f310	2522
nikapov	0:a1a69d32f310	2523	dmax_cal_range_mm =
sepp_nepp	7:41cbc431e1f4	2524	_device-> DmaxCalRangeMilliMeter;
nikapov	0:a1a69d32f310	2525
nikapov	0:a1a69d32f310	2526	dmax_cal_signal_rate_rtn_mcps =
sepp_nepp	7:41cbc431e1f4	2527	_device-> DmaxCalSignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	2528
nikapov	0:a1a69d32f310	2529	/* uint32 * FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	2530	signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
nikapov	0:a1a69d32f310	2531
nikapov	0:a1a69d32f310	2532	/* FixPoint1616 >> 8 = FixPoint2408 */
nikapov	0:a1a69d32f310	2533	signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
nikapov	0:a1a69d32f310	2534	signal_at0_mm *= dmax_cal_range_mm;
nikapov	0:a1a69d32f310	2535
nikapov	0:a1a69d32f310	2536	min_signal_needed_p1 = 0;
nikapov	0:a1a69d32f310	2537	if (total_corr_signal_rate_mcps > 0) {
nikapov	0:a1a69d32f310	2538
nikapov	0:a1a69d32f310	2539	/* Shift by 10 bits to increase resolution prior to the
nikapov	0:a1a69d32f310	2540	* division */
nikapov	0:a1a69d32f310	2541	signal_rate_temp_mcps = total_signal_rate_mcps << 10;
nikapov	0:a1a69d32f310	2542
nikapov	0:a1a69d32f310	2543	/* Add rounding value prior to division */
nikapov	0:a1a69d32f310	2544	min_signal_needed_p1 = signal_rate_temp_mcps +
nikapov	0:a1a69d32f310	2545	(total_corr_signal_rate_mcps / 2);
nikapov	0:a1a69d32f310	2546
nikapov	0:a1a69d32f310	2547	/* FixPoint0626/FixPoint1616 = FixPoint2210 */
nikapov	0:a1a69d32f310	2548	min_signal_needed_p1 /= total_corr_signal_rate_mcps;
nikapov	0:a1a69d32f310	2549
nikapov	0:a1a69d32f310	2550	/* Apply a factored version of the speed of light.
nikapov	0:a1a69d32f310	2551	Correction to be applied at the end */
nikapov	0:a1a69d32f310	2552	min_signal_needed_p1 *= 3;
nikapov	0:a1a69d32f310	2553
nikapov	0:a1a69d32f310	2554	/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
nikapov	0:a1a69d32f310	2555	min_signal_needed_p1 *= min_signal_needed_p1;
nikapov	0:a1a69d32f310	2556
nikapov	0:a1a69d32f310	2557	/* FixPoint1220 >> 16 = FixPoint2804 */
nikapov	0:a1a69d32f310	2558	min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2559	}
nikapov	0:a1a69d32f310	2560
nikapov	0:a1a69d32f310	2561	min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
nikapov	0:a1a69d32f310	2562
nikapov	0:a1a69d32f310	2563	/* FixPoint1616 >> 16 = uint32 */
nikapov	0:a1a69d32f310	2564	min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2565
nikapov	0:a1a69d32f310	2566	/* uint32 * uint32 = uint32 */
nikapov	0:a1a69d32f310	2567	min_signal_needed_p2 *= min_signal_needed_p2;
nikapov	0:a1a69d32f310	2568
nikapov	0:a1a69d32f310	2569	/* Check sigmaEstimateP2
nikapov	0:a1a69d32f310	2570	* If this value is too high there is not enough signal rate
nikapov	0:a1a69d32f310	2571	* to calculate dmax value so set a suitable value to ensure
nikapov	0:a1a69d32f310	2572	* a very small dmax.
nikapov	0:a1a69d32f310	2573	*/
nikapov	0:a1a69d32f310	2574	sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2575	sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
nikapov	0:a1a69d32f310	2576	c_amb_eff_width_sigma_est_ns;
nikapov	0:a1a69d32f310	2577	sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
nikapov	0:a1a69d32f310	2578
nikapov	0:a1a69d32f310	2579	if (sigma_est_p2_tmp > 0xffff) {
nikapov	0:a1a69d32f310	2580	min_signal_needed_p3 = 0xfff00000;
nikapov	0:a1a69d32f310	2581	} else {
nikapov	0:a1a69d32f310	2582
sepp_nepp	7:41cbc431e1f4	2583	/* DMAX uses a different ambient width from sigma,so apply
nikapov	0:a1a69d32f310	2584	* correction.
nikapov	0:a1a69d32f310	2585	* Perform division before multiplication to prevent overflow.
nikapov	0:a1a69d32f310	2586	*/
nikapov	0:a1a69d32f310	2587	sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
nikapov	0:a1a69d32f310	2588	c_amb_eff_width_sigma_est_ns;
nikapov	0:a1a69d32f310	2589	sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
nikapov	0:a1a69d32f310	2590
nikapov	0:a1a69d32f310	2591	/* FixPoint1616 >> 16 = uint32 */
nikapov	0:a1a69d32f310	2592	min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2593
nikapov	0:a1a69d32f310	2594	min_signal_needed_p3 *= min_signal_needed_p3;
nikapov	0:a1a69d32f310	2595
nikapov	0:a1a69d32f310	2596	}
nikapov	0:a1a69d32f310	2597
nikapov	0:a1a69d32f310	2598	/* FixPoint1814 / uint32 = FixPoint1814 */
nikapov	0:a1a69d32f310	2599	sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
nikapov	0:a1a69d32f310	2600
nikapov	0:a1a69d32f310	2601	/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
nikapov	0:a1a69d32f310	2602	sigma_limit_tmp *= sigma_limit_tmp;
nikapov	0:a1a69d32f310	2603
nikapov	0:a1a69d32f310	2604	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	2605	sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
nikapov	0:a1a69d32f310	2606
nikapov	0:a1a69d32f310	2607	/* FixPoint3232 >> 4 = FixPoint0428 */
nikapov	0:a1a69d32f310	2608	sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
nikapov	0:a1a69d32f310	2609
nikapov	0:a1a69d32f310	2610	/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
nikapov	0:a1a69d32f310	2611	sigma_limit_tmp -= sigma_est_sq_tmp;
nikapov	0:a1a69d32f310	2612
nikapov	0:a1a69d32f310	2613	/* uint32_t * FixPoint0428 = FixPoint0428 */
nikapov	0:a1a69d32f310	2614	min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
nikapov	0:a1a69d32f310	2615
nikapov	0:a1a69d32f310	2616	/* FixPoint0428 >> 14 = FixPoint1814 */
nikapov	0:a1a69d32f310	2617	min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14;
nikapov	0:a1a69d32f310	2618
nikapov	0:a1a69d32f310	2619	/* uint32 + uint32 = uint32 */
nikapov	0:a1a69d32f310	2620	min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3);
nikapov	0:a1a69d32f310	2621
nikapov	0:a1a69d32f310	2622	/* uint32 / uint32 = uint32 */
nikapov	0:a1a69d32f310	2623	min_signal_needed += (peak_vcsel_duration_us / 2);
nikapov	0:a1a69d32f310	2624	min_signal_needed /= peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	2625
nikapov	0:a1a69d32f310	2626	/* uint32 << 14 = FixPoint1814 */
nikapov	0:a1a69d32f310	2627	min_signal_needed <<= 14;
nikapov	0:a1a69d32f310	2628
nikapov	0:a1a69d32f310	2629	/* FixPoint1814 / FixPoint1814 = uint32 */
nikapov	0:a1a69d32f310	2630	min_signal_needed += (min_signal_needed_p4 / 2);
nikapov	0:a1a69d32f310	2631	min_signal_needed /= min_signal_needed_p4;
nikapov	0:a1a69d32f310	2632
nikapov	0:a1a69d32f310	2633	/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
nikapov	0:a1a69d32f310	2634	min_signal_needed *= min_signal_needed_p1;
nikapov	0:a1a69d32f310	2635
nikapov	0:a1a69d32f310	2636	/* Apply correction by dividing by 1000000.
nikapov	0:a1a69d32f310	2637	* This assumes 10E16 on the numerator of the equation
nikapov	0:a1a69d32f310	2638	* and 10E-22 on the denominator.
nikapov	0:a1a69d32f310	2639	* We do this because 32bit fix point calculation can't
nikapov	0:a1a69d32f310	2640	* handle the larger and smaller elements of this equation,
nikapov	0:a1a69d32f310	2641	* i.e. speed of light and pulse widths.
nikapov	0:a1a69d32f310	2642	*/
nikapov	0:a1a69d32f310	2643	min_signal_needed = (min_signal_needed + 500) / 1000;
nikapov	0:a1a69d32f310	2644	min_signal_needed <<= 4;
nikapov	0:a1a69d32f310	2645
nikapov	0:a1a69d32f310	2646	min_signal_needed = (min_signal_needed + 500) / 1000;
nikapov	0:a1a69d32f310	2647
nikapov	0:a1a69d32f310	2648	/* FixPoint1616 >> 8 = FixPoint2408 */
nikapov	0:a1a69d32f310	2649	signal_limit_tmp = (c_signal_limit + 0x80) >> 8;
nikapov	0:a1a69d32f310	2650
nikapov	0:a1a69d32f310	2651	/* FixPoint2408/FixPoint2408 = uint32 */
nikapov	0:a1a69d32f310	2652	if (signal_limit_tmp != 0) {
nikapov	0:a1a69d32f310	2653	dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
nikapov	0:a1a69d32f310	2654	/ signal_limit_tmp;
nikapov	0:a1a69d32f310	2655	} else {
nikapov	0:a1a69d32f310	2656	dmax_dark_tmp = 0;
nikapov	0:a1a69d32f310	2657	}
nikapov	0:a1a69d32f310	2658
nikapov	0:a1a69d32f310	2659	dmax_dark = VL53L0X_isqrt(dmax_dark_tmp);
nikapov	0:a1a69d32f310	2660
nikapov	0:a1a69d32f310	2661	/* FixPoint2408/FixPoint2408 = uint32 */
nikapov	0:a1a69d32f310	2662	if (min_signal_needed != 0) {
nikapov	0:a1a69d32f310	2663	dmax_ambient = (signal_at0_mm + min_signal_needed / 2)
nikapov	0:a1a69d32f310	2664	/ min_signal_needed;
nikapov	0:a1a69d32f310	2665	} else {
nikapov	0:a1a69d32f310	2666	dmax_ambient = 0;
nikapov	0:a1a69d32f310	2667	}
nikapov	0:a1a69d32f310	2668
nikapov	0:a1a69d32f310	2669	dmax_ambient = VL53L0X_isqrt(dmax_ambient);
nikapov	0:a1a69d32f310	2670
nikapov	0:a1a69d32f310	2671	*pd_max_mm = dmax_dark;
nikapov	0:a1a69d32f310	2672	if (dmax_dark > dmax_ambient) {
nikapov	0:a1a69d32f310	2673	*pd_max_mm = dmax_ambient;
nikapov	0:a1a69d32f310	2674	}
nikapov	0:a1a69d32f310	2675
sepp_nepp	5:b95f6951f7d5	2676
nikapov	0:a1a69d32f310	2677
nikapov	0:a1a69d32f310	2678	return status;
nikapov	0:a1a69d32f310	2679	}
nikapov	0:a1a69d32f310	2680
sepp_nepp	7:41cbc431e1f4	2681	VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	2682	FixPoint1616_t *p_sigma_estimate,
nikapov	0:a1a69d32f310	2683	uint32_t *p_dmax_mm)
nikapov	0:a1a69d32f310	2684	{
sepp_nepp	7:41cbc431e1f4	2685	/* Expressed in 100ths of a ns,i.e. centi-ns */
nikapov	0:a1a69d32f310	2686	const uint32_t c_pulse_effective_width_centi_ns = 800;
sepp_nepp	7:41cbc431e1f4	2687	/* Expressed in 100ths of a ns,i.e. centi-ns */
nikapov	0:a1a69d32f310	2688	const uint32_t c_ambient_effective_width_centi_ns = 600;
nikapov	0:a1a69d32f310	2689	const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
nikapov	0:a1a69d32f310	2690	const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
nikapov	0:a1a69d32f310	2691	const FixPoint1616_t c_sigma_est_max = 0x028F87AE;
nikapov	0:a1a69d32f310	2692	const FixPoint1616_t c_sigma_est_rtn_max = 0xF000;
nikapov	0:a1a69d32f310	2693	const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
nikapov	0:a1a69d32f310	2694	c_ambient_effective_width_centi_ns;
nikapov	0:a1a69d32f310	2695	/* Time Of Flight per mm (6.6 pico secs) */
nikapov	0:a1a69d32f310	2696	const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A;
nikapov	0:a1a69d32f310	2697	const uint32_t c_16bit_rounding_param = 0x00008000;
nikapov	0:a1a69d32f310	2698	const FixPoint1616_t c_max_x_talk_kcps = 0x00320000;
nikapov	0:a1a69d32f310	2699	const uint32_t c_pll_period_ps = 1655;
nikapov	0:a1a69d32f310	2700
nikapov	0:a1a69d32f310	2701	uint32_t vcsel_total_events_rtn;
nikapov	0:a1a69d32f310	2702	uint32_t final_range_timeout_micro_secs;
nikapov	0:a1a69d32f310	2703	uint32_t pre_range_timeout_micro_secs;
nikapov	0:a1a69d32f310	2704	uint32_t final_range_integration_time_milli_secs;
nikapov	0:a1a69d32f310	2705	FixPoint1616_t sigma_estimate_p1;
nikapov	0:a1a69d32f310	2706	FixPoint1616_t sigma_estimate_p2;
nikapov	0:a1a69d32f310	2707	FixPoint1616_t sigma_estimate_p3;
nikapov	0:a1a69d32f310	2708	FixPoint1616_t delta_t_ps;
nikapov	0:a1a69d32f310	2709	FixPoint1616_t pw_mult;
nikapov	0:a1a69d32f310	2710	FixPoint1616_t sigma_est_rtn;
nikapov	0:a1a69d32f310	2711	FixPoint1616_t sigma_estimate;
nikapov	0:a1a69d32f310	2712	FixPoint1616_t x_talk_correction;
nikapov	0:a1a69d32f310	2713	FixPoint1616_t ambient_rate_kcps;
nikapov	0:a1a69d32f310	2714	FixPoint1616_t peak_signal_rate_kcps;
nikapov	0:a1a69d32f310	2715	FixPoint1616_t x_talk_comp_rate_mcps;
nikapov	0:a1a69d32f310	2716	uint32_t x_talk_comp_rate_kcps;
nikapov	0:a1a69d32f310	2717	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2718	FixPoint1616_t diff1_mcps;
nikapov	0:a1a69d32f310	2719	FixPoint1616_t diff2_mcps;
nikapov	0:a1a69d32f310	2720	FixPoint1616_t sqr1;
nikapov	0:a1a69d32f310	2721	FixPoint1616_t sqr2;
nikapov	0:a1a69d32f310	2722	FixPoint1616_t sqr_sum;
nikapov	0:a1a69d32f310	2723	FixPoint1616_t sqrt_result_centi_ns;
nikapov	0:a1a69d32f310	2724	FixPoint1616_t sqrt_result;
nikapov	0:a1a69d32f310	2725	FixPoint1616_t total_signal_rate_mcps;
nikapov	0:a1a69d32f310	2726	FixPoint1616_t corrected_signal_rate_mcps;
nikapov	0:a1a69d32f310	2727	FixPoint1616_t sigma_est_ref;
nikapov	0:a1a69d32f310	2728	uint32_t vcsel_width;
nikapov	0:a1a69d32f310	2729	uint32_t final_range_macro_pclks;
nikapov	0:a1a69d32f310	2730	uint32_t pre_range_macro_pclks;
nikapov	0:a1a69d32f310	2731	uint32_t peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	2732	uint8_t final_range_vcsel_pclks;
nikapov	0:a1a69d32f310	2733	uint8_t pre_range_vcsel_pclks;
nikapov	0:a1a69d32f310	2734	/*! \addtogroup calc_sigma_estimate
nikapov	0:a1a69d32f310	2735	* @{
nikapov	0:a1a69d32f310	2736	*
nikapov	0:a1a69d32f310	2737	* Estimates the range sigma
nikapov	0:a1a69d32f310	2738	*/
nikapov	0:a1a69d32f310	2739
sepp_nepp	5:b95f6951f7d5	2740
nikapov	0:a1a69d32f310	2741
sepp_nepp	7:41cbc431e1f4	2742	x_talk_comp_rate_mcps = _device->CurrentParameters.XTalkCompensationRateMegaCps;
nikapov	0:a1a69d32f310	2743
nikapov	0:a1a69d32f310	2744	/*
nikapov	0:a1a69d32f310	2745	* We work in kcps rather than mcps as this helps keep within the
nikapov	0:a1a69d32f310	2746	* confines of the 32 Fix1616 type.
nikapov	0:a1a69d32f310	2747	*/
nikapov	0:a1a69d32f310	2748
nikapov	0:a1a69d32f310	2749	ambient_rate_kcps =
nikapov	0:a1a69d32f310	2750	(p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16;
nikapov	0:a1a69d32f310	2751
nikapov	0:a1a69d32f310	2752	corrected_signal_rate_mcps =
nikapov	0:a1a69d32f310	2753	p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	2754
nikapov	0:a1a69d32f310	2755
sepp_nepp	7:41cbc431e1f4	2756	status = VL53L0X_get_total_signal_rate(p_ranging_measurement_data,&total_signal_rate_mcps);
sepp_nepp	7:41cbc431e1f4	2757	status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data,&x_talk_comp_rate_mcps);
nikapov	0:a1a69d32f310	2758
nikapov	0:a1a69d32f310	2759
nikapov	0:a1a69d32f310	2760	/* Signal rate measurement provided by device is the
sepp_nepp	7:41cbc431e1f4	2761	* peak signal rate,not average.
nikapov	0:a1a69d32f310	2762	*/
nikapov	0:a1a69d32f310	2763	peak_signal_rate_kcps = (total_signal_rate_mcps * 1000);
nikapov	0:a1a69d32f310	2764	peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2765
nikapov	0:a1a69d32f310	2766	x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
nikapov	0:a1a69d32f310	2767
nikapov	0:a1a69d32f310	2768	if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) {
nikapov	0:a1a69d32f310	2769	x_talk_comp_rate_kcps = c_max_x_talk_kcps;
nikapov	0:a1a69d32f310	2770	}
nikapov	0:a1a69d32f310	2771
nikapov	0:a1a69d32f310	2772	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2773
nikapov	0:a1a69d32f310	2774	/* Calculate final range macro periods */
sepp_nepp	7:41cbc431e1f4	2775	final_range_timeout_micro_secs = _device->DeviceSpecificParameters.FinalRangeTimeoutMicroSecs;
sepp_nepp	7:41cbc431e1f4	2776
sepp_nepp	7:41cbc431e1f4	2777	final_range_vcsel_pclks = _device->DeviceSpecificParameters.FinalRangeVcselPulsePeriod;
sepp_nepp	7:41cbc431e1f4	2778
sepp_nepp	7:41cbc431e1f4	2779	final_range_macro_pclks = VL53L0X_calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks);
nikapov	0:a1a69d32f310	2780
nikapov	0:a1a69d32f310	2781	/* Calculate pre-range macro periods */
sepp_nepp	7:41cbc431e1f4	2782	pre_range_timeout_micro_secs = _device->DeviceSpecificParameters.PreRangeTimeoutMicroSecs;
sepp_nepp	7:41cbc431e1f4	2783
sepp_nepp	7:41cbc431e1f4	2784	pre_range_vcsel_pclks = _device->DeviceSpecificParameters.PreRangeVcselPulsePeriod;
sepp_nepp	7:41cbc431e1f4	2785
sepp_nepp	7:41cbc431e1f4	2786	pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks);
nikapov	0:a1a69d32f310	2787
nikapov	0:a1a69d32f310	2788	vcsel_width = 3;
nikapov	0:a1a69d32f310	2789	if (final_range_vcsel_pclks == 8) {
nikapov	0:a1a69d32f310	2790	vcsel_width = 2;
nikapov	0:a1a69d32f310	2791	}
nikapov	0:a1a69d32f310	2792
nikapov	0:a1a69d32f310	2793
nikapov	0:a1a69d32f310	2794	peak_vcsel_duration_us = vcsel_width * 2048 *
nikapov	0:a1a69d32f310	2795	(pre_range_macro_pclks + final_range_macro_pclks);
nikapov	0:a1a69d32f310	2796	peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov	0:a1a69d32f310	2797	peak_vcsel_duration_us *= c_pll_period_ps;
nikapov	0:a1a69d32f310	2798	peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov	0:a1a69d32f310	2799
nikapov	0:a1a69d32f310	2800	/* Fix1616 >> 8 = Fix2408 */
nikapov	0:a1a69d32f310	2801	total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8;
nikapov	0:a1a69d32f310	2802
nikapov	0:a1a69d32f310	2803	/* Fix2408 * uint32 = Fix2408 */
nikapov	0:a1a69d32f310	2804	vcsel_total_events_rtn = total_signal_rate_mcps *
nikapov	0:a1a69d32f310	2805	peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	2806
nikapov	0:a1a69d32f310	2807	/* Fix2408 >> 8 = uint32 */
nikapov	0:a1a69d32f310	2808	vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
nikapov	0:a1a69d32f310	2809
nikapov	0:a1a69d32f310	2810	/* Fix2408 << 8 = Fix1616 = */
nikapov	0:a1a69d32f310	2811	total_signal_rate_mcps <<= 8;
nikapov	0:a1a69d32f310	2812	}
nikapov	0:a1a69d32f310	2813
nikapov	0:a1a69d32f310	2814	if (status != VL53L0X_ERROR_NONE) {
sepp_nepp	5:b95f6951f7d5	2815
nikapov	0:a1a69d32f310	2816	return status;
nikapov	0:a1a69d32f310	2817	}
nikapov	0:a1a69d32f310	2818
nikapov	0:a1a69d32f310	2819	if (peak_signal_rate_kcps == 0) {
nikapov	0:a1a69d32f310	2820	*p_sigma_estimate = c_sigma_est_max;
sepp_nepp	7:41cbc431e1f4	2821	_device->SigmaEstimate = c_sigma_est_max;
nikapov	0:a1a69d32f310	2822	*p_dmax_mm = 0;
nikapov	0:a1a69d32f310	2823	} else {
nikapov	0:a1a69d32f310	2824	if (vcsel_total_events_rtn < 1) {
nikapov	0:a1a69d32f310	2825	vcsel_total_events_rtn = 1;
nikapov	0:a1a69d32f310	2826	}
nikapov	0:a1a69d32f310	2827
nikapov	0:a1a69d32f310	2828	sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
nikapov	0:a1a69d32f310	2829
nikapov	0:a1a69d32f310	2830	/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	2831	sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps;
nikapov	0:a1a69d32f310	2832	if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) {
nikapov	0:a1a69d32f310	2833	/* Clip to prevent overflow. Will ensure safe
nikapov	0:a1a69d32f310	2834	* max result. */
nikapov	0:a1a69d32f310	2835	sigma_estimate_p2 = c_amb_to_signal_ratio_max;
nikapov	0:a1a69d32f310	2836	}
nikapov	0:a1a69d32f310	2837	sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
nikapov	0:a1a69d32f310	2838
nikapov	0:a1a69d32f310	2839	sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12);
nikapov	0:a1a69d32f310	2840
nikapov	0:a1a69d32f310	2841	/* uint32 * FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	2842	delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
nikapov	0:a1a69d32f310	2843	c_tof_per_mm_ps;
nikapov	0:a1a69d32f310	2844
nikapov	0:a1a69d32f310	2845	/*
nikapov	0:a1a69d32f310	2846	* vcselRate - xtalkCompRate
nikapov	0:a1a69d32f310	2847	* (uint32 << 16) - FixPoint1616 = FixPoint1616.
nikapov	0:a1a69d32f310	2848	* Divide result by 1000 to convert to mcps.
nikapov	0:a1a69d32f310	2849	* 500 is added to ensure rounding when integer division
nikapov	0:a1a69d32f310	2850	* truncates.
nikapov	0:a1a69d32f310	2851	*/
nikapov	0:a1a69d32f310	2852	diff1_mcps = (((peak_signal_rate_kcps << 16) -
nikapov	0:a1a69d32f310	2853	2 * x_talk_comp_rate_kcps) + 500) / 1000;
nikapov	0:a1a69d32f310	2854
nikapov	0:a1a69d32f310	2855	/* vcselRate + xtalkCompRate */
nikapov	0:a1a69d32f310	2856	diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000;
nikapov	0:a1a69d32f310	2857
nikapov	0:a1a69d32f310	2858	/* Shift by 8 bits to increase resolution prior to the
nikapov	0:a1a69d32f310	2859	* division */
nikapov	0:a1a69d32f310	2860	diff1_mcps <<= 8;
nikapov	0:a1a69d32f310	2861
nikapov	0:a1a69d32f310	2862	/* FixPoint0824/FixPoint1616 = FixPoint2408 */
nikapov	0:a1a69d32f310	2863	// xTalkCorrection = abs(diff1_mcps/diff2_mcps);
sepp_nepp	7:41cbc431e1f4	2864	// abs is causing compiler overloading isue in C++,but unsigned types. So,redundant call anyway!
nikapov	0:a1a69d32f310	2865	x_talk_correction = diff1_mcps / diff2_mcps;
nikapov	0:a1a69d32f310	2866
nikapov	0:a1a69d32f310	2867	/* FixPoint2408 << 8 = FixPoint1616 */
nikapov	0:a1a69d32f310	2868	x_talk_correction <<= 8;
nikapov	0:a1a69d32f310	2869
nikapov	0:a1a69d32f310	2870	if (p_ranging_measurement_data->RangeStatus != 0) {
nikapov	0:a1a69d32f310	2871	pw_mult = 1 << 16;
nikapov	0:a1a69d32f310	2872	} else {
nikapov	0:a1a69d32f310	2873	/* FixPoint1616/uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	2874	pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
nikapov	0:a1a69d32f310	2875
nikapov	0:a1a69d32f310	2876	/*
sepp_nepp	7:41cbc431e1f4	2877	* FixPoint1616 * FixPoint1616 = FixPoint3232,however both
nikapov	0:a1a69d32f310	2878	* values are small enough such that32 bits will not be
nikapov	0:a1a69d32f310	2879	* exceeded.
nikapov	0:a1a69d32f310	2880	*/
nikapov	0:a1a69d32f310	2881	pw_mult *= ((1 << 16) - x_talk_correction);
nikapov	0:a1a69d32f310	2882
nikapov	0:a1a69d32f310	2883	/* (FixPoint3232 >> 16) = FixPoint1616 */
nikapov	0:a1a69d32f310	2884	pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
nikapov	0:a1a69d32f310	2885
nikapov	0:a1a69d32f310	2886	/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	2887	pw_mult += (1 << 16);
nikapov	0:a1a69d32f310	2888
nikapov	0:a1a69d32f310	2889	/*
sepp_nepp	7:41cbc431e1f4	2890	* At this point the value will be 1.xx,therefore if we square
nikapov	0:a1a69d32f310	2891	* the value this will exceed 32 bits. To address this perform
nikapov	0:a1a69d32f310	2892	* a single shift to the right before the multiplication.
nikapov	0:a1a69d32f310	2893	*/
nikapov	0:a1a69d32f310	2894	pw_mult >>= 1;
nikapov	0:a1a69d32f310	2895	/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
nikapov	0:a1a69d32f310	2896	pw_mult = pw_mult * pw_mult;
nikapov	0:a1a69d32f310	2897
nikapov	0:a1a69d32f310	2898	/* (FixPoint3430 >> 14) = Fix1616 */
nikapov	0:a1a69d32f310	2899	pw_mult >>= 14;
nikapov	0:a1a69d32f310	2900	}
nikapov	0:a1a69d32f310	2901
nikapov	0:a1a69d32f310	2902	/* FixPoint1616 * uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	2903	sqr1 = pw_mult * sigma_estimate_p1;
nikapov	0:a1a69d32f310	2904
nikapov	0:a1a69d32f310	2905	/* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov	0:a1a69d32f310	2906	sqr1 = (sqr1 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2907
nikapov	0:a1a69d32f310	2908	/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov	0:a1a69d32f310	2909	sqr1 *= sqr1;
nikapov	0:a1a69d32f310	2910
nikapov	0:a1a69d32f310	2911	sqr2 = sigma_estimate_p2;
nikapov	0:a1a69d32f310	2912
nikapov	0:a1a69d32f310	2913	/* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov	0:a1a69d32f310	2914	sqr2 = (sqr2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2915
nikapov	0:a1a69d32f310	2916	/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov	0:a1a69d32f310	2917	sqr2 *= sqr2;
nikapov	0:a1a69d32f310	2918
nikapov	0:a1a69d32f310	2919	/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
nikapov	0:a1a69d32f310	2920	sqr_sum = sqr1 + sqr2;
nikapov	0:a1a69d32f310	2921
nikapov	0:a1a69d32f310	2922	/* SQRT(FixPoin6400) = FixPoint3200 */
nikapov	0:a1a69d32f310	2923	sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum);
nikapov	0:a1a69d32f310	2924
nikapov	0:a1a69d32f310	2925	/* (FixPoint3200 << 16) = FixPoint1616 */
nikapov	0:a1a69d32f310	2926	sqrt_result_centi_ns <<= 16;
nikapov	0:a1a69d32f310	2927
nikapov	0:a1a69d32f310	2928	/*
nikapov	0:a1a69d32f310	2929	* Note that the Speed Of Light is expressed in um per 1E-10
nikapov	0:a1a69d32f310	2930	* seconds (2997) Therefore to get mm/ns we have to divide by
nikapov	0:a1a69d32f310	2931	* 10000
nikapov	0:a1a69d32f310	2932	*/
nikapov	0:a1a69d32f310	2933	sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
nikapov	0:a1a69d32f310	2934	sigma_estimate_p3);
nikapov	0:a1a69d32f310	2935	sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
nikapov	0:a1a69d32f310	2936
nikapov	0:a1a69d32f310	2937	/* Add 5000 before dividing by 10000 to ensure rounding. */
nikapov	0:a1a69d32f310	2938	sigma_est_rtn += 5000;
nikapov	0:a1a69d32f310	2939	sigma_est_rtn /= 10000;
nikapov	0:a1a69d32f310	2940
nikapov	0:a1a69d32f310	2941	if (sigma_est_rtn > c_sigma_est_rtn_max) {
nikapov	0:a1a69d32f310	2942	/* Clip to prevent overflow. Will ensure safe
nikapov	0:a1a69d32f310	2943	* max result. */
nikapov	0:a1a69d32f310	2944	sigma_est_rtn = c_sigma_est_rtn_max;
nikapov	0:a1a69d32f310	2945	}
nikapov	0:a1a69d32f310	2946	final_range_integration_time_milli_secs =
nikapov	0:a1a69d32f310	2947	(final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
nikapov	0:a1a69d32f310	2948
nikapov	0:a1a69d32f310	2949	/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
nikapov	0:a1a69d32f310	2950	* sqrt(FixPoint1616/int) = FixPoint2408)
nikapov	0:a1a69d32f310	2951	*/
nikapov	0:a1a69d32f310	2952	sigma_est_ref =
nikapov	0:a1a69d32f310	2953	VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs +
nikapov	0:a1a69d32f310	2954	final_range_integration_time_milli_secs / 2) /
nikapov	0:a1a69d32f310	2955	final_range_integration_time_milli_secs);
nikapov	0:a1a69d32f310	2956
nikapov	0:a1a69d32f310	2957	/* FixPoint2408 << 8 = FixPoint1616 */
nikapov	0:a1a69d32f310	2958	sigma_est_ref <<= 8;
nikapov	0:a1a69d32f310	2959	sigma_est_ref = (sigma_est_ref + 500) / 1000;
nikapov	0:a1a69d32f310	2960
nikapov	0:a1a69d32f310	2961	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	2962	sqr1 = sigma_est_rtn * sigma_est_rtn;
nikapov	0:a1a69d32f310	2963	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	2964	sqr2 = sigma_est_ref * sigma_est_ref;
nikapov	0:a1a69d32f310	2965
nikapov	0:a1a69d32f310	2966	/* sqrt(FixPoint3232) = FixPoint1616 */
nikapov	0:a1a69d32f310	2967	sqrt_result = VL53L0X_isqrt((sqr1 + sqr2));
nikapov	0:a1a69d32f310	2968	/*
nikapov	0:a1a69d32f310	2969	* Note that the Shift by 4 bits increases resolution prior to
sepp_nepp	7:41cbc431e1f4	2970	* the sqrt,therefore the result must be shifted by 2 bits to
nikapov	0:a1a69d32f310	2971	* the right to revert back to the FixPoint1616 format.
nikapov	0:a1a69d32f310	2972	*/
nikapov	0:a1a69d32f310	2973
nikapov	0:a1a69d32f310	2974	sigma_estimate = 1000 * sqrt_result;
nikapov	0:a1a69d32f310	2975
nikapov	0:a1a69d32f310	2976	if ((peak_signal_rate_kcps < 1) \|\| (vcsel_total_events_rtn < 1) \|\|
nikapov	0:a1a69d32f310	2977	(sigma_estimate > c_sigma_est_max)) {
nikapov	0:a1a69d32f310	2978	sigma_estimate = c_sigma_est_max;
nikapov	0:a1a69d32f310	2979	}
nikapov	0:a1a69d32f310	2980
nikapov	0:a1a69d32f310	2981	*p_sigma_estimate = (uint32_t)(sigma_estimate);
sepp_nepp	7:41cbc431e1f4	2982	_device->SigmaEstimate = *p_sigma_estimate;
sepp_nepp	7:41cbc431e1f4	2983	status = VL53L0X_calc_dmax(total_signal_rate_mcps,
nikapov	0:a1a69d32f310	2984	corrected_signal_rate_mcps,
nikapov	0:a1a69d32f310	2985	pw_mult,
nikapov	0:a1a69d32f310	2986	sigma_estimate_p1,
nikapov	0:a1a69d32f310	2987	sigma_estimate_p2,
nikapov	0:a1a69d32f310	2988	peak_vcsel_duration_us,
nikapov	0:a1a69d32f310	2989	p_dmax_mm);
nikapov	0:a1a69d32f310	2990	}
nikapov	0:a1a69d32f310	2991
sepp_nepp	5:b95f6951f7d5	2992
nikapov	0:a1a69d32f310	2993	return status;
nikapov	0:a1a69d32f310	2994	}
nikapov	0:a1a69d32f310	2995
sepp_nepp	7:41cbc431e1f4	2996	VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(uint8_t device_range_status,
nikapov	0:a1a69d32f310	2997	FixPoint1616_t signal_rate,
nikapov	0:a1a69d32f310	2998	uint16_t effective_spad_rtn_count,
nikapov	0:a1a69d32f310	2999	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	3000	uint8_t *p_pal_range_status)
nikapov	0:a1a69d32f310	3001	{
nikapov	0:a1a69d32f310	3002	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3003	uint8_t none_flag;
nikapov	0:a1a69d32f310	3004	uint8_t sigma_limitflag = 0;
nikapov	0:a1a69d32f310	3005	uint8_t signal_ref_clipflag = 0;
nikapov	0:a1a69d32f310	3006	uint8_t range_ignore_thresholdflag = 0;
nikapov	0:a1a69d32f310	3007	uint8_t sigma_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3008	uint8_t signal_rate_final_range_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3009	uint8_t signal_ref_clip_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3010	uint8_t range_ignore_threshold_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3011	FixPoint1616_t sigma_estimate;
nikapov	0:a1a69d32f310	3012	FixPoint1616_t sigma_limit_value;
nikapov	0:a1a69d32f310	3013	FixPoint1616_t signal_ref_clip_value;
nikapov	0:a1a69d32f310	3014	FixPoint1616_t range_ignore_threshold_value;
nikapov	0:a1a69d32f310	3015	FixPoint1616_t signal_rate_per_spad;
nikapov	0:a1a69d32f310	3016	uint8_t device_range_status_internal = 0;
nikapov	0:a1a69d32f310	3017	uint16_t tmp_word = 0;
nikapov	0:a1a69d32f310	3018	uint8_t temp8;
nikapov	0:a1a69d32f310	3019	uint32_t dmax_mm = 0;
nikapov	0:a1a69d32f310	3020	FixPoint1616_t last_signal_ref_mcps;
nikapov	0:a1a69d32f310	3021
sepp_nepp	5:b95f6951f7d5	3022
nikapov	0:a1a69d32f310	3023
nikapov	0:a1a69d32f310	3024
nikapov	0:a1a69d32f310	3025	/*
nikapov	0:a1a69d32f310	3026	* VL53L0X has a good ranging when the value of the
nikapov	0:a1a69d32f310	3027	* DeviceRangeStatus = 11. This function will replace the value 0 with
nikapov	0:a1a69d32f310	3028	* the value 11 in the DeviceRangeStatus.
sepp_nepp	7:41cbc431e1f4	3029	* In addition,the SigmaEstimator is not included in the VL53L0X
sepp_nepp	7:41cbc431e1f4	3030	* DeviceRangeStatus,this will be added in the PalRangeStatus.
nikapov	0:a1a69d32f310	3031	*/
nikapov	0:a1a69d32f310	3032
nikapov	0:a1a69d32f310	3033	device_range_status_internal = ((device_range_status & 0x78) >> 3);
nikapov	0:a1a69d32f310	3034
sepp_nepp	7:41cbc431e1f4	3035	if ( device_range_status_internal == 0 \|\|
nikapov	0:a1a69d32f310	3036	device_range_status_internal == 5 \|\|
nikapov	0:a1a69d32f310	3037	device_range_status_internal == 7 \|\|
nikapov	0:a1a69d32f310	3038	device_range_status_internal == 12 \|\|
nikapov	0:a1a69d32f310	3039	device_range_status_internal == 13 \|\|
nikapov	0:a1a69d32f310	3040	device_range_status_internal == 14 \|\|
nikapov	0:a1a69d32f310	3041	device_range_status_internal == 15
nikapov	0:a1a69d32f310	3042	) {
nikapov	0:a1a69d32f310	3043	none_flag = 1;
nikapov	0:a1a69d32f310	3044	} else {
nikapov	0:a1a69d32f310	3045	none_flag = 0;
nikapov	0:a1a69d32f310	3046	}
nikapov	0:a1a69d32f310	3047
nikapov	0:a1a69d32f310	3048	/*
sepp_nepp	7:41cbc431e1f4	3049	* Check if Sigma limit is enabled,if yes then do comparison with limit
nikapov	0:a1a69d32f310	3050	* value and put the result back into pPalRangeStatus.
nikapov	0:a1a69d32f310	3051	*/
nikapov	0:a1a69d32f310	3052	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3053	status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	3054	&sigma_limit_check_enable);
Davidroid	2:d07edeaff6f1	3055	}
nikapov	0:a1a69d32f310	3056
nikapov	0:a1a69d32f310	3057	if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	3058	/*
nikapov	0:a1a69d32f310	3059	* compute the Sigma and check with limit
nikapov	0:a1a69d32f310	3060	*/
sepp_nepp	7:41cbc431e1f4	3061	status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data,
nikapov	0:a1a69d32f310	3062	&sigma_estimate,
nikapov	0:a1a69d32f310	3063	&dmax_mm);
nikapov	0:a1a69d32f310	3064	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3065	p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
nikapov	0:a1a69d32f310	3066	}
nikapov	0:a1a69d32f310	3067
nikapov	0:a1a69d32f310	3068	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3069	status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	3070	&sigma_limit_value);
nikapov	0:a1a69d32f310	3071
nikapov	0:a1a69d32f310	3072	if ((sigma_limit_value > 0) &&
nikapov	0:a1a69d32f310	3073	(sigma_estimate > sigma_limit_value)) {
nikapov	0:a1a69d32f310	3074	/* Limit Fail */
nikapov	0:a1a69d32f310	3075	sigma_limitflag = 1;
nikapov	0:a1a69d32f310	3076	}
nikapov	0:a1a69d32f310	3077	}
nikapov	0:a1a69d32f310	3078	}
nikapov	0:a1a69d32f310	3079
nikapov	0:a1a69d32f310	3080	/*
sepp_nepp	7:41cbc431e1f4	3081	* Check if Signal ref clip limit is enabled,if yes then do comparison
nikapov	0:a1a69d32f310	3082	* with limit value and put the result back into pPalRangeStatus.
nikapov	0:a1a69d32f310	3083	*/
nikapov	0:a1a69d32f310	3084	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3085	status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	3086	&signal_ref_clip_limit_check_enable);
nikapov	0:a1a69d32f310	3087	}
nikapov	0:a1a69d32f310	3088
nikapov	0:a1a69d32f310	3089	if ((signal_ref_clip_limit_check_enable != 0) &&
nikapov	0:a1a69d32f310	3090	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	3091
sepp_nepp	7:41cbc431e1f4	3092	status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	3093	&signal_ref_clip_value);
nikapov	0:a1a69d32f310	3094
nikapov	0:a1a69d32f310	3095	/* Read LastSignalRefMcps from device */
nikapov	0:a1a69d32f310	3096	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3097	status = VL53L0X_write_byte(0xFF,0x01);
nikapov	0:a1a69d32f310	3098	}
nikapov	0:a1a69d32f310	3099
nikapov	0:a1a69d32f310	3100	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3101	status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
nikapov	0:a1a69d32f310	3102	&tmp_word);
nikapov	0:a1a69d32f310	3103	}
nikapov	0:a1a69d32f310	3104
nikapov	0:a1a69d32f310	3105	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3106	status = VL53L0X_write_byte(0xFF,0x00);
nikapov	0:a1a69d32f310	3107	}
nikapov	0:a1a69d32f310	3108
nikapov	0:a1a69d32f310	3109	last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word);
sepp_nepp	7:41cbc431e1f4	3110	_device->LastSignalRefMcps = last_signal_ref_mcps;
nikapov	0:a1a69d32f310	3111
nikapov	0:a1a69d32f310	3112	if ((signal_ref_clip_value > 0) &&
nikapov	0:a1a69d32f310	3113	(last_signal_ref_mcps > signal_ref_clip_value)) {
nikapov	0:a1a69d32f310	3114	/* Limit Fail */
nikapov	0:a1a69d32f310	3115	signal_ref_clipflag = 1;
nikapov	0:a1a69d32f310	3116	}
nikapov	0:a1a69d32f310	3117	}
nikapov	0:a1a69d32f310	3118
nikapov	0:a1a69d32f310	3119	/*
sepp_nepp	7:41cbc431e1f4	3120	* Check if Signal ref clip limit is enabled,if yes then do comparison
nikapov	0:a1a69d32f310	3121	* with limit value and put the result back into pPalRangeStatus.
nikapov	0:a1a69d32f310	3122	* EffectiveSpadRtnCount has a format 8.8
nikapov	0:a1a69d32f310	3123	* If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
nikapov	0:a1a69d32f310	3124	*/
Davidroid	2:d07edeaff6f1	3125	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3126	status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	3127	&range_ignore_threshold_limit_check_enable);
Davidroid	2:d07edeaff6f1	3128	}
nikapov	0:a1a69d32f310	3129
nikapov	0:a1a69d32f310	3130	if ((range_ignore_threshold_limit_check_enable != 0) &&
nikapov	0:a1a69d32f310	3131	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	3132
nikapov	0:a1a69d32f310	3133	/* Compute the signal rate per spad */
nikapov	0:a1a69d32f310	3134	if (effective_spad_rtn_count == 0) {
nikapov	0:a1a69d32f310	3135	signal_rate_per_spad = 0;
nikapov	0:a1a69d32f310	3136	} else {
nikapov	0:a1a69d32f310	3137	signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate)
nikapov	0:a1a69d32f310	3138	/ effective_spad_rtn_count);
nikapov	0:a1a69d32f310	3139	}
nikapov	0:a1a69d32f310	3140
sepp_nepp	7:41cbc431e1f4	3141	status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	3142	&range_ignore_threshold_value);
nikapov	0:a1a69d32f310	3143
nikapov	0:a1a69d32f310	3144	if ((range_ignore_threshold_value > 0) &&
nikapov	0:a1a69d32f310	3145	(signal_rate_per_spad < range_ignore_threshold_value)) {
nikapov	0:a1a69d32f310	3146	/* Limit Fail add 2^6 to range status */
nikapov	0:a1a69d32f310	3147	range_ignore_thresholdflag = 1;
nikapov	0:a1a69d32f310	3148	}
nikapov	0:a1a69d32f310	3149	}
nikapov	0:a1a69d32f310	3150
nikapov	0:a1a69d32f310	3151	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3152	if (none_flag == 1) {
nikapov	0:a1a69d32f310	3153	p_pal_range_status = 255; / NONE */
nikapov	0:a1a69d32f310	3154	} else if (device_range_status_internal == 1 \|\|
nikapov	0:a1a69d32f310	3155	device_range_status_internal == 2 \|\|
nikapov	0:a1a69d32f310	3156	device_range_status_internal == 3) {
nikapov	0:a1a69d32f310	3157	p_pal_range_status = 5; / HW fail */
nikapov	0:a1a69d32f310	3158	} else if (device_range_status_internal == 6 \|\|
nikapov	0:a1a69d32f310	3159	device_range_status_internal == 9) {
nikapov	0:a1a69d32f310	3160	p_pal_range_status = 4; / Phase fail */
nikapov	0:a1a69d32f310	3161	} else if (device_range_status_internal == 8 \|\|
nikapov	0:a1a69d32f310	3162	device_range_status_internal == 10 \|\|
nikapov	0:a1a69d32f310	3163	signal_ref_clipflag == 1) {
nikapov	0:a1a69d32f310	3164	p_pal_range_status = 3; / Min range */
nikapov	0:a1a69d32f310	3165	} else if (device_range_status_internal == 4 \|\|
nikapov	0:a1a69d32f310	3166	range_ignore_thresholdflag == 1) {
nikapov	0:a1a69d32f310	3167	p_pal_range_status = 2; / Signal Fail */
nikapov	0:a1a69d32f310	3168	} else if (sigma_limitflag == 1) {
nikapov	0:a1a69d32f310	3169	p_pal_range_status = 1; / Sigma Fail */
nikapov	0:a1a69d32f310	3170	} else {
nikapov	0:a1a69d32f310	3171	p_pal_range_status = 0; / Range Valid */
nikapov	0:a1a69d32f310	3172	}
nikapov	0:a1a69d32f310	3173	}
nikapov	0:a1a69d32f310	3174
nikapov	0:a1a69d32f310	3175	/* DMAX only relevant during range error */
Davidroid	3:e9269ff624ed	3176	if (*p_pal_range_status == 0) {
nikapov	0:a1a69d32f310	3177	p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
Davidroid	3:e9269ff624ed	3178	}
nikapov	0:a1a69d32f310	3179
nikapov	0:a1a69d32f310	3180	/* fill the Limit Check Status */
nikapov	0:a1a69d32f310	3181
sepp_nepp	7:41cbc431e1f4	3182	status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov	0:a1a69d32f310	3183	&signal_rate_final_range_limit_check_enable);
nikapov	0:a1a69d32f310	3184
nikapov	0:a1a69d32f310	3185	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3186	if ((sigma_limit_check_enable == 0) \|\| (sigma_limitflag == 1)) {
nikapov	0:a1a69d32f310	3187	temp8 = 1;
nikapov	0:a1a69d32f310	3188	} else {
nikapov	0:a1a69d32f310	3189	temp8 = 0;
nikapov	0:a1a69d32f310	3190	}
sepp_nepp	7:41cbc431e1f4	3191	_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = temp8;
nikapov	0:a1a69d32f310	3192
nikapov	0:a1a69d32f310	3193	if ((device_range_status_internal == 4) \|\|
nikapov	0:a1a69d32f310	3194	(signal_rate_final_range_limit_check_enable == 0)) {
nikapov	0:a1a69d32f310	3195	temp8 = 1;
nikapov	0:a1a69d32f310	3196	} else {
nikapov	0:a1a69d32f310	3197	temp8 = 0;
nikapov	0:a1a69d32f310	3198	}
sepp_nepp	7:41cbc431e1f4	3199	_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = temp8;
nikapov	0:a1a69d32f310	3200
nikapov	0:a1a69d32f310	3201	if ((signal_ref_clip_limit_check_enable == 0) \|\|
nikapov	0:a1a69d32f310	3202	(signal_ref_clipflag == 1)) {
nikapov	0:a1a69d32f310	3203	temp8 = 1;
nikapov	0:a1a69d32f310	3204	} else {
nikapov	0:a1a69d32f310	3205	temp8 = 0;
nikapov	0:a1a69d32f310	3206	}
nikapov	0:a1a69d32f310	3207
sepp_nepp	7:41cbc431e1f4	3208	_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = temp8;
nikapov	0:a1a69d32f310	3209
nikapov	0:a1a69d32f310	3210	if ((range_ignore_threshold_limit_check_enable == 0) \|\|
nikapov	0:a1a69d32f310	3211	(range_ignore_thresholdflag == 1)) {
nikapov	0:a1a69d32f310	3212	temp8 = 1;
nikapov	0:a1a69d32f310	3213	} else {
nikapov	0:a1a69d32f310	3214	temp8 = 0;
nikapov	0:a1a69d32f310	3215	}
nikapov	0:a1a69d32f310	3216
sepp_nepp	7:41cbc431e1f4	3217	_device->CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = temp8;
nikapov	0:a1a69d32f310	3218	}
nikapov	0:a1a69d32f310	3219
sepp_nepp	5:b95f6951f7d5	3220
nikapov	0:a1a69d32f310	3221	return status;
nikapov	0:a1a69d32f310	3222
nikapov	0:a1a69d32f310	3223	}
nikapov	0:a1a69d32f310	3224
sepp_nepp	7:41cbc431e1f4	3225	VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
nikapov	0:a1a69d32f310	3226	{
nikapov	0:a1a69d32f310	3227	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3228	uint8_t device_range_status;
nikapov	0:a1a69d32f310	3229	uint8_t range_fractional_enable;
nikapov	0:a1a69d32f310	3230	uint8_t pal_range_status;
nikapov	0:a1a69d32f310	3231	uint8_t x_talk_compensation_enable;
nikapov	0:a1a69d32f310	3232	uint16_t ambient_rate;
nikapov	0:a1a69d32f310	3233	FixPoint1616_t signal_rate;
nikapov	0:a1a69d32f310	3234	uint16_t x_talk_compensation_rate_mega_cps;
nikapov	0:a1a69d32f310	3235	uint16_t effective_spad_rtn_count;
nikapov	0:a1a69d32f310	3236	uint16_t tmpuint16;
nikapov	0:a1a69d32f310	3237	uint16_t xtalk_range_milli_meter;
nikapov	0:a1a69d32f310	3238	uint16_t linearity_corrective_gain;
nikapov	0:a1a69d32f310	3239	uint8_t localBuffer[12];
nikapov	0:a1a69d32f310	3240	VL53L0X_RangingMeasurementData_t last_range_data_buffer;
nikapov	0:a1a69d32f310	3241
sepp_nepp	5:b95f6951f7d5	3242
nikapov	0:a1a69d32f310	3243
nikapov	0:a1a69d32f310	3244	/*
sepp_nepp	7:41cbc431e1f4	3245	* use multi read even if some registers are not useful,result will
nikapov	0:a1a69d32f310	3246	* be more efficient
nikapov	0:a1a69d32f310	3247	* start reading at 0x14 dec20
nikapov	0:a1a69d32f310	3248	* end reading at 0x21 dec33 total 14 bytes to read
nikapov	0:a1a69d32f310	3249	*/
sepp_nepp	7:41cbc431e1f4	3250	status = VL53L0X_read_multi(0x14,localBuffer,12);
nikapov	0:a1a69d32f310	3251
nikapov	0:a1a69d32f310	3252	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3253
nikapov	0:a1a69d32f310	3254	p_ranging_measurement_data->ZoneId = 0; /* Only one zone */
nikapov	0:a1a69d32f310	3255	p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */
nikapov	0:a1a69d32f310	3256
sepp_nepp	7:41cbc431e1f4	3257	tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]);
nikapov	0:a1a69d32f310	3258	/* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
nikapov	0:a1a69d32f310	3259	*(format 11.2) else no fractional
nikapov	0:a1a69d32f310	3260	*/
nikapov	0:a1a69d32f310	3261
nikapov	0:a1a69d32f310	3262	p_ranging_measurement_data->MeasurementTimeUsec = 0;
nikapov	0:a1a69d32f310	3263
sepp_nepp	7:41cbc431e1f4	3264	signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6]));
nikapov	0:a1a69d32f310	3265	/* peak_signal_count_rate_rtn_mcps */
nikapov	0:a1a69d32f310	3266	p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
nikapov	0:a1a69d32f310	3267
sepp_nepp	7:41cbc431e1f4	3268	ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]);
nikapov	0:a1a69d32f310	3269	p_ranging_measurement_data->AmbientRateRtnMegaCps =
nikapov	0:a1a69d32f310	3270	VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate);
nikapov	0:a1a69d32f310	3271
nikapov	0:a1a69d32f310	3272	effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3],
nikapov	0:a1a69d32f310	3273	localBuffer[2]);
nikapov	0:a1a69d32f310	3274	/* EffectiveSpadRtnCount is 8.8 format */
nikapov	0:a1a69d32f310	3275	p_ranging_measurement_data->EffectiveSpadRtnCount =
nikapov	0:a1a69d32f310	3276	effective_spad_rtn_count;
nikapov	0:a1a69d32f310	3277
nikapov	0:a1a69d32f310	3278	device_range_status = localBuffer[0];
nikapov	0:a1a69d32f310	3279
nikapov	0:a1a69d32f310	3280	/* Get Linearity Corrective Gain */
sepp_nepp	7:41cbc431e1f4	3281	linearity_corrective_gain = _device->
sepp_nepp	7:41cbc431e1f4	3282	LinearityCorrectiveGain;
nikapov	0:a1a69d32f310	3283
nikapov	0:a1a69d32f310	3284	/* Get ranging configuration */
sepp_nepp	7:41cbc431e1f4	3285	range_fractional_enable = _device->
sepp_nepp	7:41cbc431e1f4	3286	RangeFractionalEnable;
nikapov	0:a1a69d32f310	3287
nikapov	0:a1a69d32f310	3288	if (linearity_corrective_gain != 1000) {
nikapov	0:a1a69d32f310	3289
nikapov	0:a1a69d32f310	3290	tmpuint16 = (uint16_t)((linearity_corrective_gain
nikapov	0:a1a69d32f310	3291	* tmpuint16 + 500) / 1000);
nikapov	0:a1a69d32f310	3292
nikapov	0:a1a69d32f310	3293	/* Implement Xtalk */
sepp_nepp	7:41cbc431e1f4	3294	x_talk_compensation_rate_mega_cps = _device->CurrentParameters.XTalkCompensationRateMegaCps;
sepp_nepp	7:41cbc431e1f4	3295	x_talk_compensation_enable = _device->CurrentParameters.XTalkCompensationEnable;
nikapov	0:a1a69d32f310	3296
nikapov	0:a1a69d32f310	3297	if (x_talk_compensation_enable) {
nikapov	0:a1a69d32f310	3298
nikapov	0:a1a69d32f310	3299	if ((signal_rate
nikapov	0:a1a69d32f310	3300	- ((x_talk_compensation_rate_mega_cps
nikapov	0:a1a69d32f310	3301	* effective_spad_rtn_count) >> 8))
nikapov	0:a1a69d32f310	3302	<= 0) {
nikapov	0:a1a69d32f310	3303	if (range_fractional_enable) {
nikapov	0:a1a69d32f310	3304	xtalk_range_milli_meter = 8888;
nikapov	0:a1a69d32f310	3305	} else {
nikapov	0:a1a69d32f310	3306	xtalk_range_milli_meter = 8888 << 2;
nikapov	0:a1a69d32f310	3307	}
nikapov	0:a1a69d32f310	3308	} else {
nikapov	0:a1a69d32f310	3309	xtalk_range_milli_meter =
nikapov	0:a1a69d32f310	3310	(tmpuint16 * signal_rate)
nikapov	0:a1a69d32f310	3311	/ (signal_rate
nikapov	0:a1a69d32f310	3312	- ((x_talk_compensation_rate_mega_cps
nikapov	0:a1a69d32f310	3313	* effective_spad_rtn_count)
nikapov	0:a1a69d32f310	3314	>> 8));
nikapov	0:a1a69d32f310	3315	}
nikapov	0:a1a69d32f310	3316
nikapov	0:a1a69d32f310	3317	tmpuint16 = xtalk_range_milli_meter;
nikapov	0:a1a69d32f310	3318	}
nikapov	0:a1a69d32f310	3319
nikapov	0:a1a69d32f310	3320	}
nikapov	0:a1a69d32f310	3321
nikapov	0:a1a69d32f310	3322	if (range_fractional_enable) {
nikapov	0:a1a69d32f310	3323	p_ranging_measurement_data->RangeMilliMeter =
nikapov	0:a1a69d32f310	3324	(uint16_t)((tmpuint16) >> 2);
nikapov	0:a1a69d32f310	3325	p_ranging_measurement_data->RangeFractionalPart =
nikapov	0:a1a69d32f310	3326	(uint8_t)((tmpuint16 & 0x03) << 6);
nikapov	0:a1a69d32f310	3327	} else {
nikapov	0:a1a69d32f310	3328	p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
nikapov	0:a1a69d32f310	3329	p_ranging_measurement_data->RangeFractionalPart = 0;
nikapov	0:a1a69d32f310	3330	}
nikapov	0:a1a69d32f310	3331
nikapov	0:a1a69d32f310	3332	/*
sepp_nepp	7:41cbc431e1f4	3333	* For a standard definition of RangeStatus,this should
nikapov	0:a1a69d32f310	3334	* return 0 in case of good result after a ranging
nikapov	0:a1a69d32f310	3335	* The range status depends on the device so call a device
nikapov	0:a1a69d32f310	3336	* specific function to obtain the right Status.
nikapov	0:a1a69d32f310	3337	*/
sepp_nepp	7:41cbc431e1f4	3338	status \|= VL53L0X_get_pal_range_status(device_range_status,
sepp_nepp	7:41cbc431e1f4	3339	signal_rate,effective_spad_rtn_count,
sepp_nepp	7:41cbc431e1f4	3340	p_ranging_measurement_data,&pal_range_status);
nikapov	0:a1a69d32f310	3341
nikapov	0:a1a69d32f310	3342	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3343	p_ranging_measurement_data->RangeStatus = pal_range_status;
nikapov	0:a1a69d32f310	3344	}
nikapov	0:a1a69d32f310	3345
nikapov	0:a1a69d32f310	3346	}
nikapov	0:a1a69d32f310	3347
nikapov	0:a1a69d32f310	3348	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3349	/* Copy last read data into Dev buffer */
sepp_nepp	7:41cbc431e1f4	3350	last_range_data_buffer = _device-> LastRangeMeasure;
nikapov	0:a1a69d32f310	3351
nikapov	0:a1a69d32f310	3352	last_range_data_buffer.RangeMilliMeter =
nikapov	0:a1a69d32f310	3353	p_ranging_measurement_data->RangeMilliMeter;
nikapov	0:a1a69d32f310	3354	last_range_data_buffer.RangeFractionalPart =
nikapov	0:a1a69d32f310	3355	p_ranging_measurement_data->RangeFractionalPart;
nikapov	0:a1a69d32f310	3356	last_range_data_buffer.RangeDMaxMilliMeter =
nikapov	0:a1a69d32f310	3357	p_ranging_measurement_data->RangeDMaxMilliMeter;
nikapov	0:a1a69d32f310	3358	last_range_data_buffer.MeasurementTimeUsec =
nikapov	0:a1a69d32f310	3359	p_ranging_measurement_data->MeasurementTimeUsec;
nikapov	0:a1a69d32f310	3360	last_range_data_buffer.SignalRateRtnMegaCps =
nikapov	0:a1a69d32f310	3361	p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	3362	last_range_data_buffer.AmbientRateRtnMegaCps =
nikapov	0:a1a69d32f310	3363	p_ranging_measurement_data->AmbientRateRtnMegaCps;
nikapov	0:a1a69d32f310	3364	last_range_data_buffer.EffectiveSpadRtnCount =
nikapov	0:a1a69d32f310	3365	p_ranging_measurement_data->EffectiveSpadRtnCount;
nikapov	0:a1a69d32f310	3366	last_range_data_buffer.RangeStatus =
nikapov	0:a1a69d32f310	3367	p_ranging_measurement_data->RangeStatus;
nikapov	0:a1a69d32f310	3368
sepp_nepp	7:41cbc431e1f4	3369	_device->LastRangeMeasure = last_range_data_buffer;
nikapov	0:a1a69d32f310	3370	}
nikapov	0:a1a69d32f310	3371
sepp_nepp	5:b95f6951f7d5	3372
nikapov	0:a1a69d32f310	3373	return status;
nikapov	0:a1a69d32f310	3374	}
nikapov	0:a1a69d32f310	3375
sepp_nepp	7:41cbc431e1f4	3376	VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
nikapov	0:a1a69d32f310	3377	{
nikapov	0:a1a69d32f310	3378	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3379
sepp_nepp	5:b95f6951f7d5	3380
nikapov	0:a1a69d32f310	3381
nikapov	0:a1a69d32f310	3382	/* This function will do a complete single ranging
nikapov	0:a1a69d32f310	3383	* Here we fix the mode! */
sepp_nepp	7:41cbc431e1f4	3384	status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING);
nikapov	0:a1a69d32f310	3385
nikapov	0:a1a69d32f310	3386	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3387	status = VL53L0X_perform_single_measurement();
nikapov	0:a1a69d32f310	3388	}
nikapov	0:a1a69d32f310	3389
nikapov	0:a1a69d32f310	3390	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3391	status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data);
nikapov	0:a1a69d32f310	3392	}
nikapov	0:a1a69d32f310	3393
nikapov	0:a1a69d32f310	3394	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3395	status = VL53L0X_clear_interrupt_mask(0);
nikapov	0:a1a69d32f310	3396	}
nikapov	0:a1a69d32f310	3397
sepp_nepp	5:b95f6951f7d5	3398
nikapov	0:a1a69d32f310	3399	return status;
nikapov	0:a1a69d32f310	3400	}
nikapov	0:a1a69d32f310	3401
sepp_nepp	7:41cbc431e1f4	3402	VL53L0X_Error VL53L0X::perform_ref_signal_measurement(uint16_t *p_ref_signal_rate)
nikapov	0:a1a69d32f310	3403	{
nikapov	0:a1a69d32f310	3404	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3405	VL53L0X_RangingMeasurementData_t ranging_measurement_data;
nikapov	0:a1a69d32f310	3406
nikapov	0:a1a69d32f310	3407	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	3408
nikapov	0:a1a69d32f310	3409	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	3410	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	3411	*/
nikapov	0:a1a69d32f310	3412
sepp_nepp	7:41cbc431e1f4	3413	sequence_config = _device-> SequenceConfig;
nikapov	0:a1a69d32f310	3414
nikapov	0:a1a69d32f310	3415	/*
nikapov	0:a1a69d32f310	3416	* This function performs a reference signal rate measurement.
nikapov	0:a1a69d32f310	3417	*/
nikapov	0:a1a69d32f310	3418	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3419	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0xC0);
nikapov	0:a1a69d32f310	3420	}
nikapov	0:a1a69d32f310	3421
nikapov	0:a1a69d32f310	3422	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3423	status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data);
nikapov	0:a1a69d32f310	3424	}
nikapov	0:a1a69d32f310	3425
nikapov	0:a1a69d32f310	3426	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3427	status = VL53L0X_write_byte(0xFF,0x01);
nikapov	0:a1a69d32f310	3428	}
nikapov	0:a1a69d32f310	3429
nikapov	0:a1a69d32f310	3430	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3431	status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
nikapov	0:a1a69d32f310	3432	p_ref_signal_rate);
Davidroid	2:d07edeaff6f1	3433	}
nikapov	0:a1a69d32f310	3434
nikapov	0:a1a69d32f310	3435	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3436	status = VL53L0X_write_byte(0xFF,0x00);
Davidroid	2:d07edeaff6f1	3437	}
nikapov	0:a1a69d32f310	3438
nikapov	0:a1a69d32f310	3439	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3440	/* restore the previous Sequence Config */
sepp_nepp	7:41cbc431e1f4	3441	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	3442	sequence_config);
nikapov	0:a1a69d32f310	3443	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3444	_device->SequenceConfig = sequence_config;
nikapov	0:a1a69d32f310	3445	}
nikapov	0:a1a69d32f310	3446	}
nikapov	0:a1a69d32f310	3447
nikapov	0:a1a69d32f310	3448	return status;
nikapov	0:a1a69d32f310	3449	}
nikapov	0:a1a69d32f310	3450
sepp_nepp	7:41cbc431e1f4	3451	VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count,
nikapov	0:a1a69d32f310	3452	uint8_t *is_aperture_spads)
nikapov	0:a1a69d32f310	3453	{
nikapov	0:a1a69d32f310	3454	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3455	uint8_t last_spad_array[6];
nikapov	0:a1a69d32f310	3456	uint8_t start_select = 0xB4;
nikapov	0:a1a69d32f310	3457	uint32_t minimum_spad_count = 3;
nikapov	0:a1a69d32f310	3458	uint32_t max_spad_count = 44;
nikapov	0:a1a69d32f310	3459	uint32_t current_spad_index = 0;
nikapov	0:a1a69d32f310	3460	uint32_t last_spad_index = 0;
nikapov	0:a1a69d32f310	3461	int32_t next_good_spad = 0;
nikapov	0:a1a69d32f310	3462	uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */
nikapov	0:a1a69d32f310	3463	uint16_t peak_signal_rate_ref;
nikapov	0:a1a69d32f310	3464	uint32_t need_apt_spads = 0;
nikapov	0:a1a69d32f310	3465	uint32_t index = 0;
nikapov	0:a1a69d32f310	3466	uint32_t spad_array_size = 6;
nikapov	0:a1a69d32f310	3467	uint32_t signal_rate_diff = 0;
nikapov	0:a1a69d32f310	3468	uint32_t last_signal_rate_diff = 0;
nikapov	0:a1a69d32f310	3469	uint8_t complete = 0;
nikapov	0:a1a69d32f310	3470	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	3471	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	3472	uint32_t ref_spad_count_int = 0;
nikapov	0:a1a69d32f310	3473	uint8_t is_aperture_spads_int = 0;
nikapov	0:a1a69d32f310	3474
nikapov	0:a1a69d32f310	3475	/*
nikapov	0:a1a69d32f310	3476	* The reference SPAD initialization procedure determines the minimum
nikapov	0:a1a69d32f310	3477	* amount of reference spads to be enables to achieve a target reference
nikapov	0:a1a69d32f310	3478	* signal rate and should be performed once during initialization.
nikapov	0:a1a69d32f310	3479	*
nikapov	0:a1a69d32f310	3480	* Either aperture or non-aperture spads are applied but never both.
sepp_nepp	7:41cbc431e1f4	3481	* Firstly non-aperture spads are set,begining with 5 spads,and
nikapov	0:a1a69d32f310	3482	* increased one spad at a time until the closest measurement to the
nikapov	0:a1a69d32f310	3483	* target rate is achieved.
nikapov	0:a1a69d32f310	3484	*
nikapov	0:a1a69d32f310	3485	* If the target rate is exceeded when 5 non-aperture spads are enabled,
nikapov	0:a1a69d32f310	3486	* initialization is performed instead with aperture spads.
nikapov	0:a1a69d32f310	3487	*
sepp_nepp	7:41cbc431e1f4	3488	* When setting spads,a 'Good Spad Map' is applied.
nikapov	0:a1a69d32f310	3489	*
nikapov	0:a1a69d32f310	3490	* This procedure operates within a SPAD window of interest of a maximum
nikapov	0:a1a69d32f310	3491	* 44 spads.
sepp_nepp	7:41cbc431e1f4	3492	* The start point is currently fixed to 180,which lies towards the end
nikapov	0:a1a69d32f310	3493	* of the non-aperture quadrant and runs in to the adjacent aperture
nikapov	0:a1a69d32f310	3494	* quadrant.
nikapov	0:a1a69d32f310	3495	*/
sepp_nepp	7:41cbc431e1f4	3496	target_ref_rate = _device-> targetRefRate;
nikapov	0:a1a69d32f310	3497
nikapov	0:a1a69d32f310	3498	/*
nikapov	0:a1a69d32f310	3499	* Initialize Spad arrays.
nikapov	0:a1a69d32f310	3500	* Currently the good spad map is initialised to 'All good'.
nikapov	0:a1a69d32f310	3501	* This is a short term implementation. The good spad map will be
nikapov	0:a1a69d32f310	3502	* provided as an input.
nikapov	0:a1a69d32f310	3503	* Note that there are 6 bytes. Only the first 44 bits will be used to
nikapov	0:a1a69d32f310	3504	* represent spads.
nikapov	0:a1a69d32f310	3505	*/
Davidroid	3:e9269ff624ed	3506	for (index = 0; index < spad_array_size; index++) {
sepp_nepp	7:41cbc431e1f4	3507	_device->SpadData.RefSpadEnables[index] = 0;
sepp_nepp	7:41cbc431e1f4	3508	}
sepp_nepp	7:41cbc431e1f4	3509
sepp_nepp	7:41cbc431e1f4	3510
sepp_nepp	7:41cbc431e1f4	3511	status = VL53L0X_write_byte(0xFF,0x01);
nikapov	0:a1a69d32f310	3512
nikapov	0:a1a69d32f310	3513	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3514	status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
nikapov	0:a1a69d32f310	3515	}
nikapov	0:a1a69d32f310	3516
nikapov	0:a1a69d32f310	3517	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3518	status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
Davidroid	2:d07edeaff6f1	3519	}
nikapov	0:a1a69d32f310	3520
nikapov	0:a1a69d32f310	3521	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3522	status = VL53L0X_write_byte(0xFF,0x00);
nikapov	0:a1a69d32f310	3523	}
nikapov	0:a1a69d32f310	3524
nikapov	0:a1a69d32f310	3525	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3526	status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
nikapov	0:a1a69d32f310	3527	start_select);
Davidroid	2:d07edeaff6f1	3528	}
nikapov	0:a1a69d32f310	3529
nikapov	0:a1a69d32f310	3530	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3531	status = VL53L0X_write_byte(VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0);
Davidroid	2:d07edeaff6f1	3532	}
nikapov	0:a1a69d32f310	3533
nikapov	0:a1a69d32f310	3534	/* Perform ref calibration */
nikapov	0:a1a69d32f310	3535	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3536	status = VL53L0X_perform_ref_calibration(&vhv_settings,
sepp_nepp	7:41cbc431e1f4	3537	&phase_cal,0);
Davidroid	2:d07edeaff6f1	3538	}
nikapov	0:a1a69d32f310	3539
nikapov	0:a1a69d32f310	3540	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3541	/* Enable Minimum NON-APERTURE Spads */
nikapov	0:a1a69d32f310	3542	current_spad_index = 0;
nikapov	0:a1a69d32f310	3543	last_spad_index = current_spad_index;
nikapov	0:a1a69d32f310	3544	need_apt_spads = 0;
sepp_nepp	7:41cbc431e1f4	3545	status = enable_ref_spads(need_apt_spads,
sepp_nepp	7:41cbc431e1f4	3546	_device->SpadData.RefGoodSpadMap,
sepp_nepp	7:41cbc431e1f4	3547	_device->SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3548	spad_array_size,
nikapov	0:a1a69d32f310	3549	start_select,
nikapov	0:a1a69d32f310	3550	current_spad_index,
nikapov	0:a1a69d32f310	3551	minimum_spad_count,
nikapov	0:a1a69d32f310	3552	&last_spad_index);
nikapov	0:a1a69d32f310	3553	}
nikapov	0:a1a69d32f310	3554
nikapov	0:a1a69d32f310	3555	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3556	current_spad_index = last_spad_index;
nikapov	0:a1a69d32f310	3557
sepp_nepp	7:41cbc431e1f4	3558	status = perform_ref_signal_measurement(&peak_signal_rate_ref);
nikapov	0:a1a69d32f310	3559	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	3560	(peak_signal_rate_ref > target_ref_rate)) {
nikapov	0:a1a69d32f310	3561	/* Signal rate measurement too high,
nikapov	0:a1a69d32f310	3562	* switch to APERTURE SPADs */
nikapov	0:a1a69d32f310	3563
nikapov	0:a1a69d32f310	3564	for (index = 0; index < spad_array_size; index++) {
sepp_nepp	7:41cbc431e1f4	3565	_device->SpadData.RefSpadEnables[index] = 0;
nikapov	0:a1a69d32f310	3566	}
nikapov	0:a1a69d32f310	3567
nikapov	0:a1a69d32f310	3568
nikapov	0:a1a69d32f310	3569	/* Increment to the first APERTURE spad */
nikapov	0:a1a69d32f310	3570	while ((is_aperture(start_select + current_spad_index)
nikapov	0:a1a69d32f310	3571	== 0) && (current_spad_index < max_spad_count)) {
nikapov	0:a1a69d32f310	3572	current_spad_index++;
nikapov	0:a1a69d32f310	3573	}
nikapov	0:a1a69d32f310	3574
nikapov	0:a1a69d32f310	3575	need_apt_spads = 1;
nikapov	0:a1a69d32f310	3576
sepp_nepp	7:41cbc431e1f4	3577	status = enable_ref_spads(need_apt_spads,
sepp_nepp	7:41cbc431e1f4	3578	_device->SpadData.RefGoodSpadMap,
sepp_nepp	7:41cbc431e1f4	3579	_device->SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3580	spad_array_size,
nikapov	0:a1a69d32f310	3581	start_select,
nikapov	0:a1a69d32f310	3582	current_spad_index,
nikapov	0:a1a69d32f310	3583	minimum_spad_count,
nikapov	0:a1a69d32f310	3584	&last_spad_index);
nikapov	0:a1a69d32f310	3585
nikapov	0:a1a69d32f310	3586	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3587	current_spad_index = last_spad_index;
sepp_nepp	7:41cbc431e1f4	3588	status = perform_ref_signal_measurement(&peak_signal_rate_ref);
nikapov	0:a1a69d32f310	3589
nikapov	0:a1a69d32f310	3590	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	3591	(peak_signal_rate_ref > target_ref_rate)) {
nikapov	0:a1a69d32f310	3592	/* Signal rate still too high after
nikapov	0:a1a69d32f310	3593	* setting the minimum number of
nikapov	0:a1a69d32f310	3594	* APERTURE spads. Can do no more
nikapov	0:a1a69d32f310	3595	* therefore set the min number of
nikapov	0:a1a69d32f310	3596	* aperture spads as the result.
nikapov	0:a1a69d32f310	3597	*/
nikapov	0:a1a69d32f310	3598	is_aperture_spads_int = 1;
nikapov	0:a1a69d32f310	3599	ref_spad_count_int = minimum_spad_count;
nikapov	0:a1a69d32f310	3600	}
nikapov	0:a1a69d32f310	3601	}
nikapov	0:a1a69d32f310	3602	} else {
nikapov	0:a1a69d32f310	3603	need_apt_spads = 0;
nikapov	0:a1a69d32f310	3604	}
nikapov	0:a1a69d32f310	3605	}
nikapov	0:a1a69d32f310	3606
nikapov	0:a1a69d32f310	3607	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	3608	(peak_signal_rate_ref < target_ref_rate)) {
sepp_nepp	7:41cbc431e1f4	3609	/* At this point,the minimum number of either aperture
nikapov	0:a1a69d32f310	3610	* or non-aperture spads have been set. Proceed to add
nikapov	0:a1a69d32f310	3611	* spads and perform measurements until the target
nikapov	0:a1a69d32f310	3612	* reference is reached.
nikapov	0:a1a69d32f310	3613	*/
nikapov	0:a1a69d32f310	3614	is_aperture_spads_int = need_apt_spads;
nikapov	0:a1a69d32f310	3615	ref_spad_count_int = minimum_spad_count;
nikapov	0:a1a69d32f310	3616
sepp_nepp	7:41cbc431e1f4	3617	memcpy(last_spad_array,_device->SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3618	spad_array_size);
nikapov	0:a1a69d32f310	3619	last_signal_rate_diff = abs(peak_signal_rate_ref -
nikapov	0:a1a69d32f310	3620	target_ref_rate);
nikapov	0:a1a69d32f310	3621	complete = 0;
nikapov	0:a1a69d32f310	3622
nikapov	0:a1a69d32f310	3623	while (!complete) {
sepp_nepp	7:41cbc431e1f4	3624	get_next_good_spad(_device->SpadData.RefGoodSpadMap,
sepp_nepp	7:41cbc431e1f4	3625	spad_array_size,current_spad_index,
nikapov	0:a1a69d32f310	3626	&next_good_spad);
nikapov	0:a1a69d32f310	3627
nikapov	0:a1a69d32f310	3628	if (next_good_spad == -1) {
nikapov	0:a1a69d32f310	3629	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	3630	break;
nikapov	0:a1a69d32f310	3631	}
nikapov	0:a1a69d32f310	3632
sepp_nepp	7:41cbc431e1f4	3633	/* Cannot combine Aperture and Non-Aperture spads,so
nikapov	0:a1a69d32f310	3634	* ensure the current spad is of the correct type.
nikapov	0:a1a69d32f310	3635	*/
nikapov	0:a1a69d32f310	3636	if (is_aperture((uint32_t)start_select + next_good_spad) !=
nikapov	0:a1a69d32f310	3637	need_apt_spads) {
nikapov	0:a1a69d32f310	3638	/* At this point we have enabled the maximum
nikapov	0:a1a69d32f310	3639	* number of Aperture spads.
nikapov	0:a1a69d32f310	3640	*/
nikapov	0:a1a69d32f310	3641	complete = 1;
nikapov	0:a1a69d32f310	3642	break;
nikapov	0:a1a69d32f310	3643	}
nikapov	0:a1a69d32f310	3644
nikapov	0:a1a69d32f310	3645	(ref_spad_count_int)++;
nikapov	0:a1a69d32f310	3646
nikapov	0:a1a69d32f310	3647	current_spad_index = next_good_spad;
sepp_nepp	7:41cbc431e1f4	3648	status = enable_spad_bit(_device->SpadData.RefSpadEnables,
sepp_nepp	7:41cbc431e1f4	3649	spad_array_size,current_spad_index);
nikapov	0:a1a69d32f310	3650
nikapov	0:a1a69d32f310	3651	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3652	current_spad_index++;
nikapov	0:a1a69d32f310	3653	/* Proceed to apply the additional spad and
nikapov	0:a1a69d32f310	3654	* perform measurement. */
sepp_nepp	7:41cbc431e1f4	3655	status = set_ref_spad_map(_device->SpadData.RefSpadEnables);
nikapov	0:a1a69d32f310	3656	}
nikapov	0:a1a69d32f310	3657
nikapov	0:a1a69d32f310	3658	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3659	break;
nikapov	0:a1a69d32f310	3660	}
nikapov	0:a1a69d32f310	3661
sepp_nepp	7:41cbc431e1f4	3662	status = perform_ref_signal_measurement(&peak_signal_rate_ref);
nikapov	0:a1a69d32f310	3663
nikapov	0:a1a69d32f310	3664	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3665	break;
nikapov	0:a1a69d32f310	3666	}
nikapov	0:a1a69d32f310	3667
nikapov	0:a1a69d32f310	3668	signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate);
nikapov	0:a1a69d32f310	3669
nikapov	0:a1a69d32f310	3670	if (peak_signal_rate_ref > target_ref_rate) {
nikapov	0:a1a69d32f310	3671	/* Select the spad map that provides the
nikapov	0:a1a69d32f310	3672	* measurement closest to the target rate,
nikapov	0:a1a69d32f310	3673	* either above or below it.
nikapov	0:a1a69d32f310	3674	*/
nikapov	0:a1a69d32f310	3675	if (signal_rate_diff > last_signal_rate_diff) {
nikapov	0:a1a69d32f310	3676	/* Previous spad map produced a closer
sepp_nepp	7:41cbc431e1f4	3677	* measurement,so choose this. */
sepp_nepp	7:41cbc431e1f4	3678	status = set_ref_spad_map(last_spad_array);
sepp_nepp	7:41cbc431e1f4	3679	memcpy(_device->SpadData.RefSpadEnables,
sepp_nepp	7:41cbc431e1f4	3680	last_spad_array,spad_array_size);
nikapov	0:a1a69d32f310	3681
nikapov	0:a1a69d32f310	3682	(ref_spad_count_int)--;
nikapov	0:a1a69d32f310	3683	}
nikapov	0:a1a69d32f310	3684	complete = 1;
nikapov	0:a1a69d32f310	3685	} else {
nikapov	0:a1a69d32f310	3686	/* Continue to add spads */
nikapov	0:a1a69d32f310	3687	last_signal_rate_diff = signal_rate_diff;
nikapov	0:a1a69d32f310	3688	memcpy(last_spad_array,
sepp_nepp	7:41cbc431e1f4	3689	_device->SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3690	spad_array_size);
nikapov	0:a1a69d32f310	3691	}
nikapov	0:a1a69d32f310	3692
nikapov	0:a1a69d32f310	3693	} /* while */
nikapov	0:a1a69d32f310	3694	}
nikapov	0:a1a69d32f310	3695
nikapov	0:a1a69d32f310	3696	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3697	*ref_spad_count = ref_spad_count_int;
nikapov	0:a1a69d32f310	3698	*is_aperture_spads = is_aperture_spads_int;
nikapov	0:a1a69d32f310	3699
sepp_nepp	7:41cbc431e1f4	3700	_device->DeviceSpecificParameters.RefSpadsInitialised = 1;
sepp_nepp	7:41cbc431e1f4	3701	_device->DeviceSpecificParameters.ReferenceSpadCount = (uint8_t)(*ref_spad_count);
sepp_nepp	7:41cbc431e1f4	3702	_device->DeviceSpecificParameters.ReferenceSpadType = *is_aperture_spads;
nikapov	0:a1a69d32f310	3703	}
nikapov	0:a1a69d32f310	3704
nikapov	0:a1a69d32f310	3705	return status;
nikapov	0:a1a69d32f310	3706	}
nikapov	0:a1a69d32f310	3707
sepp_nepp	7:41cbc431e1f4	3708	VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(uint32_t count,uint8_t is_aperture_spads)
nikapov	0:a1a69d32f310	3709	{
nikapov	0:a1a69d32f310	3710	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3711	uint32_t current_spad_index = 0;
nikapov	0:a1a69d32f310	3712	uint8_t start_select = 0xB4;
nikapov	0:a1a69d32f310	3713	uint32_t spad_array_size = 6;
nikapov	0:a1a69d32f310	3714	uint32_t max_spad_count = 44;
nikapov	0:a1a69d32f310	3715	uint32_t last_spad_index;
nikapov	0:a1a69d32f310	3716	uint32_t index;
nikapov	0:a1a69d32f310	3717
nikapov	0:a1a69d32f310	3718	/*
sepp_nepp	7:41cbc431e1f4	3719	* This function applies a requested number of reference spads,either
nikapov	0:a1a69d32f310	3720	* aperture or
sepp_nepp	7:41cbc431e1f4	3721	* non-aperture,as requested.
nikapov	0:a1a69d32f310	3722	* The good spad map will be applied.
nikapov	0:a1a69d32f310	3723	*/
nikapov	0:a1a69d32f310	3724
sepp_nepp	7:41cbc431e1f4	3725	status = VL53L0X_write_byte(0xFF,0x01);
nikapov	0:a1a69d32f310	3726
nikapov	0:a1a69d32f310	3727	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3728	status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
Davidroid	2:d07edeaff6f1	3729	}
nikapov	0:a1a69d32f310	3730
nikapov	0:a1a69d32f310	3731	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3732	status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
Davidroid	2:d07edeaff6f1	3733	}
nikapov	0:a1a69d32f310	3734
nikapov	0:a1a69d32f310	3735	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3736	status = VL53L0X_write_byte(0xFF,0x00);
Davidroid	2:d07edeaff6f1	3737	}
nikapov	0:a1a69d32f310	3738
nikapov	0:a1a69d32f310	3739	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3740	status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
nikapov	0:a1a69d32f310	3741	start_select);
Davidroid	2:d07edeaff6f1	3742	}
nikapov	0:a1a69d32f310	3743
nikapov	0:a1a69d32f310	3744	for (index = 0; index < spad_array_size; index++) {
sepp_nepp	7:41cbc431e1f4	3745	_device->SpadData.RefSpadEnables[index] = 0;
Davidroid	2:d07edeaff6f1	3746	}
nikapov	0:a1a69d32f310	3747
nikapov	0:a1a69d32f310	3748	if (is_aperture_spads) {
nikapov	0:a1a69d32f310	3749	/* Increment to the first APERTURE spad */
nikapov	0:a1a69d32f310	3750	while ((is_aperture(start_select + current_spad_index) == 0) &&
nikapov	0:a1a69d32f310	3751	(current_spad_index < max_spad_count)) {
nikapov	0:a1a69d32f310	3752	current_spad_index++;
nikapov	0:a1a69d32f310	3753	}
nikapov	0:a1a69d32f310	3754	}
sepp_nepp	7:41cbc431e1f4	3755	status = enable_ref_spads(is_aperture_spads,
sepp_nepp	7:41cbc431e1f4	3756	_device->SpadData.RefGoodSpadMap,
sepp_nepp	7:41cbc431e1f4	3757	_device->SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3758	spad_array_size,
nikapov	0:a1a69d32f310	3759	start_select,
nikapov	0:a1a69d32f310	3760	current_spad_index,
nikapov	0:a1a69d32f310	3761	count,
nikapov	0:a1a69d32f310	3762	&last_spad_index);
nikapov	0:a1a69d32f310	3763
nikapov	0:a1a69d32f310	3764	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3765	_device->DeviceSpecificParameters.RefSpadsInitialised = 1;
sepp_nepp	7:41cbc431e1f4	3766	_device->DeviceSpecificParameters.ReferenceSpadCount = (uint8_t)(count);
sepp_nepp	7:41cbc431e1f4	3767	_device->DeviceSpecificParameters.ReferenceSpadType = is_aperture_spads;
nikapov	0:a1a69d32f310	3768	}
nikapov	0:a1a69d32f310	3769
nikapov	0:a1a69d32f310	3770	return status;
nikapov	0:a1a69d32f310	3771	}
nikapov	0:a1a69d32f310	3772
sepp_nepp	7:41cbc431e1f4	3773	VL53L0X_Error VL53L0X::VL53L0X_wait_device_booted()
nikapov	0:a1a69d32f310	3774	{
nikapov	0:a1a69d32f310	3775	VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
sepp_nepp	5:b95f6951f7d5	3776
nikapov	0:a1a69d32f310	3777
nikapov	0:a1a69d32f310	3778	/* not implemented on VL53L0X */
nikapov	0:a1a69d32f310	3779
sepp_nepp	5:b95f6951f7d5	3780
nikapov	0:a1a69d32f310	3781	return status;
nikapov	0:a1a69d32f310	3782	}
nikapov	0:a1a69d32f310	3783
sepp_nepp	7:41cbc431e1f4	3784	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t *p_vhv_settings,
nikapov	0:a1a69d32f310	3785	uint8_t *p_phase_cal)
nikapov	0:a1a69d32f310	3786	{
nikapov	0:a1a69d32f310	3787	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	3788
nikapov	0:a1a69d32f310	3789
sepp_nepp	7:41cbc431e1f4	3790	status = VL53L0X_perform_ref_calibration(p_vhv_settings,
sepp_nepp	7:41cbc431e1f4	3791	p_phase_cal,1);
nikapov	0:a1a69d32f310	3792
sepp_nepp	5:b95f6951f7d5	3793
nikapov	0:a1a69d32f310	3794	return status;
nikapov	0:a1a69d32f310	3795	}
nikapov	0:a1a69d32f310	3796
sepp_nepp	7:41cbc431e1f4	3797	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(uint32_t ref_spad_count,uint8_t is_aperture_spads)
nikapov	0:a1a69d32f310	3798	{
nikapov	0:a1a69d32f310	3799	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	3800
nikapov	0:a1a69d32f310	3801
sepp_nepp	7:41cbc431e1f4	3802	status = wrapped_VL53L0X_perform_ref_spad_management(ref_spad_count,
nikapov	0:a1a69d32f310	3803	is_aperture_spads);
nikapov	0:a1a69d32f310	3804
sepp_nepp	5:b95f6951f7d5	3805
nikapov	0:a1a69d32f310	3806
nikapov	0:a1a69d32f310	3807	return status;
nikapov	0:a1a69d32f310	3808	}
nikapov	0:a1a69d32f310	3809
nikapov	0:a1a69d32f310	3810	/* Group PAL Init Functions */
sepp_nepp	7:41cbc431e1f4	3811	VL53L0X_Error VL53L0X::VL53L0X_set_device_address(uint8_t device_address)
nikapov	0:a1a69d32f310	3812	{
nikapov	0:a1a69d32f310	3813	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	3814
nikapov	0:a1a69d32f310	3815
sepp_nepp	7:41cbc431e1f4	3816	status = VL53L0X_write_byte(VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
nikapov	0:a1a69d32f310	3817	device_address / 2);
nikapov	0:a1a69d32f310	3818
sepp_nepp	5:b95f6951f7d5	3819
nikapov	0:a1a69d32f310	3820	return status;
nikapov	0:a1a69d32f310	3821	}
nikapov	0:a1a69d32f310	3822
sepp_nepp	7:41cbc431e1f4	3823	VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config(uint8_t pin,
sepp_nepp	7:41cbc431e1f4	3824	VL53L0X_DeviceModes device_mode,VL53L0X_GpioFunctionality functionality,
nikapov	0:a1a69d32f310	3825	VL53L0X_InterruptPolarity polarity)
nikapov	0:a1a69d32f310	3826	{
nikapov	0:a1a69d32f310	3827	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3828	uint8_t data;
nikapov	0:a1a69d32f310	3829
sepp_nepp	5:b95f6951f7d5	3830
nikapov	0:a1a69d32f310	3831
nikapov	0:a1a69d32f310	3832	if (pin != 0) {
nikapov	0:a1a69d32f310	3833	status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
nikapov	0:a1a69d32f310	3834	} else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
nikapov	0:a1a69d32f310	3835	if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
nikapov	0:a1a69d32f310	3836	data = 0x10;
nikapov	0:a1a69d32f310	3837	} else {
nikapov	0:a1a69d32f310	3838	data = 1;
nikapov	0:a1a69d32f310	3839	}
nikapov	0:a1a69d32f310	3840
sepp_nepp	7:41cbc431e1f4	3841	status = VL53L0X_write_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,data);
nikapov	0:a1a69d32f310	3842
nikapov	0:a1a69d32f310	3843	} else {
Davidroid	2:d07edeaff6f1	3844	if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) {
Davidroid	2:d07edeaff6f1	3845
sepp_nepp	7:41cbc431e1f4	3846	status \|= VL53L0X_write_byte(0xff,0x01);
sepp_nepp	7:41cbc431e1f4	3847	status \|= VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	3848
sepp_nepp	7:41cbc431e1f4	3849	status \|= VL53L0X_write_byte(0xff,0x00);
sepp_nepp	7:41cbc431e1f4	3850	status \|= VL53L0X_write_byte(0x80,0x01);
sepp_nepp	7:41cbc431e1f4	3851	status \|= VL53L0X_write_byte(0x85,0x02);
sepp_nepp	7:41cbc431e1f4	3852
sepp_nepp	7:41cbc431e1f4	3853	status \|= VL53L0X_write_byte(0xff,0x04);
sepp_nepp	7:41cbc431e1f4	3854	status \|= VL53L0X_write_byte(0xcd,0x00);
sepp_nepp	7:41cbc431e1f4	3855	status \|= VL53L0X_write_byte(0xcc,0x11);
sepp_nepp	7:41cbc431e1f4	3856
sepp_nepp	7:41cbc431e1f4	3857	status \|= VL53L0X_write_byte(0xff,0x07);
sepp_nepp	7:41cbc431e1f4	3858	status \|= VL53L0X_write_byte(0xbe,0x00);
sepp_nepp	7:41cbc431e1f4	3859
sepp_nepp	7:41cbc431e1f4	3860	status \|= VL53L0X_write_byte(0xff,0x06);
sepp_nepp	7:41cbc431e1f4	3861	status \|= VL53L0X_write_byte(0xcc,0x09);
sepp_nepp	7:41cbc431e1f4	3862
sepp_nepp	7:41cbc431e1f4	3863	status \|= VL53L0X_write_byte(0xff,0x00);
sepp_nepp	7:41cbc431e1f4	3864	status \|= VL53L0X_write_byte(0xff,0x01);
sepp_nepp	7:41cbc431e1f4	3865	status \|= VL53L0X_write_byte(0x00,0x00);
Davidroid	2:d07edeaff6f1	3866
Davidroid	2:d07edeaff6f1	3867	} else {
Davidroid	2:d07edeaff6f1	3868
Davidroid	2:d07edeaff6f1	3869	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	3870	switch (functionality) {
Davidroid	2:d07edeaff6f1	3871	case VL53L0X_GPIOFUNCTIONALITY_OFF:
Davidroid	2:d07edeaff6f1	3872	data = 0x00;
Davidroid	2:d07edeaff6f1	3873	break;
Davidroid	2:d07edeaff6f1	3874	case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
Davidroid	2:d07edeaff6f1	3875	data = 0x01;
Davidroid	2:d07edeaff6f1	3876	break;
Davidroid	2:d07edeaff6f1	3877	case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
Davidroid	2:d07edeaff6f1	3878	data = 0x02;
Davidroid	2:d07edeaff6f1	3879	break;
Davidroid	2:d07edeaff6f1	3880	case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
Davidroid	2:d07edeaff6f1	3881	data = 0x03;
Davidroid	2:d07edeaff6f1	3882	break;
Davidroid	2:d07edeaff6f1	3883	case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
Davidroid	2:d07edeaff6f1	3884	data = 0x04;
Davidroid	2:d07edeaff6f1	3885	break;
Davidroid	2:d07edeaff6f1	3886	default:
Davidroid	2:d07edeaff6f1	3887	status =
Davidroid	2:d07edeaff6f1	3888	VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
Davidroid	2:d07edeaff6f1	3889	}
Davidroid	2:d07edeaff6f1	3890	}
Davidroid	2:d07edeaff6f1	3891
Davidroid	2:d07edeaff6f1	3892	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3893	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO,data);
Davidroid	2:d07edeaff6f1	3894	}
Davidroid	2:d07edeaff6f1	3895
Davidroid	2:d07edeaff6f1	3896	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	3897	if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
Davidroid	2:d07edeaff6f1	3898	data = 0;
Davidroid	2:d07edeaff6f1	3899	} else {
Davidroid	2:d07edeaff6f1	3900	data = (uint8_t)(1 << 4);
Davidroid	2:d07edeaff6f1	3901	}
sepp_nepp	7:41cbc431e1f4	3902	status = VL53L0X_update_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,data);
Davidroid	2:d07edeaff6f1	3903	}
Davidroid	2:d07edeaff6f1	3904
Davidroid	2:d07edeaff6f1	3905	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3906	_device->DeviceSpecificParameters.Pin0GpioFunctionality = functionality;
Davidroid	2:d07edeaff6f1	3907	}
Davidroid	2:d07edeaff6f1	3908
Davidroid	2:d07edeaff6f1	3909	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3910	status = VL53L0X_clear_interrupt_mask(0);
Davidroid	2:d07edeaff6f1	3911	}
Davidroid	2:d07edeaff6f1	3912	}
nikapov	0:a1a69d32f310	3913	}
sepp_nepp	5:b95f6951f7d5	3914
nikapov	0:a1a69d32f310	3915	return status;
nikapov	0:a1a69d32f310	3916	}
nikapov	0:a1a69d32f310	3917
sepp_nepp	7:41cbc431e1f4	3918	VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable(uint8_t *p_enabled)
nikapov	0:a1a69d32f310	3919	{
nikapov	0:a1a69d32f310	3920	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	3921
nikapov	0:a1a69d32f310	3922
sepp_nepp	7:41cbc431e1f4	3923	status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_RANGE_CONFIG,p_enabled);
nikapov	0:a1a69d32f310	3924
nikapov	0:a1a69d32f310	3925	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3926	p_enabled = (p_enabled & 1);
nikapov	0:a1a69d32f310	3927	}
nikapov	0:a1a69d32f310	3928
sepp_nepp	5:b95f6951f7d5	3929
nikapov	0:a1a69d32f310	3930	return status;
nikapov	0:a1a69d32f310	3931	}
nikapov	0:a1a69d32f310	3932
nikapov	0:a1a69d32f310	3933	uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
nikapov	0:a1a69d32f310	3934	{
nikapov	0:a1a69d32f310	3935	/*!
nikapov	0:a1a69d32f310	3936	* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
nikapov	0:a1a69d32f310	3937	*/
nikapov	0:a1a69d32f310	3938
nikapov	0:a1a69d32f310	3939	uint16_t encoded_timeout = 0;
nikapov	0:a1a69d32f310	3940	uint32_t ls_byte = 0;
nikapov	0:a1a69d32f310	3941	uint16_t ms_byte = 0;
nikapov	0:a1a69d32f310	3942
nikapov	0:a1a69d32f310	3943	if (timeout_macro_clks > 0) {
nikapov	0:a1a69d32f310	3944	ls_byte = timeout_macro_clks - 1;
nikapov	0:a1a69d32f310	3945
nikapov	0:a1a69d32f310	3946	while ((ls_byte & 0xFFFFFF00) > 0) {
nikapov	0:a1a69d32f310	3947	ls_byte = ls_byte >> 1;
nikapov	0:a1a69d32f310	3948	ms_byte++;
nikapov	0:a1a69d32f310	3949	}
nikapov	0:a1a69d32f310	3950
nikapov	0:a1a69d32f310	3951	encoded_timeout = (ms_byte << 8)
nikapov	0:a1a69d32f310	3952	+ (uint16_t)(ls_byte & 0x000000FF);
nikapov	0:a1a69d32f310	3953	}
nikapov	0:a1a69d32f310	3954
nikapov	0:a1a69d32f310	3955	return encoded_timeout;
nikapov	0:a1a69d32f310	3956
nikapov	0:a1a69d32f310	3957	}
nikapov	0:a1a69d32f310	3958
sepp_nepp	7:41cbc431e1f4	3959	VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id,
nikapov	0:a1a69d32f310	3960	uint32_t timeout_micro_secs)
nikapov	0:a1a69d32f310	3961	{
nikapov	0:a1a69d32f310	3962	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3963	uint8_t current_vcsel_pulse_period_p_clk;
nikapov	0:a1a69d32f310	3964	uint8_t msrc_encoded_time_out;
nikapov	0:a1a69d32f310	3965	uint16_t pre_range_encoded_time_out;
nikapov	0:a1a69d32f310	3966	uint16_t pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	3967	uint16_t msrc_range_time_out_m_clks;
nikapov	0:a1a69d32f310	3968	uint32_t final_range_time_out_m_clks;
nikapov	0:a1a69d32f310	3969	uint16_t final_range_encoded_time_out;
nikapov	0:a1a69d32f310	3970	VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov	0:a1a69d32f310	3971
nikapov	0:a1a69d32f310	3972	if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) \|\|
nikapov	0:a1a69d32f310	3973	(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) \|\|
nikapov	0:a1a69d32f310	3974	(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
nikapov	0:a1a69d32f310	3975
sepp_nepp	7:41cbc431e1f4	3976	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	3977	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	3978
nikapov	0:a1a69d32f310	3979	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3980	msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(timeout_micro_secs,
nikapov	0:a1a69d32f310	3981	(uint8_t)current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	3982
nikapov	0:a1a69d32f310	3983	if (msrc_range_time_out_m_clks > 256) {
nikapov	0:a1a69d32f310	3984	msrc_encoded_time_out = 255;
nikapov	0:a1a69d32f310	3985	} else {
nikapov	0:a1a69d32f310	3986	msrc_encoded_time_out =
nikapov	0:a1a69d32f310	3987	(uint8_t)msrc_range_time_out_m_clks - 1;
nikapov	0:a1a69d32f310	3988	}
nikapov	0:a1a69d32f310	3989
sepp_nepp	7:41cbc431e1f4	3990	_device->DeviceSpecificParameters.LastEncodedTimeout = msrc_encoded_time_out;
nikapov	0:a1a69d32f310	3991	}
nikapov	0:a1a69d32f310	3992
nikapov	0:a1a69d32f310	3993	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	3994	status = VL53L0X_write_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
nikapov	0:a1a69d32f310	3995	msrc_encoded_time_out);
nikapov	0:a1a69d32f310	3996	}
nikapov	0:a1a69d32f310	3997	} else {
nikapov	0:a1a69d32f310	3998
nikapov	0:a1a69d32f310	3999	if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
nikapov	0:a1a69d32f310	4000
nikapov	0:a1a69d32f310	4001	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4002	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4003	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4004	pre_range_time_out_m_clks =
sepp_nepp	7:41cbc431e1f4	4005	VL53L0X_calc_timeout_mclks(timeout_micro_secs,
nikapov	0:a1a69d32f310	4006	(uint8_t)current_vcsel_pulse_period_p_clk);
sepp_nepp	7:41cbc431e1f4	4007	pre_range_encoded_time_out = VL53L0X_encode_timeout(pre_range_time_out_m_clks);
sepp_nepp	7:41cbc431e1f4	4008
sepp_nepp	7:41cbc431e1f4	4009	_device->DeviceSpecificParameters.LastEncodedTimeout = pre_range_encoded_time_out;
nikapov	0:a1a69d32f310	4010	}
nikapov	0:a1a69d32f310	4011
nikapov	0:a1a69d32f310	4012	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4013	status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov	0:a1a69d32f310	4014	pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4015	}
nikapov	0:a1a69d32f310	4016
nikapov	0:a1a69d32f310	4017	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4018	_device->DeviceSpecificParameters.PreRangeTimeoutMicroSecs=timeout_micro_secs;
nikapov	0:a1a69d32f310	4019	}
nikapov	0:a1a69d32f310	4020	} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov	0:a1a69d32f310	4021
sepp_nepp	7:41cbc431e1f4	4022	/* For the final range timeout,the pre-range timeout
nikapov	0:a1a69d32f310	4023	* must be added. To do this both final and pre-range
nikapov	0:a1a69d32f310	4024	* timeouts must be expressed in macro periods MClks
nikapov	0:a1a69d32f310	4025	* because they have different vcsel periods.
nikapov	0:a1a69d32f310	4026	*/
nikapov	0:a1a69d32f310	4027
sepp_nepp	7:41cbc431e1f4	4028	VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
nikapov	0:a1a69d32f310	4029	pre_range_time_out_m_clks = 0;
nikapov	0:a1a69d32f310	4030	if (scheduler_sequence_steps.PreRangeOn) {
nikapov	0:a1a69d32f310	4031
nikapov	0:a1a69d32f310	4032	/* Retrieve PRE-RANGE VCSEL Period */
sepp_nepp	7:41cbc431e1f4	4033	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4034	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4035
nikapov	0:a1a69d32f310	4036	/* Retrieve PRE-RANGE Timeout in Macro periods
nikapov	0:a1a69d32f310	4037	* (MCLKS) */
nikapov	0:a1a69d32f310	4038	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4039	status = VL53L0X_read_word(0x51,
nikapov	0:a1a69d32f310	4040	&pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4041	pre_range_time_out_m_clks =
sepp_nepp	7:41cbc431e1f4	4042	VL53L0X_decode_timeout(pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4043	}
nikapov	0:a1a69d32f310	4044	}
nikapov	0:a1a69d32f310	4045
nikapov	0:a1a69d32f310	4046	/* Calculate FINAL RANGE Timeout in Macro Periods
nikapov	0:a1a69d32f310	4047	* (MCLKS) and add PRE-RANGE value
nikapov	0:a1a69d32f310	4048	*/
nikapov	0:a1a69d32f310	4049	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4050	status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov	0:a1a69d32f310	4051	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4052	}
nikapov	0:a1a69d32f310	4053	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4054	final_range_time_out_m_clks =
sepp_nepp	7:41cbc431e1f4	4055	VL53L0X_calc_timeout_mclks(timeout_micro_secs,
nikapov	0:a1a69d32f310	4056	(uint8_t) current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4057
nikapov	0:a1a69d32f310	4058	final_range_time_out_m_clks += pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	4059
nikapov	0:a1a69d32f310	4060	final_range_encoded_time_out =
nikapov	0:a1a69d32f310	4061	VL53L0X_encode_timeout(final_range_time_out_m_clks);
nikapov	0:a1a69d32f310	4062
nikapov	0:a1a69d32f310	4063	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4064	status = VL53L0X_write_word(0x71,
nikapov	0:a1a69d32f310	4065	final_range_encoded_time_out);
nikapov	0:a1a69d32f310	4066	}
nikapov	0:a1a69d32f310	4067
nikapov	0:a1a69d32f310	4068	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4069	_device->DeviceSpecificParameters.FinalRangeTimeoutMicroSecs = timeout_micro_secs;
nikapov	0:a1a69d32f310	4070	}
nikapov	0:a1a69d32f310	4071	}
nikapov	0:a1a69d32f310	4072	} else {
nikapov	0:a1a69d32f310	4073	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4074	}
nikapov	0:a1a69d32f310	4075
nikapov	0:a1a69d32f310	4076	}
nikapov	0:a1a69d32f310	4077	return status;
nikapov	0:a1a69d32f310	4078	}
nikapov	0:a1a69d32f310	4079
sepp_nepp	7:41cbc431e1f4	4080	VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
nikapov	0:a1a69d32f310	4081	{
nikapov	0:a1a69d32f310	4082	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4083	uint32_t final_range_timing_budget_micro_seconds;
nikapov	0:a1a69d32f310	4084	VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov	0:a1a69d32f310	4085	uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
nikapov	0:a1a69d32f310	4086	uint32_t start_overhead_micro_seconds = 1910;
nikapov	0:a1a69d32f310	4087	uint32_t end_overhead_micro_seconds = 960;
nikapov	0:a1a69d32f310	4088	uint32_t msrc_overhead_micro_seconds = 660;
nikapov	0:a1a69d32f310	4089	uint32_t tcc_overhead_micro_seconds = 590;
nikapov	0:a1a69d32f310	4090	uint32_t dss_overhead_micro_seconds = 690;
nikapov	0:a1a69d32f310	4091	uint32_t pre_range_overhead_micro_seconds = 660;
nikapov	0:a1a69d32f310	4092	uint32_t final_range_overhead_micro_seconds = 550;
nikapov	0:a1a69d32f310	4093	uint32_t pre_range_timeout_micro_seconds = 0;
nikapov	0:a1a69d32f310	4094	uint32_t c_min_timing_budget_micro_seconds = 20000;
nikapov	0:a1a69d32f310	4095	uint32_t sub_timeout = 0;
nikapov	0:a1a69d32f310	4096
sepp_nepp	5:b95f6951f7d5	4097
nikapov	0:a1a69d32f310	4098
nikapov	0:a1a69d32f310	4099	if (measurement_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	4100	< c_min_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4101	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4102	return status;
nikapov	0:a1a69d32f310	4103	}
nikapov	0:a1a69d32f310	4104
nikapov	0:a1a69d32f310	4105	final_range_timing_budget_micro_seconds =
nikapov	0:a1a69d32f310	4106	measurement_timing_budget_micro_seconds -
nikapov	0:a1a69d32f310	4107	(start_overhead_micro_seconds + end_overhead_micro_seconds);
nikapov	0:a1a69d32f310	4108
sepp_nepp	7:41cbc431e1f4	4109	status = VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
nikapov	0:a1a69d32f310	4110
nikapov	0:a1a69d32f310	4111	if (status == VL53L0X_ERROR_NONE &&
nikapov	0:a1a69d32f310	4112	(scheduler_sequence_steps.TccOn \|\|
nikapov	0:a1a69d32f310	4113	scheduler_sequence_steps.MsrcOn \|\|
nikapov	0:a1a69d32f310	4114	scheduler_sequence_steps.DssOn)) {
nikapov	0:a1a69d32f310	4115
sepp_nepp	7:41cbc431e1f4	4116	/* TCC,MSRC and DSS all share the same timeout */
sepp_nepp	7:41cbc431e1f4	4117	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC,
nikapov	0:a1a69d32f310	4118	&msrc_dcc_tcc_timeout_micro_seconds);
nikapov	0:a1a69d32f310	4119
sepp_nepp	7:41cbc431e1f4	4120	/* Subtract the TCC,MSRC and DSS timeouts if they are
nikapov	0:a1a69d32f310	4121	* enabled. */
nikapov	0:a1a69d32f310	4122
nikapov	0:a1a69d32f310	4123	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4124	return status;
nikapov	0:a1a69d32f310	4125	}
nikapov	0:a1a69d32f310	4126
nikapov	0:a1a69d32f310	4127	/* TCC */
nikapov	0:a1a69d32f310	4128	if (scheduler_sequence_steps.TccOn) {
nikapov	0:a1a69d32f310	4129
nikapov	0:a1a69d32f310	4130	sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
nikapov	0:a1a69d32f310	4131	+ tcc_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4132
nikapov	0:a1a69d32f310	4133	if (sub_timeout <
nikapov	0:a1a69d32f310	4134	final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4135	final_range_timing_budget_micro_seconds -=
nikapov	0:a1a69d32f310	4136	sub_timeout;
nikapov	0:a1a69d32f310	4137	} else {
nikapov	0:a1a69d32f310	4138	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4139	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4140	}
nikapov	0:a1a69d32f310	4141	}
nikapov	0:a1a69d32f310	4142
nikapov	0:a1a69d32f310	4143	if (status != VL53L0X_ERROR_NONE) {
sepp_nepp	5:b95f6951f7d5	4144
nikapov	0:a1a69d32f310	4145	return status;
nikapov	0:a1a69d32f310	4146	}
nikapov	0:a1a69d32f310	4147
nikapov	0:a1a69d32f310	4148	/* DSS */
nikapov	0:a1a69d32f310	4149	if (scheduler_sequence_steps.DssOn) {
nikapov	0:a1a69d32f310	4150
nikapov	0:a1a69d32f310	4151	sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	4152	dss_overhead_micro_seconds);
nikapov	0:a1a69d32f310	4153
nikapov	0:a1a69d32f310	4154	if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4155	final_range_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	4156	-= sub_timeout;
nikapov	0:a1a69d32f310	4157	} else {
nikapov	0:a1a69d32f310	4158	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4159	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4160	}
nikapov	0:a1a69d32f310	4161	} else if (scheduler_sequence_steps.MsrcOn) {
nikapov	0:a1a69d32f310	4162	/* MSRC */
nikapov	0:a1a69d32f310	4163	sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	4164	msrc_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4165
nikapov	0:a1a69d32f310	4166	if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4167	final_range_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	4168	-= sub_timeout;
nikapov	0:a1a69d32f310	4169	} else {
nikapov	0:a1a69d32f310	4170	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4171	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4172	}
nikapov	0:a1a69d32f310	4173	}
nikapov	0:a1a69d32f310	4174
nikapov	0:a1a69d32f310	4175	}
nikapov	0:a1a69d32f310	4176
nikapov	0:a1a69d32f310	4177	if (status != VL53L0X_ERROR_NONE) {
sepp_nepp	5:b95f6951f7d5	4178
nikapov	0:a1a69d32f310	4179	return status;
nikapov	0:a1a69d32f310	4180	}
nikapov	0:a1a69d32f310	4181
nikapov	0:a1a69d32f310	4182	if (scheduler_sequence_steps.PreRangeOn) {
nikapov	0:a1a69d32f310	4183
nikapov	0:a1a69d32f310	4184	/* Subtract the Pre-range timeout if enabled. */
nikapov	0:a1a69d32f310	4185
sepp_nepp	7:41cbc431e1f4	4186	status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	4187	&pre_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	4188
nikapov	0:a1a69d32f310	4189	sub_timeout = pre_range_timeout_micro_seconds +
nikapov	0:a1a69d32f310	4190	pre_range_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4191
nikapov	0:a1a69d32f310	4192	if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4193	final_range_timing_budget_micro_seconds -= sub_timeout;
nikapov	0:a1a69d32f310	4194	} else {
nikapov	0:a1a69d32f310	4195	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4196	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4197	}
nikapov	0:a1a69d32f310	4198	}
nikapov	0:a1a69d32f310	4199
nikapov	0:a1a69d32f310	4200
nikapov	0:a1a69d32f310	4201	if (status == VL53L0X_ERROR_NONE &&
nikapov	0:a1a69d32f310	4202	scheduler_sequence_steps.FinalRangeOn) {
nikapov	0:a1a69d32f310	4203
nikapov	0:a1a69d32f310	4204	final_range_timing_budget_micro_seconds -=
nikapov	0:a1a69d32f310	4205	final_range_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4206
nikapov	0:a1a69d32f310	4207	/* Final Range Timeout
nikapov	0:a1a69d32f310	4208	* Note that the final range timeout is determined by the timing
nikapov	0:a1a69d32f310	4209	* budget and the sum of all other timeouts within the sequence.
sepp_nepp	7:41cbc431e1f4	4210	* If there is no room for the final range timeout,then an error
nikapov	0:a1a69d32f310	4211	* will be set. Otherwise the remaining time will be applied to
nikapov	0:a1a69d32f310	4212	* the final range.
nikapov	0:a1a69d32f310	4213	*/
sepp_nepp	7:41cbc431e1f4	4214	status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	4215	final_range_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4216
sepp_nepp	7:41cbc431e1f4	4217	_device->CurrentParameters.MeasurementTimingBudgetMicroSeconds = measurement_timing_budget_micro_seconds;
nikapov	0:a1a69d32f310	4218	}
nikapov	0:a1a69d32f310	4219
sepp_nepp	5:b95f6951f7d5	4220
nikapov	0:a1a69d32f310	4221
nikapov	0:a1a69d32f310	4222	return status;
nikapov	0:a1a69d32f310	4223	}
nikapov	0:a1a69d32f310	4224
sepp_nepp	7:41cbc431e1f4	4225	VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
nikapov	0:a1a69d32f310	4226	{
nikapov	0:a1a69d32f310	4227	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	4228
nikapov	0:a1a69d32f310	4229
sepp_nepp	7:41cbc431e1f4	4230	status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4231
nikapov	0:a1a69d32f310	4232	return status;
nikapov	0:a1a69d32f310	4233	}
nikapov	0:a1a69d32f310	4234
sepp_nepp	7:41cbc431e1f4	4235	VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id,uint8_t sequence_step_enabled)
nikapov	0:a1a69d32f310	4236	{
nikapov	0:a1a69d32f310	4237	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4238	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	4239	uint8_t sequence_config_new = 0;
nikapov	0:a1a69d32f310	4240	uint32_t measurement_timing_budget_micro_seconds;
sepp_nepp	5:b95f6951f7d5	4241
nikapov	0:a1a69d32f310	4242
sepp_nepp	7:41cbc431e1f4	4243	status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	4244	&sequence_config);
nikapov	0:a1a69d32f310	4245
nikapov	0:a1a69d32f310	4246	sequence_config_new = sequence_config;
nikapov	0:a1a69d32f310	4247
nikapov	0:a1a69d32f310	4248	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4249	if (sequence_step_enabled == 1) {
nikapov	0:a1a69d32f310	4250
nikapov	0:a1a69d32f310	4251	/* Enable requested sequence step
nikapov	0:a1a69d32f310	4252	*/
nikapov	0:a1a69d32f310	4253	switch (sequence_step_id) {
nikapov	0:a1a69d32f310	4254	case VL53L0X_SEQUENCESTEP_TCC:
nikapov	0:a1a69d32f310	4255	sequence_config_new \|= 0x10;
nikapov	0:a1a69d32f310	4256	break;
nikapov	0:a1a69d32f310	4257	case VL53L0X_SEQUENCESTEP_DSS:
nikapov	0:a1a69d32f310	4258	sequence_config_new \|= 0x28;
nikapov	0:a1a69d32f310	4259	break;
nikapov	0:a1a69d32f310	4260	case VL53L0X_SEQUENCESTEP_MSRC:
nikapov	0:a1a69d32f310	4261	sequence_config_new \|= 0x04;
nikapov	0:a1a69d32f310	4262	break;
nikapov	0:a1a69d32f310	4263	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov	0:a1a69d32f310	4264	sequence_config_new \|= 0x40;
nikapov	0:a1a69d32f310	4265	break;
nikapov	0:a1a69d32f310	4266	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov	0:a1a69d32f310	4267	sequence_config_new \|= 0x80;
nikapov	0:a1a69d32f310	4268	break;
nikapov	0:a1a69d32f310	4269	default:
nikapov	0:a1a69d32f310	4270	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4271	}
nikapov	0:a1a69d32f310	4272	} else {
nikapov	0:a1a69d32f310	4273	/* Disable requested sequence step
nikapov	0:a1a69d32f310	4274	*/
nikapov	0:a1a69d32f310	4275	switch (sequence_step_id) {
nikapov	0:a1a69d32f310	4276	case VL53L0X_SEQUENCESTEP_TCC:
nikapov	0:a1a69d32f310	4277	sequence_config_new &= 0xef;
nikapov	0:a1a69d32f310	4278	break;
nikapov	0:a1a69d32f310	4279	case VL53L0X_SEQUENCESTEP_DSS:
nikapov	0:a1a69d32f310	4280	sequence_config_new &= 0xd7;
nikapov	0:a1a69d32f310	4281	break;
nikapov	0:a1a69d32f310	4282	case VL53L0X_SEQUENCESTEP_MSRC:
nikapov	0:a1a69d32f310	4283	sequence_config_new &= 0xfb;
nikapov	0:a1a69d32f310	4284	break;
nikapov	0:a1a69d32f310	4285	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov	0:a1a69d32f310	4286	sequence_config_new &= 0xbf;
nikapov	0:a1a69d32f310	4287	break;
nikapov	0:a1a69d32f310	4288	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov	0:a1a69d32f310	4289	sequence_config_new &= 0x7f;
nikapov	0:a1a69d32f310	4290	break;
nikapov	0:a1a69d32f310	4291	default:
nikapov	0:a1a69d32f310	4292	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4293	}
nikapov	0:a1a69d32f310	4294	}
nikapov	0:a1a69d32f310	4295	}
nikapov	0:a1a69d32f310	4296
nikapov	0:a1a69d32f310	4297	if (sequence_config_new != sequence_config) {
nikapov	0:a1a69d32f310	4298	/* Apply New Setting */
nikapov	0:a1a69d32f310	4299	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4300	status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,sequence_config_new);
nikapov	0:a1a69d32f310	4301	}
nikapov	0:a1a69d32f310	4302	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4303	_device->SequenceConfig = sequence_config_new;
nikapov	0:a1a69d32f310	4304	}
nikapov	0:a1a69d32f310	4305
nikapov	0:a1a69d32f310	4306
nikapov	0:a1a69d32f310	4307	/* Recalculate timing budget */
nikapov	0:a1a69d32f310	4308	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4309	measurement_timing_budget_micro_seconds = _device->CurrentParameters.MeasurementTimingBudgetMicroSeconds;
sepp_nepp	7:41cbc431e1f4	4310
sepp_nepp	7:41cbc431e1f4	4311	VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4312	}
nikapov	0:a1a69d32f310	4313	}
nikapov	0:a1a69d32f310	4314
sepp_nepp	5:b95f6951f7d5	4315
nikapov	0:a1a69d32f310	4316
nikapov	0:a1a69d32f310	4317	return status;
nikapov	0:a1a69d32f310	4318	}
nikapov	0:a1a69d32f310	4319
sepp_nepp	7:41cbc431e1f4	4320	VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable(uint16_t limit_check_id,
nikapov	0:a1a69d32f310	4321	uint8_t limit_check_enable)
nikapov	0:a1a69d32f310	4322	{
nikapov	0:a1a69d32f310	4323	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4324	FixPoint1616_t temp_fix1616 = 0;
nikapov	0:a1a69d32f310	4325	uint8_t limit_check_enable_int = 0;
nikapov	0:a1a69d32f310	4326	uint8_t limit_check_disable = 0;
nikapov	0:a1a69d32f310	4327	uint8_t temp8;
nikapov	0:a1a69d32f310	4328
sepp_nepp	5:b95f6951f7d5	4329
nikapov	0:a1a69d32f310	4330
nikapov	0:a1a69d32f310	4331	if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
nikapov	0:a1a69d32f310	4332	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4333	} else {
nikapov	0:a1a69d32f310	4334	if (limit_check_enable == 0) {
nikapov	0:a1a69d32f310	4335	temp_fix1616 = 0;
nikapov	0:a1a69d32f310	4336	limit_check_enable_int = 0;
nikapov	0:a1a69d32f310	4337	limit_check_disable = 1;
nikapov	0:a1a69d32f310	4338
nikapov	0:a1a69d32f310	4339	} else {
sepp_nepp	7:41cbc431e1f4	4340	temp_fix1616 = _device->CurrentParameters.LimitChecksValue[limit_check_id];
nikapov	0:a1a69d32f310	4341	limit_check_disable = 0;
nikapov	0:a1a69d32f310	4342	/* this to be sure to have either 0 or 1 */
nikapov	0:a1a69d32f310	4343	limit_check_enable_int = 1;
nikapov	0:a1a69d32f310	4344	}
nikapov	0:a1a69d32f310	4345
nikapov	0:a1a69d32f310	4346	switch (limit_check_id) {
nikapov	0:a1a69d32f310	4347
nikapov	0:a1a69d32f310	4348	case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov	0:a1a69d32f310	4349	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	4350	_device->CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_enable_int;
nikapov	0:a1a69d32f310	4351
nikapov	0:a1a69d32f310	4352	break;
nikapov	0:a1a69d32f310	4353
nikapov	0:a1a69d32f310	4354	case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
nikapov	0:a1a69d32f310	4355
sepp_nepp	7:41cbc431e1f4	4356	status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
nikapov	0:a1a69d32f310	4357	VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616));
nikapov	0:a1a69d32f310	4358
nikapov	0:a1a69d32f310	4359	break;
nikapov	0:a1a69d32f310	4360
nikapov	0:a1a69d32f310	4361	case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov	0:a1a69d32f310	4362
nikapov	0:a1a69d32f310	4363	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	4364	_device->CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_enable_int;
nikapov	0:a1a69d32f310	4365
nikapov	0:a1a69d32f310	4366	break;
nikapov	0:a1a69d32f310	4367
nikapov	0:a1a69d32f310	4368	case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov	0:a1a69d32f310	4369
nikapov	0:a1a69d32f310	4370	/* internal computation: */
sepp_nepp	7:41cbc431e1f4	4371	_device->CurrentParameters.LimitChecksEnable[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_enable_int;
nikapov	0:a1a69d32f310	4372
nikapov	0:a1a69d32f310	4373	break;
nikapov	0:a1a69d32f310	4374
nikapov	0:a1a69d32f310	4375	case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov	0:a1a69d32f310	4376
nikapov	0:a1a69d32f310	4377	temp8 = (uint8_t)(limit_check_disable << 1);
sepp_nepp	7:41cbc431e1f4	4378	status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL,
sepp_nepp	7:41cbc431e1f4	4379	0xFE,temp8);
nikapov	0:a1a69d32f310	4380
nikapov	0:a1a69d32f310	4381	break;
nikapov	0:a1a69d32f310	4382
nikapov	0:a1a69d32f310	4383	case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
nikapov	0:a1a69d32f310	4384
nikapov	0:a1a69d32f310	4385	temp8 = (uint8_t)(limit_check_disable << 4);
sepp_nepp	7:41cbc431e1f4	4386	status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL,
sepp_nepp	7:41cbc431e1f4	4387	0xEF,temp8);
nikapov	0:a1a69d32f310	4388
nikapov	0:a1a69d32f310	4389	break;
nikapov	0:a1a69d32f310	4390
nikapov	0:a1a69d32f310	4391
nikapov	0:a1a69d32f310	4392	default:
nikapov	0:a1a69d32f310	4393	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4394
nikapov	0:a1a69d32f310	4395	}
nikapov	0:a1a69d32f310	4396
nikapov	0:a1a69d32f310	4397	}
nikapov	0:a1a69d32f310	4398
nikapov	0:a1a69d32f310	4399	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4400	if (limit_check_enable == 0) {
sepp_nepp	7:41cbc431e1f4	4401	_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 0;
nikapov	0:a1a69d32f310	4402	} else {
sepp_nepp	7:41cbc431e1f4	4403	_device->CurrentParameters.LimitChecksEnable[limit_check_id] = 1;
nikapov	0:a1a69d32f310	4404	}
nikapov	0:a1a69d32f310	4405	}
nikapov	0:a1a69d32f310	4406
sepp_nepp	5:b95f6951f7d5	4407
nikapov	0:a1a69d32f310	4408	return status;
nikapov	0:a1a69d32f310	4409	}
nikapov	0:a1a69d32f310	4410
sepp_nepp	7:41cbc431e1f4	4411	VL53L0X_Error VL53L0X::VL53L0X_static_init()
nikapov	0:a1a69d32f310	4412	{
nikapov	0:a1a69d32f310	4413	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4414	VL53L0X_DeviceParameters_t current_parameters = {0};
nikapov	0:a1a69d32f310	4415	uint8_t *p_tuning_setting_buffer;
nikapov	0:a1a69d32f310	4416	uint16_t tempword = 0;
nikapov	0:a1a69d32f310	4417	uint8_t tempbyte = 0;
nikapov	0:a1a69d32f310	4418	uint8_t use_internal_tuning_settings = 0;
nikapov	0:a1a69d32f310	4419	uint32_t count = 0;
nikapov	0:a1a69d32f310	4420	uint8_t is_aperture_spads = 0;
nikapov	0:a1a69d32f310	4421	uint32_t ref_spad_count = 0;
nikapov	0:a1a69d32f310	4422	uint8_t aperture_spads = 0;
nikapov	0:a1a69d32f310	4423	uint8_t vcsel_pulse_period_pclk;
nikapov	0:a1a69d32f310	4424	uint32_t seq_timeout_micro_secs;
nikapov	0:a1a69d32f310	4425
sepp_nepp	5:b95f6951f7d5	4426
nikapov	0:a1a69d32f310	4427
sepp_nepp	7:41cbc431e1f4	4428	status = VL53L0X_get_info_from_device(1);
nikapov	0:a1a69d32f310	4429
nikapov	0:a1a69d32f310	4430	/* set the ref spad from NVM */
sepp_nepp	7:41cbc431e1f4	4431	count = (uint32_t)_device->DeviceSpecificParameters.ReferenceSpadCount;
sepp_nepp	7:41cbc431e1f4	4432	aperture_spads = _device->DeviceSpecificParameters.ReferenceSpadType;
nikapov	0:a1a69d32f310	4433
nikapov	0:a1a69d32f310	4434	/* NVM value invalid */
nikapov	0:a1a69d32f310	4435	if ((aperture_spads > 1) \|\|
nikapov	0:a1a69d32f310	4436	((aperture_spads == 1) && (count > 32)) \|\|
nikapov	0:a1a69d32f310	4437	((aperture_spads == 0) && (count > 12))) {
sepp_nepp	7:41cbc431e1f4	4438	status = wrapped_VL53L0X_perform_ref_spad_management(&ref_spad_count,
nikapov	0:a1a69d32f310	4439	&is_aperture_spads);
nikapov	0:a1a69d32f310	4440	} else {
sepp_nepp	7:41cbc431e1f4	4441	status = VL53L0X_set_reference_spads(count,aperture_spads);
nikapov	0:a1a69d32f310	4442	}
nikapov	0:a1a69d32f310	4443
nikapov	0:a1a69d32f310	4444
nikapov	0:a1a69d32f310	4445	/* Initialize tuning settings buffer to prevent compiler warning. */
nikapov	0:a1a69d32f310	4446	p_tuning_setting_buffer = DefaultTuningSettings;
nikapov	0:a1a69d32f310	4447
nikapov	0:a1a69d32f310	4448	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4449	use_internal_tuning_settings = _device->UseInternalTuningSettings;
nikapov	0:a1a69d32f310	4450
nikapov	0:a1a69d32f310	4451	if (use_internal_tuning_settings == 0) {
sepp_nepp	7:41cbc431e1f4	4452	p_tuning_setting_buffer = _device->pTuningSettingsPointer;
nikapov	0:a1a69d32f310	4453	} else {
nikapov	0:a1a69d32f310	4454	p_tuning_setting_buffer = DefaultTuningSettings;
nikapov	0:a1a69d32f310	4455	}
nikapov	0:a1a69d32f310	4456
nikapov	0:a1a69d32f310	4457	}
nikapov	0:a1a69d32f310	4458
nikapov	0:a1a69d32f310	4459	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4460	status = VL53L0X_load_tuning_settings(p_tuning_setting_buffer);
nikapov	0:a1a69d32f310	4461	}
nikapov	0:a1a69d32f310	4462
nikapov	0:a1a69d32f310	4463
nikapov	0:a1a69d32f310	4464	/* Set interrupt config to new sample ready */
nikapov	0:a1a69d32f310	4465	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4466	status = VL53L0X_set_gpio_config(0,0,
nikapov	0:a1a69d32f310	4467	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov	0:a1a69d32f310	4468	VL53L0X_INTERRUPTPOLARITY_LOW);
nikapov	0:a1a69d32f310	4469	}
nikapov	0:a1a69d32f310	4470
nikapov	0:a1a69d32f310	4471	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4472	status = VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	4473	status \|= VL53L0X_read_word(0x84,&tempword);
sepp_nepp	7:41cbc431e1f4	4474	status \|= VL53L0X_write_byte(0xFF,0x00);
nikapov	0:a1a69d32f310	4475	}
nikapov	0:a1a69d32f310	4476
nikapov	0:a1a69d32f310	4477	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4478	_device->DeviceSpecificParameters.OscFrequencyMHz=
sepp_nepp	7:41cbc431e1f4	4479	VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword);
sepp_nepp	7:41cbc431e1f4	4480	}
sepp_nepp	7:41cbc431e1f4	4481
sepp_nepp	7:41cbc431e1f4	4482	/* After static init,some device parameters may be changed,
nikapov	0:a1a69d32f310	4483	* so update them */
nikapov	0:a1a69d32f310	4484	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4485	status = VL53L0X_get_device_parameters(&current_parameters);
nikapov	0:a1a69d32f310	4486	}
nikapov	0:a1a69d32f310	4487
nikapov	0:a1a69d32f310	4488
nikapov	0:a1a69d32f310	4489	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4490	status = VL53L0X_get_fraction_enable(&tempbyte);
Davidroid	3:e9269ff624ed	4491	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4492	_device->RangeFractionalEnable = tempbyte;
Davidroid	3:e9269ff624ed	4493	}
nikapov	0:a1a69d32f310	4494
nikapov	0:a1a69d32f310	4495	}
nikapov	0:a1a69d32f310	4496
nikapov	0:a1a69d32f310	4497	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4498	_device->CurrentParameters = current_parameters;
nikapov	0:a1a69d32f310	4499	}
nikapov	0:a1a69d32f310	4500
nikapov	0:a1a69d32f310	4501
nikapov	0:a1a69d32f310	4502	/* read the sequence config and save it */
nikapov	0:a1a69d32f310	4503	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4504	status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,&tempbyte);
nikapov	0:a1a69d32f310	4505	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4506	_device->SequenceConfig = tempbyte;
nikapov	0:a1a69d32f310	4507	}
nikapov	0:a1a69d32f310	4508	}
nikapov	0:a1a69d32f310	4509
nikapov	0:a1a69d32f310	4510	/* Disable MSRC and TCC by default */
nikapov	0:a1a69d32f310	4511	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4512	status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0);
Davidroid	2:d07edeaff6f1	4513	}
nikapov	0:a1a69d32f310	4514
nikapov	0:a1a69d32f310	4515	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4516	status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0);
Davidroid	2:d07edeaff6f1	4517	}
nikapov	0:a1a69d32f310	4518
nikapov	0:a1a69d32f310	4519	/* Set PAL State to standby */
nikapov	0:a1a69d32f310	4520	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4521	_device->PalState = VL53L0X_STATE_IDLE;
nikapov	0:a1a69d32f310	4522	}
nikapov	0:a1a69d32f310	4523
nikapov	0:a1a69d32f310	4524	/* Store pre-range vcsel period */
nikapov	0:a1a69d32f310	4525	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4526	status = VL53L0X_get_vcsel_pulse_period(
nikapov	0:a1a69d32f310	4527	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4528	&vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	4529	}
nikapov	0:a1a69d32f310	4530
nikapov	0:a1a69d32f310	4531	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4532	_device->DeviceSpecificParameters.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
nikapov	0:a1a69d32f310	4533	}
nikapov	0:a1a69d32f310	4534
nikapov	0:a1a69d32f310	4535	/* Store final-range vcsel period */
nikapov	0:a1a69d32f310	4536	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4537	status = VL53L0X_get_vcsel_pulse_period(
nikapov	0:a1a69d32f310	4538	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov	0:a1a69d32f310	4539	&vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	4540	}
nikapov	0:a1a69d32f310	4541
nikapov	0:a1a69d32f310	4542	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4543	_device->DeviceSpecificParameters.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk;
nikapov	0:a1a69d32f310	4544	}
nikapov	0:a1a69d32f310	4545
nikapov	0:a1a69d32f310	4546	/* Store pre-range timeout */
nikapov	0:a1a69d32f310	4547	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4548	status = get_sequence_step_timeout(
nikapov	0:a1a69d32f310	4549	VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	4550	&seq_timeout_micro_secs);
nikapov	0:a1a69d32f310	4551	}
nikapov	0:a1a69d32f310	4552
nikapov	0:a1a69d32f310	4553	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4554	_device->DeviceSpecificParameters.PreRangeTimeoutMicroSecs = seq_timeout_micro_secs;
nikapov	0:a1a69d32f310	4555	}
nikapov	0:a1a69d32f310	4556
nikapov	0:a1a69d32f310	4557	/* Store final-range timeout */
nikapov	0:a1a69d32f310	4558	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4559	status = get_sequence_step_timeout(
nikapov	0:a1a69d32f310	4560	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	4561	&seq_timeout_micro_secs);
nikapov	0:a1a69d32f310	4562	}
nikapov	0:a1a69d32f310	4563
nikapov	0:a1a69d32f310	4564	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4565	_device->DeviceSpecificParameters.FinalRangeTimeoutMicroSecs = seq_timeout_micro_secs;
nikapov	0:a1a69d32f310	4566	}
nikapov	0:a1a69d32f310	4567
sepp_nepp	5:b95f6951f7d5	4568
nikapov	0:a1a69d32f310	4569	return status;
nikapov	0:a1a69d32f310	4570	}
nikapov	0:a1a69d32f310	4571
nikapov	0:a1a69d32f310	4572
sepp_nepp	7:41cbc431e1f4	4573	VL53L0X_Error VL53L0X::VL53L0X_stop_measurement()
nikapov	0:a1a69d32f310	4574	{
nikapov	0:a1a69d32f310	4575	VL53L0X_Error status = VL53L0X_ERROR_NONE;
sepp_nepp	5:b95f6951f7d5	4576
nikapov	0:a1a69d32f310	4577
sepp_nepp	7:41cbc431e1f4	4578	status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	4579	VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
nikapov	0:a1a69d32f310	4580
sepp_nepp	7:41cbc431e1f4	4581	status = VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	4582	status = VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	4583	status = VL53L0X_write_byte(0x91,0x00);
sepp_nepp	7:41cbc431e1f4	4584	status = VL53L0X_write_byte(0x00,0x01);
sepp_nepp	7:41cbc431e1f4	4585	status = VL53L0X_write_byte(0xFF,0x00);
nikapov	0:a1a69d32f310	4586
nikapov	0:a1a69d32f310	4587	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4588	/* Set PAL State to Idle */
sepp_nepp	7:41cbc431e1f4	4589	_device->PalState = VL53L0X_STATE_IDLE;
nikapov	0:a1a69d32f310	4590	}
nikapov	0:a1a69d32f310	4591
nikapov	0:a1a69d32f310	4592	/* Check if need to apply interrupt settings */
nikapov	0:a1a69d32f310	4593	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4594	status = VL53L0X_check_and_load_interrupt_settings(0);
nikapov	0:a1a69d32f310	4595	}
nikapov	0:a1a69d32f310	4596
sepp_nepp	5:b95f6951f7d5	4597
nikapov	0:a1a69d32f310	4598	return status;
nikapov	0:a1a69d32f310	4599	}
nikapov	0:a1a69d32f310	4600
sepp_nepp	7:41cbc431e1f4	4601	VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(uint32_t *p_stop_status)
nikapov	0:a1a69d32f310	4602	{
nikapov	0:a1a69d32f310	4603	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4604	uint8_t byte = 0;
sepp_nepp	5:b95f6951f7d5	4605
nikapov	0:a1a69d32f310	4606
sepp_nepp	7:41cbc431e1f4	4607	status = VL53L0X_write_byte(0xFF,0x01);
nikapov	0:a1a69d32f310	4608
nikapov	0:a1a69d32f310	4609	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4610	status = VL53L0X_read_byte(0x04,&byte);
nikapov	0:a1a69d32f310	4611	}
nikapov	0:a1a69d32f310	4612
nikapov	0:a1a69d32f310	4613	if (status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4614	status = VL53L0X_write_byte(0xFF,0x0);
nikapov	0:a1a69d32f310	4615	}
nikapov	0:a1a69d32f310	4616
nikapov	0:a1a69d32f310	4617	*p_stop_status = byte;
nikapov	0:a1a69d32f310	4618
nikapov	0:a1a69d32f310	4619	if (byte == 0) {
sepp_nepp	7:41cbc431e1f4	4620	status = VL53L0X_write_byte(0x80,0x01);
sepp_nepp	7:41cbc431e1f4	4621	status = VL53L0X_write_byte(0xFF,0x01);
sepp_nepp	7:41cbc431e1f4	4622	status = VL53L0X_write_byte(0x00,0x00);
sepp_nepp	7:41cbc431e1f4	4623	status = VL53L0X_write_byte(0x91,_device->StopVariable);
sepp_nepp	7:41cbc431e1f4	4624	status = VL53L0X_write_byte(0x00,0x01);
sepp_nepp	7:41cbc431e1f4	4625	status = VL53L0X_write_byte(0xFF,0x00);
sepp_nepp	7:41cbc431e1f4	4626	status = VL53L0X_write_byte(0x80,0x00);
nikapov	0:a1a69d32f310	4627	}
nikapov	0:a1a69d32f310	4628
sepp_nepp	5:b95f6951f7d5	4629
nikapov	0:a1a69d32f310	4630	return status;
nikapov	0:a1a69d32f310	4631	}
nikapov	0:a1a69d32f310	4632
nikapov	0:a1a69d32f310	4633	/**************** Write and read functions from I2C ***********************/
nikapov	0:a1a69d32f310	4634
sepp_nepp	7:41cbc431e1f4	4635	VL53L0X_Error VL53L0X::VL53L0X_write_multi(uint8_t index,uint8_t *p_data,uint32_t count)
nikapov	0:a1a69d32f310	4636	{
nikapov	0:a1a69d32f310	4637	int status;
nikapov	0:a1a69d32f310	4638
sepp_nepp	7:41cbc431e1f4	4639	status = VL53L0X_i2c_write(index,p_data,(uint16_t)count);
nikapov	0:a1a69d32f310	4640	return status;
nikapov	0:a1a69d32f310	4641	}
nikapov	0:a1a69d32f310	4642
sepp_nepp	7:41cbc431e1f4	4643	VL53L0X_Error VL53L0X::VL53L0X_read_multi(uint8_t index,uint8_t *p_data,uint32_t count)
nikapov	0:a1a69d32f310	4644	{
nikapov	0:a1a69d32f310	4645	int status;
nikapov	0:a1a69d32f310	4646
nikapov	0:a1a69d32f310	4647	if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
nikapov	0:a1a69d32f310	4648	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4649	}
nikapov	0:a1a69d32f310	4650
sepp_nepp	7:41cbc431e1f4	4651	status = VL53L0X_i2c_read(index,p_data,(uint16_t)count);
nikapov	0:a1a69d32f310	4652
nikapov	0:a1a69d32f310	4653	return status;
nikapov	0:a1a69d32f310	4654	}
nikapov	0:a1a69d32f310	4655
nikapov	0:a1a69d32f310	4656
sepp_nepp	7:41cbc431e1f4	4657	VL53L0X_Error VL53L0X::VL53L0X_write_byte(uint8_t index,uint8_t data)
nikapov	0:a1a69d32f310	4658	{
nikapov	0:a1a69d32f310	4659	int status;
nikapov	0:a1a69d32f310	4660
sepp_nepp	7:41cbc431e1f4	4661	status = VL53L0X_i2c_write(index,&data,1);
nikapov	0:a1a69d32f310	4662	return status;
nikapov	0:a1a69d32f310	4663	}
nikapov	0:a1a69d32f310	4664
sepp_nepp	7:41cbc431e1f4	4665	VL53L0X_Error VL53L0X::VL53L0X_write_word(uint8_t index,uint16_t data)
nikapov	0:a1a69d32f310	4666	{
nikapov	0:a1a69d32f310	4667	int status;
nikapov	0:a1a69d32f310	4668	uint8_t buffer[2];
nikapov	0:a1a69d32f310	4669
nikapov	0:a1a69d32f310	4670	buffer[0] = data >> 8;
nikapov	0:a1a69d32f310	4671	buffer[1] = data & 0x00FF;
sepp_nepp	7:41cbc431e1f4	4672	status = VL53L0X_i2c_write(index,(uint8_t *)buffer,2);
nikapov	0:a1a69d32f310	4673	return status;
nikapov	0:a1a69d32f310	4674	}
nikapov	0:a1a69d32f310	4675
sepp_nepp	7:41cbc431e1f4	4676	VL53L0X_Error VL53L0X::VL53L0X_write_dword(uint8_t index,uint32_t data)
nikapov	0:a1a69d32f310	4677	{
nikapov	0:a1a69d32f310	4678	int status;
nikapov	0:a1a69d32f310	4679	uint8_t buffer[4];
nikapov	0:a1a69d32f310	4680
nikapov	0:a1a69d32f310	4681	buffer[0] = (data >> 24) & 0xFF;
nikapov	0:a1a69d32f310	4682	buffer[1] = (data >> 16) & 0xFF;
nikapov	0:a1a69d32f310	4683	buffer[2] = (data >> 8) & 0xFF;
nikapov	0:a1a69d32f310	4684	buffer[3] = (data >> 0) & 0xFF;
sepp_nepp	7:41cbc431e1f4	4685	status = VL53L0X_i2c_write(index,(uint8_t *)buffer,4);
nikapov	0:a1a69d32f310	4686	return status;
nikapov	0:a1a69d32f310	4687	}
nikapov	0:a1a69d32f310	4688
nikapov	0:a1a69d32f310	4689
sepp_nepp	7:41cbc431e1f4	4690	VL53L0X_Error VL53L0X::VL53L0X_read_byte(uint8_t index,uint8_t *p_data)
nikapov	0:a1a69d32f310	4691	{
nikapov	0:a1a69d32f310	4692	int status;
nikapov	0:a1a69d32f310	4693
sepp_nepp	7:41cbc431e1f4	4694	status = VL53L0X_i2c_read(index,p_data,1);
nikapov	0:a1a69d32f310	4695
Davidroid	3:e9269ff624ed	4696	if (status) {
nikapov	0:a1a69d32f310	4697	return -1;
Davidroid	3:e9269ff624ed	4698	}
nikapov	0:a1a69d32f310	4699
nikapov	0:a1a69d32f310	4700	return 0;
nikapov	0:a1a69d32f310	4701	}
nikapov	0:a1a69d32f310	4702
sepp_nepp	7:41cbc431e1f4	4703	VL53L0X_Error VL53L0X::VL53L0X_read_word(uint8_t index,uint16_t *p_data)
nikapov	0:a1a69d32f310	4704	{
nikapov	0:a1a69d32f310	4705	int status;
sepp_nepp	7:41cbc431e1f4	4706	uint8_t buffer[2] = {0,0};
sepp_nepp	7:41cbc431e1f4	4707
sepp_nepp	7:41cbc431e1f4	4708	status = VL53L0X_i2c_read(index,buffer,2);
nikapov	0:a1a69d32f310	4709	if (!status) {
nikapov	0:a1a69d32f310	4710	*p_data = (buffer[0] << 8) + buffer[1];
nikapov	0:a1a69d32f310	4711	}
nikapov	0:a1a69d32f310	4712	return status;
nikapov	0:a1a69d32f310	4713
nikapov	0:a1a69d32f310	4714	}
nikapov	0:a1a69d32f310	4715
sepp_nepp	7:41cbc431e1f4	4716	VL53L0X_Error VL53L0X::VL53L0X_read_dword(uint8_t index,uint32_t *p_data)
nikapov	0:a1a69d32f310	4717	{
nikapov	0:a1a69d32f310	4718	int status;
sepp_nepp	7:41cbc431e1f4	4719	uint8_t buffer[4] = {0,0,0,0};
sepp_nepp	7:41cbc431e1f4	4720
sepp_nepp	7:41cbc431e1f4	4721	status = VL53L0X_i2c_read(index,buffer,4);
nikapov	0:a1a69d32f310	4722	if (!status) {
nikapov	0:a1a69d32f310	4723	*p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
nikapov	0:a1a69d32f310	4724	}
nikapov	0:a1a69d32f310	4725	return status;
nikapov	0:a1a69d32f310	4726
nikapov	0:a1a69d32f310	4727	}
nikapov	0:a1a69d32f310	4728
sepp_nepp	7:41cbc431e1f4	4729	VL53L0X_Error VL53L0X::VL53L0X_update_byte(uint8_t index,uint8_t and_data,uint8_t or_data)
nikapov	0:a1a69d32f310	4730	{
nikapov	0:a1a69d32f310	4731	int status;
nikapov	0:a1a69d32f310	4732	uint8_t buffer = 0;
nikapov	0:a1a69d32f310	4733
nikapov	0:a1a69d32f310	4734	/* read data direct onto buffer */
sepp_nepp	7:41cbc431e1f4	4735	status = VL53L0X_i2c_read(index,&buffer,1);
nikapov	0:a1a69d32f310	4736	if (!status) {
nikapov	0:a1a69d32f310	4737	buffer = (buffer & and_data) \| or_data;
sepp_nepp	7:41cbc431e1f4	4738	status = VL53L0X_i2c_write(index,&buffer,(uint8_t)1);
nikapov	0:a1a69d32f310	4739	}
nikapov	0:a1a69d32f310	4740	return status;
nikapov	0:a1a69d32f310	4741	}
nikapov	0:a1a69d32f310	4742
sepp_nepp	7:41cbc431e1f4	4743	VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t RegisterAddr,uint8_t *p_data,
nikapov	0:a1a69d32f310	4744	uint16_t NumByteToWrite)
nikapov	0:a1a69d32f310	4745	{
nikapov	0:a1a69d32f310	4746	int ret;
nikapov	0:a1a69d32f310	4747
sepp_nepp	7:41cbc431e1f4	4748	ret = _dev_i2c->i2c_write(p_data,_device->I2cDevAddr,RegisterAddr,NumByteToWrite);
nikapov	0:a1a69d32f310	4749
nikapov	0:a1a69d32f310	4750	if (ret) {
nikapov	0:a1a69d32f310	4751	return -1;
nikapov	0:a1a69d32f310	4752	}
nikapov	0:a1a69d32f310	4753	return 0;
nikapov	0:a1a69d32f310	4754	}
nikapov	0:a1a69d32f310	4755
sepp_nepp	7:41cbc431e1f4	4756	VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t RegisterAddr,uint8_t *p_data,
nikapov	0:a1a69d32f310	4757	uint16_t NumByteToRead)
nikapov	0:a1a69d32f310	4758	{
nikapov	0:a1a69d32f310	4759	int ret;
nikapov	0:a1a69d32f310	4760
sepp_nepp	7:41cbc431e1f4	4761	ret = _dev_i2c->i2c_read(p_data,_device->I2cDevAddr,RegisterAddr,NumByteToRead);
nikapov	0:a1a69d32f310	4762
nikapov	0:a1a69d32f310	4763	if (ret) {
nikapov	0:a1a69d32f310	4764	return -1;
nikapov	0:a1a69d32f310	4765	}
nikapov	0:a1a69d32f310	4766	return 0;
nikapov	0:a1a69d32f310	4767	}
nikapov	0:a1a69d32f310	4768
nikapov	0:a1a69d32f310	4769	int VL53L0X::read_id(uint8_t *id)
nikapov	0:a1a69d32f310	4770	{
nikapov	0:a1a69d32f310	4771	int status = 0;
nikapov	0:a1a69d32f310	4772	uint16_t rl_id = 0;
nikapov	0:a1a69d32f310	4773
sepp_nepp	7:41cbc431e1f4	4774	status = VL53L0X_read_word(VL53L0X_REG_IDENTIFICATION_MODEL_ID,&rl_id);
nikapov	0:a1a69d32f310	4775	if (rl_id == 0xEEAA) {
nikapov	0:a1a69d32f310	4776	return status;
nikapov	0:a1a69d32f310	4777	}
nikapov	0:a1a69d32f310	4778
nikapov	0:a1a69d32f310	4779	return -1;
nikapov	0:a1a69d32f310	4780	}
nikapov	0:a1a69d32f310	4781
nikapov	0:a1a69d32f310	4782
sepp_nepp	7:41cbc431e1f4	4783	VL53L0X_Error VL53L0X::wait_measurement_data_ready()
nikapov	0:a1a69d32f310	4784	{
nikapov	0:a1a69d32f310	4785	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4786	uint8_t new_dat_ready = 0;
nikapov	0:a1a69d32f310	4787	uint32_t loop_nb;
nikapov	0:a1a69d32f310	4788
nikapov	0:a1a69d32f310	4789	// Wait until it finished
nikapov	0:a1a69d32f310	4790	// use timeout to avoid deadlock
nikapov	0:a1a69d32f310	4791	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4792	loop_nb = 0;
nikapov	0:a1a69d32f310	4793	do {
sepp_nepp	7:41cbc431e1f4	4794	status = VL53L0X_get_measurement_data_ready(&new_dat_ready);
nikapov	0:a1a69d32f310	4795	if ((new_dat_ready == 0x01) \|\| status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4796	break;
nikapov	0:a1a69d32f310	4797	}
nikapov	0:a1a69d32f310	4798	loop_nb = loop_nb + 1;
sepp_nepp	7:41cbc431e1f4	4799	VL53L0X_polling_delay();
nikapov	0:a1a69d32f310	4800	} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov	0:a1a69d32f310	4801
nikapov	0:a1a69d32f310	4802	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	4803	status = VL53L0X_ERROR_TIME_OUT;
nikapov	0:a1a69d32f310	4804	}
nikapov	0:a1a69d32f310	4805	}
nikapov	0:a1a69d32f310	4806
nikapov	0:a1a69d32f310	4807	return status;
nikapov	0:a1a69d32f310	4808	}
nikapov	0:a1a69d32f310	4809
sepp_nepp	7:41cbc431e1f4	4810	VL53L0X_Error VL53L0X::wait_stop_completed()
nikapov	0:a1a69d32f310	4811	{
nikapov	0:a1a69d32f310	4812	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4813	uint32_t stop_completed = 0;
nikapov	0:a1a69d32f310	4814	uint32_t loop_nb;
nikapov	0:a1a69d32f310	4815
nikapov	0:a1a69d32f310	4816	// Wait until it finished
nikapov	0:a1a69d32f310	4817	// use timeout to avoid deadlock
nikapov	0:a1a69d32f310	4818	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4819	loop_nb = 0;
nikapov	0:a1a69d32f310	4820	do {
sepp_nepp	7:41cbc431e1f4	4821	status = VL53L0X_get_stop_completed_status(&stop_completed);
nikapov	0:a1a69d32f310	4822	if ((stop_completed == 0x00) \|\| status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4823	break;
nikapov	0:a1a69d32f310	4824	}
nikapov	0:a1a69d32f310	4825	loop_nb = loop_nb + 1;
sepp_nepp	7:41cbc431e1f4	4826	VL53L0X_polling_delay();
nikapov	0:a1a69d32f310	4827	} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov	0:a1a69d32f310	4828
nikapov	0:a1a69d32f310	4829	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	4830	status = VL53L0X_ERROR_TIME_OUT;
nikapov	0:a1a69d32f310	4831	}
nikapov	0:a1a69d32f310	4832
nikapov	0:a1a69d32f310	4833	}
nikapov	0:a1a69d32f310	4834
nikapov	0:a1a69d32f310	4835	return status;
nikapov	0:a1a69d32f310	4836	}
nikapov	0:a1a69d32f310	4837
nikapov	0:a1a69d32f310	4838
nikapov	0:a1a69d32f310	4839	int VL53L0X::init_sensor(uint8_t new_addr)
nikapov	0:a1a69d32f310	4840	{
nikapov	0:a1a69d32f310	4841	int status;
nikapov	0:a1a69d32f310	4842
nikapov	0:a1a69d32f310	4843	VL53L0X_off();
nikapov	0:a1a69d32f310	4844	VL53L0X_on();
nikapov	0:a1a69d32f310	4845
sepp_nepp	7:41cbc431e1f4	4846	// status=VL53L0X_WaitDeviceBooted(ice);
nikapov	0:a1a69d32f310	4847	// if(status)
nikapov	0:a1a69d32f310	4848	// printf("WaitDeviceBooted fail\n\r");
nikapov	0:a1a69d32f310	4849	status = is_present();
nikapov	0:a1a69d32f310	4850	if (!status) {
nikapov	0:a1a69d32f310	4851	status = init(&_my_device);
nikapov	0:a1a69d32f310	4852	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4853	printf("Failed to init VL53L0X sensor!\n\r");
nikapov	0:a1a69d32f310	4854	return status;
nikapov	0:a1a69d32f310	4855	}
nikapov	0:a1a69d32f310	4856
nikapov	0:a1a69d32f310	4857	// deduce silicon version
sepp_nepp	7:41cbc431e1f4	4858	status = VL53L0X_get_device_info(&_device_info);
nikapov	0:a1a69d32f310	4859
nikapov	0:a1a69d32f310	4860	status = prepare();
nikapov	0:a1a69d32f310	4861	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4862	printf("Failed to prepare VL53L0X!\n\r");
nikapov	0:a1a69d32f310	4863	return status;
nikapov	0:a1a69d32f310	4864	}
nikapov	0:a1a69d32f310	4865
nikapov	1:834986cdde0a	4866	if (new_addr != VL53L0X_DEFAULT_ADDRESS) {
nikapov	0:a1a69d32f310	4867	status = set_device_address(new_addr);
nikapov	0:a1a69d32f310	4868	if (status) {
nikapov	0:a1a69d32f310	4869	printf("Failed to change I2C address!\n\r");
nikapov	0:a1a69d32f310	4870	return status;
nikapov	0:a1a69d32f310	4871	}
nikapov	0:a1a69d32f310	4872	} else {
nikapov	0:a1a69d32f310	4873	printf("Invalid new address!\n\r");
nikapov	0:a1a69d32f310	4874	return VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4875	}
nikapov	0:a1a69d32f310	4876	}
nikapov	0:a1a69d32f310	4877	return status;
nikapov	0:a1a69d32f310	4878	}
nikapov	0:a1a69d32f310	4879
nikapov	0:a1a69d32f310	4880	int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void))
nikapov	0:a1a69d32f310	4881	{
sepp_nepp	5:b95f6951f7d5	4882	int status;
nikapov	0:a1a69d32f310	4883
sepp_nepp	7:41cbc431e1f4	4884	status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped
sepp_nepp	7:41cbc431e1f4	4885
sepp_nepp	7:41cbc431e1f4	4886	status = VL53L0X_set_gpio_config(0,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov	0:a1a69d32f310	4887	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov	0:a1a69d32f310	4888	VL53L0X_INTERRUPTPOLARITY_HIGH);
nikapov	0:a1a69d32f310	4889
nikapov	0:a1a69d32f310	4890	if (!status) {
nikapov	0:a1a69d32f310	4891	attach_interrupt_measure_detection_irq(fptr);
nikapov	0:a1a69d32f310	4892	enable_interrupt_measure_detection_irq();
nikapov	0:a1a69d32f310	4893	}
nikapov	0:a1a69d32f310	4894
sepp_nepp	5:b95f6951f7d5	4895	clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS \| VL53L0X_REG_RESULT_RANGE_STATUS);
sepp_nepp	7:41cbc431e1f4	4896	// NB: return value was previously only passed to logging macro,but did not get passed back
nikapov	0:a1a69d32f310	4897
nikapov	0:a1a69d32f310	4898	if (!status) {
nikapov	0:a1a69d32f310	4899	status = range_start_continuous_mode();
nikapov	0:a1a69d32f310	4900	}
nikapov	0:a1a69d32f310	4901	return status;
nikapov	0:a1a69d32f310	4902	}
nikapov	0:a1a69d32f310	4903
nikapov	0:a1a69d32f310	4904
sepp_nepp	7:41cbc431e1f4	4905	int VL53L0X::start_measurement(OperatingMode operating_mode,void (*fptr)(void))
nikapov	0:a1a69d32f310	4906	{
nikapov	0:a1a69d32f310	4907	int Status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4908
nikapov	0:a1a69d32f310	4909	uint8_t VhvSettings;
nikapov	0:a1a69d32f310	4910	uint8_t PhaseCal;
sepp_nepp	7:41cbc431e1f4	4911	// *** from mass market cube expansion v1.1,ranging with satellites.
sepp_nepp	7:41cbc431e1f4	4912	// default settings,for normal range.
nikapov	0:a1a69d32f310	4913	FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536);
nikapov	0:a1a69d32f310	4914	FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536);
nikapov	0:a1a69d32f310	4915	uint32_t timingBudget = 33000;
nikapov	0:a1a69d32f310	4916	uint8_t preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	4917	uint8_t finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	4918
nikapov	0:a1a69d32f310	4919	if (operating_mode == range_continuous_interrupt) {
nikapov	0:a1a69d32f310	4920	if (_gpio1Int == NULL) {
nikapov	0:a1a69d32f310	4921	printf("GPIO1 Error\r\n");
nikapov	0:a1a69d32f310	4922	return 1;
nikapov	0:a1a69d32f310	4923	}
nikapov	0:a1a69d32f310	4924
sepp_nepp	7:41cbc431e1f4	4925	Status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped
sepp_nepp	7:41cbc431e1f4	4926
sepp_nepp	7:41cbc431e1f4	4927	Status = VL53L0X_set_gpio_config(0,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov	0:a1a69d32f310	4928	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov	0:a1a69d32f310	4929	VL53L0X_INTERRUPTPOLARITY_HIGH);
nikapov	0:a1a69d32f310	4930
nikapov	0:a1a69d32f310	4931	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4932	attach_interrupt_measure_detection_irq(fptr);
nikapov	0:a1a69d32f310	4933	enable_interrupt_measure_detection_irq();
nikapov	0:a1a69d32f310	4934	}
nikapov	0:a1a69d32f310	4935
sepp_nepp	5:b95f6951f7d5	4936	clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS \| VL53L0X_REG_RESULT_RANGE_STATUS);
sepp_nepp	7:41cbc431e1f4	4937	// NB: return value was previously only passed to logging macro,but did not get passed back
nikapov	0:a1a69d32f310	4938
nikapov	0:a1a69d32f310	4939	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4940	Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
nikapov	0:a1a69d32f310	4941	}
nikapov	0:a1a69d32f310	4942
nikapov	0:a1a69d32f310	4943	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4944	Status = VL53L0X_start_measurement();
nikapov	0:a1a69d32f310	4945	}
nikapov	0:a1a69d32f310	4946	}
nikapov	0:a1a69d32f310	4947
nikapov	0:a1a69d32f310	4948	if (operating_mode == range_single_shot_polling) {
sepp_nepp	7:41cbc431e1f4	4949	// singelshot,polled ranging
nikapov	0:a1a69d32f310	4950	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4951	// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
sepp_nepp	7:41cbc431e1f4	4952	Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
nikapov	0:a1a69d32f310	4953	}
nikapov	0:a1a69d32f310	4954
nikapov	0:a1a69d32f310	4955	// Enable/Disable Sigma and Signal check
nikapov	0:a1a69d32f310	4956	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4957	Status = VL53L0X_set_limit_check_enable(
sepp_nepp	7:41cbc431e1f4	4958	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,1);
nikapov	0:a1a69d32f310	4959	}
nikapov	0:a1a69d32f310	4960	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4961	Status = VL53L0X_set_limit_check_enable(
sepp_nepp	7:41cbc431e1f4	4962	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,1);
nikapov	0:a1a69d32f310	4963	}
nikapov	0:a1a69d32f310	4964
sepp_nepp	7:41cbc431e1f4	4965	// *** from mass market cube expansion v1.1,ranging with satellites.
nikapov	0:a1a69d32f310	4966	/* Ranging configuration */
nikapov	0:a1a69d32f310	4967	//*
nikapov	0:a1a69d32f310	4968	// switch(rangingConfig) {
nikapov	0:a1a69d32f310	4969	// case LONG_RANGE:
nikapov	0:a1a69d32f310	4970	signalLimit = (FixPoint1616_t)(0.1 * 65536);
nikapov	0:a1a69d32f310	4971	sigmaLimit = (FixPoint1616_t)(60 * 65536);
nikapov	0:a1a69d32f310	4972	timingBudget = 33000;
nikapov	0:a1a69d32f310	4973	preRangeVcselPeriod = 18;
nikapov	0:a1a69d32f310	4974	finalRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	4975	/* break;
nikapov	0:a1a69d32f310	4976	case HIGH_ACCURACY:
nikapov	0:a1a69d32f310	4977	signalLimit = (FixPoint1616_t)(0.25*65536);
nikapov	0:a1a69d32f310	4978	sigmaLimit = (FixPoint1616_t)(18*65536);
nikapov	0:a1a69d32f310	4979	timingBudget = 200000;
nikapov	0:a1a69d32f310	4980	preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	4981	finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	4982	break;
nikapov	0:a1a69d32f310	4983	case HIGH_SPEED:
nikapov	0:a1a69d32f310	4984	signalLimit = (FixPoint1616_t)(0.25*65536);
nikapov	0:a1a69d32f310	4985	sigmaLimit = (FixPoint1616_t)(32*65536);
nikapov	0:a1a69d32f310	4986	timingBudget = 20000;
nikapov	0:a1a69d32f310	4987	preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	4988	finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	4989	break;
nikapov	0:a1a69d32f310	4990	default:
nikapov	0:a1a69d32f310	4991	debug_printf("Not Supported");
nikapov	0:a1a69d32f310	4992	}
nikapov	0:a1a69d32f310	4993	*/
nikapov	0:a1a69d32f310	4994
nikapov	0:a1a69d32f310	4995	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	4996	Status = VL53L0X_set_limit_check_value(
sepp_nepp	7:41cbc431e1f4	4997	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,signalLimit);
nikapov	0:a1a69d32f310	4998	}
nikapov	0:a1a69d32f310	4999
nikapov	0:a1a69d32f310	5000	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5001	Status = VL53L0X_set_limit_check_value(
sepp_nepp	7:41cbc431e1f4	5002	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,sigmaLimit);
nikapov	0:a1a69d32f310	5003	}
nikapov	0:a1a69d32f310	5004
nikapov	0:a1a69d32f310	5005	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5006	Status = VL53L0X_set_measurement_timing_budget_micro_seconds(timingBudget);
nikapov	0:a1a69d32f310	5007	}
nikapov	0:a1a69d32f310	5008
nikapov	0:a1a69d32f310	5009	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5010	Status = VL53L0X_set_vcsel_pulse_period(
sepp_nepp	7:41cbc431e1f4	5011	VL53L0X_VCSEL_PERIOD_PRE_RANGE,preRangeVcselPeriod);
nikapov	0:a1a69d32f310	5012	}
nikapov	0:a1a69d32f310	5013
nikapov	0:a1a69d32f310	5014	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5015	Status = VL53L0X_set_vcsel_pulse_period(
sepp_nepp	7:41cbc431e1f4	5016	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,finalRangeVcselPeriod);
nikapov	0:a1a69d32f310	5017	}
nikapov	0:a1a69d32f310	5018
nikapov	0:a1a69d32f310	5019	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5020	Status = VL53L0X_perform_ref_calibration(&VhvSettings,&PhaseCal);
nikapov	0:a1a69d32f310	5021	}
nikapov	0:a1a69d32f310	5022
nikapov	0:a1a69d32f310	5023	}
nikapov	0:a1a69d32f310	5024
nikapov	0:a1a69d32f310	5025	if (operating_mode == range_continuous_polling) {
nikapov	0:a1a69d32f310	5026	if (Status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5027	//printf("Call of VL53L0X_SetDeviceMode\n");
sepp_nepp	7:41cbc431e1f4	5028	Status = VL53L0X_set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
nikapov	0:a1a69d32f310	5029	}
nikapov	0:a1a69d32f310	5030
nikapov	0:a1a69d32f310	5031	if (Status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5032	//printf("Call of VL53L0X_StartMeasurement\n");
sepp_nepp	7:41cbc431e1f4	5033	Status = VL53L0X_start_measurement();
nikapov	0:a1a69d32f310	5034	}
nikapov	0:a1a69d32f310	5035	}
nikapov	0:a1a69d32f310	5036
nikapov	0:a1a69d32f310	5037	return Status;
nikapov	0:a1a69d32f310	5038	}
nikapov	0:a1a69d32f310	5039
nikapov	0:a1a69d32f310	5040
sepp_nepp	7:41cbc431e1f4	5041	int VL53L0X::get_measurement(OperatingMode operating_mode,VL53L0X_RangingMeasurementData_t *p_data)
nikapov	0:a1a69d32f310	5042	{
nikapov	0:a1a69d32f310	5043	int Status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5044
nikapov	0:a1a69d32f310	5045	if (operating_mode == range_single_shot_polling) {
sepp_nepp	7:41cbc431e1f4	5046	Status = VL53L0X_perform_single_ranging_measurement(p_data);
nikapov	0:a1a69d32f310	5047	}
nikapov	0:a1a69d32f310	5048
nikapov	0:a1a69d32f310	5049	if (operating_mode == range_continuous_polling) {
Davidroid	3:e9269ff624ed	5050	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5051	Status = VL53L0X_measurement_poll_for_completion();
Davidroid	3:e9269ff624ed	5052	}
nikapov	0:a1a69d32f310	5053
nikapov	0:a1a69d32f310	5054	if (Status == VL53L0X_ERROR_NONE) {
sepp_nepp	7:41cbc431e1f4	5055	Status = VL53L0X_get_ranging_measurement_data(p_data);
nikapov	0:a1a69d32f310	5056
nikapov	0:a1a69d32f310	5057	// Clear the interrupt
sepp_nepp	7:41cbc431e1f4	5058	VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
sepp_nepp	7:41cbc431e1f4	5059	VL53L0X_polling_delay();
nikapov	0:a1a69d32f310	5060	}
nikapov	0:a1a69d32f310	5061	}
nikapov	0:a1a69d32f310	5062
nikapov	0:a1a69d32f310	5063	if (operating_mode == range_continuous_interrupt) {
sepp_nepp	7:41cbc431e1f4	5064	Status = VL53L0X_get_ranging_measurement_data(p_data);
sepp_nepp	7:41cbc431e1f4	5065	VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR \| VL53L0X_REG_RESULT_INTERRUPT_STATUS);
nikapov	0:a1a69d32f310	5066	}
nikapov	0:a1a69d32f310	5067
nikapov	0:a1a69d32f310	5068	return Status;
nikapov	0:a1a69d32f310	5069	}
nikapov	0:a1a69d32f310	5070
nikapov	0:a1a69d32f310	5071
nikapov	0:a1a69d32f310	5072	int VL53L0X::stop_measurement(OperatingMode operating_mode)
nikapov	0:a1a69d32f310	5073	{
nikapov	0:a1a69d32f310	5074	int status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5075
nikapov	0:a1a69d32f310	5076
nikapov	0:a1a69d32f310	5077	// don't need to stop for a singleshot range!
nikapov	0:a1a69d32f310	5078	if (operating_mode == range_single_shot_polling) {
nikapov	0:a1a69d32f310	5079	}
nikapov	0:a1a69d32f310	5080
nikapov	0:a1a69d32f310	5081	if (operating_mode == range_continuous_interrupt \|\| operating_mode == range_continuous_polling) {
nikapov	0:a1a69d32f310	5082	// continuous mode
nikapov	0:a1a69d32f310	5083	if (status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5084	//printf("Call of VL53L0X_StopMeasurement\n");
sepp_nepp	7:41cbc431e1f4	5085	status = VL53L0X_stop_measurement();
nikapov	0:a1a69d32f310	5086	}
nikapov	0:a1a69d32f310	5087
nikapov	0:a1a69d32f310	5088	if (status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5089	//printf("Wait Stop to be competed\n");
sepp_nepp	7:41cbc431e1f4	5090	status = wait_stop_completed();
nikapov	0:a1a69d32f310	5091	}
nikapov	0:a1a69d32f310	5092
nikapov	0:a1a69d32f310	5093	if (status == VL53L0X_ERROR_NONE)
sepp_nepp	7:41cbc431e1f4	5094	status = VL53L0X_clear_interrupt_mask(
nikapov	0:a1a69d32f310	5095	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
nikapov	0:a1a69d32f310	5096	}
nikapov	0:a1a69d32f310	5097
nikapov	0:a1a69d32f310	5098	return status;
nikapov	0:a1a69d32f310	5099	}
nikapov	0:a1a69d32f310	5100
nikapov	0:a1a69d32f310	5101
sepp_nepp	7:41cbc431e1f4	5102	int VL53L0X::handle_irq(OperatingMode operating_mode,VL53L0X_RangingMeasurementData_t *data)
nikapov	0:a1a69d32f310	5103	{
nikapov	0:a1a69d32f310	5104	int status;
sepp_nepp	7:41cbc431e1f4	5105	status = get_measurement(operating_mode,data);
nikapov	0:a1a69d32f310	5106	enable_interrupt_measure_detection_irq();
nikapov	0:a1a69d32f310	5107	return status;
nikapov	0:a1a69d32f310	5108	}
nikapov	0:a1a69d32f310	5109
nikapov	0:a1a69d32f310	5110	/******************************************************************************/

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@7:41cbc431e1f4, 2019-03-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning