Markus Järvisalo / vl53l0x_api

mbed compatible API for the VL53L0X Time-of-Flight sensor

Dependents:   VL53L0X_SingleRanging_Example robot_sm VL53L0X_SingleRanging_HighAccuracy_HANSL ENGR6002_P001unk

vl53l0x_api_calibration.c@0:e6fcdb78a136, 2016-08-23 (annotated)

Committer:
mjarvisal
Date:
Tue Aug 23 05:14:05 2016 +0000
Revision:
0:e6fcdb78a136
Initial release

Who changed what in which revision?

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