1 /*******************************************************************************
2 Copyright � 2016, STMicroelectronics International N.V.
3 All rights reserved.
4
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7 * Redistributions of source code must retain the above copyright
8 notice, this list of conditions and the following disclaimer.
9 * Redistributions in binary form must reproduce the above copyright
10 notice, this list of conditions and the following disclaimer in the
11 documentation and/or other materials provided with the distribution.
12 * Neither the name of STMicroelectronics nor the
13 names of its contributors may be used to endorse or promote products
14 derived from this software without specific prior written permission.
15
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
19 NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
20 IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
21 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 ******************************************************************************/
28
29 #include "vl53l0x_api.h"
30 #include "vl53l0x_api_core.h"
31 #include "vl53l0x_api_calibration.h"
32
33
34 #ifndef __KERNEL__
35 #include <stdlib.h>
36 #endif
37 #define LOG_FUNCTION_START(fmt, ...) \
38 _LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
39 #define LOG_FUNCTION_END(status, ...) \
40 _LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
41 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
42 _LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
43
VL53L0X_reverse_bytes(uint8_t * data,uint32_t size)44 VL53L0X_Error VL53L0X_reverse_bytes(uint8_t *data, uint32_t size)
45 {
46 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
47 uint8_t tempData;
48 uint32_t mirrorIndex;
49 uint32_t middle = size / 2;
50 uint32_t index;
51
52 for (index = 0; index < middle; index++) {
53 mirrorIndex = size - index - 1;
54 tempData = data[index];
55 data[index] = data[mirrorIndex];
56 data[mirrorIndex] = tempData;
57 }
58 return Status;
59 }
60
VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev)61 VL53L0X_Error VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev)
62 {
63 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
64 uint8_t NewDataReady = 0;
65 uint32_t LoopNb;
66
67 LOG_FUNCTION_START("");
68
69 LoopNb = 0;
70
71 do {
72 Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDataReady);
73 if (Status != 0)
74 break; /* the error is set */
75
76 if (NewDataReady == 1)
77 break; /* done note that status == 0 */
78
79 LoopNb++;
80 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
81 Status = VL53L0X_ERROR_TIME_OUT;
82 break;
83 }
84
85 VL53L0X_PollingDelay(Dev);
86 } while (1);
87
88 LOG_FUNCTION_END(Status);
89
90 return Status;
91 }
92
93
VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)94 uint8_t VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
95 {
96 /*!
97 * Converts the encoded VCSEL period register value into the real
98 * period in PLL clocks
99 */
100
101 uint8_t vcsel_period_pclks = 0;
102
103 vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
104
105 return vcsel_period_pclks;
106 }
107
VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks)108 uint8_t VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks)
109 {
110 /*!
111 * Converts the encoded VCSEL period register value into the real period
112 * in PLL clocks
113 */
114
115 uint8_t vcsel_period_reg = 0;
116
117 vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
118
119 return vcsel_period_reg;
120 }
121
122
VL53L0X_isqrt(uint32_t num)123 uint32_t VL53L0X_isqrt(uint32_t num)
124 {
125 /*
126 * Implements an integer square root
127 *
128 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
129 */
130
131 uint32_t res = 0;
132 uint32_t bit = 1 << 30;
133 /* The second-to-top bit is set:
134 * 1 << 14 for 16-bits, 1 << 30 for 32 bits */
135
136 /* "bit" starts at the highest power of four <= the argument. */
137 while (bit > num)
138 bit >>= 2;
139
140
141 while (bit != 0) {
142 if (num >= res + bit) {
143 num -= res + bit;
144 res = (res >> 1) + bit;
145 } else
146 res >>= 1;
147
148 bit >>= 2;
149 }
150
151 return res;
152 }
153
154
VL53L0X_quadrature_sum(uint32_t a,uint32_t b)155 uint32_t VL53L0X_quadrature_sum(uint32_t a, uint32_t b)
156 {
157 /*
158 * Implements a quadrature sum
159 *
160 * rea = sqrt(a^2 + b^2)
161 *
162 * Trap overflow case max input value is 65535 (16-bit value)
163 * as internal calc are 32-bit wide
164 *
165 * If overflow then seta output to maximum
166 */
167 uint32_t res = 0;
168
169 if (a > 65535 || b > 65535)
170 res = 65535;
171 else
172 res = VL53L0X_isqrt(a * a + b * b);
173
174 return res;
175 }
176
177
VL53L0X_device_read_strobe(VL53L0X_DEV Dev)178 VL53L0X_Error VL53L0X_device_read_strobe(VL53L0X_DEV Dev)
179 {
180 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
181 uint8_t strobe;
182 uint32_t LoopNb;
183 LOG_FUNCTION_START("");
184
185 Status |= VL53L0X_WrByte(Dev, 0x83, 0x00);
186
187 /* polling
188 * use timeout to avoid deadlock*/
189 if (Status == VL53L0X_ERROR_NONE) {
190 LoopNb = 0;
191 do {
192 Status = VL53L0X_RdByte(Dev, 0x83, &strobe);
193 if ((strobe != 0x00) || Status != VL53L0X_ERROR_NONE)
194 break;
195
196 LoopNb = LoopNb + 1;
197 } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
198
199 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
200 Status = VL53L0X_ERROR_TIME_OUT;
201 }
202
203 Status |= VL53L0X_WrByte(Dev, 0x83, 0x01);
204
205 LOG_FUNCTION_END(Status);
206 return Status;
207 }
208
VL53L0X_get_info_from_device(VL53L0X_DEV Dev,uint8_t option)209 VL53L0X_Error VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option)
210 {
211
212 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
213 uint8_t byte;
214 uint32_t TmpDWord;
215 uint8_t ModuleId;
216 uint8_t Revision;
217 uint8_t ReferenceSpadCount = 0;
218 uint8_t ReferenceSpadType = 0;
219 uint32_t PartUIDUpper = 0;
220 uint32_t PartUIDLower = 0;
221 uint32_t OffsetFixed1104_mm = 0;
222 int16_t OffsetMicroMeters = 0;
223 uint32_t DistMeasTgtFixed1104_mm = 400 << 4;
224 uint32_t DistMeasFixed1104_400_mm = 0;
225 uint32_t SignalRateMeasFixed1104_400_mm = 0;
226 char ProductId[19];
227 char * ProductId_tmp;
228 uint8_t ReadDataFromDeviceDone;
229 FixPoint1616_t SignalRateMeasFixed400mmFix = 0;
230 uint8_t NvmRefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
231 int i;
232
233
234 LOG_FUNCTION_START("");
235
236 ReadDataFromDeviceDone =
237 VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone);
238
239 /* This access is done only once after that a GetDeviceInfo or
240 * datainit is done*/
241 if (ReadDataFromDeviceDone != 7) {
242
243 Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
244 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
245 Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
246
247 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
248 Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
249 Status |= VL53L0X_WrByte(Dev, 0x83, byte | 4);
250 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x07);
251 Status |= VL53L0X_WrByte(Dev, 0x81, 0x01);
252
253 Status |= VL53L0X_PollingDelay(Dev);
254
255 Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
256
257 if (((option & 1) == 1) && ((ReadDataFromDeviceDone & 1) == 0)) {
258 Status |= VL53L0X_WrByte(Dev, 0x94, 0x6b);
259 Status |= VL53L0X_device_read_strobe(Dev);
260 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
261
262 ReferenceSpadCount = (uint8_t)((TmpDWord >> 8) & 0x07f);
263 ReferenceSpadType = (uint8_t)((TmpDWord >> 15) & 0x01);
264
265 Status |= VL53L0X_WrByte(Dev, 0x94, 0x24);
266 Status |= VL53L0X_device_read_strobe(Dev);
267 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
268
269
270 NvmRefGoodSpadMap[0] = (uint8_t)((TmpDWord >> 24) & 0xff);
271 NvmRefGoodSpadMap[1] = (uint8_t)((TmpDWord >> 16) & 0xff);
272 NvmRefGoodSpadMap[2] = (uint8_t)((TmpDWord >> 8) & 0xff);
273 NvmRefGoodSpadMap[3] = (uint8_t)(TmpDWord & 0xff);
274
275 Status |= VL53L0X_WrByte(Dev, 0x94, 0x25);
276 Status |= VL53L0X_device_read_strobe(Dev);
277 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
278
279 NvmRefGoodSpadMap[4] = (uint8_t)((TmpDWord >> 24) & 0xff);
280 NvmRefGoodSpadMap[5] = (uint8_t)((TmpDWord >> 16) & 0xff);
281 }
282
283 if (((option & 2) == 2) && ((ReadDataFromDeviceDone & 2) == 0)) {
284
285 Status |= VL53L0X_WrByte(Dev, 0x94, 0x02);
286 Status |= VL53L0X_device_read_strobe(Dev);
287 Status |= VL53L0X_RdByte(Dev, 0x90, &ModuleId);
288
289 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
290 Status |= VL53L0X_device_read_strobe(Dev);
291 Status |= VL53L0X_RdByte(Dev, 0x90, &Revision);
292
293 Status |= VL53L0X_WrByte(Dev, 0x94, 0x77);
294 Status |= VL53L0X_device_read_strobe(Dev);
295 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
296
297 ProductId[0] = (char)((TmpDWord >> 25) & 0x07f);
298 ProductId[1] = (char)((TmpDWord >> 18) & 0x07f);
299 ProductId[2] = (char)((TmpDWord >> 11) & 0x07f);
300 ProductId[3] = (char)((TmpDWord >> 4) & 0x07f);
301
302 byte = (uint8_t)((TmpDWord & 0x00f) << 3);
303
304 Status |= VL53L0X_WrByte(Dev, 0x94, 0x78);
305 Status |= VL53L0X_device_read_strobe(Dev);
306 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
307
308 ProductId[4] = (char)(byte + ((TmpDWord >> 29) & 0x07f));
309 ProductId[5] = (char)((TmpDWord >> 22) & 0x07f);
310 ProductId[6] = (char)((TmpDWord >> 15) & 0x07f);
311 ProductId[7] = (char)((TmpDWord >> 8) & 0x07f);
312 ProductId[8] = (char)((TmpDWord >> 1) & 0x07f);
313
314 byte = (uint8_t)((TmpDWord & 0x001) << 6);
315
316 Status |= VL53L0X_WrByte(Dev, 0x94, 0x79);
317
318 Status |= VL53L0X_device_read_strobe(Dev);
319
320 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
321
322 ProductId[9] = (char)(byte + ((TmpDWord >> 26) & 0x07f));
323 ProductId[10] = (char)((TmpDWord >> 19) & 0x07f);
324 ProductId[11] = (char)((TmpDWord >> 12) & 0x07f);
325 ProductId[12] = (char)((TmpDWord >> 5) & 0x07f);
326
327 byte = (uint8_t)((TmpDWord & 0x01f) << 2);
328
329 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7A);
330
331 Status |= VL53L0X_device_read_strobe(Dev);
332
333 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
334
335 ProductId[13] = (char)(byte + ((TmpDWord >> 30) & 0x07f));
336 ProductId[14] = (char)((TmpDWord >> 23) & 0x07f);
337 ProductId[15] = (char)((TmpDWord >> 16) & 0x07f);
338 ProductId[16] = (char)((TmpDWord >> 9) & 0x07f);
339 ProductId[17] = (char)((TmpDWord >> 2) & 0x07f);
340 ProductId[18] = '\0';
341 }
342
343 if (((option & 4) == 4) && ((ReadDataFromDeviceDone & 4) == 0)) {
344
345 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
346 Status |= VL53L0X_device_read_strobe(Dev);
347 Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDUpper);
348
349 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7C);
350 Status |= VL53L0X_device_read_strobe(Dev);
351 Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDLower);
352
353 Status |= VL53L0X_WrByte(Dev, 0x94, 0x73);
354 Status |= VL53L0X_device_read_strobe(Dev);
355 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
356
357 SignalRateMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff) << 8;
358
359 Status |= VL53L0X_WrByte(Dev, 0x94, 0x74);
360 Status |= VL53L0X_device_read_strobe(Dev);
361 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
362
363 SignalRateMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000) >> 24);
364
365 Status |= VL53L0X_WrByte(Dev, 0x94, 0x75);
366 Status |= VL53L0X_device_read_strobe(Dev);
367 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
368
369 DistMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff) << 8;
370
371 Status |= VL53L0X_WrByte(Dev, 0x94, 0x76);
372 Status |= VL53L0X_device_read_strobe(Dev);
373 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
374
375 DistMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000) >> 24);
376 }
377
378 Status |= VL53L0X_WrByte(Dev, 0x81, 0x00);
379 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
380 Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
381 Status |= VL53L0X_WrByte(Dev, 0x83, byte & 0xfb);
382 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
383 Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
384
385 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
386 Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
387 }
388
389 if ((Status == VL53L0X_ERROR_NONE) && (ReadDataFromDeviceDone != 7)) {
390 /* Assign to variable if status is ok */
391 if (((option & 1) == 1) && ((ReadDataFromDeviceDone & 1) == 0)) {
392 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReferenceSpadCount,
393 ReferenceSpadCount);
394
395 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReferenceSpadType,
396 ReferenceSpadType);
397
398 for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
399 Dev->Data.SpadData.RefGoodSpadMap[i] = NvmRefGoodSpadMap[i];
400 }
401 }
402
403 if (((option & 2) == 2) && ((ReadDataFromDeviceDone & 2) == 0)) {
404 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ModuleId, ModuleId);
405
406 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, Revision, Revision);
407
408 ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ProductId);
409 VL53L0X_COPYSTRING(ProductId_tmp, ProductId);
410 }
411
412 if (((option & 4) == 4) && ((ReadDataFromDeviceDone & 4) == 0)) {
413 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, PartUIDUpper, PartUIDUpper);
414
415 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, PartUIDLower, PartUIDLower);
416
417 SignalRateMeasFixed400mmFix =
418 VL53L0X_FIXPOINT97TOFIXPOINT1616(SignalRateMeasFixed1104_400_mm);
419
420 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, SignalRateMeasFixed400mm,
421 SignalRateMeasFixed400mmFix);
422
423 OffsetMicroMeters = 0;
424 if (DistMeasFixed1104_400_mm != 0) {
425 OffsetFixed1104_mm =
426 DistMeasFixed1104_400_mm - DistMeasTgtFixed1104_mm;
427 OffsetMicroMeters = (OffsetFixed1104_mm * 1000) >> 4;
428 OffsetMicroMeters *= -1;
429 }
430
431 PALDevDataSet(Dev, Part2PartOffsetAdjustmentNVMMicroMeter,
432 OffsetMicroMeters);
433 }
434 byte = (uint8_t)(ReadDataFromDeviceDone | option);
435 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone, byte);
436 }
437
438 LOG_FUNCTION_END(Status);
439 return Status;
440 }
441
442
VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev,uint8_t vcsel_period_pclks)443 uint32_t VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev,
444 uint8_t vcsel_period_pclks)
445 {
446 uint64_t PLL_period_ps;
447 uint32_t macro_period_vclks;
448 uint32_t macro_period_ps;
449
450 LOG_FUNCTION_START("");
451
452 /* The above calculation will produce rounding errors,
453 therefore set fixed value
454 */
455 PLL_period_ps = 1655;
456
457 macro_period_vclks = 2304;
458 macro_period_ps =
459 (uint32_t)(macro_period_vclks * vcsel_period_pclks * PLL_period_ps);
460
461 LOG_FUNCTION_END("");
462 return macro_period_ps;
463 }
464
VL53L0X_encode_timeout(uint32_t timeout_macro_clks)465 uint16_t VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
466 {
467 /*!
468 * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
469 */
470
471 uint16_t encoded_timeout = 0;
472 uint32_t ls_byte = 0;
473 uint16_t ms_byte = 0;
474
475 if (timeout_macro_clks > 0) {
476 ls_byte = timeout_macro_clks - 1;
477
478 while ((ls_byte & 0xFFFFFF00) > 0) {
479 ls_byte = ls_byte >> 1;
480 ms_byte++;
481 }
482
483 encoded_timeout = (ms_byte << 8) + (uint16_t)(ls_byte & 0x000000FF);
484 }
485
486 return encoded_timeout;
487 }
488
VL53L0X_decode_timeout(uint16_t encoded_timeout)489 uint32_t VL53L0X_decode_timeout(uint16_t encoded_timeout)
490 {
491 /*!
492 * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
493 */
494
495 uint32_t timeout_macro_clks = 0;
496
497 timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF)
498 << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) +
499 1;
500
501 return timeout_macro_clks;
502 }
503
504
505 /* To convert ms into register value */
VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,uint32_t timeout_period_us,uint8_t vcsel_period_pclks)506 uint32_t VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev, uint32_t timeout_period_us,
507 uint8_t vcsel_period_pclks)
508 {
509 uint32_t macro_period_ps;
510 uint32_t macro_period_ns;
511 uint32_t timeout_period_mclks = 0;
512
513 macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
514 macro_period_ns = (macro_period_ps + 500) / 1000;
515
516 timeout_period_mclks = (uint32_t)(
517 ((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
518
519 return timeout_period_mclks;
520 }
521
522 /* To convert register value into us */
VL53L0X_calc_timeout_us(VL53L0X_DEV Dev,uint16_t timeout_period_mclks,uint8_t vcsel_period_pclks)523 uint32_t VL53L0X_calc_timeout_us(VL53L0X_DEV Dev, uint16_t timeout_period_mclks,
524 uint8_t vcsel_period_pclks)
525 {
526 uint32_t macro_period_ps;
527 uint32_t macro_period_ns;
528 uint32_t actual_timeout_period_us = 0;
529
530 macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
531 macro_period_ns = (macro_period_ps + 500) / 1000;
532
533 actual_timeout_period_us =
534 ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
535
536 return actual_timeout_period_us;
537 }
538
539
get_sequence_step_timeout(VL53L0X_DEV Dev,VL53L0X_SequenceStepId SequenceStepId,uint32_t * pTimeOutMicroSecs)540 VL53L0X_Error get_sequence_step_timeout(VL53L0X_DEV Dev,
541 VL53L0X_SequenceStepId SequenceStepId,
542 uint32_t *pTimeOutMicroSecs)
543 {
544 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
545 uint8_t CurrentVCSELPulsePeriodPClk;
546 uint8_t EncodedTimeOutByte = 0;
547 uint32_t TimeoutMicroSeconds = 0;
548 uint16_t PreRangeEncodedTimeOut = 0;
549 uint16_t MsrcTimeOutMClks;
550 uint16_t PreRangeTimeOutMClks;
551 uint16_t FinalRangeTimeOutMClks = 0;
552 uint16_t FinalRangeEncodedTimeOut;
553 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
554
555 if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) ||
556 (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) ||
557 (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
558
559 Status = VL53L0X_GetVcselPulsePeriod(
560 Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk);
561 if (Status == VL53L0X_ERROR_NONE) {
562 Status = VL53L0X_RdByte(Dev, VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
563 &EncodedTimeOutByte);
564 }
565 MsrcTimeOutMClks = VL53L0X_decode_timeout(EncodedTimeOutByte);
566
567 TimeoutMicroSeconds = VL53L0X_calc_timeout_us(
568 Dev, MsrcTimeOutMClks, CurrentVCSELPulsePeriodPClk);
569 } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
570 /* Retrieve PRE-RANGE VCSEL Period */
571 Status = VL53L0X_GetVcselPulsePeriod(
572 Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk);
573
574 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
575 if (Status == VL53L0X_ERROR_NONE) {
576
577 /* Retrieve PRE-RANGE VCSEL Period */
578 Status =
579 VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE,
580 &CurrentVCSELPulsePeriodPClk);
581
582 if (Status == VL53L0X_ERROR_NONE) {
583 Status = VL53L0X_RdWord(
584 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
585 &PreRangeEncodedTimeOut);
586 }
587
588 PreRangeTimeOutMClks =
589 VL53L0X_decode_timeout(PreRangeEncodedTimeOut);
590
591 TimeoutMicroSeconds = VL53L0X_calc_timeout_us(
592 Dev, PreRangeTimeOutMClks, CurrentVCSELPulsePeriodPClk);
593 }
594 } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
595
596 VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
597 PreRangeTimeOutMClks = 0;
598
599 if (SchedulerSequenceSteps.PreRangeOn) {
600 /* Retrieve PRE-RANGE VCSEL Period */
601 Status =
602 VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE,
603 &CurrentVCSELPulsePeriodPClk);
604
605 /* Retrieve PRE-RANGE Timeout in Macro periods
606 * (MCLKS) */
607 if (Status == VL53L0X_ERROR_NONE) {
608 Status = VL53L0X_RdWord(
609 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
610 &PreRangeEncodedTimeOut);
611 PreRangeTimeOutMClks =
612 VL53L0X_decode_timeout(PreRangeEncodedTimeOut);
613 }
614 }
615
616 if (Status == VL53L0X_ERROR_NONE) {
617 /* Retrieve FINAL-RANGE VCSEL Period */
618 Status =
619 VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
620 &CurrentVCSELPulsePeriodPClk);
621 }
622
623 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
624 if (Status == VL53L0X_ERROR_NONE) {
625 Status = VL53L0X_RdWord(
626 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
627 &FinalRangeEncodedTimeOut);
628 FinalRangeTimeOutMClks =
629 VL53L0X_decode_timeout(FinalRangeEncodedTimeOut);
630 }
631
632 FinalRangeTimeOutMClks -= PreRangeTimeOutMClks;
633 TimeoutMicroSeconds = VL53L0X_calc_timeout_us(
634 Dev, FinalRangeTimeOutMClks, CurrentVCSELPulsePeriodPClk);
635 }
636
637 *pTimeOutMicroSecs = TimeoutMicroSeconds;
638
639 return Status;
640 }
641
642
set_sequence_step_timeout(VL53L0X_DEV Dev,VL53L0X_SequenceStepId SequenceStepId,uint32_t TimeOutMicroSecs)643 VL53L0X_Error set_sequence_step_timeout(VL53L0X_DEV Dev,
644 VL53L0X_SequenceStepId SequenceStepId,
645 uint32_t TimeOutMicroSecs)
646 {
647 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
648 uint8_t CurrentVCSELPulsePeriodPClk;
649 uint8_t MsrcEncodedTimeOut;
650 uint16_t PreRangeEncodedTimeOut;
651 uint16_t PreRangeTimeOutMClks;
652 uint16_t MsrcRangeTimeOutMClks;
653 uint16_t FinalRangeTimeOutMClks;
654 uint16_t FinalRangeEncodedTimeOut;
655 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
656
657 if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) ||
658 (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) ||
659 (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
660
661 Status = VL53L0X_GetVcselPulsePeriod(
662 Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk);
663
664 if (Status == VL53L0X_ERROR_NONE) {
665 MsrcRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(
666 Dev, TimeOutMicroSecs, (uint8_t)CurrentVCSELPulsePeriodPClk);
667
668 if (MsrcRangeTimeOutMClks > 256)
669 MsrcEncodedTimeOut = 255;
670 else
671 MsrcEncodedTimeOut = (uint8_t)MsrcRangeTimeOutMClks - 1;
672
673 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, LastEncodedTimeout,
674 MsrcEncodedTimeOut);
675 }
676
677 if (Status == VL53L0X_ERROR_NONE) {
678 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
679 MsrcEncodedTimeOut);
680 }
681 } else {
682
683 if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
684
685 if (Status == VL53L0X_ERROR_NONE) {
686 Status = VL53L0X_GetVcselPulsePeriod(
687 Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE,
688 &CurrentVCSELPulsePeriodPClk);
689 PreRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(
690 Dev, TimeOutMicroSecs, (uint8_t)CurrentVCSELPulsePeriodPClk);
691 PreRangeEncodedTimeOut =
692 VL53L0X_encode_timeout(PreRangeTimeOutMClks);
693
694 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, LastEncodedTimeout,
695 PreRangeEncodedTimeOut);
696 }
697
698 if (Status == VL53L0X_ERROR_NONE) {
699 Status = VL53L0X_WrWord(
700 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
701 PreRangeEncodedTimeOut);
702 }
703
704 if (Status == VL53L0X_ERROR_NONE) {
705 VL53L0X_SETDEVICESPECIFICPARAMETER(
706 Dev, PreRangeTimeoutMicroSecs, TimeOutMicroSecs);
707 }
708 } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
709
710 /* For the final range timeout, the pre-range timeout
711 * must be added. To do this both final and pre-range
712 * timeouts must be expressed in macro periods MClks
713 * because they have different vcsel periods.
714 */
715
716 VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
717 PreRangeTimeOutMClks = 0;
718 if (SchedulerSequenceSteps.PreRangeOn) {
719
720 /* Retrieve PRE-RANGE VCSEL Period */
721 Status = VL53L0X_GetVcselPulsePeriod(
722 Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE,
723 &CurrentVCSELPulsePeriodPClk);
724
725 /* Retrieve PRE-RANGE Timeout in Macro periods
726 * (MCLKS) */
727 if (Status == VL53L0X_ERROR_NONE) {
728 Status = VL53L0X_RdWord(Dev, 0x51, &PreRangeEncodedTimeOut);
729 PreRangeTimeOutMClks =
730 VL53L0X_decode_timeout(PreRangeEncodedTimeOut);
731 }
732 }
733
734 /* Calculate FINAL RANGE Timeout in Macro Periods
735 * (MCLKS) and add PRE-RANGE value
736 */
737 if (Status == VL53L0X_ERROR_NONE) {
738
739 Status = VL53L0X_GetVcselPulsePeriod(
740 Dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
741 &CurrentVCSELPulsePeriodPClk);
742 }
743 if (Status == VL53L0X_ERROR_NONE) {
744
745 FinalRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(
746 Dev, TimeOutMicroSecs, (uint8_t)CurrentVCSELPulsePeriodPClk);
747
748 FinalRangeTimeOutMClks += PreRangeTimeOutMClks;
749
750 FinalRangeEncodedTimeOut =
751 VL53L0X_encode_timeout(FinalRangeTimeOutMClks);
752
753 if (Status == VL53L0X_ERROR_NONE) {
754 Status =
755 VL53L0X_WrWord(Dev, 0x71, FinalRangeEncodedTimeOut);
756 }
757
758 if (Status == VL53L0X_ERROR_NONE) {
759 VL53L0X_SETDEVICESPECIFICPARAMETER(
760 Dev, FinalRangeTimeoutMicroSecs, TimeOutMicroSecs);
761 }
762 }
763 } else
764 Status = VL53L0X_ERROR_INVALID_PARAMS;
765 }
766 return Status;
767 }
768
VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,VL53L0X_VcselPeriod VcselPeriodType,uint8_t VCSELPulsePeriodPCLK)769 VL53L0X_Error VL53L0X_set_vcsel_pulse_period(
770 VL53L0X_DEV Dev, VL53L0X_VcselPeriod VcselPeriodType,
771 uint8_t VCSELPulsePeriodPCLK)
772 {
773 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
774 uint8_t vcsel_period_reg;
775 uint8_t MinPreVcselPeriodPCLK = 12;
776 uint8_t MaxPreVcselPeriodPCLK = 18;
777 uint8_t MinFinalVcselPeriodPCLK = 8;
778 uint8_t MaxFinalVcselPeriodPCLK = 14;
779 uint32_t MeasurementTimingBudgetMicroSeconds;
780 uint32_t FinalRangeTimeoutMicroSeconds;
781 uint32_t PreRangeTimeoutMicroSeconds;
782 uint32_t MsrcTimeoutMicroSeconds;
783 uint8_t PhaseCalInt = 0;
784
785 /* Check if valid clock period requested */
786
787 if ((VCSELPulsePeriodPCLK % 2) != 0) {
788 /* Value must be an even number */
789 Status = VL53L0X_ERROR_INVALID_PARAMS;
790 } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
791 (VCSELPulsePeriodPCLK < MinPreVcselPeriodPCLK ||
792 VCSELPulsePeriodPCLK > MaxPreVcselPeriodPCLK)) {
793 Status = VL53L0X_ERROR_INVALID_PARAMS;
794 } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
795 (VCSELPulsePeriodPCLK < MinFinalVcselPeriodPCLK ||
796 VCSELPulsePeriodPCLK > MaxFinalVcselPeriodPCLK)) {
797
798 Status = VL53L0X_ERROR_INVALID_PARAMS;
799 }
800
801 /* Apply specific settings for the requested clock period */
802
803 if (Status != VL53L0X_ERROR_NONE)
804 return Status;
805
806
807 if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
808
809 /* Set phase check limits */
810 if (VCSELPulsePeriodPCLK == 12) {
811
812 Status = VL53L0X_WrByte(
813 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
814 Status = VL53L0X_WrByte(
815 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
816 } else if (VCSELPulsePeriodPCLK == 14) {
817
818 Status = VL53L0X_WrByte(
819 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
820 Status = VL53L0X_WrByte(
821 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
822 } else if (VCSELPulsePeriodPCLK == 16) {
823
824 Status = VL53L0X_WrByte(
825 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
826 Status = VL53L0X_WrByte(
827 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
828 } else if (VCSELPulsePeriodPCLK == 18) {
829
830 Status = VL53L0X_WrByte(
831 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
832 Status = VL53L0X_WrByte(
833 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
834 }
835 } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
836
837 if (VCSELPulsePeriodPCLK == 8) {
838
839 Status = VL53L0X_WrByte(
840 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
841 Status = VL53L0X_WrByte(
842 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
843
844 Status |=
845 VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
846 Status |= VL53L0X_WrByte(
847 Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
848
849 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
850 Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x30);
851 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
852 } else if (VCSELPulsePeriodPCLK == 10) {
853
854 Status = VL53L0X_WrByte(
855 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
856 Status = VL53L0X_WrByte(
857 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
858
859 Status |=
860 VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
861 Status |= VL53L0X_WrByte(
862 Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
863
864 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
865 Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
866 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
867 } else if (VCSELPulsePeriodPCLK == 12) {
868
869 Status = VL53L0X_WrByte(
870 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
871 Status = VL53L0X_WrByte(
872 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
873
874 Status |=
875 VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
876 Status |= VL53L0X_WrByte(
877 Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
878
879 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
880 Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
881 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
882 } else if (VCSELPulsePeriodPCLK == 14) {
883
884 Status = VL53L0X_WrByte(
885 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x048);
886 Status = VL53L0X_WrByte(
887 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
888
889 Status |=
890 VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
891 Status |= VL53L0X_WrByte(
892 Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
893
894 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
895 Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
896 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
897 }
898 }
899
900
901 /* Re-calculate and apply timeouts, in macro periods */
902
903 if (Status == VL53L0X_ERROR_NONE) {
904 vcsel_period_reg =
905 VL53L0X_encode_vcsel_period((uint8_t)VCSELPulsePeriodPCLK);
906
907 /* When the VCSEL period for the pre or final range is changed,
908 * the corresponding timeout must be read from the device using
909 * the current VCSEL period, then the new VCSEL period can be
910 * applied. The timeout then must be written back to the device
911 * using the new VCSEL period.
912 *
913 * For the MSRC timeout, the same applies - this timeout being
914 * dependant on the pre-range vcsel period.
915 */
916 switch (VcselPeriodType) {
917 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
918 Status =
919 get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE,
920 &PreRangeTimeoutMicroSeconds);
921
922 if (Status == VL53L0X_ERROR_NONE)
923 Status = get_sequence_step_timeout(
924 Dev, VL53L0X_SEQUENCESTEP_MSRC, &MsrcTimeoutMicroSeconds);
925
926 if (Status == VL53L0X_ERROR_NONE)
927 Status = VL53L0X_WrByte(
928 Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
929 vcsel_period_reg);
930
931
932 if (Status == VL53L0X_ERROR_NONE)
933 Status = set_sequence_step_timeout(
934 Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE,
935 PreRangeTimeoutMicroSeconds);
936
937
938 if (Status == VL53L0X_ERROR_NONE)
939 Status = set_sequence_step_timeout(
940 Dev, VL53L0X_SEQUENCESTEP_MSRC, MsrcTimeoutMicroSeconds);
941
942 VL53L0X_SETDEVICESPECIFICPARAMETER(
943 Dev, PreRangeVcselPulsePeriod, VCSELPulsePeriodPCLK);
944 break;
945 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
946 Status = get_sequence_step_timeout(
947 Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE,
948 &FinalRangeTimeoutMicroSeconds);
949
950 if (Status == VL53L0X_ERROR_NONE)
951 Status = VL53L0X_WrByte(
952 Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
953 vcsel_period_reg);
954
955
956 if (Status == VL53L0X_ERROR_NONE)
957 Status = set_sequence_step_timeout(
958 Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE,
959 FinalRangeTimeoutMicroSeconds);
960
961 VL53L0X_SETDEVICESPECIFICPARAMETER(
962 Dev, FinalRangeVcselPulsePeriod, VCSELPulsePeriodPCLK);
963 break;
964 default:
965 Status = VL53L0X_ERROR_INVALID_PARAMS;
966 }
967 }
968
969 /* Finally, the timing budget must be re-applied */
970 if (Status == VL53L0X_ERROR_NONE) {
971 VL53L0X_GETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds,
972 MeasurementTimingBudgetMicroSeconds);
973
974 Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(
975 Dev, MeasurementTimingBudgetMicroSeconds);
976 }
977
978 /* Perform the phase calibration. This is needed after changing on
979 * vcsel period.
980 * get_data_enable = 0, restore_config = 1 */
981 if (Status == VL53L0X_ERROR_NONE)
982 Status = VL53L0X_perform_phase_calibration(Dev, &PhaseCalInt, 0, 1);
983
984 return Status;
985 }
986
VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,VL53L0X_VcselPeriod VcselPeriodType,uint8_t * pVCSELPulsePeriodPCLK)987 VL53L0X_Error VL53L0X_get_vcsel_pulse_period(
988 VL53L0X_DEV Dev, VL53L0X_VcselPeriod VcselPeriodType,
989 uint8_t *pVCSELPulsePeriodPCLK)
990 {
991 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
992 uint8_t vcsel_period_reg;
993
994 switch (VcselPeriodType) {
995 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
996 Status =
997 VL53L0X_RdByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
998 &vcsel_period_reg);
999 break;
1000 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
1001 Status =
1002 VL53L0X_RdByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
1003 &vcsel_period_reg);
1004 break;
1005 default:
1006 Status = VL53L0X_ERROR_INVALID_PARAMS;
1007 }
1008
1009 if (Status == VL53L0X_ERROR_NONE)
1010 *pVCSELPulsePeriodPCLK = VL53L0X_decode_vcsel_period(vcsel_period_reg);
1011
1012 return Status;
1013 }
1014
1015
VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,uint32_t MeasurementTimingBudgetMicroSeconds)1016 VL53L0X_Error VL53L0X_set_measurement_timing_budget_micro_seconds(
1017 VL53L0X_DEV Dev, uint32_t MeasurementTimingBudgetMicroSeconds)
1018 {
1019 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1020 uint32_t FinalRangeTimingBudgetMicroSeconds;
1021 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
1022 uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
1023 uint32_t StartOverheadMicroSeconds = 1320;
1024 uint32_t EndOverheadMicroSeconds = 960;
1025 uint32_t MsrcOverheadMicroSeconds = 660;
1026 uint32_t TccOverheadMicroSeconds = 590;
1027 uint32_t DssOverheadMicroSeconds = 690;
1028 uint32_t PreRangeOverheadMicroSeconds = 660;
1029 uint32_t FinalRangeOverheadMicroSeconds = 550;
1030 uint32_t PreRangeTimeoutMicroSeconds = 0;
1031 uint32_t cMinTimingBudgetMicroSeconds = 20000;
1032 uint32_t SubTimeout = 0;
1033
1034 LOG_FUNCTION_START("");
1035
1036 if (MeasurementTimingBudgetMicroSeconds < cMinTimingBudgetMicroSeconds) {
1037 Status = VL53L0X_ERROR_INVALID_PARAMS;
1038 return Status;
1039 }
1040
1041 FinalRangeTimingBudgetMicroSeconds =
1042 MeasurementTimingBudgetMicroSeconds -
1043 (StartOverheadMicroSeconds + EndOverheadMicroSeconds);
1044
1045 Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
1046
1047 if (Status == VL53L0X_ERROR_NONE &&
1048 (SchedulerSequenceSteps.TccOn || SchedulerSequenceSteps.MsrcOn ||
1049 SchedulerSequenceSteps.DssOn)) {
1050
1051 /* TCC, MSRC and DSS all share the same timeout */
1052 Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_MSRC,
1053 &MsrcDccTccTimeoutMicroSeconds);
1054
1055 /* Subtract the TCC, MSRC and DSS timeouts if they are
1056 * enabled. */
1057
1058 if (Status != VL53L0X_ERROR_NONE)
1059 return Status;
1060
1061 /* TCC */
1062 if (SchedulerSequenceSteps.TccOn) {
1063
1064 SubTimeout =
1065 MsrcDccTccTimeoutMicroSeconds + TccOverheadMicroSeconds;
1066
1067 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
1068 FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
1069 } else {
1070 /* Requested timeout too big. */
1071 Status = VL53L0X_ERROR_INVALID_PARAMS;
1072 }
1073 }
1074
1075 if (Status != VL53L0X_ERROR_NONE) {
1076 LOG_FUNCTION_END(Status);
1077 return Status;
1078 }
1079
1080 /* DSS */
1081 if (SchedulerSequenceSteps.DssOn) {
1082
1083 SubTimeout =
1084 2 * (MsrcDccTccTimeoutMicroSeconds + DssOverheadMicroSeconds);
1085
1086 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
1087 FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
1088 } else {
1089 /* Requested timeout too big. */
1090 Status = VL53L0X_ERROR_INVALID_PARAMS;
1091 }
1092 } else if (SchedulerSequenceSteps.MsrcOn) {
1093 /* MSRC */
1094 SubTimeout =
1095 MsrcDccTccTimeoutMicroSeconds + MsrcOverheadMicroSeconds;
1096
1097 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
1098 FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
1099 } else {
1100 /* Requested timeout too big. */
1101 Status = VL53L0X_ERROR_INVALID_PARAMS;
1102 }
1103 }
1104 }
1105
1106 if (Status != VL53L0X_ERROR_NONE) {
1107 LOG_FUNCTION_END(Status);
1108 return Status;
1109 }
1110
1111 if (SchedulerSequenceSteps.PreRangeOn) {
1112
1113 /* Subtract the Pre-range timeout if enabled. */
1114
1115 Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE,
1116 &PreRangeTimeoutMicroSeconds);
1117
1118 SubTimeout = PreRangeTimeoutMicroSeconds + PreRangeOverheadMicroSeconds;
1119
1120 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
1121 FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
1122 } else {
1123 /* Requested timeout too big. */
1124 Status = VL53L0X_ERROR_INVALID_PARAMS;
1125 }
1126 }
1127
1128
1129 if (Status == VL53L0X_ERROR_NONE && SchedulerSequenceSteps.FinalRangeOn) {
1130
1131 FinalRangeTimingBudgetMicroSeconds -= FinalRangeOverheadMicroSeconds;
1132
1133 /* Final Range Timeout
1134 * Note that the final range timeout is determined by the timing
1135 * budget and the sum of all other timeouts within the sequence.
1136 * If there is no room for the final range timeout, then an error
1137 * will be set. Otherwise the remaining time will be applied to
1138 * the final range.
1139 */
1140 Status =
1141 set_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE,
1142 FinalRangeTimingBudgetMicroSeconds);
1143
1144 VL53L0X_SETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds,
1145 MeasurementTimingBudgetMicroSeconds);
1146 }
1147
1148 LOG_FUNCTION_END(Status);
1149
1150 return Status;
1151 }
1152
VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,uint32_t * pMeasurementTimingBudgetMicroSeconds)1153 VL53L0X_Error VL53L0X_get_measurement_timing_budget_micro_seconds(
1154 VL53L0X_DEV Dev, uint32_t *pMeasurementTimingBudgetMicroSeconds)
1155 {
1156 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1157 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
1158 uint32_t FinalRangeTimeoutMicroSeconds;
1159 uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
1160 uint32_t StartOverheadMicroSeconds = 1910;
1161 uint32_t EndOverheadMicroSeconds = 960;
1162 uint32_t MsrcOverheadMicroSeconds = 660;
1163 uint32_t TccOverheadMicroSeconds = 590;
1164 uint32_t DssOverheadMicroSeconds = 690;
1165 uint32_t PreRangeOverheadMicroSeconds = 660;
1166 uint32_t FinalRangeOverheadMicroSeconds = 550;
1167 uint32_t PreRangeTimeoutMicroSeconds = 0;
1168
1169 LOG_FUNCTION_START("");
1170
1171 /* Start and end overhead times always present */
1172 *pMeasurementTimingBudgetMicroSeconds =
1173 StartOverheadMicroSeconds + EndOverheadMicroSeconds;
1174
1175 Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
1176
1177 if (Status != VL53L0X_ERROR_NONE) {
1178 LOG_FUNCTION_END(Status);
1179 return Status;
1180 }
1181
1182
1183 if (SchedulerSequenceSteps.TccOn || SchedulerSequenceSteps.MsrcOn ||
1184 SchedulerSequenceSteps.DssOn) {
1185
1186 Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_MSRC,
1187 &MsrcDccTccTimeoutMicroSeconds);
1188
1189 if (Status == VL53L0X_ERROR_NONE) {
1190 if (SchedulerSequenceSteps.TccOn) {
1191 *pMeasurementTimingBudgetMicroSeconds +=
1192 MsrcDccTccTimeoutMicroSeconds + TccOverheadMicroSeconds;
1193 }
1194
1195 if (SchedulerSequenceSteps.DssOn) {
1196 *pMeasurementTimingBudgetMicroSeconds +=
1197 2 * (MsrcDccTccTimeoutMicroSeconds + DssOverheadMicroSeconds);
1198 } else if (SchedulerSequenceSteps.MsrcOn) {
1199 *pMeasurementTimingBudgetMicroSeconds +=
1200 MsrcDccTccTimeoutMicroSeconds + MsrcOverheadMicroSeconds;
1201 }
1202 }
1203 }
1204
1205 if (Status == VL53L0X_ERROR_NONE) {
1206 if (SchedulerSequenceSteps.PreRangeOn) {
1207 Status =
1208 get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE,
1209 &PreRangeTimeoutMicroSeconds);
1210 *pMeasurementTimingBudgetMicroSeconds +=
1211 PreRangeTimeoutMicroSeconds + PreRangeOverheadMicroSeconds;
1212 }
1213 }
1214
1215 if (Status == VL53L0X_ERROR_NONE) {
1216 if (SchedulerSequenceSteps.FinalRangeOn) {
1217 Status =
1218 get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE,
1219 &FinalRangeTimeoutMicroSeconds);
1220 *pMeasurementTimingBudgetMicroSeconds +=
1221 (FinalRangeTimeoutMicroSeconds + FinalRangeOverheadMicroSeconds);
1222 }
1223 }
1224
1225 if (Status == VL53L0X_ERROR_NONE) {
1226 VL53L0X_SETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds,
1227 *pMeasurementTimingBudgetMicroSeconds);
1228 }
1229
1230 LOG_FUNCTION_END(Status);
1231 return Status;
1232 }
1233
1234
VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,uint8_t * pTuningSettingBuffer)1235 VL53L0X_Error VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
1236 uint8_t * pTuningSettingBuffer)
1237 {
1238 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1239 int i;
1240 int Index;
1241 uint8_t msb;
1242 uint8_t lsb;
1243 uint8_t SelectParam;
1244 uint8_t NumberOfWrites;
1245 uint8_t Address;
1246 uint8_t localBuffer[4]; /* max */
1247 uint16_t Temp16;
1248
1249 LOG_FUNCTION_START("");
1250
1251 Index = 0;
1252
1253 while ((*(pTuningSettingBuffer + Index) != 0) &&
1254 (Status == VL53L0X_ERROR_NONE)) {
1255 NumberOfWrites = *(pTuningSettingBuffer + Index);
1256 Index++;
1257 if (NumberOfWrites == 0xFF) {
1258 /* internal parameters */
1259 SelectParam = *(pTuningSettingBuffer + Index);
1260 Index++;
1261 switch (SelectParam) {
1262 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
1263 msb = *(pTuningSettingBuffer + Index);
1264 Index++;
1265 lsb = *(pTuningSettingBuffer + Index);
1266 Index++;
1267 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
1268 PALDevDataSet(Dev, SigmaEstRefArray, Temp16);
1269 break;
1270 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
1271 msb = *(pTuningSettingBuffer + Index);
1272 Index++;
1273 lsb = *(pTuningSettingBuffer + Index);
1274 Index++;
1275 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
1276 PALDevDataSet(Dev, SigmaEstEffPulseWidth, Temp16);
1277 break;
1278 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
1279 msb = *(pTuningSettingBuffer + Index);
1280 Index++;
1281 lsb = *(pTuningSettingBuffer + Index);
1282 Index++;
1283 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
1284 PALDevDataSet(Dev, SigmaEstEffAmbWidth, Temp16);
1285 break;
1286 case 3: /* uint16_t targetRefRate -> 2 bytes */
1287 msb = *(pTuningSettingBuffer + Index);
1288 Index++;
1289 lsb = *(pTuningSettingBuffer + Index);
1290 Index++;
1291 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
1292 PALDevDataSet(Dev, targetRefRate, Temp16);
1293 break;
1294 default: /* invalid parameter */
1295 Status = VL53L0X_ERROR_INVALID_PARAMS;
1296 }
1297
1298 } else if (NumberOfWrites <= 4) {
1299 Address = *(pTuningSettingBuffer + Index);
1300 Index++;
1301
1302 for (i = 0; i < NumberOfWrites; i++) {
1303 localBuffer[i] = *(pTuningSettingBuffer + Index);
1304 Index++;
1305 }
1306
1307 Status =
1308 VL53L0X_WriteMulti(Dev, Address, localBuffer, NumberOfWrites);
1309
1310 } else {
1311 Status = VL53L0X_ERROR_INVALID_PARAMS;
1312 }
1313 }
1314
1315 LOG_FUNCTION_END(Status);
1316 return Status;
1317 }
1318
VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,VL53L0X_RangingMeasurementData_t * pRangingMeasurementData,FixPoint1616_t * ptotal_xtalk_rate_mcps)1319 VL53L0X_Error VL53L0X_get_total_xtalk_rate(
1320 VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
1321 FixPoint1616_t *ptotal_xtalk_rate_mcps)
1322 {
1323 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1324
1325 uint8_t xtalkCompEnable;
1326 FixPoint1616_t totalXtalkMegaCps;
1327 FixPoint1616_t xtalkPerSpadMegaCps;
1328
1329 *ptotal_xtalk_rate_mcps = 0;
1330
1331 Status = VL53L0X_GetXTalkCompensationEnable(Dev, &xtalkCompEnable);
1332 if (Status == VL53L0X_ERROR_NONE) {
1333
1334 if (xtalkCompEnable) {
1335
1336 VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
1337 xtalkPerSpadMegaCps);
1338
1339 /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
1340 totalXtalkMegaCps = pRangingMeasurementData->EffectiveSpadRtnCount *
1341 xtalkPerSpadMegaCps;
1342
1343 /* FixPoint0824 >> 8 = FixPoint1616 */
1344 *ptotal_xtalk_rate_mcps = (totalXtalkMegaCps + 0x80) >> 8;
1345 }
1346 }
1347
1348 return Status;
1349 }
1350
VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,VL53L0X_RangingMeasurementData_t * pRangingMeasurementData,FixPoint1616_t * ptotal_signal_rate_mcps)1351 VL53L0X_Error VL53L0X_get_total_signal_rate(
1352 VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
1353 FixPoint1616_t *ptotal_signal_rate_mcps)
1354 {
1355 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1356 FixPoint1616_t totalXtalkMegaCps;
1357
1358 LOG_FUNCTION_START("");
1359
1360 *ptotal_signal_rate_mcps = pRangingMeasurementData->SignalRateRtnMegaCps;
1361
1362 Status = VL53L0X_get_total_xtalk_rate(Dev, pRangingMeasurementData,
1363 &totalXtalkMegaCps);
1364
1365 if (Status == VL53L0X_ERROR_NONE)
1366 *ptotal_signal_rate_mcps += totalXtalkMegaCps;
1367
1368 return Status;
1369 }
1370
VL53L0X_calc_dmax(VL53L0X_DEV Dev,FixPoint1616_t totalSignalRate_mcps,FixPoint1616_t totalCorrSignalRate_mcps,FixPoint1616_t pwMult,uint32_t sigmaEstimateP1,FixPoint1616_t sigmaEstimateP2,uint32_t peakVcselDuration_us,uint32_t * pdmax_mm)1371 VL53L0X_Error VL53L0X_calc_dmax(VL53L0X_DEV Dev,
1372 FixPoint1616_t totalSignalRate_mcps,
1373 FixPoint1616_t totalCorrSignalRate_mcps,
1374 FixPoint1616_t pwMult, uint32_t sigmaEstimateP1,
1375 FixPoint1616_t sigmaEstimateP2,
1376 uint32_t peakVcselDuration_us,
1377 uint32_t * pdmax_mm)
1378 {
1379 const uint32_t cSigmaLimit = 18;
1380 const FixPoint1616_t cSignalLimit = 0x4000; /* 0.25 */
1381 const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
1382 const uint32_t cAmbEffWidthSigmaEst_ns = 6;
1383 const uint32_t cAmbEffWidthDMax_ns = 7;
1384 uint32_t dmaxCalRange_mm;
1385 FixPoint1616_t dmaxCalSignalRateRtn_mcps;
1386 FixPoint1616_t minSignalNeeded;
1387 FixPoint1616_t minSignalNeeded_p1;
1388 FixPoint1616_t minSignalNeeded_p2;
1389 FixPoint1616_t minSignalNeeded_p3;
1390 FixPoint1616_t minSignalNeeded_p4;
1391 FixPoint1616_t sigmaLimitTmp;
1392 FixPoint1616_t sigmaEstSqTmp;
1393 FixPoint1616_t signalLimitTmp;
1394 FixPoint1616_t SignalAt0mm;
1395 FixPoint1616_t dmaxDark;
1396 FixPoint1616_t dmaxAmbient;
1397 FixPoint1616_t dmaxDarkTmp;
1398 FixPoint1616_t sigmaEstP2Tmp;
1399 uint32_t signalRateTemp_mcps;
1400
1401 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1402
1403 LOG_FUNCTION_START("");
1404
1405 dmaxCalRange_mm = PALDevDataGet(Dev, DmaxCalRangeMilliMeter);
1406
1407 dmaxCalSignalRateRtn_mcps = PALDevDataGet(Dev, DmaxCalSignalRateRtnMegaCps);
1408
1409 /* uint32 * FixPoint1616 = FixPoint1616 */
1410 SignalAt0mm = dmaxCalRange_mm * dmaxCalSignalRateRtn_mcps;
1411
1412 /* FixPoint1616 >> 8 = FixPoint2408 */
1413 SignalAt0mm = (SignalAt0mm + 0x80) >> 8;
1414 SignalAt0mm *= dmaxCalRange_mm;
1415
1416 minSignalNeeded_p1 = 0;
1417 if (totalCorrSignalRate_mcps > 0) {
1418
1419 /* Shift by 10 bits to increase resolution prior to the
1420 * division */
1421 signalRateTemp_mcps = totalSignalRate_mcps << 10;
1422
1423 /* Add rounding value prior to division */
1424 minSignalNeeded_p1 =
1425 signalRateTemp_mcps + (totalCorrSignalRate_mcps / 2);
1426
1427 /* FixPoint0626/FixPoint1616 = FixPoint2210 */
1428 minSignalNeeded_p1 /= totalCorrSignalRate_mcps;
1429
1430 /* Apply a factored version of the speed of light.
1431 Correction to be applied at the end */
1432 minSignalNeeded_p1 *= 3;
1433
1434 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
1435 minSignalNeeded_p1 *= minSignalNeeded_p1;
1436
1437 /* FixPoint1220 >> 16 = FixPoint2804 */
1438 minSignalNeeded_p1 = (minSignalNeeded_p1 + 0x8000) >> 16;
1439 }
1440
1441 minSignalNeeded_p2 = pwMult * sigmaEstimateP1;
1442
1443 /* FixPoint1616 >> 16 = uint32 */
1444 minSignalNeeded_p2 = (minSignalNeeded_p2 + 0x8000) >> 16;
1445
1446 /* uint32 * uint32 = uint32 */
1447 minSignalNeeded_p2 *= minSignalNeeded_p2;
1448
1449 /* Check sigmaEstimateP2
1450 * If this value is too high there is not enough signal rate
1451 * to calculate dmax value so set a suitable value to ensure
1452 * a very small dmax.
1453 */
1454 sigmaEstP2Tmp = (sigmaEstimateP2 + 0x8000) >> 16;
1455 sigmaEstP2Tmp =
1456 (sigmaEstP2Tmp + cAmbEffWidthSigmaEst_ns / 2) / cAmbEffWidthSigmaEst_ns;
1457 sigmaEstP2Tmp *= cAmbEffWidthDMax_ns;
1458
1459 if (sigmaEstP2Tmp > 0xffff) {
1460 minSignalNeeded_p3 = 0xfff00000;
1461 } else {
1462
1463 /* DMAX uses a different ambient width from sigma, so apply
1464 * correction.
1465 * Perform division before multiplication to prevent overflow.
1466 */
1467 sigmaEstimateP2 = (sigmaEstimateP2 + cAmbEffWidthSigmaEst_ns / 2) /
1468 cAmbEffWidthSigmaEst_ns;
1469 sigmaEstimateP2 *= cAmbEffWidthDMax_ns;
1470
1471 /* FixPoint1616 >> 16 = uint32 */
1472 minSignalNeeded_p3 = (sigmaEstimateP2 + 0x8000) >> 16;
1473
1474 minSignalNeeded_p3 *= minSignalNeeded_p3;
1475 }
1476
1477 /* FixPoint1814 / uint32 = FixPoint1814 */
1478 sigmaLimitTmp = ((cSigmaLimit << 14) + 500) / 1000;
1479
1480 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
1481 sigmaLimitTmp *= sigmaLimitTmp;
1482
1483 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
1484 sigmaEstSqTmp = cSigmaEstRef * cSigmaEstRef;
1485
1486 /* FixPoint3232 >> 4 = FixPoint0428 */
1487 sigmaEstSqTmp = (sigmaEstSqTmp + 0x08) >> 4;
1488
1489 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */
1490 sigmaLimitTmp -= sigmaEstSqTmp;
1491
1492 /* uint32_t * FixPoint0428 = FixPoint0428 */
1493 minSignalNeeded_p4 = 4 * 12 * sigmaLimitTmp;
1494
1495 /* FixPoint0428 >> 14 = FixPoint1814 */
1496 minSignalNeeded_p4 = (minSignalNeeded_p4 + 0x2000) >> 14;
1497
1498 /* uint32 + uint32 = uint32 */
1499 minSignalNeeded = (minSignalNeeded_p2 + minSignalNeeded_p3);
1500
1501 /* uint32 / uint32 = uint32 */
1502 minSignalNeeded += (peakVcselDuration_us / 2);
1503 minSignalNeeded /= peakVcselDuration_us;
1504
1505 /* uint32 << 14 = FixPoint1814 */
1506 minSignalNeeded <<= 14;
1507
1508 /* FixPoint1814 / FixPoint1814 = uint32 */
1509 minSignalNeeded += (minSignalNeeded_p4 / 2);
1510 minSignalNeeded /= minSignalNeeded_p4;
1511
1512 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
1513 minSignalNeeded *= minSignalNeeded_p1;
1514
1515 /* Apply correction by dividing by 1000000.
1516 * This assumes 10E16 on the numerator of the equation
1517 * and 10E-22 on the denominator.
1518 * We do this because 32bit fix point calculation can't
1519 * handle the larger and smaller elements of this equation,
1520 * i.e. speed of light and pulse widths.
1521 */
1522 minSignalNeeded = (minSignalNeeded + 500) / 1000;
1523 minSignalNeeded <<= 4;
1524
1525 minSignalNeeded = (minSignalNeeded + 500) / 1000;
1526
1527 /* FixPoint1616 >> 8 = FixPoint2408 */
1528 signalLimitTmp = (cSignalLimit + 0x80) >> 8;
1529
1530 /* FixPoint2408/FixPoint2408 = uint32 */
1531 if (signalLimitTmp != 0)
1532 dmaxDarkTmp = (SignalAt0mm + (signalLimitTmp / 2)) / signalLimitTmp;
1533 else
1534 dmaxDarkTmp = 0;
1535
1536 dmaxDark = VL53L0X_isqrt(dmaxDarkTmp);
1537
1538 /* FixPoint2408/FixPoint2408 = uint32 */
1539 if (minSignalNeeded != 0)
1540 dmaxAmbient = (SignalAt0mm + minSignalNeeded / 2) / minSignalNeeded;
1541 else
1542 dmaxAmbient = 0;
1543
1544 dmaxAmbient = VL53L0X_isqrt(dmaxAmbient);
1545
1546 *pdmax_mm = dmaxDark;
1547 if (dmaxDark > dmaxAmbient)
1548 *pdmax_mm = dmaxAmbient;
1549
1550 LOG_FUNCTION_END(Status);
1551
1552 return Status;
1553 }
1554
1555
VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,VL53L0X_RangingMeasurementData_t * pRangingMeasurementData,FixPoint1616_t * pSigmaEstimate,uint32_t * pDmax_mm)1556 VL53L0X_Error VL53L0X_calc_sigma_estimate(
1557 VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
1558 FixPoint1616_t *pSigmaEstimate, uint32_t *pDmax_mm)
1559 {
1560 /* Expressed in 100ths of a ns, i.e. centi-ns */
1561 const uint32_t cPulseEffectiveWidth_centi_ns = 800;
1562 /* Expressed in 100ths of a ns, i.e. centi-ns */
1563 const uint32_t cAmbientEffectiveWidth_centi_ns = 600;
1564 const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
1565 const uint32_t cVcselPulseWidth_ps = 4700; /* pico secs */
1566 const FixPoint1616_t cSigmaEstMax = 0x028F87AE;
1567 const FixPoint1616_t cSigmaEstRtnMax = 0xF000;
1568 const FixPoint1616_t cAmbToSignalRatioMax =
1569 0xF0000000 / cAmbientEffectiveWidth_centi_ns;
1570 /* Time Of Flight per mm (6.6 pico secs) */
1571 const FixPoint1616_t cTOF_per_mm_ps = 0x0006999A;
1572 const uint32_t c16BitRoundingParam = 0x00008000;
1573 const FixPoint1616_t cMaxXTalk_kcps = 0x00320000;
1574 const uint32_t cPllPeriod_ps = 1655;
1575
1576 uint32_t vcselTotalEventsRtn;
1577 uint32_t finalRangeTimeoutMicroSecs;
1578 uint32_t preRangeTimeoutMicroSecs;
1579 FixPoint1616_t sigmaEstimateP1;
1580 FixPoint1616_t sigmaEstimateP2;
1581 FixPoint1616_t sigmaEstimateP3;
1582 FixPoint1616_t deltaT_ps;
1583 FixPoint1616_t pwMult;
1584 FixPoint1616_t sigmaEstRtn;
1585 FixPoint1616_t sigmaEstimate;
1586 FixPoint1616_t xTalkCorrection;
1587 FixPoint1616_t ambientRate_kcps;
1588 FixPoint1616_t peakSignalRate_kcps;
1589 FixPoint1616_t xTalkCompRate_mcps;
1590 uint32_t xTalkCompRate_kcps;
1591 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1592 FixPoint1616_t diff1_mcps;
1593 FixPoint1616_t diff2_mcps;
1594 FixPoint1616_t sqr1;
1595 FixPoint1616_t sqr2;
1596 FixPoint1616_t sqrSum;
1597 FixPoint1616_t sqrtResult_centi_ns;
1598 FixPoint1616_t sqrtResult;
1599 FixPoint1616_t totalSignalRate_mcps;
1600 FixPoint1616_t correctedSignalRate_mcps;
1601 uint32_t vcselWidth;
1602 uint32_t finalRangeMacroPCLKS;
1603 uint32_t preRangeMacroPCLKS;
1604 uint32_t peakVcselDuration_us;
1605 uint8_t finalRangeVcselPCLKS;
1606 uint8_t preRangeVcselPCLKS;
1607 /*! \addtogroup calc_sigma_estimate
1608 * @{
1609 *
1610 * Estimates the range sigma based on the
1611 *
1612 * - vcsel_rate_kcps
1613 * - ambient_rate_kcps
1614 * - signal_total_events
1615 * - xtalk_rate
1616 *
1617 * and the following parameters
1618 *
1619 * - SigmaEstRefArray
1620 * - SigmaEstEffPulseWidth
1621 * - SigmaEstEffAmbWidth
1622 */
1623
1624 LOG_FUNCTION_START("");
1625
1626 VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
1627 xTalkCompRate_mcps);
1628
1629 /*
1630 * We work in kcps rather than mcps as this helps keep within the
1631 * confines of the 32 Fix1616 type.
1632 */
1633
1634 ambientRate_kcps =
1635 (pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16;
1636
1637 correctedSignalRate_mcps = pRangingMeasurementData->SignalRateRtnMegaCps;
1638
1639
1640 Status = VL53L0X_get_total_signal_rate(Dev, pRangingMeasurementData,
1641 &totalSignalRate_mcps);
1642 Status = VL53L0X_get_total_xtalk_rate(Dev, pRangingMeasurementData,
1643 &xTalkCompRate_mcps);
1644
1645
1646 /* Signal rate measurement provided by device is the
1647 * peak signal rate, not average.
1648 */
1649 peakSignalRate_kcps = (totalSignalRate_mcps * 1000);
1650 peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16;
1651
1652 xTalkCompRate_kcps = xTalkCompRate_mcps * 1000;
1653
1654 if (xTalkCompRate_kcps > cMaxXTalk_kcps)
1655 xTalkCompRate_kcps = cMaxXTalk_kcps;
1656
1657 if (Status == VL53L0X_ERROR_NONE) {
1658
1659 /* Calculate final range macro periods */
1660 finalRangeTimeoutMicroSecs =
1661 VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, FinalRangeTimeoutMicroSecs);
1662
1663 finalRangeVcselPCLKS =
1664 VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, FinalRangeVcselPulsePeriod);
1665
1666 finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
1667 Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS);
1668
1669 /* Calculate pre-range macro periods */
1670 preRangeTimeoutMicroSecs =
1671 VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, PreRangeTimeoutMicroSecs);
1672
1673 preRangeVcselPCLKS =
1674 VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, PreRangeVcselPulsePeriod);
1675
1676 preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
1677 Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS);
1678
1679 vcselWidth = 3;
1680 if (finalRangeVcselPCLKS == 8)
1681 vcselWidth = 2;
1682
1683
1684 peakVcselDuration_us =
1685 vcselWidth * 2048 * (preRangeMacroPCLKS + finalRangeMacroPCLKS);
1686 peakVcselDuration_us = (peakVcselDuration_us + 500) / 1000;
1687 peakVcselDuration_us *= cPllPeriod_ps;
1688 peakVcselDuration_us = (peakVcselDuration_us + 500) / 1000;
1689
1690 /* Fix1616 >> 8 = Fix2408 */
1691 totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8;
1692
1693 /* Fix2408 * uint32 = Fix2408 */
1694 vcselTotalEventsRtn = totalSignalRate_mcps * peakVcselDuration_us;
1695
1696 /* Fix2408 >> 8 = uint32 */
1697 vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8;
1698
1699 /* Fix2408 << 8 = Fix1616 = */
1700 totalSignalRate_mcps <<= 8;
1701 }
1702
1703 if (Status != VL53L0X_ERROR_NONE) {
1704 LOG_FUNCTION_END(Status);
1705 return Status;
1706 }
1707
1708 if (peakSignalRate_kcps == 0) {
1709 *pSigmaEstimate = cSigmaEstMax;
1710 PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax);
1711 *pDmax_mm = 0;
1712 } else {
1713 if (vcselTotalEventsRtn < 1)
1714 vcselTotalEventsRtn = 1;
1715
1716 /*
1717 * Calculate individual components of the main equation -
1718 * replicating the equation implemented in the script
1719 * OpenAll_Ewok_ranging_data.jsl.
1720 *
1721 * sigmaEstimateP1 represents the effective pulse width, which
1722 * is a tuning parameter, rather than a real value.
1723 *
1724 * sigmaEstimateP2 represents the ambient/signal rate ratio
1725 * expressed as a multiple of the effective ambient width
1726 * (tuning parameter).
1727 *
1728 * sigmaEstimateP3 provides the signal event component, with the
1729 * knowledge that
1730 * - Noise of a square pulse is 1/sqrt(12) of the pulse
1731 * width.
1732 * - at 0Lux, sigma is proportional to
1733 * effectiveVcselPulseWidth/sqrt(12 * signalTotalEvents)
1734 *
1735 * deltaT_ps represents the time of flight in pico secs for the
1736 * current range measurement, using the "TOF per mm" constant
1737 * (in ps).
1738 */
1739
1740 sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns;
1741
1742 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
1743 sigmaEstimateP2 = (ambientRate_kcps << 16) / peakSignalRate_kcps;
1744 if (sigmaEstimateP2 > cAmbToSignalRatioMax) {
1745 /* Clip to prevent overflow. Will ensure safe
1746 * max result. */
1747 sigmaEstimateP2 = cAmbToSignalRatioMax;
1748 }
1749 sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns;
1750
1751 sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12);
1752
1753 /* uint32 * FixPoint1616 = FixPoint1616 */
1754 deltaT_ps = pRangingMeasurementData->RangeMilliMeter * cTOF_per_mm_ps;
1755
1756 /*
1757 * vcselRate - xtalkCompRate
1758 * (uint32 << 16) - FixPoint1616 = FixPoint1616.
1759 * Divide result by 1000 to convert to mcps.
1760 * 500 is added to ensure rounding when integer division
1761 * truncates.
1762 */
1763 diff1_mcps =
1764 (((peakSignalRate_kcps << 16) - xTalkCompRate_kcps) + 500) / 1000;
1765
1766 /* vcselRate + xtalkCompRate */
1767 diff2_mcps =
1768 (((peakSignalRate_kcps << 16) + xTalkCompRate_kcps) + 500) / 1000;
1769
1770 /* Shift by 8 bits to increase resolution prior to the
1771 * division */
1772 diff1_mcps <<= 8;
1773
1774 /* FixPoint0824/FixPoint1616 = FixPoint2408 */
1775 xTalkCorrection = abs(diff1_mcps / diff2_mcps);
1776
1777 /* FixPoint2408 << 8 = FixPoint1616 */
1778 xTalkCorrection <<= 8;
1779
1780 /* FixPoint1616/uint32 = FixPoint1616 */
1781 pwMult = deltaT_ps / cVcselPulseWidth_ps; /* smaller than 1.0f */
1782
1783 /*
1784 * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
1785 * values are small enough such that32 bits will not be
1786 * exceeded.
1787 */
1788 pwMult *= ((1 << 16) - xTalkCorrection);
1789
1790 /* (FixPoint3232 >> 16) = FixPoint1616 */
1791 pwMult = (pwMult + c16BitRoundingParam) >> 16;
1792
1793 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
1794 pwMult += (1 << 16);
1795
1796 /*
1797 * At this point the value will be 1.xx, therefore if we square
1798 * the value this will exceed 32 bits. To address this perform
1799 * a single shift to the right before the multiplication.
1800 */
1801 pwMult >>= 1;
1802 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
1803 pwMult = pwMult * pwMult;
1804
1805 /* (FixPoint3430 >> 14) = Fix1616 */
1806 pwMult >>= 14;
1807
1808 /* FixPoint1616 * uint32 = FixPoint1616 */
1809 sqr1 = pwMult * sigmaEstimateP1;
1810
1811 /* (FixPoint1616 >> 16) = FixPoint3200 */
1812 sqr1 = (sqr1 + 0x8000) >> 16;
1813
1814 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
1815 sqr1 *= sqr1;
1816
1817 sqr2 = sigmaEstimateP2;
1818
1819 /* (FixPoint1616 >> 16) = FixPoint3200 */
1820 sqr2 = (sqr2 + 0x8000) >> 16;
1821
1822 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
1823 sqr2 *= sqr2;
1824
1825 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
1826 sqrSum = sqr1 + sqr2;
1827
1828 /* SQRT(FixPoin6400) = FixPoint3200 */
1829 sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum);
1830
1831 /* (FixPoint3200 << 16) = FixPoint1616 */
1832 sqrtResult_centi_ns <<= 16;
1833
1834 /*
1835 * Note that the Speed Of Light is expressed in um per 1E-10
1836 * seconds (2997) Therefore to get mm/ns we have to divide by
1837 * 10000
1838 */
1839 sigmaEstRtn = (((sqrtResult_centi_ns + 50) / 100) / sigmaEstimateP3);
1840 sigmaEstRtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
1841
1842 /* Add 5000 before dividing by 10000 to ensure rounding. */
1843 sigmaEstRtn += 5000;
1844 sigmaEstRtn /= 10000;
1845
1846 if (sigmaEstRtn > cSigmaEstRtnMax) {
1847 /* Clip to prevent overflow. Will ensure safe
1848 * max result. */
1849 sigmaEstRtn = cSigmaEstRtnMax;
1850 }
1851
1852 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
1853 sqr1 = sigmaEstRtn * sigmaEstRtn;
1854 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
1855 sqr2 = cSigmaEstRef * cSigmaEstRef;
1856
1857 /* sqrt(FixPoint3232) = FixPoint1616 */
1858 sqrtResult = VL53L0X_isqrt((sqr1 + sqr2));
1859 /*
1860 * Note that the Shift by 4 bits increases resolution prior to
1861 * the sqrt, therefore the result must be shifted by 2 bits to
1862 * the right to revert back to the FixPoint1616 format.
1863 */
1864
1865 sigmaEstimate = 1000 * sqrtResult;
1866
1867 if ((peakSignalRate_kcps < 1) || (vcselTotalEventsRtn < 1) ||
1868 (sigmaEstimate > cSigmaEstMax)) {
1869 sigmaEstimate = cSigmaEstMax;
1870 }
1871
1872 *pSigmaEstimate = (uint32_t)(sigmaEstimate);
1873 PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate);
1874 Status = VL53L0X_calc_dmax(
1875 Dev, totalSignalRate_mcps, correctedSignalRate_mcps, pwMult,
1876 sigmaEstimateP1, sigmaEstimateP2, peakVcselDuration_us, pDmax_mm);
1877 }
1878
1879 LOG_FUNCTION_END(Status);
1880 return Status;
1881 }
1882
VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,uint8_t DeviceRangeStatus,FixPoint1616_t SignalRate,uint16_t EffectiveSpadRtnCount,VL53L0X_RangingMeasurementData_t * pRangingMeasurementData,uint8_t * pPalRangeStatus)1883 VL53L0X_Error VL53L0X_get_pal_range_status(
1884 VL53L0X_DEV Dev, uint8_t DeviceRangeStatus, FixPoint1616_t SignalRate,
1885 uint16_t EffectiveSpadRtnCount,
1886 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
1887 uint8_t * pPalRangeStatus)
1888 {
1889 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1890 uint8_t NoneFlag;
1891 uint8_t SigmaLimitflag = 0;
1892 uint8_t SignalRefClipflag = 0;
1893 uint8_t RangeIgnoreThresholdflag = 0;
1894 uint8_t SigmaLimitCheckEnable = 0;
1895 uint8_t SignalRateFinalRangeLimitCheckEnable = 0;
1896 uint8_t SignalRefClipLimitCheckEnable = 0;
1897 uint8_t RangeIgnoreThresholdLimitCheckEnable = 0;
1898 FixPoint1616_t SigmaEstimate;
1899 FixPoint1616_t SigmaLimitValue;
1900 FixPoint1616_t SignalRefClipValue;
1901 FixPoint1616_t RangeIgnoreThresholdValue;
1902 FixPoint1616_t SignalRatePerSpad;
1903 uint8_t DeviceRangeStatusInternal = 0;
1904 uint16_t tmpWord = 0;
1905 uint8_t Temp8;
1906 uint32_t Dmax_mm = 0;
1907 FixPoint1616_t LastSignalRefMcps;
1908
1909 LOG_FUNCTION_START("");
1910
1911
1912 /*
1913 * VL53L0X has a good ranging when the value of the
1914 * DeviceRangeStatus = 11. This function will replace the value 0 with
1915 * the value 11 in the DeviceRangeStatus.
1916 * In addition, the SigmaEstimator is not included in the VL53L0X
1917 * DeviceRangeStatus, this will be added in the PalRangeStatus.
1918 */
1919
1920 DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3);
1921
1922 if (DeviceRangeStatusInternal == 0 || DeviceRangeStatusInternal == 5 ||
1923 DeviceRangeStatusInternal == 7 || DeviceRangeStatusInternal == 12 ||
1924 DeviceRangeStatusInternal == 13 || DeviceRangeStatusInternal == 14 ||
1925 DeviceRangeStatusInternal == 15) {
1926 NoneFlag = 1;
1927 } else {
1928 NoneFlag = 0;
1929 }
1930
1931 /* LastSignalRefMcps */
1932 if (Status == VL53L0X_ERROR_NONE)
1933 Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
1934
1935 if (Status == VL53L0X_ERROR_NONE)
1936 Status = VL53L0X_RdWord(Dev, VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
1937 &tmpWord);
1938
1939 LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord);
1940
1941 if (Status == VL53L0X_ERROR_NONE)
1942 Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
1943
1944 PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps);
1945
1946 /*
1947 * Check if Sigma limit is enabled, if yes then do comparison with limit
1948 * value and put the result back into pPalRangeStatus.
1949 */
1950 if (Status == VL53L0X_ERROR_NONE)
1951 Status = VL53L0X_GetLimitCheckEnable(
1952 Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, &SigmaLimitCheckEnable);
1953
1954 if ((SigmaLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) {
1955 /*
1956 * compute the Sigma and check with limit
1957 */
1958 Status = VL53L0X_calc_sigma_estimate(Dev, pRangingMeasurementData,
1959 &SigmaEstimate, &Dmax_mm);
1960 if (Status == VL53L0X_ERROR_NONE)
1961 pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm;
1962
1963 if (Status == VL53L0X_ERROR_NONE) {
1964 Status = VL53L0X_GetLimitCheckValue(
1965 Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, &SigmaLimitValue);
1966
1967 if ((SigmaLimitValue > 0) && (SigmaEstimate > SigmaLimitValue))
1968 /* Limit Fail */
1969 SigmaLimitflag = 1;
1970 }
1971 }
1972
1973 /*
1974 * Check if Signal ref clip limit is enabled, if yes then do comparison
1975 * with limit value and put the result back into pPalRangeStatus.
1976 */
1977 if (Status == VL53L0X_ERROR_NONE)
1978 Status =
1979 VL53L0X_GetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
1980 &SignalRefClipLimitCheckEnable);
1981
1982 if ((SignalRefClipLimitCheckEnable != 0) &&
1983 (Status == VL53L0X_ERROR_NONE)) {
1984
1985 Status = VL53L0X_GetLimitCheckValue(
1986 Dev, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, &SignalRefClipValue);
1987
1988 if ((SignalRefClipValue > 0) &&
1989 (LastSignalRefMcps > SignalRefClipValue)) {
1990 /* Limit Fail */
1991 SignalRefClipflag = 1;
1992 }
1993 }
1994
1995 /*
1996 * Check if Signal ref clip limit is enabled, if yes then do comparison
1997 * with limit value and put the result back into pPalRangeStatus.
1998 * EffectiveSpadRtnCount has a format 8.8
1999 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
2000 */
2001 if (Status == VL53L0X_ERROR_NONE)
2002 Status = VL53L0X_GetLimitCheckEnable(
2003 Dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
2004 &RangeIgnoreThresholdLimitCheckEnable);
2005
2006 if ((RangeIgnoreThresholdLimitCheckEnable != 0) &&
2007 (Status == VL53L0X_ERROR_NONE)) {
2008
2009 /* Compute the signal rate per spad */
2010 if (EffectiveSpadRtnCount == 0) {
2011 SignalRatePerSpad = 0;
2012 } else {
2013 SignalRatePerSpad =
2014 (FixPoint1616_t)((256 * SignalRate) / EffectiveSpadRtnCount);
2015 }
2016
2017 Status = VL53L0X_GetLimitCheckValue(
2018 Dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
2019 &RangeIgnoreThresholdValue);
2020
2021 if ((RangeIgnoreThresholdValue > 0) &&
2022 (SignalRatePerSpad < RangeIgnoreThresholdValue)) {
2023 /* Limit Fail add 2^6 to range status */
2024 RangeIgnoreThresholdflag = 1;
2025 }
2026 }
2027
2028 if (Status == VL53L0X_ERROR_NONE) {
2029 if (NoneFlag == 1) {
2030 *pPalRangeStatus = 255; /* NONE */
2031 } else if (DeviceRangeStatusInternal == 1 ||
2032 DeviceRangeStatusInternal == 2 ||
2033 DeviceRangeStatusInternal == 3) {
2034 *pPalRangeStatus = 5; /* HW fail */
2035 } else if (DeviceRangeStatusInternal == 6 ||
2036 DeviceRangeStatusInternal == 9) {
2037 *pPalRangeStatus = 4; /* Phase fail */
2038 } else if (DeviceRangeStatusInternal == 8 ||
2039 DeviceRangeStatusInternal == 10 || SignalRefClipflag == 1) {
2040 *pPalRangeStatus = 3; /* Min range */
2041 } else if (DeviceRangeStatusInternal == 4 ||
2042 RangeIgnoreThresholdflag == 1) {
2043 *pPalRangeStatus = 2; /* Signal Fail */
2044 } else if (SigmaLimitflag == 1) {
2045 *pPalRangeStatus = 1; /* Sigma Fail */
2046 } else {
2047 *pPalRangeStatus = 0; /* Range Valid */
2048 }
2049 }
2050
2051 /* DMAX only relevant during range error */
2052 if (*pPalRangeStatus == 0)
2053 pRangingMeasurementData->RangeDMaxMilliMeter = 0;
2054
2055 /* fill the Limit Check Status */
2056
2057 Status = VL53L0X_GetLimitCheckEnable(
2058 Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
2059 &SignalRateFinalRangeLimitCheckEnable);
2060
2061 if (Status == VL53L0X_ERROR_NONE) {
2062 if ((SigmaLimitCheckEnable == 0) || (SigmaLimitflag == 1))
2063 Temp8 = 1;
2064 else
2065 Temp8 = 0;
2066 VL53L0X_SETARRAYPARAMETERFIELD(
2067 Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
2068
2069 if ((DeviceRangeStatusInternal == 4) ||
2070 (SignalRateFinalRangeLimitCheckEnable == 0))
2071 Temp8 = 1;
2072 else
2073 Temp8 = 0;
2074 VL53L0X_SETARRAYPARAMETERFIELD(
2075 Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
2076 Temp8);
2077
2078 if ((SignalRefClipLimitCheckEnable == 0) || (SignalRefClipflag == 1))
2079 Temp8 = 1;
2080 else
2081 Temp8 = 0;
2082
2083 VL53L0X_SETARRAYPARAMETERFIELD(
2084 Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8);
2085
2086 if ((RangeIgnoreThresholdLimitCheckEnable == 0) ||
2087 (RangeIgnoreThresholdflag == 1))
2088 Temp8 = 1;
2089 else
2090 Temp8 = 0;
2091
2092 VL53L0X_SETARRAYPARAMETERFIELD(
2093 Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
2094 Temp8);
2095 }
2096
2097 LOG_FUNCTION_END(Status);
2098 return Status;
2099 }
2100