Statistics
| Branch: | Tag: | Revision:

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
}