vl53l0x_api_calibration.cpp

00001 /*******************************************************************************
00002  Copyright � 2016, STMicroelectronics International N.V.
00003  All rights reserved.
00004 
00005  Redistribution and use in source and binary forms, with or without
00006  modification, are permitted provided that the following conditions are met:
00007  * Redistributions of source code must retain the above copyright
00008  notice, this list of conditions and the following disclaimer.
00009  * Redistributions in binary form must reproduce the above copyright
00010  notice, this list of conditions and the following disclaimer in the
00011  documentation and/or other materials provided with the distribution.
00012  * Neither the name of STMicroelectronics nor the
00013  names of its contributors may be used to endorse or promote products
00014  derived from this software without specific prior written permission.
00015 
00016  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018  WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
00019  NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
00020  IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
00021  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027  ******************************************************************************/
00028 
00029 #include "vl53l0x_api.h"
00030 #include "vl53l0x_api_core.h"
00031 #include "vl53l0x_api_calibration.h"
00032 
00033 #ifndef __KERNEL__
00034 #include <stdlib.h>
00035 #endif
00036 
00037 
00038 #define REF_ARRAY_SPAD_0  0
00039 #define REF_ARRAY_SPAD_5  5
00040 #define REF_ARRAY_SPAD_10 10
00041 
00042 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5,
00043         REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 };
00044 
00045 VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,
00046             FixPoint1616_t XTalkCalDistance,
00047             FixPoint1616_t *pXTalkCompensationRateMegaCps)
00048 {
00049     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00050     uint16_t sum_ranging = 0;
00051     uint16_t sum_spads = 0;
00052     FixPoint1616_t sum_signalRate = 0;
00053     FixPoint1616_t total_count = 0;
00054     uint8_t xtalk_meas = 0;
00055     VL53L0X_RangingMeasurementData_t RangingMeasurementData;
00056     FixPoint1616_t xTalkStoredMeanSignalRate;
00057     FixPoint1616_t xTalkStoredMeanRange;
00058     FixPoint1616_t xTalkStoredMeanRtnSpads;
00059     uint32_t signalXTalkTotalPerSpad;
00060     uint32_t xTalkStoredMeanRtnSpadsAsInt;
00061     uint32_t xTalkCalDistanceAsInt;
00062     FixPoint1616_t XTalkCompensationRateMegaCps;
00063 
00064     if (XTalkCalDistance <= 0)
00065         Status = VL53L0X_ERROR_INVALID_PARAMS;
00066 
00067     /* Disable the XTalk compensation */
00068     if (Status == VL53L0X_ERROR_NONE)
00069         Status = VL53L0X_SetXTalkCompensationEnable(Dev, 0);
00070 
00071     /* Disable the RIT */
00072     if (Status == VL53L0X_ERROR_NONE) {
00073         Status = VL53L0X_SetLimitCheckEnable(Dev,
00074                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
00075     }
00076 
00077     /* Perform 50 measurements and compute the averages */
00078     if (Status == VL53L0X_ERROR_NONE) {
00079         sum_ranging = 0;
00080         sum_spads = 0;
00081         sum_signalRate = 0;
00082         total_count = 0;
00083         for (xtalk_meas = 0; xtalk_meas < 50; xtalk_meas++) {
00084             Status = VL53L0X_PerformSingleRangingMeasurement(Dev,
00085                 &RangingMeasurementData);
00086 
00087             if (Status != VL53L0X_ERROR_NONE)
00088                 break;
00089 
00090             /* The range is valid when RangeStatus = 0 */
00091             if (RangingMeasurementData.RangeStatus == 0) {
00092                 sum_ranging = sum_ranging +
00093                     RangingMeasurementData.RangeMilliMeter;
00094                 sum_signalRate = sum_signalRate +
00095                 RangingMeasurementData.SignalRateRtnMegaCps;
00096                 sum_spads = sum_spads +
00097                 RangingMeasurementData.EffectiveSpadRtnCount
00098                     / 256;
00099                 total_count = total_count + 1;
00100             }
00101         }
00102 
00103         /* no valid values found */
00104         if (total_count == 0)
00105             Status = VL53L0X_ERROR_RANGE_ERROR;
00106 
00107     }
00108 
00109 
00110     if (Status == VL53L0X_ERROR_NONE) {
00111         /* FixPoint1616_t / uint16_t = FixPoint1616_t */
00112         xTalkStoredMeanSignalRate = sum_signalRate / total_count;
00113         xTalkStoredMeanRange = (FixPoint1616_t)((uint32_t)(
00114             sum_ranging << 16) / total_count);
00115         xTalkStoredMeanRtnSpads = (FixPoint1616_t)((uint32_t)(
00116             sum_spads << 16) / total_count);
00117 
00118         /* Round Mean Spads to Whole Number.
00119          * Typically the calculated mean SPAD count is a whole number
00120          * or very close to a whole
00121          * number, therefore any truncation will not result in a
00122          * significant loss in accuracy.
00123          * Also, for a grey target at a typical distance of around
00124          * 400mm, around 220 SPADs will
00125          * be enabled, therefore, any truncation will result in a loss
00126          * of accuracy of less than
00127          * 0.5%.
00128          */
00129         xTalkStoredMeanRtnSpadsAsInt = (xTalkStoredMeanRtnSpads +
00130             0x8000) >> 16;
00131 
00132         /* Round Cal Distance to Whole Number.
00133          * Note that the cal distance is in mm, therefore no resolution
00134          * is lost.*/
00135          xTalkCalDistanceAsInt = (XTalkCalDistance + 0x8000) >> 16;
00136 
00137         if (xTalkStoredMeanRtnSpadsAsInt == 0 ||
00138            xTalkCalDistanceAsInt == 0 ||
00139            xTalkStoredMeanRange >= XTalkCalDistance) {
00140             XTalkCompensationRateMegaCps = 0;
00141         } else {
00142             /* Round Cal Distance to Whole Number.
00143                Note that the cal distance is in mm, therefore no
00144                resolution is lost.*/
00145             xTalkCalDistanceAsInt = (XTalkCalDistance +
00146                 0x8000) >> 16;
00147 
00148             /* Apply division by mean spad count early in the
00149              * calculation to keep the numbers small.
00150              * This ensures we can maintain a 32bit calculation.
00151              * Fixed1616 / int := Fixed1616 */
00152             signalXTalkTotalPerSpad = (xTalkStoredMeanSignalRate) /
00153                 xTalkStoredMeanRtnSpadsAsInt;
00154 
00155             /* Complete the calculation for total Signal XTalk per
00156              * SPAD
00157              * Fixed1616 * (Fixed1616 - Fixed1616/int) :=
00158              * (2^16 * Fixed1616)
00159              */
00160             signalXTalkTotalPerSpad *= ((1 << 16) -
00161                 (xTalkStoredMeanRange / xTalkCalDistanceAsInt));
00162 
00163             /* Round from 2^16 * Fixed1616, to Fixed1616. */
00164             XTalkCompensationRateMegaCps = (signalXTalkTotalPerSpad
00165                 + 0x8000) >> 16;
00166         }
00167 
00168         *pXTalkCompensationRateMegaCps = XTalkCompensationRateMegaCps;
00169 
00170         /* Enable the XTalk compensation */
00171         if (Status == VL53L0X_ERROR_NONE)
00172             Status = VL53L0X_SetXTalkCompensationEnable(Dev, 1);
00173 
00174         /* Enable the XTalk compensation */
00175         if (Status == VL53L0X_ERROR_NONE)
00176             Status = VL53L0X_SetXTalkCompensationRateMegaCps(Dev,
00177                     XTalkCompensationRateMegaCps);
00178 
00179     }
00180 
00181     return Status;
00182 }
00183 
00184 VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,
00185             FixPoint1616_t CalDistanceMilliMeter,
00186             int32_t *pOffsetMicroMeter)
00187 {
00188     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00189     uint16_t sum_ranging = 0;
00190     FixPoint1616_t total_count = 0;
00191     VL53L0X_RangingMeasurementData_t RangingMeasurementData;
00192     FixPoint1616_t StoredMeanRange;
00193     uint32_t StoredMeanRangeAsInt;
00194     uint32_t CalDistanceAsInt_mm;
00195     uint8_t SequenceStepEnabled;
00196     int meas = 0;
00197 
00198     if (CalDistanceMilliMeter <= 0)
00199         Status = VL53L0X_ERROR_INVALID_PARAMS;
00200 
00201     if (Status == VL53L0X_ERROR_NONE)
00202         Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev, 0);
00203 
00204 
00205     /* Get the value of the TCC */
00206     if (Status == VL53L0X_ERROR_NONE)
00207         Status = VL53L0X_GetSequenceStepEnable(Dev,
00208                 VL53L0X_SEQUENCESTEP_TCC, &SequenceStepEnabled);
00209 
00210 
00211     /* Disable the TCC */
00212     if (Status == VL53L0X_ERROR_NONE)
00213         Status = VL53L0X_SetSequenceStepEnable(Dev,
00214                 VL53L0X_SEQUENCESTEP_TCC, 0);
00215 
00216 
00217     /* Disable the RIT */
00218     if (Status == VL53L0X_ERROR_NONE)
00219         Status = VL53L0X_SetLimitCheckEnable(Dev,
00220                 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
00221 
00222     /* Perform 50 measurements and compute the averages */
00223     if (Status == VL53L0X_ERROR_NONE) {
00224         sum_ranging = 0;
00225         total_count = 0;
00226         for (meas = 0; meas < 50; meas++) {
00227             Status = VL53L0X_PerformSingleRangingMeasurement(Dev,
00228                     &RangingMeasurementData);
00229 
00230             if (Status != VL53L0X_ERROR_NONE)
00231                 break;
00232 
00233             /* The range is valid when RangeStatus = 0 */
00234             if (RangingMeasurementData.RangeStatus == 0) {
00235                 sum_ranging = sum_ranging +
00236                     RangingMeasurementData.RangeMilliMeter;
00237                 total_count = total_count + 1;
00238             }
00239         }
00240 
00241         /* no valid values found */
00242         if (total_count == 0)
00243             Status = VL53L0X_ERROR_RANGE_ERROR;
00244     }
00245 
00246 
00247     if (Status == VL53L0X_ERROR_NONE) {
00248         /* FixPoint1616_t / uint16_t = FixPoint1616_t */
00249         StoredMeanRange = (FixPoint1616_t)((uint32_t)(sum_ranging << 16)
00250             / total_count);
00251 
00252         StoredMeanRangeAsInt = (StoredMeanRange + 0x8000) >> 16;
00253 
00254         /* Round Cal Distance to Whole Number.
00255          * Note that the cal distance is in mm, therefore no resolution
00256          * is lost.*/
00257          CalDistanceAsInt_mm = (CalDistanceMilliMeter + 0x8000) >> 16;
00258 
00259          *pOffsetMicroMeter = (CalDistanceAsInt_mm -
00260                  StoredMeanRangeAsInt) * 1000;
00261 
00262         /* Apply the calculated offset */
00263         if (Status == VL53L0X_ERROR_NONE) {
00264             VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
00265                     *pOffsetMicroMeter);
00266             Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
00267                     *pOffsetMicroMeter);
00268         }
00269 
00270     }
00271 
00272     /* Restore the TCC */
00273     if (Status == VL53L0X_ERROR_NONE) {
00274         if (SequenceStepEnabled != 0)
00275             Status = VL53L0X_SetSequenceStepEnable(Dev,
00276                     VL53L0X_SEQUENCESTEP_TCC, 1);
00277     }
00278 
00279     return Status;
00280 }
00281 
00282 
00283 VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
00284         int32_t OffsetCalibrationDataMicroMeter)
00285 {
00286     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00287     int32_t cMaxOffsetMicroMeter = 511000;
00288     int32_t cMinOffsetMicroMeter = -512000;
00289     int16_t cOffsetRange = 4096;
00290     uint32_t encodedOffsetVal;
00291 
00292 
00293     if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter)
00294         OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter;
00295     else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter)
00296         OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter;
00297 
00298     /* The offset register is 10.2 format and units are mm
00299      * therefore conversion is applied by a division of
00300      * 250.
00301      */
00302     if (OffsetCalibrationDataMicroMeter >= 0) {
00303         encodedOffsetVal =
00304             OffsetCalibrationDataMicroMeter/250;
00305     } else {
00306         encodedOffsetVal =
00307             cOffsetRange +
00308             OffsetCalibrationDataMicroMeter/250;
00309     }
00310 
00311     Status = VL53L0X_WrWord(Dev,
00312         VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
00313         encodedOffsetVal);
00314 
00315     return Status;
00316 }
00317 
00318 VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
00319         int32_t *pOffsetCalibrationDataMicroMeter)
00320 {
00321     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00322     uint16_t RangeOffsetRegister;
00323     int16_t cMaxOffset = 2047;
00324     int16_t cOffsetRange = 4096;
00325 
00326     /* Note that offset has 10.2 format */
00327 
00328     Status = VL53L0X_RdWord(Dev,
00329                 VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
00330                 &RangeOffsetRegister);
00331 
00332     if (Status == VL53L0X_ERROR_NONE) {
00333         RangeOffsetRegister = (RangeOffsetRegister & 0x0fff);
00334 
00335         /* Apply 12 bit 2's compliment conversion */
00336         if (RangeOffsetRegister > cMaxOffset)
00337             *pOffsetCalibrationDataMicroMeter =
00338                 (int16_t)(RangeOffsetRegister - cOffsetRange)
00339                     * 250;
00340         else
00341             *pOffsetCalibrationDataMicroMeter =
00342                 (int16_t)RangeOffsetRegister * 250;
00343 
00344     }
00345 
00346     return Status;
00347 }
00348 
00349 
00350 VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev)
00351 {
00352     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00353     int32_t CorrectedOffsetMicroMeters;
00354     int32_t CurrentOffsetMicroMeters;
00355 
00356     /* if we run on this function we can read all the NVM info
00357      * used by the API */
00358     Status = VL53L0X_get_info_from_device(Dev, 7);
00359 
00360     /* Read back current device offset */
00361     if (Status == VL53L0X_ERROR_NONE) {
00362         Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev,
00363                     &CurrentOffsetMicroMeters);
00364     }
00365 
00366     /* Apply Offset Adjustment derived from 400mm measurements */
00367     if (Status == VL53L0X_ERROR_NONE) {
00368 
00369         /* Store initial device offset */
00370         PALDevDataSet(Dev, Part2PartOffsetNVMMicroMeter,
00371             CurrentOffsetMicroMeters);
00372 
00373         CorrectedOffsetMicroMeters = CurrentOffsetMicroMeters +
00374             (int32_t)PALDevDataGet(Dev,
00375                 Part2PartOffsetAdjustmentNVMMicroMeter);
00376 
00377         Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev,
00378                     CorrectedOffsetMicroMeters);
00379 
00380         /* store current, adjusted offset */
00381         if (Status == VL53L0X_ERROR_NONE) {
00382             VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters,
00383                     CorrectedOffsetMicroMeters);
00384         }
00385     }
00386 
00387     return Status;
00388 }
00389 
00390 void get_next_good_spad(uint8_t goodSpadArray[], uint32_t size,
00391             uint32_t curr, int32_t *next)
00392 {
00393 
00394     uint32_t startIndex;
00395     uint32_t fineOffset;
00396     uint32_t cSpadsPerByte = 8;
00397     uint32_t coarseIndex;
00398     uint32_t fineIndex;
00399     uint8_t dataByte;
00400     uint8_t success = 0;
00401 
00402     /*
00403      * Starting with the current good spad, loop through the array to find
00404      * the next. i.e. the next bit set in the sequence.
00405      *
00406      * The coarse index is the byte index of the array and the fine index is
00407      * the index of the bit within each byte.
00408      */
00409 
00410     *next = -1;
00411 
00412 
00413     startIndex = curr / cSpadsPerByte;
00414     fineOffset = curr % cSpadsPerByte;
00415 
00416 
00417     for (coarseIndex = startIndex; ((coarseIndex < size) && !success); coarseIndex++)
00418   {
00419         fineIndex = 0;
00420         dataByte = goodSpadArray[coarseIndex];
00421 
00422         if (coarseIndex == startIndex) {
00423             /* locate the bit position of the provided current
00424              * spad bit before iterating */
00425             dataByte >>= fineOffset;
00426             fineIndex = fineOffset;
00427         }
00428 
00429         while (fineIndex < cSpadsPerByte) {
00430             if ((dataByte & 0x1) == 1) {
00431                 success = 1;
00432                 *next = coarseIndex * cSpadsPerByte + fineIndex;
00433                 break;
00434             }
00435             dataByte >>= 1;
00436             fineIndex++;
00437         }
00438     }
00439 }
00440 
00441 
00442 uint8_t is_aperture(uint32_t spadIndex)
00443 {
00444     /*
00445      * This function reports if a given spad index is an aperture SPAD by
00446      * deriving the quadrant.
00447      */
00448     uint32_t quadrant;
00449     uint8_t isAperture = 1;
00450     quadrant = spadIndex >> 6;
00451     if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0)
00452         isAperture = 0;
00453 
00454     return isAperture;
00455 }
00456 
00457 
00458 VL53L0X_Error enable_spad_bit(uint8_t spadArray[], uint32_t size,
00459     uint32_t spadIndex)
00460 {
00461     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00462     uint32_t cSpadsPerByte = 8;
00463     uint32_t coarseIndex;
00464     uint32_t fineIndex;
00465 
00466     coarseIndex = spadIndex / cSpadsPerByte;
00467     fineIndex = spadIndex % cSpadsPerByte;
00468     if (coarseIndex >= size)
00469         status = VL53L0X_ERROR_REF_SPAD_INIT;
00470     else
00471         spadArray[coarseIndex] |= (1 << fineIndex);
00472 
00473     return status;
00474 }
00475 
00476 VL53L0X_Error count_enabled_spads(uint8_t spadArray[],
00477         uint32_t byteCount, uint32_t maxSpads,
00478         uint32_t *pTotalSpadsEnabled, uint8_t *pIsAperture)
00479 {
00480     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00481     uint32_t cSpadsPerByte = 8;
00482     uint32_t lastByte;
00483     uint32_t lastBit;
00484     uint32_t byteIndex = 0;
00485     uint32_t bitIndex = 0;
00486     uint8_t tempByte;
00487     uint8_t spadTypeIdentified = 0;
00488 
00489     /* The entire array will not be used for spads, therefore the last
00490      * byte and last bit is determined from the max spads value.
00491      */
00492 
00493     lastByte = maxSpads / cSpadsPerByte;
00494     lastBit = maxSpads % cSpadsPerByte;
00495 
00496     /* Check that the max spads value does not exceed the array bounds. */
00497     if (lastByte >= byteCount)
00498         status = VL53L0X_ERROR_REF_SPAD_INIT;
00499 
00500     *pTotalSpadsEnabled = 0;
00501 
00502     /* Count the bits enabled in the whole bytes */
00503     for (byteIndex = 0; byteIndex <= (lastByte - 1); byteIndex++) {
00504         tempByte = spadArray[byteIndex];
00505 
00506         for (bitIndex = 0; bitIndex <= cSpadsPerByte; bitIndex++) {
00507             if ((tempByte & 0x01) == 1) {
00508                 (*pTotalSpadsEnabled)++;
00509 
00510                 if (!spadTypeIdentified) {
00511                     *pIsAperture = 1;
00512                     if ((byteIndex < 2) && (bitIndex < 4))
00513                             *pIsAperture = 0;
00514                     spadTypeIdentified = 1;
00515                 }
00516             }
00517             tempByte >>= 1;
00518         }
00519     }
00520 
00521     /* Count the number of bits enabled in the last byte accounting
00522      * for the fact that not all bits in the byte may be used.
00523      */
00524     tempByte = spadArray[lastByte];
00525 
00526     for (bitIndex = 0; bitIndex <= lastBit; bitIndex++) {
00527         if ((tempByte & 0x01) == 1)
00528             (*pTotalSpadsEnabled)++;
00529     }
00530 
00531     return status;
00532 }
00533 
00534 VL53L0X_Error set_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
00535 {
00536     VL53L0X_Error status = VL53L0X_WriteMulti(Dev,
00537                 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
00538                 refSpadArray, 6);
00539     return status;
00540 }
00541 
00542 VL53L0X_Error get_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray)
00543 {
00544     VL53L0X_Error status = VL53L0X_ReadMulti(Dev,
00545                 VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
00546                 refSpadArray,
00547                 6);
00548     return status;
00549 }
00550 
00551 VL53L0X_Error enable_ref_spads(VL53L0X_DEV Dev,
00552                 uint8_t apertureSpads,
00553                 uint8_t goodSpadArray[],
00554                 uint8_t spadArray[],
00555                 uint32_t size,
00556                 uint32_t start,
00557                 uint32_t offset,
00558                 uint32_t spadCount,
00559                 uint32_t *lastSpad)
00560 {
00561   //Serial pc_internal_3(USBTX,USBRX,9600);
00562     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00563     uint32_t index;
00564     uint32_t i;
00565     int32_t nextGoodSpad = offset;
00566     uint32_t currentSpad;
00567     uint8_t checkSpadArray[6];
00568 
00569     /*
00570      * This function takes in a spad array which may or may not have SPADS
00571      * already enabled and appends from a given offset a requested number
00572      * of new SPAD enables. The 'good spad map' is applied to
00573      * determine the next SPADs to enable.
00574      *
00575      * This function applies to only aperture or only non-aperture spads.
00576      * Checks are performed to ensure this.
00577      */
00578 
00579     currentSpad = offset;
00580     for (index = 0; index < spadCount; index++) {
00581         get_next_good_spad(goodSpadArray, size, currentSpad, &nextGoodSpad);
00582 
00583         if (nextGoodSpad == -1) {
00584             status = VL53L0X_ERROR_REF_SPAD_INIT;
00585             break;
00586         }
00587     
00588         /* Confirm that the next good SPAD is non-aperture */
00589         if (is_aperture(start + nextGoodSpad) != apertureSpads) {
00590             /* if we can't get the required number of good aperture
00591              * spads from the current quadrant then this is an error
00592              */
00593             status = VL53L0X_ERROR_REF_SPAD_INIT;
00594             break;
00595         }
00596 
00597         currentSpad = (uint32_t)nextGoodSpad;
00598         enable_spad_bit(spadArray, size, currentSpad);
00599         currentSpad++;
00600     }
00601     *lastSpad = currentSpad;
00602 
00603     if (status == VL53L0X_ERROR_NONE)
00604         status = set_ref_spad_map(Dev, spadArray);
00605 
00606     if (status == VL53L0X_ERROR_NONE) {
00607         status = get_ref_spad_map(Dev, checkSpadArray);
00608 
00609         i = 0;
00610 
00611         /* Compare spad maps. If not equal report error. */
00612     //pc_internal_3.printf("size: %i\n\r",size);
00613         while (i < size) {
00614 
00615       //pc_internal_3.printf("     spadArray[%i]: %i\n\r",i, spadArray[i]);
00616       //pc_internal_3.printf("checkSpadArray[%i]: %i\n\r",i, checkSpadArray[i]);
00617 
00618             if (spadArray[i] != checkSpadArray[i]) {
00619                 status = VL53L0X_ERROR_REF_SPAD_INIT;
00620                 break;
00621             }
00622             i++;
00623         }
00624     }
00625   //pc_internal_3.printf("Status 4.5 Set Spad Reference: %i\n\r",status);
00626 
00627     return status;
00628 }
00629 
00630 
00631 VL53L0X_Error perform_ref_signal_measurement(VL53L0X_DEV Dev,
00632         uint16_t *refSignalRate)
00633 {
00634     VL53L0X_Error status = VL53L0X_ERROR_NONE;
00635     VL53L0X_RangingMeasurementData_t rangingMeasurementData;
00636 
00637     uint8_t SequenceConfig = 0;
00638 
00639     /* store the value of the sequence config,
00640      * this will be reset before the end of the function
00641      */
00642 
00643     SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
00644 
00645     /*
00646      * This function performs a reference signal rate measurement.
00647      */
00648     if (status == VL53L0X_ERROR_NONE)
00649         status = VL53L0X_WrByte(Dev,
00650             VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
00651 
00652     if (status == VL53L0X_ERROR_NONE)
00653         status = VL53L0X_PerformSingleRangingMeasurement(Dev,
00654                 &rangingMeasurementData);
00655 
00656     if (status == VL53L0X_ERROR_NONE)
00657         status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
00658 
00659     if (status == VL53L0X_ERROR_NONE)
00660         status = VL53L0X_RdWord(Dev,
00661             VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
00662             refSignalRate);
00663 
00664     if (status == VL53L0X_ERROR_NONE)
00665         status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
00666 
00667     if (status == VL53L0X_ERROR_NONE) {
00668         /* restore the previous Sequence Config */
00669         status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
00670                 SequenceConfig);
00671         if (status == VL53L0X_ERROR_NONE)
00672             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
00673     }
00674 
00675     return status;
00676 }
00677 
00678 VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
00679                 uint32_t *refSpadCount,
00680                 uint8_t *isApertureSpads)
00681 {
00682     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00683     uint8_t lastSpadArray[6];
00684     uint8_t startSelect = 0xB4;
00685     uint32_t minimumSpadCount = 3;
00686     uint32_t maxSpadCount = 44;
00687     uint32_t currentSpadIndex = 0;
00688     uint32_t lastSpadIndex = 0;
00689     int32_t nextGoodSpad = 0;
00690     uint16_t targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */
00691     uint16_t peakSignalRateRef;
00692     uint32_t needAptSpads = 0;
00693     uint32_t index = 0;
00694     uint32_t spadArraySize = 6;
00695     uint32_t signalRateDiff = 0;
00696     uint32_t lastSignalRateDiff = 0;
00697     uint8_t complete = 0;
00698     uint8_t VhvSettings = 0;
00699     uint8_t PhaseCal = 0;
00700     uint32_t refSpadCount_int = 0;
00701     uint8_t  isApertureSpads_int = 0;
00702 
00703     /*
00704      * The reference SPAD initialization procedure determines the minimum
00705      * amount of reference spads to be enables to achieve a target reference
00706      * signal rate and should be performed once during initialization.
00707      *
00708      * Either aperture or non-aperture spads are applied but never both.
00709      * Firstly non-aperture spads are set, begining with 5 spads, and
00710      * increased one spad at a time until the closest measurement to the
00711      * target rate is achieved.
00712      *
00713      * If the target rate is exceeded when 5 non-aperture spads are enabled,
00714      * initialization is performed instead with aperture spads.
00715      *
00716      * When setting spads, a 'Good Spad Map' is applied.
00717      *
00718      * This procedure operates within a SPAD window of interest of a maximum
00719      * 44 spads.
00720      * The start point is currently fixed to 180, which lies towards the end
00721      * of the non-aperture quadrant and runs in to the adjacent aperture
00722      * quadrant.
00723      */
00724 
00725 
00726     targetRefRate = PALDevDataGet(Dev, targetRefRate);
00727 
00728     /*
00729      * Initialize Spad arrays.
00730      * Currently the good spad map is initialised to 'All good'.
00731      * This is a short term implementation. The good spad map will be
00732      * provided as an input.
00733      * Note that there are 6 bytes. Only the first 44 bits will be used to
00734      * represent spads.
00735      */
00736     for (index = 0; index < spadArraySize; index++)
00737         Dev->Data .SpadData .RefSpadEnables [index] = 0;
00738 
00739 
00740     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
00741 
00742     if (Status == VL53L0X_ERROR_NONE)
00743         Status = VL53L0X_WrByte(Dev,
00744             VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
00745 
00746     if (Status == VL53L0X_ERROR_NONE)
00747         Status = VL53L0X_WrByte(Dev,
00748             VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
00749 
00750     if (Status == VL53L0X_ERROR_NONE)
00751         Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
00752 
00753     if (Status == VL53L0X_ERROR_NONE)
00754         Status = VL53L0X_WrByte(Dev,
00755             VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
00756             startSelect);
00757 
00758 
00759     if (Status == VL53L0X_ERROR_NONE)
00760         Status = VL53L0X_WrByte(Dev,
00761                 VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
00762 
00763     /* Perform ref calibration */
00764     if (Status == VL53L0X_ERROR_NONE)
00765         Status = VL53L0X_perform_ref_calibration(Dev, &VhvSettings,
00766             &PhaseCal, 0);
00767 
00768     if (Status == VL53L0X_ERROR_NONE) {
00769         /* Enable Minimum NON-APERTURE Spads */
00770         currentSpadIndex = 0;
00771         lastSpadIndex = currentSpadIndex;
00772         needAptSpads = 0;
00773         Status = enable_ref_spads(Dev,
00774                     needAptSpads,
00775                     Dev->Data .SpadData .RefGoodSpadMap ,
00776                     Dev->Data .SpadData .RefSpadEnables ,
00777                     spadArraySize,
00778                     startSelect,
00779                     currentSpadIndex,
00780                     minimumSpadCount,
00781                     &lastSpadIndex);
00782     }
00783 
00784     if (Status == VL53L0X_ERROR_NONE) {
00785         currentSpadIndex = lastSpadIndex;
00786 
00787         Status = perform_ref_signal_measurement(Dev,
00788             &peakSignalRateRef);
00789         if ((Status == VL53L0X_ERROR_NONE) &&
00790             (peakSignalRateRef > targetRefRate)) {
00791             /* Signal rate measurement too high,
00792              * switch to APERTURE SPADs */
00793 
00794             for (index = 0; index < spadArraySize; index++)
00795                 Dev->Data .SpadData .RefSpadEnables [index] = 0;
00796 
00797 
00798             /* Increment to the first APERTURE spad */
00799             while ((is_aperture(startSelect + currentSpadIndex)
00800                 == 0) && (currentSpadIndex < maxSpadCount)) {
00801                 currentSpadIndex++;
00802             }
00803 
00804             needAptSpads = 1;
00805 
00806             Status = enable_ref_spads(Dev,
00807                     needAptSpads,
00808                     Dev->Data .SpadData .RefGoodSpadMap ,
00809                     Dev->Data .SpadData .RefSpadEnables ,
00810                     spadArraySize,
00811                     startSelect,
00812                     currentSpadIndex,
00813                     minimumSpadCount,
00814                     &lastSpadIndex);
00815 
00816             if (Status == VL53L0X_ERROR_NONE) {
00817                 currentSpadIndex = lastSpadIndex;
00818                 Status = perform_ref_signal_measurement(Dev,
00819                         &peakSignalRateRef);
00820 
00821                 if ((Status == VL53L0X_ERROR_NONE) &&
00822                     (peakSignalRateRef > targetRefRate)) {
00823                     /* Signal rate still too high after
00824                      * setting the minimum number of
00825                      * APERTURE spads. Can do no more
00826                      * therefore set the min number of
00827                      * aperture spads as the result.
00828                      */
00829                     isApertureSpads_int = 1;
00830                     refSpadCount_int = minimumSpadCount;
00831                 }
00832             }
00833         } else {
00834             needAptSpads = 0;
00835         }
00836     }
00837 
00838     if ((Status == VL53L0X_ERROR_NONE) &&
00839         (peakSignalRateRef < targetRefRate)) {
00840         /* At this point, the minimum number of either aperture
00841          * or non-aperture spads have been set. Proceed to add
00842          * spads and perform measurements until the target
00843          * reference is reached.
00844          */
00845         isApertureSpads_int = needAptSpads;
00846         refSpadCount_int    = minimumSpadCount;
00847 
00848         memcpy(lastSpadArray, Dev->Data .SpadData .RefSpadEnables ,
00849                 spadArraySize);
00850         lastSignalRateDiff = abs(peakSignalRateRef -
00851             targetRefRate);
00852         complete = 0;
00853 
00854         while (!complete) {
00855             get_next_good_spad(
00856                 Dev->Data .SpadData .RefGoodSpadMap ,
00857                 spadArraySize, currentSpadIndex,
00858                 &nextGoodSpad);
00859 
00860             if (nextGoodSpad == -1) {
00861                 Status = VL53L0X_ERROR_REF_SPAD_INIT;
00862                 break;
00863             }
00864 
00865             /* Cannot combine Aperture and Non-Aperture spads, so
00866              * ensure the current spad is of the correct type.
00867              */
00868             if (is_aperture((uint32_t)startSelect + nextGoodSpad) !=
00869                     needAptSpads) {
00870                 /* At this point we have enabled the maximum
00871                  * number of Aperture spads.
00872                  */
00873                 complete = 1;
00874                 break;
00875             }
00876 
00877             (refSpadCount_int)++;
00878 
00879             currentSpadIndex = nextGoodSpad;
00880             Status = enable_spad_bit(
00881                     Dev->Data .SpadData .RefSpadEnables ,
00882                     spadArraySize, currentSpadIndex);
00883 
00884             if (Status == VL53L0X_ERROR_NONE) {
00885                 currentSpadIndex++;
00886                 /* Proceed to apply the additional spad and
00887                  * perform measurement. */
00888                 Status = set_ref_spad_map(Dev,
00889                     Dev->Data .SpadData .RefSpadEnables );
00890             }
00891 
00892             if (Status != VL53L0X_ERROR_NONE)
00893                 break;
00894 
00895             Status = perform_ref_signal_measurement(Dev,
00896                     &peakSignalRateRef);
00897 
00898             if (Status != VL53L0X_ERROR_NONE)
00899                 break;
00900 
00901             signalRateDiff = abs(peakSignalRateRef - targetRefRate);
00902 
00903             if (peakSignalRateRef > targetRefRate) {
00904                 /* Select the spad map that provides the
00905                  * measurement closest to the target rate,
00906                  * either above or below it.
00907                  */
00908                 if (signalRateDiff > lastSignalRateDiff) {
00909                     /* Previous spad map produced a closer
00910                      * measurement, so choose this. */
00911                     Status = set_ref_spad_map(Dev,
00912                             lastSpadArray);
00913                     memcpy(
00914                     Dev->Data .SpadData .RefSpadEnables ,
00915                     lastSpadArray, spadArraySize);
00916 
00917                     (refSpadCount_int)--;
00918                 }
00919                 complete = 1;
00920             } else {
00921                 /* Continue to add spads */
00922                 lastSignalRateDiff = signalRateDiff;
00923                 memcpy(lastSpadArray,
00924                     Dev->Data .SpadData .RefSpadEnables ,
00925                     spadArraySize);
00926             }
00927 
00928         } /* while */
00929     }
00930 
00931     if (Status == VL53L0X_ERROR_NONE) {
00932         *refSpadCount = refSpadCount_int;
00933         *isApertureSpads = isApertureSpads_int;
00934 
00935         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
00936         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00937             ReferenceSpadCount, (uint8_t)(*refSpadCount));
00938         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
00939             ReferenceSpadType, *isApertureSpads);
00940     }
00941 
00942     return Status;
00943 }
00944 
00945 VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
00946                  uint32_t count, uint8_t isApertureSpads)
00947 {
00948   //Serial pc_internal_2(USBTX,USBRX,9600);
00949     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00950     uint32_t currentSpadIndex = 0;
00951     uint8_t startSelect = 0xB4;
00952     uint32_t spadArraySize = 6;
00953     uint32_t maxSpadCount = 44;
00954     uint32_t lastSpadIndex;
00955     uint32_t index;
00956 
00957     /*
00958      * This function applies a requested number of reference spads, either
00959      * aperture or
00960      * non-aperture, as requested.
00961      * The good spad map will be applied.
00962      */
00963 
00964     Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
00965 
00966     if (Status == VL53L0X_ERROR_NONE)
00967         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
00968 
00969  // pc_internal_2.printf("Status 1 Set Spad Reference: %i\n\r",Status);
00970 
00971     if (Status == VL53L0X_ERROR_NONE)
00972         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
00973 
00974  // pc_internal_2.printf("Status 2 Set Spad Reference: %i\n\r",Status);
00975   
00976     if (Status == VL53L0X_ERROR_NONE)
00977         Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
00978 
00979  // pc_internal_2.printf("Status 3 Set Spad Reference: %i\n\r",Status);
00980   
00981     if (Status == VL53L0X_ERROR_NONE)
00982         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, startSelect);
00983 
00984  // pc_internal_2.printf("Status 4 Set Spad Reference: %i\n\r",Status);
00985   
00986     for (index = 0; index < spadArraySize; index++)
00987   {
00988         Dev->Data .SpadData .RefSpadEnables [index] = 0;
00989     
00990   }
00991 
00992  // pc_internal_2.printf("RefGoodSpadMap: %i\n\r", Dev->Data.SpadData.RefGoodSpadMap[0]);
00993  // pc_internal_2.printf("RefGoodSpadMap: %i\n\r", Dev->Data.SpadData.RefGoodSpadMap[1]);
00994  // pc_internal_2.printf("RefGoodSpadMap: %i\n\r", Dev->Data.SpadData.RefGoodSpadMap[2]);
00995  // pc_internal_2.printf("RefGoodSpadMap: %i\n\r", Dev->Data.SpadData.RefGoodSpadMap[3]);
00996  // pc_internal_2.printf("RefGoodSpadMap: %i\n\r", Dev->Data.SpadData.RefGoodSpadMap[4]);
00997  // pc_internal_2.printf("RefGoodSpadMap: %i\n\r", Dev->Data.SpadData.RefGoodSpadMap[5]);
00998 
00999     if (isApertureSpads) {
01000         /* Increment to the first APERTURE spad */
01001         while ((is_aperture(startSelect + currentSpadIndex) == 0) &&
01002               (currentSpadIndex < maxSpadCount)) {
01003             currentSpadIndex++;
01004         }
01005     }
01006     Status = enable_ref_spads(Dev, isApertureSpads, Dev->Data .SpadData .RefGoodSpadMap , Dev->Data .SpadData .RefSpadEnables ,
01007                    spadArraySize, startSelect, currentSpadIndex, count, &lastSpadIndex);
01008   
01009  // pc_internal_2.printf("Status 5 Set Spad Reference: %i\n\r",Status);
01010 
01011     if (Status == VL53L0X_ERROR_NONE) {
01012         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1);
01013         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
01014             ReferenceSpadCount, (uint8_t)(count));
01015         VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
01016             ReferenceSpadType, isApertureSpads);
01017     }
01018 
01019  // pc_internal_2.printf("Status 6 Set Spad Reference: %i\n\r",Status);
01020   
01021     return Status;
01022 }
01023 
01024 VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev,
01025             uint32_t *pSpadCount, uint8_t *pIsApertureSpads)
01026 {
01027     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01028     uint8_t refSpadsInitialised;
01029     uint8_t refSpadArray[6];
01030     uint32_t cMaxSpadCount = 44;
01031     uint32_t cSpadArraySize = 6;
01032     uint32_t spadsEnabled;
01033     uint8_t isApertureSpads = 0;
01034 
01035     refSpadsInitialised = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
01036                     RefSpadsInitialised);
01037 
01038     if (refSpadsInitialised == 1) {
01039 
01040         *pSpadCount = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
01041             ReferenceSpadCount);
01042         *pIsApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
01043             ReferenceSpadType);
01044     } else {
01045 
01046         /* obtain spad info from device.*/
01047         Status = get_ref_spad_map(Dev, refSpadArray);
01048 
01049         if (Status == VL53L0X_ERROR_NONE) {
01050             /* count enabled spads within spad map array and
01051              * determine if Aperture or Non-Aperture.
01052              */
01053             Status = count_enabled_spads(refSpadArray,
01054                             cSpadArraySize,
01055                             cMaxSpadCount,
01056                             &spadsEnabled,
01057                             &isApertureSpads);
01058 
01059             if (Status == VL53L0X_ERROR_NONE) {
01060 
01061                 *pSpadCount = spadsEnabled;
01062                 *pIsApertureSpads = isApertureSpads;
01063 
01064                 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
01065                     RefSpadsInitialised, 1);
01066                 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
01067                     ReferenceSpadCount,
01068                     (uint8_t)spadsEnabled);
01069                 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
01070                     ReferenceSpadType, isApertureSpads);
01071             }
01072         }
01073     }
01074 
01075     return Status;
01076 }
01077 
01078 
01079 VL53L0X_Error VL53L0X_perform_single_ref_calibration(VL53L0X_DEV Dev,
01080         uint8_t vhv_init_byte)
01081 {
01082     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01083 
01084     if (Status == VL53L0X_ERROR_NONE)
01085         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START,
01086                 VL53L0X_REG_SYSRANGE_MODE_START_STOP |
01087                 vhv_init_byte);
01088 
01089     if (Status == VL53L0X_ERROR_NONE)
01090         Status = VL53L0X_measurement_poll_for_completion(Dev);
01091 
01092     if (Status == VL53L0X_ERROR_NONE)
01093         Status = VL53L0X_ClearInterruptMask(Dev, 0);
01094 
01095     if (Status == VL53L0X_ERROR_NONE)
01096         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x00);
01097 
01098     return Status;
01099 }
01100 
01101 
01102 VL53L0X_Error VL53L0X_ref_calibration_io(VL53L0X_DEV Dev, uint8_t read_not_write,
01103     uint8_t VhvSettings, uint8_t PhaseCal,
01104     uint8_t *pVhvSettings, uint8_t *pPhaseCal,
01105     const uint8_t vhv_enable, const uint8_t phase_enable)
01106 {
01107     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01108     uint8_t PhaseCalint = 0;
01109 
01110     /* Read VHV from device */
01111     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
01112     Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
01113     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
01114 
01115     if (read_not_write) {
01116         if (vhv_enable)
01117             Status |= VL53L0X_RdByte(Dev, 0xCB, pVhvSettings);
01118         if (phase_enable)
01119             Status |= VL53L0X_RdByte(Dev, 0xEE, &PhaseCalint);
01120     } else {
01121         if (vhv_enable)
01122             Status |= VL53L0X_WrByte(Dev, 0xCB, VhvSettings);
01123         if (phase_enable)
01124             Status |= VL53L0X_UpdateByte(Dev, 0xEE, 0x80, PhaseCal);
01125     }
01126 
01127     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
01128     Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
01129     Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
01130 
01131     *pPhaseCal = (uint8_t)(PhaseCalint&0xEF);
01132 
01133     return Status;
01134 }
01135 
01136 
01137 VL53L0X_Error VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev,
01138     uint8_t *pVhvSettings, const uint8_t get_data_enable,
01139     const uint8_t restore_config)
01140 {
01141     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01142     uint8_t SequenceConfig = 0;
01143     uint8_t VhvSettings = 0;
01144     uint8_t PhaseCal = 0;
01145     uint8_t PhaseCalInt = 0;
01146 
01147     /* store the value of the sequence config,
01148      * this will be reset before the end of the function
01149      */
01150 
01151     if (restore_config)
01152         SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
01153 
01154     /* Run VHV */
01155     Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
01156 
01157     if (Status == VL53L0X_ERROR_NONE)
01158         Status = VL53L0X_perform_single_ref_calibration(Dev, 0x40);
01159 
01160     /* Read VHV from device */
01161     if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
01162         Status = VL53L0X_ref_calibration_io(Dev, 1,
01163             VhvSettings, PhaseCal, /* Not used here */
01164             pVhvSettings, &PhaseCalInt,
01165             1, 0);
01166     } else
01167         *pVhvSettings = 0;
01168 
01169 
01170     if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
01171         /* restore the previous Sequence Config */
01172         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01173                 SequenceConfig);
01174         if (Status == VL53L0X_ERROR_NONE)
01175             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
01176 
01177     }
01178 
01179     return Status;
01180 }
01181 
01182 VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
01183     uint8_t *pPhaseCal, const uint8_t get_data_enable,
01184     const uint8_t restore_config)
01185 {
01186     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01187     uint8_t SequenceConfig = 0;
01188     uint8_t VhvSettings = 0;
01189     uint8_t PhaseCal = 0;
01190     uint8_t VhvSettingsint;
01191 
01192     /* store the value of the sequence config,
01193      * this will be reset before the end of the function
01194      */
01195 
01196     if (restore_config)
01197         SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
01198 
01199     /* Run PhaseCal */
01200     Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
01201 
01202     if (Status == VL53L0X_ERROR_NONE)
01203         Status = VL53L0X_perform_single_ref_calibration(Dev, 0x0);
01204 
01205     /* Read PhaseCal from device */
01206     if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
01207         Status = VL53L0X_ref_calibration_io(Dev, 1,
01208             VhvSettings, PhaseCal, /* Not used here */
01209             &VhvSettingsint, pPhaseCal,
01210             0, 1);
01211     } else
01212         *pPhaseCal = 0;
01213 
01214 
01215     if ((Status == VL53L0X_ERROR_NONE) && restore_config) {
01216         /* restore the previous Sequence Config */
01217         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01218                 SequenceConfig);
01219         if (Status == VL53L0X_ERROR_NONE)
01220             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
01221 
01222     }
01223 
01224     return Status;
01225 }
01226 
01227 VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
01228     uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable)
01229 {
01230     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01231     uint8_t SequenceConfig = 0;
01232 
01233     /* store the value of the sequence config,
01234      * this will be reset before the end of the function
01235      */
01236 
01237     SequenceConfig = PALDevDataGet(Dev, SequenceConfig);
01238 
01239     /* In the following function we don't save the config to optimize
01240      * writes on device. Config is saved and restored only once. */
01241     Status = VL53L0X_perform_vhv_calibration(
01242             Dev, pVhvSettings, get_data_enable, 0);
01243 
01244 
01245     if (Status == VL53L0X_ERROR_NONE)
01246         Status = VL53L0X_perform_phase_calibration(
01247             Dev, pPhaseCal, get_data_enable, 0);
01248 
01249 
01250     if (Status == VL53L0X_ERROR_NONE) {
01251         /* restore the previous Sequence Config */
01252         Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
01253                 SequenceConfig);
01254         if (Status == VL53L0X_ERROR_NONE)
01255             PALDevDataSet(Dev, SequenceConfig, SequenceConfig);
01256 
01257     }
01258 
01259     return Status;
01260 }
01261 
01262 VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,
01263         uint8_t VhvSettings, uint8_t PhaseCal)
01264 {
01265     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01266     uint8_t pVhvSettings;
01267     uint8_t pPhaseCal;
01268 
01269     Status = VL53L0X_ref_calibration_io(Dev, 0,
01270         VhvSettings, PhaseCal,
01271         &pVhvSettings, &pPhaseCal,
01272         1, 1);
01273 
01274     return Status;
01275 }
01276 
01277 VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,
01278         uint8_t *pVhvSettings, uint8_t *pPhaseCal)
01279 {
01280     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
01281     uint8_t VhvSettings = 0;
01282     uint8_t PhaseCal = 0;
01283 
01284     Status = VL53L0X_ref_calibration_io(Dev, 1,
01285         VhvSettings, PhaseCal,
01286         pVhvSettings, pPhaseCal,
01287         1, 1);
01288 
01289     return Status;
01290 }