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