VL53L0X_simple - Fork of X-NUCLEO-53L0A1 Library and deleted the b…

Users » kenjiArai » Code » VL53L0X_simple

Fork of X-NUCLEO-53L0A1 Library and deleted the board related files. Just use a VL53L0X chip as a ToF sensor.

Dependents: Check_VL53L0X_simple_with_three_ToF Check_VL53L0X_simple_ToF_Sensor lidarproj Check_VL53L0X_simple_with_three_ToF ... more

Fork of VL53L0X by ST

Please refer below page.
/users/kenjiArai/notebook/vl53l0x-tof-sensor/

VL53L0X.cpp@10:c9181a97a64f, 2020-02-26 (annotated)

Committer:: kenjiArai
Date:: Wed Feb 26 09:34:31 2020 +0000
Revision:: 10:c9181a97a64f
Parent:: 5:ac6604b75d50

bug fix for XSHUT=NC

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
kenjiArai	5:ac6604b75d50	1558	#if ORIGINAL
nikapov	0:a1a69d32f310	1559	Status = VL53L0X_UpdateByte(Dev,
nikapov	0:a1a69d32f310	1560	VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
nikapov	0:a1a69d32f310	1561	0xFE,
nikapov	0:a1a69d32f310	1562	0x01);
kenjiArai	5:ac6604b75d50	1563	#else
kenjiArai	5:ac6604b75d50	1564	status = VL53L0X_update_byte(dev,
kenjiArai	5:ac6604b75d50	1565	VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
kenjiArai	5:ac6604b75d50	1566	0xFE,
kenjiArai	5:ac6604b75d50	1567	0x01);
kenjiArai	5:ac6604b75d50	1568	#endif
nikapov	0:a1a69d32f310	1569	#endif
nikapov	0:a1a69d32f310	1570
nikapov	0:a1a69d32f310	1571	/* Set I2C standard mode */
Davidroid	3:e9269ff624ed	1572	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1573	status = VL53L0X_write_byte(dev, 0x88, 0x00);
Davidroid	3:e9269ff624ed	1574	}
nikapov	0:a1a69d32f310	1575
nikapov	0:a1a69d32f310	1576	VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone, 0);
nikapov	0:a1a69d32f310	1577
nikapov	0:a1a69d32f310	1578	#ifdef USE_IQC_STATION
Davidroid	3:e9269ff624ed	1579	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1580	Status = VL53L0X_apply_offset_adjustment(Dev);
Davidroid	3:e9269ff624ed	1581	}
nikapov	0:a1a69d32f310	1582	#endif
nikapov	0:a1a69d32f310	1583
nikapov	0:a1a69d32f310	1584	/* Default value is 1000 for Linearity Corrective Gain */
nikapov	0:a1a69d32f310	1585	PALDevDataSet(dev, LinearityCorrectiveGain, 1000);
nikapov	0:a1a69d32f310	1586
nikapov	0:a1a69d32f310	1587	/* Dmax default Parameter */
nikapov	0:a1a69d32f310	1588	PALDevDataSet(dev, DmaxCalRangeMilliMeter, 400);
nikapov	0:a1a69d32f310	1589	PALDevDataSet(dev, DmaxCalSignalRateRtnMegaCps,
nikapov	0:a1a69d32f310	1590	(FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/
nikapov	0:a1a69d32f310	1591
nikapov	0:a1a69d32f310	1592	/* Set Default static parameters
nikapov	0:a1a69d32f310	1593	set first temporary values 9.44MHz 65536 = 618660 */
nikapov	0:a1a69d32f310	1594	VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz, 618660);
nikapov	0:a1a69d32f310	1595
nikapov	0:a1a69d32f310	1596	/* Set Default XTalkCompensationRateMegaCps to 0 */
nikapov	0:a1a69d32f310	1597	VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps, 0);
nikapov	0:a1a69d32f310	1598
nikapov	0:a1a69d32f310	1599	/* Get default parameters */
nikapov	0:a1a69d32f310	1600	status = VL53L0X_get_device_parameters(dev, &CurrentParameters);
nikapov	0:a1a69d32f310	1601	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1602	/* initialize PAL values */
nikapov	0:a1a69d32f310	1603	CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
nikapov	0:a1a69d32f310	1604	CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
nikapov	0:a1a69d32f310	1605	PALDevDataSet(dev, CurrentParameters, CurrentParameters);
nikapov	0:a1a69d32f310	1606	}
nikapov	0:a1a69d32f310	1607
nikapov	0:a1a69d32f310	1608	/* Sigma estimator variable */
nikapov	0:a1a69d32f310	1609	PALDevDataSet(dev, SigmaEstRefArray, 100);
nikapov	0:a1a69d32f310	1610	PALDevDataSet(dev, SigmaEstEffPulseWidth, 900);
nikapov	0:a1a69d32f310	1611	PALDevDataSet(dev, SigmaEstEffAmbWidth, 500);
nikapov	0:a1a69d32f310	1612	PALDevDataSet(dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */
nikapov	0:a1a69d32f310	1613
nikapov	0:a1a69d32f310	1614	/* Use internal default settings */
nikapov	0:a1a69d32f310	1615	PALDevDataSet(dev, UseInternalTuningSettings, 1);
nikapov	0:a1a69d32f310	1616
nikapov	0:a1a69d32f310	1617	status \|= VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov	0:a1a69d32f310	1618	status \|= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	1619	status \|= VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov	0:a1a69d32f310	1620	status \|= VL53L0X_read_byte(dev, 0x91, &StopVariable);
nikapov	0:a1a69d32f310	1621	PALDevDataSet(dev, StopVariable, StopVariable);
nikapov	0:a1a69d32f310	1622	status \|= VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov	0:a1a69d32f310	1623	status \|= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	1624	status \|= VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov	0:a1a69d32f310	1625
nikapov	0:a1a69d32f310	1626	/* Enable all check */
nikapov	0:a1a69d32f310	1627	for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
Davidroid	3:e9269ff624ed	1628	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1629	status \|= VL53L0X_set_limit_check_enable(dev, i, 1);
Davidroid	3:e9269ff624ed	1630	} else {
nikapov	0:a1a69d32f310	1631	break;
Davidroid	3:e9269ff624ed	1632	}
nikapov	0:a1a69d32f310	1633
nikapov	0:a1a69d32f310	1634	}
nikapov	0:a1a69d32f310	1635
nikapov	0:a1a69d32f310	1636	/* Disable the following checks */
nikapov	0:a1a69d32f310	1637	if (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	1638	status = VL53L0X_set_limit_check_enable(dev,
nikapov	0:a1a69d32f310	1639	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
nikapov	0:a1a69d32f310	1640
nikapov	0:a1a69d32f310	1641	if (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	1642	status = VL53L0X_set_limit_check_enable(dev,
nikapov	0:a1a69d32f310	1643	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
nikapov	0:a1a69d32f310	1644
nikapov	0:a1a69d32f310	1645	if (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	1646	status = VL53L0X_set_limit_check_enable(dev,
nikapov	0:a1a69d32f310	1647	VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
nikapov	0:a1a69d32f310	1648
nikapov	0:a1a69d32f310	1649	if (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	1650	status = VL53L0X_set_limit_check_enable(dev,
nikapov	0:a1a69d32f310	1651	VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
nikapov	0:a1a69d32f310	1652
nikapov	0:a1a69d32f310	1653	/* Limit default values */
nikapov	0:a1a69d32f310	1654	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1655	status = VL53L0X_set_limit_check_value(dev,
nikapov	0:a1a69d32f310	1656	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	1657	(FixPoint1616_t)(18 * 65536));
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_RATE_FINAL_RANGE,
nikapov	0:a1a69d32f310	1662	(FixPoint1616_t)(25 * 65536 / 100));
nikapov	0:a1a69d32f310	1663	/* 0.25 * 65536 */
nikapov	0:a1a69d32f310	1664	}
nikapov	0:a1a69d32f310	1665
nikapov	0:a1a69d32f310	1666	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1667	status = VL53L0X_set_limit_check_value(dev,
nikapov	0:a1a69d32f310	1668	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	1669	(FixPoint1616_t)(35 * 65536));
nikapov	0:a1a69d32f310	1670	}
nikapov	0:a1a69d32f310	1671
nikapov	0:a1a69d32f310	1672	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1673	status = VL53L0X_set_limit_check_value(dev,
nikapov	0:a1a69d32f310	1674	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	1675	(FixPoint1616_t)(0 * 65536));
nikapov	0:a1a69d32f310	1676	}
nikapov	0:a1a69d32f310	1677
nikapov	0:a1a69d32f310	1678	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1679
nikapov	0:a1a69d32f310	1680	PALDevDataSet(dev, SequenceConfig, 0xFF);
nikapov	0:a1a69d32f310	1681	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	1682	0xFF);
nikapov	0:a1a69d32f310	1683
nikapov	0:a1a69d32f310	1684	/* Set PAL state to tell that we are waiting for call to
nikapov	0:a1a69d32f310	1685	* VL53L0X_StaticInit */
nikapov	0:a1a69d32f310	1686	PALDevDataSet(dev, PalState, VL53L0X_STATE_WAIT_STATICINIT);
nikapov	0:a1a69d32f310	1687	}
nikapov	0:a1a69d32f310	1688
Davidroid	3:e9269ff624ed	1689	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1690	VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 0);
Davidroid	3:e9269ff624ed	1691	}
nikapov	0:a1a69d32f310	1692
nikapov	0:a1a69d32f310	1693
nikapov	0:a1a69d32f310	1694	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1695	return status;
nikapov	0:a1a69d32f310	1696	}
nikapov	0:a1a69d32f310	1697
nikapov	0:a1a69d32f310	1698	VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1699	uint8_t *revision,
nikapov	0:a1a69d32f310	1700	VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov	0:a1a69d32f310	1701	{
nikapov	0:a1a69d32f310	1702	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1703	uint8_t module_id_int;
nikapov	0:a1a69d32f310	1704	char *product_id_tmp;
nikapov	0:a1a69d32f310	1705
nikapov	0:a1a69d32f310	1706	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	1707
nikapov	0:a1a69d32f310	1708	status = VL53L0X_get_info_from_device(dev, 2);
nikapov	0:a1a69d32f310	1709
nikapov	0:a1a69d32f310	1710	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1711	module_id_int = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, ModuleId);
nikapov	0:a1a69d32f310	1712
nikapov	0:a1a69d32f310	1713	if (module_id_int == 0) {
nikapov	0:a1a69d32f310	1714	*revision = 0;
nikapov	0:a1a69d32f310	1715	VL53L0X_COPYSTRING(p_VL53L0X_device_info->ProductId, "");
nikapov	0:a1a69d32f310	1716	} else {
nikapov	0:a1a69d32f310	1717	*revision = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, Revision);
nikapov	0:a1a69d32f310	1718	product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	1719	ProductId);
nikapov	0:a1a69d32f310	1720	VL53L0X_COPYSTRING(p_VL53L0X_device_info->ProductId, product_id_tmp);
nikapov	0:a1a69d32f310	1721	}
nikapov	0:a1a69d32f310	1722	}
nikapov	0:a1a69d32f310	1723
nikapov	0:a1a69d32f310	1724	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1725	return status;
nikapov	0:a1a69d32f310	1726	}
nikapov	0:a1a69d32f310	1727
nikapov	0:a1a69d32f310	1728	VL53L0X_Error VL53L0X::wrapped_VL53L0X_get_device_info(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1729	VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov	0:a1a69d32f310	1730	{
nikapov	0:a1a69d32f310	1731	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1732	uint8_t revision_id;
nikapov	0:a1a69d32f310	1733	uint8_t revision;
nikapov	0:a1a69d32f310	1734
nikapov	0:a1a69d32f310	1735	status = VL53L0X_check_part_used(dev, &revision, p_VL53L0X_device_info);
nikapov	0:a1a69d32f310	1736
nikapov	0:a1a69d32f310	1737	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1738	if (revision == 0) {
nikapov	0:a1a69d32f310	1739	VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1740	VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
nikapov	0:a1a69d32f310	1741	} else if ((revision <= 34) && (revision != 32)) {
nikapov	0:a1a69d32f310	1742	VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1743	VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
nikapov	0:a1a69d32f310	1744	} else if (revision < 39) {
nikapov	0:a1a69d32f310	1745	VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1746	VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
nikapov	0:a1a69d32f310	1747	} else {
nikapov	0:a1a69d32f310	1748	VL53L0X_COPYSTRING(p_VL53L0X_device_info->Name,
nikapov	0:a1a69d32f310	1749	VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
nikapov	0:a1a69d32f310	1750	}
nikapov	0:a1a69d32f310	1751
nikapov	0:a1a69d32f310	1752	VL53L0X_COPYSTRING(p_VL53L0X_device_info->Type,
nikapov	0:a1a69d32f310	1753	VL53L0X_STRING_DEVICE_INFO_TYPE);
nikapov	0:a1a69d32f310	1754
nikapov	0:a1a69d32f310	1755	}
nikapov	0:a1a69d32f310	1756
nikapov	0:a1a69d32f310	1757	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1758	status = VL53L0X_read_byte(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
nikapov	0:a1a69d32f310	1759	&p_VL53L0X_device_info->ProductType);
nikapov	0:a1a69d32f310	1760	}
nikapov	0:a1a69d32f310	1761
nikapov	0:a1a69d32f310	1762	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1763	status = VL53L0X_read_byte(dev,
nikapov	0:a1a69d32f310	1764	VL53L0X_REG_IDENTIFICATION_REVISION_ID,
nikapov	0:a1a69d32f310	1765	&revision_id);
nikapov	0:a1a69d32f310	1766	p_VL53L0X_device_info->ProductRevisionMajor = 1;
nikapov	0:a1a69d32f310	1767	p_VL53L0X_device_info->ProductRevisionMinor =
nikapov	0:a1a69d32f310	1768	(revision_id & 0xF0) >> 4;
nikapov	0:a1a69d32f310	1769	}
nikapov	0:a1a69d32f310	1770
nikapov	0:a1a69d32f310	1771	return status;
nikapov	0:a1a69d32f310	1772	}
nikapov	0:a1a69d32f310	1773
nikapov	0:a1a69d32f310	1774	VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1775	VL53L0X_DeviceInfo_t *p_VL53L0X_device_info)
nikapov	0:a1a69d32f310	1776	{
nikapov	0:a1a69d32f310	1777	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1778	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	1779
nikapov	0:a1a69d32f310	1780	status = wrapped_VL53L0X_get_device_info(dev, p_VL53L0X_device_info);
nikapov	0:a1a69d32f310	1781
nikapov	0:a1a69d32f310	1782	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1783	return status;
nikapov	0:a1a69d32f310	1784	}
nikapov	0:a1a69d32f310	1785
nikapov	0:a1a69d32f310	1786	VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1787	uint32_t *p_interrupt_mask_status)
nikapov	0:a1a69d32f310	1788	{
nikapov	0:a1a69d32f310	1789	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1790	uint8_t byte;
nikapov	0:a1a69d32f310	1791	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	1792
nikapov	0:a1a69d32f310	1793	status = VL53L0X_read_byte(dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
nikapov	0:a1a69d32f310	1794	*p_interrupt_mask_status = byte & 0x07;
nikapov	0:a1a69d32f310	1795
nikapov	0:a1a69d32f310	1796	if (byte & 0x18) {
nikapov	0:a1a69d32f310	1797	status = VL53L0X_ERROR_RANGE_ERROR;
nikapov	0:a1a69d32f310	1798	}
nikapov	0:a1a69d32f310	1799
nikapov	0:a1a69d32f310	1800	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1801	return status;
nikapov	0:a1a69d32f310	1802	}
nikapov	0:a1a69d32f310	1803
nikapov	0:a1a69d32f310	1804	VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1805	uint8_t *p_measurement_data_ready)
nikapov	0:a1a69d32f310	1806	{
nikapov	0:a1a69d32f310	1807	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1808	uint8_t sys_range_status_register;
nikapov	0:a1a69d32f310	1809	uint8_t interrupt_config;
nikapov	0:a1a69d32f310	1810	uint32_t interrupt_mask;
nikapov	0:a1a69d32f310	1811	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	1812
nikapov	0:a1a69d32f310	1813	interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	1814	Pin0GpioFunctionality);
nikapov	0:a1a69d32f310	1815
nikapov	0:a1a69d32f310	1816	if (interrupt_config ==
nikapov	0:a1a69d32f310	1817	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
nikapov	0:a1a69d32f310	1818	status = VL53L0X_get_interrupt_mask_status(dev, &interrupt_mask);
nikapov	0:a1a69d32f310	1819	if (interrupt_mask ==
nikapov	0:a1a69d32f310	1820	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
nikapov	0:a1a69d32f310	1821	*p_measurement_data_ready = 1;
nikapov	0:a1a69d32f310	1822	} else {
nikapov	0:a1a69d32f310	1823	*p_measurement_data_ready = 0;
nikapov	0:a1a69d32f310	1824	}
nikapov	0:a1a69d32f310	1825	} else {
nikapov	0:a1a69d32f310	1826	status = VL53L0X_read_byte(dev, VL53L0X_REG_RESULT_RANGE_STATUS,
nikapov	0:a1a69d32f310	1827	&sys_range_status_register);
nikapov	0:a1a69d32f310	1828	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1829	if (sys_range_status_register & 0x01) {
nikapov	0:a1a69d32f310	1830	*p_measurement_data_ready = 1;
nikapov	0:a1a69d32f310	1831	} else {
nikapov	0:a1a69d32f310	1832	*p_measurement_data_ready = 0;
nikapov	0:a1a69d32f310	1833	}
nikapov	0:a1a69d32f310	1834	}
nikapov	0:a1a69d32f310	1835	}
nikapov	0:a1a69d32f310	1836
nikapov	0:a1a69d32f310	1837	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1838	return status;
nikapov	0:a1a69d32f310	1839	}
nikapov	0:a1a69d32f310	1840
nikapov	0:a1a69d32f310	1841	VL53L0X_Error VL53L0X::VL53L0X_polling_delay(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	1842	{
nikapov	0:a1a69d32f310	1843	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1844
nikapov	0:a1a69d32f310	1845	// do nothing
nikapov	0:a1a69d32f310	1846	VL53L0X_OsDelay();
nikapov	0:a1a69d32f310	1847	return status;
nikapov	0:a1a69d32f310	1848	}
nikapov	0:a1a69d32f310	1849
nikapov	0:a1a69d32f310	1850	VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	1851	{
nikapov	0:a1a69d32f310	1852	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1853	uint8_t new_data_ready = 0;
nikapov	0:a1a69d32f310	1854	uint32_t loop_nb;
nikapov	0:a1a69d32f310	1855
nikapov	0:a1a69d32f310	1856	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	1857
nikapov	0:a1a69d32f310	1858	loop_nb = 0;
nikapov	0:a1a69d32f310	1859
nikapov	0:a1a69d32f310	1860	do {
nikapov	0:a1a69d32f310	1861	status = VL53L0X_get_measurement_data_ready(dev, &new_data_ready);
nikapov	0:a1a69d32f310	1862	if (status != 0) {
nikapov	0:a1a69d32f310	1863	break; /* the error is set */
nikapov	0:a1a69d32f310	1864	}
nikapov	0:a1a69d32f310	1865
nikapov	0:a1a69d32f310	1866	if (new_data_ready == 1) {
nikapov	0:a1a69d32f310	1867	break; /* done note that status == 0 */
nikapov	0:a1a69d32f310	1868	}
nikapov	0:a1a69d32f310	1869
nikapov	0:a1a69d32f310	1870	loop_nb++;
nikapov	0:a1a69d32f310	1871	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	1872	status = VL53L0X_ERROR_TIME_OUT;
nikapov	0:a1a69d32f310	1873	break;
nikapov	0:a1a69d32f310	1874	}
nikapov	0:a1a69d32f310	1875
nikapov	0:a1a69d32f310	1876	VL53L0X_polling_delay(dev);
nikapov	0:a1a69d32f310	1877	} while (1);
nikapov	0:a1a69d32f310	1878
nikapov	0:a1a69d32f310	1879	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1880
nikapov	0:a1a69d32f310	1881	return status;
nikapov	0:a1a69d32f310	1882	}
nikapov	0:a1a69d32f310	1883
nikapov	0:a1a69d32f310	1884	/* Group PAL Interrupt Functions */
nikapov	0:a1a69d32f310	1885	VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask(VL53L0X_DEV dev, uint32_t interrupt_mask)
nikapov	0:a1a69d32f310	1886	{
nikapov	0:a1a69d32f310	1887	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1888	uint8_t loop_count;
nikapov	0:a1a69d32f310	1889	uint8_t byte;
nikapov	0:a1a69d32f310	1890	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	1891
nikapov	0:a1a69d32f310	1892	/* clear bit 0 range interrupt, bit 1 error interrupt */
nikapov	0:a1a69d32f310	1893	loop_count = 0;
nikapov	0:a1a69d32f310	1894	do {
nikapov	0:a1a69d32f310	1895	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	1896	VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
nikapov	0:a1a69d32f310	1897	status \|= VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	1898	VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
nikapov	0:a1a69d32f310	1899	status \|= VL53L0X_read_byte(dev,
nikapov	0:a1a69d32f310	1900	VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
nikapov	0:a1a69d32f310	1901	loop_count++;
nikapov	0:a1a69d32f310	1902	} while (((byte & 0x07) != 0x00)
nikapov	0:a1a69d32f310	1903	&& (loop_count < 3)
nikapov	0:a1a69d32f310	1904	&& (status == VL53L0X_ERROR_NONE));
nikapov	0:a1a69d32f310	1905
nikapov	0:a1a69d32f310	1906
nikapov	0:a1a69d32f310	1907	if (loop_count >= 3) {
nikapov	0:a1a69d32f310	1908	status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
nikapov	0:a1a69d32f310	1909	}
nikapov	0:a1a69d32f310	1910
nikapov	0:a1a69d32f310	1911	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	1912	return status;
nikapov	0:a1a69d32f310	1913	}
nikapov	0:a1a69d32f310	1914
nikapov	0:a1a69d32f310	1915	VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1916	uint8_t vhv_init_byte)
nikapov	0:a1a69d32f310	1917	{
nikapov	0:a1a69d32f310	1918	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1919
nikapov	0:a1a69d32f310	1920	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1921	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	1922	VL53L0X_REG_SYSRANGE_MODE_START_STOP \|
nikapov	0:a1a69d32f310	1923	vhv_init_byte);
nikapov	0:a1a69d32f310	1924	}
nikapov	0:a1a69d32f310	1925
nikapov	0:a1a69d32f310	1926	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1927	status = VL53L0X_measurement_poll_for_completion(dev);
nikapov	0:a1a69d32f310	1928	}
nikapov	0:a1a69d32f310	1929
nikapov	0:a1a69d32f310	1930	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1931	status = VL53L0X_clear_interrupt_mask(dev, 0);
nikapov	0:a1a69d32f310	1932	}
nikapov	0:a1a69d32f310	1933
nikapov	0:a1a69d32f310	1934	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	1935	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x00);
nikapov	0:a1a69d32f310	1936	}
nikapov	0:a1a69d32f310	1937
nikapov	0:a1a69d32f310	1938	return status;
nikapov	0:a1a69d32f310	1939	}
nikapov	0:a1a69d32f310	1940
nikapov	0:a1a69d32f310	1941	VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(VL53L0X_DEV dev, uint8_t read_not_write,
nikapov	0:a1a69d32f310	1942	uint8_t vhv_settings, uint8_t phase_cal,
nikapov	0:a1a69d32f310	1943	uint8_t p_vhv_settings, uint8_t p_phase_cal,
nikapov	0:a1a69d32f310	1944	const uint8_t vhv_enable, const uint8_t phase_enable)
nikapov	0:a1a69d32f310	1945	{
nikapov	0:a1a69d32f310	1946	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1947	uint8_t phase_calint = 0;
nikapov	0:a1a69d32f310	1948
nikapov	0:a1a69d32f310	1949	/* Read VHV from device */
nikapov	0:a1a69d32f310	1950	status \|= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	1951	status \|= VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov	0:a1a69d32f310	1952	status \|= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	1953
nikapov	0:a1a69d32f310	1954	if (read_not_write) {
nikapov	0:a1a69d32f310	1955	if (vhv_enable) {
nikapov	0:a1a69d32f310	1956	status \|= VL53L0X_read_byte(dev, 0xCB, p_vhv_settings);
nikapov	0:a1a69d32f310	1957	}
nikapov	0:a1a69d32f310	1958	if (phase_enable) {
nikapov	0:a1a69d32f310	1959	status \|= VL53L0X_read_byte(dev, 0xEE, &phase_calint);
nikapov	0:a1a69d32f310	1960	}
nikapov	0:a1a69d32f310	1961	} else {
nikapov	0:a1a69d32f310	1962	if (vhv_enable) {
nikapov	0:a1a69d32f310	1963	status \|= VL53L0X_write_byte(dev, 0xCB, vhv_settings);
nikapov	0:a1a69d32f310	1964	}
nikapov	0:a1a69d32f310	1965	if (phase_enable) {
nikapov	0:a1a69d32f310	1966	status \|= VL53L0X_update_byte(dev, 0xEE, 0x80, phase_cal);
nikapov	0:a1a69d32f310	1967	}
nikapov	0:a1a69d32f310	1968	}
nikapov	0:a1a69d32f310	1969
nikapov	0:a1a69d32f310	1970	status \|= VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	1971	status \|= VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov	0:a1a69d32f310	1972	status \|= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	1973
nikapov	0:a1a69d32f310	1974	*p_phase_cal = (uint8_t)(phase_calint & 0xEF);
nikapov	0:a1a69d32f310	1975
nikapov	0:a1a69d32f310	1976	return status;
nikapov	0:a1a69d32f310	1977	}
nikapov	0:a1a69d32f310	1978
nikapov	0:a1a69d32f310	1979	VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	1980	uint8_t *p_vhv_settings, const uint8_t get_data_enable,
nikapov	0:a1a69d32f310	1981	const uint8_t restore_config)
nikapov	0:a1a69d32f310	1982	{
nikapov	0:a1a69d32f310	1983	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	1984	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	1985	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	1986	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	1987	uint8_t phase_cal_int = 0;
nikapov	0:a1a69d32f310	1988
nikapov	0:a1a69d32f310	1989	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	1990	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	1991	*/
nikapov	0:a1a69d32f310	1992
nikapov	0:a1a69d32f310	1993	if (restore_config) {
nikapov	0:a1a69d32f310	1994	sequence_config = PALDevDataGet(dev, SequenceConfig);
nikapov	0:a1a69d32f310	1995	}
nikapov	0:a1a69d32f310	1996
nikapov	0:a1a69d32f310	1997	/* Run VHV */
nikapov	0:a1a69d32f310	1998	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
nikapov	0:a1a69d32f310	1999
nikapov	0:a1a69d32f310	2000	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2001	status = VL53L0X_perform_single_ref_calibration(dev, 0x40);
nikapov	0:a1a69d32f310	2002	}
nikapov	0:a1a69d32f310	2003
nikapov	0:a1a69d32f310	2004	/* Read VHV from device */
nikapov	0:a1a69d32f310	2005	if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
nikapov	0:a1a69d32f310	2006	status = VL53L0X_ref_calibration_io(dev, 1,
nikapov	0:a1a69d32f310	2007	vhv_settings, phase_cal, /* Not used here */
nikapov	0:a1a69d32f310	2008	p_vhv_settings, &phase_cal_int,
nikapov	0:a1a69d32f310	2009	1, 0);
nikapov	0:a1a69d32f310	2010	} else {
nikapov	0:a1a69d32f310	2011	*p_vhv_settings = 0;
nikapov	0:a1a69d32f310	2012	}
nikapov	0:a1a69d32f310	2013
nikapov	0:a1a69d32f310	2014
nikapov	0:a1a69d32f310	2015	if ((status == VL53L0X_ERROR_NONE) && restore_config) {
nikapov	0:a1a69d32f310	2016	/* restore the previous Sequence Config */
nikapov	0:a1a69d32f310	2017	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	2018	sequence_config);
nikapov	0:a1a69d32f310	2019	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2020	PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov	0:a1a69d32f310	2021	}
nikapov	0:a1a69d32f310	2022
nikapov	0:a1a69d32f310	2023	}
nikapov	0:a1a69d32f310	2024
nikapov	0:a1a69d32f310	2025	return status;
nikapov	0:a1a69d32f310	2026	}
nikapov	0:a1a69d32f310	2027
nikapov	0:a1a69d32f310	2028	VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2029	uint8_t *p_phase_cal, const uint8_t get_data_enable,
nikapov	0:a1a69d32f310	2030	const uint8_t restore_config)
nikapov	0:a1a69d32f310	2031	{
nikapov	0:a1a69d32f310	2032	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2033	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	2034	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	2035	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	2036	uint8_t vhv_settingsint;
nikapov	0:a1a69d32f310	2037
nikapov	0:a1a69d32f310	2038	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	2039	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	2040	*/
nikapov	0:a1a69d32f310	2041
Davidroid	3:e9269ff624ed	2042	if (restore_config) {
nikapov	0:a1a69d32f310	2043	sequence_config = PALDevDataGet(dev, SequenceConfig);
Davidroid	3:e9269ff624ed	2044	}
nikapov	0:a1a69d32f310	2045
nikapov	0:a1a69d32f310	2046	/* Run PhaseCal */
nikapov	0:a1a69d32f310	2047	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
nikapov	0:a1a69d32f310	2048
nikapov	0:a1a69d32f310	2049	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2050	status = VL53L0X_perform_single_ref_calibration(dev, 0x0);
nikapov	0:a1a69d32f310	2051	}
nikapov	0:a1a69d32f310	2052
nikapov	0:a1a69d32f310	2053	/* Read PhaseCal from device */
nikapov	0:a1a69d32f310	2054	if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
nikapov	0:a1a69d32f310	2055	status = VL53L0X_ref_calibration_io(dev, 1,
nikapov	0:a1a69d32f310	2056	vhv_settings, phase_cal, /* Not used here */
nikapov	0:a1a69d32f310	2057	&vhv_settingsint, p_phase_cal,
nikapov	0:a1a69d32f310	2058	0, 1);
nikapov	0:a1a69d32f310	2059	} else {
nikapov	0:a1a69d32f310	2060	*p_phase_cal = 0;
nikapov	0:a1a69d32f310	2061	}
nikapov	0:a1a69d32f310	2062
nikapov	0:a1a69d32f310	2063
nikapov	0:a1a69d32f310	2064	if ((status == VL53L0X_ERROR_NONE) && restore_config) {
nikapov	0:a1a69d32f310	2065	/* restore the previous Sequence Config */
nikapov	0:a1a69d32f310	2066	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	2067	sequence_config);
nikapov	0:a1a69d32f310	2068	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2069	PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov	0:a1a69d32f310	2070	}
nikapov	0:a1a69d32f310	2071
nikapov	0:a1a69d32f310	2072	}
nikapov	0:a1a69d32f310	2073
nikapov	0:a1a69d32f310	2074	return status;
nikapov	0:a1a69d32f310	2075	}
nikapov	0:a1a69d32f310	2076
nikapov	0:a1a69d32f310	2077	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2078	uint8_t p_vhv_settings, uint8_t p_phase_cal, uint8_t get_data_enable)
nikapov	0:a1a69d32f310	2079	{
nikapov	0:a1a69d32f310	2080	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2081	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	2082
nikapov	0:a1a69d32f310	2083	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	2084	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	2085	*/
nikapov	0:a1a69d32f310	2086
nikapov	0:a1a69d32f310	2087	sequence_config = PALDevDataGet(dev, SequenceConfig);
nikapov	0:a1a69d32f310	2088
nikapov	0:a1a69d32f310	2089	/* In the following function we don't save the config to optimize
nikapov	0:a1a69d32f310	2090	* writes on device. Config is saved and restored only once. */
nikapov	0:a1a69d32f310	2091	status = VL53L0X_perform_vhv_calibration(
nikapov	0:a1a69d32f310	2092	dev, p_vhv_settings, get_data_enable, 0);
nikapov	0:a1a69d32f310	2093
nikapov	0:a1a69d32f310	2094	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2095	status = VL53L0X_perform_phase_calibration(
nikapov	0:a1a69d32f310	2096	dev, p_phase_cal, get_data_enable, 0);
Davidroid	2:d07edeaff6f1	2097	}
nikapov	0:a1a69d32f310	2098
nikapov	0:a1a69d32f310	2099
nikapov	0:a1a69d32f310	2100	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2101	/* restore the previous Sequence Config */
nikapov	0:a1a69d32f310	2102	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	2103	sequence_config);
nikapov	0:a1a69d32f310	2104	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2105	PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov	0:a1a69d32f310	2106	}
nikapov	0:a1a69d32f310	2107
nikapov	0:a1a69d32f310	2108	}
nikapov	0:a1a69d32f310	2109
nikapov	0:a1a69d32f310	2110	return status;
nikapov	0:a1a69d32f310	2111	}
nikapov	0:a1a69d32f310	2112
nikapov	0:a1a69d32f310	2113	void VL53L0X::get_next_good_spad(uint8_t good_spad_array[], uint32_t size,
nikapov	0:a1a69d32f310	2114	uint32_t curr, int32_t *p_next)
nikapov	0:a1a69d32f310	2115	{
nikapov	0:a1a69d32f310	2116	uint32_t start_index;
nikapov	0:a1a69d32f310	2117	uint32_t fine_offset;
nikapov	0:a1a69d32f310	2118	uint32_t c_spads_per_byte = 8;
nikapov	0:a1a69d32f310	2119	uint32_t coarse_index;
nikapov	0:a1a69d32f310	2120	uint32_t fine_index;
nikapov	0:a1a69d32f310	2121	uint8_t data_byte;
nikapov	0:a1a69d32f310	2122	uint8_t success = 0;
nikapov	0:a1a69d32f310	2123
nikapov	0:a1a69d32f310	2124	/*
nikapov	0:a1a69d32f310	2125	* Starting with the current good spad, loop through the array to find
nikapov	0:a1a69d32f310	2126	* the next. i.e. the next bit set in the sequence.
nikapov	0:a1a69d32f310	2127	*
nikapov	0:a1a69d32f310	2128	* The coarse index is the byte index of the array and the fine index is
nikapov	0:a1a69d32f310	2129	* the index of the bit within each byte.
nikapov	0:a1a69d32f310	2130	*/
nikapov	0:a1a69d32f310	2131
nikapov	0:a1a69d32f310	2132	*p_next = -1;
nikapov	0:a1a69d32f310	2133
nikapov	0:a1a69d32f310	2134	start_index = curr / c_spads_per_byte;
nikapov	0:a1a69d32f310	2135	fine_offset = curr % c_spads_per_byte;
nikapov	0:a1a69d32f310	2136
nikapov	0:a1a69d32f310	2137	for (coarse_index = start_index; ((coarse_index < size) && !success);
nikapov	0:a1a69d32f310	2138	coarse_index++) {
nikapov	0:a1a69d32f310	2139	fine_index = 0;
nikapov	0:a1a69d32f310	2140	data_byte = good_spad_array[coarse_index];
nikapov	0:a1a69d32f310	2141
nikapov	0:a1a69d32f310	2142	if (coarse_index == start_index) {
nikapov	0:a1a69d32f310	2143	/* locate the bit position of the provided current
nikapov	0:a1a69d32f310	2144	* spad bit before iterating */
nikapov	0:a1a69d32f310	2145	data_byte >>= fine_offset;
nikapov	0:a1a69d32f310	2146	fine_index = fine_offset;
nikapov	0:a1a69d32f310	2147	}
nikapov	0:a1a69d32f310	2148
nikapov	0:a1a69d32f310	2149	while (fine_index < c_spads_per_byte) {
nikapov	0:a1a69d32f310	2150	if ((data_byte & 0x1) == 1) {
nikapov	0:a1a69d32f310	2151	success = 1;
nikapov	0:a1a69d32f310	2152	p_next = coarse_index c_spads_per_byte + fine_index;
nikapov	0:a1a69d32f310	2153	break;
nikapov	0:a1a69d32f310	2154	}
nikapov	0:a1a69d32f310	2155	data_byte >>= 1;
nikapov	0:a1a69d32f310	2156	fine_index++;
nikapov	0:a1a69d32f310	2157	}
nikapov	0:a1a69d32f310	2158	}
nikapov	0:a1a69d32f310	2159	}
nikapov	0:a1a69d32f310	2160
nikapov	0:a1a69d32f310	2161	uint8_t VL53L0X::is_aperture(uint32_t spad_index)
nikapov	0:a1a69d32f310	2162	{
nikapov	0:a1a69d32f310	2163	/*
nikapov	0:a1a69d32f310	2164	* This function reports if a given spad index is an aperture SPAD by
nikapov	0:a1a69d32f310	2165	* deriving the quadrant.
nikapov	0:a1a69d32f310	2166	*/
nikapov	0:a1a69d32f310	2167	uint32_t quadrant;
nikapov	0:a1a69d32f310	2168	uint8_t is_aperture = 1;
nikapov	0:a1a69d32f310	2169	quadrant = spad_index >> 6;
nikapov	0:a1a69d32f310	2170	if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) {
nikapov	0:a1a69d32f310	2171	is_aperture = 0;
Davidroid	2:d07edeaff6f1	2172	}
nikapov	0:a1a69d32f310	2173
nikapov	0:a1a69d32f310	2174	return is_aperture;
nikapov	0:a1a69d32f310	2175	}
nikapov	0:a1a69d32f310	2176
nikapov	0:a1a69d32f310	2177	VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[], uint32_t size,
nikapov	0:a1a69d32f310	2178	uint32_t spad_index)
nikapov	0:a1a69d32f310	2179	{
nikapov	0:a1a69d32f310	2180	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2181	uint32_t c_spads_per_byte = 8;
nikapov	0:a1a69d32f310	2182	uint32_t coarse_index;
nikapov	0:a1a69d32f310	2183	uint32_t fine_index;
nikapov	0:a1a69d32f310	2184
nikapov	0:a1a69d32f310	2185	coarse_index = spad_index / c_spads_per_byte;
nikapov	0:a1a69d32f310	2186	fine_index = spad_index % c_spads_per_byte;
nikapov	0:a1a69d32f310	2187	if (coarse_index >= size) {
nikapov	0:a1a69d32f310	2188	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2189	} else {
nikapov	0:a1a69d32f310	2190	spad_array[coarse_index] \|= (1 << fine_index);
nikapov	0:a1a69d32f310	2191	}
nikapov	0:a1a69d32f310	2192
nikapov	0:a1a69d32f310	2193	return status;
nikapov	0:a1a69d32f310	2194	}
nikapov	0:a1a69d32f310	2195
nikapov	0:a1a69d32f310	2196	VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array)
nikapov	0:a1a69d32f310	2197	{
nikapov	0:a1a69d32f310	2198	VL53L0X_Error status = VL53L0X_write_multi(dev,
nikapov	0:a1a69d32f310	2199	VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
nikapov	0:a1a69d32f310	2200	p_ref_spad_array, 6);
nikapov	0:a1a69d32f310	2201
nikapov	0:a1a69d32f310	2202	return status;
nikapov	0:a1a69d32f310	2203	}
nikapov	0:a1a69d32f310	2204
nikapov	0:a1a69d32f310	2205	VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array)
nikapov	0:a1a69d32f310	2206	{
nikapov	0:a1a69d32f310	2207	VL53L0X_Error status = VL53L0X_read_multi(dev,
nikapov	0:a1a69d32f310	2208	VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
nikapov	0:a1a69d32f310	2209	p_ref_spad_array,
nikapov	0:a1a69d32f310	2210	6);
nikapov	0:a1a69d32f310	2211	// VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2212	// uint8_t count=0;
nikapov	0:a1a69d32f310	2213
nikapov	0:a1a69d32f310	2214	// for (count = 0; count < 6; count++)
nikapov	0:a1a69d32f310	2215	// status = VL53L0X_RdByte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]);
nikapov	0:a1a69d32f310	2216	return status;
nikapov	0:a1a69d32f310	2217	}
nikapov	0:a1a69d32f310	2218
nikapov	0:a1a69d32f310	2219	VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2220	uint8_t aperture_spads,
nikapov	0:a1a69d32f310	2221	uint8_t good_spad_array[],
nikapov	0:a1a69d32f310	2222	uint8_t spad_array[],
nikapov	0:a1a69d32f310	2223	uint32_t size,
nikapov	0:a1a69d32f310	2224	uint32_t start,
nikapov	0:a1a69d32f310	2225	uint32_t offset,
nikapov	0:a1a69d32f310	2226	uint32_t spad_count,
nikapov	0:a1a69d32f310	2227	uint32_t *p_last_spad)
nikapov	0:a1a69d32f310	2228	{
nikapov	0:a1a69d32f310	2229	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2230	uint32_t index;
nikapov	0:a1a69d32f310	2231	uint32_t i;
nikapov	0:a1a69d32f310	2232	int32_t next_good_spad = offset;
nikapov	0:a1a69d32f310	2233	uint32_t current_spad;
nikapov	0:a1a69d32f310	2234	uint8_t check_spad_array[6];
nikapov	0:a1a69d32f310	2235
nikapov	0:a1a69d32f310	2236	/*
nikapov	0:a1a69d32f310	2237	* This function takes in a spad array which may or may not have SPADS
nikapov	0:a1a69d32f310	2238	* already enabled and appends from a given offset a requested number
nikapov	0:a1a69d32f310	2239	* of new SPAD enables. The 'good spad map' is applied to
nikapov	0:a1a69d32f310	2240	* determine the next SPADs to enable.
nikapov	0:a1a69d32f310	2241	*
nikapov	0:a1a69d32f310	2242	* This function applies to only aperture or only non-aperture spads.
nikapov	0:a1a69d32f310	2243	* Checks are performed to ensure this.
nikapov	0:a1a69d32f310	2244	*/
nikapov	0:a1a69d32f310	2245
nikapov	0:a1a69d32f310	2246	current_spad = offset;
nikapov	0:a1a69d32f310	2247	for (index = 0; index < spad_count; index++) {
nikapov	0:a1a69d32f310	2248	get_next_good_spad(good_spad_array, size, current_spad,
nikapov	0:a1a69d32f310	2249	&next_good_spad);
nikapov	0:a1a69d32f310	2250
nikapov	0:a1a69d32f310	2251	if (next_good_spad == -1) {
nikapov	0:a1a69d32f310	2252	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2253	break;
nikapov	0:a1a69d32f310	2254	}
nikapov	0:a1a69d32f310	2255
nikapov	0:a1a69d32f310	2256	/* Confirm that the next good SPAD is non-aperture */
nikapov	0:a1a69d32f310	2257	if (is_aperture(start + next_good_spad) != aperture_spads) {
nikapov	0:a1a69d32f310	2258	/* if we can't get the required number of good aperture
nikapov	0:a1a69d32f310	2259	* spads from the current quadrant then this is an error
nikapov	0:a1a69d32f310	2260	*/
nikapov	0:a1a69d32f310	2261	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2262	break;
nikapov	0:a1a69d32f310	2263	}
nikapov	0:a1a69d32f310	2264	current_spad = (uint32_t)next_good_spad;
nikapov	0:a1a69d32f310	2265	enable_spad_bit(spad_array, size, current_spad);
nikapov	0:a1a69d32f310	2266	current_spad++;
nikapov	0:a1a69d32f310	2267	}
nikapov	0:a1a69d32f310	2268	*p_last_spad = current_spad;
nikapov	0:a1a69d32f310	2269
nikapov	0:a1a69d32f310	2270	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2271	status = set_ref_spad_map(dev, spad_array);
nikapov	0:a1a69d32f310	2272	}
nikapov	0:a1a69d32f310	2273
nikapov	0:a1a69d32f310	2274
nikapov	0:a1a69d32f310	2275	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2276	status = get_ref_spad_map(dev, check_spad_array);
nikapov	0:a1a69d32f310	2277
nikapov	0:a1a69d32f310	2278	i = 0;
nikapov	0:a1a69d32f310	2279
nikapov	0:a1a69d32f310	2280	/* Compare spad maps. If not equal report error. */
nikapov	0:a1a69d32f310	2281	while (i < size) {
nikapov	0:a1a69d32f310	2282	if (spad_array[i] != check_spad_array[i]) {
nikapov	0:a1a69d32f310	2283	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	2284	break;
nikapov	0:a1a69d32f310	2285	}
nikapov	0:a1a69d32f310	2286	i++;
nikapov	0:a1a69d32f310	2287	}
nikapov	0:a1a69d32f310	2288	}
nikapov	0:a1a69d32f310	2289	return status;
nikapov	0:a1a69d32f310	2290	}
nikapov	0:a1a69d32f310	2291
nikapov	0:a1a69d32f310	2292	VL53L0X_Error VL53L0X::VL53L0X_set_device_mode(VL53L0X_DEV dev, VL53L0X_DeviceModes device_mode)
nikapov	0:a1a69d32f310	2293	{
nikapov	0:a1a69d32f310	2294	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2295
nikapov	0:a1a69d32f310	2296	LOG_FUNCTION_START("%d", (int)DeviceMode);
nikapov	0:a1a69d32f310	2297
nikapov	0:a1a69d32f310	2298	switch (device_mode) {
nikapov	0:a1a69d32f310	2299	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov	0:a1a69d32f310	2300	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov	0:a1a69d32f310	2301	case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov	0:a1a69d32f310	2302	case VL53L0X_DEVICEMODE_GPIO_DRIVE:
nikapov	0:a1a69d32f310	2303	case VL53L0X_DEVICEMODE_GPIO_OSC:
nikapov	0:a1a69d32f310	2304	/* Supported modes */
nikapov	0:a1a69d32f310	2305	VL53L0X_SETPARAMETERFIELD(dev, DeviceMode, device_mode);
nikapov	0:a1a69d32f310	2306	break;
nikapov	0:a1a69d32f310	2307	default:
nikapov	0:a1a69d32f310	2308	/* Unsupported mode */
nikapov	0:a1a69d32f310	2309	status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov	0:a1a69d32f310	2310	}
nikapov	0:a1a69d32f310	2311
nikapov	0:a1a69d32f310	2312	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2313	return status;
nikapov	0:a1a69d32f310	2314	}
nikapov	0:a1a69d32f310	2315
nikapov	0:a1a69d32f310	2316	VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2317	VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low,
nikapov	0:a1a69d32f310	2318	FixPoint1616_t threshold_high)
nikapov	0:a1a69d32f310	2319	{
nikapov	0:a1a69d32f310	2320	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2321	uint16_t threshold16;
nikapov	0:a1a69d32f310	2322	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2323
nikapov	0:a1a69d32f310	2324	/* no dependency on DeviceMode for Ewok */
nikapov	0:a1a69d32f310	2325	/* Need to divide by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2326	threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
nikapov	0:a1a69d32f310	2327	status = VL53L0X_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16);
nikapov	0:a1a69d32f310	2328
nikapov	0:a1a69d32f310	2329	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2330	/* Need to divide by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2331	threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
nikapov	0:a1a69d32f310	2332	status = VL53L0X_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
nikapov	0:a1a69d32f310	2333	threshold16);
nikapov	0:a1a69d32f310	2334	}
nikapov	0:a1a69d32f310	2335
nikapov	0:a1a69d32f310	2336	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2337	return status;
nikapov	0:a1a69d32f310	2338	}
nikapov	0:a1a69d32f310	2339
nikapov	0:a1a69d32f310	2340	VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2341	VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low,
nikapov	0:a1a69d32f310	2342	FixPoint1616_t *p_threshold_high)
nikapov	0:a1a69d32f310	2343	{
nikapov	0:a1a69d32f310	2344	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2345	uint16_t threshold16;
nikapov	0:a1a69d32f310	2346	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2347
nikapov	0:a1a69d32f310	2348	/* no dependency on DeviceMode for Ewok */
nikapov	0:a1a69d32f310	2349
nikapov	0:a1a69d32f310	2350	status = VL53L0X_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16);
nikapov	0:a1a69d32f310	2351	/* Need to multiply by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2352	*p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17);
nikapov	0:a1a69d32f310	2353
nikapov	0:a1a69d32f310	2354	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2355	status = VL53L0X_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
nikapov	0:a1a69d32f310	2356	&threshold16);
nikapov	0:a1a69d32f310	2357	/* Need to multiply by 2 because the FW will apply a x2 */
nikapov	0:a1a69d32f310	2358	*p_threshold_high =
nikapov	0:a1a69d32f310	2359	(FixPoint1616_t)((0x00fff & threshold16) << 17);
nikapov	0:a1a69d32f310	2360	}
nikapov	0:a1a69d32f310	2361
nikapov	0:a1a69d32f310	2362	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2363	return status;
nikapov	0:a1a69d32f310	2364	}
nikapov	0:a1a69d32f310	2365
nikapov	0:a1a69d32f310	2366	VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2367	uint8_t *p_tuning_setting_buffer)
nikapov	0:a1a69d32f310	2368	{
nikapov	0:a1a69d32f310	2369	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2370	int i;
nikapov	0:a1a69d32f310	2371	int index;
nikapov	0:a1a69d32f310	2372	uint8_t msb;
nikapov	0:a1a69d32f310	2373	uint8_t lsb;
nikapov	0:a1a69d32f310	2374	uint8_t select_param;
nikapov	0:a1a69d32f310	2375	uint8_t number_of_writes;
nikapov	0:a1a69d32f310	2376	uint8_t address;
nikapov	0:a1a69d32f310	2377	uint8_t local_buffer[4]; /* max */
nikapov	0:a1a69d32f310	2378	uint16_t temp16;
nikapov	0:a1a69d32f310	2379
nikapov	0:a1a69d32f310	2380	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2381
nikapov	0:a1a69d32f310	2382	index = 0;
nikapov	0:a1a69d32f310	2383
nikapov	0:a1a69d32f310	2384	while ((*(p_tuning_setting_buffer + index) != 0) &&
nikapov	0:a1a69d32f310	2385	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	2386	number_of_writes = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2387	index++;
nikapov	0:a1a69d32f310	2388	if (number_of_writes == 0xFF) {
nikapov	0:a1a69d32f310	2389	/* internal parameters */
nikapov	0:a1a69d32f310	2390	select_param = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2391	index++;
nikapov	0:a1a69d32f310	2392	switch (select_param) {
nikapov	0:a1a69d32f310	2393	case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
nikapov	0:a1a69d32f310	2394	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2395	index++;
nikapov	0:a1a69d32f310	2396	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2397	index++;
nikapov	0:a1a69d32f310	2398	temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov	0:a1a69d32f310	2399	PALDevDataSet(dev, SigmaEstRefArray, temp16);
nikapov	0:a1a69d32f310	2400	break;
nikapov	0:a1a69d32f310	2401	case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
nikapov	0:a1a69d32f310	2402	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2403	index++;
nikapov	0:a1a69d32f310	2404	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2405	index++;
nikapov	0:a1a69d32f310	2406	temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov	0:a1a69d32f310	2407	PALDevDataSet(dev, SigmaEstEffPulseWidth,
nikapov	0:a1a69d32f310	2408	temp16);
nikapov	0:a1a69d32f310	2409	break;
nikapov	0:a1a69d32f310	2410	case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
nikapov	0:a1a69d32f310	2411	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2412	index++;
nikapov	0:a1a69d32f310	2413	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2414	index++;
nikapov	0:a1a69d32f310	2415	temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov	0:a1a69d32f310	2416	PALDevDataSet(dev, SigmaEstEffAmbWidth, temp16);
nikapov	0:a1a69d32f310	2417	break;
nikapov	0:a1a69d32f310	2418	case 3: /* uint16_t targetRefRate -> 2 bytes */
nikapov	0:a1a69d32f310	2419	msb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2420	index++;
nikapov	0:a1a69d32f310	2421	lsb = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2422	index++;
nikapov	0:a1a69d32f310	2423	temp16 = VL53L0X_MAKEUINT16(lsb, msb);
nikapov	0:a1a69d32f310	2424	PALDevDataSet(dev, targetRefRate, temp16);
nikapov	0:a1a69d32f310	2425	break;
nikapov	0:a1a69d32f310	2426	default: /* invalid parameter */
nikapov	0:a1a69d32f310	2427	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	2428	}
nikapov	0:a1a69d32f310	2429
nikapov	0:a1a69d32f310	2430	} else if (number_of_writes <= 4) {
nikapov	0:a1a69d32f310	2431	address = *(p_tuning_setting_buffer + index);
nikapov	0:a1a69d32f310	2432	index++;
nikapov	0:a1a69d32f310	2433
nikapov	0:a1a69d32f310	2434	for (i = 0; i < number_of_writes; i++) {
nikapov	0:a1a69d32f310	2435	local_buffer[i] = *(p_tuning_setting_buffer +
nikapov	0:a1a69d32f310	2436	index);
nikapov	0:a1a69d32f310	2437	index++;
nikapov	0:a1a69d32f310	2438	}
nikapov	0:a1a69d32f310	2439
nikapov	0:a1a69d32f310	2440	status = VL53L0X_write_multi(dev, address, local_buffer,
nikapov	0:a1a69d32f310	2441	number_of_writes);
nikapov	0:a1a69d32f310	2442
nikapov	0:a1a69d32f310	2443	} else {
nikapov	0:a1a69d32f310	2444	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	2445	}
nikapov	0:a1a69d32f310	2446	}
nikapov	0:a1a69d32f310	2447
nikapov	0:a1a69d32f310	2448	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2449	return status;
nikapov	0:a1a69d32f310	2450	}
nikapov	0:a1a69d32f310	2451
nikapov	0:a1a69d32f310	2452	VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2453	uint8_t start_not_stopflag)
nikapov	0:a1a69d32f310	2454	{
nikapov	0:a1a69d32f310	2455	uint8_t interrupt_config;
nikapov	0:a1a69d32f310	2456	FixPoint1616_t threshold_low;
nikapov	0:a1a69d32f310	2457	FixPoint1616_t threshold_high;
nikapov	0:a1a69d32f310	2458	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2459
nikapov	0:a1a69d32f310	2460	interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	2461	Pin0GpioFunctionality);
nikapov	0:a1a69d32f310	2462
nikapov	0:a1a69d32f310	2463	if ((interrupt_config ==
nikapov	0:a1a69d32f310	2464	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) \|\|
nikapov	0:a1a69d32f310	2465	(interrupt_config ==
nikapov	0:a1a69d32f310	2466	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) \|\|
nikapov	0:a1a69d32f310	2467	(interrupt_config ==
nikapov	0:a1a69d32f310	2468	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
nikapov	0:a1a69d32f310	2469
nikapov	0:a1a69d32f310	2470	status = VL53L0X_get_interrupt_thresholds(dev,
nikapov	0:a1a69d32f310	2471	VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov	0:a1a69d32f310	2472	&threshold_low, &threshold_high);
nikapov	0:a1a69d32f310	2473
nikapov	0:a1a69d32f310	2474	if (((threshold_low > 255 * 65536) \|\|
nikapov	0:a1a69d32f310	2475	(threshold_high > 255 * 65536)) &&
nikapov	0:a1a69d32f310	2476	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	2477
nikapov	0:a1a69d32f310	2478	if (start_not_stopflag != 0) {
nikapov	0:a1a69d32f310	2479	status = VL53L0X_load_tuning_settings(dev,
nikapov	0:a1a69d32f310	2480	InterruptThresholdSettings);
nikapov	0:a1a69d32f310	2481	} else {
nikapov	0:a1a69d32f310	2482	status \|= VL53L0X_write_byte(dev, 0xFF, 0x04);
nikapov	0:a1a69d32f310	2483	status \|= VL53L0X_write_byte(dev, 0x70, 0x00);
nikapov	0:a1a69d32f310	2484	status \|= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	2485	status \|= VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov	0:a1a69d32f310	2486	}
nikapov	0:a1a69d32f310	2487
nikapov	0:a1a69d32f310	2488	}
nikapov	0:a1a69d32f310	2489
nikapov	0:a1a69d32f310	2490
nikapov	0:a1a69d32f310	2491	}
nikapov	0:a1a69d32f310	2492
nikapov	0:a1a69d32f310	2493	return status;
nikapov	0:a1a69d32f310	2494
nikapov	0:a1a69d32f310	2495	}
nikapov	0:a1a69d32f310	2496
nikapov	0:a1a69d32f310	2497	VL53L0X_Error VL53L0X::VL53L0X_start_measurement(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	2498	{
nikapov	0:a1a69d32f310	2499	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2500	VL53L0X_DeviceModes device_mode;
nikapov	0:a1a69d32f310	2501	uint8_t byte;
nikapov	0:a1a69d32f310	2502	uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
nikapov	0:a1a69d32f310	2503	uint32_t loop_nb;
nikapov	0:a1a69d32f310	2504	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2505
nikapov	0:a1a69d32f310	2506	/* Get Current DeviceMode */
nikapov	0:a1a69d32f310	2507	VL53L0X_get_device_mode(dev, &device_mode);
nikapov	0:a1a69d32f310	2508
nikapov	0:a1a69d32f310	2509	status = VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov	0:a1a69d32f310	2510	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	2511	status = VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov	0:a1a69d32f310	2512	status = VL53L0X_write_byte(dev, 0x91, PALDevDataGet(dev, StopVariable));
nikapov	0:a1a69d32f310	2513	status = VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov	0:a1a69d32f310	2514	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	2515	status = VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov	0:a1a69d32f310	2516
nikapov	0:a1a69d32f310	2517	switch (device_mode) {
nikapov	0:a1a69d32f310	2518	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov	0:a1a69d32f310	2519	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x01);
nikapov	0:a1a69d32f310	2520
nikapov	0:a1a69d32f310	2521	byte = start_stop_byte;
nikapov	0:a1a69d32f310	2522	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2523	/* Wait until start bit has been cleared */
nikapov	0:a1a69d32f310	2524	loop_nb = 0;
nikapov	0:a1a69d32f310	2525	do {
nikapov	0:a1a69d32f310	2526	if (loop_nb > 0)
nikapov	0:a1a69d32f310	2527	status = VL53L0X_read_byte(dev,
nikapov	0:a1a69d32f310	2528	VL53L0X_REG_SYSRANGE_START, &byte);
nikapov	0:a1a69d32f310	2529	loop_nb = loop_nb + 1;
nikapov	0:a1a69d32f310	2530	} while (((byte & start_stop_byte) == start_stop_byte)
nikapov	0:a1a69d32f310	2531	&& (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	2532	&& (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
nikapov	0:a1a69d32f310	2533
Davidroid	3:e9269ff624ed	2534	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	2535	status = VL53L0X_ERROR_TIME_OUT;
Davidroid	3:e9269ff624ed	2536	}
nikapov	0:a1a69d32f310	2537
nikapov	0:a1a69d32f310	2538	}
nikapov	0:a1a69d32f310	2539
nikapov	0:a1a69d32f310	2540	break;
nikapov	0:a1a69d32f310	2541	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov	0:a1a69d32f310	2542	/* Back-to-back mode */
nikapov	0:a1a69d32f310	2543
nikapov	0:a1a69d32f310	2544	/* Check if need to apply interrupt settings */
Davidroid	3:e9269ff624ed	2545	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2546	status = VL53L0X_check_and_load_interrupt_settings(dev, 1);
Davidroid	3:e9269ff624ed	2547	}
nikapov	0:a1a69d32f310	2548
nikapov	0:a1a69d32f310	2549	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	2550	VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	2551	VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
nikapov	0:a1a69d32f310	2552	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2553	/* Set PAL State to Running */
nikapov	0:a1a69d32f310	2554	PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING);
nikapov	0:a1a69d32f310	2555	}
nikapov	0:a1a69d32f310	2556	break;
nikapov	0:a1a69d32f310	2557	case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov	0:a1a69d32f310	2558	/* Continuous mode */
nikapov	0:a1a69d32f310	2559	/* Check if need to apply interrupt settings */
Davidroid	3:e9269ff624ed	2560	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2561	status = VL53L0X_check_and_load_interrupt_settings(dev, 1);
Davidroid	3:e9269ff624ed	2562	}
nikapov	0:a1a69d32f310	2563
nikapov	0:a1a69d32f310	2564	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	2565	VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	2566	VL53L0X_REG_SYSRANGE_MODE_TIMED);
nikapov	0:a1a69d32f310	2567
nikapov	0:a1a69d32f310	2568	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2569	/* Set PAL State to Running */
nikapov	0:a1a69d32f310	2570	PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING);
nikapov	0:a1a69d32f310	2571	}
nikapov	0:a1a69d32f310	2572	break;
nikapov	0:a1a69d32f310	2573	default:
nikapov	0:a1a69d32f310	2574	/* Selected mode not supported */
nikapov	0:a1a69d32f310	2575	status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov	0:a1a69d32f310	2576	}
nikapov	0:a1a69d32f310	2577
nikapov	0:a1a69d32f310	2578
nikapov	0:a1a69d32f310	2579	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2580	return status;
nikapov	0:a1a69d32f310	2581	}
nikapov	0:a1a69d32f310	2582
nikapov	0:a1a69d32f310	2583	/* Group PAL Measurement Functions */
nikapov	0:a1a69d32f310	2584	VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	2585	{
nikapov	0:a1a69d32f310	2586	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2587	VL53L0X_DeviceModes device_mode;
nikapov	0:a1a69d32f310	2588
nikapov	0:a1a69d32f310	2589	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2590
nikapov	0:a1a69d32f310	2591	/* Get Current DeviceMode */
nikapov	0:a1a69d32f310	2592	status = VL53L0X_get_device_mode(dev, &device_mode);
nikapov	0:a1a69d32f310	2593
nikapov	0:a1a69d32f310	2594	/* Start immediately to run a single ranging measurement in case of
nikapov	0:a1a69d32f310	2595	* single ranging or single histogram */
nikapov	0:a1a69d32f310	2596	if (status == VL53L0X_ERROR_NONE
nikapov	0:a1a69d32f310	2597	&& device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
nikapov	0:a1a69d32f310	2598	status = VL53L0X_start_measurement(dev);
nikapov	0:a1a69d32f310	2599	}
nikapov	0:a1a69d32f310	2600
nikapov	0:a1a69d32f310	2601
nikapov	0:a1a69d32f310	2602	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2603	status = VL53L0X_measurement_poll_for_completion(dev);
nikapov	0:a1a69d32f310	2604	}
nikapov	0:a1a69d32f310	2605
nikapov	0:a1a69d32f310	2606
nikapov	0:a1a69d32f310	2607	/* Change PAL State in case of single ranging or single histogram */
nikapov	0:a1a69d32f310	2608	if (status == VL53L0X_ERROR_NONE
Davidroid	3:e9269ff624ed	2609	&& device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
nikapov	0:a1a69d32f310	2610	PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
Davidroid	3:e9269ff624ed	2611	}
nikapov	0:a1a69d32f310	2612
nikapov	0:a1a69d32f310	2613
nikapov	0:a1a69d32f310	2614	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2615	return status;
nikapov	0:a1a69d32f310	2616	}
nikapov	0:a1a69d32f310	2617
nikapov	0:a1a69d32f310	2618	VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2619	uint8_t *p_x_talk_compensation_enable)
nikapov	0:a1a69d32f310	2620	{
nikapov	0:a1a69d32f310	2621	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2622	uint8_t temp8;
nikapov	0:a1a69d32f310	2623	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2624
nikapov	0:a1a69d32f310	2625	VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable, temp8);
nikapov	0:a1a69d32f310	2626	*p_x_talk_compensation_enable = temp8;
nikapov	0:a1a69d32f310	2627
nikapov	0:a1a69d32f310	2628	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2629	return status;
nikapov	0:a1a69d32f310	2630	}
nikapov	0:a1a69d32f310	2631
nikapov	0:a1a69d32f310	2632	VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2633	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	2634	FixPoint1616_t *p_total_xtalk_rate_mcps)
nikapov	0:a1a69d32f310	2635	{
nikapov	0:a1a69d32f310	2636	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2637
nikapov	0:a1a69d32f310	2638	uint8_t xtalk_comp_enable;
nikapov	0:a1a69d32f310	2639	FixPoint1616_t total_xtalk_mega_cps;
nikapov	0:a1a69d32f310	2640	FixPoint1616_t xtalk_per_spad_mega_cps;
nikapov	0:a1a69d32f310	2641
nikapov	0:a1a69d32f310	2642	*p_total_xtalk_rate_mcps = 0;
nikapov	0:a1a69d32f310	2643
nikapov	0:a1a69d32f310	2644	status = VL53L0X_get_x_talk_compensation_enable(dev, &xtalk_comp_enable);
nikapov	0:a1a69d32f310	2645	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2646
nikapov	0:a1a69d32f310	2647	if (xtalk_comp_enable) {
nikapov	0:a1a69d32f310	2648
nikapov	0:a1a69d32f310	2649	VL53L0X_GETPARAMETERFIELD(
nikapov	0:a1a69d32f310	2650	dev,
nikapov	0:a1a69d32f310	2651	XTalkCompensationRateMegaCps,
nikapov	0:a1a69d32f310	2652	xtalk_per_spad_mega_cps);
nikapov	0:a1a69d32f310	2653
nikapov	0:a1a69d32f310	2654	/* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
nikapov	0:a1a69d32f310	2655	total_xtalk_mega_cps =
nikapov	0:a1a69d32f310	2656	p_ranging_measurement_data->EffectiveSpadRtnCount *
nikapov	0:a1a69d32f310	2657	xtalk_per_spad_mega_cps;
nikapov	0:a1a69d32f310	2658
nikapov	0:a1a69d32f310	2659	/* FixPoint0824 >> 8 = FixPoint1616 */
nikapov	0:a1a69d32f310	2660	*p_total_xtalk_rate_mcps =
nikapov	0:a1a69d32f310	2661	(total_xtalk_mega_cps + 0x80) >> 8;
nikapov	0:a1a69d32f310	2662	}
nikapov	0:a1a69d32f310	2663	}
nikapov	0:a1a69d32f310	2664
nikapov	0:a1a69d32f310	2665	return status;
nikapov	0:a1a69d32f310	2666	}
nikapov	0:a1a69d32f310	2667
nikapov	0:a1a69d32f310	2668	VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2669	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	2670	FixPoint1616_t *p_total_signal_rate_mcps)
nikapov	0:a1a69d32f310	2671	{
nikapov	0:a1a69d32f310	2672	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2673	FixPoint1616_t total_xtalk_mega_cps;
nikapov	0:a1a69d32f310	2674
nikapov	0:a1a69d32f310	2675	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2676
nikapov	0:a1a69d32f310	2677	*p_total_signal_rate_mcps =
nikapov	0:a1a69d32f310	2678	p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	2679
nikapov	0:a1a69d32f310	2680	status = VL53L0X_get_total_xtalk_rate(
nikapov	0:a1a69d32f310	2681	dev, p_ranging_measurement_data, &total_xtalk_mega_cps);
nikapov	0:a1a69d32f310	2682
nikapov	0:a1a69d32f310	2683	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	2684	*p_total_signal_rate_mcps += total_xtalk_mega_cps;
nikapov	0:a1a69d32f310	2685	}
nikapov	0:a1a69d32f310	2686
nikapov	0:a1a69d32f310	2687	return status;
nikapov	0:a1a69d32f310	2688	}
nikapov	0:a1a69d32f310	2689
nikapov	0:a1a69d32f310	2690	/* To convert ms into register value */
nikapov	0:a1a69d32f310	2691	uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2692	uint32_t timeout_period_us,
nikapov	0:a1a69d32f310	2693	uint8_t vcsel_period_pclks)
nikapov	0:a1a69d32f310	2694	{
nikapov	0:a1a69d32f310	2695	uint32_t macro_period_ps;
nikapov	0:a1a69d32f310	2696	uint32_t macro_period_ns;
nikapov	0:a1a69d32f310	2697	uint32_t timeout_period_mclks = 0;
nikapov	0:a1a69d32f310	2698
nikapov	0:a1a69d32f310	2699	macro_period_ps = VL53L0X_calc_macro_period_ps(dev, vcsel_period_pclks);
nikapov	0:a1a69d32f310	2700	macro_period_ns = (macro_period_ps + 500) / 1000;
nikapov	0:a1a69d32f310	2701
nikapov	0:a1a69d32f310	2702	timeout_period_mclks =
nikapov	0:a1a69d32f310	2703	(uint32_t)(((timeout_period_us * 1000)
nikapov	0:a1a69d32f310	2704	+ (macro_period_ns / 2)) / macro_period_ns);
nikapov	0:a1a69d32f310	2705
nikapov	0:a1a69d32f310	2706	return timeout_period_mclks;
nikapov	0:a1a69d32f310	2707	}
nikapov	0:a1a69d32f310	2708
nikapov	0:a1a69d32f310	2709	uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
nikapov	0:a1a69d32f310	2710	{
nikapov	0:a1a69d32f310	2711	/*
nikapov	0:a1a69d32f310	2712	* Implements an integer square root
nikapov	0:a1a69d32f310	2713	*
nikapov	0:a1a69d32f310	2714	* From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
nikapov	0:a1a69d32f310	2715	*/
nikapov	0:a1a69d32f310	2716
nikapov	0:a1a69d32f310	2717	uint32_t res = 0;
nikapov	0:a1a69d32f310	2718	uint32_t bit = 1 << 30;
nikapov	0:a1a69d32f310	2719	/* The second-to-top bit is set:
nikapov	0:a1a69d32f310	2720	* 1 << 14 for 16-bits, 1 << 30 for 32 bits */
nikapov	0:a1a69d32f310	2721
nikapov	0:a1a69d32f310	2722	/* "bit" starts at the highest power of four <= the argument. */
nikapov	0:a1a69d32f310	2723	while (bit > num) {
nikapov	0:a1a69d32f310	2724	bit >>= 2;
nikapov	0:a1a69d32f310	2725	}
nikapov	0:a1a69d32f310	2726
nikapov	0:a1a69d32f310	2727
nikapov	0:a1a69d32f310	2728	while (bit != 0) {
nikapov	0:a1a69d32f310	2729	if (num >= res + bit) {
nikapov	0:a1a69d32f310	2730	num -= res + bit;
nikapov	0:a1a69d32f310	2731	res = (res >> 1) + bit;
Davidroid	3:e9269ff624ed	2732	} else {
nikapov	0:a1a69d32f310	2733	res >>= 1;
Davidroid	3:e9269ff624ed	2734	}
nikapov	0:a1a69d32f310	2735
nikapov	0:a1a69d32f310	2736	bit >>= 2;
nikapov	0:a1a69d32f310	2737	}
nikapov	0:a1a69d32f310	2738
nikapov	0:a1a69d32f310	2739	return res;
nikapov	0:a1a69d32f310	2740	}
nikapov	0:a1a69d32f310	2741
nikapov	0:a1a69d32f310	2742	VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(
nikapov	0:a1a69d32f310	2743	VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2744	FixPoint1616_t total_signal_rate_mcps,
nikapov	0:a1a69d32f310	2745	FixPoint1616_t total_corr_signal_rate_mcps,
nikapov	0:a1a69d32f310	2746	FixPoint1616_t pw_mult,
nikapov	0:a1a69d32f310	2747	uint32_t sigma_estimate_p1,
nikapov	0:a1a69d32f310	2748	FixPoint1616_t sigma_estimate_p2,
nikapov	0:a1a69d32f310	2749	uint32_t peak_vcsel_duration_us,
nikapov	0:a1a69d32f310	2750	uint32_t *pd_max_mm)
nikapov	0:a1a69d32f310	2751	{
nikapov	0:a1a69d32f310	2752	const uint32_t c_sigma_limit = 18;
nikapov	0:a1a69d32f310	2753	const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
nikapov	0:a1a69d32f310	2754	const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */
nikapov	0:a1a69d32f310	2755	const uint32_t c_amb_eff_width_sigma_est_ns = 6;
nikapov	0:a1a69d32f310	2756	const uint32_t c_amb_eff_width_d_max_ns = 7;
nikapov	0:a1a69d32f310	2757	uint32_t dmax_cal_range_mm;
nikapov	0:a1a69d32f310	2758	FixPoint1616_t dmax_cal_signal_rate_rtn_mcps;
nikapov	0:a1a69d32f310	2759	FixPoint1616_t min_signal_needed;
nikapov	0:a1a69d32f310	2760	FixPoint1616_t min_signal_needed_p1;
nikapov	0:a1a69d32f310	2761	FixPoint1616_t min_signal_needed_p2;
nikapov	0:a1a69d32f310	2762	FixPoint1616_t min_signal_needed_p3;
nikapov	0:a1a69d32f310	2763	FixPoint1616_t min_signal_needed_p4;
nikapov	0:a1a69d32f310	2764	FixPoint1616_t sigma_limit_tmp;
nikapov	0:a1a69d32f310	2765	FixPoint1616_t sigma_est_sq_tmp;
nikapov	0:a1a69d32f310	2766	FixPoint1616_t signal_limit_tmp;
nikapov	0:a1a69d32f310	2767	FixPoint1616_t signal_at0_mm;
nikapov	0:a1a69d32f310	2768	FixPoint1616_t dmax_dark;
nikapov	0:a1a69d32f310	2769	FixPoint1616_t dmax_ambient;
nikapov	0:a1a69d32f310	2770	FixPoint1616_t dmax_dark_tmp;
nikapov	0:a1a69d32f310	2771	FixPoint1616_t sigma_est_p2_tmp;
nikapov	0:a1a69d32f310	2772	uint32_t signal_rate_temp_mcps;
nikapov	0:a1a69d32f310	2773
nikapov	0:a1a69d32f310	2774	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2775
nikapov	0:a1a69d32f310	2776	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2777
nikapov	0:a1a69d32f310	2778	dmax_cal_range_mm =
nikapov	0:a1a69d32f310	2779	PALDevDataGet(dev, DmaxCalRangeMilliMeter);
nikapov	0:a1a69d32f310	2780
nikapov	0:a1a69d32f310	2781	dmax_cal_signal_rate_rtn_mcps =
nikapov	0:a1a69d32f310	2782	PALDevDataGet(dev, DmaxCalSignalRateRtnMegaCps);
nikapov	0:a1a69d32f310	2783
nikapov	0:a1a69d32f310	2784	/* uint32 * FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	2785	signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
nikapov	0:a1a69d32f310	2786
nikapov	0:a1a69d32f310	2787	/* FixPoint1616 >> 8 = FixPoint2408 */
nikapov	0:a1a69d32f310	2788	signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
nikapov	0:a1a69d32f310	2789	signal_at0_mm *= dmax_cal_range_mm;
nikapov	0:a1a69d32f310	2790
nikapov	0:a1a69d32f310	2791	min_signal_needed_p1 = 0;
nikapov	0:a1a69d32f310	2792	if (total_corr_signal_rate_mcps > 0) {
nikapov	0:a1a69d32f310	2793
nikapov	0:a1a69d32f310	2794	/* Shift by 10 bits to increase resolution prior to the
nikapov	0:a1a69d32f310	2795	* division */
nikapov	0:a1a69d32f310	2796	signal_rate_temp_mcps = total_signal_rate_mcps << 10;
nikapov	0:a1a69d32f310	2797
nikapov	0:a1a69d32f310	2798	/* Add rounding value prior to division */
nikapov	0:a1a69d32f310	2799	min_signal_needed_p1 = signal_rate_temp_mcps +
nikapov	0:a1a69d32f310	2800	(total_corr_signal_rate_mcps / 2);
nikapov	0:a1a69d32f310	2801
nikapov	0:a1a69d32f310	2802	/* FixPoint0626/FixPoint1616 = FixPoint2210 */
nikapov	0:a1a69d32f310	2803	min_signal_needed_p1 /= total_corr_signal_rate_mcps;
nikapov	0:a1a69d32f310	2804
nikapov	0:a1a69d32f310	2805	/* Apply a factored version of the speed of light.
nikapov	0:a1a69d32f310	2806	Correction to be applied at the end */
nikapov	0:a1a69d32f310	2807	min_signal_needed_p1 *= 3;
nikapov	0:a1a69d32f310	2808
nikapov	0:a1a69d32f310	2809	/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
nikapov	0:a1a69d32f310	2810	min_signal_needed_p1 *= min_signal_needed_p1;
nikapov	0:a1a69d32f310	2811
nikapov	0:a1a69d32f310	2812	/* FixPoint1220 >> 16 = FixPoint2804 */
nikapov	0:a1a69d32f310	2813	min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2814	}
nikapov	0:a1a69d32f310	2815
nikapov	0:a1a69d32f310	2816	min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
nikapov	0:a1a69d32f310	2817
nikapov	0:a1a69d32f310	2818	/* FixPoint1616 >> 16 = uint32 */
nikapov	0:a1a69d32f310	2819	min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2820
nikapov	0:a1a69d32f310	2821	/* uint32 * uint32 = uint32 */
nikapov	0:a1a69d32f310	2822	min_signal_needed_p2 *= min_signal_needed_p2;
nikapov	0:a1a69d32f310	2823
nikapov	0:a1a69d32f310	2824	/* Check sigmaEstimateP2
nikapov	0:a1a69d32f310	2825	* If this value is too high there is not enough signal rate
nikapov	0:a1a69d32f310	2826	* to calculate dmax value so set a suitable value to ensure
nikapov	0:a1a69d32f310	2827	* a very small dmax.
nikapov	0:a1a69d32f310	2828	*/
nikapov	0:a1a69d32f310	2829	sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2830	sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
nikapov	0:a1a69d32f310	2831	c_amb_eff_width_sigma_est_ns;
nikapov	0:a1a69d32f310	2832	sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
nikapov	0:a1a69d32f310	2833
nikapov	0:a1a69d32f310	2834	if (sigma_est_p2_tmp > 0xffff) {
nikapov	0:a1a69d32f310	2835	min_signal_needed_p3 = 0xfff00000;
nikapov	0:a1a69d32f310	2836	} else {
nikapov	0:a1a69d32f310	2837
nikapov	0:a1a69d32f310	2838	/* DMAX uses a different ambient width from sigma, so apply
nikapov	0:a1a69d32f310	2839	* correction.
nikapov	0:a1a69d32f310	2840	* Perform division before multiplication to prevent overflow.
nikapov	0:a1a69d32f310	2841	*/
nikapov	0:a1a69d32f310	2842	sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
nikapov	0:a1a69d32f310	2843	c_amb_eff_width_sigma_est_ns;
nikapov	0:a1a69d32f310	2844	sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
nikapov	0:a1a69d32f310	2845
nikapov	0:a1a69d32f310	2846	/* FixPoint1616 >> 16 = uint32 */
nikapov	0:a1a69d32f310	2847	min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	2848
nikapov	0:a1a69d32f310	2849	min_signal_needed_p3 *= min_signal_needed_p3;
nikapov	0:a1a69d32f310	2850
nikapov	0:a1a69d32f310	2851	}
nikapov	0:a1a69d32f310	2852
nikapov	0:a1a69d32f310	2853	/* FixPoint1814 / uint32 = FixPoint1814 */
nikapov	0:a1a69d32f310	2854	sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
nikapov	0:a1a69d32f310	2855
nikapov	0:a1a69d32f310	2856	/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
nikapov	0:a1a69d32f310	2857	sigma_limit_tmp *= sigma_limit_tmp;
nikapov	0:a1a69d32f310	2858
nikapov	0:a1a69d32f310	2859	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	2860	sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
nikapov	0:a1a69d32f310	2861
nikapov	0:a1a69d32f310	2862	/* FixPoint3232 >> 4 = FixPoint0428 */
nikapov	0:a1a69d32f310	2863	sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
nikapov	0:a1a69d32f310	2864
nikapov	0:a1a69d32f310	2865	/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
nikapov	0:a1a69d32f310	2866	sigma_limit_tmp -= sigma_est_sq_tmp;
nikapov	0:a1a69d32f310	2867
nikapov	0:a1a69d32f310	2868	/* uint32_t * FixPoint0428 = FixPoint0428 */
nikapov	0:a1a69d32f310	2869	min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
nikapov	0:a1a69d32f310	2870
nikapov	0:a1a69d32f310	2871	/* FixPoint0428 >> 14 = FixPoint1814 */
nikapov	0:a1a69d32f310	2872	min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14;
nikapov	0:a1a69d32f310	2873
nikapov	0:a1a69d32f310	2874	/* uint32 + uint32 = uint32 */
nikapov	0:a1a69d32f310	2875	min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3);
nikapov	0:a1a69d32f310	2876
nikapov	0:a1a69d32f310	2877	/* uint32 / uint32 = uint32 */
nikapov	0:a1a69d32f310	2878	min_signal_needed += (peak_vcsel_duration_us / 2);
nikapov	0:a1a69d32f310	2879	min_signal_needed /= peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	2880
nikapov	0:a1a69d32f310	2881	/* uint32 << 14 = FixPoint1814 */
nikapov	0:a1a69d32f310	2882	min_signal_needed <<= 14;
nikapov	0:a1a69d32f310	2883
nikapov	0:a1a69d32f310	2884	/* FixPoint1814 / FixPoint1814 = uint32 */
nikapov	0:a1a69d32f310	2885	min_signal_needed += (min_signal_needed_p4 / 2);
nikapov	0:a1a69d32f310	2886	min_signal_needed /= min_signal_needed_p4;
nikapov	0:a1a69d32f310	2887
nikapov	0:a1a69d32f310	2888	/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
nikapov	0:a1a69d32f310	2889	min_signal_needed *= min_signal_needed_p1;
nikapov	0:a1a69d32f310	2890
nikapov	0:a1a69d32f310	2891	/* Apply correction by dividing by 1000000.
nikapov	0:a1a69d32f310	2892	* This assumes 10E16 on the numerator of the equation
nikapov	0:a1a69d32f310	2893	* and 10E-22 on the denominator.
nikapov	0:a1a69d32f310	2894	* We do this because 32bit fix point calculation can't
nikapov	0:a1a69d32f310	2895	* handle the larger and smaller elements of this equation,
nikapov	0:a1a69d32f310	2896	* i.e. speed of light and pulse widths.
nikapov	0:a1a69d32f310	2897	*/
nikapov	0:a1a69d32f310	2898	min_signal_needed = (min_signal_needed + 500) / 1000;
nikapov	0:a1a69d32f310	2899	min_signal_needed <<= 4;
nikapov	0:a1a69d32f310	2900
nikapov	0:a1a69d32f310	2901	min_signal_needed = (min_signal_needed + 500) / 1000;
nikapov	0:a1a69d32f310	2902
nikapov	0:a1a69d32f310	2903	/* FixPoint1616 >> 8 = FixPoint2408 */
nikapov	0:a1a69d32f310	2904	signal_limit_tmp = (c_signal_limit + 0x80) >> 8;
nikapov	0:a1a69d32f310	2905
nikapov	0:a1a69d32f310	2906	/* FixPoint2408/FixPoint2408 = uint32 */
nikapov	0:a1a69d32f310	2907	if (signal_limit_tmp != 0) {
nikapov	0:a1a69d32f310	2908	dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
nikapov	0:a1a69d32f310	2909	/ signal_limit_tmp;
nikapov	0:a1a69d32f310	2910	} else {
nikapov	0:a1a69d32f310	2911	dmax_dark_tmp = 0;
nikapov	0:a1a69d32f310	2912	}
nikapov	0:a1a69d32f310	2913
nikapov	0:a1a69d32f310	2914	dmax_dark = VL53L0X_isqrt(dmax_dark_tmp);
nikapov	0:a1a69d32f310	2915
nikapov	0:a1a69d32f310	2916	/* FixPoint2408/FixPoint2408 = uint32 */
nikapov	0:a1a69d32f310	2917	if (min_signal_needed != 0) {
nikapov	0:a1a69d32f310	2918	dmax_ambient = (signal_at0_mm + min_signal_needed / 2)
nikapov	0:a1a69d32f310	2919	/ min_signal_needed;
nikapov	0:a1a69d32f310	2920	} else {
nikapov	0:a1a69d32f310	2921	dmax_ambient = 0;
nikapov	0:a1a69d32f310	2922	}
nikapov	0:a1a69d32f310	2923
nikapov	0:a1a69d32f310	2924	dmax_ambient = VL53L0X_isqrt(dmax_ambient);
nikapov	0:a1a69d32f310	2925
nikapov	0:a1a69d32f310	2926	*pd_max_mm = dmax_dark;
nikapov	0:a1a69d32f310	2927	if (dmax_dark > dmax_ambient) {
nikapov	0:a1a69d32f310	2928	*pd_max_mm = dmax_ambient;
nikapov	0:a1a69d32f310	2929	}
nikapov	0:a1a69d32f310	2930
nikapov	0:a1a69d32f310	2931	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	2932
nikapov	0:a1a69d32f310	2933	return status;
nikapov	0:a1a69d32f310	2934	}
nikapov	0:a1a69d32f310	2935
nikapov	0:a1a69d32f310	2936	VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	2937	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	2938	FixPoint1616_t *p_sigma_estimate,
nikapov	0:a1a69d32f310	2939	uint32_t *p_dmax_mm)
nikapov	0:a1a69d32f310	2940	{
nikapov	0:a1a69d32f310	2941	/* Expressed in 100ths of a ns, i.e. centi-ns */
nikapov	0:a1a69d32f310	2942	const uint32_t c_pulse_effective_width_centi_ns = 800;
nikapov	0:a1a69d32f310	2943	/* Expressed in 100ths of a ns, i.e. centi-ns */
nikapov	0:a1a69d32f310	2944	const uint32_t c_ambient_effective_width_centi_ns = 600;
nikapov	0:a1a69d32f310	2945	const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
nikapov	0:a1a69d32f310	2946	const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
nikapov	0:a1a69d32f310	2947	const FixPoint1616_t c_sigma_est_max = 0x028F87AE;
nikapov	0:a1a69d32f310	2948	const FixPoint1616_t c_sigma_est_rtn_max = 0xF000;
nikapov	0:a1a69d32f310	2949	const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
nikapov	0:a1a69d32f310	2950	c_ambient_effective_width_centi_ns;
nikapov	0:a1a69d32f310	2951	/* Time Of Flight per mm (6.6 pico secs) */
nikapov	0:a1a69d32f310	2952	const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A;
nikapov	0:a1a69d32f310	2953	const uint32_t c_16bit_rounding_param = 0x00008000;
nikapov	0:a1a69d32f310	2954	const FixPoint1616_t c_max_x_talk_kcps = 0x00320000;
nikapov	0:a1a69d32f310	2955	const uint32_t c_pll_period_ps = 1655;
nikapov	0:a1a69d32f310	2956
nikapov	0:a1a69d32f310	2957	uint32_t vcsel_total_events_rtn;
nikapov	0:a1a69d32f310	2958	uint32_t final_range_timeout_micro_secs;
nikapov	0:a1a69d32f310	2959	uint32_t pre_range_timeout_micro_secs;
nikapov	0:a1a69d32f310	2960	uint32_t final_range_integration_time_milli_secs;
nikapov	0:a1a69d32f310	2961	FixPoint1616_t sigma_estimate_p1;
nikapov	0:a1a69d32f310	2962	FixPoint1616_t sigma_estimate_p2;
nikapov	0:a1a69d32f310	2963	FixPoint1616_t sigma_estimate_p3;
nikapov	0:a1a69d32f310	2964	FixPoint1616_t delta_t_ps;
nikapov	0:a1a69d32f310	2965	FixPoint1616_t pw_mult;
nikapov	0:a1a69d32f310	2966	FixPoint1616_t sigma_est_rtn;
nikapov	0:a1a69d32f310	2967	FixPoint1616_t sigma_estimate;
nikapov	0:a1a69d32f310	2968	FixPoint1616_t x_talk_correction;
nikapov	0:a1a69d32f310	2969	FixPoint1616_t ambient_rate_kcps;
nikapov	0:a1a69d32f310	2970	FixPoint1616_t peak_signal_rate_kcps;
nikapov	0:a1a69d32f310	2971	FixPoint1616_t x_talk_comp_rate_mcps;
nikapov	0:a1a69d32f310	2972	uint32_t x_talk_comp_rate_kcps;
nikapov	0:a1a69d32f310	2973	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	2974	FixPoint1616_t diff1_mcps;
nikapov	0:a1a69d32f310	2975	FixPoint1616_t diff2_mcps;
nikapov	0:a1a69d32f310	2976	FixPoint1616_t sqr1;
nikapov	0:a1a69d32f310	2977	FixPoint1616_t sqr2;
nikapov	0:a1a69d32f310	2978	FixPoint1616_t sqr_sum;
nikapov	0:a1a69d32f310	2979	FixPoint1616_t sqrt_result_centi_ns;
nikapov	0:a1a69d32f310	2980	FixPoint1616_t sqrt_result;
nikapov	0:a1a69d32f310	2981	FixPoint1616_t total_signal_rate_mcps;
nikapov	0:a1a69d32f310	2982	FixPoint1616_t corrected_signal_rate_mcps;
nikapov	0:a1a69d32f310	2983	FixPoint1616_t sigma_est_ref;
nikapov	0:a1a69d32f310	2984	uint32_t vcsel_width;
nikapov	0:a1a69d32f310	2985	uint32_t final_range_macro_pclks;
nikapov	0:a1a69d32f310	2986	uint32_t pre_range_macro_pclks;
nikapov	0:a1a69d32f310	2987	uint32_t peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	2988	uint8_t final_range_vcsel_pclks;
nikapov	0:a1a69d32f310	2989	uint8_t pre_range_vcsel_pclks;
nikapov	0:a1a69d32f310	2990	/*! \addtogroup calc_sigma_estimate
nikapov	0:a1a69d32f310	2991	* @{
nikapov	0:a1a69d32f310	2992	*
nikapov	0:a1a69d32f310	2993	* Estimates the range sigma
nikapov	0:a1a69d32f310	2994	*/
nikapov	0:a1a69d32f310	2995
nikapov	0:a1a69d32f310	2996	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	2997
nikapov	0:a1a69d32f310	2998	VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps,
nikapov	0:a1a69d32f310	2999	x_talk_comp_rate_mcps);
nikapov	0:a1a69d32f310	3000
nikapov	0:a1a69d32f310	3001	/*
nikapov	0:a1a69d32f310	3002	* We work in kcps rather than mcps as this helps keep within the
nikapov	0:a1a69d32f310	3003	* confines of the 32 Fix1616 type.
nikapov	0:a1a69d32f310	3004	*/
nikapov	0:a1a69d32f310	3005
nikapov	0:a1a69d32f310	3006	ambient_rate_kcps =
nikapov	0:a1a69d32f310	3007	(p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16;
nikapov	0:a1a69d32f310	3008
nikapov	0:a1a69d32f310	3009	corrected_signal_rate_mcps =
nikapov	0:a1a69d32f310	3010	p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	3011
nikapov	0:a1a69d32f310	3012
nikapov	0:a1a69d32f310	3013	status = VL53L0X_get_total_signal_rate(
nikapov	0:a1a69d32f310	3014	dev, p_ranging_measurement_data, &total_signal_rate_mcps);
nikapov	0:a1a69d32f310	3015	status = VL53L0X_get_total_xtalk_rate(
nikapov	0:a1a69d32f310	3016	dev, p_ranging_measurement_data, &x_talk_comp_rate_mcps);
nikapov	0:a1a69d32f310	3017
nikapov	0:a1a69d32f310	3018
nikapov	0:a1a69d32f310	3019	/* Signal rate measurement provided by device is the
nikapov	0:a1a69d32f310	3020	* peak signal rate, not average.
nikapov	0:a1a69d32f310	3021	*/
nikapov	0:a1a69d32f310	3022	peak_signal_rate_kcps = (total_signal_rate_mcps * 1000);
nikapov	0:a1a69d32f310	3023	peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16;
nikapov	0:a1a69d32f310	3024
nikapov	0:a1a69d32f310	3025	x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
nikapov	0:a1a69d32f310	3026
nikapov	0:a1a69d32f310	3027	if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) {
nikapov	0:a1a69d32f310	3028	x_talk_comp_rate_kcps = c_max_x_talk_kcps;
nikapov	0:a1a69d32f310	3029	}
nikapov	0:a1a69d32f310	3030
nikapov	0:a1a69d32f310	3031	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3032
nikapov	0:a1a69d32f310	3033	/* Calculate final range macro periods */
nikapov	0:a1a69d32f310	3034	final_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	3035	dev, FinalRangeTimeoutMicroSecs);
nikapov	0:a1a69d32f310	3036
nikapov	0:a1a69d32f310	3037	final_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	3038	dev, FinalRangeVcselPulsePeriod);
nikapov	0:a1a69d32f310	3039
nikapov	0:a1a69d32f310	3040	final_range_macro_pclks = VL53L0X_calc_timeout_mclks(
nikapov	0:a1a69d32f310	3041	dev, final_range_timeout_micro_secs, final_range_vcsel_pclks);
nikapov	0:a1a69d32f310	3042
nikapov	0:a1a69d32f310	3043	/* Calculate pre-range macro periods */
nikapov	0:a1a69d32f310	3044	pre_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	3045	dev, PreRangeTimeoutMicroSecs);
nikapov	0:a1a69d32f310	3046
nikapov	0:a1a69d32f310	3047	pre_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	3048	dev, PreRangeVcselPulsePeriod);
nikapov	0:a1a69d32f310	3049
nikapov	0:a1a69d32f310	3050	pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(
nikapov	0:a1a69d32f310	3051	dev, pre_range_timeout_micro_secs, pre_range_vcsel_pclks);
nikapov	0:a1a69d32f310	3052
nikapov	0:a1a69d32f310	3053	vcsel_width = 3;
nikapov	0:a1a69d32f310	3054	if (final_range_vcsel_pclks == 8) {
nikapov	0:a1a69d32f310	3055	vcsel_width = 2;
nikapov	0:a1a69d32f310	3056	}
nikapov	0:a1a69d32f310	3057
nikapov	0:a1a69d32f310	3058
nikapov	0:a1a69d32f310	3059	peak_vcsel_duration_us = vcsel_width * 2048 *
nikapov	0:a1a69d32f310	3060	(pre_range_macro_pclks + final_range_macro_pclks);
nikapov	0:a1a69d32f310	3061	peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov	0:a1a69d32f310	3062	peak_vcsel_duration_us *= c_pll_period_ps;
nikapov	0:a1a69d32f310	3063	peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov	0:a1a69d32f310	3064
nikapov	0:a1a69d32f310	3065	/* Fix1616 >> 8 = Fix2408 */
nikapov	0:a1a69d32f310	3066	total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8;
nikapov	0:a1a69d32f310	3067
nikapov	0:a1a69d32f310	3068	/* Fix2408 * uint32 = Fix2408 */
nikapov	0:a1a69d32f310	3069	vcsel_total_events_rtn = total_signal_rate_mcps *
nikapov	0:a1a69d32f310	3070	peak_vcsel_duration_us;
nikapov	0:a1a69d32f310	3071
nikapov	0:a1a69d32f310	3072	/* Fix2408 >> 8 = uint32 */
nikapov	0:a1a69d32f310	3073	vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
nikapov	0:a1a69d32f310	3074
nikapov	0:a1a69d32f310	3075	/* Fix2408 << 8 = Fix1616 = */
nikapov	0:a1a69d32f310	3076	total_signal_rate_mcps <<= 8;
nikapov	0:a1a69d32f310	3077	}
nikapov	0:a1a69d32f310	3078
nikapov	0:a1a69d32f310	3079	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3080	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	3081	return status;
nikapov	0:a1a69d32f310	3082	}
nikapov	0:a1a69d32f310	3083
nikapov	0:a1a69d32f310	3084	if (peak_signal_rate_kcps == 0) {
nikapov	0:a1a69d32f310	3085	*p_sigma_estimate = c_sigma_est_max;
nikapov	0:a1a69d32f310	3086	PALDevDataSet(dev, SigmaEstimate, c_sigma_est_max);
nikapov	0:a1a69d32f310	3087	*p_dmax_mm = 0;
nikapov	0:a1a69d32f310	3088	} else {
nikapov	0:a1a69d32f310	3089	if (vcsel_total_events_rtn < 1) {
nikapov	0:a1a69d32f310	3090	vcsel_total_events_rtn = 1;
nikapov	0:a1a69d32f310	3091	}
nikapov	0:a1a69d32f310	3092
nikapov	0:a1a69d32f310	3093	sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
nikapov	0:a1a69d32f310	3094
nikapov	0:a1a69d32f310	3095	/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	3096	sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps;
nikapov	0:a1a69d32f310	3097	if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) {
nikapov	0:a1a69d32f310	3098	/* Clip to prevent overflow. Will ensure safe
nikapov	0:a1a69d32f310	3099	* max result. */
nikapov	0:a1a69d32f310	3100	sigma_estimate_p2 = c_amb_to_signal_ratio_max;
nikapov	0:a1a69d32f310	3101	}
nikapov	0:a1a69d32f310	3102	sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
nikapov	0:a1a69d32f310	3103
nikapov	0:a1a69d32f310	3104	sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12);
nikapov	0:a1a69d32f310	3105
nikapov	0:a1a69d32f310	3106	/* uint32 * FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	3107	delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
nikapov	0:a1a69d32f310	3108	c_tof_per_mm_ps;
nikapov	0:a1a69d32f310	3109
nikapov	0:a1a69d32f310	3110	/*
nikapov	0:a1a69d32f310	3111	* vcselRate - xtalkCompRate
nikapov	0:a1a69d32f310	3112	* (uint32 << 16) - FixPoint1616 = FixPoint1616.
nikapov	0:a1a69d32f310	3113	* Divide result by 1000 to convert to mcps.
nikapov	0:a1a69d32f310	3114	* 500 is added to ensure rounding when integer division
nikapov	0:a1a69d32f310	3115	* truncates.
nikapov	0:a1a69d32f310	3116	*/
nikapov	0:a1a69d32f310	3117	diff1_mcps = (((peak_signal_rate_kcps << 16) -
nikapov	0:a1a69d32f310	3118	2 * x_talk_comp_rate_kcps) + 500) / 1000;
nikapov	0:a1a69d32f310	3119
nikapov	0:a1a69d32f310	3120	/* vcselRate + xtalkCompRate */
nikapov	0:a1a69d32f310	3121	diff2_mcps = ((peak_signal_rate_kcps << 16) + 500) / 1000;
nikapov	0:a1a69d32f310	3122
nikapov	0:a1a69d32f310	3123	/* Shift by 8 bits to increase resolution prior to the
nikapov	0:a1a69d32f310	3124	* division */
nikapov	0:a1a69d32f310	3125	diff1_mcps <<= 8;
nikapov	0:a1a69d32f310	3126
nikapov	0:a1a69d32f310	3127	/* FixPoint0824/FixPoint1616 = FixPoint2408 */
nikapov	0:a1a69d32f310	3128	// xTalkCorrection = abs(diff1_mcps/diff2_mcps);
nikapov	0:a1a69d32f310	3129	// abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
nikapov	0:a1a69d32f310	3130	x_talk_correction = diff1_mcps / diff2_mcps;
nikapov	0:a1a69d32f310	3131
nikapov	0:a1a69d32f310	3132	/* FixPoint2408 << 8 = FixPoint1616 */
nikapov	0:a1a69d32f310	3133	x_talk_correction <<= 8;
nikapov	0:a1a69d32f310	3134
nikapov	0:a1a69d32f310	3135	if (p_ranging_measurement_data->RangeStatus != 0) {
nikapov	0:a1a69d32f310	3136	pw_mult = 1 << 16;
nikapov	0:a1a69d32f310	3137	} else {
nikapov	0:a1a69d32f310	3138	/* FixPoint1616/uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	3139	pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
nikapov	0:a1a69d32f310	3140
nikapov	0:a1a69d32f310	3141	/*
nikapov	0:a1a69d32f310	3142	* FixPoint1616 * FixPoint1616 = FixPoint3232, however both
nikapov	0:a1a69d32f310	3143	* values are small enough such that32 bits will not be
nikapov	0:a1a69d32f310	3144	* exceeded.
nikapov	0:a1a69d32f310	3145	*/
nikapov	0:a1a69d32f310	3146	pw_mult *= ((1 << 16) - x_talk_correction);
nikapov	0:a1a69d32f310	3147
nikapov	0:a1a69d32f310	3148	/* (FixPoint3232 >> 16) = FixPoint1616 */
nikapov	0:a1a69d32f310	3149	pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
nikapov	0:a1a69d32f310	3150
nikapov	0:a1a69d32f310	3151	/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
nikapov	0:a1a69d32f310	3152	pw_mult += (1 << 16);
nikapov	0:a1a69d32f310	3153
nikapov	0:a1a69d32f310	3154	/*
nikapov	0:a1a69d32f310	3155	* At this point the value will be 1.xx, therefore if we square
nikapov	0:a1a69d32f310	3156	* the value this will exceed 32 bits. To address this perform
nikapov	0:a1a69d32f310	3157	* a single shift to the right before the multiplication.
nikapov	0:a1a69d32f310	3158	*/
nikapov	0:a1a69d32f310	3159	pw_mult >>= 1;
nikapov	0:a1a69d32f310	3160	/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
nikapov	0:a1a69d32f310	3161	pw_mult = pw_mult * pw_mult;
nikapov	0:a1a69d32f310	3162
nikapov	0:a1a69d32f310	3163	/* (FixPoint3430 >> 14) = Fix1616 */
nikapov	0:a1a69d32f310	3164	pw_mult >>= 14;
nikapov	0:a1a69d32f310	3165	}
nikapov	0:a1a69d32f310	3166
nikapov	0:a1a69d32f310	3167	/* FixPoint1616 * uint32 = FixPoint1616 */
nikapov	0:a1a69d32f310	3168	sqr1 = pw_mult * sigma_estimate_p1;
nikapov	0:a1a69d32f310	3169
nikapov	0:a1a69d32f310	3170	/* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov	0:a1a69d32f310	3171	sqr1 = (sqr1 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	3172
nikapov	0:a1a69d32f310	3173	/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov	0:a1a69d32f310	3174	sqr1 *= sqr1;
nikapov	0:a1a69d32f310	3175
nikapov	0:a1a69d32f310	3176	sqr2 = sigma_estimate_p2;
nikapov	0:a1a69d32f310	3177
nikapov	0:a1a69d32f310	3178	/* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov	0:a1a69d32f310	3179	sqr2 = (sqr2 + 0x8000) >> 16;
nikapov	0:a1a69d32f310	3180
nikapov	0:a1a69d32f310	3181	/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov	0:a1a69d32f310	3182	sqr2 *= sqr2;
nikapov	0:a1a69d32f310	3183
nikapov	0:a1a69d32f310	3184	/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
nikapov	0:a1a69d32f310	3185	sqr_sum = sqr1 + sqr2;
nikapov	0:a1a69d32f310	3186
nikapov	0:a1a69d32f310	3187	/* SQRT(FixPoin6400) = FixPoint3200 */
nikapov	0:a1a69d32f310	3188	sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum);
nikapov	0:a1a69d32f310	3189
nikapov	0:a1a69d32f310	3190	/* (FixPoint3200 << 16) = FixPoint1616 */
nikapov	0:a1a69d32f310	3191	sqrt_result_centi_ns <<= 16;
nikapov	0:a1a69d32f310	3192
nikapov	0:a1a69d32f310	3193	/*
nikapov	0:a1a69d32f310	3194	* Note that the Speed Of Light is expressed in um per 1E-10
nikapov	0:a1a69d32f310	3195	* seconds (2997) Therefore to get mm/ns we have to divide by
nikapov	0:a1a69d32f310	3196	* 10000
nikapov	0:a1a69d32f310	3197	*/
nikapov	0:a1a69d32f310	3198	sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
nikapov	0:a1a69d32f310	3199	sigma_estimate_p3);
nikapov	0:a1a69d32f310	3200	sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
nikapov	0:a1a69d32f310	3201
nikapov	0:a1a69d32f310	3202	/* Add 5000 before dividing by 10000 to ensure rounding. */
nikapov	0:a1a69d32f310	3203	sigma_est_rtn += 5000;
nikapov	0:a1a69d32f310	3204	sigma_est_rtn /= 10000;
nikapov	0:a1a69d32f310	3205
nikapov	0:a1a69d32f310	3206	if (sigma_est_rtn > c_sigma_est_rtn_max) {
nikapov	0:a1a69d32f310	3207	/* Clip to prevent overflow. Will ensure safe
nikapov	0:a1a69d32f310	3208	* max result. */
nikapov	0:a1a69d32f310	3209	sigma_est_rtn = c_sigma_est_rtn_max;
nikapov	0:a1a69d32f310	3210	}
nikapov	0:a1a69d32f310	3211	final_range_integration_time_milli_secs =
nikapov	0:a1a69d32f310	3212	(final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
nikapov	0:a1a69d32f310	3213
nikapov	0:a1a69d32f310	3214	/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
nikapov	0:a1a69d32f310	3215	* sqrt(FixPoint1616/int) = FixPoint2408)
nikapov	0:a1a69d32f310	3216	*/
nikapov	0:a1a69d32f310	3217	sigma_est_ref =
nikapov	0:a1a69d32f310	3218	VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs +
nikapov	0:a1a69d32f310	3219	final_range_integration_time_milli_secs / 2) /
nikapov	0:a1a69d32f310	3220	final_range_integration_time_milli_secs);
nikapov	0:a1a69d32f310	3221
nikapov	0:a1a69d32f310	3222	/* FixPoint2408 << 8 = FixPoint1616 */
nikapov	0:a1a69d32f310	3223	sigma_est_ref <<= 8;
nikapov	0:a1a69d32f310	3224	sigma_est_ref = (sigma_est_ref + 500) / 1000;
nikapov	0:a1a69d32f310	3225
nikapov	0:a1a69d32f310	3226	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	3227	sqr1 = sigma_est_rtn * sigma_est_rtn;
nikapov	0:a1a69d32f310	3228	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov	0:a1a69d32f310	3229	sqr2 = sigma_est_ref * sigma_est_ref;
nikapov	0:a1a69d32f310	3230
nikapov	0:a1a69d32f310	3231	/* sqrt(FixPoint3232) = FixPoint1616 */
nikapov	0:a1a69d32f310	3232	sqrt_result = VL53L0X_isqrt((sqr1 + sqr2));
nikapov	0:a1a69d32f310	3233	/*
nikapov	0:a1a69d32f310	3234	* Note that the Shift by 4 bits increases resolution prior to
nikapov	0:a1a69d32f310	3235	* the sqrt, therefore the result must be shifted by 2 bits to
nikapov	0:a1a69d32f310	3236	* the right to revert back to the FixPoint1616 format.
nikapov	0:a1a69d32f310	3237	*/
nikapov	0:a1a69d32f310	3238
nikapov	0:a1a69d32f310	3239	sigma_estimate = 1000 * sqrt_result;
nikapov	0:a1a69d32f310	3240
nikapov	0:a1a69d32f310	3241	if ((peak_signal_rate_kcps < 1) \|\| (vcsel_total_events_rtn < 1) \|\|
nikapov	0:a1a69d32f310	3242	(sigma_estimate > c_sigma_est_max)) {
nikapov	0:a1a69d32f310	3243	sigma_estimate = c_sigma_est_max;
nikapov	0:a1a69d32f310	3244	}
nikapov	0:a1a69d32f310	3245
nikapov	0:a1a69d32f310	3246	*p_sigma_estimate = (uint32_t)(sigma_estimate);
nikapov	0:a1a69d32f310	3247	PALDevDataSet(dev, SigmaEstimate, *p_sigma_estimate);
nikapov	0:a1a69d32f310	3248	status = VL53L0X_calc_dmax(
nikapov	0:a1a69d32f310	3249	dev,
nikapov	0:a1a69d32f310	3250	total_signal_rate_mcps,
nikapov	0:a1a69d32f310	3251	corrected_signal_rate_mcps,
nikapov	0:a1a69d32f310	3252	pw_mult,
nikapov	0:a1a69d32f310	3253	sigma_estimate_p1,
nikapov	0:a1a69d32f310	3254	sigma_estimate_p2,
nikapov	0:a1a69d32f310	3255	peak_vcsel_duration_us,
nikapov	0:a1a69d32f310	3256	p_dmax_mm);
nikapov	0:a1a69d32f310	3257	}
nikapov	0:a1a69d32f310	3258
nikapov	0:a1a69d32f310	3259	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	3260	return status;
nikapov	0:a1a69d32f310	3261	}
nikapov	0:a1a69d32f310	3262
nikapov	0:a1a69d32f310	3263	VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	3264	uint8_t device_range_status,
nikapov	0:a1a69d32f310	3265	FixPoint1616_t signal_rate,
nikapov	0:a1a69d32f310	3266	uint16_t effective_spad_rtn_count,
nikapov	0:a1a69d32f310	3267	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
nikapov	0:a1a69d32f310	3268	uint8_t *p_pal_range_status)
nikapov	0:a1a69d32f310	3269	{
nikapov	0:a1a69d32f310	3270	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3271	uint8_t none_flag;
nikapov	0:a1a69d32f310	3272	uint8_t sigma_limitflag = 0;
nikapov	0:a1a69d32f310	3273	uint8_t signal_ref_clipflag = 0;
nikapov	0:a1a69d32f310	3274	uint8_t range_ignore_thresholdflag = 0;
nikapov	0:a1a69d32f310	3275	uint8_t sigma_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3276	uint8_t signal_rate_final_range_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3277	uint8_t signal_ref_clip_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3278	uint8_t range_ignore_threshold_limit_check_enable = 0;
nikapov	0:a1a69d32f310	3279	FixPoint1616_t sigma_estimate;
nikapov	0:a1a69d32f310	3280	FixPoint1616_t sigma_limit_value;
nikapov	0:a1a69d32f310	3281	FixPoint1616_t signal_ref_clip_value;
nikapov	0:a1a69d32f310	3282	FixPoint1616_t range_ignore_threshold_value;
nikapov	0:a1a69d32f310	3283	FixPoint1616_t signal_rate_per_spad;
nikapov	0:a1a69d32f310	3284	uint8_t device_range_status_internal = 0;
nikapov	0:a1a69d32f310	3285	uint16_t tmp_word = 0;
nikapov	0:a1a69d32f310	3286	uint8_t temp8;
nikapov	0:a1a69d32f310	3287	uint32_t dmax_mm = 0;
nikapov	0:a1a69d32f310	3288	FixPoint1616_t last_signal_ref_mcps;
nikapov	0:a1a69d32f310	3289
nikapov	0:a1a69d32f310	3290	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	3291
nikapov	0:a1a69d32f310	3292
nikapov	0:a1a69d32f310	3293	/*
nikapov	0:a1a69d32f310	3294	* VL53L0X has a good ranging when the value of the
nikapov	0:a1a69d32f310	3295	* DeviceRangeStatus = 11. This function will replace the value 0 with
nikapov	0:a1a69d32f310	3296	* the value 11 in the DeviceRangeStatus.
nikapov	0:a1a69d32f310	3297	* In addition, the SigmaEstimator is not included in the VL53L0X
nikapov	0:a1a69d32f310	3298	* DeviceRangeStatus, this will be added in the PalRangeStatus.
nikapov	0:a1a69d32f310	3299	*/
nikapov	0:a1a69d32f310	3300
nikapov	0:a1a69d32f310	3301	device_range_status_internal = ((device_range_status & 0x78) >> 3);
nikapov	0:a1a69d32f310	3302
nikapov	0:a1a69d32f310	3303	if (device_range_status_internal == 0 \|\|
nikapov	0:a1a69d32f310	3304	device_range_status_internal == 5 \|\|
nikapov	0:a1a69d32f310	3305	device_range_status_internal == 7 \|\|
nikapov	0:a1a69d32f310	3306	device_range_status_internal == 12 \|\|
nikapov	0:a1a69d32f310	3307	device_range_status_internal == 13 \|\|
nikapov	0:a1a69d32f310	3308	device_range_status_internal == 14 \|\|
nikapov	0:a1a69d32f310	3309	device_range_status_internal == 15
nikapov	0:a1a69d32f310	3310	) {
nikapov	0:a1a69d32f310	3311	none_flag = 1;
nikapov	0:a1a69d32f310	3312	} else {
nikapov	0:a1a69d32f310	3313	none_flag = 0;
nikapov	0:a1a69d32f310	3314	}
nikapov	0:a1a69d32f310	3315
nikapov	0:a1a69d32f310	3316	/*
nikapov	0:a1a69d32f310	3317	* Check if Sigma limit is enabled, if yes then do comparison with limit
nikapov	0:a1a69d32f310	3318	* value and put the result back into pPalRangeStatus.
nikapov	0:a1a69d32f310	3319	*/
nikapov	0:a1a69d32f310	3320	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3321	status = VL53L0X_get_limit_check_enable(dev,
nikapov	0:a1a69d32f310	3322	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	3323	&sigma_limit_check_enable);
Davidroid	2:d07edeaff6f1	3324	}
nikapov	0:a1a69d32f310	3325
nikapov	0:a1a69d32f310	3326	if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	3327	/*
nikapov	0:a1a69d32f310	3328	* compute the Sigma and check with limit
nikapov	0:a1a69d32f310	3329	*/
nikapov	0:a1a69d32f310	3330	status = VL53L0X_calc_sigma_estimate(
nikapov	0:a1a69d32f310	3331	dev,
nikapov	0:a1a69d32f310	3332	p_ranging_measurement_data,
nikapov	0:a1a69d32f310	3333	&sigma_estimate,
nikapov	0:a1a69d32f310	3334	&dmax_mm);
nikapov	0:a1a69d32f310	3335	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3336	p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
nikapov	0:a1a69d32f310	3337	}
nikapov	0:a1a69d32f310	3338
nikapov	0:a1a69d32f310	3339	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3340	status = VL53L0X_get_limit_check_value(dev,
nikapov	0:a1a69d32f310	3341	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	3342	&sigma_limit_value);
nikapov	0:a1a69d32f310	3343
nikapov	0:a1a69d32f310	3344	if ((sigma_limit_value > 0) &&
nikapov	0:a1a69d32f310	3345	(sigma_estimate > sigma_limit_value)) {
nikapov	0:a1a69d32f310	3346	/* Limit Fail */
nikapov	0:a1a69d32f310	3347	sigma_limitflag = 1;
nikapov	0:a1a69d32f310	3348	}
nikapov	0:a1a69d32f310	3349	}
nikapov	0:a1a69d32f310	3350	}
nikapov	0:a1a69d32f310	3351
nikapov	0:a1a69d32f310	3352	/*
nikapov	0:a1a69d32f310	3353	* Check if Signal ref clip limit is enabled, if yes then do comparison
nikapov	0:a1a69d32f310	3354	* with limit value and put the result back into pPalRangeStatus.
nikapov	0:a1a69d32f310	3355	*/
nikapov	0:a1a69d32f310	3356	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3357	status = VL53L0X_get_limit_check_enable(dev,
nikapov	0:a1a69d32f310	3358	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	3359	&signal_ref_clip_limit_check_enable);
nikapov	0:a1a69d32f310	3360	}
nikapov	0:a1a69d32f310	3361
nikapov	0:a1a69d32f310	3362	if ((signal_ref_clip_limit_check_enable != 0) &&
nikapov	0:a1a69d32f310	3363	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	3364
nikapov	0:a1a69d32f310	3365	status = VL53L0X_get_limit_check_value(dev,
nikapov	0:a1a69d32f310	3366	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	3367	&signal_ref_clip_value);
nikapov	0:a1a69d32f310	3368
nikapov	0:a1a69d32f310	3369	/* Read LastSignalRefMcps from device */
nikapov	0:a1a69d32f310	3370	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3371	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	3372	}
nikapov	0:a1a69d32f310	3373
nikapov	0:a1a69d32f310	3374	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3375	status = VL53L0X_read_word(dev,
nikapov	0:a1a69d32f310	3376	VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
nikapov	0:a1a69d32f310	3377	&tmp_word);
nikapov	0:a1a69d32f310	3378	}
nikapov	0:a1a69d32f310	3379
nikapov	0:a1a69d32f310	3380	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3381	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	3382	}
nikapov	0:a1a69d32f310	3383
nikapov	0:a1a69d32f310	3384	last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word);
nikapov	0:a1a69d32f310	3385	PALDevDataSet(dev, LastSignalRefMcps, last_signal_ref_mcps);
nikapov	0:a1a69d32f310	3386
nikapov	0:a1a69d32f310	3387	if ((signal_ref_clip_value > 0) &&
nikapov	0:a1a69d32f310	3388	(last_signal_ref_mcps > signal_ref_clip_value)) {
nikapov	0:a1a69d32f310	3389	/* Limit Fail */
nikapov	0:a1a69d32f310	3390	signal_ref_clipflag = 1;
nikapov	0:a1a69d32f310	3391	}
nikapov	0:a1a69d32f310	3392	}
nikapov	0:a1a69d32f310	3393
nikapov	0:a1a69d32f310	3394	/*
nikapov	0:a1a69d32f310	3395	* Check if Signal ref clip limit is enabled, if yes then do comparison
nikapov	0:a1a69d32f310	3396	* with limit value and put the result back into pPalRangeStatus.
nikapov	0:a1a69d32f310	3397	* EffectiveSpadRtnCount has a format 8.8
nikapov	0:a1a69d32f310	3398	* If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
nikapov	0:a1a69d32f310	3399	*/
Davidroid	2:d07edeaff6f1	3400	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3401	status = VL53L0X_get_limit_check_enable(dev,
nikapov	0:a1a69d32f310	3402	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	3403	&range_ignore_threshold_limit_check_enable);
Davidroid	2:d07edeaff6f1	3404	}
nikapov	0:a1a69d32f310	3405
nikapov	0:a1a69d32f310	3406	if ((range_ignore_threshold_limit_check_enable != 0) &&
nikapov	0:a1a69d32f310	3407	(status == VL53L0X_ERROR_NONE)) {
nikapov	0:a1a69d32f310	3408
nikapov	0:a1a69d32f310	3409	/* Compute the signal rate per spad */
nikapov	0:a1a69d32f310	3410	if (effective_spad_rtn_count == 0) {
nikapov	0:a1a69d32f310	3411	signal_rate_per_spad = 0;
nikapov	0:a1a69d32f310	3412	} else {
nikapov	0:a1a69d32f310	3413	signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate)
nikapov	0:a1a69d32f310	3414	/ effective_spad_rtn_count);
nikapov	0:a1a69d32f310	3415	}
nikapov	0:a1a69d32f310	3416
nikapov	0:a1a69d32f310	3417	status = VL53L0X_get_limit_check_value(dev,
nikapov	0:a1a69d32f310	3418	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	3419	&range_ignore_threshold_value);
nikapov	0:a1a69d32f310	3420
nikapov	0:a1a69d32f310	3421	if ((range_ignore_threshold_value > 0) &&
nikapov	0:a1a69d32f310	3422	(signal_rate_per_spad < range_ignore_threshold_value)) {
nikapov	0:a1a69d32f310	3423	/* Limit Fail add 2^6 to range status */
nikapov	0:a1a69d32f310	3424	range_ignore_thresholdflag = 1;
nikapov	0:a1a69d32f310	3425	}
nikapov	0:a1a69d32f310	3426	}
nikapov	0:a1a69d32f310	3427
nikapov	0:a1a69d32f310	3428	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3429	if (none_flag == 1) {
nikapov	0:a1a69d32f310	3430	p_pal_range_status = 255; / NONE */
nikapov	0:a1a69d32f310	3431	} else if (device_range_status_internal == 1 \|\|
nikapov	0:a1a69d32f310	3432	device_range_status_internal == 2 \|\|
nikapov	0:a1a69d32f310	3433	device_range_status_internal == 3) {
nikapov	0:a1a69d32f310	3434	p_pal_range_status = 5; / HW fail */
nikapov	0:a1a69d32f310	3435	} else if (device_range_status_internal == 6 \|\|
nikapov	0:a1a69d32f310	3436	device_range_status_internal == 9) {
nikapov	0:a1a69d32f310	3437	p_pal_range_status = 4; / Phase fail */
nikapov	0:a1a69d32f310	3438	} else if (device_range_status_internal == 8 \|\|
nikapov	0:a1a69d32f310	3439	device_range_status_internal == 10 \|\|
nikapov	0:a1a69d32f310	3440	signal_ref_clipflag == 1) {
nikapov	0:a1a69d32f310	3441	p_pal_range_status = 3; / Min range */
nikapov	0:a1a69d32f310	3442	} else if (device_range_status_internal == 4 \|\|
nikapov	0:a1a69d32f310	3443	range_ignore_thresholdflag == 1) {
nikapov	0:a1a69d32f310	3444	p_pal_range_status = 2; / Signal Fail */
nikapov	0:a1a69d32f310	3445	} else if (sigma_limitflag == 1) {
nikapov	0:a1a69d32f310	3446	p_pal_range_status = 1; / Sigma Fail */
nikapov	0:a1a69d32f310	3447	} else {
nikapov	0:a1a69d32f310	3448	p_pal_range_status = 0; / Range Valid */
nikapov	0:a1a69d32f310	3449	}
nikapov	0:a1a69d32f310	3450	}
nikapov	0:a1a69d32f310	3451
nikapov	0:a1a69d32f310	3452	/* DMAX only relevant during range error */
Davidroid	3:e9269ff624ed	3453	if (*p_pal_range_status == 0) {
nikapov	0:a1a69d32f310	3454	p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
Davidroid	3:e9269ff624ed	3455	}
nikapov	0:a1a69d32f310	3456
nikapov	0:a1a69d32f310	3457	/* fill the Limit Check Status */
nikapov	0:a1a69d32f310	3458
nikapov	0:a1a69d32f310	3459	status = VL53L0X_get_limit_check_enable(dev,
nikapov	0:a1a69d32f310	3460	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov	0:a1a69d32f310	3461	&signal_rate_final_range_limit_check_enable);
nikapov	0:a1a69d32f310	3462
nikapov	0:a1a69d32f310	3463	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3464	if ((sigma_limit_check_enable == 0) \|\| (sigma_limitflag == 1)) {
nikapov	0:a1a69d32f310	3465	temp8 = 1;
nikapov	0:a1a69d32f310	3466	} else {
nikapov	0:a1a69d32f310	3467	temp8 = 0;
nikapov	0:a1a69d32f310	3468	}
nikapov	0:a1a69d32f310	3469	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov	0:a1a69d32f310	3470	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp8);
nikapov	0:a1a69d32f310	3471
nikapov	0:a1a69d32f310	3472	if ((device_range_status_internal == 4) \|\|
nikapov	0:a1a69d32f310	3473	(signal_rate_final_range_limit_check_enable == 0)) {
nikapov	0:a1a69d32f310	3474	temp8 = 1;
nikapov	0:a1a69d32f310	3475	} else {
nikapov	0:a1a69d32f310	3476	temp8 = 0;
nikapov	0:a1a69d32f310	3477	}
nikapov	0:a1a69d32f310	3478	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov	0:a1a69d32f310	3479	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
nikapov	0:a1a69d32f310	3480	temp8);
nikapov	0:a1a69d32f310	3481
nikapov	0:a1a69d32f310	3482	if ((signal_ref_clip_limit_check_enable == 0) \|\|
nikapov	0:a1a69d32f310	3483	(signal_ref_clipflag == 1)) {
nikapov	0:a1a69d32f310	3484	temp8 = 1;
nikapov	0:a1a69d32f310	3485	} else {
nikapov	0:a1a69d32f310	3486	temp8 = 0;
nikapov	0:a1a69d32f310	3487	}
nikapov	0:a1a69d32f310	3488
nikapov	0:a1a69d32f310	3489	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov	0:a1a69d32f310	3490	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp8);
nikapov	0:a1a69d32f310	3491
nikapov	0:a1a69d32f310	3492	if ((range_ignore_threshold_limit_check_enable == 0) \|\|
nikapov	0:a1a69d32f310	3493	(range_ignore_thresholdflag == 1)) {
nikapov	0:a1a69d32f310	3494	temp8 = 1;
nikapov	0:a1a69d32f310	3495	} else {
nikapov	0:a1a69d32f310	3496	temp8 = 0;
nikapov	0:a1a69d32f310	3497	}
nikapov	0:a1a69d32f310	3498
nikapov	0:a1a69d32f310	3499	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
nikapov	0:a1a69d32f310	3500	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	3501	temp8);
nikapov	0:a1a69d32f310	3502	}
nikapov	0:a1a69d32f310	3503
nikapov	0:a1a69d32f310	3504	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	3505	return status;
nikapov	0:a1a69d32f310	3506
nikapov	0:a1a69d32f310	3507	}
nikapov	0:a1a69d32f310	3508
nikapov	0:a1a69d32f310	3509	VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	3510	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
nikapov	0:a1a69d32f310	3511	{
nikapov	0:a1a69d32f310	3512	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3513	uint8_t device_range_status;
nikapov	0:a1a69d32f310	3514	uint8_t range_fractional_enable;
nikapov	0:a1a69d32f310	3515	uint8_t pal_range_status;
nikapov	0:a1a69d32f310	3516	uint8_t x_talk_compensation_enable;
nikapov	0:a1a69d32f310	3517	uint16_t ambient_rate;
nikapov	0:a1a69d32f310	3518	FixPoint1616_t signal_rate;
nikapov	0:a1a69d32f310	3519	uint16_t x_talk_compensation_rate_mega_cps;
nikapov	0:a1a69d32f310	3520	uint16_t effective_spad_rtn_count;
nikapov	0:a1a69d32f310	3521	uint16_t tmpuint16;
nikapov	0:a1a69d32f310	3522	uint16_t xtalk_range_milli_meter;
nikapov	0:a1a69d32f310	3523	uint16_t linearity_corrective_gain;
nikapov	0:a1a69d32f310	3524	uint8_t localBuffer[12];
nikapov	0:a1a69d32f310	3525	VL53L0X_RangingMeasurementData_t last_range_data_buffer;
nikapov	0:a1a69d32f310	3526
nikapov	0:a1a69d32f310	3527	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	3528
nikapov	0:a1a69d32f310	3529	/*
nikapov	0:a1a69d32f310	3530	* use multi read even if some registers are not useful, result will
nikapov	0:a1a69d32f310	3531	* be more efficient
nikapov	0:a1a69d32f310	3532	* start reading at 0x14 dec20
nikapov	0:a1a69d32f310	3533	* end reading at 0x21 dec33 total 14 bytes to read
nikapov	0:a1a69d32f310	3534	*/
nikapov	0:a1a69d32f310	3535	status = VL53L0X_read_multi(dev, 0x14, localBuffer, 12);
nikapov	0:a1a69d32f310	3536
nikapov	0:a1a69d32f310	3537	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3538
nikapov	0:a1a69d32f310	3539	p_ranging_measurement_data->ZoneId = 0; /* Only one zone */
nikapov	0:a1a69d32f310	3540	p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */
nikapov	0:a1a69d32f310	3541
nikapov	0:a1a69d32f310	3542	tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]);
nikapov	0:a1a69d32f310	3543	/* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
nikapov	0:a1a69d32f310	3544	*(format 11.2) else no fractional
nikapov	0:a1a69d32f310	3545	*/
nikapov	0:a1a69d32f310	3546
nikapov	0:a1a69d32f310	3547	p_ranging_measurement_data->MeasurementTimeUsec = 0;
nikapov	0:a1a69d32f310	3548
nikapov	0:a1a69d32f310	3549	signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
nikapov	0:a1a69d32f310	3550	VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
nikapov	0:a1a69d32f310	3551	/* peak_signal_count_rate_rtn_mcps */
nikapov	0:a1a69d32f310	3552	p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
nikapov	0:a1a69d32f310	3553
nikapov	0:a1a69d32f310	3554	ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
nikapov	0:a1a69d32f310	3555	p_ranging_measurement_data->AmbientRateRtnMegaCps =
nikapov	0:a1a69d32f310	3556	VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate);
nikapov	0:a1a69d32f310	3557
nikapov	0:a1a69d32f310	3558	effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3],
nikapov	0:a1a69d32f310	3559	localBuffer[2]);
nikapov	0:a1a69d32f310	3560	/* EffectiveSpadRtnCount is 8.8 format */
nikapov	0:a1a69d32f310	3561	p_ranging_measurement_data->EffectiveSpadRtnCount =
nikapov	0:a1a69d32f310	3562	effective_spad_rtn_count;
nikapov	0:a1a69d32f310	3563
nikapov	0:a1a69d32f310	3564	device_range_status = localBuffer[0];
nikapov	0:a1a69d32f310	3565
nikapov	0:a1a69d32f310	3566	/* Get Linearity Corrective Gain */
nikapov	0:a1a69d32f310	3567	linearity_corrective_gain = PALDevDataGet(dev,
nikapov	0:a1a69d32f310	3568	LinearityCorrectiveGain);
nikapov	0:a1a69d32f310	3569
nikapov	0:a1a69d32f310	3570	/* Get ranging configuration */
nikapov	0:a1a69d32f310	3571	range_fractional_enable = PALDevDataGet(dev,
nikapov	0:a1a69d32f310	3572	RangeFractionalEnable);
nikapov	0:a1a69d32f310	3573
nikapov	0:a1a69d32f310	3574	if (linearity_corrective_gain != 1000) {
nikapov	0:a1a69d32f310	3575
nikapov	0:a1a69d32f310	3576	tmpuint16 = (uint16_t)((linearity_corrective_gain
nikapov	0:a1a69d32f310	3577	* tmpuint16 + 500) / 1000);
nikapov	0:a1a69d32f310	3578
nikapov	0:a1a69d32f310	3579	/* Implement Xtalk */
nikapov	0:a1a69d32f310	3580	VL53L0X_GETPARAMETERFIELD(dev,
nikapov	0:a1a69d32f310	3581	XTalkCompensationRateMegaCps,
nikapov	0:a1a69d32f310	3582	x_talk_compensation_rate_mega_cps);
nikapov	0:a1a69d32f310	3583	VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable,
nikapov	0:a1a69d32f310	3584	x_talk_compensation_enable);
nikapov	0:a1a69d32f310	3585
nikapov	0:a1a69d32f310	3586	if (x_talk_compensation_enable) {
nikapov	0:a1a69d32f310	3587
nikapov	0:a1a69d32f310	3588	if ((signal_rate
nikapov	0:a1a69d32f310	3589	- ((x_talk_compensation_rate_mega_cps
nikapov	0:a1a69d32f310	3590	* effective_spad_rtn_count) >> 8))
nikapov	0:a1a69d32f310	3591	<= 0) {
nikapov	0:a1a69d32f310	3592	if (range_fractional_enable) {
nikapov	0:a1a69d32f310	3593	xtalk_range_milli_meter = 8888;
nikapov	0:a1a69d32f310	3594	} else {
nikapov	0:a1a69d32f310	3595	xtalk_range_milli_meter = 8888 << 2;
nikapov	0:a1a69d32f310	3596	}
nikapov	0:a1a69d32f310	3597	} else {
nikapov	0:a1a69d32f310	3598	xtalk_range_milli_meter =
nikapov	0:a1a69d32f310	3599	(tmpuint16 * signal_rate)
nikapov	0:a1a69d32f310	3600	/ (signal_rate
nikapov	0:a1a69d32f310	3601	- ((x_talk_compensation_rate_mega_cps
nikapov	0:a1a69d32f310	3602	* effective_spad_rtn_count)
nikapov	0:a1a69d32f310	3603	>> 8));
nikapov	0:a1a69d32f310	3604	}
nikapov	0:a1a69d32f310	3605
nikapov	0:a1a69d32f310	3606	tmpuint16 = xtalk_range_milli_meter;
nikapov	0:a1a69d32f310	3607	}
nikapov	0:a1a69d32f310	3608
nikapov	0:a1a69d32f310	3609	}
nikapov	0:a1a69d32f310	3610
nikapov	0:a1a69d32f310	3611	if (range_fractional_enable) {
nikapov	0:a1a69d32f310	3612	p_ranging_measurement_data->RangeMilliMeter =
nikapov	0:a1a69d32f310	3613	(uint16_t)((tmpuint16) >> 2);
nikapov	0:a1a69d32f310	3614	p_ranging_measurement_data->RangeFractionalPart =
nikapov	0:a1a69d32f310	3615	(uint8_t)((tmpuint16 & 0x03) << 6);
nikapov	0:a1a69d32f310	3616	} else {
nikapov	0:a1a69d32f310	3617	p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
nikapov	0:a1a69d32f310	3618	p_ranging_measurement_data->RangeFractionalPart = 0;
nikapov	0:a1a69d32f310	3619	}
nikapov	0:a1a69d32f310	3620
nikapov	0:a1a69d32f310	3621	/*
nikapov	0:a1a69d32f310	3622	* For a standard definition of RangeStatus, this should
nikapov	0:a1a69d32f310	3623	* return 0 in case of good result after a ranging
nikapov	0:a1a69d32f310	3624	* The range status depends on the device so call a device
nikapov	0:a1a69d32f310	3625	* specific function to obtain the right Status.
nikapov	0:a1a69d32f310	3626	*/
nikapov	0:a1a69d32f310	3627	status \|= VL53L0X_get_pal_range_status(dev, device_range_status,
nikapov	0:a1a69d32f310	3628	signal_rate, effective_spad_rtn_count,
nikapov	0:a1a69d32f310	3629	p_ranging_measurement_data, &pal_range_status);
nikapov	0:a1a69d32f310	3630
nikapov	0:a1a69d32f310	3631	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3632	p_ranging_measurement_data->RangeStatus = pal_range_status;
nikapov	0:a1a69d32f310	3633	}
nikapov	0:a1a69d32f310	3634
nikapov	0:a1a69d32f310	3635	}
nikapov	0:a1a69d32f310	3636
nikapov	0:a1a69d32f310	3637	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3638	/* Copy last read data into Dev buffer */
nikapov	0:a1a69d32f310	3639	last_range_data_buffer = PALDevDataGet(dev, LastRangeMeasure);
nikapov	0:a1a69d32f310	3640
nikapov	0:a1a69d32f310	3641	last_range_data_buffer.RangeMilliMeter =
nikapov	0:a1a69d32f310	3642	p_ranging_measurement_data->RangeMilliMeter;
nikapov	0:a1a69d32f310	3643	last_range_data_buffer.RangeFractionalPart =
nikapov	0:a1a69d32f310	3644	p_ranging_measurement_data->RangeFractionalPart;
nikapov	0:a1a69d32f310	3645	last_range_data_buffer.RangeDMaxMilliMeter =
nikapov	0:a1a69d32f310	3646	p_ranging_measurement_data->RangeDMaxMilliMeter;
nikapov	0:a1a69d32f310	3647	last_range_data_buffer.MeasurementTimeUsec =
nikapov	0:a1a69d32f310	3648	p_ranging_measurement_data->MeasurementTimeUsec;
nikapov	0:a1a69d32f310	3649	last_range_data_buffer.SignalRateRtnMegaCps =
nikapov	0:a1a69d32f310	3650	p_ranging_measurement_data->SignalRateRtnMegaCps;
nikapov	0:a1a69d32f310	3651	last_range_data_buffer.AmbientRateRtnMegaCps =
nikapov	0:a1a69d32f310	3652	p_ranging_measurement_data->AmbientRateRtnMegaCps;
nikapov	0:a1a69d32f310	3653	last_range_data_buffer.EffectiveSpadRtnCount =
nikapov	0:a1a69d32f310	3654	p_ranging_measurement_data->EffectiveSpadRtnCount;
nikapov	0:a1a69d32f310	3655	last_range_data_buffer.RangeStatus =
nikapov	0:a1a69d32f310	3656	p_ranging_measurement_data->RangeStatus;
nikapov	0:a1a69d32f310	3657
nikapov	0:a1a69d32f310	3658	PALDevDataSet(dev, LastRangeMeasure, last_range_data_buffer);
nikapov	0:a1a69d32f310	3659	}
nikapov	0:a1a69d32f310	3660
nikapov	0:a1a69d32f310	3661	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	3662	return status;
nikapov	0:a1a69d32f310	3663	}
nikapov	0:a1a69d32f310	3664
nikapov	0:a1a69d32f310	3665	VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	3666	VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
nikapov	0:a1a69d32f310	3667	{
nikapov	0:a1a69d32f310	3668	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3669
nikapov	0:a1a69d32f310	3670	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	3671
nikapov	0:a1a69d32f310	3672	/* This function will do a complete single ranging
nikapov	0:a1a69d32f310	3673	* Here we fix the mode! */
nikapov	0:a1a69d32f310	3674	status = VL53L0X_set_device_mode(dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
nikapov	0:a1a69d32f310	3675
nikapov	0:a1a69d32f310	3676	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3677	status = VL53L0X_perform_single_measurement(dev);
nikapov	0:a1a69d32f310	3678	}
nikapov	0:a1a69d32f310	3679
nikapov	0:a1a69d32f310	3680	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3681	status = VL53L0X_get_ranging_measurement_data(dev,
nikapov	0:a1a69d32f310	3682	p_ranging_measurement_data);
nikapov	0:a1a69d32f310	3683	}
nikapov	0:a1a69d32f310	3684
nikapov	0:a1a69d32f310	3685	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3686	status = VL53L0X_clear_interrupt_mask(dev, 0);
nikapov	0:a1a69d32f310	3687	}
nikapov	0:a1a69d32f310	3688
nikapov	0:a1a69d32f310	3689	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	3690	return status;
nikapov	0:a1a69d32f310	3691	}
nikapov	0:a1a69d32f310	3692
nikapov	0:a1a69d32f310	3693	VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	3694	uint16_t *p_ref_signal_rate)
nikapov	0:a1a69d32f310	3695	{
nikapov	0:a1a69d32f310	3696	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3697	VL53L0X_RangingMeasurementData_t ranging_measurement_data;
nikapov	0:a1a69d32f310	3698
nikapov	0:a1a69d32f310	3699	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	3700
nikapov	0:a1a69d32f310	3701	/* store the value of the sequence config,
nikapov	0:a1a69d32f310	3702	* this will be reset before the end of the function
nikapov	0:a1a69d32f310	3703	*/
nikapov	0:a1a69d32f310	3704
nikapov	0:a1a69d32f310	3705	sequence_config = PALDevDataGet(dev, SequenceConfig);
nikapov	0:a1a69d32f310	3706
nikapov	0:a1a69d32f310	3707	/*
nikapov	0:a1a69d32f310	3708	* This function performs a reference signal rate measurement.
nikapov	0:a1a69d32f310	3709	*/
nikapov	0:a1a69d32f310	3710	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3711	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	3712	VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
nikapov	0:a1a69d32f310	3713	}
nikapov	0:a1a69d32f310	3714
nikapov	0:a1a69d32f310	3715	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3716	status = VL53L0X_perform_single_ranging_measurement(dev,
nikapov	0:a1a69d32f310	3717	&ranging_measurement_data);
nikapov	0:a1a69d32f310	3718	}
nikapov	0:a1a69d32f310	3719
nikapov	0:a1a69d32f310	3720	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3721	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	3722	}
nikapov	0:a1a69d32f310	3723
nikapov	0:a1a69d32f310	3724	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3725	status = VL53L0X_read_word(dev,
nikapov	0:a1a69d32f310	3726	VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
nikapov	0:a1a69d32f310	3727	p_ref_signal_rate);
Davidroid	2:d07edeaff6f1	3728	}
nikapov	0:a1a69d32f310	3729
nikapov	0:a1a69d32f310	3730	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3731	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
Davidroid	2:d07edeaff6f1	3732	}
nikapov	0:a1a69d32f310	3733
nikapov	0:a1a69d32f310	3734	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3735	/* restore the previous Sequence Config */
nikapov	0:a1a69d32f310	3736	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	3737	sequence_config);
nikapov	0:a1a69d32f310	3738	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3739	PALDevDataSet(dev, SequenceConfig, sequence_config);
nikapov	0:a1a69d32f310	3740	}
nikapov	0:a1a69d32f310	3741	}
nikapov	0:a1a69d32f310	3742
nikapov	0:a1a69d32f310	3743	return status;
nikapov	0:a1a69d32f310	3744	}
nikapov	0:a1a69d32f310	3745
nikapov	0:a1a69d32f310	3746	VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	3747	uint32_t *ref_spad_count,
nikapov	0:a1a69d32f310	3748	uint8_t *is_aperture_spads)
nikapov	0:a1a69d32f310	3749	{
nikapov	0:a1a69d32f310	3750	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	3751	uint8_t last_spad_array[6];
nikapov	0:a1a69d32f310	3752	uint8_t start_select = 0xB4;
nikapov	0:a1a69d32f310	3753	uint32_t minimum_spad_count = 3;
nikapov	0:a1a69d32f310	3754	uint32_t max_spad_count = 44;
nikapov	0:a1a69d32f310	3755	uint32_t current_spad_index = 0;
nikapov	0:a1a69d32f310	3756	uint32_t last_spad_index = 0;
nikapov	0:a1a69d32f310	3757	int32_t next_good_spad = 0;
nikapov	0:a1a69d32f310	3758	uint16_t target_ref_rate = 0x0A00; /* 20 MCPS in 9:7 format */
nikapov	0:a1a69d32f310	3759	uint16_t peak_signal_rate_ref;
nikapov	0:a1a69d32f310	3760	uint32_t need_apt_spads = 0;
nikapov	0:a1a69d32f310	3761	uint32_t index = 0;
nikapov	0:a1a69d32f310	3762	uint32_t spad_array_size = 6;
nikapov	0:a1a69d32f310	3763	uint32_t signal_rate_diff = 0;
nikapov	0:a1a69d32f310	3764	uint32_t last_signal_rate_diff = 0;
nikapov	0:a1a69d32f310	3765	uint8_t complete = 0;
nikapov	0:a1a69d32f310	3766	uint8_t vhv_settings = 0;
nikapov	0:a1a69d32f310	3767	uint8_t phase_cal = 0;
nikapov	0:a1a69d32f310	3768	uint32_t ref_spad_count_int = 0;
nikapov	0:a1a69d32f310	3769	uint8_t is_aperture_spads_int = 0;
nikapov	0:a1a69d32f310	3770
nikapov	0:a1a69d32f310	3771	/*
nikapov	0:a1a69d32f310	3772	* The reference SPAD initialization procedure determines the minimum
nikapov	0:a1a69d32f310	3773	* amount of reference spads to be enables to achieve a target reference
nikapov	0:a1a69d32f310	3774	* signal rate and should be performed once during initialization.
nikapov	0:a1a69d32f310	3775	*
nikapov	0:a1a69d32f310	3776	* Either aperture or non-aperture spads are applied but never both.
nikapov	0:a1a69d32f310	3777	* Firstly non-aperture spads are set, begining with 5 spads, and
nikapov	0:a1a69d32f310	3778	* increased one spad at a time until the closest measurement to the
nikapov	0:a1a69d32f310	3779	* target rate is achieved.
nikapov	0:a1a69d32f310	3780	*
nikapov	0:a1a69d32f310	3781	* If the target rate is exceeded when 5 non-aperture spads are enabled,
nikapov	0:a1a69d32f310	3782	* initialization is performed instead with aperture spads.
nikapov	0:a1a69d32f310	3783	*
nikapov	0:a1a69d32f310	3784	* When setting spads, a 'Good Spad Map' is applied.
nikapov	0:a1a69d32f310	3785	*
nikapov	0:a1a69d32f310	3786	* This procedure operates within a SPAD window of interest of a maximum
nikapov	0:a1a69d32f310	3787	* 44 spads.
nikapov	0:a1a69d32f310	3788	* The start point is currently fixed to 180, which lies towards the end
nikapov	0:a1a69d32f310	3789	* of the non-aperture quadrant and runs in to the adjacent aperture
nikapov	0:a1a69d32f310	3790	* quadrant.
nikapov	0:a1a69d32f310	3791	*/
nikapov	0:a1a69d32f310	3792	target_ref_rate = PALDevDataGet(dev, targetRefRate);
nikapov	0:a1a69d32f310	3793
nikapov	0:a1a69d32f310	3794	/*
nikapov	0:a1a69d32f310	3795	* Initialize Spad arrays.
nikapov	0:a1a69d32f310	3796	* Currently the good spad map is initialised to 'All good'.
nikapov	0:a1a69d32f310	3797	* This is a short term implementation. The good spad map will be
nikapov	0:a1a69d32f310	3798	* provided as an input.
nikapov	0:a1a69d32f310	3799	* Note that there are 6 bytes. Only the first 44 bits will be used to
nikapov	0:a1a69d32f310	3800	* represent spads.
nikapov	0:a1a69d32f310	3801	*/
Davidroid	3:e9269ff624ed	3802	for (index = 0; index < spad_array_size; index++) {
nikapov	0:a1a69d32f310	3803	dev->Data.SpadData.RefSpadEnables[index] = 0;
Davidroid	3:e9269ff624ed	3804	}
nikapov	0:a1a69d32f310	3805
nikapov	0:a1a69d32f310	3806
nikapov	0:a1a69d32f310	3807	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	3808
nikapov	0:a1a69d32f310	3809	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3810	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	3811	VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
nikapov	0:a1a69d32f310	3812	}
nikapov	0:a1a69d32f310	3813
nikapov	0:a1a69d32f310	3814	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3815	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	3816	VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
Davidroid	2:d07edeaff6f1	3817	}
nikapov	0:a1a69d32f310	3818
nikapov	0:a1a69d32f310	3819	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3820	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	3821	}
nikapov	0:a1a69d32f310	3822
nikapov	0:a1a69d32f310	3823	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3824	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	3825	VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
nikapov	0:a1a69d32f310	3826	start_select);
Davidroid	2:d07edeaff6f1	3827	}
nikapov	0:a1a69d32f310	3828
nikapov	0:a1a69d32f310	3829	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3830	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	3831	VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
Davidroid	2:d07edeaff6f1	3832	}
nikapov	0:a1a69d32f310	3833
nikapov	0:a1a69d32f310	3834	/* Perform ref calibration */
nikapov	0:a1a69d32f310	3835	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3836	status = VL53L0X_perform_ref_calibration(dev, &vhv_settings,
nikapov	0:a1a69d32f310	3837	&phase_cal, 0);
Davidroid	2:d07edeaff6f1	3838	}
nikapov	0:a1a69d32f310	3839
nikapov	0:a1a69d32f310	3840	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3841	/* Enable Minimum NON-APERTURE Spads */
nikapov	0:a1a69d32f310	3842	current_spad_index = 0;
nikapov	0:a1a69d32f310	3843	last_spad_index = current_spad_index;
nikapov	0:a1a69d32f310	3844	need_apt_spads = 0;
nikapov	0:a1a69d32f310	3845	status = enable_ref_spads(dev,
nikapov	0:a1a69d32f310	3846	need_apt_spads,
nikapov	0:a1a69d32f310	3847	dev->Data.SpadData.RefGoodSpadMap,
nikapov	0:a1a69d32f310	3848	dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3849	spad_array_size,
nikapov	0:a1a69d32f310	3850	start_select,
nikapov	0:a1a69d32f310	3851	current_spad_index,
nikapov	0:a1a69d32f310	3852	minimum_spad_count,
nikapov	0:a1a69d32f310	3853	&last_spad_index);
nikapov	0:a1a69d32f310	3854	}
nikapov	0:a1a69d32f310	3855
nikapov	0:a1a69d32f310	3856	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3857	current_spad_index = last_spad_index;
nikapov	0:a1a69d32f310	3858
nikapov	0:a1a69d32f310	3859	status = perform_ref_signal_measurement(dev,
nikapov	0:a1a69d32f310	3860	&peak_signal_rate_ref);
nikapov	0:a1a69d32f310	3861	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	3862	(peak_signal_rate_ref > target_ref_rate)) {
nikapov	0:a1a69d32f310	3863	/* Signal rate measurement too high,
nikapov	0:a1a69d32f310	3864	* switch to APERTURE SPADs */
nikapov	0:a1a69d32f310	3865
nikapov	0:a1a69d32f310	3866	for (index = 0; index < spad_array_size; index++) {
nikapov	0:a1a69d32f310	3867	dev->Data.SpadData.RefSpadEnables[index] = 0;
nikapov	0:a1a69d32f310	3868	}
nikapov	0:a1a69d32f310	3869
nikapov	0:a1a69d32f310	3870
nikapov	0:a1a69d32f310	3871	/* Increment to the first APERTURE spad */
nikapov	0:a1a69d32f310	3872	while ((is_aperture(start_select + current_spad_index)
nikapov	0:a1a69d32f310	3873	== 0) && (current_spad_index < max_spad_count)) {
nikapov	0:a1a69d32f310	3874	current_spad_index++;
nikapov	0:a1a69d32f310	3875	}
nikapov	0:a1a69d32f310	3876
nikapov	0:a1a69d32f310	3877	need_apt_spads = 1;
nikapov	0:a1a69d32f310	3878
nikapov	0:a1a69d32f310	3879	status = enable_ref_spads(dev,
nikapov	0:a1a69d32f310	3880	need_apt_spads,
nikapov	0:a1a69d32f310	3881	dev->Data.SpadData.RefGoodSpadMap,
nikapov	0:a1a69d32f310	3882	dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3883	spad_array_size,
nikapov	0:a1a69d32f310	3884	start_select,
nikapov	0:a1a69d32f310	3885	current_spad_index,
nikapov	0:a1a69d32f310	3886	minimum_spad_count,
nikapov	0:a1a69d32f310	3887	&last_spad_index);
nikapov	0:a1a69d32f310	3888
nikapov	0:a1a69d32f310	3889	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3890	current_spad_index = last_spad_index;
nikapov	0:a1a69d32f310	3891	status = perform_ref_signal_measurement(dev,
nikapov	0:a1a69d32f310	3892	&peak_signal_rate_ref);
nikapov	0:a1a69d32f310	3893
nikapov	0:a1a69d32f310	3894	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	3895	(peak_signal_rate_ref > target_ref_rate)) {
nikapov	0:a1a69d32f310	3896	/* Signal rate still too high after
nikapov	0:a1a69d32f310	3897	* setting the minimum number of
nikapov	0:a1a69d32f310	3898	* APERTURE spads. Can do no more
nikapov	0:a1a69d32f310	3899	* therefore set the min number of
nikapov	0:a1a69d32f310	3900	* aperture spads as the result.
nikapov	0:a1a69d32f310	3901	*/
nikapov	0:a1a69d32f310	3902	is_aperture_spads_int = 1;
nikapov	0:a1a69d32f310	3903	ref_spad_count_int = minimum_spad_count;
nikapov	0:a1a69d32f310	3904	}
nikapov	0:a1a69d32f310	3905	}
nikapov	0:a1a69d32f310	3906	} else {
nikapov	0:a1a69d32f310	3907	need_apt_spads = 0;
nikapov	0:a1a69d32f310	3908	}
nikapov	0:a1a69d32f310	3909	}
nikapov	0:a1a69d32f310	3910
nikapov	0:a1a69d32f310	3911	if ((status == VL53L0X_ERROR_NONE) &&
nikapov	0:a1a69d32f310	3912	(peak_signal_rate_ref < target_ref_rate)) {
nikapov	0:a1a69d32f310	3913	/* At this point, the minimum number of either aperture
nikapov	0:a1a69d32f310	3914	* or non-aperture spads have been set. Proceed to add
nikapov	0:a1a69d32f310	3915	* spads and perform measurements until the target
nikapov	0:a1a69d32f310	3916	* reference is reached.
nikapov	0:a1a69d32f310	3917	*/
nikapov	0:a1a69d32f310	3918	is_aperture_spads_int = need_apt_spads;
nikapov	0:a1a69d32f310	3919	ref_spad_count_int = minimum_spad_count;
nikapov	0:a1a69d32f310	3920
nikapov	0:a1a69d32f310	3921	memcpy(last_spad_array, dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3922	spad_array_size);
nikapov	0:a1a69d32f310	3923	last_signal_rate_diff = abs(peak_signal_rate_ref -
nikapov	0:a1a69d32f310	3924	target_ref_rate);
nikapov	0:a1a69d32f310	3925	complete = 0;
nikapov	0:a1a69d32f310	3926
nikapov	0:a1a69d32f310	3927	while (!complete) {
nikapov	0:a1a69d32f310	3928	get_next_good_spad(
nikapov	0:a1a69d32f310	3929	dev->Data.SpadData.RefGoodSpadMap,
nikapov	0:a1a69d32f310	3930	spad_array_size, current_spad_index,
nikapov	0:a1a69d32f310	3931	&next_good_spad);
nikapov	0:a1a69d32f310	3932
nikapov	0:a1a69d32f310	3933	if (next_good_spad == -1) {
nikapov	0:a1a69d32f310	3934	status = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov	0:a1a69d32f310	3935	break;
nikapov	0:a1a69d32f310	3936	}
nikapov	0:a1a69d32f310	3937
nikapov	0:a1a69d32f310	3938	/* Cannot combine Aperture and Non-Aperture spads, so
nikapov	0:a1a69d32f310	3939	* ensure the current spad is of the correct type.
nikapov	0:a1a69d32f310	3940	*/
nikapov	0:a1a69d32f310	3941	if (is_aperture((uint32_t)start_select + next_good_spad) !=
nikapov	0:a1a69d32f310	3942	need_apt_spads) {
nikapov	0:a1a69d32f310	3943	/* At this point we have enabled the maximum
nikapov	0:a1a69d32f310	3944	* number of Aperture spads.
nikapov	0:a1a69d32f310	3945	*/
nikapov	0:a1a69d32f310	3946	complete = 1;
nikapov	0:a1a69d32f310	3947	break;
nikapov	0:a1a69d32f310	3948	}
nikapov	0:a1a69d32f310	3949
nikapov	0:a1a69d32f310	3950	(ref_spad_count_int)++;
nikapov	0:a1a69d32f310	3951
nikapov	0:a1a69d32f310	3952	current_spad_index = next_good_spad;
nikapov	0:a1a69d32f310	3953	status = enable_spad_bit(
nikapov	0:a1a69d32f310	3954	dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3955	spad_array_size, current_spad_index);
nikapov	0:a1a69d32f310	3956
nikapov	0:a1a69d32f310	3957	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3958	current_spad_index++;
nikapov	0:a1a69d32f310	3959	/* Proceed to apply the additional spad and
nikapov	0:a1a69d32f310	3960	* perform measurement. */
nikapov	0:a1a69d32f310	3961	status = set_ref_spad_map(dev,
nikapov	0:a1a69d32f310	3962	dev->Data.SpadData.RefSpadEnables);
nikapov	0:a1a69d32f310	3963	}
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	status = perform_ref_signal_measurement(dev,
nikapov	0:a1a69d32f310	3970	&peak_signal_rate_ref);
nikapov	0:a1a69d32f310	3971
nikapov	0:a1a69d32f310	3972	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	3973	break;
nikapov	0:a1a69d32f310	3974	}
nikapov	0:a1a69d32f310	3975
nikapov	0:a1a69d32f310	3976	signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate);
nikapov	0:a1a69d32f310	3977
nikapov	0:a1a69d32f310	3978	if (peak_signal_rate_ref > target_ref_rate) {
nikapov	0:a1a69d32f310	3979	/* Select the spad map that provides the
nikapov	0:a1a69d32f310	3980	* measurement closest to the target rate,
nikapov	0:a1a69d32f310	3981	* either above or below it.
nikapov	0:a1a69d32f310	3982	*/
nikapov	0:a1a69d32f310	3983	if (signal_rate_diff > last_signal_rate_diff) {
nikapov	0:a1a69d32f310	3984	/* Previous spad map produced a closer
nikapov	0:a1a69d32f310	3985	* measurement, so choose this. */
nikapov	0:a1a69d32f310	3986	status = set_ref_spad_map(dev,
nikapov	0:a1a69d32f310	3987	last_spad_array);
nikapov	0:a1a69d32f310	3988	memcpy(
nikapov	0:a1a69d32f310	3989	dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	3990	last_spad_array, spad_array_size);
nikapov	0:a1a69d32f310	3991
nikapov	0:a1a69d32f310	3992	(ref_spad_count_int)--;
nikapov	0:a1a69d32f310	3993	}
nikapov	0:a1a69d32f310	3994	complete = 1;
nikapov	0:a1a69d32f310	3995	} else {
nikapov	0:a1a69d32f310	3996	/* Continue to add spads */
nikapov	0:a1a69d32f310	3997	last_signal_rate_diff = signal_rate_diff;
nikapov	0:a1a69d32f310	3998	memcpy(last_spad_array,
nikapov	0:a1a69d32f310	3999	dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	4000	spad_array_size);
nikapov	0:a1a69d32f310	4001	}
nikapov	0:a1a69d32f310	4002
nikapov	0:a1a69d32f310	4003	} /* while */
nikapov	0:a1a69d32f310	4004	}
nikapov	0:a1a69d32f310	4005
nikapov	0:a1a69d32f310	4006	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4007	*ref_spad_count = ref_spad_count_int;
nikapov	0:a1a69d32f310	4008	*is_aperture_spads = is_aperture_spads_int;
nikapov	0:a1a69d32f310	4009
nikapov	0:a1a69d32f310	4010	VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1);
nikapov	0:a1a69d32f310	4011	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4012	ReferenceSpadCount, (uint8_t)(*ref_spad_count));
nikapov	0:a1a69d32f310	4013	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4014	ReferenceSpadType, *is_aperture_spads);
nikapov	0:a1a69d32f310	4015	}
nikapov	0:a1a69d32f310	4016
nikapov	0:a1a69d32f310	4017	return status;
nikapov	0:a1a69d32f310	4018	}
nikapov	0:a1a69d32f310	4019
nikapov	0:a1a69d32f310	4020	VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4021	uint32_t count, uint8_t is_aperture_spads)
nikapov	0:a1a69d32f310	4022	{
nikapov	0:a1a69d32f310	4023	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4024	uint32_t current_spad_index = 0;
nikapov	0:a1a69d32f310	4025	uint8_t start_select = 0xB4;
nikapov	0:a1a69d32f310	4026	uint32_t spad_array_size = 6;
nikapov	0:a1a69d32f310	4027	uint32_t max_spad_count = 44;
nikapov	0:a1a69d32f310	4028	uint32_t last_spad_index;
nikapov	0:a1a69d32f310	4029	uint32_t index;
nikapov	0:a1a69d32f310	4030
nikapov	0:a1a69d32f310	4031	/*
nikapov	0:a1a69d32f310	4032	* This function applies a requested number of reference spads, either
nikapov	0:a1a69d32f310	4033	* aperture or
nikapov	0:a1a69d32f310	4034	* non-aperture, as requested.
nikapov	0:a1a69d32f310	4035	* The good spad map will be applied.
nikapov	0:a1a69d32f310	4036	*/
nikapov	0:a1a69d32f310	4037
nikapov	0:a1a69d32f310	4038	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	4039
nikapov	0:a1a69d32f310	4040	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4041	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	4042	VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
Davidroid	2:d07edeaff6f1	4043	}
nikapov	0:a1a69d32f310	4044
nikapov	0:a1a69d32f310	4045	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4046	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	4047	VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
Davidroid	2:d07edeaff6f1	4048	}
nikapov	0:a1a69d32f310	4049
nikapov	0:a1a69d32f310	4050	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4051	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
Davidroid	2:d07edeaff6f1	4052	}
nikapov	0:a1a69d32f310	4053
nikapov	0:a1a69d32f310	4054	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4055	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	4056	VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
nikapov	0:a1a69d32f310	4057	start_select);
Davidroid	2:d07edeaff6f1	4058	}
nikapov	0:a1a69d32f310	4059
nikapov	0:a1a69d32f310	4060	for (index = 0; index < spad_array_size; index++) {
nikapov	0:a1a69d32f310	4061	dev->Data.SpadData.RefSpadEnables[index] = 0;
Davidroid	2:d07edeaff6f1	4062	}
nikapov	0:a1a69d32f310	4063
nikapov	0:a1a69d32f310	4064	if (is_aperture_spads) {
nikapov	0:a1a69d32f310	4065	/* Increment to the first APERTURE spad */
nikapov	0:a1a69d32f310	4066	while ((is_aperture(start_select + current_spad_index) == 0) &&
nikapov	0:a1a69d32f310	4067	(current_spad_index < max_spad_count)) {
nikapov	0:a1a69d32f310	4068	current_spad_index++;
nikapov	0:a1a69d32f310	4069	}
nikapov	0:a1a69d32f310	4070	}
nikapov	0:a1a69d32f310	4071	status = enable_ref_spads(dev,
nikapov	0:a1a69d32f310	4072	is_aperture_spads,
nikapov	0:a1a69d32f310	4073	dev->Data.SpadData.RefGoodSpadMap,
nikapov	0:a1a69d32f310	4074	dev->Data.SpadData.RefSpadEnables,
nikapov	0:a1a69d32f310	4075	spad_array_size,
nikapov	0:a1a69d32f310	4076	start_select,
nikapov	0:a1a69d32f310	4077	current_spad_index,
nikapov	0:a1a69d32f310	4078	count,
nikapov	0:a1a69d32f310	4079	&last_spad_index);
nikapov	0:a1a69d32f310	4080
nikapov	0:a1a69d32f310	4081	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4082	VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1);
nikapov	0:a1a69d32f310	4083	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4084	ReferenceSpadCount, (uint8_t)(count));
nikapov	0:a1a69d32f310	4085	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4086	ReferenceSpadType, is_aperture_spads);
nikapov	0:a1a69d32f310	4087	}
nikapov	0:a1a69d32f310	4088
nikapov	0:a1a69d32f310	4089	return status;
nikapov	0:a1a69d32f310	4090	}
nikapov	0:a1a69d32f310	4091
nikapov	0:a1a69d32f310	4092	VL53L0X_Error VL53L0X::VL53L0X_wait_device_booted(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	4093	{
nikapov	0:a1a69d32f310	4094	VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
nikapov	0:a1a69d32f310	4095	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4096
nikapov	0:a1a69d32f310	4097	/* not implemented on VL53L0X */
nikapov	0:a1a69d32f310	4098
nikapov	0:a1a69d32f310	4099	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4100	return status;
nikapov	0:a1a69d32f310	4101	}
nikapov	0:a1a69d32f310	4102
nikapov	0:a1a69d32f310	4103	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV dev, uint8_t *p_vhv_settings,
nikapov	0:a1a69d32f310	4104	uint8_t *p_phase_cal)
nikapov	0:a1a69d32f310	4105	{
nikapov	0:a1a69d32f310	4106	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4107	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4108
nikapov	0:a1a69d32f310	4109	status = VL53L0X_perform_ref_calibration(dev, p_vhv_settings,
nikapov	0:a1a69d32f310	4110	p_phase_cal, 1);
nikapov	0:a1a69d32f310	4111
nikapov	0:a1a69d32f310	4112	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4113	return status;
nikapov	0:a1a69d32f310	4114	}
nikapov	0:a1a69d32f310	4115
nikapov	0:a1a69d32f310	4116	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4117	uint32_t ref_spad_count, uint8_t is_aperture_spads)
nikapov	0:a1a69d32f310	4118	{
nikapov	0:a1a69d32f310	4119	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4120	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4121
nikapov	0:a1a69d32f310	4122	status = wrapped_VL53L0X_perform_ref_spad_management(dev, ref_spad_count,
nikapov	0:a1a69d32f310	4123	is_aperture_spads);
nikapov	0:a1a69d32f310	4124
nikapov	0:a1a69d32f310	4125	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4126
nikapov	0:a1a69d32f310	4127	return status;
nikapov	0:a1a69d32f310	4128	}
nikapov	0:a1a69d32f310	4129
nikapov	0:a1a69d32f310	4130	/* Group PAL Init Functions */
nikapov	0:a1a69d32f310	4131	VL53L0X_Error VL53L0X::VL53L0X_set_device_address(VL53L0X_DEV dev, uint8_t device_address)
nikapov	0:a1a69d32f310	4132	{
nikapov	0:a1a69d32f310	4133	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4134	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4135
nikapov	0:a1a69d32f310	4136	status = VL53L0X_write_byte(dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
nikapov	0:a1a69d32f310	4137	device_address / 2);
nikapov	0:a1a69d32f310	4138
nikapov	0:a1a69d32f310	4139	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4140	return status;
nikapov	0:a1a69d32f310	4141	}
nikapov	0:a1a69d32f310	4142
nikapov	0:a1a69d32f310	4143	VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config(VL53L0X_DEV dev, uint8_t pin,
nikapov	0:a1a69d32f310	4144	VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
nikapov	0:a1a69d32f310	4145	VL53L0X_InterruptPolarity polarity)
nikapov	0:a1a69d32f310	4146	{
nikapov	0:a1a69d32f310	4147	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4148	uint8_t data;
nikapov	0:a1a69d32f310	4149
nikapov	0:a1a69d32f310	4150	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4151
nikapov	0:a1a69d32f310	4152	if (pin != 0) {
nikapov	0:a1a69d32f310	4153	status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
nikapov	0:a1a69d32f310	4154	} else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
nikapov	0:a1a69d32f310	4155	if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
nikapov	0:a1a69d32f310	4156	data = 0x10;
nikapov	0:a1a69d32f310	4157	} else {
nikapov	0:a1a69d32f310	4158	data = 1;
nikapov	0:a1a69d32f310	4159	}
nikapov	0:a1a69d32f310	4160
nikapov	0:a1a69d32f310	4161	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	4162	VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
nikapov	0:a1a69d32f310	4163
nikapov	0:a1a69d32f310	4164	} else {
Davidroid	2:d07edeaff6f1	4165	if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) {
Davidroid	2:d07edeaff6f1	4166
Davidroid	2:d07edeaff6f1	4167	status \|= VL53L0X_write_byte(dev, 0xff, 0x01);
Davidroid	2:d07edeaff6f1	4168	status \|= VL53L0X_write_byte(dev, 0x00, 0x00);
Davidroid	2:d07edeaff6f1	4169
Davidroid	2:d07edeaff6f1	4170	status \|= VL53L0X_write_byte(dev, 0xff, 0x00);
Davidroid	2:d07edeaff6f1	4171	status \|= VL53L0X_write_byte(dev, 0x80, 0x01);
Davidroid	2:d07edeaff6f1	4172	status \|= VL53L0X_write_byte(dev, 0x85, 0x02);
Davidroid	2:d07edeaff6f1	4173
Davidroid	2:d07edeaff6f1	4174	status \|= VL53L0X_write_byte(dev, 0xff, 0x04);
Davidroid	2:d07edeaff6f1	4175	status \|= VL53L0X_write_byte(dev, 0xcd, 0x00);
Davidroid	2:d07edeaff6f1	4176	status \|= VL53L0X_write_byte(dev, 0xcc, 0x11);
Davidroid	2:d07edeaff6f1	4177
Davidroid	2:d07edeaff6f1	4178	status \|= VL53L0X_write_byte(dev, 0xff, 0x07);
Davidroid	2:d07edeaff6f1	4179	status \|= VL53L0X_write_byte(dev, 0xbe, 0x00);
Davidroid	2:d07edeaff6f1	4180
Davidroid	2:d07edeaff6f1	4181	status \|= VL53L0X_write_byte(dev, 0xff, 0x06);
Davidroid	2:d07edeaff6f1	4182	status \|= VL53L0X_write_byte(dev, 0xcc, 0x09);
Davidroid	2:d07edeaff6f1	4183
Davidroid	2:d07edeaff6f1	4184	status \|= VL53L0X_write_byte(dev, 0xff, 0x00);
Davidroid	2:d07edeaff6f1	4185	status \|= VL53L0X_write_byte(dev, 0xff, 0x01);
Davidroid	2:d07edeaff6f1	4186	status \|= VL53L0X_write_byte(dev, 0x00, 0x00);
Davidroid	2:d07edeaff6f1	4187
Davidroid	2:d07edeaff6f1	4188	} else {
Davidroid	2:d07edeaff6f1	4189
Davidroid	2:d07edeaff6f1	4190	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	4191	switch (functionality) {
Davidroid	2:d07edeaff6f1	4192	case VL53L0X_GPIOFUNCTIONALITY_OFF:
Davidroid	2:d07edeaff6f1	4193	data = 0x00;
Davidroid	2:d07edeaff6f1	4194	break;
Davidroid	2:d07edeaff6f1	4195	case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
Davidroid	2:d07edeaff6f1	4196	data = 0x01;
Davidroid	2:d07edeaff6f1	4197	break;
Davidroid	2:d07edeaff6f1	4198	case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
Davidroid	2:d07edeaff6f1	4199	data = 0x02;
Davidroid	2:d07edeaff6f1	4200	break;
Davidroid	2:d07edeaff6f1	4201	case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
Davidroid	2:d07edeaff6f1	4202	data = 0x03;
Davidroid	2:d07edeaff6f1	4203	break;
Davidroid	2:d07edeaff6f1	4204	case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
Davidroid	2:d07edeaff6f1	4205	data = 0x04;
Davidroid	2:d07edeaff6f1	4206	break;
Davidroid	2:d07edeaff6f1	4207	default:
Davidroid	2:d07edeaff6f1	4208	status =
Davidroid	2:d07edeaff6f1	4209	VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
Davidroid	2:d07edeaff6f1	4210	}
Davidroid	2:d07edeaff6f1	4211	}
Davidroid	2:d07edeaff6f1	4212
Davidroid	2:d07edeaff6f1	4213	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	4214	status = VL53L0X_write_byte(dev,
Davidroid	2:d07edeaff6f1	4215	VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
Davidroid	2:d07edeaff6f1	4216	}
Davidroid	2:d07edeaff6f1	4217
Davidroid	2:d07edeaff6f1	4218	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	4219	if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) {
Davidroid	2:d07edeaff6f1	4220	data = 0;
Davidroid	2:d07edeaff6f1	4221	} else {
Davidroid	2:d07edeaff6f1	4222	data = (uint8_t)(1 << 4);
Davidroid	2:d07edeaff6f1	4223	}
Davidroid	2:d07edeaff6f1	4224	status = VL53L0X_update_byte(dev,
Davidroid	2:d07edeaff6f1	4225	VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
Davidroid	2:d07edeaff6f1	4226	}
Davidroid	2:d07edeaff6f1	4227
Davidroid	2:d07edeaff6f1	4228	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	4229	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
Davidroid	2:d07edeaff6f1	4230	Pin0GpioFunctionality, functionality);
Davidroid	2:d07edeaff6f1	4231	}
Davidroid	2:d07edeaff6f1	4232
Davidroid	2:d07edeaff6f1	4233	if (status == VL53L0X_ERROR_NONE) {
Davidroid	2:d07edeaff6f1	4234	status = VL53L0X_clear_interrupt_mask(dev, 0);
Davidroid	2:d07edeaff6f1	4235	}
Davidroid	2:d07edeaff6f1	4236	}
nikapov	0:a1a69d32f310	4237	}
nikapov	0:a1a69d32f310	4238	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4239	return status;
nikapov	0:a1a69d32f310	4240	}
nikapov	0:a1a69d32f310	4241
nikapov	0:a1a69d32f310	4242	VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable(VL53L0X_DEV dev, uint8_t *p_enabled)
nikapov	0:a1a69d32f310	4243	{
nikapov	0:a1a69d32f310	4244	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4245	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4246
nikapov	0:a1a69d32f310	4247	status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled);
nikapov	0:a1a69d32f310	4248
nikapov	0:a1a69d32f310	4249	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4250	p_enabled = (p_enabled & 1);
nikapov	0:a1a69d32f310	4251	}
nikapov	0:a1a69d32f310	4252
nikapov	0:a1a69d32f310	4253	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4254	return status;
nikapov	0:a1a69d32f310	4255	}
nikapov	0:a1a69d32f310	4256
nikapov	0:a1a69d32f310	4257	uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
nikapov	0:a1a69d32f310	4258	{
nikapov	0:a1a69d32f310	4259	/*!
nikapov	0:a1a69d32f310	4260	* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
nikapov	0:a1a69d32f310	4261	*/
nikapov	0:a1a69d32f310	4262
nikapov	0:a1a69d32f310	4263	uint16_t encoded_timeout = 0;
nikapov	0:a1a69d32f310	4264	uint32_t ls_byte = 0;
nikapov	0:a1a69d32f310	4265	uint16_t ms_byte = 0;
nikapov	0:a1a69d32f310	4266
nikapov	0:a1a69d32f310	4267	if (timeout_macro_clks > 0) {
nikapov	0:a1a69d32f310	4268	ls_byte = timeout_macro_clks - 1;
nikapov	0:a1a69d32f310	4269
nikapov	0:a1a69d32f310	4270	while ((ls_byte & 0xFFFFFF00) > 0) {
nikapov	0:a1a69d32f310	4271	ls_byte = ls_byte >> 1;
nikapov	0:a1a69d32f310	4272	ms_byte++;
nikapov	0:a1a69d32f310	4273	}
nikapov	0:a1a69d32f310	4274
nikapov	0:a1a69d32f310	4275	encoded_timeout = (ms_byte << 8)
nikapov	0:a1a69d32f310	4276	+ (uint16_t)(ls_byte & 0x000000FF);
nikapov	0:a1a69d32f310	4277	}
nikapov	0:a1a69d32f310	4278
nikapov	0:a1a69d32f310	4279	return encoded_timeout;
nikapov	0:a1a69d32f310	4280
nikapov	0:a1a69d32f310	4281	}
nikapov	0:a1a69d32f310	4282
nikapov	0:a1a69d32f310	4283	VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4284	VL53L0X_SequenceStepId sequence_step_id,
nikapov	0:a1a69d32f310	4285	uint32_t timeout_micro_secs)
nikapov	0:a1a69d32f310	4286	{
nikapov	0:a1a69d32f310	4287	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4288	uint8_t current_vcsel_pulse_period_p_clk;
nikapov	0:a1a69d32f310	4289	uint8_t msrc_encoded_time_out;
nikapov	0:a1a69d32f310	4290	uint16_t pre_range_encoded_time_out;
nikapov	0:a1a69d32f310	4291	uint16_t pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	4292	uint16_t msrc_range_time_out_m_clks;
nikapov	0:a1a69d32f310	4293	uint32_t final_range_time_out_m_clks;
nikapov	0:a1a69d32f310	4294	uint16_t final_range_encoded_time_out;
nikapov	0:a1a69d32f310	4295	VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov	0:a1a69d32f310	4296
nikapov	0:a1a69d32f310	4297	if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) \|\|
nikapov	0:a1a69d32f310	4298	(sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) \|\|
nikapov	0:a1a69d32f310	4299	(sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
nikapov	0:a1a69d32f310	4300
nikapov	0:a1a69d32f310	4301	status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov	0:a1a69d32f310	4302	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4303	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4304
nikapov	0:a1a69d32f310	4305	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4306	msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(dev,
nikapov	0:a1a69d32f310	4307	timeout_micro_secs,
nikapov	0:a1a69d32f310	4308	(uint8_t)current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4309
nikapov	0:a1a69d32f310	4310	if (msrc_range_time_out_m_clks > 256) {
nikapov	0:a1a69d32f310	4311	msrc_encoded_time_out = 255;
nikapov	0:a1a69d32f310	4312	} else {
nikapov	0:a1a69d32f310	4313	msrc_encoded_time_out =
nikapov	0:a1a69d32f310	4314	(uint8_t)msrc_range_time_out_m_clks - 1;
nikapov	0:a1a69d32f310	4315	}
nikapov	0:a1a69d32f310	4316
nikapov	0:a1a69d32f310	4317	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4318	LastEncodedTimeout,
nikapov	0:a1a69d32f310	4319	msrc_encoded_time_out);
nikapov	0:a1a69d32f310	4320	}
nikapov	0:a1a69d32f310	4321
nikapov	0:a1a69d32f310	4322	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4323	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	4324	VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
nikapov	0:a1a69d32f310	4325	msrc_encoded_time_out);
nikapov	0:a1a69d32f310	4326	}
nikapov	0:a1a69d32f310	4327	} else {
nikapov	0:a1a69d32f310	4328
nikapov	0:a1a69d32f310	4329	if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
nikapov	0:a1a69d32f310	4330
nikapov	0:a1a69d32f310	4331	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4332	status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov	0:a1a69d32f310	4333	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4334	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4335	pre_range_time_out_m_clks =
nikapov	0:a1a69d32f310	4336	VL53L0X_calc_timeout_mclks(dev,
nikapov	0:a1a69d32f310	4337	timeout_micro_secs,
nikapov	0:a1a69d32f310	4338	(uint8_t)current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4339	pre_range_encoded_time_out = VL53L0X_encode_timeout(
nikapov	0:a1a69d32f310	4340	pre_range_time_out_m_clks);
nikapov	0:a1a69d32f310	4341
nikapov	0:a1a69d32f310	4342	VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4343	LastEncodedTimeout,
nikapov	0:a1a69d32f310	4344	pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4345	}
nikapov	0:a1a69d32f310	4346
nikapov	0:a1a69d32f310	4347	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4348	status = VL53L0X_write_word(dev,
nikapov	0:a1a69d32f310	4349	VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
nikapov	0:a1a69d32f310	4350	pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4351	}
nikapov	0:a1a69d32f310	4352
nikapov	0:a1a69d32f310	4353	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4354	VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	4355	dev,
nikapov	0:a1a69d32f310	4356	PreRangeTimeoutMicroSecs,
nikapov	0:a1a69d32f310	4357	timeout_micro_secs);
nikapov	0:a1a69d32f310	4358	}
nikapov	0:a1a69d32f310	4359	} else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov	0:a1a69d32f310	4360
nikapov	0:a1a69d32f310	4361	/* For the final range timeout, the pre-range timeout
nikapov	0:a1a69d32f310	4362	* must be added. To do this both final and pre-range
nikapov	0:a1a69d32f310	4363	* timeouts must be expressed in macro periods MClks
nikapov	0:a1a69d32f310	4364	* because they have different vcsel periods.
nikapov	0:a1a69d32f310	4365	*/
nikapov	0:a1a69d32f310	4366
nikapov	0:a1a69d32f310	4367	VL53L0X_get_sequence_step_enables(dev,
nikapov	0:a1a69d32f310	4368	&scheduler_sequence_steps);
nikapov	0:a1a69d32f310	4369	pre_range_time_out_m_clks = 0;
nikapov	0:a1a69d32f310	4370	if (scheduler_sequence_steps.PreRangeOn) {
nikapov	0:a1a69d32f310	4371
nikapov	0:a1a69d32f310	4372	/* Retrieve PRE-RANGE VCSEL Period */
nikapov	0:a1a69d32f310	4373	status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov	0:a1a69d32f310	4374	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4375	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4376
nikapov	0:a1a69d32f310	4377	/* Retrieve PRE-RANGE Timeout in Macro periods
nikapov	0:a1a69d32f310	4378	* (MCLKS) */
nikapov	0:a1a69d32f310	4379	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4380	status = VL53L0X_read_word(dev, 0x51,
nikapov	0:a1a69d32f310	4381	&pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4382	pre_range_time_out_m_clks =
nikapov	0:a1a69d32f310	4383	VL53L0X_decode_timeout(
nikapov	0:a1a69d32f310	4384	pre_range_encoded_time_out);
nikapov	0:a1a69d32f310	4385	}
nikapov	0:a1a69d32f310	4386	}
nikapov	0:a1a69d32f310	4387
nikapov	0:a1a69d32f310	4388	/* Calculate FINAL RANGE Timeout in Macro Periods
nikapov	0:a1a69d32f310	4389	* (MCLKS) and add PRE-RANGE value
nikapov	0:a1a69d32f310	4390	*/
nikapov	0:a1a69d32f310	4391	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4392	status = VL53L0X_get_vcsel_pulse_period(dev,
nikapov	0:a1a69d32f310	4393	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov	0:a1a69d32f310	4394	&current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4395	}
nikapov	0:a1a69d32f310	4396	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4397	final_range_time_out_m_clks =
nikapov	0:a1a69d32f310	4398	VL53L0X_calc_timeout_mclks(dev,
nikapov	0:a1a69d32f310	4399	timeout_micro_secs,
nikapov	0:a1a69d32f310	4400	(uint8_t) current_vcsel_pulse_period_p_clk);
nikapov	0:a1a69d32f310	4401
nikapov	0:a1a69d32f310	4402	final_range_time_out_m_clks += pre_range_time_out_m_clks;
nikapov	0:a1a69d32f310	4403
nikapov	0:a1a69d32f310	4404	final_range_encoded_time_out =
nikapov	0:a1a69d32f310	4405	VL53L0X_encode_timeout(final_range_time_out_m_clks);
nikapov	0:a1a69d32f310	4406
nikapov	0:a1a69d32f310	4407	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4408	status = VL53L0X_write_word(dev, 0x71,
nikapov	0:a1a69d32f310	4409	final_range_encoded_time_out);
nikapov	0:a1a69d32f310	4410	}
nikapov	0:a1a69d32f310	4411
nikapov	0:a1a69d32f310	4412	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4413	VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	4414	dev,
nikapov	0:a1a69d32f310	4415	FinalRangeTimeoutMicroSecs,
nikapov	0:a1a69d32f310	4416	timeout_micro_secs);
nikapov	0:a1a69d32f310	4417	}
nikapov	0:a1a69d32f310	4418	}
nikapov	0:a1a69d32f310	4419	} else {
nikapov	0:a1a69d32f310	4420	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4421	}
nikapov	0:a1a69d32f310	4422
nikapov	0:a1a69d32f310	4423	}
nikapov	0:a1a69d32f310	4424	return status;
nikapov	0:a1a69d32f310	4425	}
nikapov	0:a1a69d32f310	4426
nikapov	0:a1a69d32f310	4427	VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4428	uint32_t measurement_timing_budget_micro_seconds)
nikapov	0:a1a69d32f310	4429	{
nikapov	0:a1a69d32f310	4430	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4431	uint32_t final_range_timing_budget_micro_seconds;
nikapov	0:a1a69d32f310	4432	VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
nikapov	0:a1a69d32f310	4433	uint32_t msrc_dcc_tcc_timeout_micro_seconds = 2000;
nikapov	0:a1a69d32f310	4434	uint32_t start_overhead_micro_seconds = 1910;
nikapov	0:a1a69d32f310	4435	uint32_t end_overhead_micro_seconds = 960;
nikapov	0:a1a69d32f310	4436	uint32_t msrc_overhead_micro_seconds = 660;
nikapov	0:a1a69d32f310	4437	uint32_t tcc_overhead_micro_seconds = 590;
nikapov	0:a1a69d32f310	4438	uint32_t dss_overhead_micro_seconds = 690;
nikapov	0:a1a69d32f310	4439	uint32_t pre_range_overhead_micro_seconds = 660;
nikapov	0:a1a69d32f310	4440	uint32_t final_range_overhead_micro_seconds = 550;
nikapov	0:a1a69d32f310	4441	uint32_t pre_range_timeout_micro_seconds = 0;
nikapov	0:a1a69d32f310	4442	uint32_t c_min_timing_budget_micro_seconds = 20000;
nikapov	0:a1a69d32f310	4443	uint32_t sub_timeout = 0;
nikapov	0:a1a69d32f310	4444
nikapov	0:a1a69d32f310	4445	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4446
nikapov	0:a1a69d32f310	4447	if (measurement_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	4448	< c_min_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4449	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4450	return status;
nikapov	0:a1a69d32f310	4451	}
nikapov	0:a1a69d32f310	4452
nikapov	0:a1a69d32f310	4453	final_range_timing_budget_micro_seconds =
nikapov	0:a1a69d32f310	4454	measurement_timing_budget_micro_seconds -
nikapov	0:a1a69d32f310	4455	(start_overhead_micro_seconds + end_overhead_micro_seconds);
nikapov	0:a1a69d32f310	4456
nikapov	0:a1a69d32f310	4457	status = VL53L0X_get_sequence_step_enables(dev, &scheduler_sequence_steps);
nikapov	0:a1a69d32f310	4458
nikapov	0:a1a69d32f310	4459	if (status == VL53L0X_ERROR_NONE &&
nikapov	0:a1a69d32f310	4460	(scheduler_sequence_steps.TccOn \|\|
nikapov	0:a1a69d32f310	4461	scheduler_sequence_steps.MsrcOn \|\|
nikapov	0:a1a69d32f310	4462	scheduler_sequence_steps.DssOn)) {
nikapov	0:a1a69d32f310	4463
nikapov	0:a1a69d32f310	4464	/* TCC, MSRC and DSS all share the same timeout */
nikapov	0:a1a69d32f310	4465	status = get_sequence_step_timeout(dev,
nikapov	0:a1a69d32f310	4466	VL53L0X_SEQUENCESTEP_MSRC,
nikapov	0:a1a69d32f310	4467	&msrc_dcc_tcc_timeout_micro_seconds);
nikapov	0:a1a69d32f310	4468
nikapov	0:a1a69d32f310	4469	/* Subtract the TCC, MSRC and DSS timeouts if they are
nikapov	0:a1a69d32f310	4470	* enabled. */
nikapov	0:a1a69d32f310	4471
nikapov	0:a1a69d32f310	4472	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4473	return status;
nikapov	0:a1a69d32f310	4474	}
nikapov	0:a1a69d32f310	4475
nikapov	0:a1a69d32f310	4476	/* TCC */
nikapov	0:a1a69d32f310	4477	if (scheduler_sequence_steps.TccOn) {
nikapov	0:a1a69d32f310	4478
nikapov	0:a1a69d32f310	4479	sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
nikapov	0:a1a69d32f310	4480	+ tcc_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4481
nikapov	0:a1a69d32f310	4482	if (sub_timeout <
nikapov	0:a1a69d32f310	4483	final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4484	final_range_timing_budget_micro_seconds -=
nikapov	0:a1a69d32f310	4485	sub_timeout;
nikapov	0:a1a69d32f310	4486	} else {
nikapov	0:a1a69d32f310	4487	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4488	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4489	}
nikapov	0:a1a69d32f310	4490	}
nikapov	0:a1a69d32f310	4491
nikapov	0:a1a69d32f310	4492	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4493	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4494	return status;
nikapov	0:a1a69d32f310	4495	}
nikapov	0:a1a69d32f310	4496
nikapov	0:a1a69d32f310	4497	/* DSS */
nikapov	0:a1a69d32f310	4498	if (scheduler_sequence_steps.DssOn) {
nikapov	0:a1a69d32f310	4499
nikapov	0:a1a69d32f310	4500	sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	4501	dss_overhead_micro_seconds);
nikapov	0:a1a69d32f310	4502
nikapov	0:a1a69d32f310	4503	if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4504	final_range_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	4505	-= sub_timeout;
nikapov	0:a1a69d32f310	4506	} else {
nikapov	0:a1a69d32f310	4507	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4508	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4509	}
nikapov	0:a1a69d32f310	4510	} else if (scheduler_sequence_steps.MsrcOn) {
nikapov	0:a1a69d32f310	4511	/* MSRC */
nikapov	0:a1a69d32f310	4512	sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
nikapov	0:a1a69d32f310	4513	msrc_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4514
nikapov	0:a1a69d32f310	4515	if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4516	final_range_timing_budget_micro_seconds
nikapov	0:a1a69d32f310	4517	-= sub_timeout;
nikapov	0:a1a69d32f310	4518	} else {
nikapov	0:a1a69d32f310	4519	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4520	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4521	}
nikapov	0:a1a69d32f310	4522	}
nikapov	0:a1a69d32f310	4523
nikapov	0:a1a69d32f310	4524	}
nikapov	0:a1a69d32f310	4525
nikapov	0:a1a69d32f310	4526	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4527	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4528	return status;
nikapov	0:a1a69d32f310	4529	}
nikapov	0:a1a69d32f310	4530
nikapov	0:a1a69d32f310	4531	if (scheduler_sequence_steps.PreRangeOn) {
nikapov	0:a1a69d32f310	4532
nikapov	0:a1a69d32f310	4533	/* Subtract the Pre-range timeout if enabled. */
nikapov	0:a1a69d32f310	4534
nikapov	0:a1a69d32f310	4535	status = get_sequence_step_timeout(dev,
nikapov	0:a1a69d32f310	4536	VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	4537	&pre_range_timeout_micro_seconds);
nikapov	0:a1a69d32f310	4538
nikapov	0:a1a69d32f310	4539	sub_timeout = pre_range_timeout_micro_seconds +
nikapov	0:a1a69d32f310	4540	pre_range_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4541
nikapov	0:a1a69d32f310	4542	if (sub_timeout < final_range_timing_budget_micro_seconds) {
nikapov	0:a1a69d32f310	4543	final_range_timing_budget_micro_seconds -= sub_timeout;
nikapov	0:a1a69d32f310	4544	} else {
nikapov	0:a1a69d32f310	4545	/* Requested timeout too big. */
nikapov	0:a1a69d32f310	4546	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4547	}
nikapov	0:a1a69d32f310	4548	}
nikapov	0:a1a69d32f310	4549
nikapov	0:a1a69d32f310	4550
nikapov	0:a1a69d32f310	4551	if (status == VL53L0X_ERROR_NONE &&
nikapov	0:a1a69d32f310	4552	scheduler_sequence_steps.FinalRangeOn) {
nikapov	0:a1a69d32f310	4553
nikapov	0:a1a69d32f310	4554	final_range_timing_budget_micro_seconds -=
nikapov	0:a1a69d32f310	4555	final_range_overhead_micro_seconds;
nikapov	0:a1a69d32f310	4556
nikapov	0:a1a69d32f310	4557	/* Final Range Timeout
nikapov	0:a1a69d32f310	4558	* Note that the final range timeout is determined by the timing
nikapov	0:a1a69d32f310	4559	* budget and the sum of all other timeouts within the sequence.
nikapov	0:a1a69d32f310	4560	* If there is no room for the final range timeout, then an error
nikapov	0:a1a69d32f310	4561	* will be set. Otherwise the remaining time will be applied to
nikapov	0:a1a69d32f310	4562	* the final range.
nikapov	0:a1a69d32f310	4563	*/
nikapov	0:a1a69d32f310	4564	status = set_sequence_step_timeout(dev,
nikapov	0:a1a69d32f310	4565	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	4566	final_range_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4567
nikapov	0:a1a69d32f310	4568	VL53L0X_SETPARAMETERFIELD(dev,
nikapov	0:a1a69d32f310	4569	MeasurementTimingBudgetMicroSeconds,
nikapov	0:a1a69d32f310	4570	measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4571	}
nikapov	0:a1a69d32f310	4572
nikapov	0:a1a69d32f310	4573	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4574
nikapov	0:a1a69d32f310	4575	return status;
nikapov	0:a1a69d32f310	4576	}
nikapov	0:a1a69d32f310	4577
nikapov	0:a1a69d32f310	4578	VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4579	uint32_t measurement_timing_budget_micro_seconds)
nikapov	0:a1a69d32f310	4580	{
nikapov	0:a1a69d32f310	4581	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4582	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4583
nikapov	0:a1a69d32f310	4584	status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(dev,
nikapov	0:a1a69d32f310	4585	measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4586
nikapov	0:a1a69d32f310	4587	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4588
nikapov	0:a1a69d32f310	4589	return status;
nikapov	0:a1a69d32f310	4590	}
nikapov	0:a1a69d32f310	4591
nikapov	0:a1a69d32f310	4592	VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4593	VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled)
nikapov	0:a1a69d32f310	4594	{
nikapov	0:a1a69d32f310	4595	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4596	uint8_t sequence_config = 0;
nikapov	0:a1a69d32f310	4597	uint8_t sequence_config_new = 0;
nikapov	0:a1a69d32f310	4598	uint32_t measurement_timing_budget_micro_seconds;
nikapov	0:a1a69d32f310	4599	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4600
nikapov	0:a1a69d32f310	4601	status = VL53L0X_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
nikapov	0:a1a69d32f310	4602	&sequence_config);
nikapov	0:a1a69d32f310	4603
nikapov	0:a1a69d32f310	4604	sequence_config_new = sequence_config;
nikapov	0:a1a69d32f310	4605
nikapov	0:a1a69d32f310	4606	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4607	if (sequence_step_enabled == 1) {
nikapov	0:a1a69d32f310	4608
nikapov	0:a1a69d32f310	4609	/* Enable requested sequence step
nikapov	0:a1a69d32f310	4610	*/
nikapov	0:a1a69d32f310	4611	switch (sequence_step_id) {
nikapov	0:a1a69d32f310	4612	case VL53L0X_SEQUENCESTEP_TCC:
nikapov	0:a1a69d32f310	4613	sequence_config_new \|= 0x10;
nikapov	0:a1a69d32f310	4614	break;
nikapov	0:a1a69d32f310	4615	case VL53L0X_SEQUENCESTEP_DSS:
nikapov	0:a1a69d32f310	4616	sequence_config_new \|= 0x28;
nikapov	0:a1a69d32f310	4617	break;
nikapov	0:a1a69d32f310	4618	case VL53L0X_SEQUENCESTEP_MSRC:
nikapov	0:a1a69d32f310	4619	sequence_config_new \|= 0x04;
nikapov	0:a1a69d32f310	4620	break;
nikapov	0:a1a69d32f310	4621	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov	0:a1a69d32f310	4622	sequence_config_new \|= 0x40;
nikapov	0:a1a69d32f310	4623	break;
nikapov	0:a1a69d32f310	4624	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov	0:a1a69d32f310	4625	sequence_config_new \|= 0x80;
nikapov	0:a1a69d32f310	4626	break;
nikapov	0:a1a69d32f310	4627	default:
nikapov	0:a1a69d32f310	4628	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4629	}
nikapov	0:a1a69d32f310	4630	} else {
nikapov	0:a1a69d32f310	4631	/* Disable requested sequence step
nikapov	0:a1a69d32f310	4632	*/
nikapov	0:a1a69d32f310	4633	switch (sequence_step_id) {
nikapov	0:a1a69d32f310	4634	case VL53L0X_SEQUENCESTEP_TCC:
nikapov	0:a1a69d32f310	4635	sequence_config_new &= 0xef;
nikapov	0:a1a69d32f310	4636	break;
nikapov	0:a1a69d32f310	4637	case VL53L0X_SEQUENCESTEP_DSS:
nikapov	0:a1a69d32f310	4638	sequence_config_new &= 0xd7;
nikapov	0:a1a69d32f310	4639	break;
nikapov	0:a1a69d32f310	4640	case VL53L0X_SEQUENCESTEP_MSRC:
nikapov	0:a1a69d32f310	4641	sequence_config_new &= 0xfb;
nikapov	0:a1a69d32f310	4642	break;
nikapov	0:a1a69d32f310	4643	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
nikapov	0:a1a69d32f310	4644	sequence_config_new &= 0xbf;
nikapov	0:a1a69d32f310	4645	break;
nikapov	0:a1a69d32f310	4646	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
nikapov	0:a1a69d32f310	4647	sequence_config_new &= 0x7f;
nikapov	0:a1a69d32f310	4648	break;
nikapov	0:a1a69d32f310	4649	default:
nikapov	0:a1a69d32f310	4650	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4651	}
nikapov	0:a1a69d32f310	4652	}
nikapov	0:a1a69d32f310	4653	}
nikapov	0:a1a69d32f310	4654
nikapov	0:a1a69d32f310	4655	if (sequence_config_new != sequence_config) {
nikapov	0:a1a69d32f310	4656	/* Apply New Setting */
nikapov	0:a1a69d32f310	4657	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4658	status = VL53L0X_write_byte(dev,
nikapov	0:a1a69d32f310	4659	VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new);
nikapov	0:a1a69d32f310	4660	}
nikapov	0:a1a69d32f310	4661	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4662	PALDevDataSet(dev, SequenceConfig, sequence_config_new);
nikapov	0:a1a69d32f310	4663	}
nikapov	0:a1a69d32f310	4664
nikapov	0:a1a69d32f310	4665
nikapov	0:a1a69d32f310	4666	/* Recalculate timing budget */
nikapov	0:a1a69d32f310	4667	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4668	VL53L0X_GETPARAMETERFIELD(dev,
nikapov	0:a1a69d32f310	4669	MeasurementTimingBudgetMicroSeconds,
nikapov	0:a1a69d32f310	4670	measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4671
nikapov	0:a1a69d32f310	4672	VL53L0X_set_measurement_timing_budget_micro_seconds(dev,
nikapov	0:a1a69d32f310	4673	measurement_timing_budget_micro_seconds);
nikapov	0:a1a69d32f310	4674	}
nikapov	0:a1a69d32f310	4675	}
nikapov	0:a1a69d32f310	4676
nikapov	0:a1a69d32f310	4677	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4678
nikapov	0:a1a69d32f310	4679	return status;
nikapov	0:a1a69d32f310	4680	}
nikapov	0:a1a69d32f310	4681
nikapov	0:a1a69d32f310	4682	VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id,
nikapov	0:a1a69d32f310	4683	uint8_t limit_check_enable)
nikapov	0:a1a69d32f310	4684	{
nikapov	0:a1a69d32f310	4685	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4686	FixPoint1616_t temp_fix1616 = 0;
nikapov	0:a1a69d32f310	4687	uint8_t limit_check_enable_int = 0;
nikapov	0:a1a69d32f310	4688	uint8_t limit_check_disable = 0;
nikapov	0:a1a69d32f310	4689	uint8_t temp8;
nikapov	0:a1a69d32f310	4690
nikapov	0:a1a69d32f310	4691	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4692
nikapov	0:a1a69d32f310	4693	if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
nikapov	0:a1a69d32f310	4694	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4695	} else {
nikapov	0:a1a69d32f310	4696	if (limit_check_enable == 0) {
nikapov	0:a1a69d32f310	4697	temp_fix1616 = 0;
nikapov	0:a1a69d32f310	4698	limit_check_enable_int = 0;
nikapov	0:a1a69d32f310	4699	limit_check_disable = 1;
nikapov	0:a1a69d32f310	4700
nikapov	0:a1a69d32f310	4701	} else {
nikapov	0:a1a69d32f310	4702	VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
nikapov	0:a1a69d32f310	4703	limit_check_id, temp_fix1616);
nikapov	0:a1a69d32f310	4704	limit_check_disable = 0;
nikapov	0:a1a69d32f310	4705	/* this to be sure to have either 0 or 1 */
nikapov	0:a1a69d32f310	4706	limit_check_enable_int = 1;
nikapov	0:a1a69d32f310	4707	}
nikapov	0:a1a69d32f310	4708
nikapov	0:a1a69d32f310	4709	switch (limit_check_id) {
nikapov	0:a1a69d32f310	4710
nikapov	0:a1a69d32f310	4711	case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
nikapov	0:a1a69d32f310	4712	/* internal computation: */
nikapov	0:a1a69d32f310	4713	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov	0:a1a69d32f310	4714	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
nikapov	0:a1a69d32f310	4715	limit_check_enable_int);
nikapov	0:a1a69d32f310	4716
nikapov	0:a1a69d32f310	4717	break;
nikapov	0:a1a69d32f310	4718
nikapov	0:a1a69d32f310	4719	case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
nikapov	0:a1a69d32f310	4720
nikapov	0:a1a69d32f310	4721	status = VL53L0X_write_word(dev,
nikapov	0:a1a69d32f310	4722	VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
nikapov	0:a1a69d32f310	4723	VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616));
nikapov	0:a1a69d32f310	4724
nikapov	0:a1a69d32f310	4725	break;
nikapov	0:a1a69d32f310	4726
nikapov	0:a1a69d32f310	4727	case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
nikapov	0:a1a69d32f310	4728
nikapov	0:a1a69d32f310	4729	/* internal computation: */
nikapov	0:a1a69d32f310	4730	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov	0:a1a69d32f310	4731	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
nikapov	0:a1a69d32f310	4732	limit_check_enable_int);
nikapov	0:a1a69d32f310	4733
nikapov	0:a1a69d32f310	4734	break;
nikapov	0:a1a69d32f310	4735
nikapov	0:a1a69d32f310	4736	case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
nikapov	0:a1a69d32f310	4737
nikapov	0:a1a69d32f310	4738	/* internal computation: */
nikapov	0:a1a69d32f310	4739	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov	0:a1a69d32f310	4740	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
nikapov	0:a1a69d32f310	4741	limit_check_enable_int);
nikapov	0:a1a69d32f310	4742
nikapov	0:a1a69d32f310	4743	break;
nikapov	0:a1a69d32f310	4744
nikapov	0:a1a69d32f310	4745	case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
nikapov	0:a1a69d32f310	4746
nikapov	0:a1a69d32f310	4747	temp8 = (uint8_t)(limit_check_disable << 1);
nikapov	0:a1a69d32f310	4748	status = VL53L0X_update_byte(dev,
nikapov	0:a1a69d32f310	4749	VL53L0X_REG_MSRC_CONFIG_CONTROL,
nikapov	0:a1a69d32f310	4750	0xFE, temp8);
nikapov	0:a1a69d32f310	4751
nikapov	0:a1a69d32f310	4752	break;
nikapov	0:a1a69d32f310	4753
nikapov	0:a1a69d32f310	4754	case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
nikapov	0:a1a69d32f310	4755
nikapov	0:a1a69d32f310	4756	temp8 = (uint8_t)(limit_check_disable << 4);
nikapov	0:a1a69d32f310	4757	status = VL53L0X_update_byte(dev,
nikapov	0:a1a69d32f310	4758	VL53L0X_REG_MSRC_CONFIG_CONTROL,
nikapov	0:a1a69d32f310	4759	0xEF, temp8);
nikapov	0:a1a69d32f310	4760
nikapov	0:a1a69d32f310	4761	break;
nikapov	0:a1a69d32f310	4762
nikapov	0:a1a69d32f310	4763
nikapov	0:a1a69d32f310	4764	default:
nikapov	0:a1a69d32f310	4765	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	4766
nikapov	0:a1a69d32f310	4767	}
nikapov	0:a1a69d32f310	4768
nikapov	0:a1a69d32f310	4769	}
nikapov	0:a1a69d32f310	4770
nikapov	0:a1a69d32f310	4771	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4772	if (limit_check_enable == 0) {
nikapov	0:a1a69d32f310	4773	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov	0:a1a69d32f310	4774	limit_check_id, 0);
nikapov	0:a1a69d32f310	4775	} else {
nikapov	0:a1a69d32f310	4776	VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
nikapov	0:a1a69d32f310	4777	limit_check_id, 1);
nikapov	0:a1a69d32f310	4778	}
nikapov	0:a1a69d32f310	4779	}
nikapov	0:a1a69d32f310	4780
nikapov	0:a1a69d32f310	4781	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4782	return status;
nikapov	0:a1a69d32f310	4783	}
nikapov	0:a1a69d32f310	4784
nikapov	0:a1a69d32f310	4785	VL53L0X_Error VL53L0X::VL53L0X_static_init(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	4786	{
nikapov	0:a1a69d32f310	4787	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4788	VL53L0X_DeviceParameters_t current_parameters = {0};
nikapov	0:a1a69d32f310	4789	uint8_t *p_tuning_setting_buffer;
nikapov	0:a1a69d32f310	4790	uint16_t tempword = 0;
nikapov	0:a1a69d32f310	4791	uint8_t tempbyte = 0;
nikapov	0:a1a69d32f310	4792	uint8_t use_internal_tuning_settings = 0;
nikapov	0:a1a69d32f310	4793	uint32_t count = 0;
nikapov	0:a1a69d32f310	4794	uint8_t is_aperture_spads = 0;
nikapov	0:a1a69d32f310	4795	uint32_t ref_spad_count = 0;
nikapov	0:a1a69d32f310	4796	uint8_t aperture_spads = 0;
nikapov	0:a1a69d32f310	4797	uint8_t vcsel_pulse_period_pclk;
nikapov	0:a1a69d32f310	4798	uint32_t seq_timeout_micro_secs;
nikapov	0:a1a69d32f310	4799
nikapov	0:a1a69d32f310	4800	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4801
nikapov	0:a1a69d32f310	4802	status = VL53L0X_get_info_from_device(dev, 1);
nikapov	0:a1a69d32f310	4803
nikapov	0:a1a69d32f310	4804	/* set the ref spad from NVM */
nikapov	0:a1a69d32f310	4805	count = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4806	ReferenceSpadCount);
nikapov	0:a1a69d32f310	4807	aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
nikapov	0:a1a69d32f310	4808	ReferenceSpadType);
nikapov	0:a1a69d32f310	4809
nikapov	0:a1a69d32f310	4810	/* NVM value invalid */
nikapov	0:a1a69d32f310	4811	if ((aperture_spads > 1) \|\|
nikapov	0:a1a69d32f310	4812	((aperture_spads == 1) && (count > 32)) \|\|
nikapov	0:a1a69d32f310	4813	((aperture_spads == 0) && (count > 12))) {
nikapov	0:a1a69d32f310	4814	status = wrapped_VL53L0X_perform_ref_spad_management(dev, &ref_spad_count,
nikapov	0:a1a69d32f310	4815	&is_aperture_spads);
nikapov	0:a1a69d32f310	4816	} else {
nikapov	0:a1a69d32f310	4817	status = VL53L0X_set_reference_spads(dev, count, aperture_spads);
nikapov	0:a1a69d32f310	4818	}
nikapov	0:a1a69d32f310	4819
nikapov	0:a1a69d32f310	4820
nikapov	0:a1a69d32f310	4821	/* Initialize tuning settings buffer to prevent compiler warning. */
nikapov	0:a1a69d32f310	4822	p_tuning_setting_buffer = DefaultTuningSettings;
nikapov	0:a1a69d32f310	4823
nikapov	0:a1a69d32f310	4824	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4825	use_internal_tuning_settings = PALDevDataGet(dev,
nikapov	0:a1a69d32f310	4826	UseInternalTuningSettings);
nikapov	0:a1a69d32f310	4827
nikapov	0:a1a69d32f310	4828	if (use_internal_tuning_settings == 0) {
nikapov	0:a1a69d32f310	4829	p_tuning_setting_buffer = PALDevDataGet(dev,
nikapov	0:a1a69d32f310	4830	pTuningSettingsPointer);
nikapov	0:a1a69d32f310	4831	} else {
nikapov	0:a1a69d32f310	4832	p_tuning_setting_buffer = DefaultTuningSettings;
nikapov	0:a1a69d32f310	4833	}
nikapov	0:a1a69d32f310	4834
nikapov	0:a1a69d32f310	4835	}
nikapov	0:a1a69d32f310	4836
nikapov	0:a1a69d32f310	4837	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4838	status = VL53L0X_load_tuning_settings(dev, p_tuning_setting_buffer);
nikapov	0:a1a69d32f310	4839	}
nikapov	0:a1a69d32f310	4840
nikapov	0:a1a69d32f310	4841
nikapov	0:a1a69d32f310	4842	/* Set interrupt config to new sample ready */
nikapov	0:a1a69d32f310	4843	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4844	status = VL53L0X_set_gpio_config(dev, 0, 0,
nikapov	0:a1a69d32f310	4845	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov	0:a1a69d32f310	4846	VL53L0X_INTERRUPTPOLARITY_LOW);
nikapov	0:a1a69d32f310	4847	}
nikapov	0:a1a69d32f310	4848
nikapov	0:a1a69d32f310	4849	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4850	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	4851	status \|= VL53L0X_read_word(dev, 0x84, &tempword);
nikapov	0:a1a69d32f310	4852	status \|= VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	4853	}
nikapov	0:a1a69d32f310	4854
nikapov	0:a1a69d32f310	4855	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4856	VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz,
nikapov	0:a1a69d32f310	4857	VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword));
nikapov	0:a1a69d32f310	4858	}
nikapov	0:a1a69d32f310	4859
nikapov	0:a1a69d32f310	4860	/* After static init, some device parameters may be changed,
nikapov	0:a1a69d32f310	4861	* so update them */
nikapov	0:a1a69d32f310	4862	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4863	status = VL53L0X_get_device_parameters(dev, &current_parameters);
nikapov	0:a1a69d32f310	4864	}
nikapov	0:a1a69d32f310	4865
nikapov	0:a1a69d32f310	4866
nikapov	0:a1a69d32f310	4867	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4868	status = VL53L0X_get_fraction_enable(dev, &tempbyte);
Davidroid	3:e9269ff624ed	4869	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4870	PALDevDataSet(dev, RangeFractionalEnable, tempbyte);
Davidroid	3:e9269ff624ed	4871	}
nikapov	0:a1a69d32f310	4872
nikapov	0:a1a69d32f310	4873	}
nikapov	0:a1a69d32f310	4874
nikapov	0:a1a69d32f310	4875	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4876	PALDevDataSet(dev, CurrentParameters, current_parameters);
nikapov	0:a1a69d32f310	4877	}
nikapov	0:a1a69d32f310	4878
nikapov	0:a1a69d32f310	4879
nikapov	0:a1a69d32f310	4880	/* read the sequence config and save it */
nikapov	0:a1a69d32f310	4881	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4882	status = VL53L0X_read_byte(dev,
nikapov	0:a1a69d32f310	4883	VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
nikapov	0:a1a69d32f310	4884	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4885	PALDevDataSet(dev, SequenceConfig, tempbyte);
nikapov	0:a1a69d32f310	4886	}
nikapov	0:a1a69d32f310	4887	}
nikapov	0:a1a69d32f310	4888
nikapov	0:a1a69d32f310	4889	/* Disable MSRC and TCC by default */
nikapov	0:a1a69d32f310	4890	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4891	status = VL53L0X_set_sequence_step_enable(dev,
nikapov	0:a1a69d32f310	4892	VL53L0X_SEQUENCESTEP_TCC, 0);
Davidroid	2:d07edeaff6f1	4893	}
nikapov	0:a1a69d32f310	4894
nikapov	0:a1a69d32f310	4895	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4896	status = VL53L0X_set_sequence_step_enable(dev,
nikapov	0:a1a69d32f310	4897	VL53L0X_SEQUENCESTEP_MSRC, 0);
Davidroid	2:d07edeaff6f1	4898	}
nikapov	0:a1a69d32f310	4899
nikapov	0:a1a69d32f310	4900	/* Set PAL State to standby */
nikapov	0:a1a69d32f310	4901	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4902	PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
nikapov	0:a1a69d32f310	4903	}
nikapov	0:a1a69d32f310	4904
nikapov	0:a1a69d32f310	4905	/* Store pre-range vcsel period */
nikapov	0:a1a69d32f310	4906	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4907	status = VL53L0X_get_vcsel_pulse_period(
nikapov	0:a1a69d32f310	4908	dev,
nikapov	0:a1a69d32f310	4909	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
nikapov	0:a1a69d32f310	4910	&vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	4911	}
nikapov	0:a1a69d32f310	4912
nikapov	0:a1a69d32f310	4913	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4914	VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	4915	dev,
nikapov	0:a1a69d32f310	4916	PreRangeVcselPulsePeriod,
nikapov	0:a1a69d32f310	4917	vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	4918	}
nikapov	0:a1a69d32f310	4919
nikapov	0:a1a69d32f310	4920	/* Store final-range vcsel period */
nikapov	0:a1a69d32f310	4921	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4922	status = VL53L0X_get_vcsel_pulse_period(
nikapov	0:a1a69d32f310	4923	dev,
nikapov	0:a1a69d32f310	4924	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
nikapov	0:a1a69d32f310	4925	&vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	4926	}
nikapov	0:a1a69d32f310	4927
nikapov	0:a1a69d32f310	4928	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4929	VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	4930	dev,
nikapov	0:a1a69d32f310	4931	FinalRangeVcselPulsePeriod,
nikapov	0:a1a69d32f310	4932	vcsel_pulse_period_pclk);
nikapov	0:a1a69d32f310	4933	}
nikapov	0:a1a69d32f310	4934
nikapov	0:a1a69d32f310	4935	/* Store pre-range timeout */
nikapov	0:a1a69d32f310	4936	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4937	status = get_sequence_step_timeout(
nikapov	0:a1a69d32f310	4938	dev,
nikapov	0:a1a69d32f310	4939	VL53L0X_SEQUENCESTEP_PRE_RANGE,
nikapov	0:a1a69d32f310	4940	&seq_timeout_micro_secs);
nikapov	0:a1a69d32f310	4941	}
nikapov	0:a1a69d32f310	4942
nikapov	0:a1a69d32f310	4943	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4944	VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	4945	dev,
nikapov	0:a1a69d32f310	4946	PreRangeTimeoutMicroSecs,
nikapov	0:a1a69d32f310	4947	seq_timeout_micro_secs);
nikapov	0:a1a69d32f310	4948	}
nikapov	0:a1a69d32f310	4949
nikapov	0:a1a69d32f310	4950	/* Store final-range timeout */
nikapov	0:a1a69d32f310	4951	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4952	status = get_sequence_step_timeout(
nikapov	0:a1a69d32f310	4953	dev,
nikapov	0:a1a69d32f310	4954	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
nikapov	0:a1a69d32f310	4955	&seq_timeout_micro_secs);
nikapov	0:a1a69d32f310	4956	}
nikapov	0:a1a69d32f310	4957
nikapov	0:a1a69d32f310	4958	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4959	VL53L0X_SETDEVICESPECIFICPARAMETER(
nikapov	0:a1a69d32f310	4960	dev,
nikapov	0:a1a69d32f310	4961	FinalRangeTimeoutMicroSecs,
nikapov	0:a1a69d32f310	4962	seq_timeout_micro_secs);
nikapov	0:a1a69d32f310	4963	}
nikapov	0:a1a69d32f310	4964
nikapov	0:a1a69d32f310	4965	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4966	return status;
nikapov	0:a1a69d32f310	4967	}
nikapov	0:a1a69d32f310	4968
nikapov	0:a1a69d32f310	4969
nikapov	0:a1a69d32f310	4970	VL53L0X_Error VL53L0X::VL53L0X_stop_measurement(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	4971	{
nikapov	0:a1a69d32f310	4972	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	4973	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	4974
nikapov	0:a1a69d32f310	4975	status = VL53L0X_write_byte(dev, VL53L0X_REG_SYSRANGE_START,
nikapov	0:a1a69d32f310	4976	VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
nikapov	0:a1a69d32f310	4977
nikapov	0:a1a69d32f310	4978	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	4979	status = VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov	0:a1a69d32f310	4980	status = VL53L0X_write_byte(dev, 0x91, 0x00);
nikapov	0:a1a69d32f310	4981	status = VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov	0:a1a69d32f310	4982	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	4983
nikapov	0:a1a69d32f310	4984	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4985	/* Set PAL State to Idle */
nikapov	0:a1a69d32f310	4986	PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
nikapov	0:a1a69d32f310	4987	}
nikapov	0:a1a69d32f310	4988
nikapov	0:a1a69d32f310	4989	/* Check if need to apply interrupt settings */
nikapov	0:a1a69d32f310	4990	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	4991	status = VL53L0X_check_and_load_interrupt_settings(dev, 0);
nikapov	0:a1a69d32f310	4992	}
nikapov	0:a1a69d32f310	4993
nikapov	0:a1a69d32f310	4994	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	4995	return status;
nikapov	0:a1a69d32f310	4996	}
nikapov	0:a1a69d32f310	4997
nikapov	0:a1a69d32f310	4998	VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(VL53L0X_DEV dev,
nikapov	0:a1a69d32f310	4999	uint32_t *p_stop_status)
nikapov	0:a1a69d32f310	5000	{
nikapov	0:a1a69d32f310	5001	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5002	uint8_t byte = 0;
nikapov	0:a1a69d32f310	5003	LOG_FUNCTION_START("");
nikapov	0:a1a69d32f310	5004
nikapov	0:a1a69d32f310	5005	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	5006
nikapov	0:a1a69d32f310	5007	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5008	status = VL53L0X_read_byte(dev, 0x04, &byte);
nikapov	0:a1a69d32f310	5009	}
nikapov	0:a1a69d32f310	5010
nikapov	0:a1a69d32f310	5011	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5012	status = VL53L0X_write_byte(dev, 0xFF, 0x0);
nikapov	0:a1a69d32f310	5013	}
nikapov	0:a1a69d32f310	5014
nikapov	0:a1a69d32f310	5015	*p_stop_status = byte;
nikapov	0:a1a69d32f310	5016
nikapov	0:a1a69d32f310	5017	if (byte == 0) {
nikapov	0:a1a69d32f310	5018	status = VL53L0X_write_byte(dev, 0x80, 0x01);
nikapov	0:a1a69d32f310	5019	status = VL53L0X_write_byte(dev, 0xFF, 0x01);
nikapov	0:a1a69d32f310	5020	status = VL53L0X_write_byte(dev, 0x00, 0x00);
nikapov	0:a1a69d32f310	5021	status = VL53L0X_write_byte(dev, 0x91,
nikapov	0:a1a69d32f310	5022	PALDevDataGet(dev, StopVariable));
nikapov	0:a1a69d32f310	5023	status = VL53L0X_write_byte(dev, 0x00, 0x01);
nikapov	0:a1a69d32f310	5024	status = VL53L0X_write_byte(dev, 0xFF, 0x00);
nikapov	0:a1a69d32f310	5025	status = VL53L0X_write_byte(dev, 0x80, 0x00);
nikapov	0:a1a69d32f310	5026	}
nikapov	0:a1a69d32f310	5027
nikapov	0:a1a69d32f310	5028	LOG_FUNCTION_END(status);
nikapov	0:a1a69d32f310	5029	return status;
nikapov	0:a1a69d32f310	5030	}
nikapov	0:a1a69d32f310	5031
nikapov	0:a1a69d32f310	5032	/**************** Write and read functions from I2C ***********************/
nikapov	0:a1a69d32f310	5033
nikapov	0:a1a69d32f310	5034	VL53L0X_Error VL53L0X::VL53L0X_write_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count)
nikapov	0:a1a69d32f310	5035	{
nikapov	0:a1a69d32f310	5036	int status;
nikapov	0:a1a69d32f310	5037
nikapov	0:a1a69d32f310	5038	status = VL53L0X_i2c_write(dev->I2cDevAddr, index, p_data, (uint16_t)count);
nikapov	0:a1a69d32f310	5039	return status;
nikapov	0:a1a69d32f310	5040	}
nikapov	0:a1a69d32f310	5041
nikapov	0:a1a69d32f310	5042	VL53L0X_Error VL53L0X::VL53L0X_read_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count)
nikapov	0:a1a69d32f310	5043	{
nikapov	0:a1a69d32f310	5044	int status;
nikapov	0:a1a69d32f310	5045
nikapov	0:a1a69d32f310	5046	if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
nikapov	0:a1a69d32f310	5047	status = VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	5048	}
nikapov	0:a1a69d32f310	5049
nikapov	0:a1a69d32f310	5050	status = VL53L0X_i2c_read(dev->I2cDevAddr, index, p_data, (uint16_t)count);
nikapov	0:a1a69d32f310	5051
nikapov	0:a1a69d32f310	5052	return status;
nikapov	0:a1a69d32f310	5053	}
nikapov	0:a1a69d32f310	5054
nikapov	0:a1a69d32f310	5055
nikapov	0:a1a69d32f310	5056	VL53L0X_Error VL53L0X::VL53L0X_write_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
nikapov	0:a1a69d32f310	5057	{
nikapov	0:a1a69d32f310	5058	int status;
nikapov	0:a1a69d32f310	5059
nikapov	0:a1a69d32f310	5060	status = VL53L0X_i2c_write(Dev->I2cDevAddr, index, &data, 1);
nikapov	0:a1a69d32f310	5061	return status;
nikapov	0:a1a69d32f310	5062	}
nikapov	0:a1a69d32f310	5063
nikapov	0:a1a69d32f310	5064	VL53L0X_Error VL53L0X::VL53L0X_write_word(VL53L0X_DEV dev, uint8_t index, uint16_t data)
nikapov	0:a1a69d32f310	5065	{
nikapov	0:a1a69d32f310	5066	int status;
nikapov	0:a1a69d32f310	5067	uint8_t buffer[2];
nikapov	0:a1a69d32f310	5068
nikapov	0:a1a69d32f310	5069	buffer[0] = data >> 8;
nikapov	0:a1a69d32f310	5070	buffer[1] = data & 0x00FF;
nikapov	0:a1a69d32f310	5071	status = VL53L0X_i2c_write(dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
nikapov	0:a1a69d32f310	5072	return status;
nikapov	0:a1a69d32f310	5073	}
nikapov	0:a1a69d32f310	5074
nikapov	0:a1a69d32f310	5075	VL53L0X_Error VL53L0X::VL53L0X_write_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
nikapov	0:a1a69d32f310	5076	{
nikapov	0:a1a69d32f310	5077	int status;
nikapov	0:a1a69d32f310	5078	uint8_t buffer[4];
nikapov	0:a1a69d32f310	5079
nikapov	0:a1a69d32f310	5080	buffer[0] = (data >> 24) & 0xFF;
nikapov	0:a1a69d32f310	5081	buffer[1] = (data >> 16) & 0xFF;
nikapov	0:a1a69d32f310	5082	buffer[2] = (data >> 8) & 0xFF;
nikapov	0:a1a69d32f310	5083	buffer[3] = (data >> 0) & 0xFF;
nikapov	0:a1a69d32f310	5084	status = VL53L0X_i2c_write(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
nikapov	0:a1a69d32f310	5085	return status;
nikapov	0:a1a69d32f310	5086	}
nikapov	0:a1a69d32f310	5087
nikapov	0:a1a69d32f310	5088
nikapov	0:a1a69d32f310	5089	VL53L0X_Error VL53L0X::VL53L0X_read_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t *p_data)
nikapov	0:a1a69d32f310	5090	{
nikapov	0:a1a69d32f310	5091	int status;
nikapov	0:a1a69d32f310	5092
nikapov	0:a1a69d32f310	5093	status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, p_data, 1);
nikapov	0:a1a69d32f310	5094
Davidroid	3:e9269ff624ed	5095	if (status) {
nikapov	0:a1a69d32f310	5096	return -1;
Davidroid	3:e9269ff624ed	5097	}
nikapov	0:a1a69d32f310	5098
nikapov	0:a1a69d32f310	5099	return 0;
nikapov	0:a1a69d32f310	5100	}
nikapov	0:a1a69d32f310	5101
nikapov	0:a1a69d32f310	5102	VL53L0X_Error VL53L0X::VL53L0X_read_word(VL53L0X_DEV Dev, uint8_t index, uint16_t *p_data)
nikapov	0:a1a69d32f310	5103	{
nikapov	0:a1a69d32f310	5104	int status;
nikapov	0:a1a69d32f310	5105	uint8_t buffer[2] = {0, 0};
nikapov	0:a1a69d32f310	5106
nikapov	0:a1a69d32f310	5107	status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, buffer, 2);
nikapov	0:a1a69d32f310	5108	if (!status) {
nikapov	0:a1a69d32f310	5109	*p_data = (buffer[0] << 8) + buffer[1];
nikapov	0:a1a69d32f310	5110	}
nikapov	0:a1a69d32f310	5111	return status;
nikapov	0:a1a69d32f310	5112
nikapov	0:a1a69d32f310	5113	}
nikapov	0:a1a69d32f310	5114
nikapov	0:a1a69d32f310	5115	VL53L0X_Error VL53L0X::VL53L0X_read_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t *p_data)
nikapov	0:a1a69d32f310	5116	{
nikapov	0:a1a69d32f310	5117	int status;
nikapov	0:a1a69d32f310	5118	uint8_t buffer[4] = {0, 0, 0, 0};
nikapov	0:a1a69d32f310	5119
nikapov	0:a1a69d32f310	5120	status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, buffer, 4);
nikapov	0:a1a69d32f310	5121	if (!status) {
nikapov	0:a1a69d32f310	5122	*p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
nikapov	0:a1a69d32f310	5123	}
nikapov	0:a1a69d32f310	5124	return status;
nikapov	0:a1a69d32f310	5125
nikapov	0:a1a69d32f310	5126	}
nikapov	0:a1a69d32f310	5127
nikapov	0:a1a69d32f310	5128	VL53L0X_Error VL53L0X::VL53L0X_update_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t and_data, uint8_t or_data)
nikapov	0:a1a69d32f310	5129	{
nikapov	0:a1a69d32f310	5130	int status;
nikapov	0:a1a69d32f310	5131	uint8_t buffer = 0;
nikapov	0:a1a69d32f310	5132
nikapov	0:a1a69d32f310	5133	/* read data direct onto buffer */
nikapov	0:a1a69d32f310	5134	status = VL53L0X_i2c_read(Dev->I2cDevAddr, index, &buffer, 1);
nikapov	0:a1a69d32f310	5135	if (!status) {
nikapov	0:a1a69d32f310	5136	buffer = (buffer & and_data) \| or_data;
nikapov	0:a1a69d32f310	5137	status = VL53L0X_i2c_write(Dev->I2cDevAddr, index, &buffer, (uint8_t)1);
nikapov	0:a1a69d32f310	5138	}
nikapov	0:a1a69d32f310	5139	return status;
nikapov	0:a1a69d32f310	5140	}
nikapov	0:a1a69d32f310	5141
nikapov	0:a1a69d32f310	5142	VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
nikapov	0:a1a69d32f310	5143	uint16_t NumByteToWrite)
nikapov	0:a1a69d32f310	5144	{
nikapov	0:a1a69d32f310	5145	int ret;
kenjiArai	5:ac6604b75d50	5146	#if ORIGINAL
nikapov	0:a1a69d32f310	5147	ret = _dev_i2c->i2c_write(p_data, DeviceAddr, RegisterAddr, NumByteToWrite);
kenjiArai	5:ac6604b75d50	5148	#else
kenjiArai	5:ac6604b75d50	5149	ret = i2c_write(p_data, DeviceAddr, RegisterAddr, NumByteToWrite);
kenjiArai	5:ac6604b75d50	5150	#endif
nikapov	0:a1a69d32f310	5151
nikapov	0:a1a69d32f310	5152	if (ret) {
nikapov	0:a1a69d32f310	5153	return -1;
nikapov	0:a1a69d32f310	5154	}
nikapov	0:a1a69d32f310	5155	return 0;
nikapov	0:a1a69d32f310	5156	}
nikapov	0:a1a69d32f310	5157
nikapov	0:a1a69d32f310	5158	VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
nikapov	0:a1a69d32f310	5159	uint16_t NumByteToRead)
nikapov	0:a1a69d32f310	5160	{
nikapov	0:a1a69d32f310	5161	int ret;
nikapov	0:a1a69d32f310	5162
kenjiArai	5:ac6604b75d50	5163	#if ORIGINAL
nikapov	0:a1a69d32f310	5164	ret = _dev_i2c->i2c_read(p_data, DeviceAddr, RegisterAddr, NumByteToRead);
kenjiArai	5:ac6604b75d50	5165	#else
kenjiArai	5:ac6604b75d50	5166	ret = i2c_read(p_data, DeviceAddr, RegisterAddr, NumByteToRead);
kenjiArai	5:ac6604b75d50	5167	#endif
nikapov	0:a1a69d32f310	5168
nikapov	0:a1a69d32f310	5169	if (ret) {
nikapov	0:a1a69d32f310	5170	return -1;
nikapov	0:a1a69d32f310	5171	}
nikapov	0:a1a69d32f310	5172	return 0;
nikapov	0:a1a69d32f310	5173	}
nikapov	0:a1a69d32f310	5174
nikapov	0:a1a69d32f310	5175	int VL53L0X::read_id(uint8_t *id)
nikapov	0:a1a69d32f310	5176	{
nikapov	0:a1a69d32f310	5177	int status = 0;
nikapov	0:a1a69d32f310	5178	uint16_t rl_id = 0;
nikapov	0:a1a69d32f310	5179
nikapov	0:a1a69d32f310	5180	status = VL53L0X_read_word(_device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id);
nikapov	0:a1a69d32f310	5181	if (rl_id == 0xEEAA) {
nikapov	0:a1a69d32f310	5182	return status;
nikapov	0:a1a69d32f310	5183	}
nikapov	0:a1a69d32f310	5184
nikapov	0:a1a69d32f310	5185	return -1;
nikapov	0:a1a69d32f310	5186	}
nikapov	0:a1a69d32f310	5187
nikapov	0:a1a69d32f310	5188
nikapov	0:a1a69d32f310	5189	VL53L0X_Error VL53L0X::wait_measurement_data_ready(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	5190	{
nikapov	0:a1a69d32f310	5191	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5192	uint8_t new_dat_ready = 0;
nikapov	0:a1a69d32f310	5193	uint32_t loop_nb;
nikapov	0:a1a69d32f310	5194
nikapov	0:a1a69d32f310	5195	// Wait until it finished
nikapov	0:a1a69d32f310	5196	// use timeout to avoid deadlock
nikapov	0:a1a69d32f310	5197	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5198	loop_nb = 0;
nikapov	0:a1a69d32f310	5199	do {
nikapov	0:a1a69d32f310	5200	status = VL53L0X_get_measurement_data_ready(dev, &new_dat_ready);
nikapov	0:a1a69d32f310	5201	if ((new_dat_ready == 0x01) \|\| status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5202	break;
nikapov	0:a1a69d32f310	5203	}
nikapov	0:a1a69d32f310	5204	loop_nb = loop_nb + 1;
nikapov	0:a1a69d32f310	5205	VL53L0X_polling_delay(dev);
nikapov	0:a1a69d32f310	5206	} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov	0:a1a69d32f310	5207
nikapov	0:a1a69d32f310	5208	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	5209	status = VL53L0X_ERROR_TIME_OUT;
nikapov	0:a1a69d32f310	5210	}
nikapov	0:a1a69d32f310	5211	}
nikapov	0:a1a69d32f310	5212
nikapov	0:a1a69d32f310	5213	return status;
nikapov	0:a1a69d32f310	5214	}
nikapov	0:a1a69d32f310	5215
nikapov	0:a1a69d32f310	5216	VL53L0X_Error VL53L0X::wait_stop_completed(VL53L0X_DEV dev)
nikapov	0:a1a69d32f310	5217	{
nikapov	0:a1a69d32f310	5218	VL53L0X_Error status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5219	uint32_t stop_completed = 0;
nikapov	0:a1a69d32f310	5220	uint32_t loop_nb;
nikapov	0:a1a69d32f310	5221
nikapov	0:a1a69d32f310	5222	// Wait until it finished
nikapov	0:a1a69d32f310	5223	// use timeout to avoid deadlock
nikapov	0:a1a69d32f310	5224	if (status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5225	loop_nb = 0;
nikapov	0:a1a69d32f310	5226	do {
nikapov	0:a1a69d32f310	5227	status = VL53L0X_get_stop_completed_status(dev, &stop_completed);
nikapov	0:a1a69d32f310	5228	if ((stop_completed == 0x00) \|\| status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5229	break;
nikapov	0:a1a69d32f310	5230	}
nikapov	0:a1a69d32f310	5231	loop_nb = loop_nb + 1;
nikapov	0:a1a69d32f310	5232	VL53L0X_polling_delay(dev);
nikapov	0:a1a69d32f310	5233	} while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
nikapov	0:a1a69d32f310	5234
nikapov	0:a1a69d32f310	5235	if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
nikapov	0:a1a69d32f310	5236	status = VL53L0X_ERROR_TIME_OUT;
nikapov	0:a1a69d32f310	5237	}
nikapov	0:a1a69d32f310	5238
nikapov	0:a1a69d32f310	5239	}
nikapov	0:a1a69d32f310	5240
nikapov	0:a1a69d32f310	5241	return status;
nikapov	0:a1a69d32f310	5242	}
nikapov	0:a1a69d32f310	5243
nikapov	0:a1a69d32f310	5244
nikapov	0:a1a69d32f310	5245	int VL53L0X::init_sensor(uint8_t new_addr)
nikapov	0:a1a69d32f310	5246	{
nikapov	0:a1a69d32f310	5247	int status;
nikapov	0:a1a69d32f310	5248
nikapov	0:a1a69d32f310	5249	VL53L0X_off();
nikapov	0:a1a69d32f310	5250	VL53L0X_on();
nikapov	0:a1a69d32f310	5251
nikapov	0:a1a69d32f310	5252	// status=VL53L0X_WaitDeviceBooted(Device);
nikapov	0:a1a69d32f310	5253	// if(status)
nikapov	0:a1a69d32f310	5254	// printf("WaitDeviceBooted fail\n\r");
nikapov	0:a1a69d32f310	5255	status = is_present();
nikapov	0:a1a69d32f310	5256	if (!status) {
nikapov	0:a1a69d32f310	5257	status = init(&_my_device);
nikapov	0:a1a69d32f310	5258	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5259	printf("Failed to init VL53L0X sensor!\n\r");
nikapov	0:a1a69d32f310	5260	return status;
nikapov	0:a1a69d32f310	5261	}
nikapov	0:a1a69d32f310	5262
nikapov	0:a1a69d32f310	5263	// deduce silicon version
nikapov	0:a1a69d32f310	5264	status = VL53L0X_get_device_info(&_my_device, &_device_info);
nikapov	0:a1a69d32f310	5265
nikapov	0:a1a69d32f310	5266	status = prepare();
nikapov	0:a1a69d32f310	5267	if (status != VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5268	printf("Failed to prepare VL53L0X!\n\r");
nikapov	0:a1a69d32f310	5269	return status;
nikapov	0:a1a69d32f310	5270	}
nikapov	0:a1a69d32f310	5271
nikapov	1:834986cdde0a	5272	if (new_addr != VL53L0X_DEFAULT_ADDRESS) {
nikapov	0:a1a69d32f310	5273	status = set_device_address(new_addr);
nikapov	0:a1a69d32f310	5274	if (status) {
nikapov	0:a1a69d32f310	5275	printf("Failed to change I2C address!\n\r");
nikapov	0:a1a69d32f310	5276	return status;
nikapov	0:a1a69d32f310	5277	}
nikapov	0:a1a69d32f310	5278	} else {
kenjiArai	5:ac6604b75d50	5279	#if ORIGINAL
nikapov	0:a1a69d32f310	5280	printf("Invalid new address!\n\r");
kenjiArai	5:ac6604b75d50	5281	#endif
nikapov	0:a1a69d32f310	5282	return VL53L0X_ERROR_INVALID_PARAMS;
nikapov	0:a1a69d32f310	5283	}
nikapov	0:a1a69d32f310	5284	}
nikapov	0:a1a69d32f310	5285	return status;
nikapov	0:a1a69d32f310	5286	}
nikapov	0:a1a69d32f310	5287
nikapov	0:a1a69d32f310	5288	int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void))
nikapov	0:a1a69d32f310	5289	{
nikapov	0:a1a69d32f310	5290	int status, clr_status;
nikapov	0:a1a69d32f310	5291
nikapov	0:a1a69d32f310	5292	status = VL53L0X_stop_measurement(_device); // it is safer to do this while sensor is stopped
nikapov	0:a1a69d32f310	5293
nikapov	0:a1a69d32f310	5294	// status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
nikapov	0:a1a69d32f310	5295
nikapov	0:a1a69d32f310	5296	status = VL53L0X_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov	0:a1a69d32f310	5297	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov	0:a1a69d32f310	5298	VL53L0X_INTERRUPTPOLARITY_HIGH);
nikapov	0:a1a69d32f310	5299
nikapov	0:a1a69d32f310	5300	if (!status) {
nikapov	0:a1a69d32f310	5301	attach_interrupt_measure_detection_irq(fptr);
nikapov	0:a1a69d32f310	5302	enable_interrupt_measure_detection_irq();
nikapov	0:a1a69d32f310	5303	}
nikapov	0:a1a69d32f310	5304
nikapov	0:a1a69d32f310	5305	clr_status = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS \| VL53L0X_REG_RESULT_RANGE_STATUS);
nikapov	0:a1a69d32f310	5306	if (clr_status) {
nikapov	0:a1a69d32f310	5307	VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
nikapov	0:a1a69d32f310	5308	}
nikapov	0:a1a69d32f310	5309
nikapov	0:a1a69d32f310	5310	if (!status) {
nikapov	0:a1a69d32f310	5311	status = range_start_continuous_mode();
nikapov	0:a1a69d32f310	5312	}
nikapov	0:a1a69d32f310	5313	return status;
nikapov	0:a1a69d32f310	5314	}
nikapov	0:a1a69d32f310	5315
nikapov	0:a1a69d32f310	5316
nikapov	0:a1a69d32f310	5317	int VL53L0X::start_measurement(OperatingMode operating_mode, void (*fptr)(void))
nikapov	0:a1a69d32f310	5318	{
nikapov	0:a1a69d32f310	5319	int Status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5320	int ClrStatus;
nikapov	0:a1a69d32f310	5321
nikapov	0:a1a69d32f310	5322	uint8_t VhvSettings;
nikapov	0:a1a69d32f310	5323	uint8_t PhaseCal;
nikapov	0:a1a69d32f310	5324	// *** from mass market cube expansion v1.1, ranging with satellites.
nikapov	0:a1a69d32f310	5325	// default settings, for normal range.
nikapov	0:a1a69d32f310	5326	FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536);
nikapov	0:a1a69d32f310	5327	FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536);
nikapov	0:a1a69d32f310	5328	uint32_t timingBudget = 33000;
nikapov	0:a1a69d32f310	5329	uint8_t preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	5330	uint8_t finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	5331
nikapov	0:a1a69d32f310	5332	if (operating_mode == range_continuous_interrupt) {
nikapov	0:a1a69d32f310	5333	if (_gpio1Int == NULL) {
nikapov	0:a1a69d32f310	5334	printf("GPIO1 Error\r\n");
nikapov	0:a1a69d32f310	5335	return 1;
nikapov	0:a1a69d32f310	5336	}
nikapov	0:a1a69d32f310	5337
nikapov	0:a1a69d32f310	5338	Status = VL53L0X_stop_measurement(_device); // it is safer to do this while sensor is stopped
nikapov	0:a1a69d32f310	5339
nikapov	0:a1a69d32f310	5340	// Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
nikapov	0:a1a69d32f310	5341
nikapov	0:a1a69d32f310	5342	Status = VL53L0X_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
nikapov	0:a1a69d32f310	5343	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
nikapov	0:a1a69d32f310	5344	VL53L0X_INTERRUPTPOLARITY_HIGH);
nikapov	0:a1a69d32f310	5345
nikapov	0:a1a69d32f310	5346	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5347	attach_interrupt_measure_detection_irq(fptr);
nikapov	0:a1a69d32f310	5348	enable_interrupt_measure_detection_irq();
nikapov	0:a1a69d32f310	5349	}
nikapov	0:a1a69d32f310	5350
nikapov	0:a1a69d32f310	5351	ClrStatus = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS \| VL53L0X_REG_RESULT_RANGE_STATUS);
nikapov	0:a1a69d32f310	5352	if (ClrStatus) {
nikapov	0:a1a69d32f310	5353	VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
nikapov	0:a1a69d32f310	5354	}
nikapov	0:a1a69d32f310	5355
nikapov	0:a1a69d32f310	5356	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5357	Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
nikapov	0:a1a69d32f310	5358	}
nikapov	0:a1a69d32f310	5359
nikapov	0:a1a69d32f310	5360	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5361	Status = VL53L0X_start_measurement(_device);
nikapov	0:a1a69d32f310	5362	}
nikapov	0:a1a69d32f310	5363	}
nikapov	0:a1a69d32f310	5364
nikapov	0:a1a69d32f310	5365	if (operating_mode == range_single_shot_polling) {
nikapov	0:a1a69d32f310	5366	// singelshot, polled ranging
nikapov	0:a1a69d32f310	5367	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5368	// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
nikapov	0:a1a69d32f310	5369	Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
nikapov	0:a1a69d32f310	5370	}
nikapov	0:a1a69d32f310	5371
nikapov	0:a1a69d32f310	5372	// Enable/Disable Sigma and Signal check
nikapov	0:a1a69d32f310	5373	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5374	Status = VL53L0X_set_limit_check_enable(_device,
nikapov	0:a1a69d32f310	5375	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
nikapov	0:a1a69d32f310	5376	}
nikapov	0:a1a69d32f310	5377	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5378	Status = VL53L0X_set_limit_check_enable(_device,
nikapov	0:a1a69d32f310	5379	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
nikapov	0:a1a69d32f310	5380	}
nikapov	0:a1a69d32f310	5381
nikapov	0:a1a69d32f310	5382	// *** from mass market cube expansion v1.1, ranging with satellites.
kenjiArai	5:ac6604b75d50	5383	#if ORIGINAL
nikapov	0:a1a69d32f310	5384	/* Ranging configuration */
nikapov	0:a1a69d32f310	5385	//*
nikapov	0:a1a69d32f310	5386	// switch(rangingConfig) {
nikapov	0:a1a69d32f310	5387	// case LONG_RANGE:
nikapov	0:a1a69d32f310	5388	signalLimit = (FixPoint1616_t)(0.1 * 65536);
nikapov	0:a1a69d32f310	5389	sigmaLimit = (FixPoint1616_t)(60 * 65536);
nikapov	0:a1a69d32f310	5390	timingBudget = 33000;
nikapov	0:a1a69d32f310	5391	preRangeVcselPeriod = 18;
nikapov	0:a1a69d32f310	5392	finalRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	5393	/* break;
nikapov	0:a1a69d32f310	5394	case HIGH_ACCURACY:
nikapov	0:a1a69d32f310	5395	signalLimit = (FixPoint1616_t)(0.25*65536);
nikapov	0:a1a69d32f310	5396	sigmaLimit = (FixPoint1616_t)(18*65536);
nikapov	0:a1a69d32f310	5397	timingBudget = 200000;
nikapov	0:a1a69d32f310	5398	preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	5399	finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	5400	break;
nikapov	0:a1a69d32f310	5401	case HIGH_SPEED:
nikapov	0:a1a69d32f310	5402	signalLimit = (FixPoint1616_t)(0.25*65536);
nikapov	0:a1a69d32f310	5403	sigmaLimit = (FixPoint1616_t)(32*65536);
nikapov	0:a1a69d32f310	5404	timingBudget = 20000;
nikapov	0:a1a69d32f310	5405	preRangeVcselPeriod = 14;
nikapov	0:a1a69d32f310	5406	finalRangeVcselPeriod = 10;
nikapov	0:a1a69d32f310	5407	break;
nikapov	0:a1a69d32f310	5408	default:
nikapov	0:a1a69d32f310	5409	debug_printf("Not Supported");
nikapov	0:a1a69d32f310	5410	}
nikapov	0:a1a69d32f310	5411	*/
kenjiArai	5:ac6604b75d50	5412	#else
kenjiArai	5:ac6604b75d50	5413	/* Ranging configuration */
kenjiArai	5:ac6604b75d50	5414	switch(_range_mode) {
kenjiArai	5:ac6604b75d50	5415	case range_hi_accurate_200ms_120cm:
kenjiArai	5:ac6604b75d50	5416	signalLimit = (FixPoint1616_t)(0.25*65536);
kenjiArai	5:ac6604b75d50	5417	sigmaLimit = (FixPoint1616_t)(18*65536);
kenjiArai	5:ac6604b75d50	5418	timingBudget = 200000;
kenjiArai	5:ac6604b75d50	5419	preRangeVcselPeriod = 14;
kenjiArai	5:ac6604b75d50	5420	finalRangeVcselPeriod = 10;
kenjiArai	5:ac6604b75d50	5421	break;
kenjiArai	5:ac6604b75d50	5422	case range_hi_speed_20ms_120cm:
kenjiArai	5:ac6604b75d50	5423	signalLimit = (FixPoint1616_t)(0.25*65536);
kenjiArai	5:ac6604b75d50	5424	sigmaLimit = (FixPoint1616_t)(32*65536);
kenjiArai	5:ac6604b75d50	5425	timingBudget = 20000;
kenjiArai	5:ac6604b75d50	5426	preRangeVcselPeriod = 14;
kenjiArai	5:ac6604b75d50	5427	finalRangeVcselPeriod = 10;
kenjiArai	5:ac6604b75d50	5428	break;
kenjiArai	5:ac6604b75d50	5429	case range_long_distance_33ms_200cm:
kenjiArai	5:ac6604b75d50	5430	default:
kenjiArai	5:ac6604b75d50	5431	signalLimit = (FixPoint1616_t)(0.1 * 65536);
kenjiArai	5:ac6604b75d50	5432	sigmaLimit = (FixPoint1616_t)(60 * 65536);
kenjiArai	5:ac6604b75d50	5433	timingBudget = 33000;
kenjiArai	5:ac6604b75d50	5434	preRangeVcselPeriod = 18;
kenjiArai	5:ac6604b75d50	5435	finalRangeVcselPeriod = 14;
kenjiArai	5:ac6604b75d50	5436	}
kenjiArai	5:ac6604b75d50	5437	#endif
nikapov	0:a1a69d32f310	5438	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5439	Status = VL53L0X_set_limit_check_value(_device,
nikapov	0:a1a69d32f310	5440	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
nikapov	0:a1a69d32f310	5441	}
nikapov	0:a1a69d32f310	5442
nikapov	0:a1a69d32f310	5443	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5444	Status = VL53L0X_set_limit_check_value(_device,
nikapov	0:a1a69d32f310	5445	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
nikapov	0:a1a69d32f310	5446	}
nikapov	0:a1a69d32f310	5447
nikapov	0:a1a69d32f310	5448	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5449	Status = VL53L0X_set_measurement_timing_budget_micro_seconds(_device, timingBudget);
nikapov	0:a1a69d32f310	5450	}
nikapov	0:a1a69d32f310	5451
nikapov	0:a1a69d32f310	5452	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5453	Status = VL53L0X_set_vcsel_pulse_period(_device,
nikapov	0:a1a69d32f310	5454	VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
nikapov	0:a1a69d32f310	5455	}
nikapov	0:a1a69d32f310	5456
nikapov	0:a1a69d32f310	5457	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5458	Status = VL53L0X_set_vcsel_pulse_period(_device,
nikapov	0:a1a69d32f310	5459	VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
nikapov	0:a1a69d32f310	5460	}
nikapov	0:a1a69d32f310	5461
nikapov	0:a1a69d32f310	5462	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5463	Status = VL53L0X_perform_ref_calibration(_device, &VhvSettings, &PhaseCal);
nikapov	0:a1a69d32f310	5464	}
nikapov	0:a1a69d32f310	5465
nikapov	0:a1a69d32f310	5466	}
nikapov	0:a1a69d32f310	5467
nikapov	0:a1a69d32f310	5468	if (operating_mode == range_continuous_polling) {
nikapov	0:a1a69d32f310	5469	if (Status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5470	//printf("Call of VL53L0X_SetDeviceMode\n");
nikapov	0:a1a69d32f310	5471	Status = VL53L0X_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
nikapov	0:a1a69d32f310	5472	}
nikapov	0:a1a69d32f310	5473
nikapov	0:a1a69d32f310	5474	if (Status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5475	//printf("Call of VL53L0X_StartMeasurement\n");
nikapov	0:a1a69d32f310	5476	Status = VL53L0X_start_measurement(_device);
nikapov	0:a1a69d32f310	5477	}
nikapov	0:a1a69d32f310	5478	}
nikapov	0:a1a69d32f310	5479
nikapov	0:a1a69d32f310	5480	return Status;
nikapov	0:a1a69d32f310	5481	}
nikapov	0:a1a69d32f310	5482
nikapov	0:a1a69d32f310	5483
nikapov	0:a1a69d32f310	5484	int VL53L0X::get_measurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data)
nikapov	0:a1a69d32f310	5485	{
nikapov	0:a1a69d32f310	5486	int Status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5487
nikapov	0:a1a69d32f310	5488	if (operating_mode == range_single_shot_polling) {
nikapov	0:a1a69d32f310	5489	Status = VL53L0X_perform_single_ranging_measurement(_device, p_data);
nikapov	0:a1a69d32f310	5490	}
nikapov	0:a1a69d32f310	5491
nikapov	0:a1a69d32f310	5492	if (operating_mode == range_continuous_polling) {
Davidroid	3:e9269ff624ed	5493	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5494	Status = VL53L0X_measurement_poll_for_completion(_device);
Davidroid	3:e9269ff624ed	5495	}
nikapov	0:a1a69d32f310	5496
nikapov	0:a1a69d32f310	5497	if (Status == VL53L0X_ERROR_NONE) {
nikapov	0:a1a69d32f310	5498	Status = VL53L0X_get_ranging_measurement_data(_device, p_data);
nikapov	0:a1a69d32f310	5499
nikapov	0:a1a69d32f310	5500	// Clear the interrupt
nikapov	0:a1a69d32f310	5501	VL53L0X_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
nikapov	0:a1a69d32f310	5502	VL53L0X_polling_delay(_device);
nikapov	0:a1a69d32f310	5503	}
nikapov	0:a1a69d32f310	5504	}
nikapov	0:a1a69d32f310	5505
nikapov	0:a1a69d32f310	5506	if (operating_mode == range_continuous_interrupt) {
nikapov	0:a1a69d32f310	5507	Status = VL53L0X_get_ranging_measurement_data(_device, p_data);
nikapov	0:a1a69d32f310	5508	VL53L0X_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR \| VL53L0X_REG_RESULT_INTERRUPT_STATUS);
nikapov	0:a1a69d32f310	5509	}
nikapov	0:a1a69d32f310	5510
nikapov	0:a1a69d32f310	5511	return Status;
nikapov	0:a1a69d32f310	5512	}
nikapov	0:a1a69d32f310	5513
nikapov	0:a1a69d32f310	5514
nikapov	0:a1a69d32f310	5515	int VL53L0X::stop_measurement(OperatingMode operating_mode)
nikapov	0:a1a69d32f310	5516	{
nikapov	0:a1a69d32f310	5517	int status = VL53L0X_ERROR_NONE;
nikapov	0:a1a69d32f310	5518
nikapov	0:a1a69d32f310	5519
nikapov	0:a1a69d32f310	5520	// don't need to stop for a singleshot range!
nikapov	0:a1a69d32f310	5521	if (operating_mode == range_single_shot_polling) {
nikapov	0:a1a69d32f310	5522	}
nikapov	0:a1a69d32f310	5523
nikapov	0:a1a69d32f310	5524	if (operating_mode == range_continuous_interrupt \|\| operating_mode == range_continuous_polling) {
nikapov	0:a1a69d32f310	5525	// continuous mode
nikapov	0:a1a69d32f310	5526	if (status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5527	//printf("Call of VL53L0X_StopMeasurement\n");
nikapov	0:a1a69d32f310	5528	status = VL53L0X_stop_measurement(_device);
nikapov	0:a1a69d32f310	5529	}
nikapov	0:a1a69d32f310	5530
nikapov	0:a1a69d32f310	5531	if (status == VL53L0X_ERROR_NONE) {
nikapov	1:834986cdde0a	5532	//printf("Wait Stop to be competed\n");
nikapov	0:a1a69d32f310	5533	status = wait_stop_completed(_device);
nikapov	0:a1a69d32f310	5534	}
nikapov	0:a1a69d32f310	5535
nikapov	0:a1a69d32f310	5536	if (status == VL53L0X_ERROR_NONE)
nikapov	0:a1a69d32f310	5537	status = VL53L0X_clear_interrupt_mask(_device,
nikapov	0:a1a69d32f310	5538	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
nikapov	0:a1a69d32f310	5539	}
nikapov	0:a1a69d32f310	5540
nikapov	0:a1a69d32f310	5541	return status;
nikapov	0:a1a69d32f310	5542	}
nikapov	0:a1a69d32f310	5543
nikapov	0:a1a69d32f310	5544
nikapov	0:a1a69d32f310	5545	int VL53L0X::handle_irq(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data)
nikapov	0:a1a69d32f310	5546	{
nikapov	0:a1a69d32f310	5547	int status;
nikapov	0:a1a69d32f310	5548	status = get_measurement(operating_mode, data);
nikapov	0:a1a69d32f310	5549	enable_interrupt_measure_detection_irq();
nikapov	0:a1a69d32f310	5550	return status;
nikapov	0:a1a69d32f310	5551	}
nikapov	0:a1a69d32f310	5552
nikapov	0:a1a69d32f310	5553
nikapov	0:a1a69d32f310	5554	/******************************************************************************/

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	04 Feb 2018
Imports:	110
Forks:	0
Commits:	11
Dependents:	5
Dependencies:	0
Followers:	1

The code in this repository is Apache licensed.

VL53L0X.cpp@10:c9181a97a64f, 2020-02-26 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning