X_NUCLEO_53L0A1_pullreq - Fixed STMPE1600 writeSYS_CTRL, enabled multi sens…

Users » mapellil » Code » X_NUCLEO_53L0A1_pullreq

Fixed STMPE1600 writeSYS_CTRL, enabled multi sensors support

Dependencies: ST_INTERFACES X_NUCLEO_COMMON

Components/VL53L0X/vl53l0x_class.cpp@2:58b5e9097aa3, 2016-12-01 (annotated)

Committer:: johnAlexander
Date:: Thu Dec 01 14:17:46 2016 +0000
Revision:: 2:58b5e9097aa3
Parent:: 0:c523920bcc09
Child:: 7:35ecf3e060c3

Use ST_INTERFACES package to access RangeSensor abstract class.

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	27 Mar 2017
Imports:	5
Forks:	0
Commits:	10
Dependents:	0
Dependencies:	2
Followers:	2

This repository is Public (Unlisted).

Components/VL53L0X/vl53l0x_class.cpp@2:58b5e9097aa3, 2016-12-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning