amiro-lld / drivers / VL53L1X / v1 / api / core / vl53l1_api.c @ 4dba9195
History | View | Annotate | Download (78.039 KB)
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
*/
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 */
|
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 |
|
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 |
|
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 |
|
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 */
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 */
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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
|
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
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|
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 |
|