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