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 #ifndef __KERNEL__
34 #include <stdlib.h>
35 #endif
36
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
44 #define REF_ARRAY_SPAD_0 0
45 #define REF_ARRAY_SPAD_5 5
46 #define REF_ARRAY_SPAD_10 10
47
48 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5,
49 REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 };
50
VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,FixPoint1616_t XTalkCalDistance,FixPoint1616_t * pXTalkCompensationRateMegaCps)51 VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,
52 FixPoint1616_t XTalkCalDistance,
53 FixPoint1616_t *pXTalkCompensationRateMegaCps)
54 {
55 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
56 uint16_t sum_ranging = 0;
57 uint16_t sum_spads = 0;
58 FixPoint1616_t sum_signalRate = 0;
59 FixPoint1616_t total_count = 0;
60 uint8_t xtalk_meas = 0;
61 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
62 FixPoint1616_t xTalkStoredMeanSignalRate;
63 FixPoint1616_t xTalkStoredMeanRange;
64 FixPoint1616_t xTalkStoredMeanRtnSpads;
65 uint32_t signalXTalkTotalPerSpad;
66 uint32_t xTalkStoredMeanRtnSpadsAsInt;
67 uint32_t xTalkCalDistanceAsInt;
68 FixPoint1616_t XTalkCompensationRateMegaCps;
69
70 if (XTalkCalDistance <= 0)
71 Status = VL53L0X_ERROR_INVALID_PARAMS;
72
73 /* Disable the XTalk compensation */
74 if (Status == VL53L0X_ERROR_NONE)
75 Status = VL53L0X_SetXTalkCompensationEnable(Dev, 0);
76
77 /* Disable the RIT */
78 if (Status == VL53L0X_ERROR_NONE) {
79 Status = VL53L0X_SetLimitCheckEnable(Dev,
80 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
81 }
82
83 /* Perform 50 measurements and compute the averages */
84 if (Status == VL53L0X_ERROR_NONE) {
85 sum_ranging = 0;
86 sum_spads = 0;
87 sum_signalRate = 0;
88 total_count = 0;
89 for (xtalk_meas = 0; xtalk_meas < 50; xtalk_meas++) {
90 Status = VL53L0X_PerformSingleRangingMeasurement(Dev,
91 &RangingMeasurementData);
92
93 if (Status != VL53L0X_ERROR_NONE)
94 break;
95
96 /* The range is valid when RangeStatus = 0 */
97 if (RangingMeasurementData.RangeStatus == 0) {
98 sum_ranging = sum_ranging +
99 RangingMeasurementData.RangeMilliMeter;
100 sum_signalRate = sum_signalRate +
101 RangingMeasurementData.SignalRateRtnMegaCps;
102 sum_spads = sum_spads +
103 RangingMeasurementData.EffectiveSpadRtnCount
104 / 256;
105 total_count = total_count + 1;
106 }
107 }
108
109 /* no valid values found */
110 if (total_count == 0)
111 Status = VL53L0X_ERROR_RANGE_ERROR;
112
113 }
114
115
116 if (Status == VL53L0X_ERROR_NONE) {
117 /* FixPoint1616_t / uint16_t = FixPoint1616_t */
118 xTalkStoredMeanSignalRate = sum_signalRate / total_count;
119 xTalkStoredMeanRange = (FixPoint1616_t)((uint32_t)(
120 sum_ranging << 16) / total_count);
121 xTalkStoredMeanRtnSpads = (FixPoint1616_t)((uint32_t)(
122 sum_spads << 16) / total_count);
123
124 /* Round Mean Spads to Whole Number.
125 * Typically the calculated mean SPAD count is a whole number
126 * or very close to a whole
127 * number, therefore any truncation will not result in a
128 * significant loss in accuracy.
129 * Also, for a grey target at a typical distance of around
130 * 400mm, around 220 SPADs will
131 * be enabled, therefore, any truncation will result in a loss
132 * of accuracy of less than
133 * 0.5%.
134 */
135 xTalkStoredMeanRtnSpadsAsInt = (xTalkStoredMeanRtnSpads +
136 0x8000) >> 16;
137
138 /* Round Cal Distance to Whole Number.
139 * Note that the cal distance is in mm, therefore no resolution
140 * is lost.*/
141 xTalkCalDistanceAsInt = (XTalkCalDistance + 0x8000) >> 16;
142
143 if (xTalkStoredMeanRtnSpadsAsInt == 0 ||
144 xTalkCalDistanceAsInt == 0 ||
145 xTalkStoredMeanRange >= XTalkCalDistance) {
146 XTalkCompensationRateMegaCps = 0;
147 } else {
148 /* Round Cal Distance to Whole Number.
149 Note that the cal distance is in mm, therefore no
150 resolution is lost.*/
151 xTalkCalDistanceAsInt = (XTalkCalDistance +
152 0x8000) >> 16;
153
154 /* Apply division by mean spad count early in the
155 * calculation to keep the numbers small.
156 * This ensures we can maintain a 32bit calculation.
157 * Fixed1616 / int := Fixed1616 */
158 signalXTalkTotalPerSpad = (xTalkStoredMeanSignalRate) /
159 xTalkStoredMeanRtnSpadsAsInt;
160
161 /* Complete the calculation for total Signal XTalk per
162 * SPAD
163 * Fixed1616 * (Fixed1616 - Fixed1616/int) :=
164 * (2^16 * Fixed1616)
165 */
166 signalXTalkTotalPerSpad *= ((1 << 16) -
167 (xTalkStoredMeanRange / xTalkCalDistanceAsInt));
168
169 /* Round from 2^16 * Fixed1616, to Fixed1616. */
170 XTalkCompensationRateMegaCps = (signalXTalkTotalPerSpad
171 + 0x8000) >> 16;
172 }
173
174 *pXTalkCompensationRateMegaCps = XTalkCompensationRateMegaCps;
175
176 /* Enable the XTalk compensation */
177 if (Status == VL53L0X_ERROR_NONE)
178 Status = VL53L0X_SetXTalkCompensationEnable(Dev, 1);
179
180 /* Enable the XTalk compensation */
181 if (Status == VL53L0X_ERROR_NONE)
182 Status = VL53L0X_SetXTalkCompensationRateMegaCps(Dev,
183 XTalkCompensationRateMegaCps);
184
185 }
186
187 return Status;
188 }
189
VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,FixPoint1616_t CalDistanceMilliMeter,int32_t * pOffsetMicroMeter)190 VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,
191 FixPoint1616_t CalDistanceMilliMeter,
192 int32_t *pOffsetMicroMeter)
193 {
194 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
195 uint16_t sum_ranging = 0;
196 FixPoint1616_t total_count = 0;
197 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
198 FixPoint1616_t StoredMeanRange;
199 uint32_t StoredMeanRangeAsInt;
200 uint32_t CalDistanceAsInt_mm;
201 uint8_t SequenceStepEnabled;
202 int meas = 0;
203
204 if (CalDistanceMilliMeter <= 0)
205 Status = VL53L0X_ERROR_INVALID_PARAMS;
206
207 if (Status == VL53L0X_ERROR_NONE)
208 Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev, 0);
209
210
211 /* Get the value of the TCC */
212 if (Status == VL53L0X_ERROR_NONE)
213 Status = VL53L0X_GetSequenceStepEnable(Dev,
214 VL53L0X_SEQUENCESTEP_TCC, &SequenceStepEnabled);
215
216
217 /* Disable the TCC */
218 if (Status == VL53L0X_ERROR_NONE)
219 Status = VL53L0X_SetSequenceStepEnable(Dev,
220 VL53L0X_SEQUENCESTEP_TCC, 0);
221
222
223 /* Disable the RIT */
224 if (Status == VL53L0X_ERROR_NONE)
225 Status = VL53L0X_SetLimitCheckEnable(Dev,
226 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
227
228 /* Perform 50 measurements and compute the averages */
229 if (Status == VL53L0X_ERROR_NONE) {
230 sum_ranging = 0;
231 total_count = 0;
232 for (meas = 0; meas < 50; meas++) {
233 Status = VL53L0X_PerformSingleRangingMeasurement(Dev,
234 &RangingMeasurementData);
235
236 if (Status != VL53L0X_ERROR_NONE)
237 break;
238
239 /* The range is valid when RangeStatus = 0 */
240 if (RangingMeasurementData.RangeStatus == 0) {
241 sum_ranging = sum_ranging +
242 RangingMeasurementData.RangeMilliMeter;
243 total_count = total_count + 1;
244 }
245 }
246
247 /* no valid values found */
248 if (total_count == 0)
249 Status = VL53L0X_ERROR_RANGE_ERROR;
250 }
251
252
253 if (Status == VL53L0X_ERROR_NONE) {
254 /* FixPoint1616_t / uint16_t = FixPoint1616_t */
255 StoredMeanRange = (FixPoint1616_t)((uint32_t)(sum_ranging << 16)
256 / total_count);
257
258 StoredMeanRangeAsInt = (StoredMeanRange + 0x8000) >> 16;
259
260 /* Round Cal Distance to Whole Number.
261 * Note that the cal distance is in mm, therefore no resolution
262 * is lost.*/
263 CalDistanceAsInt_mm = (CalDistanceMilliMeter + 0x8000) >> 16;
264
265 *pOffsetMicroMeter = (CalDistanceAsInt_mm -
266 StoredMeanRangeAsInt) * 1000;
267
268 /* Apply the calculated offset */
269 if (Status == VL53L0X_ERROR_NONE) {
270 VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
271 *pOffsetMicroMeter);
272 Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
273 *pOffsetMicroMeter);
274 }
275
276 }
277
278 /* Restore the TCC */
279 if (Status == VL53L0X_ERROR_NONE) {
280 if (SequenceStepEnabled != 0)
281 Status = VL53L0X_SetSequenceStepEnable(Dev,
282 VL53L0X_SEQUENCESTEP_TCC, 1);
283 }
284
285 return Status;
286 }
287
288
VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,int32_t OffsetCalibrationDataMicroMeter)289 VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
290 int32_t OffsetCalibrationDataMicroMeter)
291 {
292 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
293 int32_t cMaxOffsetMicroMeter = 511000;
294 int32_t cMinOffsetMicroMeter = -512000;
295 int16_t cOffsetRange = 4096;
296 uint32_t encodedOffsetVal;
297
298 LOG_FUNCTION_START("");
299
300 if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter)
301 OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter;
302 else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter)
303 OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter;
304
305 /* The offset register is 10.2 format and units are mm
306 * therefore conversion is applied by a division of
307 * 250.
308 */
309 if (OffsetCalibrationDataMicroMeter >= 0) {
310 encodedOffsetVal =
311 OffsetCalibrationDataMicroMeter/250;
312 } else {
313 encodedOffsetVal =
314 cOffsetRange +
315 OffsetCalibrationDataMicroMeter/250;
316 }
317
318 Status = VL53L0X_WrWord(Dev,
319 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
320 encodedOffsetVal);
321
322 LOG_FUNCTION_END(Status);
323 return Status;
324 }
325
VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,int32_t * pOffsetCalibrationDataMicroMeter)326 VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
327 int32_t *pOffsetCalibrationDataMicroMeter)
328 {
329 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
330 uint16_t RangeOffsetRegister;
331 int16_t cMaxOffset = 2047;
332 int16_t cOffsetRange = 4096;
333
334 /* Note that offset has 10.2 format */
335
336 Status = VL53L0X_RdWord(Dev,
337 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
338 &RangeOffsetRegister);
339
340 if (Status == VL53L0X_ERROR_NONE) {
341 RangeOffsetRegister = (RangeOffsetRegister & 0x0fff);
342
343 /* Apply 12 bit 2's compliment conversion */
344 if (RangeOffsetRegister > cMaxOffset)
345 *pOffsetCalibrationDataMicroMeter =
346 (int16_t)(RangeOffsetRegister - cOffsetRange)
347 * 250;
348 else
349 *pOffsetCalibrationDataMicroMeter =
350 (int16_t)RangeOffsetRegister * 250;
351
352 }
353
354 return Status;
355 }
356
357
VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev)358 VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev)
359 {
360 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
361 int32_t CorrectedOffsetMicroMeters;
362 int32_t CurrentOffsetMicroMeters;
363
364 /* if we run on this function we can read all the NVM info
365 * used by the API */
366 Status = VL53L0X_get_info_from_device(Dev, 7);
367
368 /* Read back current device offset */
369 if (Status == VL53L0X_ERROR_NONE) {
370 Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
371 &CurrentOffsetMicroMeters);
372 }
373
374 /* Apply Offset Adjustment derived from 400mm measurements */
375 if (Status == VL53L0X_ERROR_NONE) {
376
377 /* Store initial device offset */
378 PALDevDataSet(Dev, Part2PartOffsetNVMMicroMeter,
379 CurrentOffsetMicroMeters);
380
381 CorrectedOffsetMicroMeters = CurrentOffsetMicroMeters +
382 (int32_t)PALDevDataGet(Dev,
383 Part2PartOffsetAdjustmentNVMMicroMeter);
384
385 Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
386 CorrectedOffsetMicroMeters);
387
388 /* store current, adjusted offset */
389 if (Status == VL53L0X_ERROR_NONE) {
390 VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
391 CorrectedOffsetMicroMeters);
392 }
393 }
394
395 return Status;
396 }
397
get_next_good_spad(uint8_t goodSpadArray[],uint32_t size,uint32_t curr,int32_t * next)398 void get_next_good_spad(uint8_t goodSpadArray[], uint32_t size,
399 uint32_t curr, int32_t *next)
400 {
401 uint32_t startIndex;
402 uint32_t fineOffset;
403 uint32_t cSpadsPerByte = 8;
404 uint32_t coarseIndex;
405 uint32_t fineIndex;
406 uint8_t dataByte;
407 uint8_t success = 0;
408
409 /*
410 * Starting with the current good spad, loop through the array to find
411 * the next. i.e. the next bit set in the sequence.
412 *
413 * The coarse index is the byte index of the array and the fine index is
414 * the index of the bit within each byte.
415 */
416
417 *next = -1;
418
419 startIndex = curr / cSpadsPerByte;
420 fineOffset = curr % cSpadsPerByte;
421
422 for (coarseIndex = startIndex; ((coarseIndex < size) && !success);
423 coarseIndex++) {
424 fineIndex = 0;
425 dataByte = goodSpadArray[coarseIndex];
426
427 if (coarseIndex == startIndex) {
428 /* locate the bit position of the provided current
429 * spad bit before iterating */
430 dataByte >>= fineOffset;
431 fineIndex = fineOffset;
432 }
433
434 while (fineIndex < cSpadsPerByte) {
435 if ((dataByte & 0x1) == 1) {
436 success = 1;
437 *next = coarseIndex * cSpadsPerByte + fineIndex;
438 break;
439 }
440 dataByte >>= 1;
441 fineIndex++;
442 }
443 }
444 }
445
446
is_aperture(uint32_t spadIndex)447 uint8_t is_aperture(uint32_t spadIndex)
448 {
449 /*
450 * This function reports if a given spad index is an aperture SPAD by
451 * deriving the quadrant.
452 */
453 uint32_t quadrant;
454 uint8_t isAperture = 1;
455 quadrant = spadIndex >> 6;
456 if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0)
457 isAperture = 0;
458
459 return isAperture;
460 }
461
462
enable_spad_bit(uint8_t spadArray[],uint32_t size,uint32_t spadIndex)463 VL53L0X_Error enable_spad_bit(uint8_t spadArray[], uint32_t size,
464 uint32_t spadIndex)
465 {
466 VL53L0X_Error status = VL53L0X_ERROR_NONE;
467 uint32_t cSpadsPerByte = 8;
468 uint32_t coarseIndex;
469 uint32_t fineIndex;
470
471 coarseIndex = spadIndex / cSpadsPerByte;
472 fineIndex = spadIndex % cSpadsPerByte;
473 if (coarseIndex >= size)
474 status = VL53L0X_ERROR_REF_SPAD_INIT;
475 else
476 spadArray[coarseIndex] |= (1 << fineIndex);
477
478 return status;
479 }
480
count_enabled_spads(uint8_t spadArray[],uint32_t byteCount,uint32_t maxSpads,uint32_t * pTotalSpadsEnabled,uint8_t * pIsAperture)481 VL53L0X_Error count_enabled_spads(uint8_t spadArray[],
482 uint32_t byteCount, uint32_t maxSpads,
483 uint32_t *pTotalSpadsEnabled, uint8_t *pIsAperture)
484 {
485 VL53L0X_Error status = VL53L0X_ERROR_NONE;
486 uint32_t cSpadsPerByte = 8;
487 uint32_t lastByte;
488 uint32_t lastBit;
489 uint32_t byteIndex = 0;
490 uint32_t bitIndex = 0;
491 uint8_t tempByte;
492 uint8_t spadTypeIdentified = 0;
493
494 /* The entire array will not be used for spads, therefore the last
495 * byte and last bit is determined from the max spads value.
496 */
497
498 lastByte = maxSpads / cSpadsPerByte;
499 lastBit = maxSpads % cSpadsPerByte;
500
501 /* Check that the max spads value does not exceed the array bounds. */
502 if (lastByte >= byteCount)
503 status = VL53L0X_ERROR_REF_SPAD_INIT;
504
505 *pTotalSpadsEnabled = 0;
506
507 /* Count the bits enabled in the whole bytes */
508 for (byteIndex = 0; byteIndex <= (lastByte - 1); byteIndex++) {
509 tempByte = spadArray[byteIndex];
510
511 for (bitIndex = 0; bitIndex <= cSpadsPerByte; bitIndex++) {
512 if ((tempByte & 0x01) == 1) {
513 (*pTotalSpadsEnabled)++;
514
515 if (!spadTypeIdentified) {
516 *pIsAperture = 1;
517 if ((byteIndex < 2) && (bitIndex < 4))
518 *pIsAperture = 0;
519 spadTypeIdentified = 1;
520 }
521 }
522 tempByte >>= 1;
523 }
524 }
525
526 /* Count the number of bits enabled in the last byte accounting
527 * for the fact that not all bits in the byte may be used.
528 */
529 tempByte = spadArray[lastByte];
530
531 for (bitIndex = 0; bitIndex <= lastBit; bitIndex++) {
532 if ((tempByte & 0x01) == 1)
533 (*pTotalSpadsEnabled)++;
534 }
535
536 return status;
537 }
538
set_ref_spad_map(VL53L0X_DEV Dev,uint8_t * refSpadArray)539 VL53L0X_Error set_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
540 {
541 VL53L0X_Error status = VL53L0X_WriteMulti(Dev,
542 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
543 refSpadArray, 6);
544 return status;
545 }
546
get_ref_spad_map(VL53L0X_DEV Dev,uint8_t * refSpadArray)547 VL53L0X_Error get_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
548 {
549 VL53L0X_Error status = VL53L0X_ReadMulti(Dev,
550 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
551 refSpadArray,
552 6);
553 return status;
554 }
555
enable_ref_spads(VL53L0X_DEV Dev,uint8_t apertureSpads,uint8_t goodSpadArray[],uint8_t spadArray[],uint32_t size,uint32_t start,uint32_t offset,uint32_t spadCount,uint32_t * lastSpad)556 VL53L0X_Error enable_ref_spads(VL53L0X_DEV Dev,
557 uint8_t apertureSpads,
558 uint8_t goodSpadArray[],
559 uint8_t spadArray[],
560 uint32_t size,
561 uint32_t start,
562 uint32_t offset,
563 uint32_t spadCount,
564 uint32_t *lastSpad)
565 {
566 VL53L0X_Error status = VL53L0X_ERROR_NONE;
567 uint32_t index;
568 uint32_t i;
569 int32_t nextGoodSpad = offset;
570 uint32_t currentSpad;
571 uint8_t checkSpadArray[6];
572
573 /*
574 * This function takes in a spad array which may or may not have SPADS
575 * already enabled and appends from a given offset a requested number
576 * of new SPAD enables. The 'good spad map' is applied to
577 * determine the next SPADs to enable.
578 *
579 * This function applies to only aperture or only non-aperture spads.
580 * Checks are performed to ensure this.
581 */
582
583 currentSpad = offset;
584 for (index = 0; index < spadCount; index++) {
585 get_next_good_spad(goodSpadArray, size, currentSpad,
586 &nextGoodSpad);
587
588 if (nextGoodSpad == -1) {
589 status = VL53L0X_ERROR_REF_SPAD_INIT;
590 break;
591 }
592
593 /* Confirm that the next good SPAD is non-aperture */
594 if (is_aperture(start + nextGoodSpad) != apertureSpads) {
595 /* if we can't get the required number of good aperture
596 * spads from the current quadrant then this is an error
597 */
598 status = VL53L0X_ERROR_REF_SPAD_INIT;
599 break;
600 }
601 currentSpad = (uint32_t)nextGoodSpad;
602 enable_spad_bit(spadArray, size, currentSpad);
603 currentSpad++;
604 }
605 *lastSpad = currentSpad;
606
607 if (status == VL53L0X_ERROR_NONE)
608 status = set_ref_spad_map(Dev, spadArray);
609
610
611 if (status == VL53L0X_ERROR_NONE) {
612 status = get_ref_spad_map(Dev, checkSpadArray);
613
614 i = 0;
615
616 /* Compare spad maps. If not equal report error. */
617 while (i < size) {
618 if (spadArray[i] != checkSpadArray[i]) {
619 status = VL53L0X_ERROR_REF_SPAD_INIT;
620 break;
621 }
622 i++;
623 }
624 }
625 return status;
626 }
627
628
perform_ref_signal_measurement(VL53L0X_DEV Dev,uint16_t * refSignalRate)629 VL53L0X_Error perform_ref_signal_measurement(VL53L0X_DEV Dev,
630 uint16_t *refSignalRate)
631 {
632 VL53L0X_Error status = VL53L0X_ERROR_NONE;
633 VL53L0X_RangingMeasurementData_t rangingMeasurementData;
634
635 uint8_t SequenceConfig = 0;
636
637 /* store the value of the sequence config,
638 * this will be reset before the end of the function
639 */
640
641 SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
642
643 /*
644 * This function performs a reference signal rate measurement.
645 */
646 if (status == VL53L0X_ERROR_NONE)
647 status = VL53L0X_WrByte(Dev,
648 VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
649
650 if (status == VL53L0X_ERROR_NONE)
651 status = VL53L0X_PerformSingleRangingMeasurement(Dev,
652 &rangingMeasurementData);
653
654 if (status == VL53L0X_ERROR_NONE)
655 status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
656
657 if (status == VL53L0X_ERROR_NONE)
658 status = VL53L0X_RdWord(Dev,
659 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
660 refSignalRate);
661
662 if (status == VL53L0X_ERROR_NONE)
663 status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
664
665 if (status == VL53L0X_ERROR_NONE) {
666 /* restore the previous Sequence Config */
667 status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
668 SequenceConfig);
669 if (status == VL53L0X_ERROR_NONE)
670 PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
671 }
672
673 return status;
674 }
675
VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,uint32_t * refSpadCount,uint8_t * isApertureSpads)676 VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
677 uint32_t *refSpadCount,
678 uint8_t *isApertureSpads)
679 {
680 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
681 uint8_t lastSpadArray[6];
682 uint8_t startSelect = 0xB4;
683 uint32_t minimumSpadCount = 3;
684 uint32_t maxSpadCount = 44;
685 uint32_t currentSpadIndex = 0;
686 uint32_t lastSpadIndex = 0;
687 int32_t nextGoodSpad = 0;
688 uint16_t targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
689 uint16_t peakSignalRateRef;
690 uint32_t needAptSpads = 0;
691 uint32_t index = 0;
692 uint32_t spadArraySize = 6;
693 uint32_t signalRateDiff = 0;
694 uint32_t lastSignalRateDiff = 0;
695 uint8_t complete = 0;
696 uint8_t VhvSettings = 0;
697 uint8_t PhaseCal = 0;
698 uint32_t refSpadCount_int = 0;
699 uint8_t isApertureSpads_int = 0;
700
701 /*
702 * The reference SPAD initialization procedure determines the minimum
703 * amount of reference spads to be enables to achieve a target reference
704 * signal rate and should be performed once during initialization.
705 *
706 * Either aperture or non-aperture spads are applied but never both.
707 * Firstly non-aperture spads are set, begining with 5 spads, and
708 * increased one spad at a time until the closest measurement to the
709 * target rate is achieved.
710 *
711 * If the target rate is exceeded when 5 non-aperture spads are enabled,
712 * initialization is performed instead with aperture spads.
713 *
714 * When setting spads, a 'Good Spad Map' is applied.
715 *
716 * This procedure operates within a SPAD window of interest of a maximum
717 * 44 spads.
718 * The start point is currently fixed to 180, which lies towards the end
719 * of the non-aperture quadrant and runs in to the adjacent aperture
720 * quadrant.
721 */
722
723
724 targetRefRate = PALDevDataGet(Dev, targetRefRate);
725
726 /*
727 * Initialize Spad arrays.
728 * Currently the good spad map is initialised to 'All good'.
729 * This is a short term implementation. The good spad map will be
730 * provided as an input.
731 * Note that there are 6 bytes. Only the first 44 bits will be used to
732 * represent spads.
733 */
734 for (index = 0; index < spadArraySize; index++)
735 Dev->Data.SpadData.RefSpadEnables[index] = 0;
736
737
738 Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
739
740 if (Status == VL53L0X_ERROR_NONE)
741 Status = VL53L0X_WrByte(Dev,
742 VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
743
744 if (Status == VL53L0X_ERROR_NONE)
745 Status = VL53L0X_WrByte(Dev,
746 VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
747
748 if (Status == VL53L0X_ERROR_NONE)
749 Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
750
751 if (Status == VL53L0X_ERROR_NONE)
752 Status = VL53L0X_WrByte(Dev,
753 VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
754 startSelect);
755
756
757 if (Status == VL53L0X_ERROR_NONE)
758 Status = VL53L0X_WrByte(Dev,
759 VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
760
761 /* Perform ref calibration */
762 if (Status == VL53L0X_ERROR_NONE)
763 Status = VL53L0X_perform_ref_calibration(Dev, &VhvSettings,
764 &PhaseCal, 0);
765
766 if (Status == VL53L0X_ERROR_NONE) {
767 /* Enable Minimum NON-APERTURE Spads */
768 currentSpadIndex = 0;
769 lastSpadIndex = currentSpadIndex;
770 needAptSpads = 0;
771 Status = enable_ref_spads(Dev,
772 needAptSpads,
773 Dev->Data.SpadData.RefGoodSpadMap,
774 Dev->Data.SpadData.RefSpadEnables,
775 spadArraySize,
776 startSelect,
777 currentSpadIndex,
778 minimumSpadCount,
779 &lastSpadIndex);
780 }
781
782 if (Status == VL53L0X_ERROR_NONE) {
783 currentSpadIndex = lastSpadIndex;
784
785 Status = perform_ref_signal_measurement(Dev,
786 &peakSignalRateRef);
787 if ((Status == VL53L0X_ERROR_NONE) &&
788 (peakSignalRateRef > targetRefRate)) {
789 /* Signal rate measurement too high,
790 * switch to APERTURE SPADs */
791
792 for (index = 0; index < spadArraySize; index++)
793 Dev->Data.SpadData.RefSpadEnables[index] = 0;
794
795
796 /* Increment to the first APERTURE spad */
797 while ((is_aperture(startSelect + currentSpadIndex)
798 == 0) && (currentSpadIndex < maxSpadCount)) {
799 currentSpadIndex++;
800 }
801
802 needAptSpads = 1;
803
804 Status = enable_ref_spads(Dev,
805 needAptSpads,
806 Dev->Data.SpadData.RefGoodSpadMap,
807 Dev->Data.SpadData.RefSpadEnables,
808 spadArraySize,
809 startSelect,
810 currentSpadIndex,
811 minimumSpadCount,
812 &lastSpadIndex);
813
814 if (Status == VL53L0X_ERROR_NONE) {
815 currentSpadIndex = lastSpadIndex;
816 Status = perform_ref_signal_measurement(Dev,
817 &peakSignalRateRef);
818
819 if ((Status == VL53L0X_ERROR_NONE) &&
820 (peakSignalRateRef > targetRefRate)) {
821 /* Signal rate still too high after
822 * setting the minimum number of
823 * APERTURE spads. Can do no more
824 * therefore set the min number of
825 * aperture spads as the result.
826 */
827 isApertureSpads_int = 1;
828 refSpadCount_int = minimumSpadCount;
829 }
830 }
831 } else {
832 needAptSpads = 0;
833 }
834 }
835
836 if ((Status == VL53L0X_ERROR_NONE) &&
837 (peakSignalRateRef < targetRefRate)) {
838 /* At this point, the minimum number of either aperture
839 * or non-aperture spads have been set. Proceed to add
840 * spads and perform measurements until the target
841 * reference is reached.
842 */
843 isApertureSpads_int = needAptSpads;
844 refSpadCount_int = minimumSpadCount;
845
846 memcpy(lastSpadArray, Dev->Data.SpadData.RefSpadEnables,
847 spadArraySize);
848 lastSignalRateDiff = abs(peakSignalRateRef -
849 targetRefRate);
850 complete = 0;
851
852 while (!complete) {
853 get_next_good_spad(
854 Dev->Data.SpadData.RefGoodSpadMap,
855 spadArraySize, currentSpadIndex,
856 &nextGoodSpad);
857
858 if (nextGoodSpad == -1) {
859 Status = VL53L0X_ERROR_REF_SPAD_INIT;
860 break;
861 }
862
863 /* Cannot combine Aperture and Non-Aperture spads, so
864 * ensure the current spad is of the correct type.
865 */
866 if (is_aperture((uint32_t)startSelect + nextGoodSpad) !=
867 needAptSpads) {
868 /* At this point we have enabled the maximum
869 * number of Aperture spads.
870 */
871 complete = 1;
872 break;
873 }
874
875 (refSpadCount_int)++;
876
877 currentSpadIndex = nextGoodSpad;
878 Status = enable_spad_bit(
879 Dev->Data.SpadData.RefSpadEnables,
880 spadArraySize, currentSpadIndex);
881
882 if (Status == VL53L0X_ERROR_NONE) {
883 currentSpadIndex++;
884 /* Proceed to apply the additional spad and
885 * perform measurement. */
886 Status = set_ref_spad_map(Dev,
887 Dev->Data.SpadData.RefSpadEnables);
888 }
889
890 if (Status != VL53L0X_ERROR_NONE)
891 break;
892
893 Status = perform_ref_signal_measurement(Dev,
894 &peakSignalRateRef);
895
896 if (Status != VL53L0X_ERROR_NONE)
897 break;
898
899 signalRateDiff = abs(peakSignalRateRef - targetRefRate);
900
901 if (peakSignalRateRef > targetRefRate) {
902 /* Select the spad map that provides the
903 * measurement closest to the target rate,
904 * either above or below it.
905 */
906 if (signalRateDiff > lastSignalRateDiff) {
907 /* Previous spad map produced a closer
908 * measurement, so choose this. */
909 Status = set_ref_spad_map(Dev,
910 lastSpadArray);
911 memcpy(
912 Dev->Data.SpadData.RefSpadEnables,
913 lastSpadArray, spadArraySize);
914
915 (refSpadCount_int)--;
916 }
917 complete = 1;
918 } else {
919 /* Continue to add spads */
920 lastSignalRateDiff = signalRateDiff;
921 memcpy(lastSpadArray,
922 Dev->Data.SpadData.RefSpadEnables,
923 spadArraySize);
924 }
925
926 } /* while */
927 }
928
929 if (Status == VL53L0X_ERROR_NONE) {
930 *refSpadCount = refSpadCount_int;
931 *isApertureSpads = isApertureSpads_int;
932
933 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
934 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
935 ReferenceSpadCount, (uint8_t)(*refSpadCount));
936 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
937 ReferenceSpadType, *isApertureSpads);
938 }
939
940 return Status;
941 }
942
VL53L0X_set_reference_spads(VL53L0X_DEV Dev,uint32_t count,uint8_t isApertureSpads)943 VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
944 uint32_t count, uint8_t isApertureSpads)
945 {
946 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
947 uint32_t currentSpadIndex = 0;
948 uint8_t startSelect = 0xB4;
949 uint32_t spadArraySize = 6;
950 uint32_t maxSpadCount = 44;
951 uint32_t lastSpadIndex;
952 uint32_t index;
953
954 /*
955 * This function applies a requested number of reference spads, either
956 * aperture or
957 * non-aperture, as requested.
958 * The good spad map will be applied.
959 */
960
961 Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
962
963 if (Status == VL53L0X_ERROR_NONE)
964 Status = VL53L0X_WrByte(Dev,
965 VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
966
967 if (Status == VL53L0X_ERROR_NONE)
968 Status = VL53L0X_WrByte(Dev,
969 VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
970
971 if (Status == VL53L0X_ERROR_NONE)
972 Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
973
974 if (Status == VL53L0X_ERROR_NONE)
975 Status = VL53L0X_WrByte(Dev,
976 VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
977 startSelect);
978
979 for (index = 0; index < spadArraySize; index++)
980 Dev->Data.SpadData.RefSpadEnables[index] = 0;
981
982 if (isApertureSpads) {
983 /* Increment to the first APERTURE spad */
984 while ((is_aperture(startSelect + currentSpadIndex) == 0) &&
985 (currentSpadIndex < maxSpadCount)) {
986 currentSpadIndex++;
987 }
988 }
989 Status = enable_ref_spads(Dev,
990 isApertureSpads,
991 Dev->Data.SpadData.RefGoodSpadMap,
992 Dev->Data.SpadData.RefSpadEnables,
993 spadArraySize,
994 startSelect,
995 currentSpadIndex,
996 count,
997 &lastSpadIndex);
998
999 if (Status == VL53L0X_ERROR_NONE) {
1000 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
1001 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
1002 ReferenceSpadCount, (uint8_t)(count));
1003 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
1004 ReferenceSpadType, isApertureSpads);
1005 }
1006
1007 return Status;
1008 }
1009
VL53L0X_get_reference_spads(VL53L0X_DEV Dev,uint32_t * pSpadCount,uint8_t * pIsApertureSpads)1010 VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev,
1011 uint32_t *pSpadCount, uint8_t *pIsApertureSpads)
1012 {
1013 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1014 uint8_t refSpadsInitialised;
1015 uint8_t refSpadArray[6];
1016 uint32_t cMaxSpadCount = 44;
1017 uint32_t cSpadArraySize = 6;
1018 uint32_t spadsEnabled;
1019 uint8_t isApertureSpads = 0;
1020
1021 refSpadsInitialised = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
1022 RefSpadsInitialised);
1023
1024 if (refSpadsInitialised == 1) {
1025
1026 *pSpadCount = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
1027 ReferenceSpadCount);
1028 *pIsApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
1029 ReferenceSpadType);
1030 } else {
1031
1032 /* obtain spad info from device.*/
1033 Status = get_ref_spad_map(Dev, refSpadArray);
1034
1035 if (Status == VL53L0X_ERROR_NONE) {
1036 /* count enabled spads within spad map array and
1037 * determine if Aperture or Non-Aperture.
1038 */
1039 Status = count_enabled_spads(refSpadArray,
1040 cSpadArraySize,
1041 cMaxSpadCount,
1042 &spadsEnabled,
1043 &isApertureSpads);
1044
1045 if (Status == VL53L0X_ERROR_NONE) {
1046
1047 *pSpadCount = spadsEnabled;
1048 *pIsApertureSpads = isApertureSpads;
1049
1050 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
1051 RefSpadsInitialised, 1);
1052 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
1053 ReferenceSpadCount,
1054 (uint8_t)spadsEnabled);
1055 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
1056 ReferenceSpadType, isApertureSpads);
1057 }
1058 }
1059 }
1060
1061 return Status;
1062 }
1063
1064
VL53L0X_perform_single_ref_calibration(VL53L0X_DEV Dev,uint8_t vhv_init_byte)1065 VL53L0X_Error VL53L0X_perform_single_ref_calibration(VL53L0X_DEV Dev,
1066 uint8_t vhv_init_byte)
1067 {
1068 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1069
1070 if (Status == VL53L0X_ERROR_NONE)
1071 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
1072 VL53L0X_REG_SYSRANGE_MODE_START_STOP |
1073 vhv_init_byte);
1074
1075 if (Status == VL53L0X_ERROR_NONE)
1076 Status = VL53L0X_measurement_poll_for_completion(Dev);
1077
1078 if (Status == VL53L0X_ERROR_NONE)
1079 Status = VL53L0X_ClearInterruptMask(Dev, 0);
1080
1081 if (Status == VL53L0X_ERROR_NONE)
1082 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x00);
1083
1084 return Status;
1085 }
1086
1087
VL53L0X_ref_calibration_io(VL53L0X_DEV Dev,uint8_t read_not_write,uint8_t VhvSettings,uint8_t PhaseCal,uint8_t * pVhvSettings,uint8_t * pPhaseCal,const uint8_t vhv_enable,const uint8_t phase_enable)1088 VL53L0X_Error VL53L0X_ref_calibration_io(VL53L0X_DEV Dev, uint8_t read_not_write,
1089 uint8_t VhvSettings, uint8_t PhaseCal,
1090 uint8_t *pVhvSettings, uint8_t *pPhaseCal,
1091 const uint8_t vhv_enable, const uint8_t phase_enable)
1092 {
1093 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1094 uint8_t PhaseCalint = 0;
1095
1096 /* Read VHV from device */
1097 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
1098 Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
1099 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
1100
1101 if (read_not_write) {
1102 if (vhv_enable)
1103 Status |= VL53L0X_RdByte(Dev, 0xCB, pVhvSettings);
1104 if (phase_enable)
1105 Status |= VL53L0X_RdByte(Dev, 0xEE, &PhaseCalint);
1106 } else {
1107 if (vhv_enable)
1108 Status |= VL53L0X_WrByte(Dev, 0xCB, VhvSettings);
1109 if (phase_enable)
1110 Status |= VL53L0X_UpdateByte(Dev, 0xEE, 0x80, PhaseCal);
1111 }
1112
1113 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
1114 Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
1115 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
1116
1117 *pPhaseCal = (uint8_t)(PhaseCalint&0xEF);
1118
1119 return Status;
1120 }
1121
1122
VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev,uint8_t * pVhvSettings,const uint8_t get_data_enable,const uint8_t restore_config)1123 VL53L0X_Error VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev,
1124 uint8_t *pVhvSettings, const uint8_t get_data_enable,
1125 const uint8_t restore_config)
1126 {
1127 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1128 uint8_t SequenceConfig = 0;
1129 uint8_t VhvSettings = 0;
1130 uint8_t PhaseCal = 0;
1131 uint8_t PhaseCalInt = 0;
1132
1133 /* store the value of the sequence config,
1134 * this will be reset before the end of the function
1135 */
1136
1137 if (restore_config)
1138 SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
1139
1140 /* Run VHV */
1141 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
1142
1143 if (Status == VL53L0X_ERROR_NONE)
1144 Status = VL53L0X_perform_single_ref_calibration(Dev, 0x40);
1145
1146 /* Read VHV from device */
1147 if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
1148 Status = VL53L0X_ref_calibration_io(Dev, 1,
1149 VhvSettings, PhaseCal, /* Not used here */
1150 pVhvSettings, &PhaseCalInt,
1151 1, 0);
1152 } else
1153 *pVhvSettings = 0;
1154
1155
1156 if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
1157 /* restore the previous Sequence Config */
1158 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
1159 SequenceConfig);
1160 if (Status == VL53L0X_ERROR_NONE)
1161 PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
1162
1163 }
1164
1165 return Status;
1166 }
1167
VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,uint8_t * pPhaseCal,const uint8_t get_data_enable,const uint8_t restore_config)1168 VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
1169 uint8_t *pPhaseCal, const uint8_t get_data_enable,
1170 const uint8_t restore_config)
1171 {
1172 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1173 uint8_t SequenceConfig = 0;
1174 uint8_t VhvSettings = 0;
1175 uint8_t PhaseCal = 0;
1176 uint8_t VhvSettingsint;
1177
1178 /* store the value of the sequence config,
1179 * this will be reset before the end of the function
1180 */
1181
1182 if (restore_config)
1183 SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
1184
1185 /* Run PhaseCal */
1186 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
1187
1188 if (Status == VL53L0X_ERROR_NONE)
1189 Status = VL53L0X_perform_single_ref_calibration(Dev, 0x0);
1190
1191 /* Read PhaseCal from device */
1192 if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
1193 Status = VL53L0X_ref_calibration_io(Dev, 1,
1194 VhvSettings, PhaseCal, /* Not used here */
1195 &VhvSettingsint, pPhaseCal,
1196 0, 1);
1197 } else
1198 *pPhaseCal = 0;
1199
1200
1201 if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
1202 /* restore the previous Sequence Config */
1203 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
1204 SequenceConfig);
1205 if (Status == VL53L0X_ERROR_NONE)
1206 PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
1207
1208 }
1209
1210 return Status;
1211 }
1212
VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,uint8_t * pVhvSettings,uint8_t * pPhaseCal,uint8_t get_data_enable)1213 VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
1214 uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable)
1215 {
1216 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1217 uint8_t SequenceConfig = 0;
1218
1219 /* store the value of the sequence config,
1220 * this will be reset before the end of the function
1221 */
1222
1223 SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
1224
1225 /* In the following function we don't save the config to optimize
1226 * writes on device. Config is saved and restored only once. */
1227 Status = VL53L0X_perform_vhv_calibration(
1228 Dev, pVhvSettings, get_data_enable, 0);
1229
1230
1231 if (Status == VL53L0X_ERROR_NONE)
1232 Status = VL53L0X_perform_phase_calibration(
1233 Dev, pPhaseCal, get_data_enable, 0);
1234
1235
1236 if (Status == VL53L0X_ERROR_NONE) {
1237 /* restore the previous Sequence Config */
1238 Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
1239 SequenceConfig);
1240 if (Status == VL53L0X_ERROR_NONE)
1241 PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
1242
1243 }
1244
1245 return Status;
1246 }
1247
VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,uint8_t VhvSettings,uint8_t PhaseCal)1248 VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,
1249 uint8_t VhvSettings, uint8_t PhaseCal)
1250 {
1251 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1252 uint8_t pVhvSettings;
1253 uint8_t pPhaseCal;
1254
1255 Status = VL53L0X_ref_calibration_io(Dev, 0,
1256 VhvSettings, PhaseCal,
1257 &pVhvSettings, &pPhaseCal,
1258 1, 1);
1259
1260 return Status;
1261 }
1262
VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,uint8_t * pVhvSettings,uint8_t * pPhaseCal)1263 VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,
1264 uint8_t *pVhvSettings, uint8_t *pPhaseCal)
1265 {
1266 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
1267 uint8_t VhvSettings = 0;
1268 uint8_t PhaseCal = 0;
1269
1270 Status = VL53L0X_ref_calibration_io(Dev, 1,
1271 VhvSettings, PhaseCal,
1272 pVhvSettings, pPhaseCal,
1273 1, 1);
1274
1275 return Status;
1276 }
1277
