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@15:932d8b4e52c9, 2017-06-20 (annotated)

Committer:: johnAlexander
Date:: Tue Jun 20 16:00:31 2017 +0000
Revision:: 15:932d8b4e52c9
Parent:: 13:615f7e38568c
Child:: 16:98ce55ddbb1a

x_nucleo_53l0a1 & vl53l0x_class class reformatted with astyle tool.

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	0:c523920bcc09	83	VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	84	{
johnAlexander	15:932d8b4e52c9	85	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	86	uint8_t strobe;
johnAlexander	15:932d8b4e52c9	87	uint32_t LoopNb;
johnAlexander	15:932d8b4e52c9	88	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	89
johnAlexander	15:932d8b4e52c9	90	Status \|= VL53L0X_WrByte(Dev, 0x83, 0x00);
johnAlexander	15:932d8b4e52c9	91
johnAlexander	15:932d8b4e52c9	92	/* polling
johnAlexander	15:932d8b4e52c9	93	* use timeout to avoid deadlock*/
johnAlexander	15:932d8b4e52c9	94	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	95	LoopNb = 0;
johnAlexander	15:932d8b4e52c9	96	do {
johnAlexander	15:932d8b4e52c9	97	Status = VL53L0X_RdByte(Dev, 0x83, &strobe);
johnAlexander	15:932d8b4e52c9	98	if ((strobe != 0x00) \|\| Status != VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	99	break;
johnAlexander	15:932d8b4e52c9	100
johnAlexander	15:932d8b4e52c9	101	LoopNb = LoopNb + 1;
johnAlexander	15:932d8b4e52c9	102	} while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
johnAlexander	15:932d8b4e52c9	103
johnAlexander	15:932d8b4e52c9	104	if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
johnAlexander	15:932d8b4e52c9	105	Status = VL53L0X_ERROR_TIME_OUT;
johnAlexander	15:932d8b4e52c9	106
johnAlexander	15:932d8b4e52c9	107	}
johnAlexander	15:932d8b4e52c9	108
johnAlexander	15:932d8b4e52c9	109	Status \|= VL53L0X_WrByte(Dev, 0x83, 0x01);
johnAlexander	15:932d8b4e52c9	110
johnAlexander	15:932d8b4e52c9	111	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	112	return Status;
johnAlexander	0:c523920bcc09	113
johnAlexander	0:c523920bcc09	114	}
johnAlexander	0:c523920bcc09	115
johnAlexander	0:c523920bcc09	116	VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option)
johnAlexander	0:c523920bcc09	117	{
johnAlexander	0:c523920bcc09	118
johnAlexander	15:932d8b4e52c9	119	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	120	uint8_t byte;
johnAlexander	15:932d8b4e52c9	121	uint32_t TmpDWord;
johnAlexander	15:932d8b4e52c9	122	uint8_t ModuleId;
johnAlexander	15:932d8b4e52c9	123	uint8_t Revision;
johnAlexander	15:932d8b4e52c9	124	uint8_t ReferenceSpadCount = 0;
johnAlexander	15:932d8b4e52c9	125	uint8_t ReferenceSpadType = 0;
johnAlexander	15:932d8b4e52c9	126	uint32_t PartUIDUpper = 0;
johnAlexander	15:932d8b4e52c9	127	uint32_t PartUIDLower = 0;
johnAlexander	15:932d8b4e52c9	128	uint32_t OffsetFixed1104_mm = 0;
johnAlexander	15:932d8b4e52c9	129	int16_t OffsetMicroMeters = 0;
johnAlexander	15:932d8b4e52c9	130	uint32_t DistMeasTgtFixed1104_mm = 400 << 4;
johnAlexander	15:932d8b4e52c9	131	uint32_t DistMeasFixed1104_400_mm = 0;
johnAlexander	15:932d8b4e52c9	132	uint32_t SignalRateMeasFixed1104_400_mm = 0;
johnAlexander	15:932d8b4e52c9	133	char ProductId[19];
johnAlexander	15:932d8b4e52c9	134	char *ProductId_tmp;
johnAlexander	15:932d8b4e52c9	135	uint8_t ReadDataFromDeviceDone;
johnAlexander	15:932d8b4e52c9	136	FixPoint1616_t SignalRateMeasFixed400mmFix = 0;
johnAlexander	15:932d8b4e52c9	137	uint8_t NvmRefGoodSpadMap[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	15:932d8b4e52c9	143	ReadDataFromDeviceDone = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	148	if (ReadDataFromDeviceDone != 7) {
johnAlexander	15:932d8b4e52c9	149
johnAlexander	15:932d8b4e52c9	150	Status \|= VL53L0X_WrByte(Dev, 0x80, 0x01);
johnAlexander	15:932d8b4e52c9	151	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	152	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	153
johnAlexander	15:932d8b4e52c9	154	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x06);
johnAlexander	15:932d8b4e52c9	155	Status \|= VL53L0X_RdByte(Dev, 0x83, &byte);
johnAlexander	15:932d8b4e52c9	156	Status \|= VL53L0X_WrByte(Dev, 0x83, byte\|4);
johnAlexander	15:932d8b4e52c9	157	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x07);
johnAlexander	15:932d8b4e52c9	158	Status \|= VL53L0X_WrByte(Dev, 0x81, 0x01);
johnAlexander	15:932d8b4e52c9	159
johnAlexander	15:932d8b4e52c9	160	Status \|= VL53L0X_PollingDelay(Dev);
johnAlexander	15:932d8b4e52c9	161
johnAlexander	15:932d8b4e52c9	162	Status \|= VL53L0X_WrByte(Dev, 0x80, 0x01);
johnAlexander	15:932d8b4e52c9	163
johnAlexander	15:932d8b4e52c9	164	if (((option & 1) == 1) &&
johnAlexander	15:932d8b4e52c9	165	((ReadDataFromDeviceDone & 1) == 0)) {
johnAlexander	15:932d8b4e52c9	166	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x6b);
johnAlexander	15:932d8b4e52c9	167	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	168	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	169
johnAlexander	15:932d8b4e52c9	170	ReferenceSpadCount = (uint8_t)((TmpDWord >> 8) & 0x07f);
johnAlexander	15:932d8b4e52c9	171	ReferenceSpadType = (uint8_t)((TmpDWord >> 15) & 0x01);
johnAlexander	15:932d8b4e52c9	172
johnAlexander	15:932d8b4e52c9	173	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x24);
johnAlexander	15:932d8b4e52c9	174	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	175	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	176
johnAlexander	15:932d8b4e52c9	177
johnAlexander	15:932d8b4e52c9	178	NvmRefGoodSpadMap[0] = (uint8_t)((TmpDWord >> 24)
johnAlexander	15:932d8b4e52c9	179	& 0xff);
johnAlexander	15:932d8b4e52c9	180	NvmRefGoodSpadMap[1] = (uint8_t)((TmpDWord >> 16)
johnAlexander	15:932d8b4e52c9	181	& 0xff);
johnAlexander	15:932d8b4e52c9	182	NvmRefGoodSpadMap[2] = (uint8_t)((TmpDWord >> 8)
johnAlexander	15:932d8b4e52c9	183	& 0xff);
johnAlexander	15:932d8b4e52c9	184	NvmRefGoodSpadMap[3] = (uint8_t)(TmpDWord & 0xff);
johnAlexander	15:932d8b4e52c9	185
johnAlexander	15:932d8b4e52c9	186	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x25);
johnAlexander	15:932d8b4e52c9	187	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	188	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	189
johnAlexander	15:932d8b4e52c9	190	NvmRefGoodSpadMap[4] = (uint8_t)((TmpDWord >> 24)
johnAlexander	15:932d8b4e52c9	191	& 0xff);
johnAlexander	15:932d8b4e52c9	192	NvmRefGoodSpadMap[5] = (uint8_t)((TmpDWord >> 16)
johnAlexander	15:932d8b4e52c9	193	& 0xff);
johnAlexander	15:932d8b4e52c9	194	}
johnAlexander	15:932d8b4e52c9	195
johnAlexander	15:932d8b4e52c9	196	if (((option & 2) == 2) &&
johnAlexander	15:932d8b4e52c9	197	((ReadDataFromDeviceDone & 2) == 0)) {
johnAlexander	15:932d8b4e52c9	198
johnAlexander	15:932d8b4e52c9	199	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x02);
johnAlexander	15:932d8b4e52c9	200	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	201	Status \|= VL53L0X_RdByte(Dev, 0x90, &ModuleId);
johnAlexander	15:932d8b4e52c9	202
johnAlexander	15:932d8b4e52c9	203	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x7B);
johnAlexander	15:932d8b4e52c9	204	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	205	Status \|= VL53L0X_RdByte(Dev, 0x90, &Revision);
johnAlexander	15:932d8b4e52c9	206
johnAlexander	15:932d8b4e52c9	207	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x77);
johnAlexander	15:932d8b4e52c9	208	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	209	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	210
johnAlexander	15:932d8b4e52c9	211	ProductId[0] = (char)((TmpDWord >> 25) & 0x07f);
johnAlexander	15:932d8b4e52c9	212	ProductId[1] = (char)((TmpDWord >> 18) & 0x07f);
johnAlexander	15:932d8b4e52c9	213	ProductId[2] = (char)((TmpDWord >> 11) & 0x07f);
johnAlexander	15:932d8b4e52c9	214	ProductId[3] = (char)((TmpDWord >> 4) & 0x07f);
johnAlexander	15:932d8b4e52c9	215
johnAlexander	15:932d8b4e52c9	216	byte = (uint8_t)((TmpDWord & 0x00f) << 3);
johnAlexander	15:932d8b4e52c9	217
johnAlexander	15:932d8b4e52c9	218	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x78);
johnAlexander	15:932d8b4e52c9	219	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	220	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	221
johnAlexander	15:932d8b4e52c9	222	ProductId[4] = (char)(byte +
johnAlexander	15:932d8b4e52c9	223	((TmpDWord >> 29) & 0x07f));
johnAlexander	15:932d8b4e52c9	224	ProductId[5] = (char)((TmpDWord >> 22) & 0x07f);
johnAlexander	15:932d8b4e52c9	225	ProductId[6] = (char)((TmpDWord >> 15) & 0x07f);
johnAlexander	15:932d8b4e52c9	226	ProductId[7] = (char)((TmpDWord >> 8) & 0x07f);
johnAlexander	15:932d8b4e52c9	227	ProductId[8] = (char)((TmpDWord >> 1) & 0x07f);
johnAlexander	15:932d8b4e52c9	228
johnAlexander	15:932d8b4e52c9	229	byte = (uint8_t)((TmpDWord & 0x001) << 6);
johnAlexander	15:932d8b4e52c9	230
johnAlexander	15:932d8b4e52c9	231	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x79);
johnAlexander	15:932d8b4e52c9	232
johnAlexander	15:932d8b4e52c9	233	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	234
johnAlexander	15:932d8b4e52c9	235	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	236
johnAlexander	15:932d8b4e52c9	237	ProductId[9] = (char)(byte +
johnAlexander	15:932d8b4e52c9	238	((TmpDWord >> 26) & 0x07f));
johnAlexander	15:932d8b4e52c9	239	ProductId[10] = (char)((TmpDWord >> 19) & 0x07f);
johnAlexander	15:932d8b4e52c9	240	ProductId[11] = (char)((TmpDWord >> 12) & 0x07f);
johnAlexander	15:932d8b4e52c9	241	ProductId[12] = (char)((TmpDWord >> 5) & 0x07f);
johnAlexander	15:932d8b4e52c9	242
johnAlexander	15:932d8b4e52c9	243	byte = (uint8_t)((TmpDWord & 0x01f) << 2);
johnAlexander	15:932d8b4e52c9	244
johnAlexander	15:932d8b4e52c9	245	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x7A);
johnAlexander	15:932d8b4e52c9	246
johnAlexander	15:932d8b4e52c9	247	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	248
johnAlexander	15:932d8b4e52c9	249	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	250
johnAlexander	15:932d8b4e52c9	251	ProductId[13] = (char)(byte +
johnAlexander	15:932d8b4e52c9	252	((TmpDWord >> 30) & 0x07f));
johnAlexander	15:932d8b4e52c9	253	ProductId[14] = (char)((TmpDWord >> 23) & 0x07f);
johnAlexander	15:932d8b4e52c9	254	ProductId[15] = (char)((TmpDWord >> 16) & 0x07f);
johnAlexander	15:932d8b4e52c9	255	ProductId[16] = (char)((TmpDWord >> 9) & 0x07f);
johnAlexander	15:932d8b4e52c9	256	ProductId[17] = (char)((TmpDWord >> 2) & 0x07f);
johnAlexander	15:932d8b4e52c9	257	ProductId[18] = '\0';
johnAlexander	15:932d8b4e52c9	258
johnAlexander	15:932d8b4e52c9	259	}
johnAlexander	15:932d8b4e52c9	260
johnAlexander	15:932d8b4e52c9	261	if (((option & 4) == 4) &&
johnAlexander	15:932d8b4e52c9	262	((ReadDataFromDeviceDone & 4) == 0)) {
johnAlexander	15:932d8b4e52c9	263
johnAlexander	15:932d8b4e52c9	264	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x7B);
johnAlexander	15:932d8b4e52c9	265	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	266	Status \|= VL53L0X_RdDWord(Dev, 0x90, &PartUIDUpper);
johnAlexander	15:932d8b4e52c9	267
johnAlexander	15:932d8b4e52c9	268	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x7C);
johnAlexander	15:932d8b4e52c9	269	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	270	Status \|= VL53L0X_RdDWord(Dev, 0x90, &PartUIDLower);
johnAlexander	15:932d8b4e52c9	271
johnAlexander	15:932d8b4e52c9	272	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x73);
johnAlexander	15:932d8b4e52c9	273	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	274	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	275
johnAlexander	15:932d8b4e52c9	276	SignalRateMeasFixed1104_400_mm = (TmpDWord &
johnAlexander	15:932d8b4e52c9	277	0x0000000ff) << 8;
johnAlexander	15:932d8b4e52c9	278
johnAlexander	15:932d8b4e52c9	279	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x74);
johnAlexander	15:932d8b4e52c9	280	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	281	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	282
johnAlexander	15:932d8b4e52c9	283	SignalRateMeasFixed1104_400_mm \|= ((TmpDWord &
johnAlexander	15:932d8b4e52c9	284	0xff000000) >> 24);
johnAlexander	15:932d8b4e52c9	285
johnAlexander	15:932d8b4e52c9	286	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x75);
johnAlexander	15:932d8b4e52c9	287	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	288	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	289
johnAlexander	15:932d8b4e52c9	290	DistMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff)
johnAlexander	15:932d8b4e52c9	291	<< 8;
johnAlexander	15:932d8b4e52c9	292
johnAlexander	15:932d8b4e52c9	293	Status \|= VL53L0X_WrByte(Dev, 0x94, 0x76);
johnAlexander	15:932d8b4e52c9	294	Status \|= VL53L0X_device_read_strobe(Dev);
johnAlexander	15:932d8b4e52c9	295	Status \|= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
johnAlexander	15:932d8b4e52c9	296
johnAlexander	15:932d8b4e52c9	297	DistMeasFixed1104_400_mm \|= ((TmpDWord & 0xff000000)
johnAlexander	15:932d8b4e52c9	298	>> 24);
johnAlexander	15:932d8b4e52c9	299	}
johnAlexander	15:932d8b4e52c9	300
johnAlexander	15:932d8b4e52c9	301	Status \|= VL53L0X_WrByte(Dev, 0x81, 0x00);
johnAlexander	15:932d8b4e52c9	302	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x06);
johnAlexander	15:932d8b4e52c9	303	Status \|= VL53L0X_RdByte(Dev, 0x83, &byte);
johnAlexander	15:932d8b4e52c9	304	Status \|= VL53L0X_WrByte(Dev, 0x83, byte&0xfb);
johnAlexander	15:932d8b4e52c9	305	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	306	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x01);
johnAlexander	15:932d8b4e52c9	307
johnAlexander	15:932d8b4e52c9	308	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	309	Status \|= VL53L0X_WrByte(Dev, 0x80, 0x00);
johnAlexander	15:932d8b4e52c9	310	}
johnAlexander	15:932d8b4e52c9	311
johnAlexander	15:932d8b4e52c9	312	if ((Status == VL53L0X_ERROR_NONE) &&
johnAlexander	15:932d8b4e52c9	313	(ReadDataFromDeviceDone != 7)) {
johnAlexander	15:932d8b4e52c9	314	/* Assign to variable if status is ok */
johnAlexander	15:932d8b4e52c9	315	if (((option & 1) == 1) &&
johnAlexander	15:932d8b4e52c9	316	((ReadDataFromDeviceDone & 1) == 0)) {
johnAlexander	15:932d8b4e52c9	317	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	318	ReferenceSpadCount, ReferenceSpadCount);
johnAlexander	15:932d8b4e52c9	319
johnAlexander	15:932d8b4e52c9	320	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	321	ReferenceSpadType, ReferenceSpadType);
johnAlexander	15:932d8b4e52c9	322
johnAlexander	15:932d8b4e52c9	323	for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
johnAlexander	15:932d8b4e52c9	324	Dev->Data.SpadData.RefGoodSpadMap[i] =
johnAlexander	15:932d8b4e52c9	325	NvmRefGoodSpadMap[i];
johnAlexander	15:932d8b4e52c9	326	}
johnAlexander	15:932d8b4e52c9	327	}
johnAlexander	15:932d8b4e52c9	328
johnAlexander	15:932d8b4e52c9	329	if (((option & 2) == 2) &&
johnAlexander	15:932d8b4e52c9	330	((ReadDataFromDeviceDone & 2) == 0)) {
johnAlexander	15:932d8b4e52c9	331	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	332	ModuleId, ModuleId);
johnAlexander	15:932d8b4e52c9	333
johnAlexander	15:932d8b4e52c9	334	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	335	Revision, Revision);
johnAlexander	15:932d8b4e52c9	336
johnAlexander	15:932d8b4e52c9	337	ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	338	ProductId);
johnAlexander	15:932d8b4e52c9	339	VL53L0X_COPYSTRING(ProductId_tmp, ProductId);
johnAlexander	15:932d8b4e52c9	340
johnAlexander	15:932d8b4e52c9	341	}
johnAlexander	15:932d8b4e52c9	342
johnAlexander	15:932d8b4e52c9	343	if (((option & 4) == 4) &&
johnAlexander	15:932d8b4e52c9	344	((ReadDataFromDeviceDone & 4) == 0)) {
johnAlexander	15:932d8b4e52c9	345	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	346	PartUIDUpper, PartUIDUpper);
johnAlexander	15:932d8b4e52c9	347
johnAlexander	15:932d8b4e52c9	348	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	349	PartUIDLower, PartUIDLower);
johnAlexander	15:932d8b4e52c9	350
johnAlexander	15:932d8b4e52c9	351	SignalRateMeasFixed400mmFix =
johnAlexander	15:932d8b4e52c9	352	VL53L0X_FIXPOINT97TOFIXPOINT1616(
johnAlexander	15:932d8b4e52c9	353	SignalRateMeasFixed1104_400_mm);
johnAlexander	15:932d8b4e52c9	354
johnAlexander	15:932d8b4e52c9	355	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	356	SignalRateMeasFixed400mm,
johnAlexander	15:932d8b4e52c9	357	SignalRateMeasFixed400mmFix);
johnAlexander	15:932d8b4e52c9	358
johnAlexander	15:932d8b4e52c9	359	OffsetMicroMeters = 0;
johnAlexander	15:932d8b4e52c9	360	if (DistMeasFixed1104_400_mm != 0) {
johnAlexander	15:932d8b4e52c9	361	OffsetFixed1104_mm =
johnAlexander	15:932d8b4e52c9	362	DistMeasFixed1104_400_mm -
johnAlexander	15:932d8b4e52c9	363	DistMeasTgtFixed1104_mm;
johnAlexander	15:932d8b4e52c9	364	OffsetMicroMeters = (OffsetFixed1104_mm
johnAlexander	15:932d8b4e52c9	365	* 1000) >> 4;
johnAlexander	15:932d8b4e52c9	366	OffsetMicroMeters *= -1;
johnAlexander	15:932d8b4e52c9	367	}
johnAlexander	15:932d8b4e52c9	368
johnAlexander	15:932d8b4e52c9	369	PALDevDataSet(Dev,
johnAlexander	15:932d8b4e52c9	370	Part2PartOffsetAdjustmentNVMMicroMeter,
johnAlexander	15:932d8b4e52c9	371	OffsetMicroMeters);
johnAlexander	15:932d8b4e52c9	372	}
johnAlexander	15:932d8b4e52c9	373	byte = (uint8_t)(ReadDataFromDeviceDone\|option);
johnAlexander	15:932d8b4e52c9	374	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone,
johnAlexander	15:932d8b4e52c9	375	byte);
johnAlexander	15:932d8b4e52c9	376	}
johnAlexander	15:932d8b4e52c9	377
johnAlexander	15:932d8b4e52c9	378	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	379	return Status;
johnAlexander	0:c523920bcc09	380	}
johnAlexander	0:c523920bcc09	381
johnAlexander	0:c523920bcc09	382	VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	383	int32_t *pOffsetCalibrationDataMicroMeter)
johnAlexander	0:c523920bcc09	384	{
johnAlexander	15:932d8b4e52c9	385	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	386	uint16_t RangeOffsetRegister;
johnAlexander	15:932d8b4e52c9	387	int16_t cMaxOffset = 2047;
johnAlexander	15:932d8b4e52c9	388	int16_t cOffsetRange = 4096;
johnAlexander	15:932d8b4e52c9	389
johnAlexander	15:932d8b4e52c9	390	/* Note that offset has 10.2 format */
johnAlexander	15:932d8b4e52c9	391
johnAlexander	15:932d8b4e52c9	392	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	393	VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
johnAlexander	15:932d8b4e52c9	394	&RangeOffsetRegister);
johnAlexander	15:932d8b4e52c9	395
johnAlexander	15:932d8b4e52c9	396	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	397	RangeOffsetRegister = (RangeOffsetRegister & 0x0fff);
johnAlexander	15:932d8b4e52c9	398
johnAlexander	15:932d8b4e52c9	399	/* Apply 12 bit 2's compliment conversion */
johnAlexander	15:932d8b4e52c9	400	if (RangeOffsetRegister > cMaxOffset)
johnAlexander	15:932d8b4e52c9	401	*pOffsetCalibrationDataMicroMeter =
johnAlexander	15:932d8b4e52c9	402	(int16_t)(RangeOffsetRegister - cOffsetRange)
johnAlexander	15:932d8b4e52c9	403	* 250;
johnAlexander	15:932d8b4e52c9	404	else
johnAlexander	15:932d8b4e52c9	405	*pOffsetCalibrationDataMicroMeter =
johnAlexander	15:932d8b4e52c9	406	(int16_t)RangeOffsetRegister * 250;
johnAlexander	15:932d8b4e52c9	407
johnAlexander	15:932d8b4e52c9	408	}
johnAlexander	15:932d8b4e52c9	409
johnAlexander	15:932d8b4e52c9	410	return Status;
johnAlexander	0:c523920bcc09	411	}
johnAlexander	0:c523920bcc09	412
johnAlexander	0:c523920bcc09	413	VL53L0X_Error VL53L0X::VL53L0X_GetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	414	int32_t *pOffsetCalibrationDataMicroMeter)
johnAlexander	0:c523920bcc09	415	{
johnAlexander	15:932d8b4e52c9	416	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	417	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	418
johnAlexander	15:932d8b4e52c9	419	Status = VL53L0X_get_offset_calibration_data_micro_meter(Dev,
johnAlexander	15:932d8b4e52c9	420	pOffsetCalibrationDataMicroMeter);
johnAlexander	15:932d8b4e52c9	421
johnAlexander	15:932d8b4e52c9	422	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	423	return Status;
johnAlexander	0:c523920bcc09	424	}
johnAlexander	0:c523920bcc09	425
johnAlexander	0:c523920bcc09	426	VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	427	int32_t OffsetCalibrationDataMicroMeter)
johnAlexander	0:c523920bcc09	428	{
johnAlexander	15:932d8b4e52c9	429	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	430	int32_t cMaxOffsetMicroMeter = 511000;
johnAlexander	15:932d8b4e52c9	431	int32_t cMinOffsetMicroMeter = -512000;
johnAlexander	15:932d8b4e52c9	432	int16_t cOffsetRange = 4096;
johnAlexander	15:932d8b4e52c9	433	uint32_t encodedOffsetVal;
johnAlexander	15:932d8b4e52c9	434
johnAlexander	15:932d8b4e52c9	435	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	436
johnAlexander	15:932d8b4e52c9	437	if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter)
johnAlexander	15:932d8b4e52c9	438	OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter;
johnAlexander	15:932d8b4e52c9	439	else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter)
johnAlexander	15:932d8b4e52c9	440	OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter;
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	15:932d8b4e52c9	446	if (OffsetCalibrationDataMicroMeter >= 0) {
johnAlexander	15:932d8b4e52c9	447	encodedOffsetVal =
johnAlexander	15:932d8b4e52c9	448	OffsetCalibrationDataMicroMeter/250;
johnAlexander	15:932d8b4e52c9	449	} else {
johnAlexander	15:932d8b4e52c9	450	encodedOffsetVal =
johnAlexander	15:932d8b4e52c9	451	cOffsetRange +
johnAlexander	15:932d8b4e52c9	452	OffsetCalibrationDataMicroMeter/250;
johnAlexander	15:932d8b4e52c9	453	}
johnAlexander	15:932d8b4e52c9	454
johnAlexander	15:932d8b4e52c9	455	Status = VL53L0X_WrWord(Dev,
johnAlexander	15:932d8b4e52c9	456	VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
johnAlexander	15:932d8b4e52c9	457	encodedOffsetVal);
johnAlexander	15:932d8b4e52c9	458
johnAlexander	15:932d8b4e52c9	459	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	460	return Status;
johnAlexander	0:c523920bcc09	461	}
johnAlexander	0:c523920bcc09	462
johnAlexander	0:c523920bcc09	463	VL53L0X_Error VL53L0X::VL53L0X_SetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	464	int32_t OffsetCalibrationDataMicroMeter)
johnAlexander	0:c523920bcc09	465	{
johnAlexander	15:932d8b4e52c9	466	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	467	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	468
johnAlexander	15:932d8b4e52c9	469	Status = VL53L0X_set_offset_calibration_data_micro_meter(Dev,
johnAlexander	15:932d8b4e52c9	470	OffsetCalibrationDataMicroMeter);
johnAlexander	15:932d8b4e52c9	471
johnAlexander	15:932d8b4e52c9	472	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	473	return Status;
johnAlexander	0:c523920bcc09	474	}
johnAlexander	0:c523920bcc09	475
johnAlexander	0:c523920bcc09	476	VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	477	{
johnAlexander	15:932d8b4e52c9	478	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	479	int32_t CorrectedOffsetMicroMeters;
johnAlexander	15:932d8b4e52c9	480	int32_t CurrentOffsetMicroMeters;
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	15:932d8b4e52c9	484	Status = VL53L0X_get_info_from_device(Dev, 7);
johnAlexander	15:932d8b4e52c9	485
johnAlexander	15:932d8b4e52c9	486	/* Read back current device offset */
johnAlexander	15:932d8b4e52c9	487	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	488	Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
johnAlexander	15:932d8b4e52c9	489	&CurrentOffsetMicroMeters);
johnAlexander	15:932d8b4e52c9	490	}
johnAlexander	15:932d8b4e52c9	491
johnAlexander	15:932d8b4e52c9	492	/* Apply Offset Adjustment derived from 400mm measurements */
johnAlexander	15:932d8b4e52c9	493	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	494
johnAlexander	15:932d8b4e52c9	495	/* Store initial device offset */
johnAlexander	15:932d8b4e52c9	496	PALDevDataSet(Dev, Part2PartOffsetNVMMicroMeter,
johnAlexander	15:932d8b4e52c9	497	CurrentOffsetMicroMeters);
johnAlexander	15:932d8b4e52c9	498
johnAlexander	15:932d8b4e52c9	499	CorrectedOffsetMicroMeters = CurrentOffsetMicroMeters +
johnAlexander	15:932d8b4e52c9	500	(int32_t)PALDevDataGet(Dev,
johnAlexander	15:932d8b4e52c9	501	Part2PartOffsetAdjustmentNVMMicroMeter);
johnAlexander	15:932d8b4e52c9	502
johnAlexander	15:932d8b4e52c9	503	Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
johnAlexander	15:932d8b4e52c9	504	CorrectedOffsetMicroMeters);
johnAlexander	15:932d8b4e52c9	505
johnAlexander	15:932d8b4e52c9	506	/* store current, adjusted offset */
johnAlexander	15:932d8b4e52c9	507	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	508	VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
johnAlexander	15:932d8b4e52c9	509	CorrectedOffsetMicroMeters);
johnAlexander	15:932d8b4e52c9	510	}
johnAlexander	15:932d8b4e52c9	511	}
johnAlexander	15:932d8b4e52c9	512
johnAlexander	15:932d8b4e52c9	513	return Status;
johnAlexander	0:c523920bcc09	514	}
johnAlexander	0:c523920bcc09	515
johnAlexander	0:c523920bcc09	516	VL53L0X_Error VL53L0X::VL53L0X_GetDeviceMode(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	517	VL53L0X_DeviceModes *pDeviceMode)
johnAlexander	0:c523920bcc09	518	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	524	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	525	return Status;
johnAlexander	0:c523920bcc09	526	}
johnAlexander	0:c523920bcc09	527
johnAlexander	0:c523920bcc09	528	VL53L0X_Error VL53L0X::VL53L0X_GetInterMeasurementPeriodMilliSeconds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	529	uint32_t *pInterMeasurementPeriodMilliSeconds)
johnAlexander	0:c523920bcc09	530	{
johnAlexander	15:932d8b4e52c9	531	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	532	uint16_t osc_calibrate_val;
johnAlexander	15:932d8b4e52c9	533	uint32_t IMPeriodMilliSeconds;
johnAlexander	15:932d8b4e52c9	534
johnAlexander	15:932d8b4e52c9	535	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	536
johnAlexander	15:932d8b4e52c9	537	Status = VL53L0X_RdWord(Dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
johnAlexander	15:932d8b4e52c9	538	&osc_calibrate_val);
johnAlexander	15:932d8b4e52c9	539
johnAlexander	15:932d8b4e52c9	540	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	541	Status = VL53L0X_RdDWord(Dev,
johnAlexander	15:932d8b4e52c9	542	VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
johnAlexander	15:932d8b4e52c9	543	&IMPeriodMilliSeconds);
johnAlexander	15:932d8b4e52c9	544	}
johnAlexander	15:932d8b4e52c9	545
johnAlexander	15:932d8b4e52c9	546	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	547	if (osc_calibrate_val != 0) {
johnAlexander	15:932d8b4e52c9	548	*pInterMeasurementPeriodMilliSeconds =
johnAlexander	15:932d8b4e52c9	549	IMPeriodMilliSeconds / 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	15:932d8b4e52c9	556	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	557	return Status;
johnAlexander	0:c523920bcc09	558	}
johnAlexander	0:c523920bcc09	559
johnAlexander	0:c523920bcc09	560	VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationRateMegaCps(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	561	FixPoint1616_t *pXTalkCompensationRateMegaCps)
johnAlexander	0:c523920bcc09	562	{
johnAlexander	15:932d8b4e52c9	563	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	564	uint16_t Value;
johnAlexander	15:932d8b4e52c9	565	FixPoint1616_t TempFix1616;
johnAlexander	15:932d8b4e52c9	566
johnAlexander	15:932d8b4e52c9	567	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	568
johnAlexander	15:932d8b4e52c9	569	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	570	VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&Value);
johnAlexander	15:932d8b4e52c9	571	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	572	if (Value == 0) {
johnAlexander	15:932d8b4e52c9	573	/* the Xtalk is disabled return value from memory */
johnAlexander	15:932d8b4e52c9	574	VL53L0X_GETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	575	XTalkCompensationRateMegaCps, TempFix1616);
johnAlexander	15:932d8b4e52c9	576	*pXTalkCompensationRateMegaCps = TempFix1616;
johnAlexander	15:932d8b4e52c9	577	VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable,
johnAlexander	15:932d8b4e52c9	578	0);
johnAlexander	15:932d8b4e52c9	579	} else {
johnAlexander	15:932d8b4e52c9	580	TempFix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(Value);
johnAlexander	15:932d8b4e52c9	581	*pXTalkCompensationRateMegaCps = TempFix1616;
johnAlexander	15:932d8b4e52c9	582	VL53L0X_SETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	583	XTalkCompensationRateMegaCps, TempFix1616);
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	15:932d8b4e52c9	589	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	590	return Status;
johnAlexander	0:c523920bcc09	591	}
johnAlexander	0:c523920bcc09	592
johnAlexander	0:c523920bcc09	593	VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId,
johnAlexander	15:932d8b4e52c9	594	FixPoint1616_t *pLimitCheckValue)
johnAlexander	0:c523920bcc09	595	{
johnAlexander	15:932d8b4e52c9	596	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	597	uint8_t EnableZeroValue = 0;
johnAlexander	15:932d8b4e52c9	598	uint16_t Temp16;
johnAlexander	15:932d8b4e52c9	599	FixPoint1616_t TempFix1616;
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	15:932d8b4e52c9	608	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, TempFix1616);
johnAlexander	15:932d8b4e52c9	609	EnableZeroValue = 0;
johnAlexander	15:932d8b4e52c9	610	break;
johnAlexander	15:932d8b4e52c9	611
johnAlexander	15:932d8b4e52c9	612	case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	613	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	614	VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
johnAlexander	15:932d8b4e52c9	615	&Temp16);
johnAlexander	15:932d8b4e52c9	616	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	617	TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16);
johnAlexander	15:932d8b4e52c9	618
johnAlexander	15:932d8b4e52c9	619
johnAlexander	15:932d8b4e52c9	620	EnableZeroValue = 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	15:932d8b4e52c9	626	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, TempFix1616);
johnAlexander	15:932d8b4e52c9	627	EnableZeroValue = 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	15:932d8b4e52c9	633	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, TempFix1616);
johnAlexander	15:932d8b4e52c9	634	EnableZeroValue = 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	15:932d8b4e52c9	639	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	640	VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
johnAlexander	15:932d8b4e52c9	641	&Temp16);
johnAlexander	15:932d8b4e52c9	642	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	643	TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16);
johnAlexander	15:932d8b4e52c9	644
johnAlexander	15:932d8b4e52c9	645
johnAlexander	15:932d8b4e52c9	646	EnableZeroValue = 0;
johnAlexander	15:932d8b4e52c9	647	break;
johnAlexander	15:932d8b4e52c9	648
johnAlexander	15:932d8b4e52c9	649	default:
johnAlexander	15:932d8b4e52c9	650	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	651
johnAlexander	15:932d8b4e52c9	652	}
johnAlexander	15:932d8b4e52c9	653
johnAlexander	15:932d8b4e52c9	654	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	655
johnAlexander	15:932d8b4e52c9	656	if (EnableZeroValue == 1) {
johnAlexander	15:932d8b4e52c9	657
johnAlexander	15:932d8b4e52c9	658	if (TempFix1616 == 0) {
johnAlexander	15:932d8b4e52c9	659	/* disabled: return value from memory */
johnAlexander	15:932d8b4e52c9	660	VL53L0X_GETARRAYPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	661	LimitChecksValue, LimitCheckId,
johnAlexander	15:932d8b4e52c9	662	TempFix1616);
johnAlexander	15:932d8b4e52c9	663	*pLimitCheckValue = TempFix1616;
johnAlexander	15:932d8b4e52c9	664	VL53L0X_SETARRAYPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	665	LimitChecksEnable, LimitCheckId, 0);
johnAlexander	15:932d8b4e52c9	666	} else {
johnAlexander	15:932d8b4e52c9	667	*pLimitCheckValue = TempFix1616;
johnAlexander	15:932d8b4e52c9	668	VL53L0X_SETARRAYPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	669	LimitChecksValue, LimitCheckId,
johnAlexander	15:932d8b4e52c9	670	TempFix1616);
johnAlexander	15:932d8b4e52c9	671	VL53L0X_SETARRAYPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	672	LimitChecksEnable, LimitCheckId, 1);
johnAlexander	15:932d8b4e52c9	673	}
johnAlexander	15:932d8b4e52c9	674	} else {
johnAlexander	15:932d8b4e52c9	675	*pLimitCheckValue = TempFix1616;
johnAlexander	15:932d8b4e52c9	676	}
johnAlexander	15:932d8b4e52c9	677	}
johnAlexander	15:932d8b4e52c9	678
johnAlexander	15:932d8b4e52c9	679	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	680	return Status;
johnAlexander	0:c523920bcc09	681
johnAlexander	0:c523920bcc09	682	}
johnAlexander	0:c523920bcc09	683
johnAlexander	0:c523920bcc09	684	VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId,
johnAlexander	15:932d8b4e52c9	685	uint8_t *pLimitCheckEnable)
johnAlexander	0:c523920bcc09	686	{
johnAlexander	15:932d8b4e52c9	687	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	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	15:932d8b4e52c9	697	LimitCheckId, Temp8);
johnAlexander	15:932d8b4e52c9	698	*pLimitCheckEnable = Temp8;
johnAlexander	15:932d8b4e52c9	699	}
johnAlexander	15:932d8b4e52c9	700
johnAlexander	15:932d8b4e52c9	701	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	702	return Status;
johnAlexander	0:c523920bcc09	703	}
johnAlexander	0:c523920bcc09	704
johnAlexander	0:c523920bcc09	705	VL53L0X_Error VL53L0X::VL53L0X_GetWrapAroundCheckEnable(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	706	uint8_t *pWrapAroundCheckEnable)
johnAlexander	0:c523920bcc09	707	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	713	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data);
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	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	15:932d8b4e52c9	726	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	758	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	759	return Status;
johnAlexander	0:c523920bcc09	760	}
johnAlexander	0:c523920bcc09	761
johnAlexander	0:c523920bcc09	762	VL53L0X_Error VL53L0X::VL53L0X_GetSequenceStepEnables(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	763	VL53L0X_SchedulerSequenceSteps_t *pSchedulerSequenceSteps)
johnAlexander	0:c523920bcc09	764	{
johnAlexander	15:932d8b4e52c9	765	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	766	uint8_t SequenceConfig = 0;
johnAlexander	15:932d8b4e52c9	767	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	768
johnAlexander	15:932d8b4e52c9	769	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	770	&SequenceConfig);
johnAlexander	15:932d8b4e52c9	771
johnAlexander	15:932d8b4e52c9	772	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	773	Status = sequence_step_enabled(Dev,
johnAlexander	15:932d8b4e52c9	774	VL53L0X_SEQUENCESTEP_TCC, SequenceConfig,
johnAlexander	15:932d8b4e52c9	775	&pSchedulerSequenceSteps->TccOn);
johnAlexander	15:932d8b4e52c9	776	}
johnAlexander	15:932d8b4e52c9	777	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	778	Status = sequence_step_enabled(Dev,
johnAlexander	15:932d8b4e52c9	779	VL53L0X_SEQUENCESTEP_DSS, SequenceConfig,
johnAlexander	15:932d8b4e52c9	780	&pSchedulerSequenceSteps->DssOn);
johnAlexander	15:932d8b4e52c9	781	}
johnAlexander	15:932d8b4e52c9	782	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	783	Status = sequence_step_enabled(Dev,
johnAlexander	15:932d8b4e52c9	784	VL53L0X_SEQUENCESTEP_MSRC, SequenceConfig,
johnAlexander	15:932d8b4e52c9	785	&pSchedulerSequenceSteps->MsrcOn);
johnAlexander	15:932d8b4e52c9	786	}
johnAlexander	15:932d8b4e52c9	787	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	788	Status = sequence_step_enabled(Dev,
johnAlexander	15:932d8b4e52c9	789	VL53L0X_SEQUENCESTEP_PRE_RANGE, SequenceConfig,
johnAlexander	15:932d8b4e52c9	790	&pSchedulerSequenceSteps->PreRangeOn);
johnAlexander	15:932d8b4e52c9	791	}
johnAlexander	15:932d8b4e52c9	792	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	793	Status = sequence_step_enabled(Dev,
johnAlexander	15:932d8b4e52c9	794	VL53L0X_SEQUENCESTEP_FINAL_RANGE, SequenceConfig,
johnAlexander	15:932d8b4e52c9	795	&pSchedulerSequenceSteps->FinalRangeOn);
johnAlexander	15:932d8b4e52c9	796	}
johnAlexander	15:932d8b4e52c9	797
johnAlexander	15:932d8b4e52c9	798	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	799	return Status;
johnAlexander	0:c523920bcc09	800	}
johnAlexander	0:c523920bcc09	801
johnAlexander	0:c523920bcc09	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	0:c523920bcc09	816	uint8_t VL53L0X::VL53L0X_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	0:c523920bcc09	831	VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	832	VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
johnAlexander	0:c523920bcc09	833	{
johnAlexander	15:932d8b4e52c9	834	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	835	uint8_t vcsel_period_reg;
johnAlexander	15:932d8b4e52c9	836	uint8_t MinPreVcselPeriodPCLK = 12;
johnAlexander	15:932d8b4e52c9	837	uint8_t MaxPreVcselPeriodPCLK = 18;
johnAlexander	15:932d8b4e52c9	838	uint8_t MinFinalVcselPeriodPCLK = 8;
johnAlexander	15:932d8b4e52c9	839	uint8_t MaxFinalVcselPeriodPCLK = 14;
johnAlexander	15:932d8b4e52c9	840	uint32_t MeasurementTimingBudgetMicroSeconds;
johnAlexander	15:932d8b4e52c9	841	uint32_t FinalRangeTimeoutMicroSeconds;
johnAlexander	15:932d8b4e52c9	842	uint32_t PreRangeTimeoutMicroSeconds;
johnAlexander	15:932d8b4e52c9	843	uint32_t MsrcTimeoutMicroSeconds;
johnAlexander	15:932d8b4e52c9	844	uint8_t PhaseCalInt = 0;
johnAlexander	15:932d8b4e52c9	845
johnAlexander	15:932d8b4e52c9	846	/* Check if valid clock period requested */
johnAlexander	15:932d8b4e52c9	847
johnAlexander	15:932d8b4e52c9	848	if ((VCSELPulsePeriodPCLK % 2) != 0) {
johnAlexander	15:932d8b4e52c9	849	/* Value must be an even number */
johnAlexander	15:932d8b4e52c9	850	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	851	} else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
johnAlexander	15:932d8b4e52c9	852	(VCSELPulsePeriodPCLK < MinPreVcselPeriodPCLK \|\|
johnAlexander	15:932d8b4e52c9	853	VCSELPulsePeriodPCLK > MaxPreVcselPeriodPCLK)) {
johnAlexander	15:932d8b4e52c9	854	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	855	} else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
johnAlexander	15:932d8b4e52c9	856	(VCSELPulsePeriodPCLK < MinFinalVcselPeriodPCLK \|\|
johnAlexander	15:932d8b4e52c9	857	VCSELPulsePeriodPCLK > MaxFinalVcselPeriodPCLK)) {
johnAlexander	15:932d8b4e52c9	858
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	864	if (Status != VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	865	return Status;
johnAlexander	15:932d8b4e52c9	866
johnAlexander	15:932d8b4e52c9	867
johnAlexander	15:932d8b4e52c9	868	if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
johnAlexander	15:932d8b4e52c9	869
johnAlexander	15:932d8b4e52c9	870	/* Set phase check limits */
johnAlexander	15:932d8b4e52c9	871	if (VCSELPulsePeriodPCLK == 12) {
johnAlexander	15:932d8b4e52c9	872
johnAlexander	15:932d8b4e52c9	873	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	874	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	875	0x18);
johnAlexander	15:932d8b4e52c9	876	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	877	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	878	0x08);
johnAlexander	15:932d8b4e52c9	879	} else if (VCSELPulsePeriodPCLK == 14) {
johnAlexander	15:932d8b4e52c9	880
johnAlexander	15:932d8b4e52c9	881	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	882	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	883	0x30);
johnAlexander	15:932d8b4e52c9	884	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	885	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	886	0x08);
johnAlexander	15:932d8b4e52c9	887	} else if (VCSELPulsePeriodPCLK == 16) {
johnAlexander	15:932d8b4e52c9	888
johnAlexander	15:932d8b4e52c9	889	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	890	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	891	0x40);
johnAlexander	15:932d8b4e52c9	892	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	893	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	894	0x08);
johnAlexander	15:932d8b4e52c9	895	} else if (VCSELPulsePeriodPCLK == 18) {
johnAlexander	15:932d8b4e52c9	896
johnAlexander	15:932d8b4e52c9	897	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	898	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	899	0x50);
johnAlexander	15:932d8b4e52c9	900	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	901	VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	902	0x08);
johnAlexander	15:932d8b4e52c9	903	}
johnAlexander	15:932d8b4e52c9	904	} else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
johnAlexander	15:932d8b4e52c9	905
johnAlexander	15:932d8b4e52c9	906	if (VCSELPulsePeriodPCLK == 8) {
johnAlexander	15:932d8b4e52c9	907
johnAlexander	15:932d8b4e52c9	908	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	909	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	910	0x10);
johnAlexander	15:932d8b4e52c9	911	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	912	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	913	0x08);
johnAlexander	15:932d8b4e52c9	914
johnAlexander	15:932d8b4e52c9	915	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	916	VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
johnAlexander	15:932d8b4e52c9	917	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	918	VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
johnAlexander	15:932d8b4e52c9	919
johnAlexander	15:932d8b4e52c9	920	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x01);
johnAlexander	15:932d8b4e52c9	921	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	922	VL53L0X_REG_ALGO_PHASECAL_LIM,
johnAlexander	15:932d8b4e52c9	923	0x30);
johnAlexander	15:932d8b4e52c9	924	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x00);
johnAlexander	15:932d8b4e52c9	925	} else if (VCSELPulsePeriodPCLK == 10) {
johnAlexander	15:932d8b4e52c9	926
johnAlexander	15:932d8b4e52c9	927	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	928	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	929	0x28);
johnAlexander	15:932d8b4e52c9	930	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	931	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	932	0x08);
johnAlexander	15:932d8b4e52c9	933
johnAlexander	15:932d8b4e52c9	934	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	935	VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
johnAlexander	15:932d8b4e52c9	936	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	937	VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
johnAlexander	15:932d8b4e52c9	938
johnAlexander	15:932d8b4e52c9	939	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x01);
johnAlexander	15:932d8b4e52c9	940	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	941	VL53L0X_REG_ALGO_PHASECAL_LIM,
johnAlexander	15:932d8b4e52c9	942	0x20);
johnAlexander	15:932d8b4e52c9	943	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x00);
johnAlexander	15:932d8b4e52c9	944	} else if (VCSELPulsePeriodPCLK == 12) {
johnAlexander	15:932d8b4e52c9	945
johnAlexander	15:932d8b4e52c9	946	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	947	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	948	0x38);
johnAlexander	15:932d8b4e52c9	949	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	950	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	951	0x08);
johnAlexander	15:932d8b4e52c9	952
johnAlexander	15:932d8b4e52c9	953	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	954	VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
johnAlexander	15:932d8b4e52c9	955	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	956	VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
johnAlexander	15:932d8b4e52c9	957
johnAlexander	15:932d8b4e52c9	958	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x01);
johnAlexander	15:932d8b4e52c9	959	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	960	VL53L0X_REG_ALGO_PHASECAL_LIM,
johnAlexander	15:932d8b4e52c9	961	0x20);
johnAlexander	15:932d8b4e52c9	962	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x00);
johnAlexander	15:932d8b4e52c9	963	} else if (VCSELPulsePeriodPCLK == 14) {
johnAlexander	15:932d8b4e52c9	964
johnAlexander	15:932d8b4e52c9	965	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	966	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
johnAlexander	15:932d8b4e52c9	967	0x048);
johnAlexander	15:932d8b4e52c9	968	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	969	VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
johnAlexander	15:932d8b4e52c9	970	0x08);
johnAlexander	15:932d8b4e52c9	971
johnAlexander	15:932d8b4e52c9	972	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	973	VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
johnAlexander	15:932d8b4e52c9	974	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	975	VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
johnAlexander	15:932d8b4e52c9	976
johnAlexander	15:932d8b4e52c9	977	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x01);
johnAlexander	15:932d8b4e52c9	978	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	979	VL53L0X_REG_ALGO_PHASECAL_LIM,
johnAlexander	15:932d8b4e52c9	980	0x20);
johnAlexander	15:932d8b4e52c9	981	Status \|= VL53L0X_WrByte(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	15:932d8b4e52c9	988	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	989	vcsel_period_reg = VL53L0X_encode_vcsel_period((uint8_t)
johnAlexander	15:932d8b4e52c9	990	VCSELPulsePeriodPCLK);
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	15:932d8b4e52c9	1001	switch (VcselPeriodType) {
johnAlexander	15:932d8b4e52c9	1002	case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
johnAlexander	15:932d8b4e52c9	1003	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1004	VL53L0X_SEQUENCESTEP_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1005	&PreRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1006
johnAlexander	15:932d8b4e52c9	1007	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1008	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1009	VL53L0X_SEQUENCESTEP_MSRC,
johnAlexander	15:932d8b4e52c9	1010	&MsrcTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1011
johnAlexander	15:932d8b4e52c9	1012	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1013	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	1014	VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
johnAlexander	15:932d8b4e52c9	1015	vcsel_period_reg);
johnAlexander	15:932d8b4e52c9	1016
johnAlexander	15:932d8b4e52c9	1017
johnAlexander	15:932d8b4e52c9	1018	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1019	Status = set_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1020	VL53L0X_SEQUENCESTEP_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1021	PreRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1022
johnAlexander	15:932d8b4e52c9	1023
johnAlexander	15:932d8b4e52c9	1024	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1025	Status = set_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1026	VL53L0X_SEQUENCESTEP_MSRC,
johnAlexander	15:932d8b4e52c9	1027	MsrcTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1028
johnAlexander	15:932d8b4e52c9	1029	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	1030	Dev,
johnAlexander	15:932d8b4e52c9	1031	PreRangeVcselPulsePeriod,
johnAlexander	15:932d8b4e52c9	1032	VCSELPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	1033	break;
johnAlexander	15:932d8b4e52c9	1034	case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	1035	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1036	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1037	&FinalRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1038
johnAlexander	15:932d8b4e52c9	1039	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1040	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	1041	VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
johnAlexander	15:932d8b4e52c9	1042	vcsel_period_reg);
johnAlexander	15:932d8b4e52c9	1043
johnAlexander	15:932d8b4e52c9	1044
johnAlexander	15:932d8b4e52c9	1045	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1046	Status = set_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1047	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1048	FinalRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1049
johnAlexander	15:932d8b4e52c9	1050	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	1051	Dev,
johnAlexander	15:932d8b4e52c9	1052	FinalRangeVcselPulsePeriod,
johnAlexander	15:932d8b4e52c9	1053	VCSELPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	1054	break;
johnAlexander	15:932d8b4e52c9	1055	default:
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1061	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1062	VL53L0X_GETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	1063	MeasurementTimingBudgetMicroSeconds,
johnAlexander	15:932d8b4e52c9	1064	MeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	1065
johnAlexander	15:932d8b4e52c9	1066	Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
johnAlexander	15:932d8b4e52c9	1067	MeasurementTimingBudgetMicroSeconds);
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	15:932d8b4e52c9	1073	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1074	Status = VL53L0X_perform_phase_calibration(
johnAlexander	15:932d8b4e52c9	1075	Dev, &PhaseCalInt, 0, 1);
johnAlexander	15:932d8b4e52c9	1076
johnAlexander	15:932d8b4e52c9	1077	return Status;
johnAlexander	0:c523920bcc09	1078	}
johnAlexander	0:c523920bcc09	1079
johnAlexander	0:c523920bcc09	1080	VL53L0X_Error VL53L0X::VL53L0X_SetVcselPulsePeriod(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1081	VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
johnAlexander	0:c523920bcc09	1082	{
johnAlexander	15:932d8b4e52c9	1083	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1084	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1085
johnAlexander	15:932d8b4e52c9	1086	Status = VL53L0X_set_vcsel_pulse_period(Dev, VcselPeriodType,
johnAlexander	15:932d8b4e52c9	1087	VCSELPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	1088
johnAlexander	15:932d8b4e52c9	1089	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1090	return Status;
johnAlexander	0:c523920bcc09	1091	}
johnAlexander	0:c523920bcc09	1092
johnAlexander	0:c523920bcc09	1093	VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1094	VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
johnAlexander	0:c523920bcc09	1095	{
johnAlexander	15:932d8b4e52c9	1096	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1097	uint8_t vcsel_period_reg;
johnAlexander	15:932d8b4e52c9	1098
johnAlexander	15:932d8b4e52c9	1099	switch (VcselPeriodType) {
johnAlexander	15:932d8b4e52c9	1100	case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
johnAlexander	15:932d8b4e52c9	1101	Status = VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	1102	VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
johnAlexander	15:932d8b4e52c9	1103	&vcsel_period_reg);
johnAlexander	15:932d8b4e52c9	1104	break;
johnAlexander	15:932d8b4e52c9	1105	case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	1106	Status = VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	1107	VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
johnAlexander	15:932d8b4e52c9	1108	&vcsel_period_reg);
johnAlexander	15:932d8b4e52c9	1109	break;
johnAlexander	15:932d8b4e52c9	1110	default:
johnAlexander	15:932d8b4e52c9	1111	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	1112	}
johnAlexander	15:932d8b4e52c9	1113
johnAlexander	15:932d8b4e52c9	1114	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1115	*pVCSELPulsePeriodPCLK =
johnAlexander	15:932d8b4e52c9	1116	VL53L0X_decode_vcsel_period(vcsel_period_reg);
johnAlexander	15:932d8b4e52c9	1117
johnAlexander	15:932d8b4e52c9	1118	return Status;
johnAlexander	0:c523920bcc09	1119	}
johnAlexander	0:c523920bcc09	1120
johnAlexander	0:c523920bcc09	1121	VL53L0X_Error VL53L0X::VL53L0X_GetVcselPulsePeriod(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1122	VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
johnAlexander	0:c523920bcc09	1123	{
johnAlexander	15:932d8b4e52c9	1124	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1125	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1126
johnAlexander	15:932d8b4e52c9	1127	Status = VL53L0X_get_vcsel_pulse_period(Dev, VcselPeriodType,
johnAlexander	15:932d8b4e52c9	1128	pVCSELPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	1129
johnAlexander	15:932d8b4e52c9	1130	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1131	return Status;
johnAlexander	0:c523920bcc09	1132	}
johnAlexander	0:c523920bcc09	1133
johnAlexander	0:c523920bcc09	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	0:c523920bcc09	1148	uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev, uint8_t vcsel_period_pclks)
johnAlexander	0:c523920bcc09	1149	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	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	15:932d8b4e52c9	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	0:c523920bcc09	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	15:932d8b4e52c9	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	0:c523920bcc09	1187	VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1188	VL53L0X_SequenceStepId SequenceStepId,
johnAlexander	15:932d8b4e52c9	1189	uint32_t *pTimeOutMicroSecs)
johnAlexander	0:c523920bcc09	1190	{
johnAlexander	15:932d8b4e52c9	1191	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1192	uint8_t CurrentVCSELPulsePeriodPClk;
johnAlexander	15:932d8b4e52c9	1193	uint8_t EncodedTimeOutByte = 0;
johnAlexander	15:932d8b4e52c9	1194	uint32_t TimeoutMicroSeconds = 0;
johnAlexander	15:932d8b4e52c9	1195	uint16_t PreRangeEncodedTimeOut = 0;
johnAlexander	15:932d8b4e52c9	1196	uint16_t MsrcTimeOutMClks;
johnAlexander	15:932d8b4e52c9	1197	uint16_t PreRangeTimeOutMClks;
johnAlexander	15:932d8b4e52c9	1198	uint16_t FinalRangeTimeOutMClks = 0;
johnAlexander	15:932d8b4e52c9	1199	uint16_t FinalRangeEncodedTimeOut;
johnAlexander	15:932d8b4e52c9	1200	VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
johnAlexander	15:932d8b4e52c9	1201
johnAlexander	15:932d8b4e52c9	1202	if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) \|\|
johnAlexander	15:932d8b4e52c9	1203	(SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) \|\|
johnAlexander	15:932d8b4e52c9	1204	(SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
johnAlexander	15:932d8b4e52c9	1205
johnAlexander	15:932d8b4e52c9	1206	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	1207	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1208	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1209	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1210	Status = VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	1211	VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
johnAlexander	15:932d8b4e52c9	1212	&EncodedTimeOutByte);
johnAlexander	15:932d8b4e52c9	1213	}
johnAlexander	15:932d8b4e52c9	1214	MsrcTimeOutMClks = VL53L0X_decode_timeout(EncodedTimeOutByte);
johnAlexander	15:932d8b4e52c9	1215
johnAlexander	15:932d8b4e52c9	1216	TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
johnAlexander	15:932d8b4e52c9	1217	MsrcTimeOutMClks,
johnAlexander	15:932d8b4e52c9	1218	CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1219	} else if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
johnAlexander	15:932d8b4e52c9	1220	/* Retrieve PRE-RANGE VCSEL Period */
johnAlexander	15:932d8b4e52c9	1221	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	1222	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1223	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1224
johnAlexander	15:932d8b4e52c9	1225	/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
johnAlexander	15:932d8b4e52c9	1226	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1227
johnAlexander	15:932d8b4e52c9	1228	/* Retrieve PRE-RANGE VCSEL Period */
johnAlexander	15:932d8b4e52c9	1229	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	1230	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1231	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1232
johnAlexander	15:932d8b4e52c9	1233	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1234	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	1235	VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
johnAlexander	15:932d8b4e52c9	1236	&PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	1237	}
johnAlexander	15:932d8b4e52c9	1238
johnAlexander	15:932d8b4e52c9	1239	PreRangeTimeOutMClks = VL53L0X_decode_timeout(
johnAlexander	15:932d8b4e52c9	1240	PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	1241
johnAlexander	15:932d8b4e52c9	1242	TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
johnAlexander	15:932d8b4e52c9	1243	PreRangeTimeOutMClks,
johnAlexander	15:932d8b4e52c9	1244	CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1245	}
johnAlexander	15:932d8b4e52c9	1246	} else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
johnAlexander	15:932d8b4e52c9	1247
johnAlexander	15:932d8b4e52c9	1248	VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
johnAlexander	15:932d8b4e52c9	1249	PreRangeTimeOutMClks = 0;
johnAlexander	15:932d8b4e52c9	1250
johnAlexander	15:932d8b4e52c9	1251	if (SchedulerSequenceSteps.PreRangeOn) {
johnAlexander	15:932d8b4e52c9	1252	/* Retrieve PRE-RANGE VCSEL Period */
johnAlexander	15:932d8b4e52c9	1253	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	1254	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1255	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1256
johnAlexander	15:932d8b4e52c9	1257	/* Retrieve PRE-RANGE Timeout in Macro periods
johnAlexander	15:932d8b4e52c9	1258	* (MCLKS) */
johnAlexander	15:932d8b4e52c9	1259	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1260	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	1261	VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
johnAlexander	15:932d8b4e52c9	1262	&PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	1263	PreRangeTimeOutMClks = VL53L0X_decode_timeout(
johnAlexander	15:932d8b4e52c9	1264	PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	1265	}
johnAlexander	15:932d8b4e52c9	1266	}
johnAlexander	15:932d8b4e52c9	1267
johnAlexander	15:932d8b4e52c9	1268	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1269	/* Retrieve FINAL-RANGE VCSEL Period */
johnAlexander	15:932d8b4e52c9	1270	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	1271	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1272	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1273	}
johnAlexander	15:932d8b4e52c9	1274
johnAlexander	15:932d8b4e52c9	1275	/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
johnAlexander	15:932d8b4e52c9	1276	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1277	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	1278	VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
johnAlexander	15:932d8b4e52c9	1279	&FinalRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	1280	FinalRangeTimeOutMClks = VL53L0X_decode_timeout(
johnAlexander	15:932d8b4e52c9	1281	FinalRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	1282	}
johnAlexander	15:932d8b4e52c9	1283
johnAlexander	15:932d8b4e52c9	1284	FinalRangeTimeOutMClks -= PreRangeTimeOutMClks;
johnAlexander	15:932d8b4e52c9	1285	TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
johnAlexander	15:932d8b4e52c9	1286	FinalRangeTimeOutMClks,
johnAlexander	15:932d8b4e52c9	1287	CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	1288	}
johnAlexander	15:932d8b4e52c9	1289
johnAlexander	15:932d8b4e52c9	1290	*pTimeOutMicroSecs = TimeoutMicroSeconds;
johnAlexander	15:932d8b4e52c9	1291
johnAlexander	15:932d8b4e52c9	1292	return Status;
johnAlexander	0:c523920bcc09	1293	}
johnAlexander	0:c523920bcc09	1294
johnAlexander	0:c523920bcc09	1295	VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1296	uint32_t *pMeasurementTimingBudgetMicroSeconds)
johnAlexander	0:c523920bcc09	1297	{
johnAlexander	15:932d8b4e52c9	1298	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1299	VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
johnAlexander	15:932d8b4e52c9	1300	uint32_t FinalRangeTimeoutMicroSeconds;
johnAlexander	15:932d8b4e52c9	1301	uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
johnAlexander	15:932d8b4e52c9	1302	uint32_t StartOverheadMicroSeconds = 1910;
johnAlexander	15:932d8b4e52c9	1303	uint32_t EndOverheadMicroSeconds = 960;
johnAlexander	15:932d8b4e52c9	1304	uint32_t MsrcOverheadMicroSeconds = 660;
johnAlexander	15:932d8b4e52c9	1305	uint32_t TccOverheadMicroSeconds = 590;
johnAlexander	15:932d8b4e52c9	1306	uint32_t DssOverheadMicroSeconds = 690;
johnAlexander	15:932d8b4e52c9	1307	uint32_t PreRangeOverheadMicroSeconds = 660;
johnAlexander	15:932d8b4e52c9	1308	uint32_t FinalRangeOverheadMicroSeconds = 550;
johnAlexander	15:932d8b4e52c9	1309	uint32_t PreRangeTimeoutMicroSeconds = 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	15:932d8b4e52c9	1314	*pMeasurementTimingBudgetMicroSeconds
johnAlexander	15:932d8b4e52c9	1315	= StartOverheadMicroSeconds + EndOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	1316
johnAlexander	15:932d8b4e52c9	1317	Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
johnAlexander	15:932d8b4e52c9	1318
johnAlexander	15:932d8b4e52c9	1319	if (Status != VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1320	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1321	return Status;
johnAlexander	15:932d8b4e52c9	1322	}
johnAlexander	15:932d8b4e52c9	1323
johnAlexander	15:932d8b4e52c9	1324
johnAlexander	15:932d8b4e52c9	1325	if (SchedulerSequenceSteps.TccOn \|\|
johnAlexander	15:932d8b4e52c9	1326	SchedulerSequenceSteps.MsrcOn \|\|
johnAlexander	15:932d8b4e52c9	1327	SchedulerSequenceSteps.DssOn) {
johnAlexander	15:932d8b4e52c9	1328
johnAlexander	15:932d8b4e52c9	1329	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1330	VL53L0X_SEQUENCESTEP_MSRC,
johnAlexander	15:932d8b4e52c9	1331	&MsrcDccTccTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1332
johnAlexander	15:932d8b4e52c9	1333	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1334	if (SchedulerSequenceSteps.TccOn) {
johnAlexander	15:932d8b4e52c9	1335	*pMeasurementTimingBudgetMicroSeconds +=
johnAlexander	15:932d8b4e52c9	1336	MsrcDccTccTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	1337	TccOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	1338	}
johnAlexander	15:932d8b4e52c9	1339
johnAlexander	15:932d8b4e52c9	1340	if (SchedulerSequenceSteps.DssOn) {
johnAlexander	15:932d8b4e52c9	1341	*pMeasurementTimingBudgetMicroSeconds +=
johnAlexander	15:932d8b4e52c9	1342	2 * (MsrcDccTccTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	1343	DssOverheadMicroSeconds);
johnAlexander	15:932d8b4e52c9	1344	} else if (SchedulerSequenceSteps.MsrcOn) {
johnAlexander	15:932d8b4e52c9	1345	*pMeasurementTimingBudgetMicroSeconds +=
johnAlexander	15:932d8b4e52c9	1346	MsrcDccTccTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	1347	MsrcOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	1348	}
johnAlexander	15:932d8b4e52c9	1349	}
johnAlexander	15:932d8b4e52c9	1350	}
johnAlexander	15:932d8b4e52c9	1351
johnAlexander	15:932d8b4e52c9	1352	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1353	if (SchedulerSequenceSteps.PreRangeOn) {
johnAlexander	15:932d8b4e52c9	1354	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1355	VL53L0X_SEQUENCESTEP_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	1356	&PreRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1357	*pMeasurementTimingBudgetMicroSeconds +=
johnAlexander	15:932d8b4e52c9	1358	PreRangeTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	1359	PreRangeOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	1360	}
johnAlexander	15:932d8b4e52c9	1361	}
johnAlexander	15:932d8b4e52c9	1362
johnAlexander	15:932d8b4e52c9	1363	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1364	if (SchedulerSequenceSteps.FinalRangeOn) {
johnAlexander	15:932d8b4e52c9	1365	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	1366	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1367	&FinalRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	1368	*pMeasurementTimingBudgetMicroSeconds +=
johnAlexander	15:932d8b4e52c9	1369	(FinalRangeTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	1370	FinalRangeOverheadMicroSeconds);
johnAlexander	15:932d8b4e52c9	1371	}
johnAlexander	15:932d8b4e52c9	1372	}
johnAlexander	15:932d8b4e52c9	1373
johnAlexander	15:932d8b4e52c9	1374	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1375	VL53L0X_SETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	1376	MeasurementTimingBudgetMicroSeconds,
johnAlexander	15:932d8b4e52c9	1377	*pMeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	1378	}
johnAlexander	15:932d8b4e52c9	1379
johnAlexander	15:932d8b4e52c9	1380	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1381	return Status;
johnAlexander	0:c523920bcc09	1382	}
johnAlexander	0:c523920bcc09	1383
johnAlexander	0:c523920bcc09	1384	VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1385	uint32_t *pMeasurementTimingBudgetMicroSeconds)
johnAlexander	0:c523920bcc09	1386	{
johnAlexander	15:932d8b4e52c9	1387	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1388	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1389
johnAlexander	15:932d8b4e52c9	1390	Status = VL53L0X_get_measurement_timing_budget_micro_seconds(Dev,
johnAlexander	15:932d8b4e52c9	1391	pMeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	1392
johnAlexander	15:932d8b4e52c9	1393	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1394	return Status;
johnAlexander	0:c523920bcc09	1395	}
johnAlexander	0:c523920bcc09	1396
johnAlexander	0:c523920bcc09	1397	VL53L0X_Error VL53L0X::VL53L0X_GetDeviceParameters(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1398	VL53L0X_DeviceParameters_t *pDeviceParameters)
johnAlexander	0:c523920bcc09	1399	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1405	Status = VL53L0X_GetDeviceMode(Dev, &(pDeviceParameters->DeviceMode));
johnAlexander	15:932d8b4e52c9	1406
johnAlexander	15:932d8b4e52c9	1407	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1408	Status = VL53L0X_GetInterMeasurementPeriodMilliSeconds(Dev,
johnAlexander	15:932d8b4e52c9	1409	&(pDeviceParameters->InterMeasurementPeriodMilliSeconds));
johnAlexander	15:932d8b4e52c9	1410
johnAlexander	15:932d8b4e52c9	1411
johnAlexander	15:932d8b4e52c9	1412	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1413	pDeviceParameters->XTalkCompensationEnable = 0;
johnAlexander	15:932d8b4e52c9	1414
johnAlexander	15:932d8b4e52c9	1415	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1416	Status = VL53L0X_GetXTalkCompensationRateMegaCps(Dev,
johnAlexander	15:932d8b4e52c9	1417	&(pDeviceParameters->XTalkCompensationRateMegaCps));
johnAlexander	15:932d8b4e52c9	1418
johnAlexander	15:932d8b4e52c9	1419
johnAlexander	15:932d8b4e52c9	1420	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1421	Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
johnAlexander	15:932d8b4e52c9	1422	&(pDeviceParameters->RangeOffsetMicroMeters));
johnAlexander	15:932d8b4e52c9	1423
johnAlexander	15:932d8b4e52c9	1424
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1431	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1432	Status \|= VL53L0X_GetLimitCheckValue(Dev, i,
johnAlexander	15:932d8b4e52c9	1433	&(pDeviceParameters->LimitChecksValue[i]));
johnAlexander	15:932d8b4e52c9	1434	} else {
johnAlexander	15:932d8b4e52c9	1435	break;
johnAlexander	15:932d8b4e52c9	1436	}
johnAlexander	15:932d8b4e52c9	1437	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1438	Status \|= VL53L0X_GetLimitCheckEnable(Dev, i,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1446	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1447	Status = VL53L0X_GetWrapAroundCheckEnable(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	15:932d8b4e52c9	1452	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1453	Status = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(Dev,
johnAlexander	15:932d8b4e52c9	1454	&(pDeviceParameters->MeasurementTimingBudgetMicroSeconds));
johnAlexander	15:932d8b4e52c9	1455	}
johnAlexander	15:932d8b4e52c9	1456
johnAlexander	15:932d8b4e52c9	1457	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1458	return Status;
johnAlexander	0:c523920bcc09	1459	}
johnAlexander	0:c523920bcc09	1460
johnAlexander	0:c523920bcc09	1461	VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId,
johnAlexander	15:932d8b4e52c9	1462	FixPoint1616_t LimitCheckValue)
johnAlexander	0:c523920bcc09	1463	{
johnAlexander	15:932d8b4e52c9	1464	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1465	uint8_t Temp8;
johnAlexander	15:932d8b4e52c9	1466
johnAlexander	15:932d8b4e52c9	1467	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1468
johnAlexander	15:932d8b4e52c9	1469	VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId,
johnAlexander	15:932d8b4e52c9	1470	Temp8);
johnAlexander	15:932d8b4e52c9	1471
johnAlexander	15:932d8b4e52c9	1472	if (Temp8 == 0) { /* disabled write only internal value */
johnAlexander	15:932d8b4e52c9	1473	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
johnAlexander	15:932d8b4e52c9	1474	LimitCheckId, LimitCheckValue);
johnAlexander	15:932d8b4e52c9	1475	} else {
johnAlexander	15:932d8b4e52c9	1476
johnAlexander	15:932d8b4e52c9	1477	switch (LimitCheckId) {
johnAlexander	15:932d8b4e52c9	1478
johnAlexander	15:932d8b4e52c9	1479	case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	1480	/* internal computation: */
johnAlexander	15:932d8b4e52c9	1481	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
johnAlexander	15:932d8b4e52c9	1482	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1483	LimitCheckValue);
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	15:932d8b4e52c9	1488	Status = VL53L0X_WrWord(Dev,
johnAlexander	15:932d8b4e52c9	1489	VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
johnAlexander	15:932d8b4e52c9	1490	VL53L0X_FIXPOINT1616TOFIXPOINT97(
johnAlexander	15:932d8b4e52c9	1491	LimitCheckValue));
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	15:932d8b4e52c9	1498	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
johnAlexander	15:932d8b4e52c9	1499	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
johnAlexander	15:932d8b4e52c9	1500	LimitCheckValue);
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	15:932d8b4e52c9	1507	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
johnAlexander	15:932d8b4e52c9	1508	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
johnAlexander	15:932d8b4e52c9	1509	LimitCheckValue);
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	15:932d8b4e52c9	1516	Status = VL53L0X_WrWord(Dev,
johnAlexander	15:932d8b4e52c9	1517	VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
johnAlexander	15:932d8b4e52c9	1518	VL53L0X_FIXPOINT1616TOFIXPOINT97(
johnAlexander	15:932d8b4e52c9	1519	LimitCheckValue));
johnAlexander	15:932d8b4e52c9	1520
johnAlexander	15:932d8b4e52c9	1521	break;
johnAlexander	15:932d8b4e52c9	1522
johnAlexander	15:932d8b4e52c9	1523	default:
johnAlexander	15:932d8b4e52c9	1524	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	1525
johnAlexander	15:932d8b4e52c9	1526	}
johnAlexander	15:932d8b4e52c9	1527
johnAlexander	15:932d8b4e52c9	1528	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1529	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
johnAlexander	15:932d8b4e52c9	1530	LimitCheckId, LimitCheckValue);
johnAlexander	15:932d8b4e52c9	1531	}
johnAlexander	15:932d8b4e52c9	1532	}
johnAlexander	15:932d8b4e52c9	1533
johnAlexander	15:932d8b4e52c9	1534	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1535	return Status;
johnAlexander	0:c523920bcc09	1536	}
johnAlexander	0:c523920bcc09	1537
johnAlexander	0:c523920bcc09	1538	VL53L0X_Error VL53L0X::VL53L0X_DataInit(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	1539	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1557	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1558	Status = VL53L0X_WrByte(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	15:932d8b4e52c9	1583	Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters);
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1600	Status \|= VL53L0X_WrByte(Dev, 0x80, 0x01);
johnAlexander	15:932d8b4e52c9	1601	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	1602	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	1603	Status \|= VL53L0X_RdByte(Dev, 0x91, &StopVariable);
johnAlexander	15:932d8b4e52c9	1604	PALDevDataSet(Dev, StopVariable, StopVariable);
johnAlexander	15:932d8b4e52c9	1605	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x01);
johnAlexander	15:932d8b4e52c9	1606	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	1607	Status \|= VL53L0X_WrByte(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	15:932d8b4e52c9	1611	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1612	Status \|= VL53L0X_SetLimitCheckEnable(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	15:932d8b4e52c9	1619	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1620	Status = VL53L0X_SetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	1621	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
johnAlexander	15:932d8b4e52c9	1622
johnAlexander	15:932d8b4e52c9	1623	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1624	Status = VL53L0X_SetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	1625	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
johnAlexander	15:932d8b4e52c9	1626
johnAlexander	15:932d8b4e52c9	1627	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1628	Status = VL53L0X_SetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	1629	VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
johnAlexander	15:932d8b4e52c9	1630
johnAlexander	15:932d8b4e52c9	1631	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1632	Status = VL53L0X_SetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	1633	VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
johnAlexander	15:932d8b4e52c9	1634
johnAlexander	15:932d8b4e52c9	1635	/* Limit default values */
johnAlexander	15:932d8b4e52c9	1636	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1637	Status = VL53L0X_SetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	1638	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1639	(FixPoint1616_t)(18 * 65536));
johnAlexander	15:932d8b4e52c9	1640	}
johnAlexander	15:932d8b4e52c9	1641	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1642	Status = VL53L0X_SetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	1643	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	1644	(FixPoint1616_t)(25 * 65536 / 100));
johnAlexander	15:932d8b4e52c9	1645	/* 0.25 * 65536 */
johnAlexander	15:932d8b4e52c9	1646	}
johnAlexander	15:932d8b4e52c9	1647
johnAlexander	15:932d8b4e52c9	1648	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1649	Status = VL53L0X_SetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	1650	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
johnAlexander	15:932d8b4e52c9	1651	(FixPoint1616_t)(35 * 65536));
johnAlexander	15:932d8b4e52c9	1652	}
johnAlexander	15:932d8b4e52c9	1653
johnAlexander	15:932d8b4e52c9	1654	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1655	Status = VL53L0X_SetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	1656	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
johnAlexander	15:932d8b4e52c9	1657	(FixPoint1616_t)(0 * 65536));
johnAlexander	15:932d8b4e52c9	1658	}
johnAlexander	15:932d8b4e52c9	1659
johnAlexander	15:932d8b4e52c9	1660	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1661
johnAlexander	15:932d8b4e52c9	1662	PALDevDataSet(Dev, SequenceConfig, 0xFF);
johnAlexander	15:932d8b4e52c9	1663	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	1671	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1672	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 0);
johnAlexander	15:932d8b4e52c9	1673
johnAlexander	15:932d8b4e52c9	1674
johnAlexander	15:932d8b4e52c9	1675	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1676	return Status;
johnAlexander	0:c523920bcc09	1677	}
johnAlexander	0:c523920bcc09	1678
johnAlexander	0:c523920bcc09	1679	VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1680	uint8_t *Revision,
johnAlexander	15:932d8b4e52c9	1681	VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
johnAlexander	0:c523920bcc09	1682	{
johnAlexander	15:932d8b4e52c9	1683	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1684	uint8_t ModuleIdInt;
johnAlexander	15:932d8b4e52c9	1685	char *ProductId_tmp;
johnAlexander	15:932d8b4e52c9	1686
johnAlexander	15:932d8b4e52c9	1687	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1688
johnAlexander	15:932d8b4e52c9	1689	Status = VL53L0X_get_info_from_device(Dev, 2);
johnAlexander	15:932d8b4e52c9	1690
johnAlexander	15:932d8b4e52c9	1691	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1692	ModuleIdInt = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ModuleId);
johnAlexander	15:932d8b4e52c9	1693
johnAlexander	15:932d8b4e52c9	1694	if (ModuleIdInt == 0) {
johnAlexander	15:932d8b4e52c9	1695	*Revision = 0;
johnAlexander	15:932d8b4e52c9	1696	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, "");
johnAlexander	15:932d8b4e52c9	1697	} else {
johnAlexander	15:932d8b4e52c9	1698	*Revision = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Revision);
johnAlexander	15:932d8b4e52c9	1699	ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	1700	ProductId);
johnAlexander	15:932d8b4e52c9	1701	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, ProductId_tmp);
johnAlexander	15:932d8b4e52c9	1702	}
johnAlexander	15:932d8b4e52c9	1703	}
johnAlexander	15:932d8b4e52c9	1704
johnAlexander	15:932d8b4e52c9	1705	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1706	return Status;
johnAlexander	0:c523920bcc09	1707	}
johnAlexander	0:c523920bcc09	1708
johnAlexander	0:c523920bcc09	1709	VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1710	VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
johnAlexander	0:c523920bcc09	1711	{
johnAlexander	15:932d8b4e52c9	1712	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1713	uint8_t revision_id;
johnAlexander	15:932d8b4e52c9	1714	uint8_t Revision;
johnAlexander	15:932d8b4e52c9	1715
johnAlexander	15:932d8b4e52c9	1716	Status = VL53L0X_check_part_used(Dev, &Revision, pVL53L0X_DeviceInfo);
johnAlexander	15:932d8b4e52c9	1717
johnAlexander	15:932d8b4e52c9	1718	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1719	if (Revision == 0) {
johnAlexander	15:932d8b4e52c9	1720	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
johnAlexander	15:932d8b4e52c9	1721	VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
johnAlexander	15:932d8b4e52c9	1722	} else if ((Revision <= 34) && (Revision != 32)) {
johnAlexander	15:932d8b4e52c9	1723	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
johnAlexander	15:932d8b4e52c9	1724	VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
johnAlexander	15:932d8b4e52c9	1725	} else if (Revision < 39) {
johnAlexander	15:932d8b4e52c9	1726	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
johnAlexander	15:932d8b4e52c9	1727	VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
johnAlexander	15:932d8b4e52c9	1728	} else {
johnAlexander	15:932d8b4e52c9	1729	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
johnAlexander	15:932d8b4e52c9	1730	VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
johnAlexander	15:932d8b4e52c9	1731	}
johnAlexander	15:932d8b4e52c9	1732
johnAlexander	15:932d8b4e52c9	1733	VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Type,
johnAlexander	15:932d8b4e52c9	1734	VL53L0X_STRING_DEVICE_INFO_TYPE);
johnAlexander	15:932d8b4e52c9	1735
johnAlexander	15:932d8b4e52c9	1736	}
johnAlexander	15:932d8b4e52c9	1737
johnAlexander	15:932d8b4e52c9	1738	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1739	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
johnAlexander	15:932d8b4e52c9	1740	&pVL53L0X_DeviceInfo->ProductType);
johnAlexander	15:932d8b4e52c9	1741	}
johnAlexander	15:932d8b4e52c9	1742
johnAlexander	15:932d8b4e52c9	1743	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1744	Status = VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	1745	VL53L0X_REG_IDENTIFICATION_REVISION_ID,
johnAlexander	15:932d8b4e52c9	1746	&revision_id);
johnAlexander	15:932d8b4e52c9	1747	pVL53L0X_DeviceInfo->ProductRevisionMajor = 1;
johnAlexander	15:932d8b4e52c9	1748	pVL53L0X_DeviceInfo->ProductRevisionMinor =
johnAlexander	15:932d8b4e52c9	1749	(revision_id & 0xF0) >> 4;
johnAlexander	15:932d8b4e52c9	1750	}
johnAlexander	15:932d8b4e52c9	1751
johnAlexander	15:932d8b4e52c9	1752	return Status;
johnAlexander	0:c523920bcc09	1753	}
johnAlexander	0:c523920bcc09	1754
johnAlexander	0:c523920bcc09	1755	VL53L0X_Error VL53L0X::VL53L0X_GetDeviceInfo(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1756	VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
johnAlexander	0:c523920bcc09	1757	{
johnAlexander	15:932d8b4e52c9	1758	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1759	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1760
johnAlexander	15:932d8b4e52c9	1761	Status = VL53L0X_get_device_info(Dev, pVL53L0X_DeviceInfo);
johnAlexander	15:932d8b4e52c9	1762
johnAlexander	15:932d8b4e52c9	1763	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1764	return Status;
johnAlexander	0:c523920bcc09	1765	}
johnAlexander	0:c523920bcc09	1766
johnAlexander	0:c523920bcc09	1767	VL53L0X_Error VL53L0X::VL53L0X_GetInterruptMaskStatus(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1768	uint32_t *pInterruptMaskStatus)
johnAlexander	0:c523920bcc09	1769	{
johnAlexander	15:932d8b4e52c9	1770	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1771	uint8_t Byte;
johnAlexander	15:932d8b4e52c9	1772	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1773
johnAlexander	15:932d8b4e52c9	1774	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte);
johnAlexander	15:932d8b4e52c9	1775	*pInterruptMaskStatus = Byte & 0x07;
johnAlexander	15:932d8b4e52c9	1776
johnAlexander	15:932d8b4e52c9	1777	if (Byte & 0x18)
johnAlexander	15:932d8b4e52c9	1778	Status = VL53L0X_ERROR_RANGE_ERROR;
johnAlexander	15:932d8b4e52c9	1779
johnAlexander	15:932d8b4e52c9	1780	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1781	return Status;
johnAlexander	0:c523920bcc09	1782	}
johnAlexander	0:c523920bcc09	1783
johnAlexander	0:c523920bcc09	1784	VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementDataReady(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1785	uint8_t *pMeasurementDataReady)
johnAlexander	0:c523920bcc09	1786	{
johnAlexander	15:932d8b4e52c9	1787	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1788	uint8_t SysRangeStatusRegister;
johnAlexander	15:932d8b4e52c9	1789	uint8_t InterruptConfig;
johnAlexander	15:932d8b4e52c9	1790	uint32_t InterruptMask;
johnAlexander	15:932d8b4e52c9	1791	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1792
johnAlexander	15:932d8b4e52c9	1793	InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	1794	Pin0GpioFunctionality);
johnAlexander	15:932d8b4e52c9	1795
johnAlexander	15:932d8b4e52c9	1796	if (InterruptConfig ==
johnAlexander	15:932d8b4e52c9	1797	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
johnAlexander	15:932d8b4e52c9	1798	Status = VL53L0X_GetInterruptMaskStatus(Dev, &InterruptMask);
johnAlexander	15:932d8b4e52c9	1799	if (InterruptMask ==
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	15:932d8b4e52c9	1805	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_RANGE_STATUS,
johnAlexander	15:932d8b4e52c9	1806	&SysRangeStatusRegister);
johnAlexander	15:932d8b4e52c9	1807	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	1808	if (SysRangeStatusRegister & 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	15:932d8b4e52c9	1815	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1816	return Status;
johnAlexander	0:c523920bcc09	1817	}
johnAlexander	0:c523920bcc09	1818
johnAlexander	15:932d8b4e52c9	1819	VL53L0X_Error VL53L0X::VL53L0X_PollingDelay(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	0:c523920bcc09	1828	VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	1829	{
johnAlexander	15:932d8b4e52c9	1830	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1831	uint8_t NewDataReady = 0;
johnAlexander	15:932d8b4e52c9	1832	uint32_t LoopNb;
johnAlexander	15:932d8b4e52c9	1833
johnAlexander	15:932d8b4e52c9	1834	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	1835
johnAlexander	15:932d8b4e52c9	1836	LoopNb = 0;
johnAlexander	15:932d8b4e52c9	1837
johnAlexander	15:932d8b4e52c9	1838	do {
johnAlexander	15:932d8b4e52c9	1839	Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDataReady);
johnAlexander	15:932d8b4e52c9	1840	if (Status != 0)
johnAlexander	15:932d8b4e52c9	1841	break; /* the error is set */
johnAlexander	15:932d8b4e52c9	1842
johnAlexander	15:932d8b4e52c9	1843	if (NewDataReady == 1)
johnAlexander	15:932d8b4e52c9	1844	break; /* done note that status == 0 */
johnAlexander	15:932d8b4e52c9	1845
johnAlexander	15:932d8b4e52c9	1846	LoopNb++;
johnAlexander	15:932d8b4e52c9	1847	if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
johnAlexander	15:932d8b4e52c9	1848	Status = VL53L0X_ERROR_TIME_OUT;
johnAlexander	15:932d8b4e52c9	1849	break;
johnAlexander	15:932d8b4e52c9	1850	}
johnAlexander	15:932d8b4e52c9	1851
johnAlexander	15:932d8b4e52c9	1852	VL53L0X_PollingDelay(Dev);
johnAlexander	15:932d8b4e52c9	1853	} while (1);
johnAlexander	15:932d8b4e52c9	1854
johnAlexander	15:932d8b4e52c9	1855	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1856
johnAlexander	15:932d8b4e52c9	1857	return Status;
johnAlexander	0:c523920bcc09	1858	}
johnAlexander	0:c523920bcc09	1859
johnAlexander	0:c523920bcc09	1860	/* Group PAL Interrupt Functions */
johnAlexander	0:c523920bcc09	1861	VL53L0X_Error VL53L0X::VL53L0X_ClearInterruptMask(VL53L0X_DEV Dev, uint32_t InterruptMask)
johnAlexander	0:c523920bcc09	1862	{
johnAlexander	15:932d8b4e52c9	1863	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1864	uint8_t LoopCount;
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	15:932d8b4e52c9	1869	LoopCount = 0;
johnAlexander	15:932d8b4e52c9	1870	do {
johnAlexander	15:932d8b4e52c9	1871	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	1872	VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
johnAlexander	15:932d8b4e52c9	1873	Status \|= VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	1874	VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
johnAlexander	15:932d8b4e52c9	1875	Status \|= VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	1876	VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte);
johnAlexander	15:932d8b4e52c9	1877	LoopCount++;
johnAlexander	15:932d8b4e52c9	1878	} while (((Byte & 0x07) != 0x00)
johnAlexander	15:932d8b4e52c9	1879	&& (LoopCount < 3)
johnAlexander	15:932d8b4e52c9	1880	&& (Status == VL53L0X_ERROR_NONE));
johnAlexander	15:932d8b4e52c9	1881
johnAlexander	15:932d8b4e52c9	1882
johnAlexander	15:932d8b4e52c9	1883	if (LoopCount >= 3)
johnAlexander	15:932d8b4e52c9	1884	Status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
johnAlexander	15:932d8b4e52c9	1885
johnAlexander	15:932d8b4e52c9	1886	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	1887	return Status;
johnAlexander	0:c523920bcc09	1888	}
johnAlexander	0:c523920bcc09	1889
johnAlexander	0:c523920bcc09	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	15:932d8b4e52c9	1893	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1894
johnAlexander	15:932d8b4e52c9	1895	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1896	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
johnAlexander	15:932d8b4e52c9	1897	VL53L0X_REG_SYSRANGE_MODE_START_STOP \|
johnAlexander	15:932d8b4e52c9	1898	vhv_init_byte);
johnAlexander	15:932d8b4e52c9	1899
johnAlexander	15:932d8b4e52c9	1900	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1901	Status = VL53L0X_measurement_poll_for_completion(Dev);
johnAlexander	15:932d8b4e52c9	1902
johnAlexander	15:932d8b4e52c9	1903	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1904	Status = VL53L0X_ClearInterruptMask(Dev, 0);
johnAlexander	15:932d8b4e52c9	1905
johnAlexander	15:932d8b4e52c9	1906	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1907	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x00);
johnAlexander	15:932d8b4e52c9	1908
johnAlexander	15:932d8b4e52c9	1909	return Status;
johnAlexander	0:c523920bcc09	1910	}
johnAlexander	0:c523920bcc09	1911
johnAlexander	0:c523920bcc09	1912	VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io(VL53L0X_DEV Dev, uint8_t read_not_write,
johnAlexander	15:932d8b4e52c9	1913	uint8_t VhvSettings, uint8_t PhaseCal,
johnAlexander	15:932d8b4e52c9	1914	uint8_t pVhvSettings, uint8_t pPhaseCal,
johnAlexander	15:932d8b4e52c9	1915	const uint8_t vhv_enable, const uint8_t phase_enable)
johnAlexander	0:c523920bcc09	1916	{
johnAlexander	15:932d8b4e52c9	1917	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1918	uint8_t PhaseCalint = 0;
johnAlexander	15:932d8b4e52c9	1919
johnAlexander	15:932d8b4e52c9	1920	/* Read VHV from device */
johnAlexander	15:932d8b4e52c9	1921	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	1922	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	1923	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	1924
johnAlexander	15:932d8b4e52c9	1925	if (read_not_write) {
johnAlexander	15:932d8b4e52c9	1926	if (vhv_enable)
johnAlexander	15:932d8b4e52c9	1927	Status \|= VL53L0X_RdByte(Dev, 0xCB, pVhvSettings);
johnAlexander	15:932d8b4e52c9	1928	if (phase_enable)
johnAlexander	15:932d8b4e52c9	1929	Status \|= VL53L0X_RdByte(Dev, 0xEE, &PhaseCalint);
johnAlexander	15:932d8b4e52c9	1930	} else {
johnAlexander	15:932d8b4e52c9	1931	if (vhv_enable)
johnAlexander	15:932d8b4e52c9	1932	Status \|= VL53L0X_WrByte(Dev, 0xCB, VhvSettings);
johnAlexander	15:932d8b4e52c9	1933	if (phase_enable)
johnAlexander	15:932d8b4e52c9	1934	Status \|= VL53L0X_UpdateByte(Dev, 0xEE, 0x80, PhaseCal);
johnAlexander	15:932d8b4e52c9	1935	}
johnAlexander	15:932d8b4e52c9	1936
johnAlexander	15:932d8b4e52c9	1937	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	1938	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x01);
johnAlexander	15:932d8b4e52c9	1939	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	1940
johnAlexander	15:932d8b4e52c9	1941	*pPhaseCal = (uint8_t)(PhaseCalint&0xEF);
johnAlexander	15:932d8b4e52c9	1942
johnAlexander	15:932d8b4e52c9	1943	return Status;
johnAlexander	0:c523920bcc09	1944	}
johnAlexander	0:c523920bcc09	1945
johnAlexander	0:c523920bcc09	1946	VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1947	uint8_t *pVhvSettings, const uint8_t get_data_enable,
johnAlexander	15:932d8b4e52c9	1948	const uint8_t restore_config)
johnAlexander	0:c523920bcc09	1949	{
johnAlexander	15:932d8b4e52c9	1950	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1951	uint8_t SequenceConfig = 0;
johnAlexander	15:932d8b4e52c9	1952	uint8_t VhvSettings = 0;
johnAlexander	15:932d8b4e52c9	1953	uint8_t PhaseCal = 0;
johnAlexander	15:932d8b4e52c9	1954	uint8_t PhaseCalInt = 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	15:932d8b4e52c9	1961	SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
johnAlexander	15:932d8b4e52c9	1962
johnAlexander	15:932d8b4e52c9	1963	/* Run VHV */
johnAlexander	15:932d8b4e52c9	1964	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
johnAlexander	15:932d8b4e52c9	1965
johnAlexander	15:932d8b4e52c9	1966	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1967	Status = VL53L0X_perform_single_ref_calibration(Dev, 0x40);
johnAlexander	15:932d8b4e52c9	1968
johnAlexander	15:932d8b4e52c9	1969	/* Read VHV from device */
johnAlexander	15:932d8b4e52c9	1970	if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
johnAlexander	15:932d8b4e52c9	1971	Status = VL53L0X_ref_calibration_io(Dev, 1,
johnAlexander	15:932d8b4e52c9	1972	VhvSettings, PhaseCal, /* Not used here */
johnAlexander	15:932d8b4e52c9	1973	pVhvSettings, &PhaseCalInt,
johnAlexander	15:932d8b4e52c9	1974	1, 0);
johnAlexander	15:932d8b4e52c9	1975	} else
johnAlexander	15:932d8b4e52c9	1976	*pVhvSettings = 0;
johnAlexander	15:932d8b4e52c9	1977
johnAlexander	15:932d8b4e52c9	1978
johnAlexander	15:932d8b4e52c9	1979	if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
johnAlexander	15:932d8b4e52c9	1980	/* restore the previous Sequence Config */
johnAlexander	15:932d8b4e52c9	1981	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	1982	SequenceConfig);
johnAlexander	15:932d8b4e52c9	1983	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	1984	PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
johnAlexander	15:932d8b4e52c9	1985
johnAlexander	15:932d8b4e52c9	1986	}
johnAlexander	15:932d8b4e52c9	1987
johnAlexander	15:932d8b4e52c9	1988	return Status;
johnAlexander	0:c523920bcc09	1989	}
johnAlexander	0:c523920bcc09	1990
johnAlexander	0:c523920bcc09	1991	VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	1992	uint8_t *pPhaseCal, const uint8_t get_data_enable,
johnAlexander	15:932d8b4e52c9	1993	const uint8_t restore_config)
johnAlexander	0:c523920bcc09	1994	{
johnAlexander	15:932d8b4e52c9	1995	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	1996	uint8_t SequenceConfig = 0;
johnAlexander	15:932d8b4e52c9	1997	uint8_t VhvSettings = 0;
johnAlexander	15:932d8b4e52c9	1998	uint8_t PhaseCal = 0;
johnAlexander	15:932d8b4e52c9	1999	uint8_t VhvSettingsint;
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	15:932d8b4e52c9	2006	SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
johnAlexander	15:932d8b4e52c9	2007
johnAlexander	15:932d8b4e52c9	2008	/* Run PhaseCal */
johnAlexander	15:932d8b4e52c9	2009	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
johnAlexander	15:932d8b4e52c9	2010
johnAlexander	15:932d8b4e52c9	2011	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2012	Status = VL53L0X_perform_single_ref_calibration(Dev, 0x0);
johnAlexander	15:932d8b4e52c9	2013
johnAlexander	15:932d8b4e52c9	2014	/* Read PhaseCal from device */
johnAlexander	15:932d8b4e52c9	2015	if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
johnAlexander	15:932d8b4e52c9	2016	Status = VL53L0X_ref_calibration_io(Dev, 1,
johnAlexander	15:932d8b4e52c9	2017	VhvSettings, PhaseCal, /* Not used here */
johnAlexander	15:932d8b4e52c9	2018	&VhvSettingsint, pPhaseCal,
johnAlexander	15:932d8b4e52c9	2019	0, 1);
johnAlexander	15:932d8b4e52c9	2020	} else
johnAlexander	15:932d8b4e52c9	2021	*pPhaseCal = 0;
johnAlexander	15:932d8b4e52c9	2022
johnAlexander	15:932d8b4e52c9	2023
johnAlexander	15:932d8b4e52c9	2024	if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
johnAlexander	15:932d8b4e52c9	2025	/* restore the previous Sequence Config */
johnAlexander	15:932d8b4e52c9	2026	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	2027	SequenceConfig);
johnAlexander	15:932d8b4e52c9	2028	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2029	PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
johnAlexander	15:932d8b4e52c9	2030
johnAlexander	15:932d8b4e52c9	2031	}
johnAlexander	15:932d8b4e52c9	2032
johnAlexander	15:932d8b4e52c9	2033	return Status;
johnAlexander	0:c523920bcc09	2034	}
johnAlexander	0:c523920bcc09	2035
johnAlexander	0:c523920bcc09	2036	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2037	uint8_t pVhvSettings, uint8_t pPhaseCal, uint8_t get_data_enable)
johnAlexander	0:c523920bcc09	2038	{
johnAlexander	15:932d8b4e52c9	2039	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2040	uint8_t SequenceConfig = 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	15:932d8b4e52c9	2046	SequenceConfig = 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	15:932d8b4e52c9	2050	Status = VL53L0X_perform_vhv_calibration(
johnAlexander	15:932d8b4e52c9	2051	Dev, pVhvSettings, get_data_enable, 0);
johnAlexander	15:932d8b4e52c9	2052
johnAlexander	15:932d8b4e52c9	2053
johnAlexander	15:932d8b4e52c9	2054	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2055	Status = VL53L0X_perform_phase_calibration(
johnAlexander	15:932d8b4e52c9	2056	Dev, pPhaseCal, get_data_enable, 0);
johnAlexander	15:932d8b4e52c9	2057
johnAlexander	15:932d8b4e52c9	2058
johnAlexander	15:932d8b4e52c9	2059	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2060	/* restore the previous Sequence Config */
johnAlexander	15:932d8b4e52c9	2061	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	2062	SequenceConfig);
johnAlexander	15:932d8b4e52c9	2063	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2064	PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
johnAlexander	15:932d8b4e52c9	2065
johnAlexander	15:932d8b4e52c9	2066	}
johnAlexander	15:932d8b4e52c9	2067
johnAlexander	15:932d8b4e52c9	2068	return Status;
johnAlexander	0:c523920bcc09	2069	}
johnAlexander	0:c523920bcc09	2070
johnAlexander	0:c523920bcc09	2071	void VL53L0X::get_next_good_spad(uint8_t goodSpadArray[], uint32_t size,
johnAlexander	15:932d8b4e52c9	2072	uint32_t curr, int32_t *next)
johnAlexander	0:c523920bcc09	2073	{
johnAlexander	15:932d8b4e52c9	2074	uint32_t startIndex;
johnAlexander	15:932d8b4e52c9	2075	uint32_t fineOffset;
johnAlexander	15:932d8b4e52c9	2076	uint32_t cSpadsPerByte = 8;
johnAlexander	15:932d8b4e52c9	2077	uint32_t coarseIndex;
johnAlexander	15:932d8b4e52c9	2078	uint32_t fineIndex;
johnAlexander	15:932d8b4e52c9	2079	uint8_t dataByte;
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	15:932d8b4e52c9	2090	*next = -1;
johnAlexander	15:932d8b4e52c9	2091
johnAlexander	15:932d8b4e52c9	2092	startIndex = curr / cSpadsPerByte;
johnAlexander	15:932d8b4e52c9	2093	fineOffset = curr % cSpadsPerByte;
johnAlexander	15:932d8b4e52c9	2094
johnAlexander	15:932d8b4e52c9	2095	for (coarseIndex = startIndex; ((coarseIndex < size) && !success);
johnAlexander	15:932d8b4e52c9	2096	coarseIndex++) {
johnAlexander	15:932d8b4e52c9	2097	fineIndex = 0;
johnAlexander	15:932d8b4e52c9	2098	dataByte = goodSpadArray[coarseIndex];
johnAlexander	15:932d8b4e52c9	2099
johnAlexander	15:932d8b4e52c9	2100	if (coarseIndex == startIndex) {
johnAlexander	15:932d8b4e52c9	2101	/* locate the bit position of the provided current
johnAlexander	15:932d8b4e52c9	2102	* spad bit before iterating */
johnAlexander	15:932d8b4e52c9	2103	dataByte >>= fineOffset;
johnAlexander	15:932d8b4e52c9	2104	fineIndex = fineOffset;
johnAlexander	15:932d8b4e52c9	2105	}
johnAlexander	15:932d8b4e52c9	2106
johnAlexander	15:932d8b4e52c9	2107	while (fineIndex < cSpadsPerByte) {
johnAlexander	15:932d8b4e52c9	2108	if ((dataByte & 0x1) == 1) {
johnAlexander	15:932d8b4e52c9	2109	success = 1;
johnAlexander	15:932d8b4e52c9	2110	next = coarseIndex cSpadsPerByte + fineIndex;
johnAlexander	15:932d8b4e52c9	2111	break;
johnAlexander	15:932d8b4e52c9	2112	}
johnAlexander	15:932d8b4e52c9	2113	dataByte >>= 1;
johnAlexander	15:932d8b4e52c9	2114	fineIndex++;
johnAlexander	15:932d8b4e52c9	2115	}
johnAlexander	15:932d8b4e52c9	2116	}
johnAlexander	0:c523920bcc09	2117	}
johnAlexander	0:c523920bcc09	2118
johnAlexander	0:c523920bcc09	2119	uint8_t VL53L0X::is_aperture(uint32_t spadIndex)
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	15:932d8b4e52c9	2126	uint8_t isAperture = 1;
johnAlexander	15:932d8b4e52c9	2127	quadrant = spadIndex >> 6;
johnAlexander	15:932d8b4e52c9	2128	if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0)
johnAlexander	15:932d8b4e52c9	2129	isAperture = 0;
johnAlexander	15:932d8b4e52c9	2130
johnAlexander	15:932d8b4e52c9	2131	return isAperture;
johnAlexander	0:c523920bcc09	2132	}
johnAlexander	0:c523920bcc09	2133
johnAlexander	0:c523920bcc09	2134	VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spadArray[], uint32_t size,
johnAlexander	15:932d8b4e52c9	2135	uint32_t spadIndex)
johnAlexander	0:c523920bcc09	2136	{
johnAlexander	15:932d8b4e52c9	2137	VL53L0X_Error status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2138	uint32_t cSpadsPerByte = 8;
johnAlexander	15:932d8b4e52c9	2139	uint32_t coarseIndex;
johnAlexander	15:932d8b4e52c9	2140	uint32_t fineIndex;
johnAlexander	15:932d8b4e52c9	2141
johnAlexander	15:932d8b4e52c9	2142	coarseIndex = spadIndex / cSpadsPerByte;
johnAlexander	15:932d8b4e52c9	2143	fineIndex = spadIndex % cSpadsPerByte;
johnAlexander	15:932d8b4e52c9	2144	if (coarseIndex >= size)
johnAlexander	15:932d8b4e52c9	2145	status = VL53L0X_ERROR_REF_SPAD_INIT;
johnAlexander	15:932d8b4e52c9	2146	else
johnAlexander	15:932d8b4e52c9	2147	spadArray[coarseIndex] \|= (1 << fineIndex);
johnAlexander	15:932d8b4e52c9	2148
johnAlexander	15:932d8b4e52c9	2149	return status;
johnAlexander	0:c523920bcc09	2150	}
johnAlexander	0:c523920bcc09	2151
johnAlexander	0:c523920bcc09	2152	VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
johnAlexander	0:c523920bcc09	2153	{
johnAlexander	15:932d8b4e52c9	2154	VL53L0X_Error status = VL53L0X_WriteMulti(Dev,
johnAlexander	15:932d8b4e52c9	2155	VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
johnAlexander	15:932d8b4e52c9	2156	refSpadArray, 6);
johnAlexander	15:932d8b4e52c9	2157
johnAlexander	15:932d8b4e52c9	2158	return status;
johnAlexander	0:c523920bcc09	2159	}
johnAlexander	0:c523920bcc09	2160
johnAlexander	0:c523920bcc09	2161	VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
johnAlexander	0:c523920bcc09	2162	{
johnAlexander	15:932d8b4e52c9	2163	VL53L0X_Error status = VL53L0X_ReadMulti(Dev,
johnAlexander	15:932d8b4e52c9	2164	VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
johnAlexander	15:932d8b4e52c9	2165	refSpadArray,
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	0:c523920bcc09	2175	VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2176	uint8_t apertureSpads,
johnAlexander	15:932d8b4e52c9	2177	uint8_t goodSpadArray[],
johnAlexander	15:932d8b4e52c9	2178	uint8_t spadArray[],
johnAlexander	15:932d8b4e52c9	2179	uint32_t size,
johnAlexander	15:932d8b4e52c9	2180	uint32_t start,
johnAlexander	15:932d8b4e52c9	2181	uint32_t offset,
johnAlexander	15:932d8b4e52c9	2182	uint32_t spadCount,
johnAlexander	15:932d8b4e52c9	2183	uint32_t *lastSpad)
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	15:932d8b4e52c9	2188	int32_t nextGoodSpad = offset;
johnAlexander	15:932d8b4e52c9	2189	uint32_t currentSpad;
johnAlexander	15:932d8b4e52c9	2190	uint8_t checkSpadArray[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	15:932d8b4e52c9	2202	currentSpad = offset;
johnAlexander	15:932d8b4e52c9	2203	for (index = 0; index < spadCount; index++) {
johnAlexander	15:932d8b4e52c9	2204	get_next_good_spad(goodSpadArray, size, currentSpad,
johnAlexander	15:932d8b4e52c9	2205	&nextGoodSpad);
johnAlexander	15:932d8b4e52c9	2206
johnAlexander	15:932d8b4e52c9	2207	if (nextGoodSpad == -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	15:932d8b4e52c9	2213	if (is_aperture(start + nextGoodSpad) != apertureSpads) {
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	15:932d8b4e52c9	2220	currentSpad = (uint32_t)nextGoodSpad;
johnAlexander	15:932d8b4e52c9	2221	enable_spad_bit(spadArray, size, currentSpad);
johnAlexander	15:932d8b4e52c9	2222	currentSpad++;
johnAlexander	15:932d8b4e52c9	2223	}
johnAlexander	15:932d8b4e52c9	2224	*lastSpad = currentSpad;
johnAlexander	15:932d8b4e52c9	2225
johnAlexander	15:932d8b4e52c9	2226	if (status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2227	status = set_ref_spad_map(Dev, spadArray);
johnAlexander	15:932d8b4e52c9	2228
johnAlexander	15:932d8b4e52c9	2229
johnAlexander	15:932d8b4e52c9	2230	if (status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2231	status = get_ref_spad_map(Dev, checkSpadArray);
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	15:932d8b4e52c9	2237	if (spadArray[i] != checkSpadArray[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	0:c523920bcc09	2247	VL53L0X_Error VL53L0X::VL53L0X_SetDeviceMode(VL53L0X_DEV Dev, VL53L0X_DeviceModes DeviceMode)
johnAlexander	0:c523920bcc09	2248	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2253	switch (DeviceMode) {
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	15:932d8b4e52c9	2260	VL53L0X_SETPARAMETERFIELD(Dev, DeviceMode, DeviceMode);
johnAlexander	15:932d8b4e52c9	2261	break;
johnAlexander	15:932d8b4e52c9	2262	default:
johnAlexander	15:932d8b4e52c9	2263	/* Unsupported mode */
johnAlexander	15:932d8b4e52c9	2264	Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
johnAlexander	15:932d8b4e52c9	2265	}
johnAlexander	15:932d8b4e52c9	2266
johnAlexander	15:932d8b4e52c9	2267	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2268	return Status;
johnAlexander	0:c523920bcc09	2269	}
johnAlexander	0:c523920bcc09	2270
johnAlexander	13:615f7e38568c	2271	VL53L0X_Error VL53L0X::VL53L0X_SetInterruptThresholds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2272	VL53L0X_DeviceModes DeviceMode, FixPoint1616_t ThresholdLow,
johnAlexander	15:932d8b4e52c9	2273	FixPoint1616_t ThresholdHigh)
johnAlexander	13:615f7e38568c	2274	{
johnAlexander	15:932d8b4e52c9	2275	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2281	Threshold16 = (uint16_t)((ThresholdLow >> 17) & 0x00fff);
johnAlexander	15:932d8b4e52c9	2282	Status = VL53L0X_WrWord(Dev, VL53L0X_REG_SYSTEM_THRESH_LOW, Threshold16);
johnAlexander	15:932d8b4e52c9	2283
johnAlexander	15:932d8b4e52c9	2284	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2285	/* Need to divide by 2 because the FW will apply a x2 */
johnAlexander	15:932d8b4e52c9	2286	Threshold16 = (uint16_t)((ThresholdHigh >> 17) & 0x00fff);
johnAlexander	15:932d8b4e52c9	2287	Status = VL53L0X_WrWord(Dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
johnAlexander	15:932d8b4e52c9	2288	Threshold16);
johnAlexander	15:932d8b4e52c9	2289	}
johnAlexander	15:932d8b4e52c9	2290
johnAlexander	15:932d8b4e52c9	2291	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2292	return Status;
johnAlexander	13:615f7e38568c	2293	}
johnAlexander	13:615f7e38568c	2294
johnAlexander	0:c523920bcc09	2295	VL53L0X_Error VL53L0X::VL53L0X_GetInterruptThresholds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2296	VL53L0X_DeviceModes DeviceMode, FixPoint1616_t *pThresholdLow,
johnAlexander	15:932d8b4e52c9	2297	FixPoint1616_t *pThresholdHigh)
johnAlexander	0:c523920bcc09	2298	{
johnAlexander	15:932d8b4e52c9	2299	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2305	Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &Threshold16);
johnAlexander	15:932d8b4e52c9	2306	/* Need to multiply by 2 because the FW will apply a x2 */
johnAlexander	15:932d8b4e52c9	2307	*pThresholdLow = (FixPoint1616_t)((0x00fff & Threshold16) << 17);
johnAlexander	15:932d8b4e52c9	2308
johnAlexander	15:932d8b4e52c9	2309	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2310	Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
johnAlexander	15:932d8b4e52c9	2311	&Threshold16);
johnAlexander	15:932d8b4e52c9	2312	/* Need to multiply by 2 because the FW will apply a x2 */
johnAlexander	15:932d8b4e52c9	2313	*pThresholdHigh =
johnAlexander	15:932d8b4e52c9	2314	(FixPoint1616_t)((0x00fff & Threshold16) << 17);
johnAlexander	15:932d8b4e52c9	2315	}
johnAlexander	15:932d8b4e52c9	2316
johnAlexander	15:932d8b4e52c9	2317	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2318	return Status;
johnAlexander	0:c523920bcc09	2319	}
johnAlexander	0:c523920bcc09	2320
johnAlexander	0:c523920bcc09	2321	VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2322	uint8_t *pTuningSettingBuffer)
johnAlexander	0:c523920bcc09	2323	{
johnAlexander	15:932d8b4e52c9	2324	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2325	int i;
johnAlexander	15:932d8b4e52c9	2326	int Index;
johnAlexander	15:932d8b4e52c9	2327	uint8_t msb;
johnAlexander	15:932d8b4e52c9	2328	uint8_t lsb;
johnAlexander	15:932d8b4e52c9	2329	uint8_t SelectParam;
johnAlexander	15:932d8b4e52c9	2330	uint8_t NumberOfWrites;
johnAlexander	15:932d8b4e52c9	2331	uint8_t Address;
johnAlexander	15:932d8b4e52c9	2332	uint8_t localBuffer[4]; /* max */
johnAlexander	15:932d8b4e52c9	2333	uint16_t Temp16;
johnAlexander	15:932d8b4e52c9	2334
johnAlexander	15:932d8b4e52c9	2335	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	2336
johnAlexander	15:932d8b4e52c9	2337	Index = 0;
johnAlexander	15:932d8b4e52c9	2338
johnAlexander	15:932d8b4e52c9	2339	while ((*(pTuningSettingBuffer + Index) != 0) &&
johnAlexander	15:932d8b4e52c9	2340	(Status == VL53L0X_ERROR_NONE)) {
johnAlexander	15:932d8b4e52c9	2341	NumberOfWrites = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2342	Index++;
johnAlexander	15:932d8b4e52c9	2343	if (NumberOfWrites == 0xFF) {
johnAlexander	15:932d8b4e52c9	2344	/* internal parameters */
johnAlexander	15:932d8b4e52c9	2345	SelectParam = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2346	Index++;
johnAlexander	15:932d8b4e52c9	2347	switch (SelectParam) {
johnAlexander	15:932d8b4e52c9	2348	case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
johnAlexander	15:932d8b4e52c9	2349	msb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2350	Index++;
johnAlexander	15:932d8b4e52c9	2351	lsb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2352	Index++;
johnAlexander	15:932d8b4e52c9	2353	Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
johnAlexander	15:932d8b4e52c9	2354	PALDevDataSet(Dev, SigmaEstRefArray, Temp16);
johnAlexander	15:932d8b4e52c9	2355	break;
johnAlexander	15:932d8b4e52c9	2356	case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
johnAlexander	15:932d8b4e52c9	2357	msb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2358	Index++;
johnAlexander	15:932d8b4e52c9	2359	lsb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2360	Index++;
johnAlexander	15:932d8b4e52c9	2361	Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
johnAlexander	15:932d8b4e52c9	2362	PALDevDataSet(Dev, SigmaEstEffPulseWidth,
johnAlexander	15:932d8b4e52c9	2363	Temp16);
johnAlexander	15:932d8b4e52c9	2364	break;
johnAlexander	15:932d8b4e52c9	2365	case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
johnAlexander	15:932d8b4e52c9	2366	msb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2367	Index++;
johnAlexander	15:932d8b4e52c9	2368	lsb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2369	Index++;
johnAlexander	15:932d8b4e52c9	2370	Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
johnAlexander	15:932d8b4e52c9	2371	PALDevDataSet(Dev, SigmaEstEffAmbWidth, Temp16);
johnAlexander	15:932d8b4e52c9	2372	break;
johnAlexander	15:932d8b4e52c9	2373	case 3: /* uint16_t targetRefRate -> 2 bytes */
johnAlexander	15:932d8b4e52c9	2374	msb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2375	Index++;
johnAlexander	15:932d8b4e52c9	2376	lsb = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2377	Index++;
johnAlexander	15:932d8b4e52c9	2378	Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
johnAlexander	15:932d8b4e52c9	2379	PALDevDataSet(Dev, targetRefRate, Temp16);
johnAlexander	15:932d8b4e52c9	2380	break;
johnAlexander	15:932d8b4e52c9	2381	default: /* invalid parameter */
johnAlexander	15:932d8b4e52c9	2382	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	2383	}
johnAlexander	15:932d8b4e52c9	2384
johnAlexander	15:932d8b4e52c9	2385	} else if (NumberOfWrites <= 4) {
johnAlexander	15:932d8b4e52c9	2386	Address = *(pTuningSettingBuffer + Index);
johnAlexander	15:932d8b4e52c9	2387	Index++;
johnAlexander	15:932d8b4e52c9	2388
johnAlexander	15:932d8b4e52c9	2389	for (i = 0; i < NumberOfWrites; i++) {
johnAlexander	15:932d8b4e52c9	2390	localBuffer[i] = *(pTuningSettingBuffer +
johnAlexander	15:932d8b4e52c9	2391	Index);
johnAlexander	15:932d8b4e52c9	2392	Index++;
johnAlexander	15:932d8b4e52c9	2393	}
johnAlexander	15:932d8b4e52c9	2394
johnAlexander	15:932d8b4e52c9	2395	Status = VL53L0X_WriteMulti(Dev, Address, localBuffer,
johnAlexander	15:932d8b4e52c9	2396	NumberOfWrites);
johnAlexander	15:932d8b4e52c9	2397
johnAlexander	15:932d8b4e52c9	2398	} else {
johnAlexander	15:932d8b4e52c9	2399	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	2400	}
johnAlexander	15:932d8b4e52c9	2401	}
johnAlexander	15:932d8b4e52c9	2402
johnAlexander	15:932d8b4e52c9	2403	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2404	return Status;
johnAlexander	0:c523920bcc09	2405	}
johnAlexander	0:c523920bcc09	2406
johnAlexander	0:c523920bcc09	2407	VL53L0X_Error VL53L0X::VL53L0X_CheckAndLoadInterruptSettings(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2408	uint8_t StartNotStopFlag)
johnAlexander	0:c523920bcc09	2409	{
johnAlexander	15:932d8b4e52c9	2410	uint8_t InterruptConfig;
johnAlexander	15:932d8b4e52c9	2411	FixPoint1616_t ThresholdLow;
johnAlexander	15:932d8b4e52c9	2412	FixPoint1616_t ThresholdHigh;
johnAlexander	15:932d8b4e52c9	2413	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2414
johnAlexander	15:932d8b4e52c9	2415	InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	2416	Pin0GpioFunctionality);
johnAlexander	15:932d8b4e52c9	2417
johnAlexander	15:932d8b4e52c9	2418	if ((InterruptConfig ==
johnAlexander	15:932d8b4e52c9	2419	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) \|\|
johnAlexander	15:932d8b4e52c9	2420	(InterruptConfig ==
johnAlexander	15:932d8b4e52c9	2421	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) \|\|
johnAlexander	15:932d8b4e52c9	2422	(InterruptConfig ==
johnAlexander	15:932d8b4e52c9	2423	VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
johnAlexander	15:932d8b4e52c9	2424
johnAlexander	15:932d8b4e52c9	2425	Status = VL53L0X_GetInterruptThresholds(Dev,
johnAlexander	15:932d8b4e52c9	2426	VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
johnAlexander	15:932d8b4e52c9	2427	&ThresholdLow, &ThresholdHigh);
johnAlexander	15:932d8b4e52c9	2428
johnAlexander	15:932d8b4e52c9	2429	if (((ThresholdLow > 255*65536) \|\|
johnAlexander	15:932d8b4e52c9	2430	(ThresholdHigh > 255*65536)) &&
johnAlexander	15:932d8b4e52c9	2431	(Status == VL53L0X_ERROR_NONE)) {
johnAlexander	15:932d8b4e52c9	2432
johnAlexander	15:932d8b4e52c9	2433	if (StartNotStopFlag != 0) {
johnAlexander	15:932d8b4e52c9	2434	Status = VL53L0X_load_tuning_settings(Dev,
johnAlexander	15:932d8b4e52c9	2435	InterruptThresholdSettings);
johnAlexander	15:932d8b4e52c9	2436	} else {
johnAlexander	15:932d8b4e52c9	2437	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x04);
johnAlexander	15:932d8b4e52c9	2438	Status \|= VL53L0X_WrByte(Dev, 0x70, 0x00);
johnAlexander	15:932d8b4e52c9	2439	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	2440	Status \|= VL53L0X_WrByte(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	15:932d8b4e52c9	2448	return Status;
johnAlexander	0:c523920bcc09	2449
johnAlexander	0:c523920bcc09	2450	}
johnAlexander	0:c523920bcc09	2451
johnAlexander	0:c523920bcc09	2452	VL53L0X_Error VL53L0X::VL53L0X_StartMeasurement(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	2453	{
johnAlexander	15:932d8b4e52c9	2454	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2455	VL53L0X_DeviceModes DeviceMode;
johnAlexander	15:932d8b4e52c9	2456	uint8_t Byte;
johnAlexander	15:932d8b4e52c9	2457	uint8_t StartStopByte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
johnAlexander	15:932d8b4e52c9	2458	uint32_t LoopNb;
johnAlexander	15:932d8b4e52c9	2459	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	2460
johnAlexander	15:932d8b4e52c9	2461	/* Get Current DeviceMode */
johnAlexander	15:932d8b4e52c9	2462	VL53L0X_GetDeviceMode(Dev, &DeviceMode);
johnAlexander	15:932d8b4e52c9	2463
johnAlexander	15:932d8b4e52c9	2464	Status = VL53L0X_WrByte(Dev, 0x80, 0x01);
johnAlexander	15:932d8b4e52c9	2465	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	2466	Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	2467	Status = VL53L0X_WrByte(Dev, 0x91, PALDevDataGet(Dev, StopVariable));
johnAlexander	15:932d8b4e52c9	2468	Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
johnAlexander	15:932d8b4e52c9	2469	Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	2470	Status = VL53L0X_WrByte(Dev, 0x80, 0x00);
johnAlexander	15:932d8b4e52c9	2471
johnAlexander	15:932d8b4e52c9	2472	switch (DeviceMode) {
johnAlexander	15:932d8b4e52c9	2473	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
johnAlexander	15:932d8b4e52c9	2474	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x01);
johnAlexander	15:932d8b4e52c9	2475
johnAlexander	15:932d8b4e52c9	2476	Byte = StartStopByte;
johnAlexander	15:932d8b4e52c9	2477	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2478	/* Wait until start bit has been cleared */
johnAlexander	15:932d8b4e52c9	2479	LoopNb = 0;
johnAlexander	15:932d8b4e52c9	2480	do {
johnAlexander	15:932d8b4e52c9	2481	if (LoopNb > 0)
johnAlexander	15:932d8b4e52c9	2482	Status = VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	2483	VL53L0X_REG_SYSRANGE_START, &Byte);
johnAlexander	15:932d8b4e52c9	2484	LoopNb = LoopNb + 1;
johnAlexander	15:932d8b4e52c9	2485	} while (((Byte & StartStopByte) == StartStopByte)
johnAlexander	15:932d8b4e52c9	2486	&& (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2487	&& (LoopNb < VL53L0X_DEFAULT_MAX_LOOP));
johnAlexander	15:932d8b4e52c9	2488
johnAlexander	15:932d8b4e52c9	2489	if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2499	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2500	Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);
johnAlexander	15:932d8b4e52c9	2501
johnAlexander	15:932d8b4e52c9	2502	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	2503	VL53L0X_REG_SYSRANGE_START,
johnAlexander	15:932d8b4e52c9	2504	VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2513	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2514	Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);
johnAlexander	15:932d8b4e52c9	2515
johnAlexander	15:932d8b4e52c9	2516	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	2517	VL53L0X_REG_SYSRANGE_START,
johnAlexander	15:932d8b4e52c9	2518	VL53L0X_REG_SYSRANGE_MODE_TIMED);
johnAlexander	15:932d8b4e52c9	2519
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2527	Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
johnAlexander	15:932d8b4e52c9	2528	}
johnAlexander	15:932d8b4e52c9	2529
johnAlexander	15:932d8b4e52c9	2530
johnAlexander	15:932d8b4e52c9	2531	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2532	return Status;
johnAlexander	0:c523920bcc09	2533	}
johnAlexander	0:c523920bcc09	2534
johnAlexander	0:c523920bcc09	2535	/* Group PAL Measurement Functions */
johnAlexander	0:c523920bcc09	2536	VL53L0X_Error VL53L0X::VL53L0X_PerformSingleMeasurement(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	2537	{
johnAlexander	15:932d8b4e52c9	2538	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2539	VL53L0X_DeviceModes DeviceMode;
johnAlexander	15:932d8b4e52c9	2540
johnAlexander	15:932d8b4e52c9	2541	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	2542
johnAlexander	15:932d8b4e52c9	2543	/* Get Current DeviceMode */
johnAlexander	15:932d8b4e52c9	2544	Status = VL53L0X_GetDeviceMode(Dev, &DeviceMode);
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	15:932d8b4e52c9	2548	if (Status == VL53L0X_ERROR_NONE
johnAlexander	15:932d8b4e52c9	2549	&& DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
johnAlexander	15:932d8b4e52c9	2550	Status = VL53L0X_StartMeasurement(Dev);
johnAlexander	15:932d8b4e52c9	2551
johnAlexander	15:932d8b4e52c9	2552
johnAlexander	15:932d8b4e52c9	2553	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2558	if (Status == VL53L0X_ERROR_NONE
johnAlexander	15:932d8b4e52c9	2559	&& DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
johnAlexander	15:932d8b4e52c9	2560	PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE);
johnAlexander	15:932d8b4e52c9	2561
johnAlexander	15:932d8b4e52c9	2562
johnAlexander	15:932d8b4e52c9	2563	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2564	return Status;
johnAlexander	0:c523920bcc09	2565	}
johnAlexander	0:c523920bcc09	2566
johnAlexander	0:c523920bcc09	2567	VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationEnable(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2568	uint8_t *pXTalkCompensationEnable)
johnAlexander	0:c523920bcc09	2569	{
johnAlexander	15:932d8b4e52c9	2570	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2571	uint8_t Temp8;
johnAlexander	15:932d8b4e52c9	2572	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	2573
johnAlexander	15:932d8b4e52c9	2574	VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable, Temp8);
johnAlexander	15:932d8b4e52c9	2575	*pXTalkCompensationEnable = Temp8;
johnAlexander	15:932d8b4e52c9	2576
johnAlexander	15:932d8b4e52c9	2577	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2578	return Status;
johnAlexander	0:c523920bcc09	2579	}
johnAlexander	0:c523920bcc09	2580
johnAlexander	0:c523920bcc09	2581	VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2582	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
johnAlexander	15:932d8b4e52c9	2583	FixPoint1616_t *ptotal_xtalk_rate_mcps)
johnAlexander	0:c523920bcc09	2584	{
johnAlexander	15:932d8b4e52c9	2585	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2586
johnAlexander	15:932d8b4e52c9	2587	uint8_t xtalkCompEnable;
johnAlexander	15:932d8b4e52c9	2588	FixPoint1616_t totalXtalkMegaCps;
johnAlexander	15:932d8b4e52c9	2589	FixPoint1616_t xtalkPerSpadMegaCps;
johnAlexander	15:932d8b4e52c9	2590
johnAlexander	15:932d8b4e52c9	2591	*ptotal_xtalk_rate_mcps = 0;
johnAlexander	15:932d8b4e52c9	2592
johnAlexander	15:932d8b4e52c9	2593	Status = VL53L0X_GetXTalkCompensationEnable(Dev, &xtalkCompEnable);
johnAlexander	15:932d8b4e52c9	2594	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2595
johnAlexander	15:932d8b4e52c9	2596	if (xtalkCompEnable) {
johnAlexander	15:932d8b4e52c9	2597
johnAlexander	15:932d8b4e52c9	2598	VL53L0X_GETPARAMETERFIELD(
johnAlexander	15:932d8b4e52c9	2599	Dev,
johnAlexander	15:932d8b4e52c9	2600	XTalkCompensationRateMegaCps,
johnAlexander	15:932d8b4e52c9	2601	xtalkPerSpadMegaCps);
johnAlexander	15:932d8b4e52c9	2602
johnAlexander	15:932d8b4e52c9	2603	/* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
johnAlexander	15:932d8b4e52c9	2604	totalXtalkMegaCps =
johnAlexander	15:932d8b4e52c9	2605	pRangingMeasurementData->EffectiveSpadRtnCount *
johnAlexander	15:932d8b4e52c9	2606	xtalkPerSpadMegaCps;
johnAlexander	15:932d8b4e52c9	2607
johnAlexander	15:932d8b4e52c9	2608	/* FixPoint0824 >> 8 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	2609	*ptotal_xtalk_rate_mcps =
johnAlexander	15:932d8b4e52c9	2610	(totalXtalkMegaCps + 0x80) >> 8;
johnAlexander	15:932d8b4e52c9	2611	}
johnAlexander	15:932d8b4e52c9	2612	}
johnAlexander	15:932d8b4e52c9	2613
johnAlexander	15:932d8b4e52c9	2614	return Status;
johnAlexander	0:c523920bcc09	2615	}
johnAlexander	0:c523920bcc09	2616
johnAlexander	0:c523920bcc09	2617	VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2618	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
johnAlexander	15:932d8b4e52c9	2619	FixPoint1616_t *ptotal_signal_rate_mcps)
johnAlexander	0:c523920bcc09	2620	{
johnAlexander	15:932d8b4e52c9	2621	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2622	FixPoint1616_t totalXtalkMegaCps;
johnAlexander	15:932d8b4e52c9	2623
johnAlexander	15:932d8b4e52c9	2624	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	2625
johnAlexander	15:932d8b4e52c9	2626	*ptotal_signal_rate_mcps =
johnAlexander	15:932d8b4e52c9	2627	pRangingMeasurementData->SignalRateRtnMegaCps;
johnAlexander	15:932d8b4e52c9	2628
johnAlexander	15:932d8b4e52c9	2629	Status = VL53L0X_get_total_xtalk_rate(
johnAlexander	15:932d8b4e52c9	2630	Dev, pRangingMeasurementData, &totalXtalkMegaCps);
johnAlexander	15:932d8b4e52c9	2631
johnAlexander	15:932d8b4e52c9	2632	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	2633	*ptotal_signal_rate_mcps += totalXtalkMegaCps;
johnAlexander	15:932d8b4e52c9	2634
johnAlexander	15:932d8b4e52c9	2635	return Status;
johnAlexander	0:c523920bcc09	2636	}
johnAlexander	0:c523920bcc09	2637
johnAlexander	0:c523920bcc09	2638	/* To convert ms into register value */
johnAlexander	0:c523920bcc09	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	15:932d8b4e52c9	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	0:c523920bcc09	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	0:c523920bcc09	2688	VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(
johnAlexander	15:932d8b4e52c9	2689	VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2690	FixPoint1616_t totalSignalRate_mcps,
johnAlexander	15:932d8b4e52c9	2691	FixPoint1616_t totalCorrSignalRate_mcps,
johnAlexander	15:932d8b4e52c9	2692	FixPoint1616_t pwMult,
johnAlexander	15:932d8b4e52c9	2693	uint32_t sigmaEstimateP1,
johnAlexander	15:932d8b4e52c9	2694	FixPoint1616_t sigmaEstimateP2,
johnAlexander	15:932d8b4e52c9	2695	uint32_t peakVcselDuration_us,
johnAlexander	15:932d8b4e52c9	2696	uint32_t *pdmax_mm)
johnAlexander	0:c523920bcc09	2697	{
johnAlexander	15:932d8b4e52c9	2698	const uint32_t cSigmaLimit = 18;
johnAlexander	15:932d8b4e52c9	2699	const FixPoint1616_t cSignalLimit = 0x4000; /* 0.25 */
johnAlexander	15:932d8b4e52c9	2700	const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
johnAlexander	15:932d8b4e52c9	2701	const uint32_t cAmbEffWidthSigmaEst_ns = 6;
johnAlexander	15:932d8b4e52c9	2702	const uint32_t cAmbEffWidthDMax_ns = 7;
johnAlexander	15:932d8b4e52c9	2703	uint32_t dmaxCalRange_mm;
johnAlexander	15:932d8b4e52c9	2704	FixPoint1616_t dmaxCalSignalRateRtn_mcps;
johnAlexander	15:932d8b4e52c9	2705	FixPoint1616_t minSignalNeeded;
johnAlexander	15:932d8b4e52c9	2706	FixPoint1616_t minSignalNeeded_p1;
johnAlexander	15:932d8b4e52c9	2707	FixPoint1616_t minSignalNeeded_p2;
johnAlexander	15:932d8b4e52c9	2708	FixPoint1616_t minSignalNeeded_p3;
johnAlexander	15:932d8b4e52c9	2709	FixPoint1616_t minSignalNeeded_p4;
johnAlexander	15:932d8b4e52c9	2710	FixPoint1616_t sigmaLimitTmp;
johnAlexander	15:932d8b4e52c9	2711	FixPoint1616_t sigmaEstSqTmp;
johnAlexander	15:932d8b4e52c9	2712	FixPoint1616_t signalLimitTmp;
johnAlexander	15:932d8b4e52c9	2713	FixPoint1616_t SignalAt0mm;
johnAlexander	15:932d8b4e52c9	2714	FixPoint1616_t dmaxDark;
johnAlexander	15:932d8b4e52c9	2715	FixPoint1616_t dmaxAmbient;
johnAlexander	15:932d8b4e52c9	2716	FixPoint1616_t dmaxDarkTmp;
johnAlexander	15:932d8b4e52c9	2717	FixPoint1616_t sigmaEstP2Tmp;
johnAlexander	15:932d8b4e52c9	2718	uint32_t signalRateTemp_mcps;
johnAlexander	15:932d8b4e52c9	2719
johnAlexander	15:932d8b4e52c9	2720	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	2721
johnAlexander	15:932d8b4e52c9	2722	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	2723
johnAlexander	15:932d8b4e52c9	2724	dmaxCalRange_mm =
johnAlexander	15:932d8b4e52c9	2725	PALDevDataGet(Dev, DmaxCalRangeMilliMeter);
johnAlexander	15:932d8b4e52c9	2726
johnAlexander	15:932d8b4e52c9	2727	dmaxCalSignalRateRtn_mcps =
johnAlexander	15:932d8b4e52c9	2728	PALDevDataGet(Dev, DmaxCalSignalRateRtnMegaCps);
johnAlexander	15:932d8b4e52c9	2729
johnAlexander	15:932d8b4e52c9	2730	/* uint32 * FixPoint1616 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	2731	SignalAt0mm = dmaxCalRange_mm * dmaxCalSignalRateRtn_mcps;
johnAlexander	15:932d8b4e52c9	2732
johnAlexander	15:932d8b4e52c9	2733	/* FixPoint1616 >> 8 = FixPoint2408 */
johnAlexander	15:932d8b4e52c9	2734	SignalAt0mm = (SignalAt0mm + 0x80) >> 8;
johnAlexander	15:932d8b4e52c9	2735	SignalAt0mm *= dmaxCalRange_mm;
johnAlexander	15:932d8b4e52c9	2736
johnAlexander	15:932d8b4e52c9	2737	minSignalNeeded_p1 = 0;
johnAlexander	15:932d8b4e52c9	2738	if (totalCorrSignalRate_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	15:932d8b4e52c9	2742	signalRateTemp_mcps = totalSignalRate_mcps << 10;
johnAlexander	15:932d8b4e52c9	2743
johnAlexander	15:932d8b4e52c9	2744	/* Add rounding value prior to division */
johnAlexander	15:932d8b4e52c9	2745	minSignalNeeded_p1 = signalRateTemp_mcps +
johnAlexander	15:932d8b4e52c9	2746	(totalCorrSignalRate_mcps/2);
johnAlexander	15:932d8b4e52c9	2747
johnAlexander	15:932d8b4e52c9	2748	/* FixPoint0626/FixPoint1616 = FixPoint2210 */
johnAlexander	15:932d8b4e52c9	2749	minSignalNeeded_p1 /= totalCorrSignalRate_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	15:932d8b4e52c9	2753	minSignalNeeded_p1 *= 3;
johnAlexander	15:932d8b4e52c9	2754
johnAlexander	15:932d8b4e52c9	2755	/* FixPoint2210 * FixPoint2210 = FixPoint1220 */
johnAlexander	15:932d8b4e52c9	2756	minSignalNeeded_p1 *= minSignalNeeded_p1;
johnAlexander	15:932d8b4e52c9	2757
johnAlexander	15:932d8b4e52c9	2758	/* FixPoint1220 >> 16 = FixPoint2804 */
johnAlexander	15:932d8b4e52c9	2759	minSignalNeeded_p1 = (minSignalNeeded_p1 + 0x8000) >> 16;
johnAlexander	15:932d8b4e52c9	2760	}
johnAlexander	15:932d8b4e52c9	2761
johnAlexander	15:932d8b4e52c9	2762	minSignalNeeded_p2 = pwMult * sigmaEstimateP1;
johnAlexander	15:932d8b4e52c9	2763
johnAlexander	15:932d8b4e52c9	2764	/* FixPoint1616 >> 16 = uint32 */
johnAlexander	15:932d8b4e52c9	2765	minSignalNeeded_p2 = (minSignalNeeded_p2 + 0x8000) >> 16;
johnAlexander	15:932d8b4e52c9	2766
johnAlexander	15:932d8b4e52c9	2767	/* uint32 * uint32 = uint32 */
johnAlexander	15:932d8b4e52c9	2768	minSignalNeeded_p2 *= minSignalNeeded_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	15:932d8b4e52c9	2775	sigmaEstP2Tmp = (sigmaEstimateP2 + 0x8000) >> 16;
johnAlexander	15:932d8b4e52c9	2776	sigmaEstP2Tmp = (sigmaEstP2Tmp + cAmbEffWidthSigmaEst_ns/2)/
johnAlexander	15:932d8b4e52c9	2777	cAmbEffWidthSigmaEst_ns;
johnAlexander	15:932d8b4e52c9	2778	sigmaEstP2Tmp *= cAmbEffWidthDMax_ns;
johnAlexander	15:932d8b4e52c9	2779
johnAlexander	15:932d8b4e52c9	2780	if (sigmaEstP2Tmp > 0xffff) {
johnAlexander	15:932d8b4e52c9	2781	minSignalNeeded_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	15:932d8b4e52c9	2788	sigmaEstimateP2 = (sigmaEstimateP2 + cAmbEffWidthSigmaEst_ns/2)/
johnAlexander	15:932d8b4e52c9	2789	cAmbEffWidthSigmaEst_ns;
johnAlexander	15:932d8b4e52c9	2790	sigmaEstimateP2 *= cAmbEffWidthDMax_ns;
johnAlexander	15:932d8b4e52c9	2791
johnAlexander	15:932d8b4e52c9	2792	/* FixPoint1616 >> 16 = uint32 */
johnAlexander	15:932d8b4e52c9	2793	minSignalNeeded_p3 = (sigmaEstimateP2 + 0x8000) >> 16;
johnAlexander	15:932d8b4e52c9	2794
johnAlexander	15:932d8b4e52c9	2795	minSignalNeeded_p3 *= minSignalNeeded_p3;
johnAlexander	15:932d8b4e52c9	2796
johnAlexander	15:932d8b4e52c9	2797	}
johnAlexander	15:932d8b4e52c9	2798
johnAlexander	15:932d8b4e52c9	2799	/* FixPoint1814 / uint32 = FixPoint1814 */
johnAlexander	15:932d8b4e52c9	2800	sigmaLimitTmp = ((cSigmaLimit << 14) + 500) / 1000;
johnAlexander	15:932d8b4e52c9	2801
johnAlexander	15:932d8b4e52c9	2802	/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
johnAlexander	15:932d8b4e52c9	2803	sigmaLimitTmp *= sigmaLimitTmp;
johnAlexander	15:932d8b4e52c9	2804
johnAlexander	15:932d8b4e52c9	2805	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
johnAlexander	15:932d8b4e52c9	2806	sigmaEstSqTmp = cSigmaEstRef * cSigmaEstRef;
johnAlexander	15:932d8b4e52c9	2807
johnAlexander	15:932d8b4e52c9	2808	/* FixPoint3232 >> 4 = FixPoint0428 */
johnAlexander	15:932d8b4e52c9	2809	sigmaEstSqTmp = (sigmaEstSqTmp + 0x08) >> 4;
johnAlexander	15:932d8b4e52c9	2810
johnAlexander	15:932d8b4e52c9	2811	/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
johnAlexander	15:932d8b4e52c9	2812	sigmaLimitTmp -= sigmaEstSqTmp;
johnAlexander	15:932d8b4e52c9	2813
johnAlexander	15:932d8b4e52c9	2814	/* uint32_t * FixPoint0428 = FixPoint0428 */
johnAlexander	15:932d8b4e52c9	2815	minSignalNeeded_p4 = 4 * 12 * sigmaLimitTmp;
johnAlexander	15:932d8b4e52c9	2816
johnAlexander	15:932d8b4e52c9	2817	/* FixPoint0428 >> 14 = FixPoint1814 */
johnAlexander	15:932d8b4e52c9	2818	minSignalNeeded_p4 = (minSignalNeeded_p4 + 0x2000) >> 14;
johnAlexander	15:932d8b4e52c9	2819
johnAlexander	15:932d8b4e52c9	2820	/* uint32 + uint32 = uint32 */
johnAlexander	15:932d8b4e52c9	2821	minSignalNeeded = (minSignalNeeded_p2 + minSignalNeeded_p3);
johnAlexander	15:932d8b4e52c9	2822
johnAlexander	15:932d8b4e52c9	2823	/* uint32 / uint32 = uint32 */
johnAlexander	15:932d8b4e52c9	2824	minSignalNeeded += (peakVcselDuration_us/2);
johnAlexander	15:932d8b4e52c9	2825	minSignalNeeded /= peakVcselDuration_us;
johnAlexander	15:932d8b4e52c9	2826
johnAlexander	15:932d8b4e52c9	2827	/* uint32 << 14 = FixPoint1814 */
johnAlexander	15:932d8b4e52c9	2828	minSignalNeeded <<= 14;
johnAlexander	15:932d8b4e52c9	2829
johnAlexander	15:932d8b4e52c9	2830	/* FixPoint1814 / FixPoint1814 = uint32 */
johnAlexander	15:932d8b4e52c9	2831	minSignalNeeded += (minSignalNeeded_p4/2);
johnAlexander	15:932d8b4e52c9	2832	minSignalNeeded /= minSignalNeeded_p4;
johnAlexander	15:932d8b4e52c9	2833
johnAlexander	15:932d8b4e52c9	2834	/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
johnAlexander	15:932d8b4e52c9	2835	minSignalNeeded *= minSignalNeeded_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	15:932d8b4e52c9	2844	minSignalNeeded = (minSignalNeeded + 500) / 1000;
johnAlexander	15:932d8b4e52c9	2845	minSignalNeeded <<= 4;
johnAlexander	15:932d8b4e52c9	2846
johnAlexander	15:932d8b4e52c9	2847	minSignalNeeded = (minSignalNeeded + 500) / 1000;
johnAlexander	15:932d8b4e52c9	2848
johnAlexander	15:932d8b4e52c9	2849	/* FixPoint1616 >> 8 = FixPoint2408 */
johnAlexander	15:932d8b4e52c9	2850	signalLimitTmp = (cSignalLimit + 0x80) >> 8;
johnAlexander	15:932d8b4e52c9	2851
johnAlexander	15:932d8b4e52c9	2852	/* FixPoint2408/FixPoint2408 = uint32 */
johnAlexander	15:932d8b4e52c9	2853	if (signalLimitTmp != 0)
johnAlexander	15:932d8b4e52c9	2854	dmaxDarkTmp = (SignalAt0mm + (signalLimitTmp / 2))
johnAlexander	15:932d8b4e52c9	2855	/ signalLimitTmp;
johnAlexander	15:932d8b4e52c9	2856	else
johnAlexander	15:932d8b4e52c9	2857	dmaxDarkTmp = 0;
johnAlexander	15:932d8b4e52c9	2858
johnAlexander	15:932d8b4e52c9	2859	dmaxDark = VL53L0X_isqrt(dmaxDarkTmp);
johnAlexander	15:932d8b4e52c9	2860
johnAlexander	15:932d8b4e52c9	2861	/* FixPoint2408/FixPoint2408 = uint32 */
johnAlexander	15:932d8b4e52c9	2862	if (minSignalNeeded != 0)
johnAlexander	15:932d8b4e52c9	2863	dmaxAmbient = (SignalAt0mm + minSignalNeeded/2)
johnAlexander	15:932d8b4e52c9	2864	/ minSignalNeeded;
johnAlexander	15:932d8b4e52c9	2865	else
johnAlexander	15:932d8b4e52c9	2866	dmaxAmbient = 0;
johnAlexander	15:932d8b4e52c9	2867
johnAlexander	15:932d8b4e52c9	2868	dmaxAmbient = VL53L0X_isqrt(dmaxAmbient);
johnAlexander	15:932d8b4e52c9	2869
johnAlexander	15:932d8b4e52c9	2870	*pdmax_mm = dmaxDark;
johnAlexander	15:932d8b4e52c9	2871	if (dmaxDark > dmaxAmbient)
johnAlexander	15:932d8b4e52c9	2872	*pdmax_mm = dmaxAmbient;
johnAlexander	15:932d8b4e52c9	2873
johnAlexander	15:932d8b4e52c9	2874	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	2875
johnAlexander	15:932d8b4e52c9	2876	return Status;
johnAlexander	0:c523920bcc09	2877	}
johnAlexander	0:c523920bcc09	2878
johnAlexander	0:c523920bcc09	2879	VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	2880	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
johnAlexander	15:932d8b4e52c9	2881	FixPoint1616_t *pSigmaEstimate,
johnAlexander	15:932d8b4e52c9	2882	uint32_t *pDmax_mm)
johnAlexander	0:c523920bcc09	2883	{
johnAlexander	15:932d8b4e52c9	2884	/* Expressed in 100ths of a ns, i.e. centi-ns */
johnAlexander	15:932d8b4e52c9	2885	const uint32_t cPulseEffectiveWidth_centi_ns = 800;
johnAlexander	15:932d8b4e52c9	2886	/* Expressed in 100ths of a ns, i.e. centi-ns */
johnAlexander	15:932d8b4e52c9	2887	const uint32_t cAmbientEffectiveWidth_centi_ns = 600;
johnAlexander	15:932d8b4e52c9	2888	const FixPoint1616_t cDfltFinalRangeIntegrationTimeMilliSecs = 0x00190000; /* 25ms */
johnAlexander	15:932d8b4e52c9	2889	const uint32_t cVcselPulseWidth_ps = 4700; /* pico secs */
johnAlexander	15:932d8b4e52c9	2890	const FixPoint1616_t cSigmaEstMax = 0x028F87AE;
johnAlexander	15:932d8b4e52c9	2891	const FixPoint1616_t cSigmaEstRtnMax = 0xF000;
johnAlexander	15:932d8b4e52c9	2892	const FixPoint1616_t cAmbToSignalRatioMax = 0xF0000000/
johnAlexander	15:932d8b4e52c9	2893	cAmbientEffectiveWidth_centi_ns;
johnAlexander	15:932d8b4e52c9	2894	/* Time Of Flight per mm (6.6 pico secs) */
johnAlexander	15:932d8b4e52c9	2895	const FixPoint1616_t cTOF_per_mm_ps = 0x0006999A;
johnAlexander	15:932d8b4e52c9	2896	const uint32_t c16BitRoundingParam = 0x00008000;
johnAlexander	15:932d8b4e52c9	2897	const FixPoint1616_t cMaxXTalk_kcps = 0x00320000;
johnAlexander	15:932d8b4e52c9	2898	const uint32_t cPllPeriod_ps = 1655;
johnAlexander	15:932d8b4e52c9	2899
johnAlexander	15:932d8b4e52c9	2900	uint32_t vcselTotalEventsRtn;
johnAlexander	15:932d8b4e52c9	2901	uint32_t finalRangeTimeoutMicroSecs;
johnAlexander	15:932d8b4e52c9	2902	uint32_t preRangeTimeoutMicroSecs;
johnAlexander	15:932d8b4e52c9	2903	uint32_t finalRangeIntegrationTimeMilliSecs;
johnAlexander	15:932d8b4e52c9	2904	FixPoint1616_t sigmaEstimateP1;
johnAlexander	15:932d8b4e52c9	2905	FixPoint1616_t sigmaEstimateP2;
johnAlexander	15:932d8b4e52c9	2906	FixPoint1616_t sigmaEstimateP3;
johnAlexander	15:932d8b4e52c9	2907	FixPoint1616_t deltaT_ps;
johnAlexander	15:932d8b4e52c9	2908	FixPoint1616_t pwMult;
johnAlexander	15:932d8b4e52c9	2909	FixPoint1616_t sigmaEstRtn;
johnAlexander	15:932d8b4e52c9	2910	FixPoint1616_t sigmaEstimate;
johnAlexander	15:932d8b4e52c9	2911	FixPoint1616_t xTalkCorrection;
johnAlexander	15:932d8b4e52c9	2912	FixPoint1616_t ambientRate_kcps;
johnAlexander	15:932d8b4e52c9	2913	FixPoint1616_t peakSignalRate_kcps;
johnAlexander	15:932d8b4e52c9	2914	FixPoint1616_t xTalkCompRate_mcps;
johnAlexander	15:932d8b4e52c9	2915	uint32_t xTalkCompRate_kcps;
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	2921	FixPoint1616_t sqrSum;
johnAlexander	15:932d8b4e52c9	2922	FixPoint1616_t sqrtResult_centi_ns;
johnAlexander	15:932d8b4e52c9	2923	FixPoint1616_t sqrtResult;
johnAlexander	15:932d8b4e52c9	2924	FixPoint1616_t totalSignalRate_mcps;
johnAlexander	15:932d8b4e52c9	2925	FixPoint1616_t correctedSignalRate_mcps;
johnAlexander	15:932d8b4e52c9	2926	FixPoint1616_t sigmaEstRef;
johnAlexander	15:932d8b4e52c9	2927	uint32_t vcselWidth;
johnAlexander	15:932d8b4e52c9	2928	uint32_t finalRangeMacroPCLKS;
johnAlexander	15:932d8b4e52c9	2929	uint32_t preRangeMacroPCLKS;
johnAlexander	15:932d8b4e52c9	2930	uint32_t peakVcselDuration_us;
johnAlexander	15:932d8b4e52c9	2931	uint8_t finalRangeVcselPCLKS;
johnAlexander	15:932d8b4e52c9	2932	uint8_t preRangeVcselPCLKS;
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	15:932d8b4e52c9	2941	VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
johnAlexander	15:932d8b4e52c9	2942	xTalkCompRate_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	15:932d8b4e52c9	2949	ambientRate_kcps =
johnAlexander	15:932d8b4e52c9	2950	(pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16;
johnAlexander	15:932d8b4e52c9	2951
johnAlexander	15:932d8b4e52c9	2952	correctedSignalRate_mcps =
johnAlexander	15:932d8b4e52c9	2953	pRangingMeasurementData->SignalRateRtnMegaCps;
johnAlexander	15:932d8b4e52c9	2954
johnAlexander	15:932d8b4e52c9	2955
johnAlexander	15:932d8b4e52c9	2956	Status = VL53L0X_get_total_signal_rate(
johnAlexander	15:932d8b4e52c9	2957	Dev, pRangingMeasurementData, &totalSignalRate_mcps);
johnAlexander	15:932d8b4e52c9	2958	Status = VL53L0X_get_total_xtalk_rate(
johnAlexander	15:932d8b4e52c9	2959	Dev, pRangingMeasurementData, &xTalkCompRate_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	15:932d8b4e52c9	2965	peakSignalRate_kcps = (totalSignalRate_mcps * 1000);
johnAlexander	15:932d8b4e52c9	2966	peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16;
johnAlexander	15:932d8b4e52c9	2967
johnAlexander	15:932d8b4e52c9	2968	xTalkCompRate_kcps = xTalkCompRate_mcps * 1000;
johnAlexander	15:932d8b4e52c9	2969
johnAlexander	15:932d8b4e52c9	2970	if (xTalkCompRate_kcps > cMaxXTalk_kcps)
johnAlexander	15:932d8b4e52c9	2971	xTalkCompRate_kcps = cMaxXTalk_kcps;
johnAlexander	15:932d8b4e52c9	2972
johnAlexander	15:932d8b4e52c9	2973	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	2974
johnAlexander	15:932d8b4e52c9	2975	/* Calculate final range macro periods */
johnAlexander	15:932d8b4e52c9	2976	finalRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	2977	Dev, FinalRangeTimeoutMicroSecs);
johnAlexander	15:932d8b4e52c9	2978
johnAlexander	15:932d8b4e52c9	2979	finalRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	2980	Dev, FinalRangeVcselPulsePeriod);
johnAlexander	15:932d8b4e52c9	2981
johnAlexander	15:932d8b4e52c9	2982	finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
johnAlexander	15:932d8b4e52c9	2983	Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS);
johnAlexander	15:932d8b4e52c9	2984
johnAlexander	15:932d8b4e52c9	2985	/* Calculate pre-range macro periods */
johnAlexander	15:932d8b4e52c9	2986	preRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	2987	Dev, PreRangeTimeoutMicroSecs);
johnAlexander	15:932d8b4e52c9	2988
johnAlexander	15:932d8b4e52c9	2989	preRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	2990	Dev, PreRangeVcselPulsePeriod);
johnAlexander	15:932d8b4e52c9	2991
johnAlexander	15:932d8b4e52c9	2992	preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
johnAlexander	15:932d8b4e52c9	2993	Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS);
johnAlexander	15:932d8b4e52c9	2994
johnAlexander	15:932d8b4e52c9	2995	vcselWidth = 3;
johnAlexander	15:932d8b4e52c9	2996	if (finalRangeVcselPCLKS == 8)
johnAlexander	15:932d8b4e52c9	2997	vcselWidth = 2;
johnAlexander	15:932d8b4e52c9	2998
johnAlexander	15:932d8b4e52c9	2999
johnAlexander	15:932d8b4e52c9	3000	peakVcselDuration_us = vcselWidth * 2048 *
johnAlexander	15:932d8b4e52c9	3001	(preRangeMacroPCLKS + finalRangeMacroPCLKS);
johnAlexander	15:932d8b4e52c9	3002	peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
johnAlexander	15:932d8b4e52c9	3003	peakVcselDuration_us *= cPllPeriod_ps;
johnAlexander	15:932d8b4e52c9	3004	peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
johnAlexander	15:932d8b4e52c9	3005
johnAlexander	15:932d8b4e52c9	3006	/* Fix1616 >> 8 = Fix2408 */
johnAlexander	15:932d8b4e52c9	3007	totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8;
johnAlexander	15:932d8b4e52c9	3008
johnAlexander	15:932d8b4e52c9	3009	/* Fix2408 * uint32 = Fix2408 */
johnAlexander	15:932d8b4e52c9	3010	vcselTotalEventsRtn = totalSignalRate_mcps *
johnAlexander	15:932d8b4e52c9	3011	peakVcselDuration_us;
johnAlexander	15:932d8b4e52c9	3012
johnAlexander	15:932d8b4e52c9	3013	/* Fix2408 >> 8 = uint32 */
johnAlexander	15:932d8b4e52c9	3014	vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8;
johnAlexander	15:932d8b4e52c9	3015
johnAlexander	15:932d8b4e52c9	3016	/* Fix2408 << 8 = Fix1616 = */
johnAlexander	15:932d8b4e52c9	3017	totalSignalRate_mcps <<= 8;
johnAlexander	15:932d8b4e52c9	3018	}
johnAlexander	15:932d8b4e52c9	3019
johnAlexander	15:932d8b4e52c9	3020	if (Status != VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3021	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	3022	return Status;
johnAlexander	15:932d8b4e52c9	3023	}
johnAlexander	15:932d8b4e52c9	3024
johnAlexander	15:932d8b4e52c9	3025	if (peakSignalRate_kcps == 0) {
johnAlexander	15:932d8b4e52c9	3026	*pSigmaEstimate = cSigmaEstMax;
johnAlexander	15:932d8b4e52c9	3027	PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax);
johnAlexander	15:932d8b4e52c9	3028	*pDmax_mm = 0;
johnAlexander	15:932d8b4e52c9	3029	} else {
johnAlexander	15:932d8b4e52c9	3030	if (vcselTotalEventsRtn < 1)
johnAlexander	15:932d8b4e52c9	3031	vcselTotalEventsRtn = 1;
johnAlexander	15:932d8b4e52c9	3032
johnAlexander	15:932d8b4e52c9	3033	sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns;
johnAlexander	15:932d8b4e52c9	3034
johnAlexander	15:932d8b4e52c9	3035	/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3036	sigmaEstimateP2 = (ambientRate_kcps << 16)/peakSignalRate_kcps;
johnAlexander	15:932d8b4e52c9	3037	if (sigmaEstimateP2 > cAmbToSignalRatioMax) {
johnAlexander	15:932d8b4e52c9	3038	/* Clip to prevent overflow. Will ensure safe
johnAlexander	15:932d8b4e52c9	3039	* max result. */
johnAlexander	15:932d8b4e52c9	3040	sigmaEstimateP2 = cAmbToSignalRatioMax;
johnAlexander	15:932d8b4e52c9	3041	}
johnAlexander	15:932d8b4e52c9	3042	sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns;
johnAlexander	15:932d8b4e52c9	3043
johnAlexander	15:932d8b4e52c9	3044	sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12);
johnAlexander	15:932d8b4e52c9	3045
johnAlexander	15:932d8b4e52c9	3046	/* uint32 * FixPoint1616 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3047	deltaT_ps = pRangingMeasurementData->RangeMilliMeter *
johnAlexander	15:932d8b4e52c9	3048	cTOF_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	15:932d8b4e52c9	3057	diff1_mcps = (((peakSignalRate_kcps << 16) -
johnAlexander	15:932d8b4e52c9	3058	2 * xTalkCompRate_kcps) + 500)/1000;
johnAlexander	15:932d8b4e52c9	3059
johnAlexander	15:932d8b4e52c9	3060	/* vcselRate + xtalkCompRate */
johnAlexander	15:932d8b4e52c9	3061	diff2_mcps = ((peakSignalRate_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	15:932d8b4e52c9	3070	xTalkCorrection = diff1_mcps/diff2_mcps;
johnAlexander	15:932d8b4e52c9	3071
johnAlexander	15:932d8b4e52c9	3072	/* FixPoint2408 << 8 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3073	xTalkCorrection <<= 8;
johnAlexander	15:932d8b4e52c9	3074
johnAlexander	15:932d8b4e52c9	3075	if(pRangingMeasurementData->RangeStatus != 0) {
johnAlexander	15:932d8b4e52c9	3076	pwMult = 1 << 16;
johnAlexander	15:932d8b4e52c9	3077	} else {
johnAlexander	15:932d8b4e52c9	3078	/* FixPoint1616/uint32 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3079	pwMult = deltaT_ps/cVcselPulseWidth_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	15:932d8b4e52c9	3086	pwMult *= ((1 << 16) - xTalkCorrection);
johnAlexander	15:932d8b4e52c9	3087
johnAlexander	15:932d8b4e52c9	3088	/* (FixPoint3232 >> 16) = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3089	pwMult = (pwMult + c16BitRoundingParam) >> 16;
johnAlexander	15:932d8b4e52c9	3090
johnAlexander	15:932d8b4e52c9	3091	/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3092	pwMult += (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	15:932d8b4e52c9	3099	pwMult >>= 1;
johnAlexander	15:932d8b4e52c9	3100	/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
johnAlexander	15:932d8b4e52c9	3101	pwMult = pwMult * pwMult;
johnAlexander	15:932d8b4e52c9	3102
johnAlexander	15:932d8b4e52c9	3103	/* (FixPoint3430 >> 14) = Fix1616 */
johnAlexander	15:932d8b4e52c9	3104	pwMult >>= 14;
johnAlexander	15:932d8b4e52c9	3105	}
johnAlexander	15:932d8b4e52c9	3106
johnAlexander	15:932d8b4e52c9	3107	/* FixPoint1616 * uint32 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3108	sqr1 = pwMult * sigmaEstimateP1;
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	15:932d8b4e52c9	3116	sqr2 = sigmaEstimateP2;
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	15:932d8b4e52c9	3125	sqrSum = sqr1 + sqr2;
johnAlexander	15:932d8b4e52c9	3126
johnAlexander	15:932d8b4e52c9	3127	/* SQRT(FixPoin6400) = FixPoint3200 */
johnAlexander	15:932d8b4e52c9	3128	sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum);
johnAlexander	15:932d8b4e52c9	3129
johnAlexander	15:932d8b4e52c9	3130	/* (FixPoint3200 << 16) = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3131	sqrtResult_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	15:932d8b4e52c9	3138	sigmaEstRtn = (((sqrtResult_centi_ns+50)/100) /
johnAlexander	15:932d8b4e52c9	3139	sigmaEstimateP3);
johnAlexander	15:932d8b4e52c9	3140	sigmaEstRtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
johnAlexander	15:932d8b4e52c9	3141
johnAlexander	15:932d8b4e52c9	3142	/* Add 5000 before dividing by 10000 to ensure rounding. */
johnAlexander	15:932d8b4e52c9	3143	sigmaEstRtn += 5000;
johnAlexander	15:932d8b4e52c9	3144	sigmaEstRtn /= 10000;
johnAlexander	15:932d8b4e52c9	3145
johnAlexander	15:932d8b4e52c9	3146	if (sigmaEstRtn > cSigmaEstRtnMax) {
johnAlexander	15:932d8b4e52c9	3147	/* Clip to prevent overflow. Will ensure safe
johnAlexander	15:932d8b4e52c9	3148	* max result. */
johnAlexander	15:932d8b4e52c9	3149	sigmaEstRtn = cSigmaEstRtnMax;
johnAlexander	15:932d8b4e52c9	3150	}
johnAlexander	15:932d8b4e52c9	3151	finalRangeIntegrationTimeMilliSecs =
johnAlexander	15:932d8b4e52c9	3152	(finalRangeTimeoutMicroSecs + preRangeTimeoutMicroSecs + 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	15:932d8b4e52c9	3157	sigmaEstRef =
johnAlexander	15:932d8b4e52c9	3158	VL53L0X_isqrt((cDfltFinalRangeIntegrationTimeMilliSecs +
johnAlexander	15:932d8b4e52c9	3159	finalRangeIntegrationTimeMilliSecs/2)/
johnAlexander	15:932d8b4e52c9	3160	finalRangeIntegrationTimeMilliSecs);
johnAlexander	15:932d8b4e52c9	3161
johnAlexander	15:932d8b4e52c9	3162	/* FixPoint2408 << 8 = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3163	sigmaEstRef <<= 8;
johnAlexander	15:932d8b4e52c9	3164	sigmaEstRef = (sigmaEstRef + 500)/1000;
johnAlexander	15:932d8b4e52c9	3165
johnAlexander	15:932d8b4e52c9	3166	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
johnAlexander	15:932d8b4e52c9	3167	sqr1 = sigmaEstRtn * sigmaEstRtn;
johnAlexander	15:932d8b4e52c9	3168	/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
johnAlexander	15:932d8b4e52c9	3169	sqr2 = sigmaEstRef * sigmaEstRef;
johnAlexander	15:932d8b4e52c9	3170
johnAlexander	15:932d8b4e52c9	3171	/* sqrt(FixPoint3232) = FixPoint1616 */
johnAlexander	15:932d8b4e52c9	3172	sqrtResult = 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	15:932d8b4e52c9	3179	sigmaEstimate = 1000 * sqrtResult;
johnAlexander	15:932d8b4e52c9	3180
johnAlexander	15:932d8b4e52c9	3181	if ((peakSignalRate_kcps < 1) \|\| (vcselTotalEventsRtn < 1) \|\|
johnAlexander	15:932d8b4e52c9	3182	(sigmaEstimate > cSigmaEstMax)) {
johnAlexander	15:932d8b4e52c9	3183	sigmaEstimate = cSigmaEstMax;
johnAlexander	15:932d8b4e52c9	3184	}
johnAlexander	15:932d8b4e52c9	3185
johnAlexander	15:932d8b4e52c9	3186	*pSigmaEstimate = (uint32_t)(sigmaEstimate);
johnAlexander	15:932d8b4e52c9	3187	PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate);
johnAlexander	15:932d8b4e52c9	3188	Status = VL53L0X_calc_dmax(
johnAlexander	15:932d8b4e52c9	3189	Dev,
johnAlexander	15:932d8b4e52c9	3190	totalSignalRate_mcps,
johnAlexander	15:932d8b4e52c9	3191	correctedSignalRate_mcps,
johnAlexander	15:932d8b4e52c9	3192	pwMult,
johnAlexander	15:932d8b4e52c9	3193	sigmaEstimateP1,
johnAlexander	15:932d8b4e52c9	3194	sigmaEstimateP2,
johnAlexander	15:932d8b4e52c9	3195	peakVcselDuration_us,
johnAlexander	15:932d8b4e52c9	3196	pDmax_mm);
johnAlexander	15:932d8b4e52c9	3197	}
johnAlexander	15:932d8b4e52c9	3198
johnAlexander	15:932d8b4e52c9	3199	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	3200	return Status;
johnAlexander	0:c523920bcc09	3201	}
johnAlexander	0:c523920bcc09	3202
johnAlexander	0:c523920bcc09	3203	VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	3204	uint8_t DeviceRangeStatus,
johnAlexander	15:932d8b4e52c9	3205	FixPoint1616_t SignalRate,
johnAlexander	15:932d8b4e52c9	3206	uint16_t EffectiveSpadRtnCount,
johnAlexander	15:932d8b4e52c9	3207	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
johnAlexander	15:932d8b4e52c9	3208	uint8_t *pPalRangeStatus)
johnAlexander	0:c523920bcc09	3209	{
johnAlexander	15:932d8b4e52c9	3210	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	3211	uint8_t NoneFlag;
johnAlexander	15:932d8b4e52c9	3212	uint8_t SigmaLimitflag = 0;
johnAlexander	15:932d8b4e52c9	3213	uint8_t SignalRefClipflag = 0;
johnAlexander	15:932d8b4e52c9	3214	uint8_t RangeIgnoreThresholdflag = 0;
johnAlexander	15:932d8b4e52c9	3215	uint8_t SigmaLimitCheckEnable = 0;
johnAlexander	15:932d8b4e52c9	3216	uint8_t SignalRateFinalRangeLimitCheckEnable = 0;
johnAlexander	15:932d8b4e52c9	3217	uint8_t SignalRefClipLimitCheckEnable = 0;
johnAlexander	15:932d8b4e52c9	3218	uint8_t RangeIgnoreThresholdLimitCheckEnable = 0;
johnAlexander	15:932d8b4e52c9	3219	FixPoint1616_t SigmaEstimate;
johnAlexander	15:932d8b4e52c9	3220	FixPoint1616_t SigmaLimitValue;
johnAlexander	15:932d8b4e52c9	3221	FixPoint1616_t SignalRefClipValue;
johnAlexander	15:932d8b4e52c9	3222	FixPoint1616_t RangeIgnoreThresholdValue;
johnAlexander	15:932d8b4e52c9	3223	FixPoint1616_t SignalRatePerSpad;
johnAlexander	15:932d8b4e52c9	3224	uint8_t DeviceRangeStatusInternal = 0;
johnAlexander	15:932d8b4e52c9	3225	uint16_t tmpWord = 0;
johnAlexander	15:932d8b4e52c9	3226	uint8_t Temp8;
johnAlexander	15:932d8b4e52c9	3227	uint32_t Dmax_mm = 0;
johnAlexander	15:932d8b4e52c9	3228	FixPoint1616_t LastSignalRefMcps;
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	15:932d8b4e52c9	3241	DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3);
johnAlexander	15:932d8b4e52c9	3242
johnAlexander	15:932d8b4e52c9	3243	if (DeviceRangeStatusInternal == 0 \|\|
johnAlexander	15:932d8b4e52c9	3244	DeviceRangeStatusInternal == 5 \|\|
johnAlexander	15:932d8b4e52c9	3245	DeviceRangeStatusInternal == 7 \|\|
johnAlexander	15:932d8b4e52c9	3246	DeviceRangeStatusInternal == 12 \|\|
johnAlexander	15:932d8b4e52c9	3247	DeviceRangeStatusInternal == 13 \|\|
johnAlexander	15:932d8b4e52c9	3248	DeviceRangeStatusInternal == 14 \|\|
johnAlexander	15:932d8b4e52c9	3249	DeviceRangeStatusInternal == 15
johnAlexander	15:932d8b4e52c9	3250	) {
johnAlexander	15:932d8b4e52c9	3251	NoneFlag = 1;
johnAlexander	15:932d8b4e52c9	3252	} else {
johnAlexander	15:932d8b4e52c9	3253	NoneFlag = 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	15:932d8b4e52c9	3260	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3261	Status = VL53L0X_GetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	3262	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	3263	&SigmaLimitCheckEnable);
johnAlexander	15:932d8b4e52c9	3264
johnAlexander	15:932d8b4e52c9	3265	if ((SigmaLimitCheckEnable != 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	15:932d8b4e52c9	3269	Status = VL53L0X_calc_sigma_estimate(
johnAlexander	15:932d8b4e52c9	3270	Dev,
johnAlexander	15:932d8b4e52c9	3271	pRangingMeasurementData,
johnAlexander	15:932d8b4e52c9	3272	&SigmaEstimate,
johnAlexander	15:932d8b4e52c9	3273	&Dmax_mm);
johnAlexander	15:932d8b4e52c9	3274	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3275	pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm;
johnAlexander	15:932d8b4e52c9	3276
johnAlexander	15:932d8b4e52c9	3277	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3278	Status = VL53L0X_GetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	3279	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	3280	&SigmaLimitValue);
johnAlexander	15:932d8b4e52c9	3281
johnAlexander	15:932d8b4e52c9	3282	if ((SigmaLimitValue > 0) &&
johnAlexander	15:932d8b4e52c9	3283	(SigmaEstimate > SigmaLimitValue))
johnAlexander	15:932d8b4e52c9	3284	/* Limit Fail */
johnAlexander	15:932d8b4e52c9	3285	SigmaLimitflag = 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	15:932d8b4e52c9	3293	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3294	Status = VL53L0X_GetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	3295	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
johnAlexander	15:932d8b4e52c9	3296	&SignalRefClipLimitCheckEnable);
johnAlexander	15:932d8b4e52c9	3297
johnAlexander	15:932d8b4e52c9	3298	if ((SignalRefClipLimitCheckEnable != 0) &&
johnAlexander	15:932d8b4e52c9	3299	(Status == VL53L0X_ERROR_NONE)) {
johnAlexander	15:932d8b4e52c9	3300
johnAlexander	15:932d8b4e52c9	3301	Status = VL53L0X_GetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	3302	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
johnAlexander	15:932d8b4e52c9	3303	&SignalRefClipValue);
johnAlexander	15:932d8b4e52c9	3304
johnAlexander	15:932d8b4e52c9	3305	/* Read LastSignalRefMcps from device */
johnAlexander	15:932d8b4e52c9	3306	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3307	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	3308
johnAlexander	15:932d8b4e52c9	3309	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3310	Status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	3311	VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
johnAlexander	15:932d8b4e52c9	3312	&tmpWord);
johnAlexander	15:932d8b4e52c9	3313
johnAlexander	15:932d8b4e52c9	3314	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3315	Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	3316
johnAlexander	15:932d8b4e52c9	3317	LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord);
johnAlexander	15:932d8b4e52c9	3318	PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps);
johnAlexander	15:932d8b4e52c9	3319
johnAlexander	15:932d8b4e52c9	3320	if ((SignalRefClipValue > 0) &&
johnAlexander	15:932d8b4e52c9	3321	(LastSignalRefMcps > SignalRefClipValue)) {
johnAlexander	15:932d8b4e52c9	3322	/* Limit Fail */
johnAlexander	15:932d8b4e52c9	3323	SignalRefClipflag = 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	15:932d8b4e52c9	3333	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3334	Status = VL53L0X_GetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	3335	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
johnAlexander	15:932d8b4e52c9	3336	&RangeIgnoreThresholdLimitCheckEnable);
johnAlexander	15:932d8b4e52c9	3337
johnAlexander	15:932d8b4e52c9	3338	if ((RangeIgnoreThresholdLimitCheckEnable != 0) &&
johnAlexander	15:932d8b4e52c9	3339	(Status == VL53L0X_ERROR_NONE)) {
johnAlexander	15:932d8b4e52c9	3340
johnAlexander	15:932d8b4e52c9	3341	/* Compute the signal rate per spad */
johnAlexander	15:932d8b4e52c9	3342	if (EffectiveSpadRtnCount == 0) {
johnAlexander	15:932d8b4e52c9	3343	SignalRatePerSpad = 0;
johnAlexander	15:932d8b4e52c9	3344	} else {
johnAlexander	15:932d8b4e52c9	3345	SignalRatePerSpad = (FixPoint1616_t)((256 * SignalRate)
johnAlexander	15:932d8b4e52c9	3346	/ EffectiveSpadRtnCount);
johnAlexander	15:932d8b4e52c9	3347	}
johnAlexander	15:932d8b4e52c9	3348
johnAlexander	15:932d8b4e52c9	3349	Status = VL53L0X_GetLimitCheckValue(Dev,
johnAlexander	15:932d8b4e52c9	3350	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
johnAlexander	15:932d8b4e52c9	3351	&RangeIgnoreThresholdValue);
johnAlexander	15:932d8b4e52c9	3352
johnAlexander	15:932d8b4e52c9	3353	if ((RangeIgnoreThresholdValue > 0) &&
johnAlexander	15:932d8b4e52c9	3354	(SignalRatePerSpad < RangeIgnoreThresholdValue)) {
johnAlexander	15:932d8b4e52c9	3355	/* Limit Fail add 2^6 to range status */
johnAlexander	15:932d8b4e52c9	3356	RangeIgnoreThresholdflag = 1;
johnAlexander	15:932d8b4e52c9	3357	}
johnAlexander	15:932d8b4e52c9	3358	}
johnAlexander	15:932d8b4e52c9	3359
johnAlexander	15:932d8b4e52c9	3360	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3361	if (NoneFlag == 1) {
johnAlexander	15:932d8b4e52c9	3362	pPalRangeStatus = 255; / NONE */
johnAlexander	15:932d8b4e52c9	3363	} else if (DeviceRangeStatusInternal == 1 \|\|
johnAlexander	15:932d8b4e52c9	3364	DeviceRangeStatusInternal == 2 \|\|
johnAlexander	15:932d8b4e52c9	3365	DeviceRangeStatusInternal == 3) {
johnAlexander	15:932d8b4e52c9	3366	pPalRangeStatus = 5; / HW fail */
johnAlexander	15:932d8b4e52c9	3367	} else if (DeviceRangeStatusInternal == 6 \|\|
johnAlexander	15:932d8b4e52c9	3368	DeviceRangeStatusInternal == 9) {
johnAlexander	15:932d8b4e52c9	3369	pPalRangeStatus = 4; / Phase fail */
johnAlexander	15:932d8b4e52c9	3370	} else if (DeviceRangeStatusInternal == 8 \|\|
johnAlexander	15:932d8b4e52c9	3371	DeviceRangeStatusInternal == 10 \|\|
johnAlexander	15:932d8b4e52c9	3372	SignalRefClipflag == 1) {
johnAlexander	15:932d8b4e52c9	3373	pPalRangeStatus = 3; / Min range */
johnAlexander	15:932d8b4e52c9	3374	} else if (DeviceRangeStatusInternal == 4 \|\|
johnAlexander	15:932d8b4e52c9	3375	RangeIgnoreThresholdflag == 1) {
johnAlexander	15:932d8b4e52c9	3376	pPalRangeStatus = 2; / Signal Fail */
johnAlexander	15:932d8b4e52c9	3377	} else if (SigmaLimitflag == 1) {
johnAlexander	15:932d8b4e52c9	3378	pPalRangeStatus = 1; / Sigma Fail */
johnAlexander	15:932d8b4e52c9	3379	} else {
johnAlexander	15:932d8b4e52c9	3380	pPalRangeStatus = 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	15:932d8b4e52c9	3385	if (*pPalRangeStatus == 0)
johnAlexander	15:932d8b4e52c9	3386	pRangingMeasurementData->RangeDMaxMilliMeter = 0;
johnAlexander	15:932d8b4e52c9	3387
johnAlexander	15:932d8b4e52c9	3388	/* fill the Limit Check Status */
johnAlexander	15:932d8b4e52c9	3389
johnAlexander	15:932d8b4e52c9	3390	Status = VL53L0X_GetLimitCheckEnable(Dev,
johnAlexander	15:932d8b4e52c9	3391	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	3392	&SignalRateFinalRangeLimitCheckEnable);
johnAlexander	15:932d8b4e52c9	3393
johnAlexander	15:932d8b4e52c9	3394	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3395	if ((SigmaLimitCheckEnable == 0) \|\| (SigmaLimitflag == 1))
johnAlexander	15:932d8b4e52c9	3396	Temp8 = 1;
johnAlexander	15:932d8b4e52c9	3397	else
johnAlexander	15:932d8b4e52c9	3398	Temp8 = 0;
johnAlexander	15:932d8b4e52c9	3399	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
johnAlexander	15:932d8b4e52c9	3400	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
johnAlexander	15:932d8b4e52c9	3401
johnAlexander	15:932d8b4e52c9	3402	if ((DeviceRangeStatusInternal == 4) \|\|
johnAlexander	15:932d8b4e52c9	3403	(SignalRateFinalRangeLimitCheckEnable == 0))
johnAlexander	15:932d8b4e52c9	3404	Temp8 = 1;
johnAlexander	15:932d8b4e52c9	3405	else
johnAlexander	15:932d8b4e52c9	3406	Temp8 = 0;
johnAlexander	15:932d8b4e52c9	3407	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
johnAlexander	15:932d8b4e52c9	3408	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	3409	Temp8);
johnAlexander	15:932d8b4e52c9	3410
johnAlexander	15:932d8b4e52c9	3411	if ((SignalRefClipLimitCheckEnable == 0) \|\|
johnAlexander	15:932d8b4e52c9	3412	(SignalRefClipflag == 1))
johnAlexander	15:932d8b4e52c9	3413	Temp8 = 1;
johnAlexander	15:932d8b4e52c9	3414	else
johnAlexander	15:932d8b4e52c9	3415	Temp8 = 0;
johnAlexander	15:932d8b4e52c9	3416
johnAlexander	15:932d8b4e52c9	3417	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
johnAlexander	15:932d8b4e52c9	3418	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8);
johnAlexander	15:932d8b4e52c9	3419
johnAlexander	15:932d8b4e52c9	3420	if ((RangeIgnoreThresholdLimitCheckEnable == 0) \|\|
johnAlexander	15:932d8b4e52c9	3421	(RangeIgnoreThresholdflag == 1))
johnAlexander	15:932d8b4e52c9	3422	Temp8 = 1;
johnAlexander	15:932d8b4e52c9	3423	else
johnAlexander	15:932d8b4e52c9	3424	Temp8 = 0;
johnAlexander	15:932d8b4e52c9	3425
johnAlexander	15:932d8b4e52c9	3426	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
johnAlexander	15:932d8b4e52c9	3427	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
johnAlexander	15:932d8b4e52c9	3428	Temp8);
johnAlexander	15:932d8b4e52c9	3429	}
johnAlexander	15:932d8b4e52c9	3430
johnAlexander	15:932d8b4e52c9	3431	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	3432	return Status;
johnAlexander	0:c523920bcc09	3433
johnAlexander	0:c523920bcc09	3434	}
johnAlexander	0:c523920bcc09	3435
johnAlexander	0:c523920bcc09	3436	VL53L0X_Error VL53L0X::VL53L0X_GetRangingMeasurementData(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	3437	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData)
johnAlexander	0:c523920bcc09	3438	{
johnAlexander	15:932d8b4e52c9	3439	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	3440	uint8_t DeviceRangeStatus;
johnAlexander	15:932d8b4e52c9	3441	uint8_t RangeFractionalEnable;
johnAlexander	15:932d8b4e52c9	3442	uint8_t PalRangeStatus;
johnAlexander	15:932d8b4e52c9	3443	uint8_t XTalkCompensationEnable;
johnAlexander	15:932d8b4e52c9	3444	uint16_t AmbientRate;
johnAlexander	15:932d8b4e52c9	3445	FixPoint1616_t SignalRate;
johnAlexander	15:932d8b4e52c9	3446	uint16_t XTalkCompensationRateMegaCps;
johnAlexander	15:932d8b4e52c9	3447	uint16_t EffectiveSpadRtnCount;
johnAlexander	15:932d8b4e52c9	3448	uint16_t tmpuint16;
johnAlexander	15:932d8b4e52c9	3449	uint16_t XtalkRangeMilliMeter;
johnAlexander	15:932d8b4e52c9	3450	uint16_t LinearityCorrectiveGain;
johnAlexander	15:932d8b4e52c9	3451	uint8_t localBuffer[12];
johnAlexander	15:932d8b4e52c9	3452	VL53L0X_RangingMeasurementData_t LastRangeDataBuffer;
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	15:932d8b4e52c9	3462	Status = VL53L0X_ReadMulti(Dev, 0x14, localBuffer, 12);
johnAlexander	15:932d8b4e52c9	3463
johnAlexander	15:932d8b4e52c9	3464	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3465
johnAlexander	15:932d8b4e52c9	3466	pRangingMeasurementData->ZoneId = 0; /* Only one zone */
johnAlexander	15:932d8b4e52c9	3467	pRangingMeasurementData->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	15:932d8b4e52c9	3474	pRangingMeasurementData->MeasurementTimeUsec = 0;
johnAlexander	15:932d8b4e52c9	3475
johnAlexander	15:932d8b4e52c9	3476	SignalRate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
johnAlexander	15:932d8b4e52c9	3477	VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
johnAlexander	15:932d8b4e52c9	3478	/* peak_signal_count_rate_rtn_mcps */
johnAlexander	15:932d8b4e52c9	3479	pRangingMeasurementData->SignalRateRtnMegaCps = SignalRate;
johnAlexander	15:932d8b4e52c9	3480
johnAlexander	15:932d8b4e52c9	3481	AmbientRate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
johnAlexander	15:932d8b4e52c9	3482	pRangingMeasurementData->AmbientRateRtnMegaCps =
johnAlexander	15:932d8b4e52c9	3483	VL53L0X_FIXPOINT97TOFIXPOINT1616(AmbientRate);
johnAlexander	15:932d8b4e52c9	3484
johnAlexander	15:932d8b4e52c9	3485	EffectiveSpadRtnCount = VL53L0X_MAKEUINT16(localBuffer[3],
johnAlexander	15:932d8b4e52c9	3486	localBuffer[2]);
johnAlexander	15:932d8b4e52c9	3487	/* EffectiveSpadRtnCount is 8.8 format */
johnAlexander	15:932d8b4e52c9	3488	pRangingMeasurementData->EffectiveSpadRtnCount =
johnAlexander	15:932d8b4e52c9	3489	EffectiveSpadRtnCount;
johnAlexander	15:932d8b4e52c9	3490
johnAlexander	15:932d8b4e52c9	3491	DeviceRangeStatus = localBuffer[0];
johnAlexander	15:932d8b4e52c9	3492
johnAlexander	15:932d8b4e52c9	3493	/* Get Linearity Corrective Gain */
johnAlexander	15:932d8b4e52c9	3494	LinearityCorrectiveGain = PALDevDataGet(Dev,
johnAlexander	15:932d8b4e52c9	3495	LinearityCorrectiveGain);
johnAlexander	15:932d8b4e52c9	3496
johnAlexander	15:932d8b4e52c9	3497	/* Get ranging configuration */
johnAlexander	15:932d8b4e52c9	3498	RangeFractionalEnable = PALDevDataGet(Dev,
johnAlexander	15:932d8b4e52c9	3499	RangeFractionalEnable);
johnAlexander	15:932d8b4e52c9	3500
johnAlexander	15:932d8b4e52c9	3501	if (LinearityCorrectiveGain != 1000) {
johnAlexander	15:932d8b4e52c9	3502
johnAlexander	15:932d8b4e52c9	3503	tmpuint16 = (uint16_t)((LinearityCorrectiveGain
johnAlexander	15:932d8b4e52c9	3504	* tmpuint16 + 500) / 1000);
johnAlexander	15:932d8b4e52c9	3505
johnAlexander	15:932d8b4e52c9	3506	/* Implement Xtalk */
johnAlexander	15:932d8b4e52c9	3507	VL53L0X_GETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	3508	XTalkCompensationRateMegaCps,
johnAlexander	15:932d8b4e52c9	3509	XTalkCompensationRateMegaCps);
johnAlexander	15:932d8b4e52c9	3510	VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable,
johnAlexander	15:932d8b4e52c9	3511	XTalkCompensationEnable);
johnAlexander	15:932d8b4e52c9	3512
johnAlexander	15:932d8b4e52c9	3513	if (XTalkCompensationEnable) {
johnAlexander	15:932d8b4e52c9	3514
johnAlexander	15:932d8b4e52c9	3515	if ((SignalRate
johnAlexander	15:932d8b4e52c9	3516	- ((XTalkCompensationRateMegaCps
johnAlexander	15:932d8b4e52c9	3517	* EffectiveSpadRtnCount) >> 8))
johnAlexander	15:932d8b4e52c9	3518	<= 0) {
johnAlexander	15:932d8b4e52c9	3519	if (RangeFractionalEnable)
johnAlexander	15:932d8b4e52c9	3520	XtalkRangeMilliMeter = 8888;
johnAlexander	15:932d8b4e52c9	3521	else
johnAlexander	15:932d8b4e52c9	3522	XtalkRangeMilliMeter = 8888
johnAlexander	15:932d8b4e52c9	3523	<< 2;
johnAlexander	15:932d8b4e52c9	3524	} else {
johnAlexander	15:932d8b4e52c9	3525	XtalkRangeMilliMeter =
johnAlexander	15:932d8b4e52c9	3526	(tmpuint16 * SignalRate)
johnAlexander	15:932d8b4e52c9	3527	/ (SignalRate
johnAlexander	15:932d8b4e52c9	3528	- ((XTalkCompensationRateMegaCps
johnAlexander	15:932d8b4e52c9	3529	* EffectiveSpadRtnCount)
johnAlexander	15:932d8b4e52c9	3530	>> 8));
johnAlexander	15:932d8b4e52c9	3531	}
johnAlexander	15:932d8b4e52c9	3532
johnAlexander	15:932d8b4e52c9	3533	tmpuint16 = XtalkRangeMilliMeter;
johnAlexander	15:932d8b4e52c9	3534	}
johnAlexander	15:932d8b4e52c9	3535
johnAlexander	15:932d8b4e52c9	3536	}
johnAlexander	15:932d8b4e52c9	3537
johnAlexander	15:932d8b4e52c9	3538	if (RangeFractionalEnable) {
johnAlexander	15:932d8b4e52c9	3539	pRangingMeasurementData->RangeMilliMeter =
johnAlexander	15:932d8b4e52c9	3540	(uint16_t)((tmpuint16) >> 2);
johnAlexander	15:932d8b4e52c9	3541	pRangingMeasurementData->RangeFractionalPart =
johnAlexander	15:932d8b4e52c9	3542	(uint8_t)((tmpuint16 & 0x03) << 6);
johnAlexander	15:932d8b4e52c9	3543	} else {
johnAlexander	15:932d8b4e52c9	3544	pRangingMeasurementData->RangeMilliMeter = tmpuint16;
johnAlexander	15:932d8b4e52c9	3545	pRangingMeasurementData->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	15:932d8b4e52c9	3554	Status \|= VL53L0X_get_pal_range_status(Dev, DeviceRangeStatus,
johnAlexander	15:932d8b4e52c9	3555	SignalRate, EffectiveSpadRtnCount,
johnAlexander	15:932d8b4e52c9	3556	pRangingMeasurementData, &PalRangeStatus);
johnAlexander	15:932d8b4e52c9	3557
johnAlexander	15:932d8b4e52c9	3558	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3559	pRangingMeasurementData->RangeStatus = PalRangeStatus;
johnAlexander	15:932d8b4e52c9	3560
johnAlexander	15:932d8b4e52c9	3561	}
johnAlexander	15:932d8b4e52c9	3562
johnAlexander	15:932d8b4e52c9	3563	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3564	/* Copy last read data into Dev buffer */
johnAlexander	15:932d8b4e52c9	3565	LastRangeDataBuffer = PALDevDataGet(Dev, LastRangeMeasure);
johnAlexander	15:932d8b4e52c9	3566
johnAlexander	15:932d8b4e52c9	3567	LastRangeDataBuffer.RangeMilliMeter =
johnAlexander	15:932d8b4e52c9	3568	pRangingMeasurementData->RangeMilliMeter;
johnAlexander	15:932d8b4e52c9	3569	LastRangeDataBuffer.RangeFractionalPart =
johnAlexander	15:932d8b4e52c9	3570	pRangingMeasurementData->RangeFractionalPart;
johnAlexander	15:932d8b4e52c9	3571	LastRangeDataBuffer.RangeDMaxMilliMeter =
johnAlexander	15:932d8b4e52c9	3572	pRangingMeasurementData->RangeDMaxMilliMeter;
johnAlexander	15:932d8b4e52c9	3573	LastRangeDataBuffer.MeasurementTimeUsec =
johnAlexander	15:932d8b4e52c9	3574	pRangingMeasurementData->MeasurementTimeUsec;
johnAlexander	15:932d8b4e52c9	3575	LastRangeDataBuffer.SignalRateRtnMegaCps =
johnAlexander	15:932d8b4e52c9	3576	pRangingMeasurementData->SignalRateRtnMegaCps;
johnAlexander	15:932d8b4e52c9	3577	LastRangeDataBuffer.AmbientRateRtnMegaCps =
johnAlexander	15:932d8b4e52c9	3578	pRangingMeasurementData->AmbientRateRtnMegaCps;
johnAlexander	15:932d8b4e52c9	3579	LastRangeDataBuffer.EffectiveSpadRtnCount =
johnAlexander	15:932d8b4e52c9	3580	pRangingMeasurementData->EffectiveSpadRtnCount;
johnAlexander	15:932d8b4e52c9	3581	LastRangeDataBuffer.RangeStatus =
johnAlexander	15:932d8b4e52c9	3582	pRangingMeasurementData->RangeStatus;
johnAlexander	15:932d8b4e52c9	3583
johnAlexander	15:932d8b4e52c9	3584	PALDevDataSet(Dev, LastRangeMeasure, LastRangeDataBuffer);
johnAlexander	15:932d8b4e52c9	3585	}
johnAlexander	15:932d8b4e52c9	3586
johnAlexander	15:932d8b4e52c9	3587	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	3588	return Status;
johnAlexander	0:c523920bcc09	3589	}
johnAlexander	0:c523920bcc09	3590
johnAlexander	0:c523920bcc09	3591	VL53L0X_Error VL53L0X::VL53L0X_PerformSingleRangingMeasurement(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	3592	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData)
johnAlexander	0:c523920bcc09	3593	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	3600	Status = VL53L0X_SetDeviceMode(Dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
johnAlexander	15:932d8b4e52c9	3601
johnAlexander	15:932d8b4e52c9	3602	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3603	Status = VL53L0X_PerformSingleMeasurement(Dev);
johnAlexander	15:932d8b4e52c9	3604
johnAlexander	15:932d8b4e52c9	3605
johnAlexander	15:932d8b4e52c9	3606	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3607	Status = VL53L0X_GetRangingMeasurementData(Dev,
johnAlexander	15:932d8b4e52c9	3608	pRangingMeasurementData);
johnAlexander	15:932d8b4e52c9	3609
johnAlexander	15:932d8b4e52c9	3610
johnAlexander	15:932d8b4e52c9	3611	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3612	Status = VL53L0X_ClearInterruptMask(Dev, 0);
johnAlexander	15:932d8b4e52c9	3613
johnAlexander	15:932d8b4e52c9	3614
johnAlexander	15:932d8b4e52c9	3615	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	3616	return Status;
johnAlexander	0:c523920bcc09	3617	}
johnAlexander	0:c523920bcc09	3618
johnAlexander	0:c523920bcc09	3619	VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	3620	uint16_t *refSignalRate)
johnAlexander	0:c523920bcc09	3621	{
johnAlexander	15:932d8b4e52c9	3622	VL53L0X_Error status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	3623	VL53L0X_RangingMeasurementData_t rangingMeasurementData;
johnAlexander	15:932d8b4e52c9	3624
johnAlexander	15:932d8b4e52c9	3625	uint8_t SequenceConfig = 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	15:932d8b4e52c9	3631	SequenceConfig = 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	15:932d8b4e52c9	3637	status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3638	VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
johnAlexander	15:932d8b4e52c9	3639
johnAlexander	15:932d8b4e52c9	3640	if (status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3641	status = VL53L0X_PerformSingleRangingMeasurement(Dev,
johnAlexander	15:932d8b4e52c9	3642	&rangingMeasurementData);
johnAlexander	15:932d8b4e52c9	3643
johnAlexander	15:932d8b4e52c9	3644	if (status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3645	status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	3646
johnAlexander	15:932d8b4e52c9	3647	if (status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3648	status = VL53L0X_RdWord(Dev,
johnAlexander	15:932d8b4e52c9	3649	VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
johnAlexander	15:932d8b4e52c9	3650	refSignalRate);
johnAlexander	15:932d8b4e52c9	3651
johnAlexander	15:932d8b4e52c9	3652	if (status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3653	status = VL53L0X_WrByte(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	15:932d8b4e52c9	3657	status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	3658	SequenceConfig);
johnAlexander	15:932d8b4e52c9	3659	if (status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3660	PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
johnAlexander	15:932d8b4e52c9	3661	}
johnAlexander	15:932d8b4e52c9	3662
johnAlexander	15:932d8b4e52c9	3663	return status;
johnAlexander	0:c523920bcc09	3664	}
johnAlexander	0:c523920bcc09	3665
johnAlexander	0:c523920bcc09	3666	VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	3667	uint32_t *refSpadCount,
johnAlexander	15:932d8b4e52c9	3668	uint8_t *isApertureSpads)
johnAlexander	0:c523920bcc09	3669	{
johnAlexander	15:932d8b4e52c9	3670	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	3671	uint8_t lastSpadArray[6];
johnAlexander	15:932d8b4e52c9	3672	uint8_t startSelect = 0xB4;
johnAlexander	15:932d8b4e52c9	3673	uint32_t minimumSpadCount = 3;
johnAlexander	15:932d8b4e52c9	3674	uint32_t maxSpadCount = 44;
johnAlexander	15:932d8b4e52c9	3675	uint32_t currentSpadIndex = 0;
johnAlexander	15:932d8b4e52c9	3676	uint32_t lastSpadIndex = 0;
johnAlexander	15:932d8b4e52c9	3677	int32_t nextGoodSpad = 0;
johnAlexander	15:932d8b4e52c9	3678	uint16_t targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
johnAlexander	15:932d8b4e52c9	3679	uint16_t peakSignalRateRef;
johnAlexander	15:932d8b4e52c9	3680	uint32_t needAptSpads = 0;
johnAlexander	15:932d8b4e52c9	3681	uint32_t index = 0;
johnAlexander	15:932d8b4e52c9	3682	uint32_t spadArraySize = 6;
johnAlexander	15:932d8b4e52c9	3683	uint32_t signalRateDiff = 0;
johnAlexander	15:932d8b4e52c9	3684	uint32_t lastSignalRateDiff = 0;
johnAlexander	15:932d8b4e52c9	3685	uint8_t complete = 0;
johnAlexander	15:932d8b4e52c9	3686	uint8_t VhvSettings = 0;
johnAlexander	15:932d8b4e52c9	3687	uint8_t PhaseCal = 0;
johnAlexander	15:932d8b4e52c9	3688	uint32_t refSpadCount_int = 0;
johnAlexander	15:932d8b4e52c9	3689	uint8_t isApertureSpads_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	15:932d8b4e52c9	3714	targetRefRate = 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	15:932d8b4e52c9	3724	for (index = 0; index < spadArraySize; index++)
johnAlexander	15:932d8b4e52c9	3725	Dev->Data.SpadData.RefSpadEnables[index] = 0;
johnAlexander	15:932d8b4e52c9	3726
johnAlexander	15:932d8b4e52c9	3727
johnAlexander	15:932d8b4e52c9	3728	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	3729
johnAlexander	15:932d8b4e52c9	3730	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3731	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3732	VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
johnAlexander	15:932d8b4e52c9	3733
johnAlexander	15:932d8b4e52c9	3734	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3735	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3736	VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
johnAlexander	15:932d8b4e52c9	3737
johnAlexander	15:932d8b4e52c9	3738	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3739	Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	3740
johnAlexander	15:932d8b4e52c9	3741	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3742	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3743	VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
johnAlexander	15:932d8b4e52c9	3744	startSelect);
johnAlexander	15:932d8b4e52c9	3745
johnAlexander	15:932d8b4e52c9	3746
johnAlexander	15:932d8b4e52c9	3747	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3748	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3749	VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
johnAlexander	15:932d8b4e52c9	3750
johnAlexander	15:932d8b4e52c9	3751	/* Perform ref calibration */
johnAlexander	15:932d8b4e52c9	3752	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3753	Status = VL53L0X_perform_ref_calibration(Dev, &VhvSettings,
johnAlexander	15:932d8b4e52c9	3754	&PhaseCal, 0);
johnAlexander	15:932d8b4e52c9	3755
johnAlexander	15:932d8b4e52c9	3756	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3757	/* Enable Minimum NON-APERTURE Spads */
johnAlexander	15:932d8b4e52c9	3758	currentSpadIndex = 0;
johnAlexander	15:932d8b4e52c9	3759	lastSpadIndex = currentSpadIndex;
johnAlexander	15:932d8b4e52c9	3760	needAptSpads = 0;
johnAlexander	15:932d8b4e52c9	3761	Status = enable_ref_spads(Dev,
johnAlexander	15:932d8b4e52c9	3762	needAptSpads,
johnAlexander	15:932d8b4e52c9	3763	Dev->Data.SpadData.RefGoodSpadMap,
johnAlexander	15:932d8b4e52c9	3764	Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3765	spadArraySize,
johnAlexander	15:932d8b4e52c9	3766	startSelect,
johnAlexander	15:932d8b4e52c9	3767	currentSpadIndex,
johnAlexander	15:932d8b4e52c9	3768	minimumSpadCount,
johnAlexander	15:932d8b4e52c9	3769	&lastSpadIndex);
johnAlexander	15:932d8b4e52c9	3770	}
johnAlexander	15:932d8b4e52c9	3771
johnAlexander	15:932d8b4e52c9	3772	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3773	currentSpadIndex = lastSpadIndex;
johnAlexander	15:932d8b4e52c9	3774
johnAlexander	15:932d8b4e52c9	3775	Status = perform_ref_signal_measurement(Dev,
johnAlexander	15:932d8b4e52c9	3776	&peakSignalRateRef);
johnAlexander	15:932d8b4e52c9	3777	if ((Status == VL53L0X_ERROR_NONE) &&
johnAlexander	15:932d8b4e52c9	3778	(peakSignalRateRef > targetRefRate)) {
johnAlexander	15:932d8b4e52c9	3779	/* Signal rate measurement too high,
johnAlexander	15:932d8b4e52c9	3780	* switch to APERTURE SPADs */
johnAlexander	15:932d8b4e52c9	3781
johnAlexander	15:932d8b4e52c9	3782	for (index = 0; index < spadArraySize; index++)
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	3787	while ((is_aperture(startSelect + currentSpadIndex)
johnAlexander	15:932d8b4e52c9	3788	== 0) && (currentSpadIndex < maxSpadCount)) {
johnAlexander	15:932d8b4e52c9	3789	currentSpadIndex++;
johnAlexander	15:932d8b4e52c9	3790	}
johnAlexander	15:932d8b4e52c9	3791
johnAlexander	15:932d8b4e52c9	3792	needAptSpads = 1;
johnAlexander	15:932d8b4e52c9	3793
johnAlexander	15:932d8b4e52c9	3794	Status = enable_ref_spads(Dev,
johnAlexander	15:932d8b4e52c9	3795	needAptSpads,
johnAlexander	15:932d8b4e52c9	3796	Dev->Data.SpadData.RefGoodSpadMap,
johnAlexander	15:932d8b4e52c9	3797	Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3798	spadArraySize,
johnAlexander	15:932d8b4e52c9	3799	startSelect,
johnAlexander	15:932d8b4e52c9	3800	currentSpadIndex,
johnAlexander	15:932d8b4e52c9	3801	minimumSpadCount,
johnAlexander	15:932d8b4e52c9	3802	&lastSpadIndex);
johnAlexander	15:932d8b4e52c9	3803
johnAlexander	15:932d8b4e52c9	3804	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3805	currentSpadIndex = lastSpadIndex;
johnAlexander	15:932d8b4e52c9	3806	Status = perform_ref_signal_measurement(Dev,
johnAlexander	15:932d8b4e52c9	3807	&peakSignalRateRef);
johnAlexander	15:932d8b4e52c9	3808
johnAlexander	15:932d8b4e52c9	3809	if ((Status == VL53L0X_ERROR_NONE) &&
johnAlexander	15:932d8b4e52c9	3810	(peakSignalRateRef > targetRefRate)) {
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	15:932d8b4e52c9	3817	isApertureSpads_int = 1;
johnAlexander	15:932d8b4e52c9	3818	refSpadCount_int = minimumSpadCount;
johnAlexander	15:932d8b4e52c9	3819	}
johnAlexander	15:932d8b4e52c9	3820	}
johnAlexander	15:932d8b4e52c9	3821	} else {
johnAlexander	15:932d8b4e52c9	3822	needAptSpads = 0;
johnAlexander	15:932d8b4e52c9	3823	}
johnAlexander	15:932d8b4e52c9	3824	}
johnAlexander	15:932d8b4e52c9	3825
johnAlexander	15:932d8b4e52c9	3826	if ((Status == VL53L0X_ERROR_NONE) &&
johnAlexander	15:932d8b4e52c9	3827	(peakSignalRateRef < targetRefRate)) {
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	15:932d8b4e52c9	3833	isApertureSpads_int = needAptSpads;
johnAlexander	15:932d8b4e52c9	3834	refSpadCount_int = minimumSpadCount;
johnAlexander	15:932d8b4e52c9	3835
johnAlexander	15:932d8b4e52c9	3836	memcpy(lastSpadArray, Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3837	spadArraySize);
johnAlexander	15:932d8b4e52c9	3838	lastSignalRateDiff = abs(peakSignalRateRef -
johnAlexander	15:932d8b4e52c9	3839	targetRefRate);
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	15:932d8b4e52c9	3844	Dev->Data.SpadData.RefGoodSpadMap,
johnAlexander	15:932d8b4e52c9	3845	spadArraySize, currentSpadIndex,
johnAlexander	15:932d8b4e52c9	3846	&nextGoodSpad);
johnAlexander	15:932d8b4e52c9	3847
johnAlexander	15:932d8b4e52c9	3848	if (nextGoodSpad == -1) {
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	3856	if (is_aperture((uint32_t)startSelect + nextGoodSpad) !=
johnAlexander	15:932d8b4e52c9	3857	needAptSpads) {
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	15:932d8b4e52c9	3865	(refSpadCount_int)++;
johnAlexander	15:932d8b4e52c9	3866
johnAlexander	15:932d8b4e52c9	3867	currentSpadIndex = nextGoodSpad;
johnAlexander	15:932d8b4e52c9	3868	Status = enable_spad_bit(
johnAlexander	15:932d8b4e52c9	3869	Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3870	spadArraySize, currentSpadIndex);
johnAlexander	15:932d8b4e52c9	3871
johnAlexander	15:932d8b4e52c9	3872	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3873	currentSpadIndex++;
johnAlexander	15:932d8b4e52c9	3874	/* Proceed to apply the additional spad and
johnAlexander	15:932d8b4e52c9	3875	* perform measurement. */
johnAlexander	15:932d8b4e52c9	3876	Status = set_ref_spad_map(Dev,
johnAlexander	15:932d8b4e52c9	3877	Dev->Data.SpadData.RefSpadEnables);
johnAlexander	15:932d8b4e52c9	3878	}
johnAlexander	15:932d8b4e52c9	3879
johnAlexander	15:932d8b4e52c9	3880	if (Status != VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3881	break;
johnAlexander	15:932d8b4e52c9	3882
johnAlexander	15:932d8b4e52c9	3883	Status = perform_ref_signal_measurement(Dev,
johnAlexander	15:932d8b4e52c9	3884	&peakSignalRateRef);
johnAlexander	15:932d8b4e52c9	3885
johnAlexander	15:932d8b4e52c9	3886	if (Status != VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3887	break;
johnAlexander	15:932d8b4e52c9	3888
johnAlexander	15:932d8b4e52c9	3889	signalRateDiff = abs(peakSignalRateRef - targetRefRate);
johnAlexander	15:932d8b4e52c9	3890
johnAlexander	15:932d8b4e52c9	3891	if (peakSignalRateRef > targetRefRate) {
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	15:932d8b4e52c9	3896	if (signalRateDiff > lastSignalRateDiff) {
johnAlexander	15:932d8b4e52c9	3897	/* Previous spad map produced a closer
johnAlexander	15:932d8b4e52c9	3898	* measurement, so choose this. */
johnAlexander	15:932d8b4e52c9	3899	Status = set_ref_spad_map(Dev,
johnAlexander	15:932d8b4e52c9	3900	lastSpadArray);
johnAlexander	15:932d8b4e52c9	3901	memcpy(
johnAlexander	15:932d8b4e52c9	3902	Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3903	lastSpadArray, spadArraySize);
johnAlexander	15:932d8b4e52c9	3904
johnAlexander	15:932d8b4e52c9	3905	(refSpadCount_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	15:932d8b4e52c9	3910	lastSignalRateDiff = signalRateDiff;
johnAlexander	15:932d8b4e52c9	3911	memcpy(lastSpadArray,
johnAlexander	15:932d8b4e52c9	3912	Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3913	spadArraySize);
johnAlexander	15:932d8b4e52c9	3914	}
johnAlexander	15:932d8b4e52c9	3915
johnAlexander	15:932d8b4e52c9	3916	} /* while */
johnAlexander	15:932d8b4e52c9	3917	}
johnAlexander	15:932d8b4e52c9	3918
johnAlexander	15:932d8b4e52c9	3919	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3920	*refSpadCount = refSpadCount_int;
johnAlexander	15:932d8b4e52c9	3921	*isApertureSpads = isApertureSpads_int;
johnAlexander	15:932d8b4e52c9	3922
johnAlexander	15:932d8b4e52c9	3923	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
johnAlexander	15:932d8b4e52c9	3924	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	3925	ReferenceSpadCount, (uint8_t)(*refSpadCount));
johnAlexander	15:932d8b4e52c9	3926	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	3927	ReferenceSpadType, *isApertureSpads);
johnAlexander	15:932d8b4e52c9	3928	}
johnAlexander	15:932d8b4e52c9	3929
johnAlexander	15:932d8b4e52c9	3930	return Status;
johnAlexander	0:c523920bcc09	3931	}
johnAlexander	0:c523920bcc09	3932
johnAlexander	0:c523920bcc09	3933	VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	3934	uint32_t count, uint8_t isApertureSpads)
johnAlexander	0:c523920bcc09	3935	{
johnAlexander	15:932d8b4e52c9	3936	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	3937	uint32_t currentSpadIndex = 0;
johnAlexander	15:932d8b4e52c9	3938	uint8_t startSelect = 0xB4;
johnAlexander	15:932d8b4e52c9	3939	uint32_t spadArraySize = 6;
johnAlexander	15:932d8b4e52c9	3940	uint32_t maxSpadCount = 44;
johnAlexander	15:932d8b4e52c9	3941	uint32_t lastSpadIndex;
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	15:932d8b4e52c9	3951	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	3952
johnAlexander	15:932d8b4e52c9	3953	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3954	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3955	VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
johnAlexander	15:932d8b4e52c9	3956
johnAlexander	15:932d8b4e52c9	3957	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3958	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3959	VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
johnAlexander	15:932d8b4e52c9	3960
johnAlexander	15:932d8b4e52c9	3961	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3962	Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	3963
johnAlexander	15:932d8b4e52c9	3964	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	3965	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	3966	VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
johnAlexander	15:932d8b4e52c9	3967	startSelect);
johnAlexander	15:932d8b4e52c9	3968
johnAlexander	15:932d8b4e52c9	3969	for (index = 0; index < spadArraySize; index++)
johnAlexander	15:932d8b4e52c9	3970	Dev->Data.SpadData.RefSpadEnables[index] = 0;
johnAlexander	15:932d8b4e52c9	3971
johnAlexander	15:932d8b4e52c9	3972	if (isApertureSpads) {
johnAlexander	15:932d8b4e52c9	3973	/* Increment to the first APERTURE spad */
johnAlexander	15:932d8b4e52c9	3974	while ((is_aperture(startSelect + currentSpadIndex) == 0) &&
johnAlexander	15:932d8b4e52c9	3975	(currentSpadIndex < maxSpadCount)) {
johnAlexander	15:932d8b4e52c9	3976	currentSpadIndex++;
johnAlexander	15:932d8b4e52c9	3977	}
johnAlexander	15:932d8b4e52c9	3978	}
johnAlexander	15:932d8b4e52c9	3979	Status = enable_ref_spads(Dev,
johnAlexander	15:932d8b4e52c9	3980	isApertureSpads,
johnAlexander	15:932d8b4e52c9	3981	Dev->Data.SpadData.RefGoodSpadMap,
johnAlexander	15:932d8b4e52c9	3982	Dev->Data.SpadData.RefSpadEnables,
johnAlexander	15:932d8b4e52c9	3983	spadArraySize,
johnAlexander	15:932d8b4e52c9	3984	startSelect,
johnAlexander	15:932d8b4e52c9	3985	currentSpadIndex,
johnAlexander	15:932d8b4e52c9	3986	count,
johnAlexander	15:932d8b4e52c9	3987	&lastSpadIndex);
johnAlexander	15:932d8b4e52c9	3988
johnAlexander	15:932d8b4e52c9	3989	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	3990	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
johnAlexander	15:932d8b4e52c9	3991	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	3992	ReferenceSpadCount, (uint8_t)(count));
johnAlexander	15:932d8b4e52c9	3993	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	3994	ReferenceSpadType, isApertureSpads);
johnAlexander	15:932d8b4e52c9	3995	}
johnAlexander	15:932d8b4e52c9	3996
johnAlexander	15:932d8b4e52c9	3997	return Status;
johnAlexander	0:c523920bcc09	3998	}
johnAlexander	0:c523920bcc09	3999
johnAlexander	0:c523920bcc09	4000	VL53L0X_Error VL53L0X::VL53L0X_WaitDeviceBooted(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	4001	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4007	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4008	return Status;
johnAlexander	0:c523920bcc09	4009	}
johnAlexander	0:c523920bcc09	4010
johnAlexander	0:c523920bcc09	4011	VL53L0X_Error VL53L0X::VL53L0X_PerformRefCalibration(VL53L0X_DEV Dev, uint8_t *pVhvSettings,
johnAlexander	15:932d8b4e52c9	4012	uint8_t *pPhaseCal)
johnAlexander	0:c523920bcc09	4013	{
johnAlexander	15:932d8b4e52c9	4014	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4015	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4016
johnAlexander	15:932d8b4e52c9	4017	Status = VL53L0X_perform_ref_calibration(Dev, pVhvSettings,
johnAlexander	15:932d8b4e52c9	4018	pPhaseCal, 1);
johnAlexander	15:932d8b4e52c9	4019
johnAlexander	15:932d8b4e52c9	4020	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4021	return Status;
johnAlexander	0:c523920bcc09	4022	}
johnAlexander	0:c523920bcc09	4023
johnAlexander	0:c523920bcc09	4024	VL53L0X_Error VL53L0X::VL53L0X_PerformRefSpadManagement(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	4025	uint32_t refSpadCount, uint8_t isApertureSpads)
johnAlexander	0:c523920bcc09	4026	{
johnAlexander	15:932d8b4e52c9	4027	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4028	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4029
johnAlexander	15:932d8b4e52c9	4030	Status = VL53L0X_perform_ref_spad_management(Dev, refSpadCount,
johnAlexander	15:932d8b4e52c9	4031	isApertureSpads);
johnAlexander	15:932d8b4e52c9	4032
johnAlexander	15:932d8b4e52c9	4033	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4034
johnAlexander	15:932d8b4e52c9	4035	return Status;
johnAlexander	0:c523920bcc09	4036	}
johnAlexander	0:c523920bcc09	4037
johnAlexander	0:c523920bcc09	4038	/* Group PAL Init Functions */
johnAlexander	0:c523920bcc09	4039	VL53L0X_Error VL53L0X::VL53L0X_SetDeviceAddress(VL53L0X_DEV Dev, uint8_t DeviceAddress)
johnAlexander	0:c523920bcc09	4040	{
johnAlexander	15:932d8b4e52c9	4041	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4042	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4043
johnAlexander	15:932d8b4e52c9	4044	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
johnAlexander	15:932d8b4e52c9	4045	DeviceAddress / 2);
johnAlexander	15:932d8b4e52c9	4046
johnAlexander	15:932d8b4e52c9	4047	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4048	return Status;
johnAlexander	0:c523920bcc09	4049	}
johnAlexander	0:c523920bcc09	4050
johnAlexander	0:c523920bcc09	4051	VL53L0X_Error VL53L0X::VL53L0X_SetGpioConfig(VL53L0X_DEV Dev, uint8_t Pin,
johnAlexander	15:932d8b4e52c9	4052	VL53L0X_DeviceModes DeviceMode, VL53L0X_GpioFunctionality Functionality,
johnAlexander	15:932d8b4e52c9	4053	VL53L0X_InterruptPolarity Polarity)
johnAlexander	0:c523920bcc09	4054	{
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4060	if (Pin != 0) {
johnAlexander	15:932d8b4e52c9	4061	Status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
johnAlexander	15:932d8b4e52c9	4062	} else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4068	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	4069	VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
johnAlexander	15:932d8b4e52c9	4070
johnAlexander	15:932d8b4e52c9	4071	} else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_OSC) {
johnAlexander	15:932d8b4e52c9	4072
johnAlexander	15:932d8b4e52c9	4073	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x01);
johnAlexander	15:932d8b4e52c9	4074	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	4075
johnAlexander	15:932d8b4e52c9	4076	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x00);
johnAlexander	15:932d8b4e52c9	4077	Status \|= VL53L0X_WrByte(Dev, 0x80, 0x01);
johnAlexander	15:932d8b4e52c9	4078	Status \|= VL53L0X_WrByte(Dev, 0x85, 0x02);
johnAlexander	15:932d8b4e52c9	4079
johnAlexander	15:932d8b4e52c9	4080	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x04);
johnAlexander	15:932d8b4e52c9	4081	Status \|= VL53L0X_WrByte(Dev, 0xcd, 0x00);
johnAlexander	15:932d8b4e52c9	4082	Status \|= VL53L0X_WrByte(Dev, 0xcc, 0x11);
johnAlexander	15:932d8b4e52c9	4083
johnAlexander	15:932d8b4e52c9	4084	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x07);
johnAlexander	15:932d8b4e52c9	4085	Status \|= VL53L0X_WrByte(Dev, 0xbe, 0x00);
johnAlexander	15:932d8b4e52c9	4086
johnAlexander	15:932d8b4e52c9	4087	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x06);
johnAlexander	15:932d8b4e52c9	4088	Status \|= VL53L0X_WrByte(Dev, 0xcc, 0x09);
johnAlexander	15:932d8b4e52c9	4089
johnAlexander	15:932d8b4e52c9	4090	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x00);
johnAlexander	15:932d8b4e52c9	4091	Status \|= VL53L0X_WrByte(Dev, 0xff, 0x01);
johnAlexander	15:932d8b4e52c9	4092	Status \|= VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	4093
johnAlexander	15:932d8b4e52c9	4094	} else {
johnAlexander	15:932d8b4e52c9	4095
johnAlexander	15:932d8b4e52c9	4096	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	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	15:932d8b4e52c9	4119	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4120	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	4121	VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
johnAlexander	15:932d8b4e52c9	4122
johnAlexander	15:932d8b4e52c9	4123	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4129	Status = VL53L0X_UpdateByte(Dev,
johnAlexander	15:932d8b4e52c9	4130	VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
johnAlexander	15:932d8b4e52c9	4131	}
johnAlexander	15:932d8b4e52c9	4132
johnAlexander	15:932d8b4e52c9	4133	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4134	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	4135	Pin0GpioFunctionality, Functionality);
johnAlexander	15:932d8b4e52c9	4136
johnAlexander	15:932d8b4e52c9	4137	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4138	Status = VL53L0X_ClearInterruptMask(Dev, 0);
johnAlexander	15:932d8b4e52c9	4139
johnAlexander	15:932d8b4e52c9	4140	}
johnAlexander	15:932d8b4e52c9	4141	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4142	return Status;
johnAlexander	0:c523920bcc09	4143	}
johnAlexander	0:c523920bcc09	4144
johnAlexander	0:c523920bcc09	4145	VL53L0X_Error VL53L0X::VL53L0X_GetFractionEnable(VL53L0X_DEV Dev, uint8_t *pEnabled)
johnAlexander	0:c523920bcc09	4146	{
johnAlexander	15:932d8b4e52c9	4147	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4148	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4149
johnAlexander	15:932d8b4e52c9	4150	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, pEnabled);
johnAlexander	15:932d8b4e52c9	4151
johnAlexander	15:932d8b4e52c9	4152	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4153	pEnabled = (pEnabled & 1);
johnAlexander	15:932d8b4e52c9	4154
johnAlexander	15:932d8b4e52c9	4155	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4156	return Status;
johnAlexander	0:c523920bcc09	4157	}
johnAlexander	0:c523920bcc09	4158
johnAlexander	0:c523920bcc09	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	0:c523920bcc09	4185	VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	4186	VL53L0X_SequenceStepId SequenceStepId,
johnAlexander	15:932d8b4e52c9	4187	uint32_t TimeOutMicroSecs)
johnAlexander	0:c523920bcc09	4188	{
johnAlexander	15:932d8b4e52c9	4189	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4190	uint8_t CurrentVCSELPulsePeriodPClk;
johnAlexander	15:932d8b4e52c9	4191	uint8_t MsrcEncodedTimeOut;
johnAlexander	15:932d8b4e52c9	4192	uint16_t PreRangeEncodedTimeOut;
johnAlexander	15:932d8b4e52c9	4193	uint16_t PreRangeTimeOutMClks;
johnAlexander	15:932d8b4e52c9	4194	uint16_t MsrcRangeTimeOutMClks;
johnAlexander	15:932d8b4e52c9	4195	uint32_t FinalRangeTimeOutMClks;
johnAlexander	15:932d8b4e52c9	4196	uint16_t FinalRangeEncodedTimeOut;
johnAlexander	15:932d8b4e52c9	4197	VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
johnAlexander	15:932d8b4e52c9	4198
johnAlexander	15:932d8b4e52c9	4199	if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) \|\|
johnAlexander	15:932d8b4e52c9	4200	(SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) \|\|
johnAlexander	15:932d8b4e52c9	4201	(SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
johnAlexander	15:932d8b4e52c9	4202
johnAlexander	15:932d8b4e52c9	4203	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	4204	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	4205	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4206
johnAlexander	15:932d8b4e52c9	4207	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4208	MsrcRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev,
johnAlexander	15:932d8b4e52c9	4209	TimeOutMicroSecs,
johnAlexander	15:932d8b4e52c9	4210	(uint8_t)CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4211
johnAlexander	15:932d8b4e52c9	4212	if (MsrcRangeTimeOutMClks > 256)
johnAlexander	15:932d8b4e52c9	4213	MsrcEncodedTimeOut = 255;
johnAlexander	15:932d8b4e52c9	4214	else
johnAlexander	15:932d8b4e52c9	4215	MsrcEncodedTimeOut =
johnAlexander	15:932d8b4e52c9	4216	(uint8_t)MsrcRangeTimeOutMClks - 1;
johnAlexander	15:932d8b4e52c9	4217
johnAlexander	15:932d8b4e52c9	4218	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	4219	LastEncodedTimeout,
johnAlexander	15:932d8b4e52c9	4220	MsrcEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	4221	}
johnAlexander	15:932d8b4e52c9	4222
johnAlexander	15:932d8b4e52c9	4223	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4224	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	4225	VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
johnAlexander	15:932d8b4e52c9	4226	MsrcEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	4227	}
johnAlexander	15:932d8b4e52c9	4228	} else {
johnAlexander	15:932d8b4e52c9	4229
johnAlexander	15:932d8b4e52c9	4230	if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
johnAlexander	15:932d8b4e52c9	4231
johnAlexander	15:932d8b4e52c9	4232	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4233	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	4234	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	4235	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4236	PreRangeTimeOutMClks =
johnAlexander	15:932d8b4e52c9	4237	VL53L0X_calc_timeout_mclks(Dev,
johnAlexander	15:932d8b4e52c9	4238	TimeOutMicroSecs,
johnAlexander	15:932d8b4e52c9	4239	(uint8_t)CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4240	PreRangeEncodedTimeOut = VL53L0X_encode_timeout(
johnAlexander	15:932d8b4e52c9	4241	PreRangeTimeOutMClks);
johnAlexander	15:932d8b4e52c9	4242
johnAlexander	15:932d8b4e52c9	4243	VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	4244	LastEncodedTimeout,
johnAlexander	15:932d8b4e52c9	4245	PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	4246	}
johnAlexander	15:932d8b4e52c9	4247
johnAlexander	15:932d8b4e52c9	4248	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4249	Status = VL53L0X_WrWord(Dev,
johnAlexander	15:932d8b4e52c9	4250	VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
johnAlexander	15:932d8b4e52c9	4251	PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	4252	}
johnAlexander	15:932d8b4e52c9	4253
johnAlexander	15:932d8b4e52c9	4254	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4255	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	4256	Dev,
johnAlexander	15:932d8b4e52c9	4257	PreRangeTimeoutMicroSecs,
johnAlexander	15:932d8b4e52c9	4258	TimeOutMicroSecs);
johnAlexander	15:932d8b4e52c9	4259	}
johnAlexander	15:932d8b4e52c9	4260	} else if (SequenceStepId == 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	15:932d8b4e52c9	4268	VL53L0X_GetSequenceStepEnables(Dev,
johnAlexander	15:932d8b4e52c9	4269	&SchedulerSequenceSteps);
johnAlexander	15:932d8b4e52c9	4270	PreRangeTimeOutMClks = 0;
johnAlexander	15:932d8b4e52c9	4271	if (SchedulerSequenceSteps.PreRangeOn) {
johnAlexander	15:932d8b4e52c9	4272
johnAlexander	15:932d8b4e52c9	4273	/* Retrieve PRE-RANGE VCSEL Period */
johnAlexander	15:932d8b4e52c9	4274	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	4275	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	4276	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4277
johnAlexander	15:932d8b4e52c9	4278	/* Retrieve PRE-RANGE Timeout in Macro periods
johnAlexander	15:932d8b4e52c9	4279	* (MCLKS) */
johnAlexander	15:932d8b4e52c9	4280	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4281	Status = VL53L0X_RdWord(Dev, 0x51,
johnAlexander	15:932d8b4e52c9	4282	&PreRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	4283	PreRangeTimeOutMClks =
johnAlexander	15:932d8b4e52c9	4284	VL53L0X_decode_timeout(
johnAlexander	15:932d8b4e52c9	4285	PreRangeEncodedTimeOut);
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	15:932d8b4e52c9	4292	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4293
johnAlexander	15:932d8b4e52c9	4294	Status = VL53L0X_GetVcselPulsePeriod(Dev,
johnAlexander	15:932d8b4e52c9	4295	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	4296	&CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4297	}
johnAlexander	15:932d8b4e52c9	4298	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4299
johnAlexander	15:932d8b4e52c9	4300	FinalRangeTimeOutMClks =
johnAlexander	15:932d8b4e52c9	4301	VL53L0X_calc_timeout_mclks(Dev,
johnAlexander	15:932d8b4e52c9	4302	TimeOutMicroSecs,
johnAlexander	15:932d8b4e52c9	4303	(uint8_t) CurrentVCSELPulsePeriodPClk);
johnAlexander	15:932d8b4e52c9	4304
johnAlexander	15:932d8b4e52c9	4305	FinalRangeTimeOutMClks += PreRangeTimeOutMClks;
johnAlexander	15:932d8b4e52c9	4306
johnAlexander	15:932d8b4e52c9	4307	FinalRangeEncodedTimeOut =
johnAlexander	15:932d8b4e52c9	4308	VL53L0X_encode_timeout(FinalRangeTimeOutMClks);
johnAlexander	15:932d8b4e52c9	4309
johnAlexander	15:932d8b4e52c9	4310	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4311	Status = VL53L0X_WrWord(Dev, 0x71,
johnAlexander	15:932d8b4e52c9	4312	FinalRangeEncodedTimeOut);
johnAlexander	15:932d8b4e52c9	4313	}
johnAlexander	15:932d8b4e52c9	4314
johnAlexander	15:932d8b4e52c9	4315	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4316	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	4317	Dev,
johnAlexander	15:932d8b4e52c9	4318	FinalRangeTimeoutMicroSecs,
johnAlexander	15:932d8b4e52c9	4319	TimeOutMicroSecs);
johnAlexander	15:932d8b4e52c9	4320	}
johnAlexander	15:932d8b4e52c9	4321	}
johnAlexander	15:932d8b4e52c9	4322	} else
johnAlexander	15:932d8b4e52c9	4323	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4324
johnAlexander	15:932d8b4e52c9	4325	}
johnAlexander	15:932d8b4e52c9	4326	return Status;
johnAlexander	0:c523920bcc09	4327	}
johnAlexander	0:c523920bcc09	4328
johnAlexander	0:c523920bcc09	4329	VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	4330	uint32_t MeasurementTimingBudgetMicroSeconds)
johnAlexander	0:c523920bcc09	4331	{
johnAlexander	15:932d8b4e52c9	4332	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4333	uint32_t FinalRangeTimingBudgetMicroSeconds;
johnAlexander	15:932d8b4e52c9	4334	VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
johnAlexander	15:932d8b4e52c9	4335	uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
johnAlexander	15:932d8b4e52c9	4336	uint32_t StartOverheadMicroSeconds = 1910;
johnAlexander	15:932d8b4e52c9	4337	uint32_t EndOverheadMicroSeconds = 960;
johnAlexander	15:932d8b4e52c9	4338	uint32_t MsrcOverheadMicroSeconds = 660;
johnAlexander	15:932d8b4e52c9	4339	uint32_t TccOverheadMicroSeconds = 590;
johnAlexander	15:932d8b4e52c9	4340	uint32_t DssOverheadMicroSeconds = 690;
johnAlexander	15:932d8b4e52c9	4341	uint32_t PreRangeOverheadMicroSeconds = 660;
johnAlexander	15:932d8b4e52c9	4342	uint32_t FinalRangeOverheadMicroSeconds = 550;
johnAlexander	15:932d8b4e52c9	4343	uint32_t PreRangeTimeoutMicroSeconds = 0;
johnAlexander	15:932d8b4e52c9	4344	uint32_t cMinTimingBudgetMicroSeconds = 20000;
johnAlexander	15:932d8b4e52c9	4345	uint32_t SubTimeout = 0;
johnAlexander	15:932d8b4e52c9	4346
johnAlexander	15:932d8b4e52c9	4347	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4348
johnAlexander	15:932d8b4e52c9	4349	if (MeasurementTimingBudgetMicroSeconds
johnAlexander	15:932d8b4e52c9	4350	< cMinTimingBudgetMicroSeconds) {
johnAlexander	15:932d8b4e52c9	4351	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4352	return Status;
johnAlexander	15:932d8b4e52c9	4353	}
johnAlexander	15:932d8b4e52c9	4354
johnAlexander	15:932d8b4e52c9	4355	FinalRangeTimingBudgetMicroSeconds =
johnAlexander	15:932d8b4e52c9	4356	MeasurementTimingBudgetMicroSeconds -
johnAlexander	15:932d8b4e52c9	4357	(StartOverheadMicroSeconds + EndOverheadMicroSeconds);
johnAlexander	15:932d8b4e52c9	4358
johnAlexander	15:932d8b4e52c9	4359	Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
johnAlexander	15:932d8b4e52c9	4360
johnAlexander	15:932d8b4e52c9	4361	if (Status == VL53L0X_ERROR_NONE &&
johnAlexander	15:932d8b4e52c9	4362	(SchedulerSequenceSteps.TccOn \|\|
johnAlexander	15:932d8b4e52c9	4363	SchedulerSequenceSteps.MsrcOn \|\|
johnAlexander	15:932d8b4e52c9	4364	SchedulerSequenceSteps.DssOn)) {
johnAlexander	15:932d8b4e52c9	4365
johnAlexander	15:932d8b4e52c9	4366	/* TCC, MSRC and DSS all share the same timeout */
johnAlexander	15:932d8b4e52c9	4367	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	4368	VL53L0X_SEQUENCESTEP_MSRC,
johnAlexander	15:932d8b4e52c9	4369	&MsrcDccTccTimeoutMicroSeconds);
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	15:932d8b4e52c9	4374	if (Status != VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4375	return Status;
johnAlexander	15:932d8b4e52c9	4376
johnAlexander	15:932d8b4e52c9	4377	/* TCC */
johnAlexander	15:932d8b4e52c9	4378	if (SchedulerSequenceSteps.TccOn) {
johnAlexander	15:932d8b4e52c9	4379
johnAlexander	15:932d8b4e52c9	4380	SubTimeout = MsrcDccTccTimeoutMicroSeconds
johnAlexander	15:932d8b4e52c9	4381	+ TccOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	4382
johnAlexander	15:932d8b4e52c9	4383	if (SubTimeout <
johnAlexander	15:932d8b4e52c9	4384	FinalRangeTimingBudgetMicroSeconds) {
johnAlexander	15:932d8b4e52c9	4385	FinalRangeTimingBudgetMicroSeconds -=
johnAlexander	15:932d8b4e52c9	4386	SubTimeout;
johnAlexander	15:932d8b4e52c9	4387	} else {
johnAlexander	15:932d8b4e52c9	4388	/* Requested timeout too big. */
johnAlexander	15:932d8b4e52c9	4389	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4390	}
johnAlexander	15:932d8b4e52c9	4391	}
johnAlexander	15:932d8b4e52c9	4392
johnAlexander	15:932d8b4e52c9	4393	if (Status != VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4394	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4395	return Status;
johnAlexander	15:932d8b4e52c9	4396	}
johnAlexander	15:932d8b4e52c9	4397
johnAlexander	15:932d8b4e52c9	4398	/* DSS */
johnAlexander	15:932d8b4e52c9	4399	if (SchedulerSequenceSteps.DssOn) {
johnAlexander	15:932d8b4e52c9	4400
johnAlexander	15:932d8b4e52c9	4401	SubTimeout = 2 * (MsrcDccTccTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	4402	DssOverheadMicroSeconds);
johnAlexander	15:932d8b4e52c9	4403
johnAlexander	15:932d8b4e52c9	4404	if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
johnAlexander	15:932d8b4e52c9	4405	FinalRangeTimingBudgetMicroSeconds
johnAlexander	15:932d8b4e52c9	4406	-= SubTimeout;
johnAlexander	15:932d8b4e52c9	4407	} else {
johnAlexander	15:932d8b4e52c9	4408	/* Requested timeout too big. */
johnAlexander	15:932d8b4e52c9	4409	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4410	}
johnAlexander	15:932d8b4e52c9	4411	} else if (SchedulerSequenceSteps.MsrcOn) {
johnAlexander	15:932d8b4e52c9	4412	/* MSRC */
johnAlexander	15:932d8b4e52c9	4413	SubTimeout = MsrcDccTccTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	4414	MsrcOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	4415
johnAlexander	15:932d8b4e52c9	4416	if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
johnAlexander	15:932d8b4e52c9	4417	FinalRangeTimingBudgetMicroSeconds
johnAlexander	15:932d8b4e52c9	4418	-= SubTimeout;
johnAlexander	15:932d8b4e52c9	4419	} else {
johnAlexander	15:932d8b4e52c9	4420	/* Requested timeout too big. */
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4427	if (Status != VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4428	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4429	return Status;
johnAlexander	15:932d8b4e52c9	4430	}
johnAlexander	15:932d8b4e52c9	4431
johnAlexander	15:932d8b4e52c9	4432	if (SchedulerSequenceSteps.PreRangeOn) {
johnAlexander	15:932d8b4e52c9	4433
johnAlexander	15:932d8b4e52c9	4434	/* Subtract the Pre-range timeout if enabled. */
johnAlexander	15:932d8b4e52c9	4435
johnAlexander	15:932d8b4e52c9	4436	Status = get_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	4437	VL53L0X_SEQUENCESTEP_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	4438	&PreRangeTimeoutMicroSeconds);
johnAlexander	15:932d8b4e52c9	4439
johnAlexander	15:932d8b4e52c9	4440	SubTimeout = PreRangeTimeoutMicroSeconds +
johnAlexander	15:932d8b4e52c9	4441	PreRangeOverheadMicroSeconds;
johnAlexander	15:932d8b4e52c9	4442
johnAlexander	15:932d8b4e52c9	4443	if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
johnAlexander	15:932d8b4e52c9	4444	FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
johnAlexander	15:932d8b4e52c9	4445	} else {
johnAlexander	15:932d8b4e52c9	4446	/* Requested timeout too big. */
johnAlexander	15:932d8b4e52c9	4447	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4448	}
johnAlexander	15:932d8b4e52c9	4449	}
johnAlexander	15:932d8b4e52c9	4450
johnAlexander	15:932d8b4e52c9	4451
johnAlexander	15:932d8b4e52c9	4452	if (Status == VL53L0X_ERROR_NONE &&
johnAlexander	15:932d8b4e52c9	4453	SchedulerSequenceSteps.FinalRangeOn) {
johnAlexander	15:932d8b4e52c9	4454
johnAlexander	15:932d8b4e52c9	4455	FinalRangeTimingBudgetMicroSeconds -=
johnAlexander	15:932d8b4e52c9	4456	FinalRangeOverheadMicroSeconds;
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	15:932d8b4e52c9	4465	Status = set_sequence_step_timeout(Dev,
johnAlexander	15:932d8b4e52c9	4466	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	4467	FinalRangeTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	4468
johnAlexander	15:932d8b4e52c9	4469	VL53L0X_SETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	4470	MeasurementTimingBudgetMicroSeconds,
johnAlexander	15:932d8b4e52c9	4471	MeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	4472	}
johnAlexander	15:932d8b4e52c9	4473
johnAlexander	15:932d8b4e52c9	4474	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4475
johnAlexander	15:932d8b4e52c9	4476	return Status;
johnAlexander	0:c523920bcc09	4477	}
johnAlexander	0:c523920bcc09	4478
johnAlexander	0:c523920bcc09	4479	VL53L0X_Error VL53L0X::VL53L0X_SetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	4480	uint32_t MeasurementTimingBudgetMicroSeconds)
johnAlexander	0:c523920bcc09	4481	{
johnAlexander	15:932d8b4e52c9	4482	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4483	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4484
johnAlexander	15:932d8b4e52c9	4485	Status = VL53L0X_set_measurement_timing_budget_micro_seconds(Dev,
johnAlexander	15:932d8b4e52c9	4486	MeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	4487
johnAlexander	15:932d8b4e52c9	4488	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4489
johnAlexander	15:932d8b4e52c9	4490	return Status;
johnAlexander	0:c523920bcc09	4491	}
johnAlexander	0:c523920bcc09	4492
johnAlexander	0:c523920bcc09	4493	VL53L0X_Error VL53L0X::VL53L0X_SetSequenceStepEnable(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	4494	VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled)
johnAlexander	0:c523920bcc09	4495	{
johnAlexander	15:932d8b4e52c9	4496	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4497	uint8_t SequenceConfig = 0;
johnAlexander	15:932d8b4e52c9	4498	uint8_t SequenceConfigNew = 0;
johnAlexander	15:932d8b4e52c9	4499	uint32_t MeasurementTimingBudgetMicroSeconds;
johnAlexander	15:932d8b4e52c9	4500	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4501
johnAlexander	15:932d8b4e52c9	4502	Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
johnAlexander	15:932d8b4e52c9	4503	&SequenceConfig);
johnAlexander	15:932d8b4e52c9	4504
johnAlexander	15:932d8b4e52c9	4505	SequenceConfigNew = SequenceConfig;
johnAlexander	15:932d8b4e52c9	4506
johnAlexander	15:932d8b4e52c9	4507	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4508	if (SequenceStepEnabled == 1) {
johnAlexander	15:932d8b4e52c9	4509
johnAlexander	15:932d8b4e52c9	4510	/* Enable requested sequence step
johnAlexander	15:932d8b4e52c9	4511	*/
johnAlexander	15:932d8b4e52c9	4512	switch (SequenceStepId) {
johnAlexander	15:932d8b4e52c9	4513	case VL53L0X_SEQUENCESTEP_TCC:
johnAlexander	15:932d8b4e52c9	4514	SequenceConfigNew \|= 0x10;
johnAlexander	15:932d8b4e52c9	4515	break;
johnAlexander	15:932d8b4e52c9	4516	case VL53L0X_SEQUENCESTEP_DSS:
johnAlexander	15:932d8b4e52c9	4517	SequenceConfigNew \|= 0x28;
johnAlexander	15:932d8b4e52c9	4518	break;
johnAlexander	15:932d8b4e52c9	4519	case VL53L0X_SEQUENCESTEP_MSRC:
johnAlexander	15:932d8b4e52c9	4520	SequenceConfigNew \|= 0x04;
johnAlexander	15:932d8b4e52c9	4521	break;
johnAlexander	15:932d8b4e52c9	4522	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
johnAlexander	15:932d8b4e52c9	4523	SequenceConfigNew \|= 0x40;
johnAlexander	15:932d8b4e52c9	4524	break;
johnAlexander	15:932d8b4e52c9	4525	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	4526	SequenceConfigNew \|= 0x80;
johnAlexander	15:932d8b4e52c9	4527	break;
johnAlexander	15:932d8b4e52c9	4528	default:
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4534	switch (SequenceStepId) {
johnAlexander	15:932d8b4e52c9	4535	case VL53L0X_SEQUENCESTEP_TCC:
johnAlexander	15:932d8b4e52c9	4536	SequenceConfigNew &= 0xef;
johnAlexander	15:932d8b4e52c9	4537	break;
johnAlexander	15:932d8b4e52c9	4538	case VL53L0X_SEQUENCESTEP_DSS:
johnAlexander	15:932d8b4e52c9	4539	SequenceConfigNew &= 0xd7;
johnAlexander	15:932d8b4e52c9	4540	break;
johnAlexander	15:932d8b4e52c9	4541	case VL53L0X_SEQUENCESTEP_MSRC:
johnAlexander	15:932d8b4e52c9	4542	SequenceConfigNew &= 0xfb;
johnAlexander	15:932d8b4e52c9	4543	break;
johnAlexander	15:932d8b4e52c9	4544	case VL53L0X_SEQUENCESTEP_PRE_RANGE:
johnAlexander	15:932d8b4e52c9	4545	SequenceConfigNew &= 0xbf;
johnAlexander	15:932d8b4e52c9	4546	break;
johnAlexander	15:932d8b4e52c9	4547	case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	4548	SequenceConfigNew &= 0x7f;
johnAlexander	15:932d8b4e52c9	4549	break;
johnAlexander	15:932d8b4e52c9	4550	default:
johnAlexander	15:932d8b4e52c9	4551	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4552	}
johnAlexander	15:932d8b4e52c9	4553	}
johnAlexander	15:932d8b4e52c9	4554	}
johnAlexander	15:932d8b4e52c9	4555
johnAlexander	15:932d8b4e52c9	4556	if (SequenceConfigNew != SequenceConfig) {
johnAlexander	15:932d8b4e52c9	4557	/* Apply New Setting */
johnAlexander	15:932d8b4e52c9	4558	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4559	Status = VL53L0X_WrByte(Dev,
johnAlexander	15:932d8b4e52c9	4560	VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, SequenceConfigNew);
johnAlexander	15:932d8b4e52c9	4561	}
johnAlexander	15:932d8b4e52c9	4562	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4563	PALDevDataSet(Dev, SequenceConfig, SequenceConfigNew);
johnAlexander	15:932d8b4e52c9	4564
johnAlexander	15:932d8b4e52c9	4565
johnAlexander	15:932d8b4e52c9	4566	/* Recalculate timing budget */
johnAlexander	15:932d8b4e52c9	4567	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4568	VL53L0X_GETPARAMETERFIELD(Dev,
johnAlexander	15:932d8b4e52c9	4569	MeasurementTimingBudgetMicroSeconds,
johnAlexander	15:932d8b4e52c9	4570	MeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	4571
johnAlexander	15:932d8b4e52c9	4572	VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
johnAlexander	15:932d8b4e52c9	4573	MeasurementTimingBudgetMicroSeconds);
johnAlexander	15:932d8b4e52c9	4574	}
johnAlexander	15:932d8b4e52c9	4575	}
johnAlexander	15:932d8b4e52c9	4576
johnAlexander	15:932d8b4e52c9	4577	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4578
johnAlexander	15:932d8b4e52c9	4579	return Status;
johnAlexander	0:c523920bcc09	4580	}
johnAlexander	0:c523920bcc09	4581
johnAlexander	0:c523920bcc09	4582	VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId,
johnAlexander	15:932d8b4e52c9	4583	uint8_t LimitCheckEnable)
johnAlexander	0:c523920bcc09	4584	{
johnAlexander	15:932d8b4e52c9	4585	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4586	FixPoint1616_t TempFix1616 = 0;
johnAlexander	15:932d8b4e52c9	4587	uint8_t LimitCheckEnableInt = 0;
johnAlexander	15:932d8b4e52c9	4588	uint8_t LimitCheckDisable = 0;
johnAlexander	15:932d8b4e52c9	4589	uint8_t Temp8;
johnAlexander	15:932d8b4e52c9	4590
johnAlexander	15:932d8b4e52c9	4591	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4592
johnAlexander	15:932d8b4e52c9	4593	if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
johnAlexander	15:932d8b4e52c9	4594	Status = VL53L0X_ERROR_INVALID_PARAMS;
johnAlexander	15:932d8b4e52c9	4595	} else {
johnAlexander	15:932d8b4e52c9	4596	if (LimitCheckEnable == 0) {
johnAlexander	15:932d8b4e52c9	4597	TempFix1616 = 0;
johnAlexander	15:932d8b4e52c9	4598	LimitCheckEnableInt = 0;
johnAlexander	15:932d8b4e52c9	4599	LimitCheckDisable = 1;
johnAlexander	15:932d8b4e52c9	4600
johnAlexander	15:932d8b4e52c9	4601	} else {
johnAlexander	15:932d8b4e52c9	4602	VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
johnAlexander	15:932d8b4e52c9	4603	LimitCheckId, TempFix1616);
johnAlexander	15:932d8b4e52c9	4604	LimitCheckDisable = 0;
johnAlexander	15:932d8b4e52c9	4605	/* this to be sure to have either 0 or 1 */
johnAlexander	15:932d8b4e52c9	4606	LimitCheckEnableInt = 1;
johnAlexander	15:932d8b4e52c9	4607	}
johnAlexander	15:932d8b4e52c9	4608
johnAlexander	15:932d8b4e52c9	4609	switch (LimitCheckId) {
johnAlexander	15:932d8b4e52c9	4610
johnAlexander	15:932d8b4e52c9	4611	case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
johnAlexander	15:932d8b4e52c9	4612	/* internal computation: */
johnAlexander	15:932d8b4e52c9	4613	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
johnAlexander	15:932d8b4e52c9	4614	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	4615	LimitCheckEnableInt);
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	15:932d8b4e52c9	4621	Status = VL53L0X_WrWord(Dev,
johnAlexander	15:932d8b4e52c9	4622	VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
johnAlexander	15:932d8b4e52c9	4623	VL53L0X_FIXPOINT1616TOFIXPOINT97(TempFix1616));
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	15:932d8b4e52c9	4630	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
johnAlexander	15:932d8b4e52c9	4631	VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
johnAlexander	15:932d8b4e52c9	4632	LimitCheckEnableInt);
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	15:932d8b4e52c9	4639	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
johnAlexander	15:932d8b4e52c9	4640	VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
johnAlexander	15:932d8b4e52c9	4641	LimitCheckEnableInt);
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	15:932d8b4e52c9	4647	Temp8 = (uint8_t)(LimitCheckDisable << 1);
johnAlexander	15:932d8b4e52c9	4648	Status = VL53L0X_UpdateByte(Dev,
johnAlexander	15:932d8b4e52c9	4649	VL53L0X_REG_MSRC_CONFIG_CONTROL,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4656	Temp8 = (uint8_t)(LimitCheckDisable << 4);
johnAlexander	15:932d8b4e52c9	4657	Status = VL53L0X_UpdateByte(Dev,
johnAlexander	15:932d8b4e52c9	4658	VL53L0X_REG_MSRC_CONFIG_CONTROL,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	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	15:932d8b4e52c9	4671	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4672	if (LimitCheckEnable == 0) {
johnAlexander	15:932d8b4e52c9	4673	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
johnAlexander	15:932d8b4e52c9	4674	LimitCheckId, 0);
johnAlexander	15:932d8b4e52c9	4675	} else {
johnAlexander	15:932d8b4e52c9	4676	VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
johnAlexander	15:932d8b4e52c9	4677	LimitCheckId, 1);
johnAlexander	15:932d8b4e52c9	4678	}
johnAlexander	15:932d8b4e52c9	4679	}
johnAlexander	15:932d8b4e52c9	4680
johnAlexander	15:932d8b4e52c9	4681	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4682	return Status;
johnAlexander	0:c523920bcc09	4683	}
johnAlexander	0:c523920bcc09	4684
johnAlexander	0:c523920bcc09	4685	VL53L0X_Error VL53L0X::VL53L0X_StaticInit(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	4686	{
johnAlexander	15:932d8b4e52c9	4687	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4688	VL53L0X_DeviceParameters_t CurrentParameters = {0};
johnAlexander	15:932d8b4e52c9	4689	uint8_t *pTuningSettingBuffer;
johnAlexander	15:932d8b4e52c9	4690	uint16_t tempword = 0;
johnAlexander	15:932d8b4e52c9	4691	uint8_t tempbyte = 0;
johnAlexander	15:932d8b4e52c9	4692	uint8_t UseInternalTuningSettings = 0;
johnAlexander	15:932d8b4e52c9	4693	uint32_t count = 0;
johnAlexander	15:932d8b4e52c9	4694	uint8_t isApertureSpads = 0;
johnAlexander	15:932d8b4e52c9	4695	uint32_t refSpadCount = 0;
johnAlexander	15:932d8b4e52c9	4696	uint8_t ApertureSpads = 0;
johnAlexander	15:932d8b4e52c9	4697	uint8_t vcselPulsePeriodPCLK;
johnAlexander	15:932d8b4e52c9	4698	uint32_t seqTimeoutMicroSecs;
johnAlexander	15:932d8b4e52c9	4699
johnAlexander	15:932d8b4e52c9	4700	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4701
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4707	ApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
johnAlexander	15:932d8b4e52c9	4708	ReferenceSpadType);
johnAlexander	15:932d8b4e52c9	4709
johnAlexander	15:932d8b4e52c9	4710	/* NVM value invalid */
johnAlexander	15:932d8b4e52c9	4711	if ((ApertureSpads > 1) \|\|
johnAlexander	15:932d8b4e52c9	4712	((ApertureSpads == 1) && (count > 32)) \|\|
johnAlexander	15:932d8b4e52c9	4713	((ApertureSpads == 0) && (count > 12)))
johnAlexander	15:932d8b4e52c9	4714	Status = VL53L0X_perform_ref_spad_management(Dev, &refSpadCount,
johnAlexander	15:932d8b4e52c9	4715	&isApertureSpads);
johnAlexander	15:932d8b4e52c9	4716	else
johnAlexander	15:932d8b4e52c9	4717	Status = VL53L0X_set_reference_spads(Dev, count, ApertureSpads);
johnAlexander	15:932d8b4e52c9	4718
johnAlexander	15:932d8b4e52c9	4719
johnAlexander	15:932d8b4e52c9	4720	/* Initialize tuning settings buffer to prevent compiler warning. */
johnAlexander	15:932d8b4e52c9	4721	pTuningSettingBuffer = DefaultTuningSettings;
johnAlexander	15:932d8b4e52c9	4722
johnAlexander	15:932d8b4e52c9	4723	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4724	UseInternalTuningSettings = PALDevDataGet(Dev,
johnAlexander	15:932d8b4e52c9	4725	UseInternalTuningSettings);
johnAlexander	15:932d8b4e52c9	4726
johnAlexander	15:932d8b4e52c9	4727	if (UseInternalTuningSettings == 0)
johnAlexander	15:932d8b4e52c9	4728	pTuningSettingBuffer = PALDevDataGet(Dev,
johnAlexander	15:932d8b4e52c9	4729	pTuningSettingsPointer);
johnAlexander	15:932d8b4e52c9	4730	else
johnAlexander	15:932d8b4e52c9	4731	pTuningSettingBuffer = DefaultTuningSettings;
johnAlexander	15:932d8b4e52c9	4732
johnAlexander	15:932d8b4e52c9	4733	}
johnAlexander	15:932d8b4e52c9	4734
johnAlexander	15:932d8b4e52c9	4735	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4736	Status = VL53L0X_load_tuning_settings(Dev, pTuningSettingBuffer);
johnAlexander	15:932d8b4e52c9	4737
johnAlexander	15:932d8b4e52c9	4738
johnAlexander	15:932d8b4e52c9	4739	/* Set interrupt config to new sample ready */
johnAlexander	15:932d8b4e52c9	4740	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4741	Status = VL53L0X_SetGpioConfig(Dev, 0, 0,
johnAlexander	15:932d8b4e52c9	4742	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
johnAlexander	15:932d8b4e52c9	4743	VL53L0X_INTERRUPTPOLARITY_LOW);
johnAlexander	15:932d8b4e52c9	4744	}
johnAlexander	15:932d8b4e52c9	4745
johnAlexander	15:932d8b4e52c9	4746	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4747	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	4748	Status \|= VL53L0X_RdWord(Dev, 0x84, &tempword);
johnAlexander	15:932d8b4e52c9	4749	Status \|= VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	4750	}
johnAlexander	15:932d8b4e52c9	4751
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4759	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4760	Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters);
johnAlexander	15:932d8b4e52c9	4761
johnAlexander	15:932d8b4e52c9	4762
johnAlexander	15:932d8b4e52c9	4763	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4764	Status = VL53L0X_GetFractionEnable(Dev, &tempbyte);
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4770	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4771	PALDevDataSet(Dev, CurrentParameters, CurrentParameters);
johnAlexander	15:932d8b4e52c9	4772
johnAlexander	15:932d8b4e52c9	4773
johnAlexander	15:932d8b4e52c9	4774	/* read the sequence config and save it */
johnAlexander	15:932d8b4e52c9	4775	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4776	Status = VL53L0X_RdByte(Dev,
johnAlexander	15:932d8b4e52c9	4777	VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4784	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4785	Status = VL53L0X_SetSequenceStepEnable(Dev,
johnAlexander	15:932d8b4e52c9	4786	VL53L0X_SEQUENCESTEP_TCC, 0);
johnAlexander	15:932d8b4e52c9	4787
johnAlexander	15:932d8b4e52c9	4788
johnAlexander	15:932d8b4e52c9	4789	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4790	Status = VL53L0X_SetSequenceStepEnable(Dev,
johnAlexander	15:932d8b4e52c9	4791	VL53L0X_SEQUENCESTEP_MSRC, 0);
johnAlexander	15:932d8b4e52c9	4792
johnAlexander	15:932d8b4e52c9	4793
johnAlexander	15:932d8b4e52c9	4794	/* Set PAL State to standby */
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4801	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4802	Status = VL53L0X_GetVcselPulsePeriod(
johnAlexander	15:932d8b4e52c9	4803	Dev,
johnAlexander	15:932d8b4e52c9	4804	VL53L0X_VCSEL_PERIOD_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	4805	&vcselPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	4806	}
johnAlexander	15:932d8b4e52c9	4807
johnAlexander	15:932d8b4e52c9	4808	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4809	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	4810	Dev,
johnAlexander	15:932d8b4e52c9	4811	PreRangeVcselPulsePeriod,
johnAlexander	15:932d8b4e52c9	4812	vcselPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	4813	}
johnAlexander	15:932d8b4e52c9	4814
johnAlexander	15:932d8b4e52c9	4815	/* Store final-range vcsel period */
johnAlexander	15:932d8b4e52c9	4816	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4817	Status = VL53L0X_GetVcselPulsePeriod(
johnAlexander	15:932d8b4e52c9	4818	Dev,
johnAlexander	15:932d8b4e52c9	4819	VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	4820	&vcselPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	4821	}
johnAlexander	15:932d8b4e52c9	4822
johnAlexander	15:932d8b4e52c9	4823	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4824	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	4825	Dev,
johnAlexander	15:932d8b4e52c9	4826	FinalRangeVcselPulsePeriod,
johnAlexander	15:932d8b4e52c9	4827	vcselPulsePeriodPCLK);
johnAlexander	15:932d8b4e52c9	4828	}
johnAlexander	15:932d8b4e52c9	4829
johnAlexander	15:932d8b4e52c9	4830	/* Store pre-range timeout */
johnAlexander	15:932d8b4e52c9	4831	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4832	Status = get_sequence_step_timeout(
johnAlexander	15:932d8b4e52c9	4833	Dev,
johnAlexander	15:932d8b4e52c9	4834	VL53L0X_SEQUENCESTEP_PRE_RANGE,
johnAlexander	15:932d8b4e52c9	4835	&seqTimeoutMicroSecs);
johnAlexander	15:932d8b4e52c9	4836	}
johnAlexander	15:932d8b4e52c9	4837
johnAlexander	15:932d8b4e52c9	4838	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4839	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	4840	Dev,
johnAlexander	15:932d8b4e52c9	4841	PreRangeTimeoutMicroSecs,
johnAlexander	15:932d8b4e52c9	4842	seqTimeoutMicroSecs);
johnAlexander	15:932d8b4e52c9	4843	}
johnAlexander	15:932d8b4e52c9	4844
johnAlexander	15:932d8b4e52c9	4845	/* Store final-range timeout */
johnAlexander	15:932d8b4e52c9	4846	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4847	Status = get_sequence_step_timeout(
johnAlexander	15:932d8b4e52c9	4848	Dev,
johnAlexander	15:932d8b4e52c9	4849	VL53L0X_SEQUENCESTEP_FINAL_RANGE,
johnAlexander	15:932d8b4e52c9	4850	&seqTimeoutMicroSecs);
johnAlexander	15:932d8b4e52c9	4851	}
johnAlexander	15:932d8b4e52c9	4852
johnAlexander	15:932d8b4e52c9	4853	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4854	VL53L0X_SETDEVICESPECIFICPARAMETER(
johnAlexander	15:932d8b4e52c9	4855	Dev,
johnAlexander	15:932d8b4e52c9	4856	FinalRangeTimeoutMicroSecs,
johnAlexander	15:932d8b4e52c9	4857	seqTimeoutMicroSecs);
johnAlexander	15:932d8b4e52c9	4858	}
johnAlexander	15:932d8b4e52c9	4859
johnAlexander	15:932d8b4e52c9	4860	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4861	return Status;
johnAlexander	0:c523920bcc09	4862	}
johnAlexander	0:c523920bcc09	4863
johnAlexander	0:c523920bcc09	4864
johnAlexander	0:c523920bcc09	4865	VL53L0X_Error VL53L0X::VL53L0X_StopMeasurement(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	4866	{
johnAlexander	15:932d8b4e52c9	4867	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4868	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4869
johnAlexander	15:932d8b4e52c9	4870	Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
johnAlexander	15:932d8b4e52c9	4871	VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
johnAlexander	15:932d8b4e52c9	4872
johnAlexander	15:932d8b4e52c9	4873	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	4874	Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	4875	Status = VL53L0X_WrByte(Dev, 0x91, 0x00);
johnAlexander	15:932d8b4e52c9	4876	Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
johnAlexander	15:932d8b4e52c9	4877	Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	4878
johnAlexander	15:932d8b4e52c9	4879	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	4880	/* Set PAL State to Idle */
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	4885	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4886	Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 0);
johnAlexander	15:932d8b4e52c9	4887
johnAlexander	15:932d8b4e52c9	4888	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	4889	return Status;
johnAlexander	0:c523920bcc09	4890	}
johnAlexander	0:c523920bcc09	4891
johnAlexander	0:c523920bcc09	4892	VL53L0X_Error VL53L0X::VL53L0X_GetStopCompletedStatus(VL53L0X_DEV Dev,
johnAlexander	15:932d8b4e52c9	4893	uint32_t *pStopStatus)
johnAlexander	0:c523920bcc09	4894	{
johnAlexander	15:932d8b4e52c9	4895	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	15:932d8b4e52c9	4896	uint8_t Byte = 0;
johnAlexander	15:932d8b4e52c9	4897	LOG_FUNCTION_START("");
johnAlexander	15:932d8b4e52c9	4898
johnAlexander	15:932d8b4e52c9	4899	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	4900
johnAlexander	15:932d8b4e52c9	4901	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4902	Status = VL53L0X_RdByte(Dev, 0x04, &Byte);
johnAlexander	15:932d8b4e52c9	4903
johnAlexander	15:932d8b4e52c9	4904	if (Status == VL53L0X_ERROR_NONE)
johnAlexander	15:932d8b4e52c9	4905	Status = VL53L0X_WrByte(Dev, 0xFF, 0x0);
johnAlexander	15:932d8b4e52c9	4906
johnAlexander	15:932d8b4e52c9	4907	*pStopStatus = Byte;
johnAlexander	15:932d8b4e52c9	4908
johnAlexander	15:932d8b4e52c9	4909	if (Byte == 0) {
johnAlexander	15:932d8b4e52c9	4910	Status = VL53L0X_WrByte(Dev, 0x80, 0x01);
johnAlexander	15:932d8b4e52c9	4911	Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
johnAlexander	15:932d8b4e52c9	4912	Status = VL53L0X_WrByte(Dev, 0x00, 0x00);
johnAlexander	15:932d8b4e52c9	4913	Status = VL53L0X_WrByte(Dev, 0x91,
johnAlexander	15:932d8b4e52c9	4914	PALDevDataGet(Dev, StopVariable));
johnAlexander	15:932d8b4e52c9	4915	Status = VL53L0X_WrByte(Dev, 0x00, 0x01);
johnAlexander	15:932d8b4e52c9	4916	Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
johnAlexander	15:932d8b4e52c9	4917	Status = VL53L0X_WrByte(Dev, 0x80, 0x00);
johnAlexander	15:932d8b4e52c9	4918	}
johnAlexander	15:932d8b4e52c9	4919
johnAlexander	15:932d8b4e52c9	4920	LOG_FUNCTION_END(Status);
johnAlexander	15:932d8b4e52c9	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	0:c523920bcc09	4926	VL53L0X_Error VL53L0X::VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
johnAlexander	0:c523920bcc09	4927	{
johnAlexander	15:932d8b4e52c9	4928	int status;
johnAlexander	15:932d8b4e52c9	4929
johnAlexander	15:932d8b4e52c9	4930	status = VL53L0X_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
johnAlexander	15:932d8b4e52c9	4931	return status;
johnAlexander	0:c523920bcc09	4932	}
johnAlexander	0:c523920bcc09	4933
johnAlexander	0:c523920bcc09	4934	VL53L0X_Error VL53L0X::VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, 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	0:c523920bcc09	4942	status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, pdata, (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	0:c523920bcc09	4948	VL53L0X_Error VL53L0X::VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
johnAlexander	0:c523920bcc09	4949	{
johnAlexander	15:932d8b4e52c9	4950	int status;
johnAlexander	15:932d8b4e52c9	4951
johnAlexander	15:932d8b4e52c9	4952	status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, &data, 1);
johnAlexander	15:932d8b4e52c9	4953	return status;
johnAlexander	0:c523920bcc09	4954	}
johnAlexander	15:932d8b4e52c9	4955
johnAlexander	0:c523920bcc09	4956	VL53L0X_Error VL53L0X::VL53L0X_WrWord(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	15:932d8b4e52c9	4963	status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
johnAlexander	15:932d8b4e52c9	4964	return status;
johnAlexander	0:c523920bcc09	4965	}
johnAlexander	15:932d8b4e52c9	4966
johnAlexander	0:c523920bcc09	4967	VL53L0X_Error VL53L0X::VL53L0X_WrDWord(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	15:932d8b4e52c9	4976	status=VL53L0X_I2CWrite(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	0:c523920bcc09	4981	VL53L0X_Error VL53L0X::VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
johnAlexander	0:c523920bcc09	4982	{
johnAlexander	15:932d8b4e52c9	4983	int status;
johnAlexander	15:932d8b4e52c9	4984
johnAlexander	15:932d8b4e52c9	4985	status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, 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	0:c523920bcc09	4993	VL53L0X_Error VL53L0X::VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *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	15:932d8b4e52c9	4998	status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, buffer, 2);
johnAlexander	15:932d8b4e52c9	4999	if (!status) {
johnAlexander	15:932d8b4e52c9	5000	*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	0:c523920bcc09	5006	VL53L0X_Error VL53L0X::VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *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	15:932d8b4e52c9	5011	status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, buffer, 4);
johnAlexander	15:932d8b4e52c9	5012	if(!status) {
johnAlexander	15:932d8b4e52c9	5013	*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	0:c523920bcc09	5019	VL53L0X_Error VL53L0X::VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData)
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	15:932d8b4e52c9	5025	status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, &buffer,1);
johnAlexander	15:932d8b4e52c9	5026	if (!status) {
johnAlexander	15:932d8b4e52c9	5027	buffer = (buffer & AndData) \| OrData;
johnAlexander	15:932d8b4e52c9	5028	status = VL53L0X_I2CWrite(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	0:c523920bcc09	5033	VL53L0X_Error VL53L0X::VL53L0X_I2CWrite(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
johnAlexander	0:c523920bcc09	5034	{
johnAlexander	15:932d8b4e52c9	5035	int ret;
johnAlexander	15:932d8b4e52c9	5036
johnAlexander	15:932d8b4e52c9	5037	ret = dev_i2c.i2c_write(pBuffer, 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	0:c523920bcc09	5044	VL53L0X_Error VL53L0X::VL53L0X_I2CRead(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
johnAlexander	0:c523920bcc09	5045	{
johnAlexander	15:932d8b4e52c9	5046	int ret;
johnAlexander	15:932d8b4e52c9	5047
johnAlexander	15:932d8b4e52c9	5048	ret = dev_i2c.i2c_read(pBuffer, 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	0:c523920bcc09	5060	status = VL53L0X_RdWord(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	0:c523920bcc09	5073	VL53L0X_Error VL53L0X::WaitMeasurementDataReady(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	5074	{
johnAlexander	0:c523920bcc09	5075	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	0:c523920bcc09	5076	uint8_t NewDatReady=0;
johnAlexander	0:c523920bcc09	5077	uint32_t LoopNb;
johnAlexander	0:c523920bcc09	5078
johnAlexander	0:c523920bcc09	5079	// Wait until it finished
johnAlexander	0:c523920bcc09	5080	// use timeout to avoid deadlock
johnAlexander	0:c523920bcc09	5081	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5082	LoopNb = 0;
johnAlexander	0:c523920bcc09	5083	do {
johnAlexander	0:c523920bcc09	5084	Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
johnAlexander	0:c523920bcc09	5085	if ((NewDatReady == 0x01) \|\| Status != VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5086	break;
johnAlexander	0:c523920bcc09	5087	}
johnAlexander	0:c523920bcc09	5088	LoopNb = LoopNb + 1;
johnAlexander	0:c523920bcc09	5089	VL53L0X_PollingDelay(Dev);
johnAlexander	0:c523920bcc09	5090	} while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
johnAlexander	0:c523920bcc09	5091
johnAlexander	0:c523920bcc09	5092	if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
johnAlexander	0:c523920bcc09	5093	Status = VL53L0X_ERROR_TIME_OUT;
johnAlexander	0:c523920bcc09	5094	}
johnAlexander	0:c523920bcc09	5095	}
johnAlexander	0:c523920bcc09	5096
johnAlexander	0:c523920bcc09	5097	return Status;
johnAlexander	0:c523920bcc09	5098	}
johnAlexander	0:c523920bcc09	5099
johnAlexander	0:c523920bcc09	5100	VL53L0X_Error VL53L0X::WaitStopCompleted(VL53L0X_DEV Dev)
johnAlexander	0:c523920bcc09	5101	{
johnAlexander	0:c523920bcc09	5102	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
johnAlexander	0:c523920bcc09	5103	uint32_t StopCompleted=0;
johnAlexander	0:c523920bcc09	5104	uint32_t LoopNb;
johnAlexander	0:c523920bcc09	5105
johnAlexander	0:c523920bcc09	5106	// Wait until it finished
johnAlexander	0:c523920bcc09	5107	// use timeout to avoid deadlock
johnAlexander	0:c523920bcc09	5108	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5109	LoopNb = 0;
johnAlexander	0:c523920bcc09	5110	do {
johnAlexander	0:c523920bcc09	5111	Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
johnAlexander	0:c523920bcc09	5112	if ((StopCompleted == 0x00) \|\| Status != VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5113	break;
johnAlexander	0:c523920bcc09	5114	}
johnAlexander	0:c523920bcc09	5115	LoopNb = LoopNb + 1;
johnAlexander	0:c523920bcc09	5116	VL53L0X_PollingDelay(Dev);
johnAlexander	0:c523920bcc09	5117	} while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
johnAlexander	0:c523920bcc09	5118
johnAlexander	0:c523920bcc09	5119	if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
johnAlexander	0:c523920bcc09	5120	Status = VL53L0X_ERROR_TIME_OUT;
johnAlexander	0:c523920bcc09	5121	}
johnAlexander	0:c523920bcc09	5122
johnAlexander	0:c523920bcc09	5123	}
johnAlexander	0:c523920bcc09	5124
johnAlexander	0:c523920bcc09	5125	return Status;
johnAlexander	0:c523920bcc09	5126	}
johnAlexander	0:c523920bcc09	5127
johnAlexander	0:c523920bcc09	5128
johnAlexander	0:c523920bcc09	5129	int VL53L0X::InitSensor(uint8_t NewAddr)
johnAlexander	0:c523920bcc09	5130	{
johnAlexander	15:932d8b4e52c9	5131	int status;
johnAlexander	15:932d8b4e52c9	5132
johnAlexander	15:932d8b4e52c9	5133	VL53L0X_Off();
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	5139	status=IsPresent();
johnAlexander	15:932d8b4e52c9	5140	if(!status) {
johnAlexander	15:932d8b4e52c9	5141	status=Init(&MyDevice);
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	15:932d8b4e52c9	5148	status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
johnAlexander	15:932d8b4e52c9	5149
johnAlexander	15:932d8b4e52c9	5150
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	5157	if(NewAddr!=DEFAULT_DEVICE_ADDRESS) {
johnAlexander	15:932d8b4e52c9	5158	status=SetDeviceAddress(NewAddr);
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	12:f6e2bad00dc7	5171	int VL53L0X::RangeMeasIntContinuousMode(void (*fptr)(void))
johnAlexander	12:f6e2bad00dc7	5172	{
johnAlexander	15:932d8b4e52c9	5173	int status, ClrStatus;
johnAlexander	15:932d8b4e52c9	5174
johnAlexander	15:932d8b4e52c9	5175	status = VL53L0X_StopMeasurement(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	15:932d8b4e52c9	5179	status = VL53L0X_SetGpioConfig(Device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
johnAlexander	15:932d8b4e52c9	5180	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
johnAlexander	15:932d8b4e52c9	5181	VL53L0X_INTERRUPTPOLARITY_HIGH);
johnAlexander	15:932d8b4e52c9	5182
johnAlexander	15:932d8b4e52c9	5183	if (!status) {
johnAlexander	15:932d8b4e52c9	5184	AttachInterruptMeasureDetectionIRQ(fptr);
johnAlexander	15:932d8b4e52c9	5185	EnableInterruptMeasureDetectionIRQ();
johnAlexander	15:932d8b4e52c9	5186	}
johnAlexander	15:932d8b4e52c9	5187
johnAlexander	15:932d8b4e52c9	5188	ClrStatus=ClearInterrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS\|VL53L0X_REG_RESULT_RANGE_STATUS);
johnAlexander	15:932d8b4e52c9	5189	if(ClrStatus)
johnAlexander	15:932d8b4e52c9	5190	VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
johnAlexander	15:932d8b4e52c9	5191
johnAlexander	15:932d8b4e52c9	5192	if(!status) {
johnAlexander	15:932d8b4e52c9	5193	status=RangeStartContinuousMode();
johnAlexander	15:932d8b4e52c9	5194	}
johnAlexander	15:932d8b4e52c9	5195	return status;
johnAlexander	12:f6e2bad00dc7	5196	}
johnAlexander	0:c523920bcc09	5197
johnAlexander	0:c523920bcc09	5198
johnAlexander	0:c523920bcc09	5199	int VL53L0X::StartMeasurement(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	13:615f7e38568c	5220	Status = VL53L0X_StopMeasurement(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	13:615f7e38568c	5224	Status = VL53L0X_SetGpioConfig(Device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
johnAlexander	15:932d8b4e52c9	5225	VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
johnAlexander	15:932d8b4e52c9	5226	VL53L0X_INTERRUPTPOLARITY_HIGH);
johnAlexander	15:932d8b4e52c9	5227
johnAlexander	15:932d8b4e52c9	5228	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	15:932d8b4e52c9	5229	AttachInterruptMeasureDetectionIRQ(fptr);
johnAlexander	15:932d8b4e52c9	5230	EnableInterruptMeasureDetectionIRQ();
johnAlexander	12:f6e2bad00dc7	5231	}
johnAlexander	13:615f7e38568c	5232
johnAlexander	13:615f7e38568c	5233	ClrStatus=ClearInterrupt(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	13:615f7e38568c	5238	Status = VL53L0X_SetDeviceMode(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	15:932d8b4e52c9	5242	Status = VL53L0X_StartMeasurement(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	0:c523920bcc09	5250	Status = VL53L0X_SetDeviceMode(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	0:c523920bcc09	5255	Status = VL53L0X_SetLimitCheckEnable(Device,
johnAlexander	15:932d8b4e52c9	5256	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
johnAlexander	0:c523920bcc09	5257	}
johnAlexander	0:c523920bcc09	5258	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5259	Status = VL53L0X_SetLimitCheckEnable(Device,
johnAlexander	15:932d8b4e52c9	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	0:c523920bcc09	5294	Status = VL53L0X_SetLimitCheckValue(Device,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	5299	Status = VL53L0X_SetLimitCheckValue(Device,
johnAlexander	15:932d8b4e52c9	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	0:c523920bcc09	5304	Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Device, timingBudget);
johnAlexander	0:c523920bcc09	5305	}
johnAlexander	0:c523920bcc09	5306
johnAlexander	0:c523920bcc09	5307	if (Status == VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5308	Status = VL53L0X_SetVcselPulsePeriod(Device,
johnAlexander	15:932d8b4e52c9	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	0:c523920bcc09	5313	Status = VL53L0X_SetVcselPulsePeriod(Device,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	5318	Status = VL53L0X_PerformRefCalibration(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	0:c523920bcc09	5326	Status = VL53L0X_SetDeviceMode(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	15:932d8b4e52c9	5331	Status = VL53L0X_StartMeasurement(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	0:c523920bcc09	5339	int VL53L0X::GetMeasurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *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	0:c523920bcc09	5344	Status = VL53L0X_PerformSingleRangingMeasurement(Device, 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	15:932d8b4e52c9	5349	Status = VL53L0X_measurement_poll_for_completion(Device);
johnAlexander	15:932d8b4e52c9	5350
johnAlexander	15:932d8b4e52c9	5351	if(Status == VL53L0X_ERROR_NONE) {
johnAlexander	0:c523920bcc09	5352	Status = VL53L0X_GetRangingMeasurementData(Device, Data);
johnAlexander	0:c523920bcc09	5353
johnAlexander	15:932d8b4e52c9	5354	// Clear the interrupt
johnAlexander	0:c523920bcc09	5355	VL53L0X_ClearInterruptMask(Device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
johnAlexander	0:c523920bcc09	5356	VL53L0X_PollingDelay(Device);
johnAlexander	0:c523920bcc09	5357	}
johnAlexander	0:c523920bcc09	5358	}
johnAlexander	0:c523920bcc09	5359
johnAlexander	15:932d8b4e52c9	5360	if (operating_mode == range_continuous_interrupt) {
johnAlexander	12:f6e2bad00dc7	5361	Status = VL53L0X_GetRangingMeasurementData(Device, Data);
johnAlexander	15:932d8b4e52c9	5362	VL53L0X_ClearInterruptMask(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	0:c523920bcc09	5369	int VL53L0X::StopMeasurement(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	0:c523920bcc09	5382	status = VL53L0X_StopMeasurement(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	0:c523920bcc09	5387	status = WaitStopCompleted(Device);
johnAlexander	0:c523920bcc09	5388	}
johnAlexander	0:c523920bcc09	5389
johnAlexander	0:c523920bcc09	5390	if(status == VL53L0X_ERROR_NONE)
johnAlexander	0:c523920bcc09	5391	status = VL53L0X_ClearInterruptMask(Device,
johnAlexander	15:932d8b4e52c9	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	15:932d8b4e52c9	5402	status=GetMeasurement(operating_mode, Data);
johnAlexander	15:932d8b4e52c9	5403	EnableInterruptMeasureDetectionIRQ();
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	15:932d8b4e52c9	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@15:932d8b4e52c9, 2017-06-20 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning