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