amiro-lld / include / VL53L0X / v1 / Api_vl53l0x / core / src / vl53l0x_api_calibration.c @ 6ebebd4d
History | View | Annotate | Download (40.8 KB)
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
} |