X_NUCLEO_53L0A1_mbed_sdk - Updates to follow mbed SDK coding style guideline…

ST Expansion SW Team » Code » X_NUCLEO_53L0A1_mbed_sdk

ST Expansion SW Team / X_NUCLEO_53L0A1_mbed_sdk

Updates to follow mbed SDK coding style guidelines.

Dependencies: ST_INTERFACES X_NUCLEO_COMMON

Dependents: 53L0A1_Satellites_with_Interrupts_OS5 Display_53L0A1_OS5

Fork of X_NUCLEO_53L0A1 by ST

Components/VL53L0X/vl53l0x_class.cpp@16:98ce55ddbb1a, 2017-06-22 (annotated)

Committer:: johnAlexander
Date:: Thu Jun 22 09:55:10 2017 +0000
Revision:: 16:98ce55ddbb1a
Parent:: 15:932d8b4e52c9
Child:: 17:1b842521063a

further code refactoring with resharper & astyle.; removal of deprecated interfaces.

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	26 Jun 2017
Imports:	10
Forks:	0
Commits:	22
Dependents:	2
Dependencies:	2
Followers:	64

Components/VL53L0X/vl53l0x_class.cpp@16:98ce55ddbb1a, 2017-06-22 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning