VL53L0X_MAX32630FTHR - Makes the VL53L0X library compatible with MAX3263…

Users » DVLevine » Code » VL53L0X_MAX32630FTHR

Makes the VL53L0X library compatible with MAX32630FTHR definitions. Adjusts default sensor settings to fastest reading (20ms) vs. default (30ms).

Dependencies: ST_INTERFACES X_NUCLEO_COMMON

Fork of VL53L0X by ST

VL53L0X.cpp@5:d25feb417ac1, 2018-03-13 (annotated)

Committer:: DVLevine
Date:: Tue Mar 13 01:23:22 2018 +0000
Revision:: 5:d25feb417ac1
Parent:: 3:e9269ff624ed

changed library for compatibility with MAX32630 definitions and changed prox mode to fast readings (20ms) vs. default long distance setting (33ms).

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	13 Mar 2018
Imports:	0
Forks:	0
Commits:	6
Dependents:	0
Dependencies:	2
Followers:	1

VL53L0X.cpp@5:d25feb417ac1, 2018-03-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning