1
2 /*
3 * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
4 *
5 * This file is part of VL53L1 Core and is dual licensed,
6 * either 'STMicroelectronics
7 * Proprietary license'
8 * or 'BSD 3-clause "New" or "Revised" License' , at your option.
9 *
10 ********************************************************************************
11 *
12 * 'STMicroelectronics Proprietary license'
13 *
14 ********************************************************************************
15 *
16 * License terms: STMicroelectronics Proprietary in accordance with licensing
17 * terms at www.st.com/sla0081
18 *
19 * STMicroelectronics confidential
20 * Reproduction and Communication of this document is strictly prohibited unless
21 * specifically authorized in writing by STMicroelectronics.
22 *
23 *
24 ********************************************************************************
25 *
26 * Alternatively, VL53L1 Core may be distributed under the terms of
27 * 'BSD 3-clause "New" or "Revised" License', in which case the following
28 * provisions apply instead of the ones mentioned above :
29 *
30 ********************************************************************************
31 *
32 * License terms: BSD 3-clause "New" or "Revised" License.
33 *
34 * Redistribution and use in source and binary forms, with or without
35 * modification, are permitted provided that the following conditions are met:
36 *
37 * 1. Redistributions of source code must retain the above copyright notice, this
38 * list of conditions and the following disclaimer.
39 *
40 * 2. Redistributions in binary form must reproduce the above copyright notice,
41 * this list of conditions and the following disclaimer in the documentation
42 * and/or other materials provided with the distribution.
43 *
44 * 3. Neither the name of the copyright holder nor the names of its contributors
45 * may be used to endorse or promote products derived from this software
46 * without specific prior written permission.
47 *
48 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
49 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
50 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
51 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
52 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
53 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
54 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
55 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
56 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
57 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
58 *
59 *
60 ********************************************************************************
61 *
62 */
63 #include "vl53l1_api.h"
64 #include "vl53l1_api_strings.h"
65 #include "vl53l1_register_settings.h"
66 #include "vl53l1_register_funcs.h"
67 #include "vl53l1_core.h"
68 #include "vl53l1_api_calibration.h"
69 #include "vl53l1_wait.h"
70 #include "vl53l1_preset_setup.h"
71 #include "vl53l1_api_debug.h"
72 #include "vl53l1_api_core.h"
73
74 /* Check for minimum user zone requested by Xtalk calibration */
75 /* no need for VL53L1_MAX_USER_ZONES check, set 5 to pass the test */
76 #define ZONE_CHECK 5
77
78 #if ZONE_CHECK < 5
79 #error Must define at least 5 zones in MAX_USER_ZONES constant
80 #endif
81
82
83 #ifdef VL53L1_NOCALIB
84 #define OFFSET_CALIB_EMPTY
85 #endif
86
87 #ifndef VL53L1_NOCALIB
88 #define OFFSET_CALIB
89 #endif
90
91 #define LOG_FUNCTION_START(fmt, ...) \
92 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_API, fmt, ##__VA_ARGS__)
93 #define LOG_FUNCTION_END(status, ...) \
94 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_API, status, ##__VA_ARGS__)
95 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
96 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_API, status, \
97 fmt, ##__VA_ARGS__)
98
99 #ifdef VL53L1_LOG_ENABLE
100 #define trace_print(level, ...) trace_print_module_function(\
101 VL53L1_TRACE_MODULE_API, level, VL53L1_TRACE_FUNCTION_NONE, \
102 ##__VA_ARGS__)
103 #endif
104
105 #ifndef MIN
106 #define MIN(v1, v2) ((v1) < (v2) ? (v1) : (v2))
107 #endif
108 #ifndef MAX
109 #define MAX(v1, v2) ((v1) < (v2) ? (v2) : (v1))
110 #endif
111
112 #define DMAX_REFLECTANCE_IDX 2
113 /* Use Dmax index 2 because it's the 50% reflectance case by default */
114
115 /* Following LOWPOWER_AUTO figures have been measured observing vcsel
116 * emissions on an actual device
117 */
118 #define LOWPOWER_AUTO_VHV_LOOP_DURATION_US 245
119 #define LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING 1448
120 #define LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING 2100
121
122 #define FDA_MAX_TIMING_BUDGET_US 550000
123 /* Maximum timing budget allowed codex #456189*/
124
125
126 /* local static utilities functions */
127
128 /* Bare Driver Tuning parameter table indexed with VL53L1_Tuning_t */
129 static int32_t BDTable[VL53L1_TUNING_MAX_TUNABLE_KEY] = {
130 TUNING_VERSION,
131 TUNING_PROXY_MIN,
132 TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM,
133 TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER,
134 TUNING_MIN_AMBIENT_DMAX_VALID,
135 TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER,
136 TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM,
137 TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT
138 };
139
140
141 #define VL53L1_NVM_POWER_UP_DELAY_US 50
142 #define VL53L1_NVM_READ_TRIGGER_DELAY_US 5
143
VL53L1_nvm_enable(VL53L1_DEV Dev,uint16_t nvm_ctrl_pulse_width,int32_t nvm_power_up_delay_us)144 static VL53L1_Error VL53L1_nvm_enable(
145 VL53L1_DEV Dev,
146 uint16_t nvm_ctrl_pulse_width,
147 int32_t nvm_power_up_delay_us)
148 {
149 /*
150 * Sequence below enables NVM for reading
151 *
152 * - Enable power force
153 * - Disable firmware
154 * - Power up NVM
155 * - Wait for 50us while the NVM powers up
156 * - Configure for reading and set the pulse width (16-bit)
157 */
158
159 VL53L1_Error status = VL53L1_ERROR_NONE;
160
161 LOG_FUNCTION_START("");
162
163
164 /* Disable Firmware */
165
166 if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
167 status = VL53L1_disable_firmware(Dev);
168
169
170 /* Enable Power Force */
171
172 if (status == VL53L1_ERROR_NONE)
173 status = VL53L1_enable_powerforce(Dev);
174
175 /* Wait the required time for the regulators, bandgap,
176 * oscillator to wake up and settle
177 */
178
179 if (status == VL53L1_ERROR_NONE)
180 status = VL53L1_WaitUs(
181 Dev,
182 VL53L1_ENABLE_POWERFORCE_SETTLING_TIME_US);
183
184 /* Power up NVM */
185
186 if (status == VL53L1_ERROR_NONE)
187 status = VL53L1_WrByte(
188 Dev,
189 VL53L1_RANGING_CORE__NVM_CTRL__PDN,
190 0x01);
191
192 /* Enable NVM Clock */
193
194 if (status == VL53L1_ERROR_NONE)
195 status = VL53L1_WrByte(
196 Dev,
197 VL53L1_RANGING_CORE__CLK_CTRL1,
198 0x05);
199
200 /* Wait the required time for NVM to power up*/
201
202 if (status == VL53L1_ERROR_NONE)
203 status = VL53L1_WaitUs(
204 Dev,
205 nvm_power_up_delay_us);
206
207 /* Select read mode and set control pulse width */
208
209 if (status == VL53L1_ERROR_NONE)
210 status = VL53L1_WrByte(
211 Dev,
212 VL53L1_RANGING_CORE__NVM_CTRL__MODE,
213 0x01);
214
215 if (status == VL53L1_ERROR_NONE)
216 status = VL53L1_WrWord(
217 Dev,
218 VL53L1_RANGING_CORE__NVM_CTRL__PULSE_WIDTH_MSB,
219 nvm_ctrl_pulse_width);
220
221 LOG_FUNCTION_END(status);
222
223 return status;
224
225 }
226
227
VL53L1_nvm_read(VL53L1_DEV Dev,uint8_t start_address,uint8_t count,uint8_t * pdata)228 static VL53L1_Error VL53L1_nvm_read(
229 VL53L1_DEV Dev,
230 uint8_t start_address,
231 uint8_t count,
232 uint8_t *pdata)
233 {
234 /*
235 * Sequence per 32-bit NVM read access:
236 *
237 * - Set up the 5-bit (0-127) NVM Address
238 * - Trigger the read of the NVM data by toggling NVM_CTRL__READN
239 * - Read the NVM data - 4 bytes wide read/write interface
240 * - Increment data byte pointer by 4 ready for the next loop
241 */
242
243 VL53L1_Error status = VL53L1_ERROR_NONE;
244 uint8_t nvm_addr = 0;
245
246 LOG_FUNCTION_START("");
247
248 for (nvm_addr = start_address; nvm_addr < (start_address+count) ; nvm_addr++) {
249
250 /* Step 1 : set address */
251
252 if (status == VL53L1_ERROR_NONE)
253 status = VL53L1_WrByte(
254 Dev,
255 VL53L1_RANGING_CORE__NVM_CTRL__ADDR,
256 nvm_addr);
257
258 /* Step 2 : trigger reading of data */
259
260 if (status == VL53L1_ERROR_NONE)
261 status = VL53L1_WrByte(
262 Dev,
263 VL53L1_RANGING_CORE__NVM_CTRL__READN,
264 0x00);
265
266 /* Step 3 : wait the required time */
267
268 if (status == VL53L1_ERROR_NONE)
269 status = VL53L1_WaitUs(
270 Dev,
271 VL53L1_NVM_READ_TRIGGER_DELAY_US);
272
273 if (status == VL53L1_ERROR_NONE)
274 status = VL53L1_WrByte(
275 Dev,
276 VL53L1_RANGING_CORE__NVM_CTRL__READN,
277 0x01);
278
279 /* Step 3 : read 4-byte wide data register */
280 if (status == VL53L1_ERROR_NONE)
281 status = VL53L1_ReadMulti(
282 Dev,
283 VL53L1_RANGING_CORE__NVM_CTRL__DATAOUT_MMM,
284 pdata,
285 4);
286
287 /* Step 4 : increment byte buffer pointer */
288
289 pdata = pdata + 4;
290
291
292 }
293
294 LOG_FUNCTION_END(status);
295
296 return status;
297 }
298
VL53L1_nvm_disable(VL53L1_DEV Dev)299 static VL53L1_Error VL53L1_nvm_disable(
300 VL53L1_DEV Dev)
301 {
302 /*
303 * Power down NVM (OTP) to extend lifetime
304 */
305
306 VL53L1_Error status = VL53L1_ERROR_NONE;
307
308 LOG_FUNCTION_START("");
309
310 if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
311 status = VL53L1_WrByte(
312 Dev,
313 VL53L1_RANGING_CORE__NVM_CTRL__READN,
314 0x01);
315
316 /* Power down NVM */
317
318 if (status == VL53L1_ERROR_NONE)
319 status = VL53L1_WrByte(
320 Dev,
321 VL53L1_RANGING_CORE__NVM_CTRL__PDN,
322 0x00);
323
324 /* Keep power force enabled */
325
326 if (status == VL53L1_ERROR_NONE)
327 status = VL53L1_disable_powerforce(Dev);
328
329 /* (Re)Enable Firmware */
330
331 if (status == VL53L1_ERROR_NONE)
332 status = VL53L1_enable_firmware(Dev);
333
334 LOG_FUNCTION_END(status);
335
336 return status;
337
338 }
339
VL53L1_read_nvm_raw_data(VL53L1_DEV Dev,uint8_t start_address,uint8_t count,uint8_t * pnvm_raw_data)340 static VL53L1_Error VL53L1_read_nvm_raw_data(
341 VL53L1_DEV Dev,
342 uint8_t start_address,
343 uint8_t count,
344 uint8_t *pnvm_raw_data)
345 {
346
347 /*
348 * Reads ALL 512 bytes of NVM data
349 */
350
351 VL53L1_Error status = VL53L1_ERROR_NONE;
352
353 LOG_FUNCTION_START("");
354
355 /*
356 * Enable NVM and set control pulse width
357 */
358
359 if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
360 status = VL53L1_nvm_enable(
361 Dev,
362 0x0004,
363 VL53L1_NVM_POWER_UP_DELAY_US);
364
365 /*
366 * Read the raw NVM data
367 * - currently all of 128 * 4 bytes = 512 bytes are read
368 */
369
370 if (status == VL53L1_ERROR_NONE)
371 status = VL53L1_nvm_read(
372 Dev,
373 start_address,
374 count,
375 pnvm_raw_data);
376
377 /*
378 * Disable NVM
379 */
380
381 if (status == VL53L1_ERROR_NONE)
382 status = VL53L1_nvm_disable(Dev);
383
384 LOG_FUNCTION_END(status);
385
386 return status;
387
388 }
389
SingleTargetXTalkCalibration(VL53L1_DEV Dev)390 static VL53L1_Error SingleTargetXTalkCalibration(VL53L1_DEV Dev)
391 {
392 VL53L1_Error Status = VL53L1_ERROR_NONE;
393
394 uint32_t sum_ranging = 0;
395 uint32_t sum_spads = 0;
396 FixPoint1616_t sum_signalRate = 0;
397 FixPoint1616_t total_count = 0;
398 uint8_t xtalk_meas = 0;
399 uint8_t xtalk_measmax =
400 BDTable[VL53L1_TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER];
401 VL53L1_RangingMeasurementData_t RMData;
402 FixPoint1616_t xTalkStoredMeanSignalRate;
403 FixPoint1616_t xTalkStoredMeanRange;
404 FixPoint1616_t xTalkStoredMeanRtnSpads;
405 uint32_t xTalkStoredMeanRtnSpadsAsInt;
406 uint32_t xTalkCalDistanceAsInt;
407 FixPoint1616_t XTalkCompensationRateMegaCps;
408 uint32_t signalXTalkTotalPerSpad;
409 VL53L1_PresetModes PresetMode;
410 VL53L1_CalibrationData_t CalibrationData;
411 VL53L1_CustomerNvmManaged_t *pC;
412
413
414 LOG_FUNCTION_START("");
415
416 /* check if the following are selected
417 * VL53L1_PRESETMODE_AUTONOMOUS,
418 * VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS
419 * VL53L1_PRESETMODE_LITE_RANGING
420 */
421 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
422
423 if ((PresetMode != VL53L1_PRESETMODE_AUTONOMOUS) &&
424 (PresetMode != VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS) &&
425 (PresetMode != VL53L1_PRESETMODE_LITE_RANGING)) {
426 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
427 goto ENDFUNC;
428 }
429
430 /* disable crosstalk calibration */
431 Status = VL53L1_disable_xtalk_compensation(Dev);
432 CHECK_ERROR_GO_ENDFUNC;
433
434 Status = VL53L1_StartMeasurement(Dev);
435 CHECK_ERROR_GO_ENDFUNC;
436
437 sum_ranging = 0;
438 sum_spads = 0;
439 sum_signalRate = 0;
440 total_count = 0;
441 for (xtalk_meas = 0; xtalk_meas < xtalk_measmax; xtalk_meas++) {
442 VL53L1_WaitMeasurementDataReady(Dev);
443 VL53L1_GetRangingMeasurementData(Dev, &RMData);
444 VL53L1_ClearInterruptAndStartMeasurement(Dev);
445 if (RMData.RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
446 sum_ranging += RMData.RangeMilliMeter;
447 sum_signalRate += RMData.SignalRateRtnMegaCps;
448 sum_spads += RMData.EffectiveSpadRtnCount / 256;
449 total_count++;
450 }
451 }
452 Status = VL53L1_StopMeasurement(Dev);
453
454 if (total_count > 0) {
455 /* FixPoint1616_t / uint16_t = FixPoint1616_t */
456 xTalkStoredMeanSignalRate = sum_signalRate / total_count;
457 xTalkStoredMeanRange = (FixPoint1616_t)(sum_ranging << 16);
458 xTalkStoredMeanRange /= total_count;
459 xTalkStoredMeanRtnSpads = (FixPoint1616_t)(sum_spads << 16);
460 xTalkStoredMeanRtnSpads /= total_count;
461
462 /* Round Mean Spads to Whole Number.
463 * Typically the calculated mean SPAD count is a whole number
464 * or very close to a whole
465 * number, therefore any truncation will not result in a
466 * significant loss in accuracy.
467 * Also, for a grey target at a typical distance of around
468 * 400mm, around 220 SPADs will
469 * be enabled, therefore, any truncation will result in a loss
470 * of accuracy of less than
471 * 0.5%.
472 */
473 xTalkStoredMeanRtnSpadsAsInt = (xTalkStoredMeanRtnSpads +
474 0x8000) >> 16;
475
476 /* Round Cal Distance to Whole Number.
477 * Note that the cal distance is in mm, therefore no resolution
478 * is lost.
479 */
480 xTalkCalDistanceAsInt = ((uint32_t)BDTable[
481 VL53L1_TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM]);
482 if (xTalkStoredMeanRtnSpadsAsInt == 0 ||
483 xTalkCalDistanceAsInt == 0 ||
484 xTalkStoredMeanRange >= (xTalkCalDistanceAsInt << 16)) {
485 XTalkCompensationRateMegaCps = 0;
486 } else {
487 /* Apply division by mean spad count early in the
488 * calculation to keep the numbers small.
489 * This ensures we can maintain a 32bit calculation.
490 * Fixed1616 / int := Fixed1616
491 */
492 signalXTalkTotalPerSpad = (xTalkStoredMeanSignalRate) /
493 xTalkStoredMeanRtnSpadsAsInt;
494
495 /* Complete the calculation for total Signal XTalk per
496 * SPAD
497 * Fixed1616 * (Fixed1616 - Fixed1616/int) :=
498 * (2^16 * Fixed1616)
499 */
500 signalXTalkTotalPerSpad *= (((uint32_t)1 << 16) -
501 (xTalkStoredMeanRange / xTalkCalDistanceAsInt));
502
503 /* Round from 2^16 * Fixed1616, to Fixed1616. */
504 XTalkCompensationRateMegaCps = (signalXTalkTotalPerSpad
505 + 0x8000) >> 16;
506 }
507
508
509 Status = VL53L1_GetCalibrationData(Dev, &CalibrationData);
510 CHECK_ERROR_GO_ENDFUNC;
511
512 pC = &CalibrationData.customer;
513
514 pC->algo__crosstalk_compensation_plane_offset_kcps =
515 (uint32_t)(1000 * ((XTalkCompensationRateMegaCps +
516 ((uint32_t)1<<6)) >> (16-9)));
517
518 Status = VL53L1_SetCalibrationData(Dev, &CalibrationData);
519 CHECK_ERROR_GO_ENDFUNC;
520
521 Status = VL53L1_enable_xtalk_compensation(Dev);
522
523 } else
524 /* return error because no valid data found */
525 Status = VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL;
526
527 ENDFUNC:
528 LOG_FUNCTION_END(Status);
529 return Status;
530
531 }
532
533 /* Check Rectangle in user's coordinate system:
534 * 15 TL(x,y) o-----*
535 * ^ | |
536 * | *-----o BR(x,y)
537 * 0------------------------- >15
538 * check Rectangle definition conforms to the (0,15,15) coordinate system
539 * with a minimum of 4x4 size
540 */
CheckValidRectRoi(VL53L1_UserRoi_t ROI)541 static VL53L1_Error CheckValidRectRoi(VL53L1_UserRoi_t ROI)
542 {
543 VL53L1_Error Status = VL53L1_ERROR_NONE;
544
545 LOG_FUNCTION_START("");
546
547 /* Negative check are not necessary because value is unsigned */
548 if ((ROI.TopLeftX > 15) || (ROI.TopLeftY > 15) ||
549 (ROI.BotRightX > 15) || (ROI.BotRightY > 15))
550 Status = VL53L1_ERROR_INVALID_PARAMS;
551
552 if ((ROI.TopLeftX > ROI.BotRightX) || (ROI.TopLeftY < ROI.BotRightY))
553 Status = VL53L1_ERROR_INVALID_PARAMS;
554
555 LOG_FUNCTION_END(Status);
556 return Status;
557 }
558
ConvertModeToLLD(VL53L1_Error * pStatus,VL53L1_ThresholdMode CrossMode)559 static VL53L1_GPIO_Interrupt_Mode ConvertModeToLLD(VL53L1_Error *pStatus,
560 VL53L1_ThresholdMode CrossMode)
561 {
562 VL53L1_GPIO_Interrupt_Mode Mode;
563
564 switch (CrossMode) {
565 case VL53L1_THRESHOLD_CROSSED_LOW:
566 Mode = VL53L1_GPIOINTMODE_LEVEL_LOW;
567 break;
568 case VL53L1_THRESHOLD_CROSSED_HIGH:
569 Mode = VL53L1_GPIOINTMODE_LEVEL_HIGH;
570 break;
571 case VL53L1_THRESHOLD_OUT_OF_WINDOW:
572 Mode = VL53L1_GPIOINTMODE_OUT_OF_WINDOW;
573 break;
574 case VL53L1_THRESHOLD_IN_WINDOW:
575 Mode = VL53L1_GPIOINTMODE_IN_WINDOW;
576 break;
577 default:
578 /* define Mode to avoid warning but actual value doesn't mind */
579 Mode = VL53L1_GPIOINTMODE_LEVEL_HIGH;
580 *pStatus = VL53L1_ERROR_INVALID_PARAMS;
581 }
582 return Mode;
583 }
584
ConvertModeFromLLD(VL53L1_Error * pStatus,VL53L1_GPIO_Interrupt_Mode CrossMode)585 static VL53L1_ThresholdMode ConvertModeFromLLD(VL53L1_Error *pStatus,
586 VL53L1_GPIO_Interrupt_Mode CrossMode)
587 {
588 VL53L1_ThresholdMode Mode;
589
590 switch (CrossMode) {
591 case VL53L1_GPIOINTMODE_LEVEL_LOW:
592 Mode = VL53L1_THRESHOLD_CROSSED_LOW;
593 break;
594 case VL53L1_GPIOINTMODE_LEVEL_HIGH:
595 Mode = VL53L1_THRESHOLD_CROSSED_HIGH;
596 break;
597 case VL53L1_GPIOINTMODE_OUT_OF_WINDOW:
598 Mode = VL53L1_THRESHOLD_OUT_OF_WINDOW;
599 break;
600 case VL53L1_GPIOINTMODE_IN_WINDOW:
601 Mode = VL53L1_THRESHOLD_IN_WINDOW;
602 break;
603 default:
604 /* define Mode to avoid warning but actual value doesn't mind */
605 Mode = VL53L1_THRESHOLD_CROSSED_HIGH;
606 *pStatus = VL53L1_ERROR_UNDEFINED;
607 }
608 return Mode;
609 }
610
611 /* Group PAL General Functions */
612
VL53L1_GetVersion(VL53L1_Version_t * pVersion)613 VL53L1_Error VL53L1_GetVersion(VL53L1_Version_t *pVersion)
614 {
615 VL53L1_Error Status = VL53L1_ERROR_NONE;
616
617 LOG_FUNCTION_START("");
618
619 pVersion->major = VL53L1_IMPLEMENTATION_VER_MAJOR;
620 pVersion->minor = VL53L1_IMPLEMENTATION_VER_MINOR;
621 pVersion->build = VL53L1_IMPLEMENTATION_VER_SUB;
622
623 pVersion->revision = VL53L1_IMPLEMENTATION_VER_REVISION;
624
625 LOG_FUNCTION_END(Status);
626 return Status;
627 }
628
VL53L1_GetProductRevision(VL53L1_DEV Dev,uint8_t * pProductRevisionMajor,uint8_t * pProductRevisionMinor)629 VL53L1_Error VL53L1_GetProductRevision(VL53L1_DEV Dev,
630 uint8_t *pProductRevisionMajor, uint8_t *pProductRevisionMinor)
631 {
632 VL53L1_Error Status = VL53L1_ERROR_NONE;
633 uint8_t revision_id;
634 VL53L1_LLDriverData_t *pLLData;
635
636 LOG_FUNCTION_START("");
637
638 pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
639 revision_id = pLLData->nvm_copy_data.identification__revision_id;
640 *pProductRevisionMajor = 1;
641 *pProductRevisionMinor = (revision_id & 0xF0) >> 4;
642
643 LOG_FUNCTION_END(Status);
644 return Status;
645
646 }
647
VL53L1_GetDeviceInfo(VL53L1_DEV Dev,VL53L1_DeviceInfo_t * pVL53L1_DeviceInfo)648 VL53L1_Error VL53L1_GetDeviceInfo(VL53L1_DEV Dev,
649 VL53L1_DeviceInfo_t *pVL53L1_DeviceInfo)
650 {
651 VL53L1_Error Status = VL53L1_ERROR_NONE;
652 uint8_t revision_id;
653 VL53L1_LLDriverData_t *pLLData;
654
655 LOG_FUNCTION_START("");
656
657 pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
658
659 strncpy(pVL53L1_DeviceInfo->ProductId, "",
660 VL53L1_DEVINFO_STRLEN-1);
661 pVL53L1_DeviceInfo->ProductType =
662 pLLData->nvm_copy_data.identification__module_type;
663
664 revision_id = pLLData->nvm_copy_data.identification__revision_id;
665 pVL53L1_DeviceInfo->ProductRevisionMajor = 1;
666 pVL53L1_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4;
667
668 #ifndef VL53L1_USE_EMPTY_STRING
669 if (pVL53L1_DeviceInfo->ProductRevisionMinor == 0)
670 strncpy(pVL53L1_DeviceInfo->Name,
671 VL53L1_STRING_DEVICE_INFO_NAME0,
672 VL53L1_DEVINFO_STRLEN-1);
673 else
674 strncpy(pVL53L1_DeviceInfo->Name,
675 VL53L1_STRING_DEVICE_INFO_NAME1,
676 VL53L1_DEVINFO_STRLEN-1);
677 strncpy(pVL53L1_DeviceInfo->Type,
678 VL53L1_STRING_DEVICE_INFO_TYPE,
679 VL53L1_DEVINFO_STRLEN-1);
680 #else
681 pVL53L1_DeviceInfo->Name[0] = 0;
682 pVL53L1_DeviceInfo->Type[0] = 0;
683 #endif
684
685 LOG_FUNCTION_END(Status);
686 return Status;
687 }
688
689
VL53L1_GetRangeStatusString(uint8_t RangeStatus,char * pRangeStatusString)690 VL53L1_Error VL53L1_GetRangeStatusString(uint8_t RangeStatus,
691 char *pRangeStatusString)
692 {
693 VL53L1_Error Status = VL53L1_ERROR_NONE;
694
695 LOG_FUNCTION_START("");
696
697 Status = VL53L1_get_range_status_string(RangeStatus,
698 pRangeStatusString);
699
700 LOG_FUNCTION_END(Status);
701 return Status;
702 }
703
VL53L1_GetPalErrorString(VL53L1_Error PalErrorCode,char * pPalErrorString)704 VL53L1_Error VL53L1_GetPalErrorString(VL53L1_Error PalErrorCode,
705 char *pPalErrorString)
706 {
707 VL53L1_Error Status = VL53L1_ERROR_NONE;
708
709 LOG_FUNCTION_START("");
710
711 Status = VL53L1_get_pal_error_string(PalErrorCode, pPalErrorString);
712
713 LOG_FUNCTION_END(Status);
714 return Status;
715 }
716
VL53L1_GetPalStateString(VL53L1_State PalStateCode,char * pPalStateString)717 VL53L1_Error VL53L1_GetPalStateString(VL53L1_State PalStateCode,
718 char *pPalStateString)
719 {
720 VL53L1_Error Status = VL53L1_ERROR_NONE;
721
722 LOG_FUNCTION_START("");
723
724 Status = VL53L1_get_pal_state_string(PalStateCode, pPalStateString);
725
726 LOG_FUNCTION_END(Status);
727 return Status;
728 }
729
VL53L1_GetPalState(VL53L1_DEV Dev,VL53L1_State * pPalState)730 VL53L1_Error VL53L1_GetPalState(VL53L1_DEV Dev, VL53L1_State *pPalState)
731 {
732 VL53L1_Error Status = VL53L1_ERROR_NONE;
733
734 LOG_FUNCTION_START("");
735
736 *pPalState = VL53L1DevDataGet(Dev, PalState);
737
738 LOG_FUNCTION_END(Status);
739 return Status;
740 }
741
742 /* End Group PAL General Functions */
743
744 /* Group PAL Init Functions */
VL53L1_SetDeviceAddress(VL53L1_DEV Dev,uint8_t DeviceAddress)745 VL53L1_Error VL53L1_SetDeviceAddress(VL53L1_DEV Dev, uint8_t DeviceAddress)
746 {
747 VL53L1_Error Status = VL53L1_ERROR_NONE;
748
749 LOG_FUNCTION_START("");
750
751 Status = VL53L1_WrByte(Dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS,
752 DeviceAddress / 2);
753
754 LOG_FUNCTION_END(Status);
755 return Status;
756 }
757
VL53L1_DataInit(VL53L1_DEV Dev)758 VL53L1_Error VL53L1_DataInit(VL53L1_DEV Dev)
759 {
760 VL53L1_Error Status = VL53L1_ERROR_NONE;
761 uint8_t i;
762
763 LOG_FUNCTION_START("");
764
765 /* 2V8 power mode selection codex 447463 */
766 #ifdef USE_I2C_2V8
767 Status = VL53L1_RdByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, &i);
768 if (Status == VL53L1_ERROR_NONE) {
769 i = (i & 0xfe) | 0x01;
770 Status = VL53L1_WrByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG,
771 i);
772 }
773 #endif
774
775 if (Status == VL53L1_ERROR_NONE)
776 Status = VL53L1_data_init(Dev, 1);
777
778 if (Status == VL53L1_ERROR_NONE) {
779 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_WAIT_STATICINIT);
780 VL53L1DevDataSet(Dev, CurrentParameters.PresetMode,
781 VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS);
782 }
783
784 /* Enable all check */
785 for (i = 0; i < VL53L1_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
786 if (Status == VL53L1_ERROR_NONE)
787 Status |= VL53L1_SetLimitCheckEnable(Dev, i, 1);
788 else
789 break;
790
791 }
792
793 /* Limit default values */
794 if (Status == VL53L1_ERROR_NONE) {
795 Status = VL53L1_SetLimitCheckValue(Dev,
796 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
797 (FixPoint1616_t)(18 * 65536));
798 }
799 if (Status == VL53L1_ERROR_NONE) {
800 Status = VL53L1_SetLimitCheckValue(Dev,
801 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
802 (FixPoint1616_t)(25 * 65536 / 100));
803 /* 0.25 * 65536 */
804 }
805
806 LOG_FUNCTION_END(Status);
807 return Status;
808 }
809
810
VL53L1_StaticInit(VL53L1_DEV Dev)811 VL53L1_Error VL53L1_StaticInit(VL53L1_DEV Dev)
812 {
813 VL53L1_Error Status = VL53L1_ERROR_NONE;
814 uint8_t measurement_mode;
815
816 LOG_FUNCTION_START("");
817
818 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_IDLE);
819
820 measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
821 VL53L1DevDataSet(Dev, LLData.measurement_mode, measurement_mode);
822
823 VL53L1DevDataSet(Dev, CurrentParameters.NewDistanceMode,
824 VL53L1_DISTANCEMODE_LONG);
825
826 VL53L1DevDataSet(Dev, CurrentParameters.InternalDistanceMode,
827 VL53L1_DISTANCEMODE_LONG);
828
829 VL53L1DevDataSet(Dev, CurrentParameters.DistanceMode,
830 VL53L1_DISTANCEMODE_LONG);
831
832 /* ticket 472728 fix */
833 Status = VL53L1_SetPresetMode(Dev,
834 VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS);
835 /* end of ticket 472728 fix */
836 LOG_FUNCTION_END(Status);
837 return Status;
838 }
839
VL53L1_WaitDeviceBooted(VL53L1_DEV Dev)840 VL53L1_Error VL53L1_WaitDeviceBooted(VL53L1_DEV Dev)
841 {
842 VL53L1_Error Status = VL53L1_ERROR_NONE;
843
844 LOG_FUNCTION_START("");
845
846 Status = VL53L1_poll_for_boot_completion(Dev,
847 VL53L1_BOOT_COMPLETION_POLLING_TIMEOUT_MS);
848
849 LOG_FUNCTION_END(Status);
850 return Status;
851 }
852
853 /* End Group PAL Init Functions */
854
855 /* Group PAL Parameters Functions */
ComputeDevicePresetMode(VL53L1_PresetModes PresetMode,VL53L1_DistanceModes DistanceMode,VL53L1_DevicePresetModes * pDevicePresetMode)856 static VL53L1_Error ComputeDevicePresetMode(
857 VL53L1_PresetModes PresetMode,
858 VL53L1_DistanceModes DistanceMode,
859 VL53L1_DevicePresetModes *pDevicePresetMode)
860 {
861 VL53L1_Error Status = VL53L1_ERROR_NONE;
862
863 uint8_t DistIdx;
864 VL53L1_DevicePresetModes LightModes[3] = {
865 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_SHORT_RANGE,
866 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING,
867 VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_LONG_RANGE};
868
869
870 VL53L1_DevicePresetModes TimedModes[3] = {
871 VL53L1_DEVICEPRESETMODE_TIMED_RANGING_SHORT_RANGE,
872 VL53L1_DEVICEPRESETMODE_TIMED_RANGING,
873 VL53L1_DEVICEPRESETMODE_TIMED_RANGING_LONG_RANGE};
874
875 VL53L1_DevicePresetModes LowPowerTimedModes[3] = {
876 VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_SHORT_RANGE,
877 VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_MEDIUM_RANGE,
878 VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_LONG_RANGE};
879
880 *pDevicePresetMode = VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
881
882 switch (DistanceMode) {
883 case VL53L1_DISTANCEMODE_SHORT:
884 DistIdx = 0;
885 break;
886 case VL53L1_DISTANCEMODE_MEDIUM:
887 DistIdx = 1;
888 break;
889 default:
890 DistIdx = 2;
891 }
892
893 switch (PresetMode) {
894 case VL53L1_PRESETMODE_LITE_RANGING:
895 *pDevicePresetMode = LightModes[DistIdx];
896 break;
897
898
899 case VL53L1_PRESETMODE_AUTONOMOUS:
900 *pDevicePresetMode = TimedModes[DistIdx];
901 break;
902
903 case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
904 *pDevicePresetMode = LowPowerTimedModes[DistIdx];
905 break;
906
907 default:
908 /* Unsupported mode */
909 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
910 }
911
912 return Status;
913 }
914
SetPresetMode(VL53L1_DEV Dev,VL53L1_PresetModes PresetMode,VL53L1_DistanceModes DistanceMode,uint32_t inter_measurement_period_ms)915 static VL53L1_Error SetPresetMode(VL53L1_DEV Dev,
916 VL53L1_PresetModes PresetMode,
917 VL53L1_DistanceModes DistanceMode,
918 uint32_t inter_measurement_period_ms)
919 {
920 VL53L1_Error Status = VL53L1_ERROR_NONE;
921 VL53L1_DevicePresetModes device_preset_mode;
922 uint8_t measurement_mode;
923 uint16_t dss_config__target_total_rate_mcps;
924 uint32_t phasecal_config_timeout_us;
925 uint32_t mm_config_timeout_us;
926 uint32_t lld_range_config_timeout_us;
927
928 LOG_FUNCTION_START("%d", (int)PresetMode);
929
930 if ((PresetMode == VL53L1_PRESETMODE_AUTONOMOUS) ||
931 (PresetMode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS))
932 measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_TIMED;
933 else
934 measurement_mode = VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
935
936
937 Status = ComputeDevicePresetMode(PresetMode, DistanceMode,
938 &device_preset_mode);
939
940 if (Status == VL53L1_ERROR_NONE)
941 Status = VL53L1_get_preset_mode_timing_cfg(Dev,
942 device_preset_mode,
943 &dss_config__target_total_rate_mcps,
944 &phasecal_config_timeout_us,
945 &mm_config_timeout_us,
946 &lld_range_config_timeout_us);
947
948 if (Status == VL53L1_ERROR_NONE)
949 Status = VL53L1_set_preset_mode(
950 Dev,
951 device_preset_mode,
952 dss_config__target_total_rate_mcps,
953 phasecal_config_timeout_us,
954 mm_config_timeout_us,
955 lld_range_config_timeout_us,
956 inter_measurement_period_ms);
957
958 if (Status == VL53L1_ERROR_NONE)
959 VL53L1DevDataSet(Dev, LLData.measurement_mode, measurement_mode);
960
961 if (Status == VL53L1_ERROR_NONE)
962 VL53L1DevDataSet(Dev, CurrentParameters.PresetMode, PresetMode);
963
964 LOG_FUNCTION_END(Status);
965 return Status;
966 }
967
VL53L1_SetPresetMode(VL53L1_DEV Dev,VL53L1_PresetModes PresetMode)968 VL53L1_Error VL53L1_SetPresetMode(VL53L1_DEV Dev, VL53L1_PresetModes PresetMode)
969 {
970 VL53L1_Error Status = VL53L1_ERROR_NONE;
971 VL53L1_DistanceModes DistanceMode = VL53L1_DISTANCEMODE_LONG;
972
973 LOG_FUNCTION_START("%d", (int)PresetMode);
974
975 Status = SetPresetMode(Dev,
976 PresetMode,
977 DistanceMode,
978 1000);
979
980 if (Status == VL53L1_ERROR_NONE) {
981 VL53L1DevDataSet(Dev, CurrentParameters.InternalDistanceMode,
982 DistanceMode);
983
984 VL53L1DevDataSet(Dev, CurrentParameters.NewDistanceMode,
985 DistanceMode);
986
987 if ((PresetMode == VL53L1_PRESETMODE_LITE_RANGING) ||
988 (PresetMode == VL53L1_PRESETMODE_AUTONOMOUS) ||
989 (PresetMode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS))
990 Status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(
991 Dev, 41000);
992 else
993 /* Set default timing budget to 30Hz (33.33 ms)*/
994 Status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(
995 Dev, 33333);
996 }
997
998 if (Status == VL53L1_ERROR_NONE) {
999 /* Set default intermeasurement period to 1000 ms */
1000 Status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev,
1001 1000);
1002 }
1003
1004 LOG_FUNCTION_END(Status);
1005 return Status;
1006 }
1007
1008
VL53L1_GetPresetMode(VL53L1_DEV Dev,VL53L1_PresetModes * pPresetMode)1009 VL53L1_Error VL53L1_GetPresetMode(VL53L1_DEV Dev,
1010 VL53L1_PresetModes *pPresetMode)
1011 {
1012 VL53L1_Error Status = VL53L1_ERROR_NONE;
1013
1014 LOG_FUNCTION_START("");
1015
1016 *pPresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
1017
1018 LOG_FUNCTION_END(Status);
1019 return Status;
1020 }
1021
VL53L1_SetDistanceMode(VL53L1_DEV Dev,VL53L1_DistanceModes DistanceMode)1022 VL53L1_Error VL53L1_SetDistanceMode(VL53L1_DEV Dev,
1023 VL53L1_DistanceModes DistanceMode)
1024 {
1025 VL53L1_Error Status = VL53L1_ERROR_NONE;
1026 VL53L1_PresetModes PresetMode;
1027 VL53L1_DistanceModes InternalDistanceMode;
1028 uint32_t inter_measurement_period_ms;
1029 uint32_t TimingBudget;
1030 uint32_t MmTimeoutUs;
1031 uint32_t PhaseCalTimeoutUs;
1032 VL53L1_user_zone_t user_zone;
1033
1034 LOG_FUNCTION_START("%d", (int)DistanceMode);
1035
1036 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
1037
1038 /* when the distance mode is valid:
1039 * Manual Mode: all modes
1040 * AUTO AUTO_LITE : LITE_RANGING, RANGING
1041 */
1042
1043 if ((DistanceMode != VL53L1_DISTANCEMODE_SHORT) &&
1044 (DistanceMode != VL53L1_DISTANCEMODE_MEDIUM) &&
1045 (DistanceMode != VL53L1_DISTANCEMODE_LONG))
1046 return VL53L1_ERROR_INVALID_PARAMS;
1047
1048 /* The internal distance mode is limited to Short, Medium or
1049 * long only
1050 */
1051 if (Status == VL53L1_ERROR_NONE) {
1052 if ((DistanceMode == VL53L1_DISTANCEMODE_SHORT) ||
1053 (DistanceMode == VL53L1_DISTANCEMODE_MEDIUM))
1054 InternalDistanceMode = DistanceMode;
1055 else /* (DistanceMode == VL53L1_DISTANCEMODE_LONG) */
1056 InternalDistanceMode = VL53L1_DISTANCEMODE_LONG;
1057 }
1058
1059 if (Status == VL53L1_ERROR_NONE)
1060 Status = VL53L1_get_user_zone(Dev, &user_zone);
1061
1062 inter_measurement_period_ms = VL53L1DevDataGet(Dev,
1063 LLData.inter_measurement_period_ms);
1064
1065 if (Status == VL53L1_ERROR_NONE)
1066 Status = VL53L1_get_timeouts_us(Dev, &PhaseCalTimeoutUs,
1067 &MmTimeoutUs, &TimingBudget);
1068
1069 if (Status == VL53L1_ERROR_NONE)
1070 Status = SetPresetMode(Dev,
1071 PresetMode,
1072 InternalDistanceMode,
1073 inter_measurement_period_ms);
1074
1075 if (Status == VL53L1_ERROR_NONE) {
1076 VL53L1DevDataSet(Dev, CurrentParameters.InternalDistanceMode,
1077 InternalDistanceMode);
1078 VL53L1DevDataSet(Dev, CurrentParameters.NewDistanceMode,
1079 InternalDistanceMode);
1080 VL53L1DevDataSet(Dev, CurrentParameters.DistanceMode,
1081 DistanceMode);
1082 }
1083
1084 if (Status == VL53L1_ERROR_NONE) {
1085 Status = VL53L1_set_timeouts_us(Dev, PhaseCalTimeoutUs,
1086 MmTimeoutUs, TimingBudget);
1087
1088 if (Status == VL53L1_ERROR_NONE)
1089 VL53L1DevDataSet(Dev, LLData.range_config_timeout_us,
1090 TimingBudget);
1091 }
1092
1093 if (Status == VL53L1_ERROR_NONE)
1094 Status = VL53L1_set_user_zone(Dev, &user_zone);
1095
1096 LOG_FUNCTION_END(Status);
1097 return Status;
1098 }
1099
VL53L1_GetDistanceMode(VL53L1_DEV Dev,VL53L1_DistanceModes * pDistanceMode)1100 VL53L1_Error VL53L1_GetDistanceMode(VL53L1_DEV Dev,
1101 VL53L1_DistanceModes *pDistanceMode)
1102 {
1103 VL53L1_Error Status = VL53L1_ERROR_NONE;
1104
1105 LOG_FUNCTION_START("");
1106
1107 *pDistanceMode = VL53L1DevDataGet(Dev, CurrentParameters.DistanceMode);
1108
1109 LOG_FUNCTION_END(Status);
1110 return Status;
1111 }
1112
1113
1114
1115
VL53L1_SetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,uint32_t MeasurementTimingBudgetMicroSeconds)1116 VL53L1_Error VL53L1_SetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,
1117 uint32_t MeasurementTimingBudgetMicroSeconds)
1118 {
1119 VL53L1_Error Status = VL53L1_ERROR_NONE;
1120 uint8_t Mm1Enabled;
1121 uint8_t Mm2Enabled;
1122 uint32_t TimingGuard;
1123 uint32_t divisor;
1124 uint32_t TimingBudget;
1125 uint32_t MmTimeoutUs;
1126 VL53L1_PresetModes PresetMode;
1127 uint32_t PhaseCalTimeoutUs;
1128 uint32_t vhv;
1129 int32_t vhv_loops;
1130 uint32_t FDAMaxTimingBudgetUs = FDA_MAX_TIMING_BUDGET_US;
1131
1132 LOG_FUNCTION_START("");
1133
1134 /* Timing budget is limited to 10 seconds */
1135 if (MeasurementTimingBudgetMicroSeconds > 10000000)
1136 Status = VL53L1_ERROR_INVALID_PARAMS;
1137
1138 if (Status == VL53L1_ERROR_NONE) {
1139 Status = VL53L1_GetSequenceStepEnable(Dev,
1140 VL53L1_SEQUENCESTEP_MM1, &Mm1Enabled);
1141 }
1142
1143 if (Status == VL53L1_ERROR_NONE) {
1144 Status = VL53L1_GetSequenceStepEnable(Dev,
1145 VL53L1_SEQUENCESTEP_MM2, &Mm2Enabled);
1146 }
1147
1148 if (Status == VL53L1_ERROR_NONE)
1149 Status = VL53L1_get_timeouts_us(Dev,
1150 &PhaseCalTimeoutUs,
1151 &MmTimeoutUs,
1152 &TimingBudget);
1153
1154 if (Status == VL53L1_ERROR_NONE) {
1155 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
1156
1157 TimingGuard = 0;
1158 divisor = 1;
1159 switch (PresetMode) {
1160 case VL53L1_PRESETMODE_LITE_RANGING:
1161 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
1162 TimingGuard = 5000;
1163 else
1164 TimingGuard = 1000;
1165 break;
1166
1167 case VL53L1_PRESETMODE_AUTONOMOUS:
1168 FDAMaxTimingBudgetUs *= 2;
1169 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
1170 TimingGuard = 26600;
1171 else
1172 TimingGuard = 21600;
1173 divisor = 2;
1174 break;
1175
1176 case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
1177 FDAMaxTimingBudgetUs *= 2;
1178 vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
1179 VL53L1_get_tuning_parm(Dev,
1180 VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND,
1181 &vhv_loops);
1182 if (vhv_loops > 0) {
1183 vhv += vhv_loops *
1184 LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
1185 }
1186 TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING +
1187 LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING +
1188 vhv;
1189 divisor = 2;
1190 break;
1191
1192 default:
1193 /* Unsupported mode */
1194 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
1195 }
1196
1197 if (MeasurementTimingBudgetMicroSeconds <= TimingGuard)
1198 Status = VL53L1_ERROR_INVALID_PARAMS;
1199 else {
1200 TimingBudget = (MeasurementTimingBudgetMicroSeconds
1201 - TimingGuard);
1202 }
1203
1204 if (Status == VL53L1_ERROR_NONE) {
1205 if (TimingBudget > FDAMaxTimingBudgetUs)
1206 Status = VL53L1_ERROR_INVALID_PARAMS;
1207 else {
1208 TimingBudget /= divisor;
1209 Status = VL53L1_set_timeouts_us(
1210 Dev,
1211 PhaseCalTimeoutUs,
1212 MmTimeoutUs,
1213 TimingBudget);
1214 }
1215
1216 if (Status == VL53L1_ERROR_NONE)
1217 VL53L1DevDataSet(Dev,
1218 LLData.range_config_timeout_us,
1219 TimingBudget);
1220 }
1221 }
1222 if (Status == VL53L1_ERROR_NONE) {
1223 VL53L1DevDataSet(Dev,
1224 CurrentParameters.MeasurementTimingBudgetMicroSeconds,
1225 MeasurementTimingBudgetMicroSeconds);
1226 }
1227
1228 LOG_FUNCTION_END(Status);
1229 return Status;
1230 }
1231
1232
VL53L1_GetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,uint32_t * pMeasurementTimingBudgetMicroSeconds)1233 VL53L1_Error VL53L1_GetMeasurementTimingBudgetMicroSeconds(VL53L1_DEV Dev,
1234 uint32_t *pMeasurementTimingBudgetMicroSeconds)
1235 {
1236 VL53L1_Error Status = VL53L1_ERROR_NONE;
1237 uint8_t Mm1Enabled = 0;
1238 uint8_t Mm2Enabled = 0;
1239 uint32_t MmTimeoutUs = 0;
1240 uint32_t RangeTimeoutUs = 0;
1241 uint32_t MeasTimingBdg = 0;
1242 uint32_t PhaseCalTimeoutUs = 0;
1243 VL53L1_PresetModes PresetMode;
1244 uint32_t TimingGuard;
1245 uint32_t vhv;
1246 int32_t vhv_loops;
1247
1248 LOG_FUNCTION_START("");
1249
1250 *pMeasurementTimingBudgetMicroSeconds = 0;
1251
1252 if (Status == VL53L1_ERROR_NONE)
1253 Status = VL53L1_GetSequenceStepEnable(Dev,
1254 VL53L1_SEQUENCESTEP_MM1, &Mm1Enabled);
1255
1256 if (Status == VL53L1_ERROR_NONE)
1257 Status = VL53L1_GetSequenceStepEnable(Dev,
1258 VL53L1_SEQUENCESTEP_MM2, &Mm2Enabled);
1259
1260 if (Status == VL53L1_ERROR_NONE)
1261 Status = VL53L1_get_timeouts_us(Dev,
1262 &PhaseCalTimeoutUs,
1263 &MmTimeoutUs,
1264 &RangeTimeoutUs);
1265
1266 if (Status == VL53L1_ERROR_NONE) {
1267 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
1268
1269 switch (PresetMode) {
1270 case VL53L1_PRESETMODE_LITE_RANGING:
1271 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
1272 MeasTimingBdg = RangeTimeoutUs + 5000;
1273 else
1274 MeasTimingBdg = RangeTimeoutUs + 1000;
1275
1276 break;
1277
1278 case VL53L1_PRESETMODE_AUTONOMOUS:
1279 if ((Mm1Enabled == 1) || (Mm2Enabled == 1))
1280 MeasTimingBdg = 2 * RangeTimeoutUs + 26600;
1281 else
1282 MeasTimingBdg = 2 * RangeTimeoutUs + 21600;
1283
1284 break;
1285
1286 case VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS:
1287 vhv = LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
1288 VL53L1_get_tuning_parm(Dev,
1289 VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND,
1290 &vhv_loops);
1291 if (vhv_loops > 0) {
1292 vhv += vhv_loops *
1293 LOWPOWER_AUTO_VHV_LOOP_DURATION_US;
1294 }
1295 TimingGuard = LOWPOWER_AUTO_OVERHEAD_BEFORE_A_RANGING +
1296 LOWPOWER_AUTO_OVERHEAD_BETWEEN_A_B_RANGING +
1297 vhv;
1298 MeasTimingBdg = 2 * RangeTimeoutUs + TimingGuard;
1299 break;
1300
1301 default:
1302 /* Unsupported mode */
1303 Status = VL53L1_ERROR_MODE_NOT_SUPPORTED;
1304 }
1305 }
1306 if (Status == VL53L1_ERROR_NONE)
1307 *pMeasurementTimingBudgetMicroSeconds = MeasTimingBdg;
1308
1309 LOG_FUNCTION_END(Status);
1310 return Status;
1311 }
1312
1313
1314
VL53L1_SetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,uint32_t InterMeasurementPeriodMilliSeconds)1315 VL53L1_Error VL53L1_SetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,
1316 uint32_t InterMeasurementPeriodMilliSeconds)
1317 {
1318 VL53L1_Error Status = VL53L1_ERROR_NONE;
1319 uint32_t adjustedIMP;
1320
1321 LOG_FUNCTION_START("");
1322
1323 /* Fix for Ticket 468205 actual measurement period shorter than set */
1324 adjustedIMP = InterMeasurementPeriodMilliSeconds;
1325 adjustedIMP += (adjustedIMP * 64) / 1000;
1326 /* End of fix for Ticket 468205 */
1327 Status = VL53L1_set_inter_measurement_period_ms(Dev,
1328 adjustedIMP);
1329
1330 LOG_FUNCTION_END(Status);
1331 return Status;
1332 }
1333
VL53L1_GetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,uint32_t * pInterMeasurementPeriodMilliSeconds)1334 VL53L1_Error VL53L1_GetInterMeasurementPeriodMilliSeconds(VL53L1_DEV Dev,
1335 uint32_t *pInterMeasurementPeriodMilliSeconds)
1336 {
1337 VL53L1_Error Status = VL53L1_ERROR_NONE;
1338 uint32_t adjustedIMP;
1339
1340 LOG_FUNCTION_START("");
1341
1342 Status = VL53L1_get_inter_measurement_period_ms(Dev, &adjustedIMP);
1343 /* Fix for Ticket 468205 actual measurement period shorter than set */
1344 adjustedIMP -= (adjustedIMP * 64) / 1000;
1345 *pInterMeasurementPeriodMilliSeconds = adjustedIMP;
1346 /* End of fix for Ticket 468205 */
1347
1348 LOG_FUNCTION_END(Status);
1349 return Status;
1350 }
1351
1352
1353 /* End Group PAL Parameters Functions */
1354
1355
1356 /* Group Limit check Functions */
1357
VL53L1_GetNumberOfLimitCheck(uint16_t * pNumberOfLimitCheck)1358 VL53L1_Error VL53L1_GetNumberOfLimitCheck(uint16_t *pNumberOfLimitCheck)
1359 {
1360 VL53L1_Error Status = VL53L1_ERROR_NONE;
1361
1362 LOG_FUNCTION_START("");
1363
1364 *pNumberOfLimitCheck = VL53L1_CHECKENABLE_NUMBER_OF_CHECKS;
1365
1366 LOG_FUNCTION_END(Status);
1367 return Status;
1368 }
1369
VL53L1_GetLimitCheckInfo(uint16_t LimitCheckId,char * pLimitCheckString)1370 VL53L1_Error VL53L1_GetLimitCheckInfo(uint16_t LimitCheckId,
1371 char *pLimitCheckString)
1372 {
1373 VL53L1_Error Status = VL53L1_ERROR_NONE;
1374
1375 LOG_FUNCTION_START("");
1376
1377 Status = VL53L1_get_limit_check_info(LimitCheckId,
1378 pLimitCheckString);
1379
1380 LOG_FUNCTION_END(Status);
1381 return Status;
1382 }
1383
VL53L1_GetLimitCheckStatus(VL53L1_DEV Dev,uint16_t LimitCheckId,uint8_t * pLimitCheckStatus)1384 VL53L1_Error VL53L1_GetLimitCheckStatus(VL53L1_DEV Dev, uint16_t LimitCheckId,
1385 uint8_t *pLimitCheckStatus)
1386 {
1387 VL53L1_Error Status = VL53L1_ERROR_NONE;
1388 uint8_t Temp8;
1389
1390 LOG_FUNCTION_START("");
1391
1392 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
1393 Status = VL53L1_ERROR_INVALID_PARAMS;
1394 } else {
1395 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
1396 LimitCheckId, Temp8);
1397 *pLimitCheckStatus = Temp8;
1398 }
1399
1400 LOG_FUNCTION_END(Status);
1401 return Status;
1402 }
1403
SetLimitValue(VL53L1_DEV Dev,uint16_t LimitCheckId,FixPoint1616_t value)1404 static VL53L1_Error SetLimitValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
1405 FixPoint1616_t value)
1406 {
1407 VL53L1_Error Status = VL53L1_ERROR_NONE;
1408 uint16_t tmpuint16; /* temporary variable */
1409
1410 LOG_FUNCTION_START("");
1411
1412 switch (LimitCheckId) {
1413 case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
1414 tmpuint16 = VL53L1_FIXPOINT1616TOFIXPOINT142(value);
1415 VL53L1_set_lite_sigma_threshold(Dev, tmpuint16);
1416 break;
1417 case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
1418 tmpuint16 = VL53L1_FIXPOINT1616TOFIXPOINT97(value);
1419 VL53L1_set_lite_min_count_rate(Dev, tmpuint16);
1420 break;
1421 default:
1422 Status = VL53L1_ERROR_INVALID_PARAMS;
1423 }
1424
1425 LOG_FUNCTION_END(Status);
1426 return Status;
1427 }
1428
1429
VL53L1_SetLimitCheckEnable(VL53L1_DEV Dev,uint16_t LimitCheckId,uint8_t LimitCheckEnable)1430 VL53L1_Error VL53L1_SetLimitCheckEnable(VL53L1_DEV Dev, uint16_t LimitCheckId,
1431 uint8_t LimitCheckEnable)
1432 {
1433 VL53L1_Error Status = VL53L1_ERROR_NONE;
1434 FixPoint1616_t TempFix1616 = 0;
1435
1436 LOG_FUNCTION_START("");
1437
1438
1439 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
1440 Status = VL53L1_ERROR_INVALID_PARAMS;
1441 } else {
1442 /* TempFix1616 contains either 0 or the limit value */
1443 if (LimitCheckEnable == 0)
1444 TempFix1616 = 0;
1445 else
1446 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
1447 LimitCheckId, TempFix1616);
1448
1449 Status = SetLimitValue(Dev, LimitCheckId, TempFix1616);
1450 }
1451
1452 if (Status == VL53L1_ERROR_NONE)
1453 VL53L1_SETARRAYPARAMETERFIELD(Dev,
1454 LimitChecksEnable,
1455 LimitCheckId,
1456 ((LimitCheckEnable == 0) ? 0 : 1));
1457
1458
1459
1460 LOG_FUNCTION_END(Status);
1461 return Status;
1462 }
1463
VL53L1_GetLimitCheckEnable(VL53L1_DEV Dev,uint16_t LimitCheckId,uint8_t * pLimitCheckEnable)1464 VL53L1_Error VL53L1_GetLimitCheckEnable(VL53L1_DEV Dev, uint16_t LimitCheckId,
1465 uint8_t *pLimitCheckEnable)
1466 {
1467 VL53L1_Error Status = VL53L1_ERROR_NONE;
1468 uint8_t Temp8;
1469
1470 LOG_FUNCTION_START("");
1471
1472 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
1473 Status = VL53L1_ERROR_INVALID_PARAMS;
1474 *pLimitCheckEnable = 0;
1475 } else {
1476 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
1477 LimitCheckId, Temp8);
1478 *pLimitCheckEnable = Temp8;
1479 }
1480
1481
1482 LOG_FUNCTION_END(Status);
1483 return Status;
1484 }
1485
VL53L1_SetLimitCheckValue(VL53L1_DEV Dev,uint16_t LimitCheckId,FixPoint1616_t LimitCheckValue)1486 VL53L1_Error VL53L1_SetLimitCheckValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
1487 FixPoint1616_t LimitCheckValue)
1488 {
1489 VL53L1_Error Status = VL53L1_ERROR_NONE;
1490 uint8_t LimitChecksEnable;
1491
1492 LOG_FUNCTION_START("");
1493
1494 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
1495 Status = VL53L1_ERROR_INVALID_PARAMS;
1496 } else {
1497
1498 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable,
1499 LimitCheckId,
1500 LimitChecksEnable);
1501
1502 if (LimitChecksEnable == 0) {
1503 /* disabled write only internal value */
1504 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue,
1505 LimitCheckId, LimitCheckValue);
1506 } else {
1507
1508 Status = SetLimitValue(Dev, LimitCheckId,
1509 LimitCheckValue);
1510
1511 if (Status == VL53L1_ERROR_NONE) {
1512 VL53L1_SETARRAYPARAMETERFIELD(Dev,
1513 LimitChecksValue,
1514 LimitCheckId, LimitCheckValue);
1515 }
1516 }
1517 }
1518
1519 LOG_FUNCTION_END(Status);
1520 return Status;
1521 }
1522
VL53L1_GetLimitCheckValue(VL53L1_DEV Dev,uint16_t LimitCheckId,FixPoint1616_t * pLimitCheckValue)1523 VL53L1_Error VL53L1_GetLimitCheckValue(VL53L1_DEV Dev, uint16_t LimitCheckId,
1524 FixPoint1616_t *pLimitCheckValue)
1525 {
1526 VL53L1_Error Status = VL53L1_ERROR_NONE;
1527 uint16_t MinCountRate;
1528 FixPoint1616_t TempFix1616;
1529 uint16_t SigmaThresh;
1530
1531 LOG_FUNCTION_START("");
1532
1533 switch (LimitCheckId) {
1534 case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
1535 Status = VL53L1_get_lite_sigma_threshold(Dev, &SigmaThresh);
1536 TempFix1616 = VL53L1_FIXPOINT142TOFIXPOINT1616(SigmaThresh);
1537 break;
1538 case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
1539 Status = VL53L1_get_lite_min_count_rate(Dev, &MinCountRate);
1540 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(MinCountRate);
1541 break;
1542 default:
1543 Status = VL53L1_ERROR_INVALID_PARAMS;
1544 }
1545
1546 if (Status == VL53L1_ERROR_NONE) {
1547
1548 if (TempFix1616 == 0) {
1549 /* disabled: return value from memory */
1550 VL53L1_GETARRAYPARAMETERFIELD(Dev,
1551 LimitChecksValue, LimitCheckId,
1552 TempFix1616);
1553 *pLimitCheckValue = TempFix1616;
1554 VL53L1_SETARRAYPARAMETERFIELD(Dev,
1555 LimitChecksEnable, LimitCheckId, 0);
1556 } else {
1557 *pLimitCheckValue = TempFix1616;
1558 VL53L1_SETARRAYPARAMETERFIELD(Dev,
1559 LimitChecksValue, LimitCheckId,
1560 TempFix1616);
1561 VL53L1_SETARRAYPARAMETERFIELD(Dev,
1562 LimitChecksEnable, LimitCheckId, 1);
1563 }
1564 }
1565 LOG_FUNCTION_END(Status);
1566 return Status;
1567
1568 }
1569
VL53L1_GetLimitCheckCurrent(VL53L1_DEV Dev,uint16_t LimitCheckId,FixPoint1616_t * pLimitCheckCurrent)1570 VL53L1_Error VL53L1_GetLimitCheckCurrent(VL53L1_DEV Dev, uint16_t LimitCheckId,
1571 FixPoint1616_t *pLimitCheckCurrent)
1572 {
1573 VL53L1_Error Status = VL53L1_ERROR_NONE;
1574 FixPoint1616_t TempFix1616 = 0;
1575
1576 LOG_FUNCTION_START("");
1577
1578 if (LimitCheckId >= VL53L1_CHECKENABLE_NUMBER_OF_CHECKS) {
1579 Status = VL53L1_ERROR_INVALID_PARAMS;
1580 } else {
1581 VL53L1_GETARRAYPARAMETERFIELD(Dev, LimitChecksCurrent,
1582 LimitCheckId, TempFix1616);
1583 *pLimitCheckCurrent = TempFix1616;
1584 }
1585
1586 LOG_FUNCTION_END(Status);
1587 return Status;
1588
1589 }
1590
1591 /* End Group Limit check Functions */
1592
1593
1594
1595 /* Group ROI Functions */
1596
VL53L1_SetUserROI(VL53L1_DEV Dev,VL53L1_UserRoi_t * pRoi)1597 VL53L1_Error VL53L1_SetUserROI(VL53L1_DEV Dev,
1598 VL53L1_UserRoi_t *pRoi)
1599 {
1600 VL53L1_Error Status = VL53L1_ERROR_NONE;
1601 VL53L1_user_zone_t user_zone;
1602
1603 Status = CheckValidRectRoi(*pRoi);
1604 if (Status != VL53L1_ERROR_NONE)
1605 return VL53L1_ERROR_INVALID_PARAMS;
1606
1607 user_zone.x_centre = (pRoi->BotRightX + pRoi->TopLeftX + 1) / 2;
1608 user_zone.y_centre = (pRoi->TopLeftY + pRoi->BotRightY + 1) / 2;
1609 user_zone.width = (pRoi->BotRightX - pRoi->TopLeftX);
1610 user_zone.height = (pRoi->TopLeftY - pRoi->BotRightY);
1611 if ((user_zone.width < 3) || (user_zone.height < 3))
1612 Status = VL53L1_ERROR_INVALID_PARAMS;
1613 else
1614 Status = VL53L1_set_user_zone(Dev, &user_zone);
1615
1616 LOG_FUNCTION_END(Status);
1617 return Status;
1618 }
1619
VL53L1_GetUserROI(VL53L1_DEV Dev,VL53L1_UserRoi_t * pRoi)1620 VL53L1_Error VL53L1_GetUserROI(VL53L1_DEV Dev,
1621 VL53L1_UserRoi_t *pRoi)
1622 {
1623 VL53L1_Error Status = VL53L1_ERROR_NONE;
1624 VL53L1_user_zone_t user_zone;
1625
1626 Status = VL53L1_get_user_zone(Dev, &user_zone);
1627
1628 pRoi->TopLeftX = (2 * user_zone.x_centre - user_zone.width) >> 1;
1629 pRoi->TopLeftY = (2 * user_zone.y_centre + user_zone.height) >> 1;
1630 pRoi->BotRightX = (2 * user_zone.x_centre + user_zone.width) >> 1;
1631 pRoi->BotRightY = (2 * user_zone.y_centre - user_zone.height) >> 1;
1632
1633 LOG_FUNCTION_END(Status);
1634 return Status;
1635 }
1636
1637
1638
1639 /* End Group ROI Functions */
1640
1641
1642 /* Group Sequence Step Functions */
1643
VL53L1_GetNumberOfSequenceSteps(VL53L1_DEV Dev,uint8_t * pNumberOfSequenceSteps)1644 VL53L1_Error VL53L1_GetNumberOfSequenceSteps(VL53L1_DEV Dev,
1645 uint8_t *pNumberOfSequenceSteps)
1646 {
1647 VL53L1_Error Status = VL53L1_ERROR_NONE;
1648
1649 SUPPRESS_UNUSED_WARNING(Dev);
1650
1651 LOG_FUNCTION_START("");
1652
1653 *pNumberOfSequenceSteps = VL53L1_SEQUENCESTEP_NUMBER_OF_ITEMS;
1654
1655 LOG_FUNCTION_END(Status);
1656 return Status;
1657 }
1658
VL53L1_GetSequenceStepsInfo(VL53L1_SequenceStepId SequenceStepId,char * pSequenceStepsString)1659 VL53L1_Error VL53L1_GetSequenceStepsInfo(VL53L1_SequenceStepId SequenceStepId,
1660 char *pSequenceStepsString)
1661 {
1662 VL53L1_Error Status = VL53L1_ERROR_NONE;
1663
1664 LOG_FUNCTION_START("");
1665
1666 Status = VL53L1_get_sequence_steps_info(
1667 SequenceStepId,
1668 pSequenceStepsString);
1669
1670 LOG_FUNCTION_END(Status);
1671
1672 return Status;
1673 }
1674
VL53L1_SetSequenceStepEnable(VL53L1_DEV Dev,VL53L1_SequenceStepId SequenceStepId,uint8_t SequenceStepEnabled)1675 VL53L1_Error VL53L1_SetSequenceStepEnable(VL53L1_DEV Dev,
1676 VL53L1_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled)
1677 {
1678 VL53L1_Error Status = VL53L1_ERROR_NONE;
1679 uint32_t MeasurementTimingBudgetMicroSeconds;
1680
1681 LOG_FUNCTION_START("");
1682
1683 /* the VL53L1_SequenceStepId correspond to the LLD
1684 * VL53L1_DeviceSequenceConfig
1685 */
1686
1687 Status = VL53L1_set_sequence_config_bit(Dev,
1688 (VL53L1_DeviceSequenceConfig)SequenceStepId,
1689 SequenceStepEnabled);
1690
1691 /* Apply New Setting */
1692 if (Status == VL53L1_ERROR_NONE) {
1693
1694 /* Recalculate timing budget */
1695 MeasurementTimingBudgetMicroSeconds = VL53L1DevDataGet(Dev,
1696 CurrentParameters.MeasurementTimingBudgetMicroSeconds);
1697
1698 VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev,
1699 MeasurementTimingBudgetMicroSeconds);
1700 }
1701
1702 LOG_FUNCTION_END(Status);
1703
1704 return Status;
1705 }
1706
1707
VL53L1_GetSequenceStepEnable(VL53L1_DEV Dev,VL53L1_SequenceStepId SequenceStepId,uint8_t * pSequenceStepEnabled)1708 VL53L1_Error VL53L1_GetSequenceStepEnable(VL53L1_DEV Dev,
1709 VL53L1_SequenceStepId SequenceStepId, uint8_t *pSequenceStepEnabled)
1710 {
1711 VL53L1_Error Status = VL53L1_ERROR_NONE;
1712
1713 LOG_FUNCTION_START("");
1714
1715 Status = VL53L1_get_sequence_config_bit(Dev,
1716 (VL53L1_DeviceSequenceConfig)SequenceStepId,
1717 pSequenceStepEnabled);
1718
1719 LOG_FUNCTION_END(Status);
1720 return Status;
1721 }
1722
1723
1724 /* End Group Sequence Step Functions Functions */
1725
1726
1727
1728 /* Group PAL Measurement Functions */
1729
1730
1731
VL53L1_StartMeasurement(VL53L1_DEV Dev)1732 VL53L1_Error VL53L1_StartMeasurement(VL53L1_DEV Dev)
1733 {
1734 #define TIMED_MODE_TIMING_GUARD_MILLISECONDS 4
1735 VL53L1_Error Status = VL53L1_ERROR_NONE;
1736 uint8_t DeviceMeasurementMode;
1737 VL53L1_State CurrPalState;
1738 VL53L1_Error lStatus;
1739 uint32_t MTBus, IMPms;
1740
1741 LOG_FUNCTION_START("");
1742
1743 CurrPalState = VL53L1DevDataGet(Dev, PalState);
1744 switch (CurrPalState) {
1745 case VL53L1_STATE_IDLE:
1746 Status = VL53L1_ERROR_NONE;
1747 break;
1748 case VL53L1_STATE_POWERDOWN:
1749 case VL53L1_STATE_WAIT_STATICINIT:
1750 case VL53L1_STATE_STANDBY:
1751 case VL53L1_STATE_RUNNING:
1752 case VL53L1_STATE_RESET:
1753 case VL53L1_STATE_UNKNOWN:
1754 case VL53L1_STATE_ERROR:
1755 Status = VL53L1_ERROR_INVALID_COMMAND;
1756 break;
1757 default:
1758 Status = VL53L1_ERROR_UNDEFINED;
1759 }
1760
1761 DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
1762
1763 /* Check timing configuration between timing budget and
1764 * inter measurement period */
1765 if ((Status == VL53L1_ERROR_NONE) &&
1766 (DeviceMeasurementMode == VL53L1_DEVICEMEASUREMENTMODE_TIMED)) {
1767 lStatus = VL53L1_GetMeasurementTimingBudgetMicroSeconds(Dev,
1768 &MTBus);
1769 /* convert timing budget in ms */
1770 MTBus /= 1000;
1771 lStatus = VL53L1_GetInterMeasurementPeriodMilliSeconds(Dev,
1772 &IMPms);
1773 /* trick to get rid of compiler "set but not used" warning */
1774 SUPPRESS_UNUSED_WARNING(lStatus);
1775 if (IMPms < MTBus + TIMED_MODE_TIMING_GUARD_MILLISECONDS)
1776 Status = VL53L1_ERROR_INVALID_PARAMS;
1777 }
1778
1779 if (Status == VL53L1_ERROR_NONE)
1780 Status = VL53L1_init_and_start_range(
1781 Dev,
1782 DeviceMeasurementMode,
1783 VL53L1_DEVICECONFIGLEVEL_FULL);
1784
1785 /* Set PAL State to Running */
1786 if (Status == VL53L1_ERROR_NONE)
1787 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_RUNNING);
1788
1789
1790 LOG_FUNCTION_END(Status);
1791 return Status;
1792 }
1793
VL53L1_StopMeasurement(VL53L1_DEV Dev)1794 VL53L1_Error VL53L1_StopMeasurement(VL53L1_DEV Dev)
1795 {
1796 VL53L1_Error Status = VL53L1_ERROR_NONE;
1797
1798 LOG_FUNCTION_START("");
1799
1800 Status = VL53L1_stop_range(Dev);
1801
1802 /* Set PAL State to Idle */
1803 if (Status == VL53L1_ERROR_NONE)
1804 VL53L1DevDataSet(Dev, PalState, VL53L1_STATE_IDLE);
1805
1806 LOG_FUNCTION_END(Status);
1807 return Status;
1808 }
1809
ChangePresetMode(VL53L1_DEV Dev)1810 static VL53L1_Error ChangePresetMode(VL53L1_DEV Dev)
1811 {
1812 VL53L1_Error Status = VL53L1_ERROR_NONE;
1813 VL53L1_PresetModes PresetMode;
1814 VL53L1_DistanceModes NewDistanceMode;
1815 VL53L1_user_zone_t user_zone;
1816 uint32_t TimingBudget;
1817 uint32_t MmTimeoutUs;
1818 uint32_t PhaseCalTimeoutUs;
1819 uint8_t DeviceMeasurementMode;
1820 uint32_t inter_measurement_period_ms;
1821
1822 LOG_FUNCTION_START("");
1823
1824 Status = VL53L1_get_user_zone(Dev, &user_zone);
1825 /* Initialize variables fix ticket EwokP #475395 */
1826 PresetMode = VL53L1DevDataGet(Dev,
1827 CurrentParameters.PresetMode);
1828 NewDistanceMode = VL53L1DevDataGet(Dev,
1829 CurrentParameters.NewDistanceMode);
1830 /* End of Initialize variables fix ticket EwokP #475395 */
1831 if (Status == VL53L1_ERROR_NONE)
1832 Status = VL53L1_get_timeouts_us(Dev, &PhaseCalTimeoutUs,
1833 &MmTimeoutUs, &TimingBudget);
1834
1835 if (Status == VL53L1_ERROR_NONE)
1836 Status = VL53L1_stop_range(Dev);
1837
1838 if (Status == VL53L1_ERROR_NONE)
1839 Status = VL53L1_WaitUs(Dev, 500);
1840
1841 if (Status == VL53L1_ERROR_NONE) {
1842 inter_measurement_period_ms = VL53L1DevDataGet(Dev,
1843 LLData.inter_measurement_period_ms);
1844
1845 Status = SetPresetMode(Dev,
1846 PresetMode,
1847 NewDistanceMode,
1848 inter_measurement_period_ms);
1849 }
1850
1851 if (Status == VL53L1_ERROR_NONE) {
1852 Status = VL53L1_set_timeouts_us(Dev, PhaseCalTimeoutUs,
1853 MmTimeoutUs, TimingBudget);
1854
1855 if (Status == VL53L1_ERROR_NONE)
1856 VL53L1DevDataSet(Dev, LLData.range_config_timeout_us,
1857 TimingBudget);
1858 }
1859
1860 if (Status == VL53L1_ERROR_NONE)
1861 Status = VL53L1_set_user_zone(Dev, &user_zone);
1862
1863 if (Status == VL53L1_ERROR_NONE) {
1864 DeviceMeasurementMode = VL53L1DevDataGet(Dev,
1865 LLData.measurement_mode);
1866
1867 Status = VL53L1_init_and_start_range(
1868 Dev,
1869 DeviceMeasurementMode,
1870 VL53L1_DEVICECONFIGLEVEL_FULL);
1871 }
1872
1873 if (Status == VL53L1_ERROR_NONE)
1874 VL53L1DevDataSet(Dev,
1875 CurrentParameters.InternalDistanceMode,
1876 NewDistanceMode);
1877
1878 LOG_FUNCTION_END(Status);
1879 return Status;
1880 }
1881
1882
VL53L1_ClearInterruptAndStartMeasurement(VL53L1_DEV Dev)1883 VL53L1_Error VL53L1_ClearInterruptAndStartMeasurement(VL53L1_DEV Dev)
1884 {
1885 VL53L1_Error Status = VL53L1_ERROR_NONE;
1886 uint8_t DeviceMeasurementMode;
1887 VL53L1_DistanceModes InternalDistanceMode;
1888 VL53L1_DistanceModes NewDistanceMode;
1889
1890 LOG_FUNCTION_START("");
1891
1892 DeviceMeasurementMode = VL53L1DevDataGet(Dev, LLData.measurement_mode);
1893 InternalDistanceMode = VL53L1DevDataGet(Dev,
1894 CurrentParameters.InternalDistanceMode);
1895 NewDistanceMode = VL53L1DevDataGet(Dev,
1896 CurrentParameters.NewDistanceMode);
1897
1898 if (NewDistanceMode != InternalDistanceMode)
1899 Status = ChangePresetMode(Dev);
1900 else
1901 Status = VL53L1_clear_interrupt_and_enable_next_range(
1902 Dev,
1903 DeviceMeasurementMode);
1904
1905 LOG_FUNCTION_END(Status);
1906 return Status;
1907 }
1908
1909
VL53L1_GetMeasurementDataReady(VL53L1_DEV Dev,uint8_t * pMeasurementDataReady)1910 VL53L1_Error VL53L1_GetMeasurementDataReady(VL53L1_DEV Dev,
1911 uint8_t *pMeasurementDataReady)
1912 {
1913 VL53L1_Error Status = VL53L1_ERROR_NONE;
1914
1915 LOG_FUNCTION_START("");
1916
1917 Status = VL53L1_is_new_data_ready(Dev, pMeasurementDataReady);
1918
1919 LOG_FUNCTION_END(Status);
1920 return Status;
1921 }
1922
VL53L1_WaitMeasurementDataReady(VL53L1_DEV Dev)1923 VL53L1_Error VL53L1_WaitMeasurementDataReady(VL53L1_DEV Dev)
1924 {
1925 VL53L1_Error Status = VL53L1_ERROR_NONE;
1926
1927 LOG_FUNCTION_START("");
1928
1929 /* Note that the timeout is given by:
1930 * VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS defined in def.h
1931 */
1932
1933 Status = VL53L1_poll_for_range_completion(Dev,
1934 VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS);
1935
1936 LOG_FUNCTION_END(Status);
1937 return Status;
1938 }
1939
1940
1941
ComputeRQL(uint8_t active_results,uint8_t FilteredRangeStatus,VL53L1_range_data_t * presults_data)1942 static uint8_t ComputeRQL(uint8_t active_results,
1943 uint8_t FilteredRangeStatus,
1944 VL53L1_range_data_t *presults_data)
1945 {
1946 int16_t SRL = 300;
1947 uint16_t SRAS = 30;
1948 FixPoint1616_t RAS;
1949 FixPoint1616_t SRQL;
1950 FixPoint1616_t GI = 7713587; /* 117.7 * 65536 */
1951 FixPoint1616_t GGm = 3198157; /* 48.8 * 65536 */
1952 FixPoint1616_t LRAP = 6554; /* 0.1 * 65536 */
1953 FixPoint1616_t partial;
1954 uint8_t finalvalue;
1955 uint8_t returnvalue;
1956
1957 if (active_results == 0)
1958 returnvalue = 0;
1959 else if (FilteredRangeStatus == VL53L1_DEVICEERROR_PHASECONSISTENCY)
1960 returnvalue = 50;
1961 else {
1962 if (presults_data->median_range_mm < SRL)
1963 RAS = SRAS * 65536;
1964 else
1965 RAS = LRAP * presults_data->median_range_mm;
1966
1967 /* Fix1616 + (fix1616 * uint16_t / fix1616) * 65536 = fix1616 */
1968 if (RAS != 0) {
1969 partial = (GGm * presults_data->sigma_mm);
1970 partial = partial + (RAS >> 1);
1971 partial = partial / RAS;
1972 partial = partial * 65536;
1973 if (partial <= GI)
1974 SRQL = GI - partial;
1975 else
1976 SRQL = 50 * 65536;
1977 } else
1978 SRQL = 100 * 65536;
1979
1980 finalvalue = (uint8_t)(SRQL >> 16);
1981 returnvalue = MAX(50, MIN(100, finalvalue));
1982 }
1983
1984 return returnvalue;
1985 }
1986
1987
ConvertStatusLite(uint8_t FilteredRangeStatus)1988 static uint8_t ConvertStatusLite(uint8_t FilteredRangeStatus)
1989 {
1990 uint8_t RangeStatus;
1991
1992 switch (FilteredRangeStatus) {
1993 case VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY:
1994 RangeStatus = VL53L1_RANGESTATUS_SYNCRONISATION_INT;
1995 break;
1996 case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
1997 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
1998 break;
1999 case VL53L1_DEVICEERROR_RANGEPHASECHECK:
2000 RangeStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
2001 break;
2002 case VL53L1_DEVICEERROR_MSRCNOTARGET:
2003 RangeStatus = VL53L1_RANGESTATUS_SIGNAL_FAIL;
2004 break;
2005 case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
2006 RangeStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
2007 break;
2008 case VL53L1_DEVICEERROR_PHASECONSISTENCY:
2009 RangeStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
2010 break;
2011 case VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD:
2012 RangeStatus = VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL;
2013 break;
2014 case VL53L1_DEVICEERROR_MINCLIP:
2015 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED;
2016 break;
2017 case VL53L1_DEVICEERROR_RANGECOMPLETE:
2018 RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
2019 break;
2020 default:
2021 RangeStatus = VL53L1_RANGESTATUS_NONE;
2022 }
2023
2024 return RangeStatus;
2025 }
2026
2027
2028
SetSimpleData(VL53L1_DEV Dev,uint8_t active_results,uint8_t device_status,VL53L1_range_data_t * presults_data,VL53L1_RangingMeasurementData_t * pRangeData)2029 static VL53L1_Error SetSimpleData(VL53L1_DEV Dev,
2030 uint8_t active_results, uint8_t device_status,
2031 VL53L1_range_data_t *presults_data,
2032 VL53L1_RangingMeasurementData_t *pRangeData)
2033 {
2034 VL53L1_Error Status = VL53L1_ERROR_NONE;
2035 uint8_t FilteredRangeStatus;
2036 uint8_t SigmaLimitflag;
2037 uint8_t SignalLimitflag;
2038 uint8_t Temp8Enable;
2039 uint8_t Temp8;
2040 FixPoint1616_t AmbientRate;
2041 FixPoint1616_t SignalRate;
2042 FixPoint1616_t TempFix1616;
2043 FixPoint1616_t LimitCheckValue;
2044 int16_t Range;
2045
2046 pRangeData->TimeStamp = presults_data->time_stamp;
2047
2048 FilteredRangeStatus = presults_data->range_status & 0x1F;
2049
2050 pRangeData->RangeQualityLevel = ComputeRQL(active_results,
2051 FilteredRangeStatus,
2052 presults_data);
2053
2054 SignalRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
2055 presults_data->peak_signal_count_rate_mcps);
2056 pRangeData->SignalRateRtnMegaCps
2057 = SignalRate;
2058
2059 AmbientRate = VL53L1_FIXPOINT97TOFIXPOINT1616(
2060 presults_data->ambient_count_rate_mcps);
2061 pRangeData->AmbientRateRtnMegaCps = AmbientRate;
2062
2063 pRangeData->EffectiveSpadRtnCount =
2064 presults_data->actual_effective_spads;
2065
2066 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
2067 presults_data->sigma_mm);
2068
2069 pRangeData->SigmaMilliMeter = TempFix1616;
2070
2071 pRangeData->RangeMilliMeter = presults_data->median_range_mm;
2072
2073 pRangeData->RangeFractionalPart = 0;
2074
2075 /* Treat device error status first */
2076 switch (device_status) {
2077 case VL53L1_DEVICEERROR_MULTCLIPFAIL:
2078 case VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
2079 case VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
2080 case VL53L1_DEVICEERROR_NOVHVVALUEFOUND:
2081 pRangeData->RangeStatus = VL53L1_RANGESTATUS_HARDWARE_FAIL;
2082 break;
2083 case VL53L1_DEVICEERROR_USERROICLIP:
2084 pRangeData->RangeStatus = VL53L1_RANGESTATUS_MIN_RANGE_FAIL;
2085 break;
2086 default:
2087 pRangeData->RangeStatus = VL53L1_RANGESTATUS_RANGE_VALID;
2088 }
2089
2090 /* Now deal with range status according to the ranging preset */
2091 if (pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) {
2092 pRangeData->RangeStatus =
2093 ConvertStatusLite(FilteredRangeStatus);
2094 }
2095
2096 /* Update current Limit Check */
2097 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
2098 presults_data->sigma_mm);
2099 VL53L1_SETARRAYPARAMETERFIELD(Dev,
2100 LimitChecksCurrent, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
2101 TempFix1616);
2102
2103 TempFix1616 = VL53L1_FIXPOINT97TOFIXPOINT1616(
2104 presults_data->peak_signal_count_rate_mcps);
2105 VL53L1_SETARRAYPARAMETERFIELD(Dev,
2106 LimitChecksCurrent, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
2107 TempFix1616);
2108
2109 /* Update Limit Check Status */
2110 /* Sigma */
2111 VL53L1_GetLimitCheckValue(Dev,
2112 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
2113 &LimitCheckValue);
2114
2115 SigmaLimitflag = (FilteredRangeStatus ==
2116 VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK)
2117 ? 1 : 0;
2118
2119 VL53L1_GetLimitCheckEnable(Dev,
2120 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE,
2121 &Temp8Enable);
2122
2123 Temp8 = ((Temp8Enable == 1) && (SigmaLimitflag == 1)) ? 1 : 0;
2124 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
2125 VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
2126
2127 /* Signal Rate */
2128 VL53L1_GetLimitCheckValue(Dev,
2129 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
2130 &LimitCheckValue);
2131
2132 SignalLimitflag = (FilteredRangeStatus ==
2133 VL53L1_DEVICEERROR_MSRCNOTARGET)
2134 ? 1 : 0;
2135
2136 VL53L1_GetLimitCheckEnable(Dev,
2137 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
2138 &Temp8Enable);
2139
2140 Temp8 = ((Temp8Enable == 1) && (SignalLimitflag == 1)) ? 1 : 0;
2141 VL53L1_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
2142 VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, Temp8);
2143
2144 Range = pRangeData->RangeMilliMeter;
2145 if ((pRangeData->RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) &&
2146 (Range < 0)) {
2147 if (Range < BDTable[VL53L1_TUNING_PROXY_MIN])
2148 pRangeData->RangeStatus =
2149 VL53L1_RANGESTATUS_RANGE_INVALID;
2150 else
2151 pRangeData->RangeMilliMeter = 0;
2152 }
2153
2154 return Status;
2155 }
2156
2157
2158
VL53L1_GetRangingMeasurementData(VL53L1_DEV Dev,VL53L1_RangingMeasurementData_t * pRangingMeasurementData)2159 VL53L1_Error VL53L1_GetRangingMeasurementData(VL53L1_DEV Dev,
2160 VL53L1_RangingMeasurementData_t *pRangingMeasurementData)
2161 {
2162 VL53L1_Error Status = VL53L1_ERROR_NONE;
2163 VL53L1_range_results_t results;
2164 VL53L1_range_results_t *presults = &results;
2165 VL53L1_range_data_t *presults_data;
2166
2167 LOG_FUNCTION_START("");
2168
2169
2170 /* Clear Ranging Data */
2171 memset(pRangingMeasurementData, 0xFF,
2172 sizeof(VL53L1_RangingMeasurementData_t));
2173
2174 /* Get Ranging Data */
2175 Status = VL53L1_get_device_results(
2176 Dev,
2177 VL53L1_DEVICERESULTSLEVEL_FULL,
2178 presults);
2179
2180 if (Status == VL53L1_ERROR_NONE) {
2181 pRangingMeasurementData->StreamCount = presults->stream_count;
2182
2183 /* in case of lite ranging or autonomous the following function
2184 * returns index = 0
2185 */
2186 presults_data = &(presults->data[0]);
2187 Status = SetSimpleData(Dev, 1,
2188 presults->device_status,
2189 presults_data,
2190 pRangingMeasurementData);
2191 }
2192
2193 LOG_FUNCTION_END(Status);
2194 return Status;
2195 }
2196
2197
2198
2199
2200
2201 /* End Group PAL Measurement Functions */
2202
2203
2204 /* Group Calibration functions */
VL53L1_SetTuningParameter(VL53L1_DEV Dev,uint16_t TuningParameterId,int32_t TuningParameterValue)2205 VL53L1_Error VL53L1_SetTuningParameter(VL53L1_DEV Dev,
2206 uint16_t TuningParameterId, int32_t TuningParameterValue)
2207 {
2208 VL53L1_Error Status = VL53L1_ERROR_NONE;
2209
2210 LOG_FUNCTION_START("");
2211 if (TuningParameterId >= 32768)
2212 Status = VL53L1_set_tuning_parm(Dev,
2213 TuningParameterId,
2214 TuningParameterValue);
2215 else {
2216 if (TuningParameterId < VL53L1_TUNING_MAX_TUNABLE_KEY)
2217 BDTable[TuningParameterId] = TuningParameterValue;
2218 else
2219 Status = VL53L1_ERROR_INVALID_PARAMS;
2220 }
2221
2222 LOG_FUNCTION_END(Status);
2223 return Status;
2224 }
2225
VL53L1_GetTuningParameter(VL53L1_DEV Dev,uint16_t TuningParameterId,int32_t * pTuningParameterValue)2226 VL53L1_Error VL53L1_GetTuningParameter(VL53L1_DEV Dev,
2227 uint16_t TuningParameterId, int32_t *pTuningParameterValue)
2228 {
2229 VL53L1_Error Status = VL53L1_ERROR_NONE;
2230
2231 LOG_FUNCTION_START("");
2232
2233 if (TuningParameterId >= 32768)
2234 Status = VL53L1_get_tuning_parm(Dev,
2235 TuningParameterId,
2236 pTuningParameterValue);
2237 else {
2238 if (TuningParameterId < VL53L1_TUNING_MAX_TUNABLE_KEY)
2239 *pTuningParameterValue = BDTable[TuningParameterId];
2240 else
2241 Status = VL53L1_ERROR_INVALID_PARAMS;
2242 }
2243
2244 LOG_FUNCTION_END(Status);
2245 return Status;
2246 }
2247
2248
VL53L1_PerformRefSpadManagement(VL53L1_DEV Dev)2249 VL53L1_Error VL53L1_PerformRefSpadManagement(VL53L1_DEV Dev)
2250 {
2251 #ifdef VL53L1_NOCALIB
2252 VL53L1_Error Status = VL53L1_ERROR_NOT_SUPPORTED;
2253
2254 SUPPRESS_UNUSED_WARNING(Dev);
2255
2256 LOG_FUNCTION_START("");
2257 #else
2258 VL53L1_Error Status = VL53L1_ERROR_NONE;
2259 VL53L1_Error RawStatus;
2260 uint8_t dcrbuffer[24];
2261 uint8_t *comms_buffer;
2262 uint8_t numloc[2] = {5,3};
2263 VL53L1_LLDriverData_t *pdev;
2264 VL53L1_customer_nvm_managed_t *pc;
2265 VL53L1_PresetModes PresetMode;
2266
2267 LOG_FUNCTION_START("");
2268
2269 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
2270 pc = &pdev->customer;
2271
2272 if (Status == VL53L1_ERROR_NONE)
2273 {
2274 PresetMode = VL53L1DevDataGet(Dev, CurrentParameters.PresetMode);
2275 Status = VL53L1_run_ref_spad_char(Dev, &RawStatus);
2276 /* We discovered RefSpad mngt badly breaks some preset mode
2277 * The WA is to apply again the current one
2278 */
2279 if (Status == VL53L1_ERROR_NONE)
2280 Status = VL53L1_SetPresetMode(Dev, PresetMode);
2281 }
2282
2283 if (Status == VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH) {
2284 /* Fix ticket #466282 RefSpad management error/warning -29
2285 * force usage of location 3 and 5 refspads in registers
2286 */
2287 Status = VL53L1_read_nvm_raw_data(Dev,
2288 (uint8_t)(0xA0 >> 2),
2289 (uint8_t)(24 >> 2),
2290 dcrbuffer);
2291
2292 if (Status == VL53L1_ERROR_NONE)
2293 Status = VL53L1_WriteMulti( Dev,
2294 VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
2295 numloc, 2);
2296
2297 if (Status == VL53L1_ERROR_NONE) {
2298 pc->ref_spad_man__num_requested_ref_spads = numloc[0];
2299 pc->ref_spad_man__ref_location = numloc[1];
2300 }
2301
2302 if (Status == VL53L1_ERROR_NONE)
2303 comms_buffer = &dcrbuffer[16];
2304
2305 /*
2306 * update & copy reference SPAD enables to customer nvm managed
2307 */
2308
2309 if (Status == VL53L1_ERROR_NONE)
2310 Status = VL53L1_WriteMulti(Dev,
2311 VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
2312 comms_buffer, 6);
2313
2314 if (Status == VL53L1_ERROR_NONE) {
2315 pc->global_config__spad_enables_ref_0 = comms_buffer[0];
2316 pc->global_config__spad_enables_ref_1 = comms_buffer[1];
2317 pc->global_config__spad_enables_ref_2 = comms_buffer[2];
2318 pc->global_config__spad_enables_ref_3 = comms_buffer[3];
2319 pc->global_config__spad_enables_ref_4 = comms_buffer[4];
2320 pc->global_config__spad_enables_ref_5 = comms_buffer[5];
2321 }
2322 /* End of fix ticket #466282 */
2323 }
2324
2325 #endif
2326
2327 LOG_FUNCTION_END(Status);
2328 return Status;
2329 }
2330
2331
VL53L1_SetXTalkCompensationEnable(VL53L1_DEV Dev,uint8_t XTalkCompensationEnable)2332 VL53L1_Error VL53L1_SetXTalkCompensationEnable(VL53L1_DEV Dev,
2333 uint8_t XTalkCompensationEnable)
2334 {
2335 VL53L1_Error Status = VL53L1_ERROR_NONE;
2336
2337 LOG_FUNCTION_START("");
2338
2339 if (XTalkCompensationEnable == 0)
2340 Status = VL53L1_disable_xtalk_compensation(Dev);
2341 else
2342 Status = VL53L1_enable_xtalk_compensation(Dev);
2343
2344 LOG_FUNCTION_END(Status);
2345 return Status;
2346 }
2347
2348
VL53L1_GetXTalkCompensationEnable(VL53L1_DEV Dev,uint8_t * pXTalkCompensationEnable)2349 VL53L1_Error VL53L1_GetXTalkCompensationEnable(VL53L1_DEV Dev,
2350 uint8_t *pXTalkCompensationEnable)
2351 {
2352 VL53L1_Error Status = VL53L1_ERROR_NONE;
2353
2354 LOG_FUNCTION_START("");
2355
2356 VL53L1_get_xtalk_compensation_enable(
2357 Dev,
2358 pXTalkCompensationEnable);
2359
2360 LOG_FUNCTION_END(Status);
2361 return Status;
2362 }
2363
VL53L1_PerformSingleTargetXTalkCalibration(VL53L1_DEV Dev,int32_t CalDistanceMilliMeter)2364 VL53L1_Error VL53L1_PerformSingleTargetXTalkCalibration(VL53L1_DEV Dev,
2365 int32_t CalDistanceMilliMeter)
2366 {
2367 VL53L1_Error Status = VL53L1_ERROR_NONE;
2368
2369 LOG_FUNCTION_START("");
2370
2371 if (CalDistanceMilliMeter > 0) {
2372 BDTable[VL53L1_TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM] =
2373 CalDistanceMilliMeter;
2374 Status = SingleTargetXTalkCalibration(Dev);
2375 } else
2376 Status = VL53L1_ERROR_INVALID_PARAMS;
2377
2378 LOG_FUNCTION_END(Status);
2379 return Status;
2380 }
2381
2382
VL53L1_SetOffsetCalibrationMode(VL53L1_DEV Dev,VL53L1_OffsetCalibrationModes OffsetCalibrationMode)2383 VL53L1_Error VL53L1_SetOffsetCalibrationMode(VL53L1_DEV Dev,
2384 VL53L1_OffsetCalibrationModes OffsetCalibrationMode)
2385 {
2386 VL53L1_Error Status = VL53L1_ERROR_NONE;
2387 VL53L1_OffsetCalibrationMode offset_cal_mode;
2388
2389 LOG_FUNCTION_START("");
2390
2391 if (OffsetCalibrationMode == VL53L1_OFFSETCALIBRATIONMODE_STANDARD) {
2392 offset_cal_mode = VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
2393 } else if (OffsetCalibrationMode ==
2394 VL53L1_OFFSETCALIBRATIONMODE_PRERANGE_ONLY) {
2395 offset_cal_mode =
2396 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY;
2397 } else {
2398 Status = VL53L1_ERROR_INVALID_PARAMS;
2399 }
2400
2401 if (Status == VL53L1_ERROR_NONE)
2402 Status = VL53L1_set_offset_calibration_mode(Dev,
2403 offset_cal_mode);
2404
2405 LOG_FUNCTION_END(Status);
2406 return Status;
2407 }
2408
2409
2410 #ifdef OFFSET_CALIB
VL53L1_PerformOffsetCalibration(VL53L1_DEV Dev,int32_t CalDistanceMilliMeter)2411 VL53L1_Error VL53L1_PerformOffsetCalibration(VL53L1_DEV Dev,
2412 int32_t CalDistanceMilliMeter)
2413 {
2414 VL53L1_Error Status = VL53L1_ERROR_NONE;
2415 VL53L1_Error UnfilteredStatus;
2416 VL53L1_OffsetCalibrationMode offset_cal_mode;
2417
2418 LOG_FUNCTION_START("");
2419
2420 if (Status == VL53L1_ERROR_NONE)
2421 Status = VL53L1_get_offset_calibration_mode(Dev,
2422 &offset_cal_mode);
2423
2424 if (Status != VL53L1_ERROR_NONE) {
2425 LOG_FUNCTION_END(Status);
2426 return Status;
2427 }
2428
2429 if ((offset_cal_mode ==
2430 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD) ||
2431 (offset_cal_mode ==
2432 VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY
2433 )) {
2434 if (Status == VL53L1_ERROR_NONE)
2435 Status = VL53L1_run_offset_calibration(
2436 Dev,
2437 (int16_t)CalDistanceMilliMeter,
2438 &UnfilteredStatus);
2439 } else {
2440 Status = VL53L1_ERROR_INVALID_PARAMS;
2441 }
2442 LOG_FUNCTION_END(Status);
2443 return Status;
2444 }
2445 #endif
2446 #ifdef OFFSET_CALIB_EMPTY
VL53L1_PerformOffsetCalibration(VL53L1_DEV Dev,int32_t CalDistanceMilliMeter)2447 VL53L1_Error VL53L1_PerformOffsetCalibration(VL53L1_DEV Dev,
2448 int32_t CalDistanceMilliMeter)
2449 {
2450 VL53L1_Error Status = VL53L1_ERROR_NOT_SUPPORTED;
2451 SUPPRESS_UNUSED_WARNING(Dev);
2452 SUPPRESS_UNUSED_WARNING(CalDistanceMilliMeter);
2453 return Status;
2454 }
2455 #endif
2456
VL53L1_PerformOffsetSimpleCalibration(VL53L1_DEV Dev,int32_t CalDistanceMilliMeter)2457 VL53L1_Error VL53L1_PerformOffsetSimpleCalibration(VL53L1_DEV Dev,
2458 int32_t CalDistanceMilliMeter)
2459 {
2460 VL53L1_Error Status = VL53L1_ERROR_NONE;
2461 int32_t sum_ranging;
2462 uint8_t offset_meas;
2463 int16_t Max, UnderMax, OverMax, Repeat;
2464 int32_t total_count, inloopcount;
2465 int32_t IncRounding;
2466 int16_t meanDistance_mm;
2467 int16_t offset;
2468 VL53L1_RangingMeasurementData_t RangingMeasurementData;
2469 VL53L1_LLDriverData_t *pdev;
2470 uint8_t goodmeas;
2471
2472 LOG_FUNCTION_START("");
2473
2474 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
2475 /* Disable any offsets */
2476 pdev->customer.algo__part_to_part_range_offset_mm = 0;
2477 pdev->customer.mm_config__inner_offset_mm = 0;
2478 pdev->customer.mm_config__outer_offset_mm = 0;
2479
2480 Repeat=BDTable[VL53L1_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT];
2481 Max=BDTable[VL53L1_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER];
2482 UnderMax = 1 + (Max / 2);
2483 OverMax = Max + (Max / 2);
2484 sum_ranging = 0;
2485 total_count = 0;
2486
2487 while ((Repeat > 0) && (Status == VL53L1_ERROR_NONE)) {
2488 Status = VL53L1_StartMeasurement(Dev);
2489 /* Very first ranging completion interrupt must be ignored */
2490 if (Status == VL53L1_ERROR_NONE)
2491 Status = VL53L1_WaitMeasurementDataReady(Dev);
2492 if (Status == VL53L1_ERROR_NONE)
2493 Status = VL53L1_GetRangingMeasurementData(Dev,
2494 &RangingMeasurementData);
2495 if (Status == VL53L1_ERROR_NONE)
2496 Status = VL53L1_ClearInterruptAndStartMeasurement(Dev);
2497 /* offset calibration main loop */
2498 inloopcount = 0;
2499 offset_meas = 0;
2500 while ((Status == VL53L1_ERROR_NONE) && (inloopcount < Max) &&
2501 (offset_meas < OverMax)) {
2502 Status = VL53L1_WaitMeasurementDataReady(Dev);
2503 if (Status == VL53L1_ERROR_NONE)
2504 Status = VL53L1_GetRangingMeasurementData(Dev,
2505 &RangingMeasurementData);
2506 goodmeas = (RangingMeasurementData.RangeStatus ==
2507 VL53L1_RANGESTATUS_RANGE_VALID);
2508 if ((Status == VL53L1_ERROR_NONE) && goodmeas) {
2509 sum_ranging = sum_ranging +
2510 RangingMeasurementData.RangeMilliMeter;
2511 inloopcount++;
2512 }
2513 if (Status == VL53L1_ERROR_NONE) {
2514 Status = VL53L1_ClearInterruptAndStartMeasurement(
2515 Dev);
2516 }
2517 offset_meas++;
2518 }
2519 total_count += inloopcount;
2520
2521 /* no enough valid values found */
2522 if (inloopcount < UnderMax) {
2523 Status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
2524 }
2525
2526 VL53L1_StopMeasurement(Dev);
2527
2528 Repeat--;
2529
2530 }
2531 /* check overflow (unlikely if target is near to the device) */
2532 if ((sum_ranging < 0) ||
2533 (sum_ranging > ((int32_t) total_count * 0xffff))) {
2534 Status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
2535 }
2536
2537 if ((Status == VL53L1_ERROR_NONE) && (total_count > 0)) {
2538 IncRounding = total_count / 2;
2539 meanDistance_mm = (int16_t)((sum_ranging + IncRounding)
2540 / total_count);
2541 offset = (int16_t)CalDistanceMilliMeter - meanDistance_mm;
2542 pdev->customer.algo__part_to_part_range_offset_mm = 0;
2543 pdev->customer.mm_config__inner_offset_mm = offset;
2544 pdev->customer.mm_config__outer_offset_mm = offset;
2545
2546 Status = VL53L1_set_customer_nvm_managed(Dev,
2547 &(pdev->customer));
2548 }
2549
2550 LOG_FUNCTION_END(Status);
2551 return Status;
2552 }
2553
VL53L1_SetCalibrationData(VL53L1_DEV Dev,VL53L1_CalibrationData_t * pCalibrationData)2554 VL53L1_Error VL53L1_SetCalibrationData(VL53L1_DEV Dev,
2555 VL53L1_CalibrationData_t *pCalibrationData)
2556 {
2557 VL53L1_Error Status = VL53L1_ERROR_NONE;
2558 VL53L1_CustomerNvmManaged_t *pC;
2559 VL53L1_calibration_data_t cal_data;
2560 uint32_t x;
2561
2562 LOG_FUNCTION_START("");
2563
2564 cal_data.struct_version = pCalibrationData->struct_version -
2565 VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
2566
2567
2568
2569 /* memcpy(DEST, SRC, N) */
2570 memcpy(
2571 &(cal_data.add_off_cal_data),
2572 &(pCalibrationData->add_off_cal_data),
2573 sizeof(VL53L1_additional_offset_cal_data_t));
2574
2575 /* memcpy (DEST, SRC, N) */
2576 memcpy(
2577 &(cal_data.optical_centre),
2578 &(pCalibrationData->optical_centre),
2579 sizeof(VL53L1_optical_centre_t));
2580
2581 /* memcpy (DEST, SRC, N) */
2582 memcpy(
2583 &(cal_data.gain_cal),
2584 &(pCalibrationData->gain_cal),
2585 sizeof(VL53L1_gain_calibration_data_t));
2586
2587 /* memcpy (DEST, SRC, N) */
2588 memcpy(
2589 &(cal_data.cal_peak_rate_map),
2590 &(pCalibrationData->cal_peak_rate_map),
2591 sizeof(VL53L1_cal_peak_rate_map_t));
2592
2593 pC = &pCalibrationData->customer;
2594 x = pC->algo__crosstalk_compensation_plane_offset_kcps;
2595 cal_data.customer.algo__crosstalk_compensation_plane_offset_kcps =
2596 (uint16_t)(x&0x0000FFFF);
2597
2598 cal_data.customer.global_config__spad_enables_ref_0 =
2599 pC->global_config__spad_enables_ref_0;
2600 cal_data.customer.global_config__spad_enables_ref_1 =
2601 pC->global_config__spad_enables_ref_1;
2602 cal_data.customer.global_config__spad_enables_ref_2 =
2603 pC->global_config__spad_enables_ref_2;
2604 cal_data.customer.global_config__spad_enables_ref_3 =
2605 pC->global_config__spad_enables_ref_3;
2606 cal_data.customer.global_config__spad_enables_ref_4 =
2607 pC->global_config__spad_enables_ref_4;
2608 cal_data.customer.global_config__spad_enables_ref_5 =
2609 pC->global_config__spad_enables_ref_5;
2610 cal_data.customer.global_config__ref_en_start_select =
2611 pC->global_config__ref_en_start_select;
2612 cal_data.customer.ref_spad_man__num_requested_ref_spads =
2613 pC->ref_spad_man__num_requested_ref_spads;
2614 cal_data.customer.ref_spad_man__ref_location =
2615 pC->ref_spad_man__ref_location;
2616 cal_data.customer.algo__crosstalk_compensation_x_plane_gradient_kcps =
2617 pC->algo__crosstalk_compensation_x_plane_gradient_kcps;
2618 cal_data.customer.algo__crosstalk_compensation_y_plane_gradient_kcps =
2619 pC->algo__crosstalk_compensation_y_plane_gradient_kcps;
2620 cal_data.customer.ref_spad_char__total_rate_target_mcps =
2621 pC->ref_spad_char__total_rate_target_mcps;
2622 cal_data.customer.algo__part_to_part_range_offset_mm =
2623 pC->algo__part_to_part_range_offset_mm;
2624 cal_data.customer.mm_config__inner_offset_mm =
2625 pC->mm_config__inner_offset_mm;
2626 cal_data.customer.mm_config__outer_offset_mm =
2627 pC->mm_config__outer_offset_mm;
2628
2629 Status = VL53L1_set_part_to_part_data(Dev, &cal_data);
2630 LOG_FUNCTION_END(Status);
2631 return Status;
2632
2633 }
2634
VL53L1_GetCalibrationData(VL53L1_DEV Dev,VL53L1_CalibrationData_t * pCalibrationData)2635 VL53L1_Error VL53L1_GetCalibrationData(VL53L1_DEV Dev,
2636 VL53L1_CalibrationData_t *pCalibrationData)
2637 {
2638 VL53L1_Error Status = VL53L1_ERROR_NONE;
2639 VL53L1_calibration_data_t cal_data;
2640 VL53L1_CustomerNvmManaged_t *pC;
2641 VL53L1_customer_nvm_managed_t *pC2;
2642
2643 LOG_FUNCTION_START("");
2644
2645 /* struct_version is filled inside get part to part function */
2646 Status = VL53L1_get_part_to_part_data(Dev, &cal_data);
2647
2648 pCalibrationData->struct_version = cal_data.struct_version +
2649 VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION;
2650
2651
2652 /* memcpy(DEST, SRC, N) */
2653 memcpy(
2654 &(pCalibrationData->add_off_cal_data),
2655 &(cal_data.add_off_cal_data),
2656 sizeof(VL53L1_additional_offset_cal_data_t));
2657
2658 /* memcpy (DEST, SRC, N) */
2659 memcpy(
2660 &(pCalibrationData->optical_centre),
2661 &(cal_data.optical_centre),
2662 sizeof(VL53L1_optical_centre_t));
2663
2664 /* memcpy (DEST, SRC, N) */
2665 memcpy(
2666 &(pCalibrationData->gain_cal),
2667 &(cal_data.gain_cal),
2668 sizeof(VL53L1_gain_calibration_data_t));
2669
2670 /* memcpy (DEST, SRC, N) */
2671 memcpy(
2672 &(pCalibrationData->cal_peak_rate_map),
2673 &(cal_data.cal_peak_rate_map),
2674 sizeof(VL53L1_cal_peak_rate_map_t));
2675
2676
2677 pC = &pCalibrationData->customer;
2678 pC2 = &cal_data.customer;
2679 pC->global_config__spad_enables_ref_0 =
2680 pC2->global_config__spad_enables_ref_0;
2681 pC->global_config__spad_enables_ref_1 =
2682 pC2->global_config__spad_enables_ref_1;
2683 pC->global_config__spad_enables_ref_2 =
2684 pC2->global_config__spad_enables_ref_2;
2685 pC->global_config__spad_enables_ref_3 =
2686 pC2->global_config__spad_enables_ref_3;
2687 pC->global_config__spad_enables_ref_4 =
2688 pC2->global_config__spad_enables_ref_4;
2689 pC->global_config__spad_enables_ref_5 =
2690 pC2->global_config__spad_enables_ref_5;
2691 pC->global_config__ref_en_start_select =
2692 pC2->global_config__ref_en_start_select;
2693 pC->ref_spad_man__num_requested_ref_spads =
2694 pC2->ref_spad_man__num_requested_ref_spads;
2695 pC->ref_spad_man__ref_location =
2696 pC2->ref_spad_man__ref_location;
2697 pC->algo__crosstalk_compensation_x_plane_gradient_kcps =
2698 pC2->algo__crosstalk_compensation_x_plane_gradient_kcps;
2699 pC->algo__crosstalk_compensation_y_plane_gradient_kcps =
2700 pC2->algo__crosstalk_compensation_y_plane_gradient_kcps;
2701 pC->ref_spad_char__total_rate_target_mcps =
2702 pC2->ref_spad_char__total_rate_target_mcps;
2703 pC->algo__part_to_part_range_offset_mm =
2704 pC2->algo__part_to_part_range_offset_mm;
2705 pC->mm_config__inner_offset_mm =
2706 pC2->mm_config__inner_offset_mm;
2707 pC->mm_config__outer_offset_mm =
2708 pC2->mm_config__outer_offset_mm;
2709
2710 pC->algo__crosstalk_compensation_plane_offset_kcps =
2711 (uint32_t)(
2712 pC2->algo__crosstalk_compensation_plane_offset_kcps);
2713 LOG_FUNCTION_END(Status);
2714 return Status;
2715 }
2716
2717
2718
VL53L1_GetOpticalCenter(VL53L1_DEV Dev,FixPoint1616_t * pOpticalCenterX,FixPoint1616_t * pOpticalCenterY)2719 VL53L1_Error VL53L1_GetOpticalCenter(VL53L1_DEV Dev,
2720 FixPoint1616_t *pOpticalCenterX,
2721 FixPoint1616_t *pOpticalCenterY)
2722 {
2723 VL53L1_Error Status = VL53L1_ERROR_NONE;
2724 VL53L1_calibration_data_t CalibrationData;
2725
2726 LOG_FUNCTION_START("");
2727
2728 *pOpticalCenterX = 0;
2729 *pOpticalCenterY = 0;
2730 Status = VL53L1_get_part_to_part_data(Dev, &CalibrationData);
2731 if (Status == VL53L1_ERROR_NONE) {
2732 *pOpticalCenterX = VL53L1_FIXPOINT44TOFIXPOINT1616(
2733 CalibrationData.optical_centre.x_centre);
2734 *pOpticalCenterY = VL53L1_FIXPOINT44TOFIXPOINT1616(
2735 CalibrationData.optical_centre.y_centre);
2736 }
2737
2738 LOG_FUNCTION_END(Status);
2739 return Status;
2740 }
2741
2742 /* END Group Calibration functions */
2743
2744
2745 /* Group PAL detection triggered events Functions */
2746
VL53L1_SetThresholdConfig(VL53L1_DEV Dev,VL53L1_DetectionConfig_t * pConfig)2747 VL53L1_Error VL53L1_SetThresholdConfig(VL53L1_DEV Dev,
2748 VL53L1_DetectionConfig_t *pConfig)
2749 {
2750 #define BADTHRESBOUNDS(T) \
2751 (((T.CrossMode == VL53L1_THRESHOLD_OUT_OF_WINDOW) || \
2752 (T.CrossMode == VL53L1_THRESHOLD_IN_WINDOW)) && (T.Low > T.High))
2753
2754 VL53L1_Error Status = VL53L1_ERROR_NONE;
2755 VL53L1_GPIO_interrupt_config_t Cfg;
2756 uint16_t g;
2757 FixPoint1616_t gain, high1616, low1616;
2758 VL53L1_LLDriverData_t *pdev;
2759
2760 LOG_FUNCTION_START("");
2761
2762 pdev = VL53L1DevStructGetLLDriverHandle(Dev);
2763
2764 Status = VL53L1_get_GPIO_interrupt_config(Dev, &Cfg);
2765 if (Status == VL53L1_ERROR_NONE) {
2766 if (pConfig->DetectionMode == VL53L1_DETECTION_NORMAL_RUN) {
2767 Cfg.intr_new_measure_ready = 1;
2768 Status = VL53L1_set_GPIO_interrupt_config_struct(Dev,
2769 Cfg);
2770 } else {
2771 if (BADTHRESBOUNDS(pConfig->Distance))
2772 Status = VL53L1_ERROR_INVALID_PARAMS;
2773 if ((Status == VL53L1_ERROR_NONE) &&
2774 (BADTHRESBOUNDS(pConfig->Rate)))
2775 Status = VL53L1_ERROR_INVALID_PARAMS;
2776 if (Status == VL53L1_ERROR_NONE) {
2777 Cfg.intr_new_measure_ready = 0;
2778 Cfg.intr_no_target = pConfig->IntrNoTarget;
2779 /* fix ticket 466238
2780 * Apply invert distance gain to thresholds */
2781 g = pdev->gain_cal.standard_ranging_gain_factor;
2782 /* gain is ufix 5.11, convert to 16.16 */
2783 gain = (FixPoint1616_t) ((uint32_t)g << 5);
2784 high1616 = (FixPoint1616_t) ((uint32_t)
2785 pConfig->Distance.High << 16);
2786 low1616 = (FixPoint1616_t) ((uint32_t)
2787 pConfig->Distance.Low << 16);
2788 /* +32768 to round the results*/
2789 high1616 = (high1616 + 32768) / gain;
2790 low1616 = (low1616 + 32768) / gain;
2791 Cfg.threshold_distance_high = (uint16_t)
2792 (high1616 & 0xFFFF);
2793 Cfg.threshold_distance_low = (uint16_t)
2794 (low1616 & 0xFFFF);
2795 /* end fix ticket 466238 */
2796 Cfg.threshold_rate_high =
2797 VL53L1_FIXPOINT1616TOFIXPOINT97(
2798 pConfig->Rate.High);
2799 Cfg.threshold_rate_low =
2800 VL53L1_FIXPOINT1616TOFIXPOINT97(
2801 pConfig->Rate.Low);
2802
2803 Cfg.intr_mode_distance = ConvertModeToLLD(
2804 &Status,
2805 pConfig->Distance.CrossMode);
2806 if (Status == VL53L1_ERROR_NONE)
2807 Cfg.intr_mode_rate = ConvertModeToLLD(
2808 &Status,
2809 pConfig->Rate.CrossMode);
2810 }
2811
2812 /* Refine thresholds combination now */
2813 if (Status == VL53L1_ERROR_NONE) {
2814 Cfg.intr_combined_mode = 1;
2815 switch (pConfig->DetectionMode) {
2816 case VL53L1_DETECTION_DISTANCE_ONLY:
2817 Cfg.threshold_rate_high = 0;
2818 Cfg.threshold_rate_low = 0;
2819 break;
2820 case VL53L1_DETECTION_RATE_ONLY:
2821 Cfg.threshold_distance_high = 0;
2822 Cfg.threshold_distance_low = 0;
2823 break;
2824 case VL53L1_DETECTION_DISTANCE_OR_RATE:
2825 /* Nothing to do all is already
2826 * in place
2827 */
2828 break;
2829 case VL53L1_DETECTION_DISTANCE_AND_RATE:
2830 Cfg.intr_combined_mode = 0;
2831 break;
2832 default:
2833 Status = VL53L1_ERROR_INVALID_PARAMS;
2834 }
2835 }
2836
2837 if (Status == VL53L1_ERROR_NONE)
2838 Status =
2839 VL53L1_set_GPIO_interrupt_config_struct(Dev,
2840 Cfg);
2841
2842 }
2843 }
2844
2845 LOG_FUNCTION_END(Status);
2846 return Status;
2847 }
2848
2849
VL53L1_GetThresholdConfig(VL53L1_DEV Dev,VL53L1_DetectionConfig_t * pConfig)2850 VL53L1_Error VL53L1_GetThresholdConfig(VL53L1_DEV Dev,
2851 VL53L1_DetectionConfig_t *pConfig)
2852 {
2853 VL53L1_Error Status = VL53L1_ERROR_NONE;
2854 VL53L1_GPIO_interrupt_config_t Cfg;
2855
2856 LOG_FUNCTION_START("");
2857
2858 Status = VL53L1_get_GPIO_interrupt_config(Dev, &Cfg);
2859
2860 if (Status != VL53L1_ERROR_NONE) {
2861 LOG_FUNCTION_END(Status);
2862 return Status;
2863 }
2864
2865 pConfig->IntrNoTarget = Cfg.intr_no_target;
2866 pConfig->Distance.High = Cfg.threshold_distance_high;
2867 pConfig->Distance.Low = Cfg.threshold_distance_low;
2868 pConfig->Rate.High =
2869 VL53L1_FIXPOINT97TOFIXPOINT1616(
2870 Cfg.threshold_rate_high);
2871 pConfig->Rate.Low =
2872 VL53L1_FIXPOINT97TOFIXPOINT1616(Cfg.threshold_rate_low);
2873 pConfig->Distance.CrossMode =
2874 ConvertModeFromLLD(&Status, Cfg.intr_mode_distance);
2875 if (Status == VL53L1_ERROR_NONE)
2876 pConfig->Rate.CrossMode =
2877 ConvertModeFromLLD(&Status, Cfg.intr_mode_rate);
2878
2879 if (Cfg.intr_new_measure_ready == 1) {
2880 pConfig->DetectionMode = VL53L1_DETECTION_NORMAL_RUN;
2881 } else {
2882 /* Refine thresholds combination now */
2883 if (Status == VL53L1_ERROR_NONE) {
2884 if (Cfg.intr_combined_mode == 0)
2885 pConfig->DetectionMode =
2886 VL53L1_DETECTION_DISTANCE_AND_RATE;
2887 else {
2888 if ((Cfg.threshold_distance_high == 0) &&
2889 (Cfg.threshold_distance_low == 0))
2890 pConfig->DetectionMode =
2891 VL53L1_DETECTION_RATE_ONLY;
2892 else if ((Cfg.threshold_rate_high == 0) &&
2893 (Cfg.threshold_rate_low == 0))
2894 pConfig->DetectionMode =
2895 VL53L1_DETECTION_DISTANCE_ONLY;
2896 else
2897 pConfig->DetectionMode =
2898 VL53L1_DETECTION_DISTANCE_OR_RATE;
2899 }
2900 }
2901 }
2902
2903 LOG_FUNCTION_END(Status);
2904 return Status;
2905 }
2906
2907
2908 /* End Group PAL IRQ Triggered events Functions */
2909
2910
