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_core.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
mjarvisal 0:e6fcdb78a136 34 #ifndef __KERNEL__
mjarvisal 0:e6fcdb78a136 35 #include <stdlib.h>
mjarvisal 0:e6fcdb78a136 36 #endif
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 VL53L0X_Error VL53L0X_reverse_bytes(uint8_t *data, uint32_t size)
mjarvisal 0:e6fcdb78a136 45 {
mjarvisal 0:e6fcdb78a136 46 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 47 uint8_t tempData;
mjarvisal 0:e6fcdb78a136 48 uint32_t mirrorIndex;
mjarvisal 0:e6fcdb78a136 49 uint32_t middle = size/2;
mjarvisal 0:e6fcdb78a136 50 uint32_t index;
mjarvisal 0:e6fcdb78a136 51
mjarvisal 0:e6fcdb78a136 52 for (index = 0; index < middle; index++) {
mjarvisal 0:e6fcdb78a136 53 mirrorIndex = size - index - 1;
mjarvisal 0:e6fcdb78a136 54 tempData = data[index];
mjarvisal 0:e6fcdb78a136 55 data[index] = data[mirrorIndex];
mjarvisal 0:e6fcdb78a136 56 data[mirrorIndex] = tempData;
mjarvisal 0:e6fcdb78a136 57 }
mjarvisal 0:e6fcdb78a136 58 return Status;
mjarvisal 0:e6fcdb78a136 59 }
mjarvisal 0:e6fcdb78a136 60
mjarvisal 0:e6fcdb78a136 61 VL53L0X_Error VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev)
mjarvisal 0:e6fcdb78a136 62 {
mjarvisal 0:e6fcdb78a136 63 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 64 uint8_t NewDataReady = 0;
mjarvisal 0:e6fcdb78a136 65 uint32_t LoopNb;
mjarvisal 0:e6fcdb78a136 66
mjarvisal 0:e6fcdb78a136 67 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 68
mjarvisal 0:e6fcdb78a136 69 LoopNb = 0;
mjarvisal 0:e6fcdb78a136 70
mjarvisal 0:e6fcdb78a136 71 do {
mjarvisal 0:e6fcdb78a136 72 Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDataReady);
mjarvisal 0:e6fcdb78a136 73 if (Status != 0)
mjarvisal 0:e6fcdb78a136 74 break; /* the error is set */
mjarvisal 0:e6fcdb78a136 75
mjarvisal 0:e6fcdb78a136 76 if (NewDataReady == 1)
mjarvisal 0:e6fcdb78a136 77 break; /* done note that status == 0 */
mjarvisal 0:e6fcdb78a136 78
mjarvisal 0:e6fcdb78a136 79 LoopNb++;
mjarvisal 0:e6fcdb78a136 80 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
mjarvisal 0:e6fcdb78a136 81 Status = VL53L0X_ERROR_TIME_OUT;
mjarvisal 0:e6fcdb78a136 82 break;
mjarvisal 0:e6fcdb78a136 83 }
mjarvisal 0:e6fcdb78a136 84
mjarvisal 0:e6fcdb78a136 85 VL53L0X_PollingDelay(Dev);
mjarvisal 0:e6fcdb78a136 86 } while (1);
mjarvisal 0:e6fcdb78a136 87
mjarvisal 0:e6fcdb78a136 88 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 89
mjarvisal 0:e6fcdb78a136 90 return Status;
mjarvisal 0:e6fcdb78a136 91 }
mjarvisal 0:e6fcdb78a136 92
mjarvisal 0:e6fcdb78a136 93
mjarvisal 0:e6fcdb78a136 94 uint8_t VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg)
mjarvisal 0:e6fcdb78a136 95 {
mjarvisal 0:e6fcdb78a136 96 /*!
mjarvisal 0:e6fcdb78a136 97 * Converts the encoded VCSEL period register value into the real
mjarvisal 0:e6fcdb78a136 98 * period in PLL clocks
mjarvisal 0:e6fcdb78a136 99 */
mjarvisal 0:e6fcdb78a136 100
mjarvisal 0:e6fcdb78a136 101 uint8_t vcsel_period_pclks = 0;
mjarvisal 0:e6fcdb78a136 102
mjarvisal 0:e6fcdb78a136 103 vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
mjarvisal 0:e6fcdb78a136 104
mjarvisal 0:e6fcdb78a136 105 return vcsel_period_pclks;
mjarvisal 0:e6fcdb78a136 106 }
mjarvisal 0:e6fcdb78a136 107
mjarvisal 0:e6fcdb78a136 108 uint8_t VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks)
mjarvisal 0:e6fcdb78a136 109 {
mjarvisal 0:e6fcdb78a136 110 /*!
mjarvisal 0:e6fcdb78a136 111 * Converts the encoded VCSEL period register value into the real period
mjarvisal 0:e6fcdb78a136 112 * in PLL clocks
mjarvisal 0:e6fcdb78a136 113 */
mjarvisal 0:e6fcdb78a136 114
mjarvisal 0:e6fcdb78a136 115 uint8_t vcsel_period_reg = 0;
mjarvisal 0:e6fcdb78a136 116
mjarvisal 0:e6fcdb78a136 117 vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
mjarvisal 0:e6fcdb78a136 118
mjarvisal 0:e6fcdb78a136 119 return vcsel_period_reg;
mjarvisal 0:e6fcdb78a136 120 }
mjarvisal 0:e6fcdb78a136 121
mjarvisal 0:e6fcdb78a136 122
mjarvisal 0:e6fcdb78a136 123 uint32_t VL53L0X_isqrt(uint32_t num)
mjarvisal 0:e6fcdb78a136 124 {
mjarvisal 0:e6fcdb78a136 125 /*
mjarvisal 0:e6fcdb78a136 126 * Implements an integer square root
mjarvisal 0:e6fcdb78a136 127 *
mjarvisal 0:e6fcdb78a136 128 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
mjarvisal 0:e6fcdb78a136 129 */
mjarvisal 0:e6fcdb78a136 130
mjarvisal 0:e6fcdb78a136 131 uint32_t res = 0;
mjarvisal 0:e6fcdb78a136 132 uint32_t bit = 1 << 30;
mjarvisal 0:e6fcdb78a136 133 /* The second-to-top bit is set:
mjarvisal 0:e6fcdb78a136 134 * 1 << 14 for 16-bits, 1 << 30 for 32 bits */
mjarvisal 0:e6fcdb78a136 135
mjarvisal 0:e6fcdb78a136 136 /* "bit" starts at the highest power of four <= the argument. */
mjarvisal 0:e6fcdb78a136 137 while (bit > num)
mjarvisal 0:e6fcdb78a136 138 bit >>= 2;
mjarvisal 0:e6fcdb78a136 139
mjarvisal 0:e6fcdb78a136 140
mjarvisal 0:e6fcdb78a136 141 while (bit != 0) {
mjarvisal 0:e6fcdb78a136 142 if (num >= res + bit) {
mjarvisal 0:e6fcdb78a136 143 num -= res + bit;
mjarvisal 0:e6fcdb78a136 144 res = (res >> 1) + bit;
mjarvisal 0:e6fcdb78a136 145 } else
mjarvisal 0:e6fcdb78a136 146 res >>= 1;
mjarvisal 0:e6fcdb78a136 147
mjarvisal 0:e6fcdb78a136 148 bit >>= 2;
mjarvisal 0:e6fcdb78a136 149 }
mjarvisal 0:e6fcdb78a136 150
mjarvisal 0:e6fcdb78a136 151 return res;
mjarvisal 0:e6fcdb78a136 152 }
mjarvisal 0:e6fcdb78a136 153
mjarvisal 0:e6fcdb78a136 154
mjarvisal 0:e6fcdb78a136 155 uint32_t VL53L0X_quadrature_sum(uint32_t a, uint32_t b)
mjarvisal 0:e6fcdb78a136 156 {
mjarvisal 0:e6fcdb78a136 157 /*
mjarvisal 0:e6fcdb78a136 158 * Implements a quadrature sum
mjarvisal 0:e6fcdb78a136 159 *
mjarvisal 0:e6fcdb78a136 160 * rea = sqrt(a^2 + b^2)
mjarvisal 0:e6fcdb78a136 161 *
mjarvisal 0:e6fcdb78a136 162 * Trap overflow case max input value is 65535 (16-bit value)
mjarvisal 0:e6fcdb78a136 163 * as internal calc are 32-bit wide
mjarvisal 0:e6fcdb78a136 164 *
mjarvisal 0:e6fcdb78a136 165 * If overflow then seta output to maximum
mjarvisal 0:e6fcdb78a136 166 */
mjarvisal 0:e6fcdb78a136 167 uint32_t res = 0;
mjarvisal 0:e6fcdb78a136 168
mjarvisal 0:e6fcdb78a136 169 if (a > 65535 || b > 65535)
mjarvisal 0:e6fcdb78a136 170 res = 65535;
mjarvisal 0:e6fcdb78a136 171 else
mjarvisal 0:e6fcdb78a136 172 res = VL53L0X_isqrt(a * a + b * b);
mjarvisal 0:e6fcdb78a136 173
mjarvisal 0:e6fcdb78a136 174 return res;
mjarvisal 0:e6fcdb78a136 175 }
mjarvisal 0:e6fcdb78a136 176
mjarvisal 0:e6fcdb78a136 177
mjarvisal 0:e6fcdb78a136 178 VL53L0X_Error VL53L0X_device_read_strobe(VL53L0X_DEV Dev)
mjarvisal 0:e6fcdb78a136 179 {
mjarvisal 0:e6fcdb78a136 180 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 181 uint8_t strobe;
mjarvisal 0:e6fcdb78a136 182 uint32_t LoopNb;
mjarvisal 0:e6fcdb78a136 183 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 184
mjarvisal 0:e6fcdb78a136 185 Status |= VL53L0X_WrByte(Dev, 0x83, 0x00);
mjarvisal 0:e6fcdb78a136 186
mjarvisal 0:e6fcdb78a136 187 /* polling
mjarvisal 0:e6fcdb78a136 188 * use timeout to avoid deadlock*/
mjarvisal 0:e6fcdb78a136 189 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 190 LoopNb = 0;
mjarvisal 0:e6fcdb78a136 191 do {
mjarvisal 0:e6fcdb78a136 192 Status = VL53L0X_RdByte(Dev, 0x83, &strobe);
mjarvisal 0:e6fcdb78a136 193 if ((strobe != 0x00) || Status != VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 194 break;
mjarvisal 0:e6fcdb78a136 195
mjarvisal 0:e6fcdb78a136 196 LoopNb = LoopNb + 1;
mjarvisal 0:e6fcdb78a136 197 } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
mjarvisal 0:e6fcdb78a136 198
mjarvisal 0:e6fcdb78a136 199 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP)
mjarvisal 0:e6fcdb78a136 200 Status = VL53L0X_ERROR_TIME_OUT;
mjarvisal 0:e6fcdb78a136 201
mjarvisal 0:e6fcdb78a136 202 }
mjarvisal 0:e6fcdb78a136 203
mjarvisal 0:e6fcdb78a136 204 Status |= VL53L0X_WrByte(Dev, 0x83, 0x01);
mjarvisal 0:e6fcdb78a136 205
mjarvisal 0:e6fcdb78a136 206 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 207 return Status;
mjarvisal 0:e6fcdb78a136 208
mjarvisal 0:e6fcdb78a136 209 }
mjarvisal 0:e6fcdb78a136 210
mjarvisal 0:e6fcdb78a136 211 VL53L0X_Error VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option)
mjarvisal 0:e6fcdb78a136 212 {
mjarvisal 0:e6fcdb78a136 213
mjarvisal 0:e6fcdb78a136 214 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 215 uint8_t byte;
mjarvisal 0:e6fcdb78a136 216 uint32_t TmpDWord;
mjarvisal 0:e6fcdb78a136 217 uint8_t ModuleId;
mjarvisal 0:e6fcdb78a136 218 uint8_t Revision;
mjarvisal 0:e6fcdb78a136 219 uint8_t ReferenceSpadCount = 0;
mjarvisal 0:e6fcdb78a136 220 uint8_t ReferenceSpadType = 0;
mjarvisal 0:e6fcdb78a136 221 uint32_t PartUIDUpper = 0;
mjarvisal 0:e6fcdb78a136 222 uint32_t PartUIDLower = 0;
mjarvisal 0:e6fcdb78a136 223 uint32_t OffsetFixed1104_mm = 0;
mjarvisal 0:e6fcdb78a136 224 int16_t OffsetMicroMeters = 0;
mjarvisal 0:e6fcdb78a136 225 uint32_t DistMeasTgtFixed1104_mm = 400 << 4;
mjarvisal 0:e6fcdb78a136 226 uint32_t DistMeasFixed1104_400_mm = 0;
mjarvisal 0:e6fcdb78a136 227 uint32_t SignalRateMeasFixed1104_400_mm = 0;
mjarvisal 0:e6fcdb78a136 228 char ProductId[19];
mjarvisal 0:e6fcdb78a136 229 char *ProductId_tmp;
mjarvisal 0:e6fcdb78a136 230 uint8_t ReadDataFromDeviceDone;
mjarvisal 0:e6fcdb78a136 231 FixPoint1616_t SignalRateMeasFixed400mmFix = 0;
mjarvisal 0:e6fcdb78a136 232 uint8_t NvmRefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
mjarvisal 0:e6fcdb78a136 233 int i;
mjarvisal 0:e6fcdb78a136 234
mjarvisal 0:e6fcdb78a136 235
mjarvisal 0:e6fcdb78a136 236 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 237
mjarvisal 0:e6fcdb78a136 238 ReadDataFromDeviceDone = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 239 ReadDataFromDeviceDone);
mjarvisal 0:e6fcdb78a136 240
mjarvisal 0:e6fcdb78a136 241 /* This access is done only once after that a GetDeviceInfo or
mjarvisal 0:e6fcdb78a136 242 * datainit is done*/
mjarvisal 0:e6fcdb78a136 243 if (ReadDataFromDeviceDone != 7) {
mjarvisal 0:e6fcdb78a136 244
mjarvisal 0:e6fcdb78a136 245 Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
mjarvisal 0:e6fcdb78a136 246 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
mjarvisal 0:e6fcdb78a136 247 Status |= VL53L0X_WrByte(Dev, 0x00, 0x00);
mjarvisal 0:e6fcdb78a136 248
mjarvisal 0:e6fcdb78a136 249 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
mjarvisal 0:e6fcdb78a136 250 Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
mjarvisal 0:e6fcdb78a136 251 Status |= VL53L0X_WrByte(Dev, 0x83, byte|4);
mjarvisal 0:e6fcdb78a136 252 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x07);
mjarvisal 0:e6fcdb78a136 253 Status |= VL53L0X_WrByte(Dev, 0x81, 0x01);
mjarvisal 0:e6fcdb78a136 254
mjarvisal 0:e6fcdb78a136 255 Status |= VL53L0X_PollingDelay(Dev);
mjarvisal 0:e6fcdb78a136 256
mjarvisal 0:e6fcdb78a136 257 Status |= VL53L0X_WrByte(Dev, 0x80, 0x01);
mjarvisal 0:e6fcdb78a136 258
mjarvisal 0:e6fcdb78a136 259 if (((option & 1) == 1) &&
mjarvisal 0:e6fcdb78a136 260 ((ReadDataFromDeviceDone & 1) == 0)) {
mjarvisal 0:e6fcdb78a136 261 Status |= VL53L0X_WrByte(Dev, 0x94, 0x6b);
mjarvisal 0:e6fcdb78a136 262 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 263 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 264
mjarvisal 0:e6fcdb78a136 265 ReferenceSpadCount = (uint8_t)((TmpDWord >> 8) & 0x07f);
mjarvisal 0:e6fcdb78a136 266 ReferenceSpadType = (uint8_t)((TmpDWord >> 15) & 0x01);
mjarvisal 0:e6fcdb78a136 267
mjarvisal 0:e6fcdb78a136 268 Status |= VL53L0X_WrByte(Dev, 0x94, 0x24);
mjarvisal 0:e6fcdb78a136 269 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 270 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 271
mjarvisal 0:e6fcdb78a136 272
mjarvisal 0:e6fcdb78a136 273 NvmRefGoodSpadMap[0] = (uint8_t)((TmpDWord >> 24)
mjarvisal 0:e6fcdb78a136 274 & 0xff);
mjarvisal 0:e6fcdb78a136 275 NvmRefGoodSpadMap[1] = (uint8_t)((TmpDWord >> 16)
mjarvisal 0:e6fcdb78a136 276 & 0xff);
mjarvisal 0:e6fcdb78a136 277 NvmRefGoodSpadMap[2] = (uint8_t)((TmpDWord >> 8)
mjarvisal 0:e6fcdb78a136 278 & 0xff);
mjarvisal 0:e6fcdb78a136 279 NvmRefGoodSpadMap[3] = (uint8_t)(TmpDWord & 0xff);
mjarvisal 0:e6fcdb78a136 280
mjarvisal 0:e6fcdb78a136 281 Status |= VL53L0X_WrByte(Dev, 0x94, 0x25);
mjarvisal 0:e6fcdb78a136 282 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 283 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 284
mjarvisal 0:e6fcdb78a136 285 NvmRefGoodSpadMap[4] = (uint8_t)((TmpDWord >> 24)
mjarvisal 0:e6fcdb78a136 286 & 0xff);
mjarvisal 0:e6fcdb78a136 287 NvmRefGoodSpadMap[5] = (uint8_t)((TmpDWord >> 16)
mjarvisal 0:e6fcdb78a136 288 & 0xff);
mjarvisal 0:e6fcdb78a136 289 }
mjarvisal 0:e6fcdb78a136 290
mjarvisal 0:e6fcdb78a136 291 if (((option & 2) == 2) &&
mjarvisal 0:e6fcdb78a136 292 ((ReadDataFromDeviceDone & 2) == 0)) {
mjarvisal 0:e6fcdb78a136 293
mjarvisal 0:e6fcdb78a136 294 Status |= VL53L0X_WrByte(Dev, 0x94, 0x02);
mjarvisal 0:e6fcdb78a136 295 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 296 Status |= VL53L0X_RdByte(Dev, 0x90, &ModuleId);
mjarvisal 0:e6fcdb78a136 297
mjarvisal 0:e6fcdb78a136 298 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
mjarvisal 0:e6fcdb78a136 299 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 300 Status |= VL53L0X_RdByte(Dev, 0x90, &Revision);
mjarvisal 0:e6fcdb78a136 301
mjarvisal 0:e6fcdb78a136 302 Status |= VL53L0X_WrByte(Dev, 0x94, 0x77);
mjarvisal 0:e6fcdb78a136 303 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 304 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 305
mjarvisal 0:e6fcdb78a136 306 ProductId[0] = (char)((TmpDWord >> 25) & 0x07f);
mjarvisal 0:e6fcdb78a136 307 ProductId[1] = (char)((TmpDWord >> 18) & 0x07f);
mjarvisal 0:e6fcdb78a136 308 ProductId[2] = (char)((TmpDWord >> 11) & 0x07f);
mjarvisal 0:e6fcdb78a136 309 ProductId[3] = (char)((TmpDWord >> 4) & 0x07f);
mjarvisal 0:e6fcdb78a136 310
mjarvisal 0:e6fcdb78a136 311 byte = (uint8_t)((TmpDWord & 0x00f) << 3);
mjarvisal 0:e6fcdb78a136 312
mjarvisal 0:e6fcdb78a136 313 Status |= VL53L0X_WrByte(Dev, 0x94, 0x78);
mjarvisal 0:e6fcdb78a136 314 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 315 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 316
mjarvisal 0:e6fcdb78a136 317 ProductId[4] = (char)(byte +
mjarvisal 0:e6fcdb78a136 318 ((TmpDWord >> 29) & 0x07f));
mjarvisal 0:e6fcdb78a136 319 ProductId[5] = (char)((TmpDWord >> 22) & 0x07f);
mjarvisal 0:e6fcdb78a136 320 ProductId[6] = (char)((TmpDWord >> 15) & 0x07f);
mjarvisal 0:e6fcdb78a136 321 ProductId[7] = (char)((TmpDWord >> 8) & 0x07f);
mjarvisal 0:e6fcdb78a136 322 ProductId[8] = (char)((TmpDWord >> 1) & 0x07f);
mjarvisal 0:e6fcdb78a136 323
mjarvisal 0:e6fcdb78a136 324 byte = (uint8_t)((TmpDWord & 0x001) << 6);
mjarvisal 0:e6fcdb78a136 325
mjarvisal 0:e6fcdb78a136 326 Status |= VL53L0X_WrByte(Dev, 0x94, 0x79);
mjarvisal 0:e6fcdb78a136 327
mjarvisal 0:e6fcdb78a136 328 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 329
mjarvisal 0:e6fcdb78a136 330 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 331
mjarvisal 0:e6fcdb78a136 332 ProductId[9] = (char)(byte +
mjarvisal 0:e6fcdb78a136 333 ((TmpDWord >> 26) & 0x07f));
mjarvisal 0:e6fcdb78a136 334 ProductId[10] = (char)((TmpDWord >> 19) & 0x07f);
mjarvisal 0:e6fcdb78a136 335 ProductId[11] = (char)((TmpDWord >> 12) & 0x07f);
mjarvisal 0:e6fcdb78a136 336 ProductId[12] = (char)((TmpDWord >> 5) & 0x07f);
mjarvisal 0:e6fcdb78a136 337
mjarvisal 0:e6fcdb78a136 338 byte = (uint8_t)((TmpDWord & 0x01f) << 2);
mjarvisal 0:e6fcdb78a136 339
mjarvisal 0:e6fcdb78a136 340 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7A);
mjarvisal 0:e6fcdb78a136 341
mjarvisal 0:e6fcdb78a136 342 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 343
mjarvisal 0:e6fcdb78a136 344 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 345
mjarvisal 0:e6fcdb78a136 346 ProductId[13] = (char)(byte +
mjarvisal 0:e6fcdb78a136 347 ((TmpDWord >> 30) & 0x07f));
mjarvisal 0:e6fcdb78a136 348 ProductId[14] = (char)((TmpDWord >> 23) & 0x07f);
mjarvisal 0:e6fcdb78a136 349 ProductId[15] = (char)((TmpDWord >> 16) & 0x07f);
mjarvisal 0:e6fcdb78a136 350 ProductId[16] = (char)((TmpDWord >> 9) & 0x07f);
mjarvisal 0:e6fcdb78a136 351 ProductId[17] = (char)((TmpDWord >> 2) & 0x07f);
mjarvisal 0:e6fcdb78a136 352 ProductId[18] = '\0';
mjarvisal 0:e6fcdb78a136 353
mjarvisal 0:e6fcdb78a136 354 }
mjarvisal 0:e6fcdb78a136 355
mjarvisal 0:e6fcdb78a136 356 if (((option & 4) == 4) &&
mjarvisal 0:e6fcdb78a136 357 ((ReadDataFromDeviceDone & 4) == 0)) {
mjarvisal 0:e6fcdb78a136 358
mjarvisal 0:e6fcdb78a136 359 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B);
mjarvisal 0:e6fcdb78a136 360 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 361 Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDUpper);
mjarvisal 0:e6fcdb78a136 362
mjarvisal 0:e6fcdb78a136 363 Status |= VL53L0X_WrByte(Dev, 0x94, 0x7C);
mjarvisal 0:e6fcdb78a136 364 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 365 Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDLower);
mjarvisal 0:e6fcdb78a136 366
mjarvisal 0:e6fcdb78a136 367 Status |= VL53L0X_WrByte(Dev, 0x94, 0x73);
mjarvisal 0:e6fcdb78a136 368 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 369 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 370
mjarvisal 0:e6fcdb78a136 371 SignalRateMeasFixed1104_400_mm = (TmpDWord &
mjarvisal 0:e6fcdb78a136 372 0x0000000ff) << 8;
mjarvisal 0:e6fcdb78a136 373
mjarvisal 0:e6fcdb78a136 374 Status |= VL53L0X_WrByte(Dev, 0x94, 0x74);
mjarvisal 0:e6fcdb78a136 375 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 376 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 377
mjarvisal 0:e6fcdb78a136 378 SignalRateMeasFixed1104_400_mm |= ((TmpDWord &
mjarvisal 0:e6fcdb78a136 379 0xff000000) >> 24);
mjarvisal 0:e6fcdb78a136 380
mjarvisal 0:e6fcdb78a136 381 Status |= VL53L0X_WrByte(Dev, 0x94, 0x75);
mjarvisal 0:e6fcdb78a136 382 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 383 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 384
mjarvisal 0:e6fcdb78a136 385 DistMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff)
mjarvisal 0:e6fcdb78a136 386 << 8;
mjarvisal 0:e6fcdb78a136 387
mjarvisal 0:e6fcdb78a136 388 Status |= VL53L0X_WrByte(Dev, 0x94, 0x76);
mjarvisal 0:e6fcdb78a136 389 Status |= VL53L0X_device_read_strobe(Dev);
mjarvisal 0:e6fcdb78a136 390 Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord);
mjarvisal 0:e6fcdb78a136 391
mjarvisal 0:e6fcdb78a136 392 DistMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000)
mjarvisal 0:e6fcdb78a136 393 >> 24);
mjarvisal 0:e6fcdb78a136 394 }
mjarvisal 0:e6fcdb78a136 395
mjarvisal 0:e6fcdb78a136 396 Status |= VL53L0X_WrByte(Dev, 0x81, 0x00);
mjarvisal 0:e6fcdb78a136 397 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06);
mjarvisal 0:e6fcdb78a136 398 Status |= VL53L0X_RdByte(Dev, 0x83, &byte);
mjarvisal 0:e6fcdb78a136 399 Status |= VL53L0X_WrByte(Dev, 0x83, byte&0xfb);
mjarvisal 0:e6fcdb78a136 400 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01);
mjarvisal 0:e6fcdb78a136 401 Status |= VL53L0X_WrByte(Dev, 0x00, 0x01);
mjarvisal 0:e6fcdb78a136 402
mjarvisal 0:e6fcdb78a136 403 Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00);
mjarvisal 0:e6fcdb78a136 404 Status |= VL53L0X_WrByte(Dev, 0x80, 0x00);
mjarvisal 0:e6fcdb78a136 405 }
mjarvisal 0:e6fcdb78a136 406
mjarvisal 0:e6fcdb78a136 407 if ((Status == VL53L0X_ERROR_NONE) &&
mjarvisal 0:e6fcdb78a136 408 (ReadDataFromDeviceDone != 7)) {
mjarvisal 0:e6fcdb78a136 409 /* Assign to variable if status is ok */
mjarvisal 0:e6fcdb78a136 410 if (((option & 1) == 1) &&
mjarvisal 0:e6fcdb78a136 411 ((ReadDataFromDeviceDone & 1) == 0)) {
mjarvisal 0:e6fcdb78a136 412 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 413 ReferenceSpadCount, ReferenceSpadCount);
mjarvisal 0:e6fcdb78a136 414
mjarvisal 0:e6fcdb78a136 415 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 416 ReferenceSpadType, ReferenceSpadType);
mjarvisal 0:e6fcdb78a136 417
mjarvisal 0:e6fcdb78a136 418 for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
mjarvisal 0:e6fcdb78a136 419 Dev->Data.SpadData.RefGoodSpadMap[i] =
mjarvisal 0:e6fcdb78a136 420 NvmRefGoodSpadMap[i];
mjarvisal 0:e6fcdb78a136 421 }
mjarvisal 0:e6fcdb78a136 422 }
mjarvisal 0:e6fcdb78a136 423
mjarvisal 0:e6fcdb78a136 424 if (((option & 2) == 2) &&
mjarvisal 0:e6fcdb78a136 425 ((ReadDataFromDeviceDone & 2) == 0)) {
mjarvisal 0:e6fcdb78a136 426 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 427 ModuleId, ModuleId);
mjarvisal 0:e6fcdb78a136 428
mjarvisal 0:e6fcdb78a136 429 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 430 Revision, Revision);
mjarvisal 0:e6fcdb78a136 431
mjarvisal 0:e6fcdb78a136 432 ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 433 ProductId);
mjarvisal 0:e6fcdb78a136 434 VL53L0X_COPYSTRING(ProductId_tmp, ProductId);
mjarvisal 0:e6fcdb78a136 435
mjarvisal 0:e6fcdb78a136 436 }
mjarvisal 0:e6fcdb78a136 437
mjarvisal 0:e6fcdb78a136 438 if (((option & 4) == 4) &&
mjarvisal 0:e6fcdb78a136 439 ((ReadDataFromDeviceDone & 4) == 0)) {
mjarvisal 0:e6fcdb78a136 440 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 441 PartUIDUpper, PartUIDUpper);
mjarvisal 0:e6fcdb78a136 442
mjarvisal 0:e6fcdb78a136 443 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 444 PartUIDLower, PartUIDLower);
mjarvisal 0:e6fcdb78a136 445
mjarvisal 0:e6fcdb78a136 446 SignalRateMeasFixed400mmFix =
mjarvisal 0:e6fcdb78a136 447 VL53L0X_FIXPOINT97TOFIXPOINT1616(
mjarvisal 0:e6fcdb78a136 448 SignalRateMeasFixed1104_400_mm);
mjarvisal 0:e6fcdb78a136 449
mjarvisal 0:e6fcdb78a136 450 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 451 SignalRateMeasFixed400mm,
mjarvisal 0:e6fcdb78a136 452 SignalRateMeasFixed400mmFix);
mjarvisal 0:e6fcdb78a136 453
mjarvisal 0:e6fcdb78a136 454 OffsetMicroMeters = 0;
mjarvisal 0:e6fcdb78a136 455 if (DistMeasFixed1104_400_mm != 0) {
mjarvisal 0:e6fcdb78a136 456 OffsetFixed1104_mm =
mjarvisal 0:e6fcdb78a136 457 DistMeasFixed1104_400_mm -
mjarvisal 0:e6fcdb78a136 458 DistMeasTgtFixed1104_mm;
mjarvisal 0:e6fcdb78a136 459 OffsetMicroMeters = (OffsetFixed1104_mm
mjarvisal 0:e6fcdb78a136 460 * 1000) >> 4;
mjarvisal 0:e6fcdb78a136 461 OffsetMicroMeters *= -1;
mjarvisal 0:e6fcdb78a136 462 }
mjarvisal 0:e6fcdb78a136 463
mjarvisal 0:e6fcdb78a136 464 PALDevDataSet(Dev,
mjarvisal 0:e6fcdb78a136 465 Part2PartOffsetAdjustmentNVMMicroMeter,
mjarvisal 0:e6fcdb78a136 466 OffsetMicroMeters);
mjarvisal 0:e6fcdb78a136 467 }
mjarvisal 0:e6fcdb78a136 468 byte = (uint8_t)(ReadDataFromDeviceDone|option);
mjarvisal 0:e6fcdb78a136 469 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone,
mjarvisal 0:e6fcdb78a136 470 byte);
mjarvisal 0:e6fcdb78a136 471 }
mjarvisal 0:e6fcdb78a136 472
mjarvisal 0:e6fcdb78a136 473 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 474 return Status;
mjarvisal 0:e6fcdb78a136 475 }
mjarvisal 0:e6fcdb78a136 476
mjarvisal 0:e6fcdb78a136 477
mjarvisal 0:e6fcdb78a136 478 uint32_t VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev, uint8_t vcsel_period_pclks)
mjarvisal 0:e6fcdb78a136 479 {
mjarvisal 0:e6fcdb78a136 480 uint64_t PLL_period_ps;
mjarvisal 0:e6fcdb78a136 481 uint32_t macro_period_vclks;
mjarvisal 0:e6fcdb78a136 482 uint32_t macro_period_ps;
mjarvisal 0:e6fcdb78a136 483
mjarvisal 0:e6fcdb78a136 484 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 485
mjarvisal 0:e6fcdb78a136 486 /* The above calculation will produce rounding errors,
mjarvisal 0:e6fcdb78a136 487 therefore set fixed value
mjarvisal 0:e6fcdb78a136 488 */
mjarvisal 0:e6fcdb78a136 489 PLL_period_ps = 1655;
mjarvisal 0:e6fcdb78a136 490
mjarvisal 0:e6fcdb78a136 491 macro_period_vclks = 2304;
mjarvisal 0:e6fcdb78a136 492 macro_period_ps = (uint32_t)(macro_period_vclks
mjarvisal 0:e6fcdb78a136 493 * vcsel_period_pclks * PLL_period_ps);
mjarvisal 0:e6fcdb78a136 494
mjarvisal 0:e6fcdb78a136 495 LOG_FUNCTION_END("");
mjarvisal 0:e6fcdb78a136 496 return macro_period_ps;
mjarvisal 0:e6fcdb78a136 497 }
mjarvisal 0:e6fcdb78a136 498
mjarvisal 0:e6fcdb78a136 499 uint16_t VL53L0X_encode_timeout(uint32_t timeout_macro_clks)
mjarvisal 0:e6fcdb78a136 500 {
mjarvisal 0:e6fcdb78a136 501 /*!
mjarvisal 0:e6fcdb78a136 502 * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
mjarvisal 0:e6fcdb78a136 503 */
mjarvisal 0:e6fcdb78a136 504
mjarvisal 0:e6fcdb78a136 505 uint16_t encoded_timeout = 0;
mjarvisal 0:e6fcdb78a136 506 uint32_t ls_byte = 0;
mjarvisal 0:e6fcdb78a136 507 uint16_t ms_byte = 0;
mjarvisal 0:e6fcdb78a136 508
mjarvisal 0:e6fcdb78a136 509 if (timeout_macro_clks > 0) {
mjarvisal 0:e6fcdb78a136 510 ls_byte = timeout_macro_clks - 1;
mjarvisal 0:e6fcdb78a136 511
mjarvisal 0:e6fcdb78a136 512 while ((ls_byte & 0xFFFFFF00) > 0) {
mjarvisal 0:e6fcdb78a136 513 ls_byte = ls_byte >> 1;
mjarvisal 0:e6fcdb78a136 514 ms_byte++;
mjarvisal 0:e6fcdb78a136 515 }
mjarvisal 0:e6fcdb78a136 516
mjarvisal 0:e6fcdb78a136 517 encoded_timeout = (ms_byte << 8)
mjarvisal 0:e6fcdb78a136 518 + (uint16_t) (ls_byte & 0x000000FF);
mjarvisal 0:e6fcdb78a136 519 }
mjarvisal 0:e6fcdb78a136 520
mjarvisal 0:e6fcdb78a136 521 return encoded_timeout;
mjarvisal 0:e6fcdb78a136 522
mjarvisal 0:e6fcdb78a136 523 }
mjarvisal 0:e6fcdb78a136 524
mjarvisal 0:e6fcdb78a136 525 uint32_t VL53L0X_decode_timeout(uint16_t encoded_timeout)
mjarvisal 0:e6fcdb78a136 526 {
mjarvisal 0:e6fcdb78a136 527 /*!
mjarvisal 0:e6fcdb78a136 528 * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
mjarvisal 0:e6fcdb78a136 529 */
mjarvisal 0:e6fcdb78a136 530
mjarvisal 0:e6fcdb78a136 531 uint32_t timeout_macro_clks = 0;
mjarvisal 0:e6fcdb78a136 532
mjarvisal 0:e6fcdb78a136 533 timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
mjarvisal 0:e6fcdb78a136 534 << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1;
mjarvisal 0:e6fcdb78a136 535
mjarvisal 0:e6fcdb78a136 536 return timeout_macro_clks;
mjarvisal 0:e6fcdb78a136 537 }
mjarvisal 0:e6fcdb78a136 538
mjarvisal 0:e6fcdb78a136 539
mjarvisal 0:e6fcdb78a136 540 /* To convert ms into register value */
mjarvisal 0:e6fcdb78a136 541 uint32_t VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 542 uint32_t timeout_period_us,
mjarvisal 0:e6fcdb78a136 543 uint8_t vcsel_period_pclks)
mjarvisal 0:e6fcdb78a136 544 {
mjarvisal 0:e6fcdb78a136 545 uint32_t macro_period_ps;
mjarvisal 0:e6fcdb78a136 546 uint32_t macro_period_ns;
mjarvisal 0:e6fcdb78a136 547 uint32_t timeout_period_mclks = 0;
mjarvisal 0:e6fcdb78a136 548
mjarvisal 0:e6fcdb78a136 549 macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
mjarvisal 0:e6fcdb78a136 550 macro_period_ns = (macro_period_ps + 500) / 1000;
mjarvisal 0:e6fcdb78a136 551
mjarvisal 0:e6fcdb78a136 552 timeout_period_mclks =
mjarvisal 0:e6fcdb78a136 553 (uint32_t) (((timeout_period_us * 1000)
mjarvisal 0:e6fcdb78a136 554 + (macro_period_ns / 2)) / macro_period_ns);
mjarvisal 0:e6fcdb78a136 555
mjarvisal 0:e6fcdb78a136 556 return timeout_period_mclks;
mjarvisal 0:e6fcdb78a136 557 }
mjarvisal 0:e6fcdb78a136 558
mjarvisal 0:e6fcdb78a136 559 /* To convert register value into us */
mjarvisal 0:e6fcdb78a136 560 uint32_t VL53L0X_calc_timeout_us(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 561 uint16_t timeout_period_mclks,
mjarvisal 0:e6fcdb78a136 562 uint8_t vcsel_period_pclks)
mjarvisal 0:e6fcdb78a136 563 {
mjarvisal 0:e6fcdb78a136 564 uint32_t macro_period_ps;
mjarvisal 0:e6fcdb78a136 565 uint32_t macro_period_ns;
mjarvisal 0:e6fcdb78a136 566 uint32_t actual_timeout_period_us = 0;
mjarvisal 0:e6fcdb78a136 567
mjarvisal 0:e6fcdb78a136 568 macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks);
mjarvisal 0:e6fcdb78a136 569 macro_period_ns = (macro_period_ps + 500) / 1000;
mjarvisal 0:e6fcdb78a136 570
mjarvisal 0:e6fcdb78a136 571 actual_timeout_period_us =
mjarvisal 0:e6fcdb78a136 572 ((timeout_period_mclks * macro_period_ns)
mjarvisal 0:e6fcdb78a136 573 + (macro_period_ns / 2)) / 1000;
mjarvisal 0:e6fcdb78a136 574
mjarvisal 0:e6fcdb78a136 575 return actual_timeout_period_us;
mjarvisal 0:e6fcdb78a136 576 }
mjarvisal 0:e6fcdb78a136 577
mjarvisal 0:e6fcdb78a136 578
mjarvisal 0:e6fcdb78a136 579 VL53L0X_Error get_sequence_step_timeout(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 580 VL53L0X_SequenceStepId SequenceStepId,
mjarvisal 0:e6fcdb78a136 581 uint32_t *pTimeOutMicroSecs)
mjarvisal 0:e6fcdb78a136 582 {
mjarvisal 0:e6fcdb78a136 583 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 584 uint8_t CurrentVCSELPulsePeriodPClk;
mjarvisal 0:e6fcdb78a136 585 uint8_t EncodedTimeOutByte = 0;
mjarvisal 0:e6fcdb78a136 586 uint32_t TimeoutMicroSeconds = 0;
mjarvisal 0:e6fcdb78a136 587 uint16_t PreRangeEncodedTimeOut = 0;
mjarvisal 0:e6fcdb78a136 588 uint16_t MsrcTimeOutMClks;
mjarvisal 0:e6fcdb78a136 589 uint16_t PreRangeTimeOutMClks;
mjarvisal 0:e6fcdb78a136 590 uint16_t FinalRangeTimeOutMClks = 0;
mjarvisal 0:e6fcdb78a136 591 uint16_t FinalRangeEncodedTimeOut;
mjarvisal 0:e6fcdb78a136 592 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
mjarvisal 0:e6fcdb78a136 593
mjarvisal 0:e6fcdb78a136 594 if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) ||
mjarvisal 0:e6fcdb78a136 595 (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) ||
mjarvisal 0:e6fcdb78a136 596 (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
mjarvisal 0:e6fcdb78a136 597
mjarvisal 0:e6fcdb78a136 598 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 599 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 600 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 601 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 602 Status = VL53L0X_RdByte(Dev,
mjarvisal 0:e6fcdb78a136 603 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
mjarvisal 0:e6fcdb78a136 604 &EncodedTimeOutByte);
mjarvisal 0:e6fcdb78a136 605 }
mjarvisal 0:e6fcdb78a136 606 MsrcTimeOutMClks = VL53L0X_decode_timeout(EncodedTimeOutByte);
mjarvisal 0:e6fcdb78a136 607
mjarvisal 0:e6fcdb78a136 608 TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
mjarvisal 0:e6fcdb78a136 609 MsrcTimeOutMClks,
mjarvisal 0:e6fcdb78a136 610 CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 611 } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
mjarvisal 0:e6fcdb78a136 612 /* Retrieve PRE-RANGE VCSEL Period */
mjarvisal 0:e6fcdb78a136 613 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 614 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 615 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 616
mjarvisal 0:e6fcdb78a136 617 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
mjarvisal 0:e6fcdb78a136 618 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 619
mjarvisal 0:e6fcdb78a136 620 /* Retrieve PRE-RANGE VCSEL Period */
mjarvisal 0:e6fcdb78a136 621 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 622 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 623 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 624
mjarvisal 0:e6fcdb78a136 625 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 626 Status = VL53L0X_RdWord(Dev,
mjarvisal 0:e6fcdb78a136 627 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
mjarvisal 0:e6fcdb78a136 628 &PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 629 }
mjarvisal 0:e6fcdb78a136 630
mjarvisal 0:e6fcdb78a136 631 PreRangeTimeOutMClks = VL53L0X_decode_timeout(
mjarvisal 0:e6fcdb78a136 632 PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 633
mjarvisal 0:e6fcdb78a136 634 TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
mjarvisal 0:e6fcdb78a136 635 PreRangeTimeOutMClks,
mjarvisal 0:e6fcdb78a136 636 CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 637 }
mjarvisal 0:e6fcdb78a136 638 } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
mjarvisal 0:e6fcdb78a136 639
mjarvisal 0:e6fcdb78a136 640 VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
mjarvisal 0:e6fcdb78a136 641 PreRangeTimeOutMClks = 0;
mjarvisal 0:e6fcdb78a136 642
mjarvisal 0:e6fcdb78a136 643 if (SchedulerSequenceSteps.PreRangeOn) {
mjarvisal 0:e6fcdb78a136 644 /* Retrieve PRE-RANGE VCSEL Period */
mjarvisal 0:e6fcdb78a136 645 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 646 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 647 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 648
mjarvisal 0:e6fcdb78a136 649 /* Retrieve PRE-RANGE Timeout in Macro periods
mjarvisal 0:e6fcdb78a136 650 * (MCLKS) */
mjarvisal 0:e6fcdb78a136 651 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 652 Status = VL53L0X_RdWord(Dev,
mjarvisal 0:e6fcdb78a136 653 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
mjarvisal 0:e6fcdb78a136 654 &PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 655 PreRangeTimeOutMClks = VL53L0X_decode_timeout(
mjarvisal 0:e6fcdb78a136 656 PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 657 }
mjarvisal 0:e6fcdb78a136 658 }
mjarvisal 0:e6fcdb78a136 659
mjarvisal 0:e6fcdb78a136 660 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 661 /* Retrieve FINAL-RANGE VCSEL Period */
mjarvisal 0:e6fcdb78a136 662 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 663 VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 664 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 665 }
mjarvisal 0:e6fcdb78a136 666
mjarvisal 0:e6fcdb78a136 667 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
mjarvisal 0:e6fcdb78a136 668 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 669 Status = VL53L0X_RdWord(Dev,
mjarvisal 0:e6fcdb78a136 670 VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
mjarvisal 0:e6fcdb78a136 671 &FinalRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 672 FinalRangeTimeOutMClks = VL53L0X_decode_timeout(
mjarvisal 0:e6fcdb78a136 673 FinalRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 674 }
mjarvisal 0:e6fcdb78a136 675
mjarvisal 0:e6fcdb78a136 676 FinalRangeTimeOutMClks -= PreRangeTimeOutMClks;
mjarvisal 0:e6fcdb78a136 677 TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev,
mjarvisal 0:e6fcdb78a136 678 FinalRangeTimeOutMClks,
mjarvisal 0:e6fcdb78a136 679 CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 680 }
mjarvisal 0:e6fcdb78a136 681
mjarvisal 0:e6fcdb78a136 682 *pTimeOutMicroSecs = TimeoutMicroSeconds;
mjarvisal 0:e6fcdb78a136 683
mjarvisal 0:e6fcdb78a136 684 return Status;
mjarvisal 0:e6fcdb78a136 685 }
mjarvisal 0:e6fcdb78a136 686
mjarvisal 0:e6fcdb78a136 687
mjarvisal 0:e6fcdb78a136 688 VL53L0X_Error set_sequence_step_timeout(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 689 VL53L0X_SequenceStepId SequenceStepId,
mjarvisal 0:e6fcdb78a136 690 uint32_t TimeOutMicroSecs)
mjarvisal 0:e6fcdb78a136 691 {
mjarvisal 0:e6fcdb78a136 692 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 693 uint8_t CurrentVCSELPulsePeriodPClk;
mjarvisal 0:e6fcdb78a136 694 uint8_t MsrcEncodedTimeOut;
mjarvisal 0:e6fcdb78a136 695 uint16_t PreRangeEncodedTimeOut;
mjarvisal 0:e6fcdb78a136 696 uint16_t PreRangeTimeOutMClks;
mjarvisal 0:e6fcdb78a136 697 uint16_t MsrcRangeTimeOutMClks;
mjarvisal 0:e6fcdb78a136 698 uint16_t FinalRangeTimeOutMClks;
mjarvisal 0:e6fcdb78a136 699 uint16_t FinalRangeEncodedTimeOut;
mjarvisal 0:e6fcdb78a136 700 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
mjarvisal 0:e6fcdb78a136 701
mjarvisal 0:e6fcdb78a136 702 if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) ||
mjarvisal 0:e6fcdb78a136 703 (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) ||
mjarvisal 0:e6fcdb78a136 704 (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) {
mjarvisal 0:e6fcdb78a136 705
mjarvisal 0:e6fcdb78a136 706 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 707 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 708 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 709
mjarvisal 0:e6fcdb78a136 710 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 711 MsrcRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev,
mjarvisal 0:e6fcdb78a136 712 TimeOutMicroSecs,
mjarvisal 0:e6fcdb78a136 713 (uint8_t)CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 714
mjarvisal 0:e6fcdb78a136 715 if (MsrcRangeTimeOutMClks > 256)
mjarvisal 0:e6fcdb78a136 716 MsrcEncodedTimeOut = 255;
mjarvisal 0:e6fcdb78a136 717 else
mjarvisal 0:e6fcdb78a136 718 MsrcEncodedTimeOut =
mjarvisal 0:e6fcdb78a136 719 (uint8_t)MsrcRangeTimeOutMClks - 1;
mjarvisal 0:e6fcdb78a136 720
mjarvisal 0:e6fcdb78a136 721 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 722 LastEncodedTimeout,
mjarvisal 0:e6fcdb78a136 723 MsrcEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 724 }
mjarvisal 0:e6fcdb78a136 725
mjarvisal 0:e6fcdb78a136 726 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 727 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 728 VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
mjarvisal 0:e6fcdb78a136 729 MsrcEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 730 }
mjarvisal 0:e6fcdb78a136 731 } else {
mjarvisal 0:e6fcdb78a136 732
mjarvisal 0:e6fcdb78a136 733 if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
mjarvisal 0:e6fcdb78a136 734
mjarvisal 0:e6fcdb78a136 735 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 736 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 737 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 738 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 739 PreRangeTimeOutMClks =
mjarvisal 0:e6fcdb78a136 740 VL53L0X_calc_timeout_mclks(Dev,
mjarvisal 0:e6fcdb78a136 741 TimeOutMicroSecs,
mjarvisal 0:e6fcdb78a136 742 (uint8_t)CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 743 PreRangeEncodedTimeOut = VL53L0X_encode_timeout(
mjarvisal 0:e6fcdb78a136 744 PreRangeTimeOutMClks);
mjarvisal 0:e6fcdb78a136 745
mjarvisal 0:e6fcdb78a136 746 VL53L0X_SETDEVICESPECIFICPARAMETER(Dev,
mjarvisal 0:e6fcdb78a136 747 LastEncodedTimeout,
mjarvisal 0:e6fcdb78a136 748 PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 749 }
mjarvisal 0:e6fcdb78a136 750
mjarvisal 0:e6fcdb78a136 751 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 752 Status = VL53L0X_WrWord(Dev,
mjarvisal 0:e6fcdb78a136 753 VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
mjarvisal 0:e6fcdb78a136 754 PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 755 }
mjarvisal 0:e6fcdb78a136 756
mjarvisal 0:e6fcdb78a136 757 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 758 VL53L0X_SETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 759 Dev,
mjarvisal 0:e6fcdb78a136 760 PreRangeTimeoutMicroSecs,
mjarvisal 0:e6fcdb78a136 761 TimeOutMicroSecs);
mjarvisal 0:e6fcdb78a136 762 }
mjarvisal 0:e6fcdb78a136 763 } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
mjarvisal 0:e6fcdb78a136 764
mjarvisal 0:e6fcdb78a136 765 /* For the final range timeout, the pre-range timeout
mjarvisal 0:e6fcdb78a136 766 * must be added. To do this both final and pre-range
mjarvisal 0:e6fcdb78a136 767 * timeouts must be expressed in macro periods MClks
mjarvisal 0:e6fcdb78a136 768 * because they have different vcsel periods.
mjarvisal 0:e6fcdb78a136 769 */
mjarvisal 0:e6fcdb78a136 770
mjarvisal 0:e6fcdb78a136 771 VL53L0X_GetSequenceStepEnables(Dev,
mjarvisal 0:e6fcdb78a136 772 &SchedulerSequenceSteps);
mjarvisal 0:e6fcdb78a136 773 PreRangeTimeOutMClks = 0;
mjarvisal 0:e6fcdb78a136 774 if (SchedulerSequenceSteps.PreRangeOn) {
mjarvisal 0:e6fcdb78a136 775
mjarvisal 0:e6fcdb78a136 776 /* Retrieve PRE-RANGE VCSEL Period */
mjarvisal 0:e6fcdb78a136 777 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 778 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 779 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 780
mjarvisal 0:e6fcdb78a136 781 /* Retrieve PRE-RANGE Timeout in Macro periods
mjarvisal 0:e6fcdb78a136 782 * (MCLKS) */
mjarvisal 0:e6fcdb78a136 783 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 784 Status = VL53L0X_RdWord(Dev, 0x51,
mjarvisal 0:e6fcdb78a136 785 &PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 786 PreRangeTimeOutMClks =
mjarvisal 0:e6fcdb78a136 787 VL53L0X_decode_timeout(
mjarvisal 0:e6fcdb78a136 788 PreRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 789 }
mjarvisal 0:e6fcdb78a136 790 }
mjarvisal 0:e6fcdb78a136 791
mjarvisal 0:e6fcdb78a136 792 /* Calculate FINAL RANGE Timeout in Macro Periods
mjarvisal 0:e6fcdb78a136 793 * (MCLKS) and add PRE-RANGE value
mjarvisal 0:e6fcdb78a136 794 */
mjarvisal 0:e6fcdb78a136 795 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 796
mjarvisal 0:e6fcdb78a136 797 Status = VL53L0X_GetVcselPulsePeriod(Dev,
mjarvisal 0:e6fcdb78a136 798 VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 799 &CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 800 }
mjarvisal 0:e6fcdb78a136 801 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 802
mjarvisal 0:e6fcdb78a136 803 FinalRangeTimeOutMClks =
mjarvisal 0:e6fcdb78a136 804 VL53L0X_calc_timeout_mclks(Dev,
mjarvisal 0:e6fcdb78a136 805 TimeOutMicroSecs,
mjarvisal 0:e6fcdb78a136 806 (uint8_t) CurrentVCSELPulsePeriodPClk);
mjarvisal 0:e6fcdb78a136 807
mjarvisal 0:e6fcdb78a136 808 FinalRangeTimeOutMClks += PreRangeTimeOutMClks;
mjarvisal 0:e6fcdb78a136 809
mjarvisal 0:e6fcdb78a136 810 FinalRangeEncodedTimeOut =
mjarvisal 0:e6fcdb78a136 811 VL53L0X_encode_timeout(FinalRangeTimeOutMClks);
mjarvisal 0:e6fcdb78a136 812
mjarvisal 0:e6fcdb78a136 813 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 814 Status = VL53L0X_WrWord(Dev, 0x71,
mjarvisal 0:e6fcdb78a136 815 FinalRangeEncodedTimeOut);
mjarvisal 0:e6fcdb78a136 816 }
mjarvisal 0:e6fcdb78a136 817
mjarvisal 0:e6fcdb78a136 818 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 819 VL53L0X_SETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 820 Dev,
mjarvisal 0:e6fcdb78a136 821 FinalRangeTimeoutMicroSecs,
mjarvisal 0:e6fcdb78a136 822 TimeOutMicroSecs);
mjarvisal 0:e6fcdb78a136 823 }
mjarvisal 0:e6fcdb78a136 824 }
mjarvisal 0:e6fcdb78a136 825 } else
mjarvisal 0:e6fcdb78a136 826 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 827
mjarvisal 0:e6fcdb78a136 828 }
mjarvisal 0:e6fcdb78a136 829 return Status;
mjarvisal 0:e6fcdb78a136 830 }
mjarvisal 0:e6fcdb78a136 831
mjarvisal 0:e6fcdb78a136 832 VL53L0X_Error VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 833 VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK)
mjarvisal 0:e6fcdb78a136 834 {
mjarvisal 0:e6fcdb78a136 835 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 836 uint8_t vcsel_period_reg;
mjarvisal 0:e6fcdb78a136 837 uint8_t MinPreVcselPeriodPCLK = 12;
mjarvisal 0:e6fcdb78a136 838 uint8_t MaxPreVcselPeriodPCLK = 18;
mjarvisal 0:e6fcdb78a136 839 uint8_t MinFinalVcselPeriodPCLK = 8;
mjarvisal 0:e6fcdb78a136 840 uint8_t MaxFinalVcselPeriodPCLK = 14;
mjarvisal 0:e6fcdb78a136 841 uint32_t MeasurementTimingBudgetMicroSeconds;
mjarvisal 0:e6fcdb78a136 842 uint32_t FinalRangeTimeoutMicroSeconds;
mjarvisal 0:e6fcdb78a136 843 uint32_t PreRangeTimeoutMicroSeconds;
mjarvisal 0:e6fcdb78a136 844 uint32_t MsrcTimeoutMicroSeconds;
mjarvisal 0:e6fcdb78a136 845 uint8_t PhaseCalInt = 0;
mjarvisal 0:e6fcdb78a136 846
mjarvisal 0:e6fcdb78a136 847 /* Check if valid clock period requested */
mjarvisal 0:e6fcdb78a136 848
mjarvisal 0:e6fcdb78a136 849 if ((VCSELPulsePeriodPCLK % 2) != 0) {
mjarvisal 0:e6fcdb78a136 850 /* Value must be an even number */
mjarvisal 0:e6fcdb78a136 851 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 852 } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
mjarvisal 0:e6fcdb78a136 853 (VCSELPulsePeriodPCLK < MinPreVcselPeriodPCLK ||
mjarvisal 0:e6fcdb78a136 854 VCSELPulsePeriodPCLK > MaxPreVcselPeriodPCLK)) {
mjarvisal 0:e6fcdb78a136 855 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 856 } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
mjarvisal 0:e6fcdb78a136 857 (VCSELPulsePeriodPCLK < MinFinalVcselPeriodPCLK ||
mjarvisal 0:e6fcdb78a136 858 VCSELPulsePeriodPCLK > MaxFinalVcselPeriodPCLK)) {
mjarvisal 0:e6fcdb78a136 859
mjarvisal 0:e6fcdb78a136 860 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 861 }
mjarvisal 0:e6fcdb78a136 862
mjarvisal 0:e6fcdb78a136 863 /* Apply specific settings for the requested clock period */
mjarvisal 0:e6fcdb78a136 864
mjarvisal 0:e6fcdb78a136 865 if (Status != VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 866 return Status;
mjarvisal 0:e6fcdb78a136 867
mjarvisal 0:e6fcdb78a136 868
mjarvisal 0:e6fcdb78a136 869 if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
mjarvisal 0:e6fcdb78a136 870
mjarvisal 0:e6fcdb78a136 871 /* Set phase check limits */
mjarvisal 0:e6fcdb78a136 872 if (VCSELPulsePeriodPCLK == 12) {
mjarvisal 0:e6fcdb78a136 873
mjarvisal 0:e6fcdb78a136 874 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 875 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 876 0x18);
mjarvisal 0:e6fcdb78a136 877 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 878 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 879 0x08);
mjarvisal 0:e6fcdb78a136 880 } else if (VCSELPulsePeriodPCLK == 14) {
mjarvisal 0:e6fcdb78a136 881
mjarvisal 0:e6fcdb78a136 882 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 883 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 884 0x30);
mjarvisal 0:e6fcdb78a136 885 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 886 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 887 0x08);
mjarvisal 0:e6fcdb78a136 888 } else if (VCSELPulsePeriodPCLK == 16) {
mjarvisal 0:e6fcdb78a136 889
mjarvisal 0:e6fcdb78a136 890 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 891 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 892 0x40);
mjarvisal 0:e6fcdb78a136 893 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 894 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 895 0x08);
mjarvisal 0:e6fcdb78a136 896 } else if (VCSELPulsePeriodPCLK == 18) {
mjarvisal 0:e6fcdb78a136 897
mjarvisal 0:e6fcdb78a136 898 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 899 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 900 0x50);
mjarvisal 0:e6fcdb78a136 901 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 902 VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 903 0x08);
mjarvisal 0:e6fcdb78a136 904 }
mjarvisal 0:e6fcdb78a136 905 } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
mjarvisal 0:e6fcdb78a136 906
mjarvisal 0:e6fcdb78a136 907 if (VCSELPulsePeriodPCLK == 8) {
mjarvisal 0:e6fcdb78a136 908
mjarvisal 0:e6fcdb78a136 909 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 910 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 911 0x10);
mjarvisal 0:e6fcdb78a136 912 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 913 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 914 0x08);
mjarvisal 0:e6fcdb78a136 915
mjarvisal 0:e6fcdb78a136 916 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 917 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
mjarvisal 0:e6fcdb78a136 918 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 919 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
mjarvisal 0:e6fcdb78a136 920
mjarvisal 0:e6fcdb78a136 921 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
mjarvisal 0:e6fcdb78a136 922 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 923 VL53L0X_REG_ALGO_PHASECAL_LIM,
mjarvisal 0:e6fcdb78a136 924 0x30);
mjarvisal 0:e6fcdb78a136 925 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
mjarvisal 0:e6fcdb78a136 926 } else if (VCSELPulsePeriodPCLK == 10) {
mjarvisal 0:e6fcdb78a136 927
mjarvisal 0:e6fcdb78a136 928 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 929 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 930 0x28);
mjarvisal 0:e6fcdb78a136 931 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 932 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 933 0x08);
mjarvisal 0:e6fcdb78a136 934
mjarvisal 0:e6fcdb78a136 935 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 936 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
mjarvisal 0:e6fcdb78a136 937 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 938 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
mjarvisal 0:e6fcdb78a136 939
mjarvisal 0:e6fcdb78a136 940 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
mjarvisal 0:e6fcdb78a136 941 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 942 VL53L0X_REG_ALGO_PHASECAL_LIM,
mjarvisal 0:e6fcdb78a136 943 0x20);
mjarvisal 0:e6fcdb78a136 944 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
mjarvisal 0:e6fcdb78a136 945 } else if (VCSELPulsePeriodPCLK == 12) {
mjarvisal 0:e6fcdb78a136 946
mjarvisal 0:e6fcdb78a136 947 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 948 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 949 0x38);
mjarvisal 0:e6fcdb78a136 950 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 951 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 952 0x08);
mjarvisal 0:e6fcdb78a136 953
mjarvisal 0:e6fcdb78a136 954 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 955 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
mjarvisal 0:e6fcdb78a136 956 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 957 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
mjarvisal 0:e6fcdb78a136 958
mjarvisal 0:e6fcdb78a136 959 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
mjarvisal 0:e6fcdb78a136 960 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 961 VL53L0X_REG_ALGO_PHASECAL_LIM,
mjarvisal 0:e6fcdb78a136 962 0x20);
mjarvisal 0:e6fcdb78a136 963 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
mjarvisal 0:e6fcdb78a136 964 } else if (VCSELPulsePeriodPCLK == 14) {
mjarvisal 0:e6fcdb78a136 965
mjarvisal 0:e6fcdb78a136 966 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 967 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
mjarvisal 0:e6fcdb78a136 968 0x048);
mjarvisal 0:e6fcdb78a136 969 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 970 VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
mjarvisal 0:e6fcdb78a136 971 0x08);
mjarvisal 0:e6fcdb78a136 972
mjarvisal 0:e6fcdb78a136 973 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 974 VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
mjarvisal 0:e6fcdb78a136 975 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 976 VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
mjarvisal 0:e6fcdb78a136 977
mjarvisal 0:e6fcdb78a136 978 Status |= VL53L0X_WrByte(Dev, 0xff, 0x01);
mjarvisal 0:e6fcdb78a136 979 Status |= VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 980 VL53L0X_REG_ALGO_PHASECAL_LIM,
mjarvisal 0:e6fcdb78a136 981 0x20);
mjarvisal 0:e6fcdb78a136 982 Status |= VL53L0X_WrByte(Dev, 0xff, 0x00);
mjarvisal 0:e6fcdb78a136 983 }
mjarvisal 0:e6fcdb78a136 984 }
mjarvisal 0:e6fcdb78a136 985
mjarvisal 0:e6fcdb78a136 986
mjarvisal 0:e6fcdb78a136 987 /* Re-calculate and apply timeouts, in macro periods */
mjarvisal 0:e6fcdb78a136 988
mjarvisal 0:e6fcdb78a136 989 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 990 vcsel_period_reg = VL53L0X_encode_vcsel_period((uint8_t)
mjarvisal 0:e6fcdb78a136 991 VCSELPulsePeriodPCLK);
mjarvisal 0:e6fcdb78a136 992
mjarvisal 0:e6fcdb78a136 993 /* When the VCSEL period for the pre or final range is changed,
mjarvisal 0:e6fcdb78a136 994 * the corresponding timeout must be read from the device using
mjarvisal 0:e6fcdb78a136 995 * the current VCSEL period, then the new VCSEL period can be
mjarvisal 0:e6fcdb78a136 996 * applied. The timeout then must be written back to the device
mjarvisal 0:e6fcdb78a136 997 * using the new VCSEL period.
mjarvisal 0:e6fcdb78a136 998 *
mjarvisal 0:e6fcdb78a136 999 * For the MSRC timeout, the same applies - this timeout being
mjarvisal 0:e6fcdb78a136 1000 * dependant on the pre-range vcsel period.
mjarvisal 0:e6fcdb78a136 1001 */
mjarvisal 0:e6fcdb78a136 1002 switch (VcselPeriodType) {
mjarvisal 0:e6fcdb78a136 1003 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
mjarvisal 0:e6fcdb78a136 1004 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1005 VL53L0X_SEQUENCESTEP_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 1006 &PreRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1007
mjarvisal 0:e6fcdb78a136 1008 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1009 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1010 VL53L0X_SEQUENCESTEP_MSRC,
mjarvisal 0:e6fcdb78a136 1011 &MsrcTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1012
mjarvisal 0:e6fcdb78a136 1013 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1014 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 1015 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
mjarvisal 0:e6fcdb78a136 1016 vcsel_period_reg);
mjarvisal 0:e6fcdb78a136 1017
mjarvisal 0:e6fcdb78a136 1018
mjarvisal 0:e6fcdb78a136 1019 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1020 Status = set_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1021 VL53L0X_SEQUENCESTEP_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 1022 PreRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1023
mjarvisal 0:e6fcdb78a136 1024
mjarvisal 0:e6fcdb78a136 1025 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1026 Status = set_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1027 VL53L0X_SEQUENCESTEP_MSRC,
mjarvisal 0:e6fcdb78a136 1028 MsrcTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1029
mjarvisal 0:e6fcdb78a136 1030 VL53L0X_SETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 1031 Dev,
mjarvisal 0:e6fcdb78a136 1032 PreRangeVcselPulsePeriod,
mjarvisal 0:e6fcdb78a136 1033 VCSELPulsePeriodPCLK);
mjarvisal 0:e6fcdb78a136 1034 break;
mjarvisal 0:e6fcdb78a136 1035 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
mjarvisal 0:e6fcdb78a136 1036 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1037 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 1038 &FinalRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1039
mjarvisal 0:e6fcdb78a136 1040 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1041 Status = VL53L0X_WrByte(Dev,
mjarvisal 0:e6fcdb78a136 1042 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
mjarvisal 0:e6fcdb78a136 1043 vcsel_period_reg);
mjarvisal 0:e6fcdb78a136 1044
mjarvisal 0:e6fcdb78a136 1045
mjarvisal 0:e6fcdb78a136 1046 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1047 Status = set_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1048 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 1049 FinalRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1050
mjarvisal 0:e6fcdb78a136 1051 VL53L0X_SETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 1052 Dev,
mjarvisal 0:e6fcdb78a136 1053 FinalRangeVcselPulsePeriod,
mjarvisal 0:e6fcdb78a136 1054 VCSELPulsePeriodPCLK);
mjarvisal 0:e6fcdb78a136 1055 break;
mjarvisal 0:e6fcdb78a136 1056 default:
mjarvisal 0:e6fcdb78a136 1057 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1058 }
mjarvisal 0:e6fcdb78a136 1059 }
mjarvisal 0:e6fcdb78a136 1060
mjarvisal 0:e6fcdb78a136 1061 /* Finally, the timing budget must be re-applied */
mjarvisal 0:e6fcdb78a136 1062 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1063 VL53L0X_GETPARAMETERFIELD(Dev,
mjarvisal 0:e6fcdb78a136 1064 MeasurementTimingBudgetMicroSeconds,
mjarvisal 0:e6fcdb78a136 1065 MeasurementTimingBudgetMicroSeconds);
mjarvisal 0:e6fcdb78a136 1066
mjarvisal 0:e6fcdb78a136 1067 Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev,
mjarvisal 0:e6fcdb78a136 1068 MeasurementTimingBudgetMicroSeconds);
mjarvisal 0:e6fcdb78a136 1069 }
mjarvisal 0:e6fcdb78a136 1070
mjarvisal 0:e6fcdb78a136 1071 /* Perform the phase calibration. This is needed after changing on
mjarvisal 0:e6fcdb78a136 1072 * vcsel period.
mjarvisal 0:e6fcdb78a136 1073 * get_data_enable = 0, restore_config = 1 */
mjarvisal 0:e6fcdb78a136 1074 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1075 Status = VL53L0X_perform_phase_calibration(
mjarvisal 0:e6fcdb78a136 1076 Dev, &PhaseCalInt, 0, 1);
mjarvisal 0:e6fcdb78a136 1077
mjarvisal 0:e6fcdb78a136 1078 return Status;
mjarvisal 0:e6fcdb78a136 1079 }
mjarvisal 0:e6fcdb78a136 1080
mjarvisal 0:e6fcdb78a136 1081 VL53L0X_Error VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1082 VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK)
mjarvisal 0:e6fcdb78a136 1083 {
mjarvisal 0:e6fcdb78a136 1084 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1085 uint8_t vcsel_period_reg;
mjarvisal 0:e6fcdb78a136 1086
mjarvisal 0:e6fcdb78a136 1087 switch (VcselPeriodType) {
mjarvisal 0:e6fcdb78a136 1088 case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
mjarvisal 0:e6fcdb78a136 1089 Status = VL53L0X_RdByte(Dev,
mjarvisal 0:e6fcdb78a136 1090 VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
mjarvisal 0:e6fcdb78a136 1091 &vcsel_period_reg);
mjarvisal 0:e6fcdb78a136 1092 break;
mjarvisal 0:e6fcdb78a136 1093 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
mjarvisal 0:e6fcdb78a136 1094 Status = VL53L0X_RdByte(Dev,
mjarvisal 0:e6fcdb78a136 1095 VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
mjarvisal 0:e6fcdb78a136 1096 &vcsel_period_reg);
mjarvisal 0:e6fcdb78a136 1097 break;
mjarvisal 0:e6fcdb78a136 1098 default:
mjarvisal 0:e6fcdb78a136 1099 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1100 }
mjarvisal 0:e6fcdb78a136 1101
mjarvisal 0:e6fcdb78a136 1102 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1103 *pVCSELPulsePeriodPCLK =
mjarvisal 0:e6fcdb78a136 1104 VL53L0X_decode_vcsel_period(vcsel_period_reg);
mjarvisal 0:e6fcdb78a136 1105
mjarvisal 0:e6fcdb78a136 1106 return Status;
mjarvisal 0:e6fcdb78a136 1107 }
mjarvisal 0:e6fcdb78a136 1108
mjarvisal 0:e6fcdb78a136 1109
mjarvisal 0:e6fcdb78a136 1110
mjarvisal 0:e6fcdb78a136 1111 VL53L0X_Error VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1112 uint32_t MeasurementTimingBudgetMicroSeconds)
mjarvisal 0:e6fcdb78a136 1113 {
mjarvisal 0:e6fcdb78a136 1114 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1115 uint32_t FinalRangeTimingBudgetMicroSeconds;
mjarvisal 0:e6fcdb78a136 1116 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
mjarvisal 0:e6fcdb78a136 1117 uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
mjarvisal 0:e6fcdb78a136 1118 uint32_t StartOverheadMicroSeconds = 1320;
mjarvisal 0:e6fcdb78a136 1119 uint32_t EndOverheadMicroSeconds = 960;
mjarvisal 0:e6fcdb78a136 1120 uint32_t MsrcOverheadMicroSeconds = 660;
mjarvisal 0:e6fcdb78a136 1121 uint32_t TccOverheadMicroSeconds = 590;
mjarvisal 0:e6fcdb78a136 1122 uint32_t DssOverheadMicroSeconds = 690;
mjarvisal 0:e6fcdb78a136 1123 uint32_t PreRangeOverheadMicroSeconds = 660;
mjarvisal 0:e6fcdb78a136 1124 uint32_t FinalRangeOverheadMicroSeconds = 550;
mjarvisal 0:e6fcdb78a136 1125 uint32_t PreRangeTimeoutMicroSeconds = 0;
mjarvisal 0:e6fcdb78a136 1126 uint32_t cMinTimingBudgetMicroSeconds = 20000;
mjarvisal 0:e6fcdb78a136 1127 uint32_t SubTimeout = 0;
mjarvisal 0:e6fcdb78a136 1128
mjarvisal 0:e6fcdb78a136 1129 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 1130
mjarvisal 0:e6fcdb78a136 1131 if (MeasurementTimingBudgetMicroSeconds
mjarvisal 0:e6fcdb78a136 1132 < cMinTimingBudgetMicroSeconds) {
mjarvisal 0:e6fcdb78a136 1133 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1134 return Status;
mjarvisal 0:e6fcdb78a136 1135 }
mjarvisal 0:e6fcdb78a136 1136
mjarvisal 0:e6fcdb78a136 1137 FinalRangeTimingBudgetMicroSeconds =
mjarvisal 0:e6fcdb78a136 1138 MeasurementTimingBudgetMicroSeconds -
mjarvisal 0:e6fcdb78a136 1139 (StartOverheadMicroSeconds + EndOverheadMicroSeconds);
mjarvisal 0:e6fcdb78a136 1140
mjarvisal 0:e6fcdb78a136 1141 Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
mjarvisal 0:e6fcdb78a136 1142
mjarvisal 0:e6fcdb78a136 1143 if (Status == VL53L0X_ERROR_NONE &&
mjarvisal 0:e6fcdb78a136 1144 (SchedulerSequenceSteps.TccOn ||
mjarvisal 0:e6fcdb78a136 1145 SchedulerSequenceSteps.MsrcOn ||
mjarvisal 0:e6fcdb78a136 1146 SchedulerSequenceSteps.DssOn)) {
mjarvisal 0:e6fcdb78a136 1147
mjarvisal 0:e6fcdb78a136 1148 /* TCC, MSRC and DSS all share the same timeout */
mjarvisal 0:e6fcdb78a136 1149 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1150 VL53L0X_SEQUENCESTEP_MSRC,
mjarvisal 0:e6fcdb78a136 1151 &MsrcDccTccTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1152
mjarvisal 0:e6fcdb78a136 1153 /* Subtract the TCC, MSRC and DSS timeouts if they are
mjarvisal 0:e6fcdb78a136 1154 * enabled. */
mjarvisal 0:e6fcdb78a136 1155
mjarvisal 0:e6fcdb78a136 1156 if (Status != VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1157 return Status;
mjarvisal 0:e6fcdb78a136 1158
mjarvisal 0:e6fcdb78a136 1159 /* TCC */
mjarvisal 0:e6fcdb78a136 1160 if (SchedulerSequenceSteps.TccOn) {
mjarvisal 0:e6fcdb78a136 1161
mjarvisal 0:e6fcdb78a136 1162 SubTimeout = MsrcDccTccTimeoutMicroSeconds
mjarvisal 0:e6fcdb78a136 1163 + TccOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1164
mjarvisal 0:e6fcdb78a136 1165 if (SubTimeout <
mjarvisal 0:e6fcdb78a136 1166 FinalRangeTimingBudgetMicroSeconds) {
mjarvisal 0:e6fcdb78a136 1167 FinalRangeTimingBudgetMicroSeconds -=
mjarvisal 0:e6fcdb78a136 1168 SubTimeout;
mjarvisal 0:e6fcdb78a136 1169 } else {
mjarvisal 0:e6fcdb78a136 1170 /* Requested timeout too big. */
mjarvisal 0:e6fcdb78a136 1171 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1172 }
mjarvisal 0:e6fcdb78a136 1173 }
mjarvisal 0:e6fcdb78a136 1174
mjarvisal 0:e6fcdb78a136 1175 if (Status != VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1176 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1177 return Status;
mjarvisal 0:e6fcdb78a136 1178 }
mjarvisal 0:e6fcdb78a136 1179
mjarvisal 0:e6fcdb78a136 1180 /* DSS */
mjarvisal 0:e6fcdb78a136 1181 if (SchedulerSequenceSteps.DssOn) {
mjarvisal 0:e6fcdb78a136 1182
mjarvisal 0:e6fcdb78a136 1183 SubTimeout = 2 * (MsrcDccTccTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1184 DssOverheadMicroSeconds);
mjarvisal 0:e6fcdb78a136 1185
mjarvisal 0:e6fcdb78a136 1186 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
mjarvisal 0:e6fcdb78a136 1187 FinalRangeTimingBudgetMicroSeconds
mjarvisal 0:e6fcdb78a136 1188 -= SubTimeout;
mjarvisal 0:e6fcdb78a136 1189 } else {
mjarvisal 0:e6fcdb78a136 1190 /* Requested timeout too big. */
mjarvisal 0:e6fcdb78a136 1191 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1192 }
mjarvisal 0:e6fcdb78a136 1193 } else if (SchedulerSequenceSteps.MsrcOn) {
mjarvisal 0:e6fcdb78a136 1194 /* MSRC */
mjarvisal 0:e6fcdb78a136 1195 SubTimeout = MsrcDccTccTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1196 MsrcOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1197
mjarvisal 0:e6fcdb78a136 1198 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
mjarvisal 0:e6fcdb78a136 1199 FinalRangeTimingBudgetMicroSeconds
mjarvisal 0:e6fcdb78a136 1200 -= SubTimeout;
mjarvisal 0:e6fcdb78a136 1201 } else {
mjarvisal 0:e6fcdb78a136 1202 /* Requested timeout too big. */
mjarvisal 0:e6fcdb78a136 1203 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1204 }
mjarvisal 0:e6fcdb78a136 1205 }
mjarvisal 0:e6fcdb78a136 1206
mjarvisal 0:e6fcdb78a136 1207 }
mjarvisal 0:e6fcdb78a136 1208
mjarvisal 0:e6fcdb78a136 1209 if (Status != VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1210 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1211 return Status;
mjarvisal 0:e6fcdb78a136 1212 }
mjarvisal 0:e6fcdb78a136 1213
mjarvisal 0:e6fcdb78a136 1214 if (SchedulerSequenceSteps.PreRangeOn) {
mjarvisal 0:e6fcdb78a136 1215
mjarvisal 0:e6fcdb78a136 1216 /* Subtract the Pre-range timeout if enabled. */
mjarvisal 0:e6fcdb78a136 1217
mjarvisal 0:e6fcdb78a136 1218 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1219 VL53L0X_SEQUENCESTEP_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 1220 &PreRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1221
mjarvisal 0:e6fcdb78a136 1222 SubTimeout = PreRangeTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1223 PreRangeOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1224
mjarvisal 0:e6fcdb78a136 1225 if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) {
mjarvisal 0:e6fcdb78a136 1226 FinalRangeTimingBudgetMicroSeconds -= SubTimeout;
mjarvisal 0:e6fcdb78a136 1227 } else {
mjarvisal 0:e6fcdb78a136 1228 /* Requested timeout too big. */
mjarvisal 0:e6fcdb78a136 1229 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1230 }
mjarvisal 0:e6fcdb78a136 1231 }
mjarvisal 0:e6fcdb78a136 1232
mjarvisal 0:e6fcdb78a136 1233
mjarvisal 0:e6fcdb78a136 1234 if (Status == VL53L0X_ERROR_NONE &&
mjarvisal 0:e6fcdb78a136 1235 SchedulerSequenceSteps.FinalRangeOn) {
mjarvisal 0:e6fcdb78a136 1236
mjarvisal 0:e6fcdb78a136 1237 FinalRangeTimingBudgetMicroSeconds -=
mjarvisal 0:e6fcdb78a136 1238 FinalRangeOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1239
mjarvisal 0:e6fcdb78a136 1240 /* Final Range Timeout
mjarvisal 0:e6fcdb78a136 1241 * Note that the final range timeout is determined by the timing
mjarvisal 0:e6fcdb78a136 1242 * budget and the sum of all other timeouts within the sequence.
mjarvisal 0:e6fcdb78a136 1243 * If there is no room for the final range timeout, then an error
mjarvisal 0:e6fcdb78a136 1244 * will be set. Otherwise the remaining time will be applied to
mjarvisal 0:e6fcdb78a136 1245 * the final range.
mjarvisal 0:e6fcdb78a136 1246 */
mjarvisal 0:e6fcdb78a136 1247 Status = set_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1248 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 1249 FinalRangeTimingBudgetMicroSeconds);
mjarvisal 0:e6fcdb78a136 1250
mjarvisal 0:e6fcdb78a136 1251 VL53L0X_SETPARAMETERFIELD(Dev,
mjarvisal 0:e6fcdb78a136 1252 MeasurementTimingBudgetMicroSeconds,
mjarvisal 0:e6fcdb78a136 1253 MeasurementTimingBudgetMicroSeconds);
mjarvisal 0:e6fcdb78a136 1254 }
mjarvisal 0:e6fcdb78a136 1255
mjarvisal 0:e6fcdb78a136 1256 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1257
mjarvisal 0:e6fcdb78a136 1258 return Status;
mjarvisal 0:e6fcdb78a136 1259 }
mjarvisal 0:e6fcdb78a136 1260
mjarvisal 0:e6fcdb78a136 1261 VL53L0X_Error VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1262 uint32_t *pMeasurementTimingBudgetMicroSeconds)
mjarvisal 0:e6fcdb78a136 1263 {
mjarvisal 0:e6fcdb78a136 1264 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1265 VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps;
mjarvisal 0:e6fcdb78a136 1266 uint32_t FinalRangeTimeoutMicroSeconds;
mjarvisal 0:e6fcdb78a136 1267 uint32_t MsrcDccTccTimeoutMicroSeconds = 2000;
mjarvisal 0:e6fcdb78a136 1268 uint32_t StartOverheadMicroSeconds = 1910;
mjarvisal 0:e6fcdb78a136 1269 uint32_t EndOverheadMicroSeconds = 960;
mjarvisal 0:e6fcdb78a136 1270 uint32_t MsrcOverheadMicroSeconds = 660;
mjarvisal 0:e6fcdb78a136 1271 uint32_t TccOverheadMicroSeconds = 590;
mjarvisal 0:e6fcdb78a136 1272 uint32_t DssOverheadMicroSeconds = 690;
mjarvisal 0:e6fcdb78a136 1273 uint32_t PreRangeOverheadMicroSeconds = 660;
mjarvisal 0:e6fcdb78a136 1274 uint32_t FinalRangeOverheadMicroSeconds = 550;
mjarvisal 0:e6fcdb78a136 1275 uint32_t PreRangeTimeoutMicroSeconds = 0;
mjarvisal 0:e6fcdb78a136 1276
mjarvisal 0:e6fcdb78a136 1277 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 1278
mjarvisal 0:e6fcdb78a136 1279 /* Start and end overhead times always present */
mjarvisal 0:e6fcdb78a136 1280 *pMeasurementTimingBudgetMicroSeconds
mjarvisal 0:e6fcdb78a136 1281 = StartOverheadMicroSeconds + EndOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1282
mjarvisal 0:e6fcdb78a136 1283 Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps);
mjarvisal 0:e6fcdb78a136 1284
mjarvisal 0:e6fcdb78a136 1285 if (Status != VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1286 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1287 return Status;
mjarvisal 0:e6fcdb78a136 1288 }
mjarvisal 0:e6fcdb78a136 1289
mjarvisal 0:e6fcdb78a136 1290
mjarvisal 0:e6fcdb78a136 1291 if (SchedulerSequenceSteps.TccOn ||
mjarvisal 0:e6fcdb78a136 1292 SchedulerSequenceSteps.MsrcOn ||
mjarvisal 0:e6fcdb78a136 1293 SchedulerSequenceSteps.DssOn) {
mjarvisal 0:e6fcdb78a136 1294
mjarvisal 0:e6fcdb78a136 1295 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1296 VL53L0X_SEQUENCESTEP_MSRC,
mjarvisal 0:e6fcdb78a136 1297 &MsrcDccTccTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1298
mjarvisal 0:e6fcdb78a136 1299 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1300 if (SchedulerSequenceSteps.TccOn) {
mjarvisal 0:e6fcdb78a136 1301 *pMeasurementTimingBudgetMicroSeconds +=
mjarvisal 0:e6fcdb78a136 1302 MsrcDccTccTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1303 TccOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1304 }
mjarvisal 0:e6fcdb78a136 1305
mjarvisal 0:e6fcdb78a136 1306 if (SchedulerSequenceSteps.DssOn) {
mjarvisal 0:e6fcdb78a136 1307 *pMeasurementTimingBudgetMicroSeconds +=
mjarvisal 0:e6fcdb78a136 1308 2 * (MsrcDccTccTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1309 DssOverheadMicroSeconds);
mjarvisal 0:e6fcdb78a136 1310 } else if (SchedulerSequenceSteps.MsrcOn) {
mjarvisal 0:e6fcdb78a136 1311 *pMeasurementTimingBudgetMicroSeconds +=
mjarvisal 0:e6fcdb78a136 1312 MsrcDccTccTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1313 MsrcOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1314 }
mjarvisal 0:e6fcdb78a136 1315 }
mjarvisal 0:e6fcdb78a136 1316 }
mjarvisal 0:e6fcdb78a136 1317
mjarvisal 0:e6fcdb78a136 1318 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1319 if (SchedulerSequenceSteps.PreRangeOn) {
mjarvisal 0:e6fcdb78a136 1320 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1321 VL53L0X_SEQUENCESTEP_PRE_RANGE,
mjarvisal 0:e6fcdb78a136 1322 &PreRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1323 *pMeasurementTimingBudgetMicroSeconds +=
mjarvisal 0:e6fcdb78a136 1324 PreRangeTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1325 PreRangeOverheadMicroSeconds;
mjarvisal 0:e6fcdb78a136 1326 }
mjarvisal 0:e6fcdb78a136 1327 }
mjarvisal 0:e6fcdb78a136 1328
mjarvisal 0:e6fcdb78a136 1329 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1330 if (SchedulerSequenceSteps.FinalRangeOn) {
mjarvisal 0:e6fcdb78a136 1331 Status = get_sequence_step_timeout(Dev,
mjarvisal 0:e6fcdb78a136 1332 VL53L0X_SEQUENCESTEP_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 1333 &FinalRangeTimeoutMicroSeconds);
mjarvisal 0:e6fcdb78a136 1334 *pMeasurementTimingBudgetMicroSeconds +=
mjarvisal 0:e6fcdb78a136 1335 (FinalRangeTimeoutMicroSeconds +
mjarvisal 0:e6fcdb78a136 1336 FinalRangeOverheadMicroSeconds);
mjarvisal 0:e6fcdb78a136 1337 }
mjarvisal 0:e6fcdb78a136 1338 }
mjarvisal 0:e6fcdb78a136 1339
mjarvisal 0:e6fcdb78a136 1340 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1341 VL53L0X_SETPARAMETERFIELD(Dev,
mjarvisal 0:e6fcdb78a136 1342 MeasurementTimingBudgetMicroSeconds,
mjarvisal 0:e6fcdb78a136 1343 *pMeasurementTimingBudgetMicroSeconds);
mjarvisal 0:e6fcdb78a136 1344 }
mjarvisal 0:e6fcdb78a136 1345
mjarvisal 0:e6fcdb78a136 1346 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1347 return Status;
mjarvisal 0:e6fcdb78a136 1348 }
mjarvisal 0:e6fcdb78a136 1349
mjarvisal 0:e6fcdb78a136 1350
mjarvisal 0:e6fcdb78a136 1351
mjarvisal 0:e6fcdb78a136 1352 VL53L0X_Error VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1353 uint8_t *pTuningSettingBuffer)
mjarvisal 0:e6fcdb78a136 1354 {
mjarvisal 0:e6fcdb78a136 1355 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1356 int i;
mjarvisal 0:e6fcdb78a136 1357 int Index;
mjarvisal 0:e6fcdb78a136 1358 uint8_t msb;
mjarvisal 0:e6fcdb78a136 1359 uint8_t lsb;
mjarvisal 0:e6fcdb78a136 1360 uint8_t SelectParam;
mjarvisal 0:e6fcdb78a136 1361 uint8_t NumberOfWrites;
mjarvisal 0:e6fcdb78a136 1362 uint8_t Address;
mjarvisal 0:e6fcdb78a136 1363 uint8_t localBuffer[4]; /* max */
mjarvisal 0:e6fcdb78a136 1364 uint16_t Temp16;
mjarvisal 0:e6fcdb78a136 1365
mjarvisal 0:e6fcdb78a136 1366 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 1367
mjarvisal 0:e6fcdb78a136 1368 Index = 0;
mjarvisal 0:e6fcdb78a136 1369
mjarvisal 0:e6fcdb78a136 1370 while ((*(pTuningSettingBuffer + Index) != 0) &&
mjarvisal 0:e6fcdb78a136 1371 (Status == VL53L0X_ERROR_NONE)) {
mjarvisal 0:e6fcdb78a136 1372 NumberOfWrites = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1373 Index++;
mjarvisal 0:e6fcdb78a136 1374 if (NumberOfWrites == 0xFF) {
mjarvisal 0:e6fcdb78a136 1375 /* internal parameters */
mjarvisal 0:e6fcdb78a136 1376 SelectParam = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1377 Index++;
mjarvisal 0:e6fcdb78a136 1378 switch (SelectParam) {
mjarvisal 0:e6fcdb78a136 1379 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
mjarvisal 0:e6fcdb78a136 1380 msb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1381 Index++;
mjarvisal 0:e6fcdb78a136 1382 lsb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1383 Index++;
mjarvisal 0:e6fcdb78a136 1384 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
mjarvisal 0:e6fcdb78a136 1385 PALDevDataSet(Dev, SigmaEstRefArray, Temp16);
mjarvisal 0:e6fcdb78a136 1386 break;
mjarvisal 0:e6fcdb78a136 1387 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
mjarvisal 0:e6fcdb78a136 1388 msb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1389 Index++;
mjarvisal 0:e6fcdb78a136 1390 lsb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1391 Index++;
mjarvisal 0:e6fcdb78a136 1392 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
mjarvisal 0:e6fcdb78a136 1393 PALDevDataSet(Dev, SigmaEstEffPulseWidth,
mjarvisal 0:e6fcdb78a136 1394 Temp16);
mjarvisal 0:e6fcdb78a136 1395 break;
mjarvisal 0:e6fcdb78a136 1396 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
mjarvisal 0:e6fcdb78a136 1397 msb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1398 Index++;
mjarvisal 0:e6fcdb78a136 1399 lsb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1400 Index++;
mjarvisal 0:e6fcdb78a136 1401 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
mjarvisal 0:e6fcdb78a136 1402 PALDevDataSet(Dev, SigmaEstEffAmbWidth, Temp16);
mjarvisal 0:e6fcdb78a136 1403 break;
mjarvisal 0:e6fcdb78a136 1404 case 3: /* uint16_t targetRefRate -> 2 bytes */
mjarvisal 0:e6fcdb78a136 1405 msb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1406 Index++;
mjarvisal 0:e6fcdb78a136 1407 lsb = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1408 Index++;
mjarvisal 0:e6fcdb78a136 1409 Temp16 = VL53L0X_MAKEUINT16(lsb, msb);
mjarvisal 0:e6fcdb78a136 1410 PALDevDataSet(Dev, targetRefRate, Temp16);
mjarvisal 0:e6fcdb78a136 1411 break;
mjarvisal 0:e6fcdb78a136 1412 default: /* invalid parameter */
mjarvisal 0:e6fcdb78a136 1413 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1414 }
mjarvisal 0:e6fcdb78a136 1415
mjarvisal 0:e6fcdb78a136 1416 } else if (NumberOfWrites <= 4) {
mjarvisal 0:e6fcdb78a136 1417 Address = *(pTuningSettingBuffer + Index);
mjarvisal 0:e6fcdb78a136 1418 Index++;
mjarvisal 0:e6fcdb78a136 1419
mjarvisal 0:e6fcdb78a136 1420 for (i = 0; i < NumberOfWrites; i++) {
mjarvisal 0:e6fcdb78a136 1421 localBuffer[i] = *(pTuningSettingBuffer +
mjarvisal 0:e6fcdb78a136 1422 Index);
mjarvisal 0:e6fcdb78a136 1423 Index++;
mjarvisal 0:e6fcdb78a136 1424 }
mjarvisal 0:e6fcdb78a136 1425
mjarvisal 0:e6fcdb78a136 1426 Status = VL53L0X_WriteMulti(Dev, Address, localBuffer,
mjarvisal 0:e6fcdb78a136 1427 NumberOfWrites);
mjarvisal 0:e6fcdb78a136 1428
mjarvisal 0:e6fcdb78a136 1429 } else {
mjarvisal 0:e6fcdb78a136 1430 Status = VL53L0X_ERROR_INVALID_PARAMS;
mjarvisal 0:e6fcdb78a136 1431 }
mjarvisal 0:e6fcdb78a136 1432 }
mjarvisal 0:e6fcdb78a136 1433
mjarvisal 0:e6fcdb78a136 1434 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1435 return Status;
mjarvisal 0:e6fcdb78a136 1436 }
mjarvisal 0:e6fcdb78a136 1437
mjarvisal 0:e6fcdb78a136 1438 VL53L0X_Error VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1439 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
mjarvisal 0:e6fcdb78a136 1440 FixPoint1616_t *ptotal_xtalk_rate_mcps)
mjarvisal 0:e6fcdb78a136 1441 {
mjarvisal 0:e6fcdb78a136 1442 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1443
mjarvisal 0:e6fcdb78a136 1444 uint8_t xtalkCompEnable;
mjarvisal 0:e6fcdb78a136 1445 FixPoint1616_t totalXtalkMegaCps;
mjarvisal 0:e6fcdb78a136 1446 FixPoint1616_t xtalkPerSpadMegaCps;
mjarvisal 0:e6fcdb78a136 1447
mjarvisal 0:e6fcdb78a136 1448 *ptotal_xtalk_rate_mcps = 0;
mjarvisal 0:e6fcdb78a136 1449
mjarvisal 0:e6fcdb78a136 1450 Status = VL53L0X_GetXTalkCompensationEnable(Dev, &xtalkCompEnable);
mjarvisal 0:e6fcdb78a136 1451 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1452
mjarvisal 0:e6fcdb78a136 1453 if (xtalkCompEnable) {
mjarvisal 0:e6fcdb78a136 1454
mjarvisal 0:e6fcdb78a136 1455 VL53L0X_GETPARAMETERFIELD(
mjarvisal 0:e6fcdb78a136 1456 Dev,
mjarvisal 0:e6fcdb78a136 1457 XTalkCompensationRateMegaCps,
mjarvisal 0:e6fcdb78a136 1458 xtalkPerSpadMegaCps);
mjarvisal 0:e6fcdb78a136 1459
mjarvisal 0:e6fcdb78a136 1460 /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
mjarvisal 0:e6fcdb78a136 1461 totalXtalkMegaCps =
mjarvisal 0:e6fcdb78a136 1462 pRangingMeasurementData->EffectiveSpadRtnCount *
mjarvisal 0:e6fcdb78a136 1463 xtalkPerSpadMegaCps;
mjarvisal 0:e6fcdb78a136 1464
mjarvisal 0:e6fcdb78a136 1465 /* FixPoint0824 >> 8 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1466 *ptotal_xtalk_rate_mcps =
mjarvisal 0:e6fcdb78a136 1467 (totalXtalkMegaCps + 0x80) >> 8;
mjarvisal 0:e6fcdb78a136 1468 }
mjarvisal 0:e6fcdb78a136 1469 }
mjarvisal 0:e6fcdb78a136 1470
mjarvisal 0:e6fcdb78a136 1471 return Status;
mjarvisal 0:e6fcdb78a136 1472 }
mjarvisal 0:e6fcdb78a136 1473
mjarvisal 0:e6fcdb78a136 1474 VL53L0X_Error VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1475 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
mjarvisal 0:e6fcdb78a136 1476 FixPoint1616_t *ptotal_signal_rate_mcps)
mjarvisal 0:e6fcdb78a136 1477 {
mjarvisal 0:e6fcdb78a136 1478 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1479 FixPoint1616_t totalXtalkMegaCps;
mjarvisal 0:e6fcdb78a136 1480
mjarvisal 0:e6fcdb78a136 1481 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 1482
mjarvisal 0:e6fcdb78a136 1483 *ptotal_signal_rate_mcps =
mjarvisal 0:e6fcdb78a136 1484 pRangingMeasurementData->SignalRateRtnMegaCps;
mjarvisal 0:e6fcdb78a136 1485
mjarvisal 0:e6fcdb78a136 1486 Status = VL53L0X_get_total_xtalk_rate(
mjarvisal 0:e6fcdb78a136 1487 Dev, pRangingMeasurementData, &totalXtalkMegaCps);
mjarvisal 0:e6fcdb78a136 1488
mjarvisal 0:e6fcdb78a136 1489 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 1490 *ptotal_signal_rate_mcps += totalXtalkMegaCps;
mjarvisal 0:e6fcdb78a136 1491
mjarvisal 0:e6fcdb78a136 1492 return Status;
mjarvisal 0:e6fcdb78a136 1493 }
mjarvisal 0:e6fcdb78a136 1494
mjarvisal 0:e6fcdb78a136 1495 VL53L0X_Error VL53L0X_calc_dmax(
mjarvisal 0:e6fcdb78a136 1496 VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1497 FixPoint1616_t totalSignalRate_mcps,
mjarvisal 0:e6fcdb78a136 1498 FixPoint1616_t totalCorrSignalRate_mcps,
mjarvisal 0:e6fcdb78a136 1499 FixPoint1616_t pwMult,
mjarvisal 0:e6fcdb78a136 1500 uint32_t sigmaEstimateP1,
mjarvisal 0:e6fcdb78a136 1501 FixPoint1616_t sigmaEstimateP2,
mjarvisal 0:e6fcdb78a136 1502 uint32_t peakVcselDuration_us,
mjarvisal 0:e6fcdb78a136 1503 uint32_t *pdmax_mm)
mjarvisal 0:e6fcdb78a136 1504 {
mjarvisal 0:e6fcdb78a136 1505 const uint32_t cSigmaLimit = 18;
mjarvisal 0:e6fcdb78a136 1506 const FixPoint1616_t cSignalLimit = 0x4000; /* 0.25 */
mjarvisal 0:e6fcdb78a136 1507 const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
mjarvisal 0:e6fcdb78a136 1508 const uint32_t cAmbEffWidthSigmaEst_ns = 6;
mjarvisal 0:e6fcdb78a136 1509 const uint32_t cAmbEffWidthDMax_ns = 7;
mjarvisal 0:e6fcdb78a136 1510 uint32_t dmaxCalRange_mm;
mjarvisal 0:e6fcdb78a136 1511 FixPoint1616_t dmaxCalSignalRateRtn_mcps;
mjarvisal 0:e6fcdb78a136 1512 FixPoint1616_t minSignalNeeded;
mjarvisal 0:e6fcdb78a136 1513 FixPoint1616_t minSignalNeeded_p1;
mjarvisal 0:e6fcdb78a136 1514 FixPoint1616_t minSignalNeeded_p2;
mjarvisal 0:e6fcdb78a136 1515 FixPoint1616_t minSignalNeeded_p3;
mjarvisal 0:e6fcdb78a136 1516 FixPoint1616_t minSignalNeeded_p4;
mjarvisal 0:e6fcdb78a136 1517 FixPoint1616_t sigmaLimitTmp;
mjarvisal 0:e6fcdb78a136 1518 FixPoint1616_t sigmaEstSqTmp;
mjarvisal 0:e6fcdb78a136 1519 FixPoint1616_t signalLimitTmp;
mjarvisal 0:e6fcdb78a136 1520 FixPoint1616_t SignalAt0mm;
mjarvisal 0:e6fcdb78a136 1521 FixPoint1616_t dmaxDark;
mjarvisal 0:e6fcdb78a136 1522 FixPoint1616_t dmaxAmbient;
mjarvisal 0:e6fcdb78a136 1523 FixPoint1616_t dmaxDarkTmp;
mjarvisal 0:e6fcdb78a136 1524 FixPoint1616_t sigmaEstP2Tmp;
mjarvisal 0:e6fcdb78a136 1525 uint32_t signalRateTemp_mcps;
mjarvisal 0:e6fcdb78a136 1526
mjarvisal 0:e6fcdb78a136 1527 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1528
mjarvisal 0:e6fcdb78a136 1529 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 1530
mjarvisal 0:e6fcdb78a136 1531 dmaxCalRange_mm =
mjarvisal 0:e6fcdb78a136 1532 PALDevDataGet(Dev, DmaxCalRangeMilliMeter);
mjarvisal 0:e6fcdb78a136 1533
mjarvisal 0:e6fcdb78a136 1534 dmaxCalSignalRateRtn_mcps =
mjarvisal 0:e6fcdb78a136 1535 PALDevDataGet(Dev, DmaxCalSignalRateRtnMegaCps);
mjarvisal 0:e6fcdb78a136 1536
mjarvisal 0:e6fcdb78a136 1537 /* uint32 * FixPoint1616 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1538 SignalAt0mm = dmaxCalRange_mm * dmaxCalSignalRateRtn_mcps;
mjarvisal 0:e6fcdb78a136 1539
mjarvisal 0:e6fcdb78a136 1540 /* FixPoint1616 >> 8 = FixPoint2408 */
mjarvisal 0:e6fcdb78a136 1541 SignalAt0mm = (SignalAt0mm + 0x80) >> 8;
mjarvisal 0:e6fcdb78a136 1542 SignalAt0mm *= dmaxCalRange_mm;
mjarvisal 0:e6fcdb78a136 1543
mjarvisal 0:e6fcdb78a136 1544 minSignalNeeded_p1 = 0;
mjarvisal 0:e6fcdb78a136 1545 if (totalCorrSignalRate_mcps > 0) {
mjarvisal 0:e6fcdb78a136 1546
mjarvisal 0:e6fcdb78a136 1547 /* Shift by 10 bits to increase resolution prior to the
mjarvisal 0:e6fcdb78a136 1548 * division */
mjarvisal 0:e6fcdb78a136 1549 signalRateTemp_mcps = totalSignalRate_mcps << 10;
mjarvisal 0:e6fcdb78a136 1550
mjarvisal 0:e6fcdb78a136 1551 /* Add rounding value prior to division */
mjarvisal 0:e6fcdb78a136 1552 minSignalNeeded_p1 = signalRateTemp_mcps +
mjarvisal 0:e6fcdb78a136 1553 (totalCorrSignalRate_mcps/2);
mjarvisal 0:e6fcdb78a136 1554
mjarvisal 0:e6fcdb78a136 1555 /* FixPoint0626/FixPoint1616 = FixPoint2210 */
mjarvisal 0:e6fcdb78a136 1556 minSignalNeeded_p1 /= totalCorrSignalRate_mcps;
mjarvisal 0:e6fcdb78a136 1557
mjarvisal 0:e6fcdb78a136 1558 /* Apply a factored version of the speed of light.
mjarvisal 0:e6fcdb78a136 1559 Correction to be applied at the end */
mjarvisal 0:e6fcdb78a136 1560 minSignalNeeded_p1 *= 3;
mjarvisal 0:e6fcdb78a136 1561
mjarvisal 0:e6fcdb78a136 1562 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
mjarvisal 0:e6fcdb78a136 1563 minSignalNeeded_p1 *= minSignalNeeded_p1;
mjarvisal 0:e6fcdb78a136 1564
mjarvisal 0:e6fcdb78a136 1565 /* FixPoint1220 >> 16 = FixPoint2804 */
mjarvisal 0:e6fcdb78a136 1566 minSignalNeeded_p1 = (minSignalNeeded_p1 + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1567 }
mjarvisal 0:e6fcdb78a136 1568
mjarvisal 0:e6fcdb78a136 1569 minSignalNeeded_p2 = pwMult * sigmaEstimateP1;
mjarvisal 0:e6fcdb78a136 1570
mjarvisal 0:e6fcdb78a136 1571 /* FixPoint1616 >> 16 = uint32 */
mjarvisal 0:e6fcdb78a136 1572 minSignalNeeded_p2 = (minSignalNeeded_p2 + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1573
mjarvisal 0:e6fcdb78a136 1574 /* uint32 * uint32 = uint32 */
mjarvisal 0:e6fcdb78a136 1575 minSignalNeeded_p2 *= minSignalNeeded_p2;
mjarvisal 0:e6fcdb78a136 1576
mjarvisal 0:e6fcdb78a136 1577 /* Check sigmaEstimateP2
mjarvisal 0:e6fcdb78a136 1578 * If this value is too high there is not enough signal rate
mjarvisal 0:e6fcdb78a136 1579 * to calculate dmax value so set a suitable value to ensure
mjarvisal 0:e6fcdb78a136 1580 * a very small dmax.
mjarvisal 0:e6fcdb78a136 1581 */
mjarvisal 0:e6fcdb78a136 1582 sigmaEstP2Tmp = (sigmaEstimateP2 + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1583 sigmaEstP2Tmp = (sigmaEstP2Tmp + cAmbEffWidthSigmaEst_ns/2)/
mjarvisal 0:e6fcdb78a136 1584 cAmbEffWidthSigmaEst_ns;
mjarvisal 0:e6fcdb78a136 1585 sigmaEstP2Tmp *= cAmbEffWidthDMax_ns;
mjarvisal 0:e6fcdb78a136 1586
mjarvisal 0:e6fcdb78a136 1587 if (sigmaEstP2Tmp > 0xffff) {
mjarvisal 0:e6fcdb78a136 1588 minSignalNeeded_p3 = 0xfff00000;
mjarvisal 0:e6fcdb78a136 1589 } else {
mjarvisal 0:e6fcdb78a136 1590
mjarvisal 0:e6fcdb78a136 1591 /* DMAX uses a different ambient width from sigma, so apply
mjarvisal 0:e6fcdb78a136 1592 * correction.
mjarvisal 0:e6fcdb78a136 1593 * Perform division before multiplication to prevent overflow.
mjarvisal 0:e6fcdb78a136 1594 */
mjarvisal 0:e6fcdb78a136 1595 sigmaEstimateP2 = (sigmaEstimateP2 + cAmbEffWidthSigmaEst_ns/2)/
mjarvisal 0:e6fcdb78a136 1596 cAmbEffWidthSigmaEst_ns;
mjarvisal 0:e6fcdb78a136 1597 sigmaEstimateP2 *= cAmbEffWidthDMax_ns;
mjarvisal 0:e6fcdb78a136 1598
mjarvisal 0:e6fcdb78a136 1599 /* FixPoint1616 >> 16 = uint32 */
mjarvisal 0:e6fcdb78a136 1600 minSignalNeeded_p3 = (sigmaEstimateP2 + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1601
mjarvisal 0:e6fcdb78a136 1602 minSignalNeeded_p3 *= minSignalNeeded_p3;
mjarvisal 0:e6fcdb78a136 1603
mjarvisal 0:e6fcdb78a136 1604 }
mjarvisal 0:e6fcdb78a136 1605
mjarvisal 0:e6fcdb78a136 1606 /* FixPoint1814 / uint32 = FixPoint1814 */
mjarvisal 0:e6fcdb78a136 1607 sigmaLimitTmp = ((cSigmaLimit << 14) + 500) / 1000;
mjarvisal 0:e6fcdb78a136 1608
mjarvisal 0:e6fcdb78a136 1609 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
mjarvisal 0:e6fcdb78a136 1610 sigmaLimitTmp *= sigmaLimitTmp;
mjarvisal 0:e6fcdb78a136 1611
mjarvisal 0:e6fcdb78a136 1612 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
mjarvisal 0:e6fcdb78a136 1613 sigmaEstSqTmp = cSigmaEstRef * cSigmaEstRef;
mjarvisal 0:e6fcdb78a136 1614
mjarvisal 0:e6fcdb78a136 1615 /* FixPoint3232 >> 4 = FixPoint0428 */
mjarvisal 0:e6fcdb78a136 1616 sigmaEstSqTmp = (sigmaEstSqTmp + 0x08) >> 4;
mjarvisal 0:e6fcdb78a136 1617
mjarvisal 0:e6fcdb78a136 1618 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */
mjarvisal 0:e6fcdb78a136 1619 sigmaLimitTmp -= sigmaEstSqTmp;
mjarvisal 0:e6fcdb78a136 1620
mjarvisal 0:e6fcdb78a136 1621 /* uint32_t * FixPoint0428 = FixPoint0428 */
mjarvisal 0:e6fcdb78a136 1622 minSignalNeeded_p4 = 4 * 12 * sigmaLimitTmp;
mjarvisal 0:e6fcdb78a136 1623
mjarvisal 0:e6fcdb78a136 1624 /* FixPoint0428 >> 14 = FixPoint1814 */
mjarvisal 0:e6fcdb78a136 1625 minSignalNeeded_p4 = (minSignalNeeded_p4 + 0x2000) >> 14;
mjarvisal 0:e6fcdb78a136 1626
mjarvisal 0:e6fcdb78a136 1627 /* uint32 + uint32 = uint32 */
mjarvisal 0:e6fcdb78a136 1628 minSignalNeeded = (minSignalNeeded_p2 + minSignalNeeded_p3);
mjarvisal 0:e6fcdb78a136 1629
mjarvisal 0:e6fcdb78a136 1630 /* uint32 / uint32 = uint32 */
mjarvisal 0:e6fcdb78a136 1631 minSignalNeeded += (peakVcselDuration_us/2);
mjarvisal 0:e6fcdb78a136 1632 minSignalNeeded /= peakVcselDuration_us;
mjarvisal 0:e6fcdb78a136 1633
mjarvisal 0:e6fcdb78a136 1634 /* uint32 << 14 = FixPoint1814 */
mjarvisal 0:e6fcdb78a136 1635 minSignalNeeded <<= 14;
mjarvisal 0:e6fcdb78a136 1636
mjarvisal 0:e6fcdb78a136 1637 /* FixPoint1814 / FixPoint1814 = uint32 */
mjarvisal 0:e6fcdb78a136 1638 minSignalNeeded += (minSignalNeeded_p4/2);
mjarvisal 0:e6fcdb78a136 1639 minSignalNeeded /= minSignalNeeded_p4;
mjarvisal 0:e6fcdb78a136 1640
mjarvisal 0:e6fcdb78a136 1641 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
mjarvisal 0:e6fcdb78a136 1642 minSignalNeeded *= minSignalNeeded_p1;
mjarvisal 0:e6fcdb78a136 1643
mjarvisal 0:e6fcdb78a136 1644 /* Apply correction by dividing by 1000000.
mjarvisal 0:e6fcdb78a136 1645 * This assumes 10E16 on the numerator of the equation
mjarvisal 0:e6fcdb78a136 1646 * and 10E-22 on the denominator.
mjarvisal 0:e6fcdb78a136 1647 * We do this because 32bit fix point calculation can't
mjarvisal 0:e6fcdb78a136 1648 * handle the larger and smaller elements of this equation,
mjarvisal 0:e6fcdb78a136 1649 * i.e. speed of light and pulse widths.
mjarvisal 0:e6fcdb78a136 1650 */
mjarvisal 0:e6fcdb78a136 1651 minSignalNeeded = (minSignalNeeded + 500) / 1000;
mjarvisal 0:e6fcdb78a136 1652 minSignalNeeded <<= 4;
mjarvisal 0:e6fcdb78a136 1653
mjarvisal 0:e6fcdb78a136 1654 minSignalNeeded = (minSignalNeeded + 500) / 1000;
mjarvisal 0:e6fcdb78a136 1655
mjarvisal 0:e6fcdb78a136 1656 /* FixPoint1616 >> 8 = FixPoint2408 */
mjarvisal 0:e6fcdb78a136 1657 signalLimitTmp = (cSignalLimit + 0x80) >> 8;
mjarvisal 0:e6fcdb78a136 1658
mjarvisal 0:e6fcdb78a136 1659 /* FixPoint2408/FixPoint2408 = uint32 */
mjarvisal 0:e6fcdb78a136 1660 if (signalLimitTmp != 0)
mjarvisal 0:e6fcdb78a136 1661 dmaxDarkTmp = (SignalAt0mm + (signalLimitTmp / 2))
mjarvisal 0:e6fcdb78a136 1662 / signalLimitTmp;
mjarvisal 0:e6fcdb78a136 1663 else
mjarvisal 0:e6fcdb78a136 1664 dmaxDarkTmp = 0;
mjarvisal 0:e6fcdb78a136 1665
mjarvisal 0:e6fcdb78a136 1666 dmaxDark = VL53L0X_isqrt(dmaxDarkTmp);
mjarvisal 0:e6fcdb78a136 1667
mjarvisal 0:e6fcdb78a136 1668 /* FixPoint2408/FixPoint2408 = uint32 */
mjarvisal 0:e6fcdb78a136 1669 if (minSignalNeeded != 0)
mjarvisal 0:e6fcdb78a136 1670 dmaxAmbient = (SignalAt0mm + minSignalNeeded/2)
mjarvisal 0:e6fcdb78a136 1671 / minSignalNeeded;
mjarvisal 0:e6fcdb78a136 1672 else
mjarvisal 0:e6fcdb78a136 1673 dmaxAmbient = 0;
mjarvisal 0:e6fcdb78a136 1674
mjarvisal 0:e6fcdb78a136 1675 dmaxAmbient = VL53L0X_isqrt(dmaxAmbient);
mjarvisal 0:e6fcdb78a136 1676
mjarvisal 0:e6fcdb78a136 1677 *pdmax_mm = dmaxDark;
mjarvisal 0:e6fcdb78a136 1678 if (dmaxDark > dmaxAmbient)
mjarvisal 0:e6fcdb78a136 1679 *pdmax_mm = dmaxAmbient;
mjarvisal 0:e6fcdb78a136 1680
mjarvisal 0:e6fcdb78a136 1681 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1682
mjarvisal 0:e6fcdb78a136 1683 return Status;
mjarvisal 0:e6fcdb78a136 1684 }
mjarvisal 0:e6fcdb78a136 1685
mjarvisal 0:e6fcdb78a136 1686
mjarvisal 0:e6fcdb78a136 1687 VL53L0X_Error VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 1688 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
mjarvisal 0:e6fcdb78a136 1689 FixPoint1616_t *pSigmaEstimate,
mjarvisal 0:e6fcdb78a136 1690 uint32_t *pDmax_mm)
mjarvisal 0:e6fcdb78a136 1691 {
mjarvisal 0:e6fcdb78a136 1692 /* Expressed in 100ths of a ns, i.e. centi-ns */
mjarvisal 0:e6fcdb78a136 1693 const uint32_t cPulseEffectiveWidth_centi_ns = 800;
mjarvisal 0:e6fcdb78a136 1694 /* Expressed in 100ths of a ns, i.e. centi-ns */
mjarvisal 0:e6fcdb78a136 1695 const uint32_t cAmbientEffectiveWidth_centi_ns = 600;
mjarvisal 0:e6fcdb78a136 1696 const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
mjarvisal 0:e6fcdb78a136 1697 const uint32_t cVcselPulseWidth_ps = 4700; /* pico secs */
mjarvisal 0:e6fcdb78a136 1698 const FixPoint1616_t cSigmaEstMax = 0x028F87AE;
mjarvisal 0:e6fcdb78a136 1699 const FixPoint1616_t cSigmaEstRtnMax = 0xF000;
mjarvisal 0:e6fcdb78a136 1700 const FixPoint1616_t cAmbToSignalRatioMax = 0xF0000000/
mjarvisal 0:e6fcdb78a136 1701 cAmbientEffectiveWidth_centi_ns;
mjarvisal 0:e6fcdb78a136 1702 /* Time Of Flight per mm (6.6 pico secs) */
mjarvisal 0:e6fcdb78a136 1703 const FixPoint1616_t cTOF_per_mm_ps = 0x0006999A;
mjarvisal 0:e6fcdb78a136 1704 const uint32_t c16BitRoundingParam = 0x00008000;
mjarvisal 0:e6fcdb78a136 1705 const FixPoint1616_t cMaxXTalk_kcps = 0x00320000;
mjarvisal 0:e6fcdb78a136 1706 const uint32_t cPllPeriod_ps = 1655;
mjarvisal 0:e6fcdb78a136 1707
mjarvisal 0:e6fcdb78a136 1708 uint32_t vcselTotalEventsRtn;
mjarvisal 0:e6fcdb78a136 1709 uint32_t finalRangeTimeoutMicroSecs;
mjarvisal 0:e6fcdb78a136 1710 uint32_t preRangeTimeoutMicroSecs;
mjarvisal 0:e6fcdb78a136 1711 FixPoint1616_t sigmaEstimateP1;
mjarvisal 0:e6fcdb78a136 1712 FixPoint1616_t sigmaEstimateP2;
mjarvisal 0:e6fcdb78a136 1713 FixPoint1616_t sigmaEstimateP3;
mjarvisal 0:e6fcdb78a136 1714 FixPoint1616_t deltaT_ps;
mjarvisal 0:e6fcdb78a136 1715 FixPoint1616_t pwMult;
mjarvisal 0:e6fcdb78a136 1716 FixPoint1616_t sigmaEstRtn;
mjarvisal 0:e6fcdb78a136 1717 FixPoint1616_t sigmaEstimate;
mjarvisal 0:e6fcdb78a136 1718 FixPoint1616_t xTalkCorrection;
mjarvisal 0:e6fcdb78a136 1719 FixPoint1616_t ambientRate_kcps;
mjarvisal 0:e6fcdb78a136 1720 FixPoint1616_t peakSignalRate_kcps;
mjarvisal 0:e6fcdb78a136 1721 FixPoint1616_t xTalkCompRate_mcps;
mjarvisal 0:e6fcdb78a136 1722 uint32_t xTalkCompRate_kcps;
mjarvisal 0:e6fcdb78a136 1723 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 1724 FixPoint1616_t diff1_mcps;
mjarvisal 0:e6fcdb78a136 1725 FixPoint1616_t diff2_mcps;
mjarvisal 0:e6fcdb78a136 1726 FixPoint1616_t sqr1;
mjarvisal 0:e6fcdb78a136 1727 FixPoint1616_t sqr2;
mjarvisal 0:e6fcdb78a136 1728 FixPoint1616_t sqrSum;
mjarvisal 0:e6fcdb78a136 1729 FixPoint1616_t sqrtResult_centi_ns;
mjarvisal 0:e6fcdb78a136 1730 FixPoint1616_t sqrtResult;
mjarvisal 0:e6fcdb78a136 1731 FixPoint1616_t totalSignalRate_mcps;
mjarvisal 0:e6fcdb78a136 1732 FixPoint1616_t correctedSignalRate_mcps;
mjarvisal 0:e6fcdb78a136 1733 uint32_t vcselWidth;
mjarvisal 0:e6fcdb78a136 1734 uint32_t finalRangeMacroPCLKS;
mjarvisal 0:e6fcdb78a136 1735 uint32_t preRangeMacroPCLKS;
mjarvisal 0:e6fcdb78a136 1736 uint32_t peakVcselDuration_us;
mjarvisal 0:e6fcdb78a136 1737 uint8_t finalRangeVcselPCLKS;
mjarvisal 0:e6fcdb78a136 1738 uint8_t preRangeVcselPCLKS;
mjarvisal 0:e6fcdb78a136 1739 /*! \addtogroup calc_sigma_estimate
mjarvisal 0:e6fcdb78a136 1740 * @{
mjarvisal 0:e6fcdb78a136 1741 *
mjarvisal 0:e6fcdb78a136 1742 * Estimates the range sigma based on the
mjarvisal 0:e6fcdb78a136 1743 *
mjarvisal 0:e6fcdb78a136 1744 * - vcsel_rate_kcps
mjarvisal 0:e6fcdb78a136 1745 * - ambient_rate_kcps
mjarvisal 0:e6fcdb78a136 1746 * - signal_total_events
mjarvisal 0:e6fcdb78a136 1747 * - xtalk_rate
mjarvisal 0:e6fcdb78a136 1748 *
mjarvisal 0:e6fcdb78a136 1749 * and the following parameters
mjarvisal 0:e6fcdb78a136 1750 *
mjarvisal 0:e6fcdb78a136 1751 * - SigmaEstRefArray
mjarvisal 0:e6fcdb78a136 1752 * - SigmaEstEffPulseWidth
mjarvisal 0:e6fcdb78a136 1753 * - SigmaEstEffAmbWidth
mjarvisal 0:e6fcdb78a136 1754 */
mjarvisal 0:e6fcdb78a136 1755
mjarvisal 0:e6fcdb78a136 1756 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 1757
mjarvisal 0:e6fcdb78a136 1758 VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
mjarvisal 0:e6fcdb78a136 1759 xTalkCompRate_mcps);
mjarvisal 0:e6fcdb78a136 1760
mjarvisal 0:e6fcdb78a136 1761 /*
mjarvisal 0:e6fcdb78a136 1762 * We work in kcps rather than mcps as this helps keep within the
mjarvisal 0:e6fcdb78a136 1763 * confines of the 32 Fix1616 type.
mjarvisal 0:e6fcdb78a136 1764 */
mjarvisal 0:e6fcdb78a136 1765
mjarvisal 0:e6fcdb78a136 1766 ambientRate_kcps =
mjarvisal 0:e6fcdb78a136 1767 (pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16;
mjarvisal 0:e6fcdb78a136 1768
mjarvisal 0:e6fcdb78a136 1769 correctedSignalRate_mcps =
mjarvisal 0:e6fcdb78a136 1770 pRangingMeasurementData->SignalRateRtnMegaCps;
mjarvisal 0:e6fcdb78a136 1771
mjarvisal 0:e6fcdb78a136 1772
mjarvisal 0:e6fcdb78a136 1773 Status = VL53L0X_get_total_signal_rate(
mjarvisal 0:e6fcdb78a136 1774 Dev, pRangingMeasurementData, &totalSignalRate_mcps);
mjarvisal 0:e6fcdb78a136 1775 Status = VL53L0X_get_total_xtalk_rate(
mjarvisal 0:e6fcdb78a136 1776 Dev, pRangingMeasurementData, &xTalkCompRate_mcps);
mjarvisal 0:e6fcdb78a136 1777
mjarvisal 0:e6fcdb78a136 1778
mjarvisal 0:e6fcdb78a136 1779 /* Signal rate measurement provided by device is the
mjarvisal 0:e6fcdb78a136 1780 * peak signal rate, not average.
mjarvisal 0:e6fcdb78a136 1781 */
mjarvisal 0:e6fcdb78a136 1782 peakSignalRate_kcps = (totalSignalRate_mcps * 1000);
mjarvisal 0:e6fcdb78a136 1783 peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1784
mjarvisal 0:e6fcdb78a136 1785 xTalkCompRate_kcps = xTalkCompRate_mcps * 1000;
mjarvisal 0:e6fcdb78a136 1786
mjarvisal 0:e6fcdb78a136 1787 if (xTalkCompRate_kcps > cMaxXTalk_kcps)
mjarvisal 0:e6fcdb78a136 1788 xTalkCompRate_kcps = cMaxXTalk_kcps;
mjarvisal 0:e6fcdb78a136 1789
mjarvisal 0:e6fcdb78a136 1790 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1791
mjarvisal 0:e6fcdb78a136 1792 /* Calculate final range macro periods */
mjarvisal 0:e6fcdb78a136 1793 finalRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 1794 Dev, FinalRangeTimeoutMicroSecs);
mjarvisal 0:e6fcdb78a136 1795
mjarvisal 0:e6fcdb78a136 1796 finalRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 1797 Dev, FinalRangeVcselPulsePeriod);
mjarvisal 0:e6fcdb78a136 1798
mjarvisal 0:e6fcdb78a136 1799 finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
mjarvisal 0:e6fcdb78a136 1800 Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS);
mjarvisal 0:e6fcdb78a136 1801
mjarvisal 0:e6fcdb78a136 1802 /* Calculate pre-range macro periods */
mjarvisal 0:e6fcdb78a136 1803 preRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 1804 Dev, PreRangeTimeoutMicroSecs);
mjarvisal 0:e6fcdb78a136 1805
mjarvisal 0:e6fcdb78a136 1806 preRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
mjarvisal 0:e6fcdb78a136 1807 Dev, PreRangeVcselPulsePeriod);
mjarvisal 0:e6fcdb78a136 1808
mjarvisal 0:e6fcdb78a136 1809 preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
mjarvisal 0:e6fcdb78a136 1810 Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS);
mjarvisal 0:e6fcdb78a136 1811
mjarvisal 0:e6fcdb78a136 1812 vcselWidth = 3;
mjarvisal 0:e6fcdb78a136 1813 if (finalRangeVcselPCLKS == 8)
mjarvisal 0:e6fcdb78a136 1814 vcselWidth = 2;
mjarvisal 0:e6fcdb78a136 1815
mjarvisal 0:e6fcdb78a136 1816
mjarvisal 0:e6fcdb78a136 1817 peakVcselDuration_us = vcselWidth * 2048 *
mjarvisal 0:e6fcdb78a136 1818 (preRangeMacroPCLKS + finalRangeMacroPCLKS);
mjarvisal 0:e6fcdb78a136 1819 peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
mjarvisal 0:e6fcdb78a136 1820 peakVcselDuration_us *= cPllPeriod_ps;
mjarvisal 0:e6fcdb78a136 1821 peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
mjarvisal 0:e6fcdb78a136 1822
mjarvisal 0:e6fcdb78a136 1823 /* Fix1616 >> 8 = Fix2408 */
mjarvisal 0:e6fcdb78a136 1824 totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8;
mjarvisal 0:e6fcdb78a136 1825
mjarvisal 0:e6fcdb78a136 1826 /* Fix2408 * uint32 = Fix2408 */
mjarvisal 0:e6fcdb78a136 1827 vcselTotalEventsRtn = totalSignalRate_mcps *
mjarvisal 0:e6fcdb78a136 1828 peakVcselDuration_us;
mjarvisal 0:e6fcdb78a136 1829
mjarvisal 0:e6fcdb78a136 1830 /* Fix2408 >> 8 = uint32 */
mjarvisal 0:e6fcdb78a136 1831 vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8;
mjarvisal 0:e6fcdb78a136 1832
mjarvisal 0:e6fcdb78a136 1833 /* Fix2408 << 8 = Fix1616 = */
mjarvisal 0:e6fcdb78a136 1834 totalSignalRate_mcps <<= 8;
mjarvisal 0:e6fcdb78a136 1835 }
mjarvisal 0:e6fcdb78a136 1836
mjarvisal 0:e6fcdb78a136 1837 if (Status != VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 1838 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 1839 return Status;
mjarvisal 0:e6fcdb78a136 1840 }
mjarvisal 0:e6fcdb78a136 1841
mjarvisal 0:e6fcdb78a136 1842 if (peakSignalRate_kcps == 0) {
mjarvisal 0:e6fcdb78a136 1843 *pSigmaEstimate = cSigmaEstMax;
mjarvisal 0:e6fcdb78a136 1844 PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax);
mjarvisal 0:e6fcdb78a136 1845 *pDmax_mm = 0;
mjarvisal 0:e6fcdb78a136 1846 } else {
mjarvisal 0:e6fcdb78a136 1847 if (vcselTotalEventsRtn < 1)
mjarvisal 0:e6fcdb78a136 1848 vcselTotalEventsRtn = 1;
mjarvisal 0:e6fcdb78a136 1849
mjarvisal 0:e6fcdb78a136 1850 /*
mjarvisal 0:e6fcdb78a136 1851 * Calculate individual components of the main equation -
mjarvisal 0:e6fcdb78a136 1852 * replicating the equation implemented in the script
mjarvisal 0:e6fcdb78a136 1853 * OpenAll_Ewok_ranging_data.jsl.
mjarvisal 0:e6fcdb78a136 1854 *
mjarvisal 0:e6fcdb78a136 1855 * sigmaEstimateP1 represents the effective pulse width, which
mjarvisal 0:e6fcdb78a136 1856 * is a tuning parameter, rather than a real value.
mjarvisal 0:e6fcdb78a136 1857 *
mjarvisal 0:e6fcdb78a136 1858 * sigmaEstimateP2 represents the ambient/signal rate ratio
mjarvisal 0:e6fcdb78a136 1859 * expressed as a multiple of the effective ambient width
mjarvisal 0:e6fcdb78a136 1860 * (tuning parameter).
mjarvisal 0:e6fcdb78a136 1861 *
mjarvisal 0:e6fcdb78a136 1862 * sigmaEstimateP3 provides the signal event component, with the
mjarvisal 0:e6fcdb78a136 1863 * knowledge that
mjarvisal 0:e6fcdb78a136 1864 * - Noise of a square pulse is 1/sqrt(12) of the pulse
mjarvisal 0:e6fcdb78a136 1865 * width.
mjarvisal 0:e6fcdb78a136 1866 * - at 0Lux, sigma is proportional to
mjarvisal 0:e6fcdb78a136 1867 * effectiveVcselPulseWidth/sqrt(12 * signalTotalEvents)
mjarvisal 0:e6fcdb78a136 1868 *
mjarvisal 0:e6fcdb78a136 1869 * deltaT_ps represents the time of flight in pico secs for the
mjarvisal 0:e6fcdb78a136 1870 * current range measurement, using the "TOF per mm" constant
mjarvisal 0:e6fcdb78a136 1871 * (in ps).
mjarvisal 0:e6fcdb78a136 1872 */
mjarvisal 0:e6fcdb78a136 1873
mjarvisal 0:e6fcdb78a136 1874 sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns;
mjarvisal 0:e6fcdb78a136 1875
mjarvisal 0:e6fcdb78a136 1876 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1877 sigmaEstimateP2 = (ambientRate_kcps << 16)/peakSignalRate_kcps;
mjarvisal 0:e6fcdb78a136 1878 if (sigmaEstimateP2 > cAmbToSignalRatioMax) {
mjarvisal 0:e6fcdb78a136 1879 /* Clip to prevent overflow. Will ensure safe
mjarvisal 0:e6fcdb78a136 1880 * max result. */
mjarvisal 0:e6fcdb78a136 1881 sigmaEstimateP2 = cAmbToSignalRatioMax;
mjarvisal 0:e6fcdb78a136 1882 }
mjarvisal 0:e6fcdb78a136 1883 sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns;
mjarvisal 0:e6fcdb78a136 1884
mjarvisal 0:e6fcdb78a136 1885 sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12);
mjarvisal 0:e6fcdb78a136 1886
mjarvisal 0:e6fcdb78a136 1887 /* uint32 * FixPoint1616 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1888 deltaT_ps = pRangingMeasurementData->RangeMilliMeter *
mjarvisal 0:e6fcdb78a136 1889 cTOF_per_mm_ps;
mjarvisal 0:e6fcdb78a136 1890
mjarvisal 0:e6fcdb78a136 1891 /*
mjarvisal 0:e6fcdb78a136 1892 * vcselRate - xtalkCompRate
mjarvisal 0:e6fcdb78a136 1893 * (uint32 << 16) - FixPoint1616 = FixPoint1616.
mjarvisal 0:e6fcdb78a136 1894 * Divide result by 1000 to convert to mcps.
mjarvisal 0:e6fcdb78a136 1895 * 500 is added to ensure rounding when integer division
mjarvisal 0:e6fcdb78a136 1896 * truncates.
mjarvisal 0:e6fcdb78a136 1897 */
mjarvisal 0:e6fcdb78a136 1898 diff1_mcps = (((peakSignalRate_kcps << 16) -
mjarvisal 0:e6fcdb78a136 1899 xTalkCompRate_kcps) + 500)/1000;
mjarvisal 0:e6fcdb78a136 1900
mjarvisal 0:e6fcdb78a136 1901 /* vcselRate + xtalkCompRate */
mjarvisal 0:e6fcdb78a136 1902 diff2_mcps = (((peakSignalRate_kcps << 16) +
mjarvisal 0:e6fcdb78a136 1903 xTalkCompRate_kcps) + 500)/1000;
mjarvisal 0:e6fcdb78a136 1904
mjarvisal 0:e6fcdb78a136 1905 /* Shift by 8 bits to increase resolution prior to the
mjarvisal 0:e6fcdb78a136 1906 * division */
mjarvisal 0:e6fcdb78a136 1907 diff1_mcps <<= 8;
mjarvisal 0:e6fcdb78a136 1908
mjarvisal 0:e6fcdb78a136 1909 /* FixPoint0824/FixPoint1616 = FixPoint2408 */
mjarvisal 0:e6fcdb78a136 1910 xTalkCorrection = abs(diff1_mcps/diff2_mcps);
mjarvisal 0:e6fcdb78a136 1911
mjarvisal 0:e6fcdb78a136 1912 /* FixPoint2408 << 8 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1913 xTalkCorrection <<= 8;
mjarvisal 0:e6fcdb78a136 1914
mjarvisal 0:e6fcdb78a136 1915 /* FixPoint1616/uint32 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1916 pwMult = deltaT_ps/cVcselPulseWidth_ps; /* smaller than 1.0f */
mjarvisal 0:e6fcdb78a136 1917
mjarvisal 0:e6fcdb78a136 1918 /*
mjarvisal 0:e6fcdb78a136 1919 * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
mjarvisal 0:e6fcdb78a136 1920 * values are small enough such that32 bits will not be
mjarvisal 0:e6fcdb78a136 1921 * exceeded.
mjarvisal 0:e6fcdb78a136 1922 */
mjarvisal 0:e6fcdb78a136 1923 pwMult *= ((1 << 16) - xTalkCorrection);
mjarvisal 0:e6fcdb78a136 1924
mjarvisal 0:e6fcdb78a136 1925 /* (FixPoint3232 >> 16) = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1926 pwMult = (pwMult + c16BitRoundingParam) >> 16;
mjarvisal 0:e6fcdb78a136 1927
mjarvisal 0:e6fcdb78a136 1928 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1929 pwMult += (1 << 16);
mjarvisal 0:e6fcdb78a136 1930
mjarvisal 0:e6fcdb78a136 1931 /*
mjarvisal 0:e6fcdb78a136 1932 * At this point the value will be 1.xx, therefore if we square
mjarvisal 0:e6fcdb78a136 1933 * the value this will exceed 32 bits. To address this perform
mjarvisal 0:e6fcdb78a136 1934 * a single shift to the right before the multiplication.
mjarvisal 0:e6fcdb78a136 1935 */
mjarvisal 0:e6fcdb78a136 1936 pwMult >>= 1;
mjarvisal 0:e6fcdb78a136 1937 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
mjarvisal 0:e6fcdb78a136 1938 pwMult = pwMult * pwMult;
mjarvisal 0:e6fcdb78a136 1939
mjarvisal 0:e6fcdb78a136 1940 /* (FixPoint3430 >> 14) = Fix1616 */
mjarvisal 0:e6fcdb78a136 1941 pwMult >>= 14;
mjarvisal 0:e6fcdb78a136 1942
mjarvisal 0:e6fcdb78a136 1943 /* FixPoint1616 * uint32 = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1944 sqr1 = pwMult * sigmaEstimateP1;
mjarvisal 0:e6fcdb78a136 1945
mjarvisal 0:e6fcdb78a136 1946 /* (FixPoint1616 >> 16) = FixPoint3200 */
mjarvisal 0:e6fcdb78a136 1947 sqr1 = (sqr1 + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1948
mjarvisal 0:e6fcdb78a136 1949 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
mjarvisal 0:e6fcdb78a136 1950 sqr1 *= sqr1;
mjarvisal 0:e6fcdb78a136 1951
mjarvisal 0:e6fcdb78a136 1952 sqr2 = sigmaEstimateP2;
mjarvisal 0:e6fcdb78a136 1953
mjarvisal 0:e6fcdb78a136 1954 /* (FixPoint1616 >> 16) = FixPoint3200 */
mjarvisal 0:e6fcdb78a136 1955 sqr2 = (sqr2 + 0x8000) >> 16;
mjarvisal 0:e6fcdb78a136 1956
mjarvisal 0:e6fcdb78a136 1957 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
mjarvisal 0:e6fcdb78a136 1958 sqr2 *= sqr2;
mjarvisal 0:e6fcdb78a136 1959
mjarvisal 0:e6fcdb78a136 1960 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
mjarvisal 0:e6fcdb78a136 1961 sqrSum = sqr1 + sqr2;
mjarvisal 0:e6fcdb78a136 1962
mjarvisal 0:e6fcdb78a136 1963 /* SQRT(FixPoin6400) = FixPoint3200 */
mjarvisal 0:e6fcdb78a136 1964 sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum);
mjarvisal 0:e6fcdb78a136 1965
mjarvisal 0:e6fcdb78a136 1966 /* (FixPoint3200 << 16) = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1967 sqrtResult_centi_ns <<= 16;
mjarvisal 0:e6fcdb78a136 1968
mjarvisal 0:e6fcdb78a136 1969 /*
mjarvisal 0:e6fcdb78a136 1970 * Note that the Speed Of Light is expressed in um per 1E-10
mjarvisal 0:e6fcdb78a136 1971 * seconds (2997) Therefore to get mm/ns we have to divide by
mjarvisal 0:e6fcdb78a136 1972 * 10000
mjarvisal 0:e6fcdb78a136 1973 */
mjarvisal 0:e6fcdb78a136 1974 sigmaEstRtn = (((sqrtResult_centi_ns+50)/100) /
mjarvisal 0:e6fcdb78a136 1975 sigmaEstimateP3);
mjarvisal 0:e6fcdb78a136 1976 sigmaEstRtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
mjarvisal 0:e6fcdb78a136 1977
mjarvisal 0:e6fcdb78a136 1978 /* Add 5000 before dividing by 10000 to ensure rounding. */
mjarvisal 0:e6fcdb78a136 1979 sigmaEstRtn += 5000;
mjarvisal 0:e6fcdb78a136 1980 sigmaEstRtn /= 10000;
mjarvisal 0:e6fcdb78a136 1981
mjarvisal 0:e6fcdb78a136 1982 if (sigmaEstRtn > cSigmaEstRtnMax) {
mjarvisal 0:e6fcdb78a136 1983 /* Clip to prevent overflow. Will ensure safe
mjarvisal 0:e6fcdb78a136 1984 * max result. */
mjarvisal 0:e6fcdb78a136 1985 sigmaEstRtn = cSigmaEstRtnMax;
mjarvisal 0:e6fcdb78a136 1986 }
mjarvisal 0:e6fcdb78a136 1987
mjarvisal 0:e6fcdb78a136 1988 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
mjarvisal 0:e6fcdb78a136 1989 sqr1 = sigmaEstRtn * sigmaEstRtn;
mjarvisal 0:e6fcdb78a136 1990 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
mjarvisal 0:e6fcdb78a136 1991 sqr2 = cSigmaEstRef * cSigmaEstRef;
mjarvisal 0:e6fcdb78a136 1992
mjarvisal 0:e6fcdb78a136 1993 /* sqrt(FixPoint3232) = FixPoint1616 */
mjarvisal 0:e6fcdb78a136 1994 sqrtResult = VL53L0X_isqrt((sqr1 + sqr2));
mjarvisal 0:e6fcdb78a136 1995 /*
mjarvisal 0:e6fcdb78a136 1996 * Note that the Shift by 4 bits increases resolution prior to
mjarvisal 0:e6fcdb78a136 1997 * the sqrt, therefore the result must be shifted by 2 bits to
mjarvisal 0:e6fcdb78a136 1998 * the right to revert back to the FixPoint1616 format.
mjarvisal 0:e6fcdb78a136 1999 */
mjarvisal 0:e6fcdb78a136 2000
mjarvisal 0:e6fcdb78a136 2001 sigmaEstimate = 1000 * sqrtResult;
mjarvisal 0:e6fcdb78a136 2002
mjarvisal 0:e6fcdb78a136 2003 if ((peakSignalRate_kcps < 1) || (vcselTotalEventsRtn < 1) ||
mjarvisal 0:e6fcdb78a136 2004 (sigmaEstimate > cSigmaEstMax)) {
mjarvisal 0:e6fcdb78a136 2005 sigmaEstimate = cSigmaEstMax;
mjarvisal 0:e6fcdb78a136 2006 }
mjarvisal 0:e6fcdb78a136 2007
mjarvisal 0:e6fcdb78a136 2008 *pSigmaEstimate = (uint32_t)(sigmaEstimate);
mjarvisal 0:e6fcdb78a136 2009 PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate);
mjarvisal 0:e6fcdb78a136 2010 Status = VL53L0X_calc_dmax(
mjarvisal 0:e6fcdb78a136 2011 Dev,
mjarvisal 0:e6fcdb78a136 2012 totalSignalRate_mcps,
mjarvisal 0:e6fcdb78a136 2013 correctedSignalRate_mcps,
mjarvisal 0:e6fcdb78a136 2014 pwMult,
mjarvisal 0:e6fcdb78a136 2015 sigmaEstimateP1,
mjarvisal 0:e6fcdb78a136 2016 sigmaEstimateP2,
mjarvisal 0:e6fcdb78a136 2017 peakVcselDuration_us,
mjarvisal 0:e6fcdb78a136 2018 pDmax_mm);
mjarvisal 0:e6fcdb78a136 2019 }
mjarvisal 0:e6fcdb78a136 2020
mjarvisal 0:e6fcdb78a136 2021 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 2022 return Status;
mjarvisal 0:e6fcdb78a136 2023 }
mjarvisal 0:e6fcdb78a136 2024
mjarvisal 0:e6fcdb78a136 2025 VL53L0X_Error VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
mjarvisal 0:e6fcdb78a136 2026 uint8_t DeviceRangeStatus,
mjarvisal 0:e6fcdb78a136 2027 FixPoint1616_t SignalRate,
mjarvisal 0:e6fcdb78a136 2028 uint16_t EffectiveSpadRtnCount,
mjarvisal 0:e6fcdb78a136 2029 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
mjarvisal 0:e6fcdb78a136 2030 uint8_t *pPalRangeStatus)
mjarvisal 0:e6fcdb78a136 2031 {
mjarvisal 0:e6fcdb78a136 2032 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:e6fcdb78a136 2033 uint8_t NoneFlag;
mjarvisal 0:e6fcdb78a136 2034 uint8_t SigmaLimitflag = 0;
mjarvisal 0:e6fcdb78a136 2035 uint8_t SignalRefClipflag = 0;
mjarvisal 0:e6fcdb78a136 2036 uint8_t RangeIgnoreThresholdflag = 0;
mjarvisal 0:e6fcdb78a136 2037 uint8_t SigmaLimitCheckEnable = 0;
mjarvisal 0:e6fcdb78a136 2038 uint8_t SignalRateFinalRangeLimitCheckEnable = 0;
mjarvisal 0:e6fcdb78a136 2039 uint8_t SignalRefClipLimitCheckEnable = 0;
mjarvisal 0:e6fcdb78a136 2040 uint8_t RangeIgnoreThresholdLimitCheckEnable = 0;
mjarvisal 0:e6fcdb78a136 2041 FixPoint1616_t SigmaEstimate;
mjarvisal 0:e6fcdb78a136 2042 FixPoint1616_t SigmaLimitValue;
mjarvisal 0:e6fcdb78a136 2043 FixPoint1616_t SignalRefClipValue;
mjarvisal 0:e6fcdb78a136 2044 FixPoint1616_t RangeIgnoreThresholdValue;
mjarvisal 0:e6fcdb78a136 2045 FixPoint1616_t SignalRatePerSpad;
mjarvisal 0:e6fcdb78a136 2046 uint8_t DeviceRangeStatusInternal = 0;
mjarvisal 0:e6fcdb78a136 2047 uint16_t tmpWord = 0;
mjarvisal 0:e6fcdb78a136 2048 uint8_t Temp8;
mjarvisal 0:e6fcdb78a136 2049 uint32_t Dmax_mm = 0;
mjarvisal 0:e6fcdb78a136 2050 FixPoint1616_t LastSignalRefMcps;
mjarvisal 0:e6fcdb78a136 2051
mjarvisal 0:e6fcdb78a136 2052 LOG_FUNCTION_START("");
mjarvisal 0:e6fcdb78a136 2053
mjarvisal 0:e6fcdb78a136 2054
mjarvisal 0:e6fcdb78a136 2055 /*
mjarvisal 0:e6fcdb78a136 2056 * VL53L0X has a good ranging when the value of the
mjarvisal 0:e6fcdb78a136 2057 * DeviceRangeStatus = 11. This function will replace the value 0 with
mjarvisal 0:e6fcdb78a136 2058 * the value 11 in the DeviceRangeStatus.
mjarvisal 0:e6fcdb78a136 2059 * In addition, the SigmaEstimator is not included in the VL53L0X
mjarvisal 0:e6fcdb78a136 2060 * DeviceRangeStatus, this will be added in the PalRangeStatus.
mjarvisal 0:e6fcdb78a136 2061 */
mjarvisal 0:e6fcdb78a136 2062
mjarvisal 0:e6fcdb78a136 2063 DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3);
mjarvisal 0:e6fcdb78a136 2064
mjarvisal 0:e6fcdb78a136 2065 if (DeviceRangeStatusInternal == 0 ||
mjarvisal 0:e6fcdb78a136 2066 DeviceRangeStatusInternal == 5 ||
mjarvisal 0:e6fcdb78a136 2067 DeviceRangeStatusInternal == 7 ||
mjarvisal 0:e6fcdb78a136 2068 DeviceRangeStatusInternal == 12 ||
mjarvisal 0:e6fcdb78a136 2069 DeviceRangeStatusInternal == 13 ||
mjarvisal 0:e6fcdb78a136 2070 DeviceRangeStatusInternal == 14 ||
mjarvisal 0:e6fcdb78a136 2071 DeviceRangeStatusInternal == 15
mjarvisal 0:e6fcdb78a136 2072 ) {
mjarvisal 0:e6fcdb78a136 2073 NoneFlag = 1;
mjarvisal 0:e6fcdb78a136 2074 } else {
mjarvisal 0:e6fcdb78a136 2075 NoneFlag = 0;
mjarvisal 0:e6fcdb78a136 2076 }
mjarvisal 0:e6fcdb78a136 2077
mjarvisal 0:e6fcdb78a136 2078 /* LastSignalRefMcps */
mjarvisal 0:e6fcdb78a136 2079 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2080 Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
mjarvisal 0:e6fcdb78a136 2081
mjarvisal 0:e6fcdb78a136 2082 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2083 Status = VL53L0X_RdWord(Dev,
mjarvisal 0:e6fcdb78a136 2084 VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
mjarvisal 0:e6fcdb78a136 2085 &tmpWord);
mjarvisal 0:e6fcdb78a136 2086
mjarvisal 0:e6fcdb78a136 2087 LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord);
mjarvisal 0:e6fcdb78a136 2088
mjarvisal 0:e6fcdb78a136 2089 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2090 Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
mjarvisal 0:e6fcdb78a136 2091
mjarvisal 0:e6fcdb78a136 2092 PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps);
mjarvisal 0:e6fcdb78a136 2093
mjarvisal 0:e6fcdb78a136 2094 /*
mjarvisal 0:e6fcdb78a136 2095 * Check if Sigma limit is enabled, if yes then do comparison with limit
mjarvisal 0:e6fcdb78a136 2096 * value and put the result back into pPalRangeStatus.
mjarvisal 0:e6fcdb78a136 2097 */
mjarvisal 0:e6fcdb78a136 2098 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2099 Status = VL53L0X_GetLimitCheckEnable(Dev,
mjarvisal 0:e6fcdb78a136 2100 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 2101 &SigmaLimitCheckEnable);
mjarvisal 0:e6fcdb78a136 2102
mjarvisal 0:e6fcdb78a136 2103 if ((SigmaLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) {
mjarvisal 0:e6fcdb78a136 2104 /*
mjarvisal 0:e6fcdb78a136 2105 * compute the Sigma and check with limit
mjarvisal 0:e6fcdb78a136 2106 */
mjarvisal 0:e6fcdb78a136 2107 Status = VL53L0X_calc_sigma_estimate(
mjarvisal 0:e6fcdb78a136 2108 Dev,
mjarvisal 0:e6fcdb78a136 2109 pRangingMeasurementData,
mjarvisal 0:e6fcdb78a136 2110 &SigmaEstimate,
mjarvisal 0:e6fcdb78a136 2111 &Dmax_mm);
mjarvisal 0:e6fcdb78a136 2112 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2113 pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm;
mjarvisal 0:e6fcdb78a136 2114
mjarvisal 0:e6fcdb78a136 2115 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 2116 Status = VL53L0X_GetLimitCheckValue(Dev,
mjarvisal 0:e6fcdb78a136 2117 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 2118 &SigmaLimitValue);
mjarvisal 0:e6fcdb78a136 2119
mjarvisal 0:e6fcdb78a136 2120 if ((SigmaLimitValue > 0) &&
mjarvisal 0:e6fcdb78a136 2121 (SigmaEstimate > SigmaLimitValue))
mjarvisal 0:e6fcdb78a136 2122 /* Limit Fail */
mjarvisal 0:e6fcdb78a136 2123 SigmaLimitflag = 1;
mjarvisal 0:e6fcdb78a136 2124 }
mjarvisal 0:e6fcdb78a136 2125 }
mjarvisal 0:e6fcdb78a136 2126
mjarvisal 0:e6fcdb78a136 2127 /*
mjarvisal 0:e6fcdb78a136 2128 * Check if Signal ref clip limit is enabled, if yes then do comparison
mjarvisal 0:e6fcdb78a136 2129 * with limit value and put the result back into pPalRangeStatus.
mjarvisal 0:e6fcdb78a136 2130 */
mjarvisal 0:e6fcdb78a136 2131 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2132 Status = VL53L0X_GetLimitCheckEnable(Dev,
mjarvisal 0:e6fcdb78a136 2133 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
mjarvisal 0:e6fcdb78a136 2134 &SignalRefClipLimitCheckEnable);
mjarvisal 0:e6fcdb78a136 2135
mjarvisal 0:e6fcdb78a136 2136 if ((SignalRefClipLimitCheckEnable != 0) &&
mjarvisal 0:e6fcdb78a136 2137 (Status == VL53L0X_ERROR_NONE)) {
mjarvisal 0:e6fcdb78a136 2138
mjarvisal 0:e6fcdb78a136 2139 Status = VL53L0X_GetLimitCheckValue(Dev,
mjarvisal 0:e6fcdb78a136 2140 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
mjarvisal 0:e6fcdb78a136 2141 &SignalRefClipValue);
mjarvisal 0:e6fcdb78a136 2142
mjarvisal 0:e6fcdb78a136 2143 if ((SignalRefClipValue > 0) &&
mjarvisal 0:e6fcdb78a136 2144 (LastSignalRefMcps > SignalRefClipValue)) {
mjarvisal 0:e6fcdb78a136 2145 /* Limit Fail */
mjarvisal 0:e6fcdb78a136 2146 SignalRefClipflag = 1;
mjarvisal 0:e6fcdb78a136 2147 }
mjarvisal 0:e6fcdb78a136 2148 }
mjarvisal 0:e6fcdb78a136 2149
mjarvisal 0:e6fcdb78a136 2150 /*
mjarvisal 0:e6fcdb78a136 2151 * Check if Signal ref clip limit is enabled, if yes then do comparison
mjarvisal 0:e6fcdb78a136 2152 * with limit value and put the result back into pPalRangeStatus.
mjarvisal 0:e6fcdb78a136 2153 * EffectiveSpadRtnCount has a format 8.8
mjarvisal 0:e6fcdb78a136 2154 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
mjarvisal 0:e6fcdb78a136 2155 */
mjarvisal 0:e6fcdb78a136 2156 if (Status == VL53L0X_ERROR_NONE)
mjarvisal 0:e6fcdb78a136 2157 Status = VL53L0X_GetLimitCheckEnable(Dev,
mjarvisal 0:e6fcdb78a136 2158 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
mjarvisal 0:e6fcdb78a136 2159 &RangeIgnoreThresholdLimitCheckEnable);
mjarvisal 0:e6fcdb78a136 2160
mjarvisal 0:e6fcdb78a136 2161 if ((RangeIgnoreThresholdLimitCheckEnable != 0) &&
mjarvisal 0:e6fcdb78a136 2162 (Status == VL53L0X_ERROR_NONE)) {
mjarvisal 0:e6fcdb78a136 2163
mjarvisal 0:e6fcdb78a136 2164 /* Compute the signal rate per spad */
mjarvisal 0:e6fcdb78a136 2165 if (EffectiveSpadRtnCount == 0) {
mjarvisal 0:e6fcdb78a136 2166 SignalRatePerSpad = 0;
mjarvisal 0:e6fcdb78a136 2167 } else {
mjarvisal 0:e6fcdb78a136 2168 SignalRatePerSpad = (FixPoint1616_t)((256 * SignalRate)
mjarvisal 0:e6fcdb78a136 2169 / EffectiveSpadRtnCount);
mjarvisal 0:e6fcdb78a136 2170 }
mjarvisal 0:e6fcdb78a136 2171
mjarvisal 0:e6fcdb78a136 2172 Status = VL53L0X_GetLimitCheckValue(Dev,
mjarvisal 0:e6fcdb78a136 2173 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
mjarvisal 0:e6fcdb78a136 2174 &RangeIgnoreThresholdValue);
mjarvisal 0:e6fcdb78a136 2175
mjarvisal 0:e6fcdb78a136 2176 if ((RangeIgnoreThresholdValue > 0) &&
mjarvisal 0:e6fcdb78a136 2177 (SignalRatePerSpad < RangeIgnoreThresholdValue)) {
mjarvisal 0:e6fcdb78a136 2178 /* Limit Fail add 2^6 to range status */
mjarvisal 0:e6fcdb78a136 2179 RangeIgnoreThresholdflag = 1;
mjarvisal 0:e6fcdb78a136 2180 }
mjarvisal 0:e6fcdb78a136 2181 }
mjarvisal 0:e6fcdb78a136 2182
mjarvisal 0:e6fcdb78a136 2183 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 2184 if (NoneFlag == 1) {
mjarvisal 0:e6fcdb78a136 2185 *pPalRangeStatus = 255; /* NONE */
mjarvisal 0:e6fcdb78a136 2186 } else if (DeviceRangeStatusInternal == 1 ||
mjarvisal 0:e6fcdb78a136 2187 DeviceRangeStatusInternal == 2 ||
mjarvisal 0:e6fcdb78a136 2188 DeviceRangeStatusInternal == 3) {
mjarvisal 0:e6fcdb78a136 2189 *pPalRangeStatus = 5; /* HW fail */
mjarvisal 0:e6fcdb78a136 2190 } else if (DeviceRangeStatusInternal == 6 ||
mjarvisal 0:e6fcdb78a136 2191 DeviceRangeStatusInternal == 9) {
mjarvisal 0:e6fcdb78a136 2192 *pPalRangeStatus = 4; /* Phase fail */
mjarvisal 0:e6fcdb78a136 2193 } else if (DeviceRangeStatusInternal == 8 ||
mjarvisal 0:e6fcdb78a136 2194 DeviceRangeStatusInternal == 10 ||
mjarvisal 0:e6fcdb78a136 2195 SignalRefClipflag == 1) {
mjarvisal 0:e6fcdb78a136 2196 *pPalRangeStatus = 3; /* Min range */
mjarvisal 0:e6fcdb78a136 2197 } else if (DeviceRangeStatusInternal == 4 ||
mjarvisal 0:e6fcdb78a136 2198 RangeIgnoreThresholdflag == 1) {
mjarvisal 0:e6fcdb78a136 2199 *pPalRangeStatus = 2; /* Signal Fail */
mjarvisal 0:e6fcdb78a136 2200 } else if (SigmaLimitflag == 1) {
mjarvisal 0:e6fcdb78a136 2201 *pPalRangeStatus = 1; /* Sigma Fail */
mjarvisal 0:e6fcdb78a136 2202 } else {
mjarvisal 0:e6fcdb78a136 2203 *pPalRangeStatus = 0; /* Range Valid */
mjarvisal 0:e6fcdb78a136 2204 }
mjarvisal 0:e6fcdb78a136 2205 }
mjarvisal 0:e6fcdb78a136 2206
mjarvisal 0:e6fcdb78a136 2207 /* DMAX only relevant during range error */
mjarvisal 0:e6fcdb78a136 2208 if (*pPalRangeStatus == 0)
mjarvisal 0:e6fcdb78a136 2209 pRangingMeasurementData->RangeDMaxMilliMeter = 0;
mjarvisal 0:e6fcdb78a136 2210
mjarvisal 0:e6fcdb78a136 2211 /* fill the Limit Check Status */
mjarvisal 0:e6fcdb78a136 2212
mjarvisal 0:e6fcdb78a136 2213 Status = VL53L0X_GetLimitCheckEnable(Dev,
mjarvisal 0:e6fcdb78a136 2214 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 2215 &SignalRateFinalRangeLimitCheckEnable);
mjarvisal 0:e6fcdb78a136 2216
mjarvisal 0:e6fcdb78a136 2217 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:e6fcdb78a136 2218 if ((SigmaLimitCheckEnable == 0) || (SigmaLimitflag == 1))
mjarvisal 0:e6fcdb78a136 2219 Temp8 = 1;
mjarvisal 0:e6fcdb78a136 2220 else
mjarvisal 0:e6fcdb78a136 2221 Temp8 = 0;
mjarvisal 0:e6fcdb78a136 2222 VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
mjarvisal 0:e6fcdb78a136 2223 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
mjarvisal 0:e6fcdb78a136 2224
mjarvisal 0:e6fcdb78a136 2225 if ((DeviceRangeStatusInternal == 4) ||
mjarvisal 0:e6fcdb78a136 2226 (SignalRateFinalRangeLimitCheckEnable == 0))
mjarvisal 0:e6fcdb78a136 2227 Temp8 = 1;
mjarvisal 0:e6fcdb78a136 2228 else
mjarvisal 0:e6fcdb78a136 2229 Temp8 = 0;
mjarvisal 0:e6fcdb78a136 2230 VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
mjarvisal 0:e6fcdb78a136 2231 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
mjarvisal 0:e6fcdb78a136 2232 Temp8);
mjarvisal 0:e6fcdb78a136 2233
mjarvisal 0:e6fcdb78a136 2234 if ((SignalRefClipLimitCheckEnable == 0) ||
mjarvisal 0:e6fcdb78a136 2235 (SignalRefClipflag == 1))
mjarvisal 0:e6fcdb78a136 2236 Temp8 = 1;
mjarvisal 0:e6fcdb78a136 2237 else
mjarvisal 0:e6fcdb78a136 2238 Temp8 = 0;
mjarvisal 0:e6fcdb78a136 2239
mjarvisal 0:e6fcdb78a136 2240 VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
mjarvisal 0:e6fcdb78a136 2241 VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8);
mjarvisal 0:e6fcdb78a136 2242
mjarvisal 0:e6fcdb78a136 2243 if ((RangeIgnoreThresholdLimitCheckEnable == 0) ||
mjarvisal 0:e6fcdb78a136 2244 (RangeIgnoreThresholdflag == 1))
mjarvisal 0:e6fcdb78a136 2245 Temp8 = 1;
mjarvisal 0:e6fcdb78a136 2246 else
mjarvisal 0:e6fcdb78a136 2247 Temp8 = 0;
mjarvisal 0:e6fcdb78a136 2248
mjarvisal 0:e6fcdb78a136 2249 VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
mjarvisal 0:e6fcdb78a136 2250 VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
mjarvisal 0:e6fcdb78a136 2251 Temp8);
mjarvisal 0:e6fcdb78a136 2252 }
mjarvisal 0:e6fcdb78a136 2253
mjarvisal 0:e6fcdb78a136 2254 LOG_FUNCTION_END(Status);
mjarvisal 0:e6fcdb78a136 2255 return Status;
mjarvisal 0:e6fcdb78a136 2256
mjarvisal 0:e6fcdb78a136 2257 }