amiro-lld / drivers / VL53L1X / v1 / api / core / vl53l1_core.c @ f0dd1ac4
History | View | Annotate | Download (57.025 KB)
1 |
/*
|
---|---|
2 |
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
3 |
*
|
4 |
* This file is part of VL53L1 Core and is dual licensed,
|
5 |
* either 'STMicroelectronics
|
6 |
* Proprietary license'
|
7 |
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
8 |
*
|
9 |
********************************************************************************
|
10 |
*
|
11 |
* 'STMicroelectronics Proprietary license'
|
12 |
*
|
13 |
********************************************************************************
|
14 |
*
|
15 |
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
16 |
* terms at www.st.com/sla0081
|
17 |
*
|
18 |
* STMicroelectronics confidential
|
19 |
* Reproduction and Communication of this document is strictly prohibited unless
|
20 |
* specifically authorized in writing by STMicroelectronics.
|
21 |
*
|
22 |
*
|
23 |
********************************************************************************
|
24 |
*
|
25 |
* Alternatively, VL53L1 Core may be distributed under the terms of
|
26 |
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
27 |
* provisions apply instead of the ones mentioned above :
|
28 |
*
|
29 |
********************************************************************************
|
30 |
*
|
31 |
* License terms: BSD 3-clause "New" or "Revised" License.
|
32 |
*
|
33 |
* Redistribution and use in source and binary forms, with or without
|
34 |
* modification, are permitted provided that the following conditions are met:
|
35 |
*
|
36 |
* 1. Redistributions of source code must retain the above copyright notice, this
|
37 |
* list of conditions and the following disclaimer.
|
38 |
*
|
39 |
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
40 |
* this list of conditions and the following disclaimer in the documentation
|
41 |
* and/or other materials provided with the distribution.
|
42 |
*
|
43 |
* 3. Neither the name of the copyright holder nor the names of its contributors
|
44 |
* may be used to endorse or promote products derived from this software
|
45 |
* without specific prior written permission.
|
46 |
*
|
47 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
48 |
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
49 |
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
50 |
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
51 |
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
52 |
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
53 |
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
54 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
55 |
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
56 |
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
57 |
*
|
58 |
*
|
59 |
********************************************************************************
|
60 |
*
|
61 |
*/
|
62 |
|
63 |
/**
|
64 |
* @file vl53l1_core.c
|
65 |
*
|
66 |
* @brief EwokPlus25 core function definition
|
67 |
*/
|
68 |
|
69 |
#include "vl53l1_ll_def.h" |
70 |
#include "vl53l1_ll_device.h" |
71 |
#include "vl53l1_platform.h" |
72 |
#include "vl53l1_register_map.h" |
73 |
#include "vl53l1_register_funcs.h" |
74 |
#include "vl53l1_register_settings.h" |
75 |
#include "vl53l1_api_preset_modes.h" |
76 |
#include "vl53l1_core.h" |
77 |
#include "vl53l1_tuning_parm_defaults.h" |
78 |
|
79 |
#ifdef VL53L1_LOGGING
|
80 |
#include "vl53l1_api_debug.h" |
81 |
#include "vl53l1_debug.h" |
82 |
#include "vl53l1_register_debug.h" |
83 |
#endif
|
84 |
|
85 |
#define LOG_FUNCTION_START(fmt, ...) \
|
86 |
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__) |
87 |
#define LOG_FUNCTION_END(status, ...) \
|
88 |
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__) |
89 |
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
90 |
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \ |
91 |
status, fmt, ##__VA_ARGS__) |
92 |
|
93 |
#define trace_print(level, ...) \
|
94 |
_LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \ |
95 |
level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) |
96 |
|
97 |
|
98 |
void VL53L1_init_version(
|
99 |
VL53L1_DEV Dev) |
100 |
{ |
101 |
/**
|
102 |
* Initialise version structure
|
103 |
*/
|
104 |
|
105 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
106 |
|
107 |
pdev->version.ll_major = VL53L1_LL_API_IMPLEMENTATION_VER_MAJOR; |
108 |
pdev->version.ll_minor = VL53L1_LL_API_IMPLEMENTATION_VER_MINOR; |
109 |
pdev->version.ll_build = VL53L1_LL_API_IMPLEMENTATION_VER_SUB; |
110 |
pdev->version.ll_revision = VL53L1_LL_API_IMPLEMENTATION_VER_REVISION; |
111 |
} |
112 |
|
113 |
|
114 |
void VL53L1_init_ll_driver_state(
|
115 |
VL53L1_DEV Dev, |
116 |
VL53L1_DeviceState device_state) |
117 |
{ |
118 |
/**
|
119 |
* Initialise LL Driver state variables
|
120 |
*/
|
121 |
|
122 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
123 |
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); |
124 |
|
125 |
pstate->cfg_device_state = device_state; |
126 |
pstate->cfg_stream_count = 0;
|
127 |
pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
128 |
pstate->cfg_timing_status = 0;
|
129 |
|
130 |
pstate->rd_device_state = device_state; |
131 |
pstate->rd_stream_count = 0;
|
132 |
pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
133 |
pstate->rd_timing_status = 0;
|
134 |
|
135 |
} |
136 |
|
137 |
|
138 |
VL53L1_Error VL53L1_update_ll_driver_rd_state( |
139 |
VL53L1_DEV Dev) |
140 |
{ |
141 |
/**
|
142 |
* State machine for read device state
|
143 |
*
|
144 |
* VL53L1_DEVICESTATE_SW_STANDBY
|
145 |
* VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC
|
146 |
* VL53L1_DEVICESTATE_RANGING_GATHER_DATA
|
147 |
* VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA
|
148 |
*/
|
149 |
|
150 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
151 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
152 |
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); |
153 |
|
154 |
/* if top bits of mode start reset are zero then in standby state */
|
155 |
|
156 |
LOG_FUNCTION_START("");
|
157 |
|
158 |
#ifdef VL53L1_LOGGING
|
159 |
VL53L1_print_ll_driver_state(pstate); |
160 |
#endif
|
161 |
|
162 |
if ((pdev->sys_ctrl.system__mode_start &
|
163 |
VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
|
164 |
|
165 |
pstate->rd_device_state = VL53L1_DEVICESTATE_SW_STANDBY; |
166 |
pstate->rd_stream_count = 0;
|
167 |
pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
168 |
pstate->rd_timing_status = 0;
|
169 |
|
170 |
} else {
|
171 |
|
172 |
/*
|
173 |
* implement read stream count
|
174 |
*/
|
175 |
|
176 |
if (pstate->rd_stream_count == 0xFF) { |
177 |
pstate->rd_stream_count = 0x80;
|
178 |
} else {
|
179 |
pstate->rd_stream_count++; |
180 |
} |
181 |
|
182 |
|
183 |
/*
|
184 |
* Toggle grouped parameter hold ID
|
185 |
*/
|
186 |
|
187 |
pstate->rd_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
188 |
|
189 |
/* Ok now ranging */
|
190 |
|
191 |
switch (pstate->rd_device_state) {
|
192 |
|
193 |
case VL53L1_DEVICESTATE_SW_STANDBY:
|
194 |
|
195 |
if ((pdev->dyn_cfg.system__grouped_parameter_hold &
|
196 |
VL53L1_GROUPEDPARAMETERHOLD_ID_MASK) > 0) {
|
197 |
pstate->rd_device_state = |
198 |
VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC; |
199 |
} else {
|
200 |
pstate->rd_device_state = |
201 |
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; |
202 |
} |
203 |
|
204 |
pstate->rd_stream_count = 0;
|
205 |
pstate->rd_timing_status = 0;
|
206 |
|
207 |
break;
|
208 |
|
209 |
case VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC:
|
210 |
|
211 |
pstate->rd_stream_count = 0;
|
212 |
pstate->rd_device_state = |
213 |
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; |
214 |
|
215 |
break;
|
216 |
|
217 |
case VL53L1_DEVICESTATE_RANGING_GATHER_DATA:
|
218 |
|
219 |
pstate->rd_device_state = |
220 |
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; |
221 |
|
222 |
break;
|
223 |
|
224 |
case VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA:
|
225 |
|
226 |
pstate->rd_timing_status ^= 0x01;
|
227 |
|
228 |
pstate->rd_device_state = |
229 |
VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA; |
230 |
|
231 |
break;
|
232 |
|
233 |
default:
|
234 |
|
235 |
pstate->rd_device_state = |
236 |
VL53L1_DEVICESTATE_SW_STANDBY; |
237 |
pstate->rd_stream_count = 0;
|
238 |
pstate->rd_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
239 |
pstate->rd_timing_status = 0;
|
240 |
|
241 |
break;
|
242 |
} |
243 |
} |
244 |
|
245 |
#ifdef VL53L1_LOGGING
|
246 |
VL53L1_print_ll_driver_state(pstate); |
247 |
#endif
|
248 |
|
249 |
LOG_FUNCTION_END(status); |
250 |
|
251 |
return status;
|
252 |
} |
253 |
|
254 |
|
255 |
VL53L1_Error VL53L1_check_ll_driver_rd_state( |
256 |
VL53L1_DEV Dev) |
257 |
{ |
258 |
/*
|
259 |
* Checks if the LL Driver Read state and expected stream count
|
260 |
* matches the state and stream count received from the device
|
261 |
*
|
262 |
* Check is only use in back to back mode
|
263 |
*/
|
264 |
|
265 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
266 |
VL53L1_LLDriverData_t *pdev = |
267 |
VL53L1DevStructGetLLDriverHandle(Dev); |
268 |
|
269 |
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); |
270 |
VL53L1_system_results_t *psys_results = &(pdev->sys_results); |
271 |
|
272 |
uint8_t device_range_status = 0;
|
273 |
uint8_t device_stream_count = 0;
|
274 |
uint8_t device_gph_id = 0;
|
275 |
|
276 |
LOG_FUNCTION_START("");
|
277 |
|
278 |
#ifdef VL53L1_LOGGING
|
279 |
VL53L1_print_ll_driver_state(pstate); |
280 |
#endif
|
281 |
|
282 |
device_range_status = |
283 |
psys_results->result__range_status & |
284 |
VL53L1_RANGE_STATUS__RANGE_STATUS_MASK; |
285 |
|
286 |
device_stream_count = psys_results->result__stream_count; |
287 |
|
288 |
/* load the correct GPH ID */
|
289 |
device_gph_id = (psys_results->result__interrupt_status & |
290 |
VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;
|
291 |
|
292 |
/* only apply checks in back to back mode */
|
293 |
|
294 |
if ((pdev->sys_ctrl.system__mode_start &
|
295 |
VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) == |
296 |
VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) { |
297 |
|
298 |
/* if read state is wait for GPH sync interrupt then check the
|
299 |
* device returns a GPH range status value otherwise check that
|
300 |
* the stream count matches
|
301 |
*
|
302 |
* In theory the stream count should zero for the GPH interrupt
|
303 |
* but that is not the case after at abort ....
|
304 |
*/
|
305 |
|
306 |
if (pstate->rd_device_state ==
|
307 |
VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC) { |
308 |
|
309 |
if (device_range_status !=
|
310 |
VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY) { |
311 |
status = VL53L1_ERROR_GPH_SYNC_CHECK_FAIL; |
312 |
} |
313 |
} else {
|
314 |
if (pstate->rd_stream_count != device_stream_count) {
|
315 |
status = VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL; |
316 |
} |
317 |
|
318 |
/*
|
319 |
* Check Read state GPH ID
|
320 |
*/
|
321 |
|
322 |
if (pstate->rd_gph_id != device_gph_id) {
|
323 |
status = VL53L1_ERROR_GPH_ID_CHECK_FAIL; |
324 |
#ifdef VL53L1_LOGGING
|
325 |
trace_print(VL53L1_TRACE_LEVEL_ALL, |
326 |
" RDSTATECHECK: Check failed: rd_gph_id: %d, device_gph_id: %d\n",
|
327 |
pstate->rd_gph_id, |
328 |
device_gph_id); |
329 |
#endif
|
330 |
} else {
|
331 |
#ifdef VL53L1_LOGGING
|
332 |
trace_print(VL53L1_TRACE_LEVEL_ALL, |
333 |
" RDSTATECHECK: Check passed: rd_gph_id: %d, device_gph_id: %d\n",
|
334 |
pstate->rd_gph_id, |
335 |
device_gph_id); |
336 |
#endif
|
337 |
} |
338 |
|
339 |
} /* else (not in WAIT_GPH_SYNC) */
|
340 |
|
341 |
} /* if back to back */
|
342 |
|
343 |
LOG_FUNCTION_END(status); |
344 |
|
345 |
return status;
|
346 |
} |
347 |
|
348 |
|
349 |
VL53L1_Error VL53L1_update_ll_driver_cfg_state( |
350 |
VL53L1_DEV Dev) |
351 |
{ |
352 |
/**
|
353 |
* State machine for configuration device state
|
354 |
*/
|
355 |
|
356 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
357 |
VL53L1_LLDriverData_t *pdev = |
358 |
VL53L1DevStructGetLLDriverHandle(Dev); |
359 |
|
360 |
VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state); |
361 |
|
362 |
LOG_FUNCTION_START("");
|
363 |
|
364 |
#ifdef VL53L1_LOGGING
|
365 |
VL53L1_print_ll_driver_state(pstate); |
366 |
#endif
|
367 |
|
368 |
/* if top bits of mode start reset are zero then in standby state */
|
369 |
|
370 |
if ((pdev->sys_ctrl.system__mode_start &
|
371 |
VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00) {
|
372 |
|
373 |
pstate->cfg_device_state = VL53L1_DEVICESTATE_SW_STANDBY; |
374 |
pstate->cfg_stream_count = 0;
|
375 |
pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
376 |
pstate->cfg_timing_status = 0;
|
377 |
|
378 |
} else {
|
379 |
|
380 |
/*
|
381 |
* implement configuration stream count
|
382 |
*/
|
383 |
|
384 |
if (pstate->cfg_stream_count == 0xFF) { |
385 |
pstate->cfg_stream_count = 0x80;
|
386 |
} else {
|
387 |
pstate->cfg_stream_count++; |
388 |
} |
389 |
|
390 |
/*
|
391 |
* Toggle grouped parameter hold ID
|
392 |
*/
|
393 |
|
394 |
pstate->cfg_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
395 |
|
396 |
/*
|
397 |
* Implement configuration state machine
|
398 |
*/
|
399 |
|
400 |
switch (pstate->cfg_device_state) {
|
401 |
|
402 |
case VL53L1_DEVICESTATE_SW_STANDBY:
|
403 |
|
404 |
pstate->cfg_timing_status ^= 0x01;
|
405 |
pstate->cfg_stream_count = 1;
|
406 |
|
407 |
pstate->cfg_device_state = VL53L1_DEVICESTATE_RANGING_DSS_AUTO; |
408 |
break;
|
409 |
|
410 |
case VL53L1_DEVICESTATE_RANGING_DSS_AUTO:
|
411 |
|
412 |
pstate->cfg_timing_status ^= 0x01;
|
413 |
|
414 |
break;
|
415 |
|
416 |
default:
|
417 |
|
418 |
pstate->cfg_device_state = VL53L1_DEVICESTATE_SW_STANDBY; |
419 |
pstate->cfg_stream_count = 0;
|
420 |
pstate->cfg_gph_id = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK; |
421 |
pstate->cfg_timing_status = 0;
|
422 |
|
423 |
break;
|
424 |
} |
425 |
} |
426 |
|
427 |
#ifdef VL53L1_LOGGING
|
428 |
VL53L1_print_ll_driver_state(pstate); |
429 |
#endif
|
430 |
|
431 |
LOG_FUNCTION_END(status); |
432 |
|
433 |
return status;
|
434 |
} |
435 |
|
436 |
|
437 |
void VL53L1_copy_rtn_good_spads_to_buffer(
|
438 |
VL53L1_nvm_copy_data_t *pdata, |
439 |
uint8_t *pbuffer) |
440 |
{ |
441 |
/*
|
442 |
* Convenience function to copy return SPAD enables to buffer
|
443 |
*/
|
444 |
|
445 |
*(pbuffer + 0) = pdata->global_config__spad_enables_rtn_0;
|
446 |
*(pbuffer + 1) = pdata->global_config__spad_enables_rtn_1;
|
447 |
*(pbuffer + 2) = pdata->global_config__spad_enables_rtn_2;
|
448 |
*(pbuffer + 3) = pdata->global_config__spad_enables_rtn_3;
|
449 |
*(pbuffer + 4) = pdata->global_config__spad_enables_rtn_4;
|
450 |
*(pbuffer + 5) = pdata->global_config__spad_enables_rtn_5;
|
451 |
*(pbuffer + 6) = pdata->global_config__spad_enables_rtn_6;
|
452 |
*(pbuffer + 7) = pdata->global_config__spad_enables_rtn_7;
|
453 |
*(pbuffer + 8) = pdata->global_config__spad_enables_rtn_8;
|
454 |
*(pbuffer + 9) = pdata->global_config__spad_enables_rtn_9;
|
455 |
*(pbuffer + 10) = pdata->global_config__spad_enables_rtn_10;
|
456 |
*(pbuffer + 11) = pdata->global_config__spad_enables_rtn_11;
|
457 |
*(pbuffer + 12) = pdata->global_config__spad_enables_rtn_12;
|
458 |
*(pbuffer + 13) = pdata->global_config__spad_enables_rtn_13;
|
459 |
*(pbuffer + 14) = pdata->global_config__spad_enables_rtn_14;
|
460 |
*(pbuffer + 15) = pdata->global_config__spad_enables_rtn_15;
|
461 |
*(pbuffer + 16) = pdata->global_config__spad_enables_rtn_16;
|
462 |
*(pbuffer + 17) = pdata->global_config__spad_enables_rtn_17;
|
463 |
*(pbuffer + 18) = pdata->global_config__spad_enables_rtn_18;
|
464 |
*(pbuffer + 19) = pdata->global_config__spad_enables_rtn_19;
|
465 |
*(pbuffer + 20) = pdata->global_config__spad_enables_rtn_20;
|
466 |
*(pbuffer + 21) = pdata->global_config__spad_enables_rtn_21;
|
467 |
*(pbuffer + 22) = pdata->global_config__spad_enables_rtn_22;
|
468 |
*(pbuffer + 23) = pdata->global_config__spad_enables_rtn_23;
|
469 |
*(pbuffer + 24) = pdata->global_config__spad_enables_rtn_24;
|
470 |
*(pbuffer + 25) = pdata->global_config__spad_enables_rtn_25;
|
471 |
*(pbuffer + 26) = pdata->global_config__spad_enables_rtn_26;
|
472 |
*(pbuffer + 27) = pdata->global_config__spad_enables_rtn_27;
|
473 |
*(pbuffer + 28) = pdata->global_config__spad_enables_rtn_28;
|
474 |
*(pbuffer + 29) = pdata->global_config__spad_enables_rtn_29;
|
475 |
*(pbuffer + 30) = pdata->global_config__spad_enables_rtn_30;
|
476 |
*(pbuffer + 31) = pdata->global_config__spad_enables_rtn_31;
|
477 |
} |
478 |
|
479 |
|
480 |
void VL53L1_init_system_results(
|
481 |
VL53L1_system_results_t *pdata) |
482 |
{ |
483 |
/*
|
484 |
* Initialises the system results to all 0xFF just like the
|
485 |
* device firmware does a the start of a range
|
486 |
*/
|
487 |
|
488 |
pdata->result__interrupt_status = 0xFF;
|
489 |
pdata->result__range_status = 0xFF;
|
490 |
pdata->result__report_status = 0xFF;
|
491 |
pdata->result__stream_count = 0xFF;
|
492 |
|
493 |
pdata->result__dss_actual_effective_spads_sd0 = 0xFFFF;
|
494 |
pdata->result__peak_signal_count_rate_mcps_sd0 = 0xFFFF;
|
495 |
pdata->result__ambient_count_rate_mcps_sd0 = 0xFFFF;
|
496 |
pdata->result__sigma_sd0 = 0xFFFF;
|
497 |
pdata->result__phase_sd0 = 0xFFFF;
|
498 |
pdata->result__final_crosstalk_corrected_range_mm_sd0 = 0xFFFF;
|
499 |
pdata->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = |
500 |
0xFFFF;
|
501 |
pdata->result__mm_inner_actual_effective_spads_sd0 = 0xFFFF;
|
502 |
pdata->result__mm_outer_actual_effective_spads_sd0 = 0xFFFF;
|
503 |
pdata->result__avg_signal_count_rate_mcps_sd0 = 0xFFFF;
|
504 |
|
505 |
pdata->result__dss_actual_effective_spads_sd1 = 0xFFFF;
|
506 |
pdata->result__peak_signal_count_rate_mcps_sd1 = 0xFFFF;
|
507 |
pdata->result__ambient_count_rate_mcps_sd1 = 0xFFFF;
|
508 |
pdata->result__sigma_sd1 = 0xFFFF;
|
509 |
pdata->result__phase_sd1 = 0xFFFF;
|
510 |
pdata->result__final_crosstalk_corrected_range_mm_sd1 = 0xFFFF;
|
511 |
pdata->result__spare_0_sd1 = 0xFFFF;
|
512 |
pdata->result__spare_1_sd1 = 0xFFFF;
|
513 |
pdata->result__spare_2_sd1 = 0xFFFF;
|
514 |
pdata->result__spare_3_sd1 = 0xFF;
|
515 |
|
516 |
} |
517 |
|
518 |
|
519 |
void VL53L1_i2c_encode_uint16_t(
|
520 |
uint16_t ip_value, |
521 |
uint16_t count, |
522 |
uint8_t *pbuffer) |
523 |
{ |
524 |
/*
|
525 |
* Encodes a uint16_t register value into an I2C write buffer
|
526 |
* MS byte first order (as per I2C register map.
|
527 |
*/
|
528 |
|
529 |
uint16_t i = 0;
|
530 |
uint16_t data = 0;
|
531 |
|
532 |
data = ip_value; |
533 |
|
534 |
for (i = 0; i < count ; i++) { |
535 |
pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); |
536 |
data = data >> 8;
|
537 |
} |
538 |
} |
539 |
|
540 |
uint16_t VL53L1_i2c_decode_uint16_t( |
541 |
uint16_t count, |
542 |
uint8_t *pbuffer) |
543 |
{ |
544 |
/*
|
545 |
* Decodes a uint16_t from the input I2C read buffer
|
546 |
* (MS byte first order)
|
547 |
*/
|
548 |
|
549 |
uint16_t value = 0x00;
|
550 |
|
551 |
while (count-- > 0) { |
552 |
value = (value << 8) | (uint16_t)*pbuffer++;
|
553 |
} |
554 |
|
555 |
return value;
|
556 |
} |
557 |
|
558 |
|
559 |
void VL53L1_i2c_encode_int16_t(
|
560 |
int16_t ip_value, |
561 |
uint16_t count, |
562 |
uint8_t *pbuffer) |
563 |
{ |
564 |
/*
|
565 |
* Encodes a int16_t register value into an I2C write buffer
|
566 |
* MS byte first order (as per I2C register map.
|
567 |
*/
|
568 |
|
569 |
uint16_t i = 0;
|
570 |
int16_t data = 0;
|
571 |
|
572 |
data = ip_value; |
573 |
|
574 |
for (i = 0; i < count ; i++) { |
575 |
pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); |
576 |
data = data >> 8;
|
577 |
} |
578 |
} |
579 |
|
580 |
int16_t VL53L1_i2c_decode_int16_t( |
581 |
uint16_t count, |
582 |
uint8_t *pbuffer) |
583 |
{ |
584 |
/*
|
585 |
* Decodes a int16_t from the input I2C read buffer
|
586 |
* (MS byte first order)
|
587 |
*/
|
588 |
|
589 |
int16_t value = 0x00;
|
590 |
|
591 |
/* implement sign extension */
|
592 |
if (*pbuffer >= 0x80) { |
593 |
value = 0xFFFF;
|
594 |
} |
595 |
|
596 |
while (count-- > 0) { |
597 |
value = (value << 8) | (int16_t)*pbuffer++;
|
598 |
} |
599 |
|
600 |
return value;
|
601 |
} |
602 |
|
603 |
void VL53L1_i2c_encode_uint32_t(
|
604 |
uint32_t ip_value, |
605 |
uint16_t count, |
606 |
uint8_t *pbuffer) |
607 |
{ |
608 |
/*
|
609 |
* Encodes a uint32_t register value into an I2C write buffer
|
610 |
* MS byte first order (as per I2C register map.
|
611 |
*/
|
612 |
|
613 |
uint16_t i = 0;
|
614 |
uint32_t data = 0;
|
615 |
|
616 |
data = ip_value; |
617 |
|
618 |
for (i = 0; i < count ; i++) { |
619 |
pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); |
620 |
data = data >> 8;
|
621 |
} |
622 |
} |
623 |
|
624 |
uint32_t VL53L1_i2c_decode_uint32_t( |
625 |
uint16_t count, |
626 |
uint8_t *pbuffer) |
627 |
{ |
628 |
/*
|
629 |
* Decodes a uint32_t from the input I2C read buffer
|
630 |
* (MS byte first order)
|
631 |
*/
|
632 |
|
633 |
uint32_t value = 0x00;
|
634 |
|
635 |
while (count-- > 0) { |
636 |
value = (value << 8) | (uint32_t)*pbuffer++;
|
637 |
} |
638 |
|
639 |
return value;
|
640 |
} |
641 |
|
642 |
|
643 |
uint32_t VL53L1_i2c_decode_with_mask( |
644 |
uint16_t count, |
645 |
uint8_t *pbuffer, |
646 |
uint32_t bit_mask, |
647 |
uint32_t down_shift, |
648 |
uint32_t offset) |
649 |
{ |
650 |
/*
|
651 |
* Decodes an integer from the input I2C read buffer
|
652 |
* (MS byte first order)
|
653 |
*/
|
654 |
|
655 |
uint32_t value = 0x00;
|
656 |
|
657 |
/* extract from buffer */
|
658 |
while (count-- > 0) { |
659 |
value = (value << 8) | (uint32_t)*pbuffer++;
|
660 |
} |
661 |
|
662 |
/* Apply bit mask and down shift */
|
663 |
value = value & bit_mask; |
664 |
if (down_shift > 0) { |
665 |
value = value >> down_shift; |
666 |
} |
667 |
|
668 |
/* add offset */
|
669 |
value = value + offset; |
670 |
|
671 |
return value;
|
672 |
} |
673 |
|
674 |
|
675 |
void VL53L1_i2c_encode_int32_t(
|
676 |
int32_t ip_value, |
677 |
uint16_t count, |
678 |
uint8_t *pbuffer) |
679 |
{ |
680 |
/*
|
681 |
* Encodes a int32_t register value into an I2C write buffer
|
682 |
* MS byte first order (as per I2C register map.
|
683 |
*/
|
684 |
|
685 |
uint16_t i = 0;
|
686 |
int32_t data = 0;
|
687 |
|
688 |
data = ip_value; |
689 |
|
690 |
for (i = 0; i < count ; i++) { |
691 |
pbuffer[count-i-1] = (uint8_t)(data & 0x00FF); |
692 |
data = data >> 8;
|
693 |
} |
694 |
} |
695 |
|
696 |
int32_t VL53L1_i2c_decode_int32_t( |
697 |
uint16_t count, |
698 |
uint8_t *pbuffer) |
699 |
{ |
700 |
/*
|
701 |
* Decodes a int32_t from the input I2C read buffer
|
702 |
* (MS byte first order)
|
703 |
*/
|
704 |
|
705 |
int32_t value = 0x00;
|
706 |
|
707 |
/* implement sign extension */
|
708 |
if (*pbuffer >= 0x80) { |
709 |
value = 0xFFFFFFFF;
|
710 |
} |
711 |
|
712 |
while (count-- > 0) { |
713 |
value = (value << 8) | (int32_t)*pbuffer++;
|
714 |
} |
715 |
|
716 |
return value;
|
717 |
} |
718 |
|
719 |
|
720 |
#ifndef VL53L1_NOCALIB
|
721 |
VL53L1_Error VL53L1_start_test( |
722 |
VL53L1_DEV Dev, |
723 |
uint8_t test_mode__ctrl) |
724 |
{ |
725 |
/*
|
726 |
* Triggers the start of a test mode
|
727 |
*/
|
728 |
|
729 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
730 |
|
731 |
LOG_FUNCTION_START("");
|
732 |
|
733 |
if (status == VL53L1_ERROR_NONE) { /*lint !e774 always true*/ |
734 |
status = VL53L1_WrByte( |
735 |
Dev, |
736 |
VL53L1_TEST_MODE__CTRL, |
737 |
test_mode__ctrl); |
738 |
} |
739 |
|
740 |
LOG_FUNCTION_END(status); |
741 |
|
742 |
return status;
|
743 |
} |
744 |
#endif
|
745 |
|
746 |
|
747 |
VL53L1_Error VL53L1_set_firmware_enable_register( |
748 |
VL53L1_DEV Dev, |
749 |
uint8_t value) |
750 |
{ |
751 |
/*
|
752 |
* Set FIRMWARE__ENABLE register
|
753 |
*/
|
754 |
|
755 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
756 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
757 |
|
758 |
pdev->sys_ctrl.firmware__enable = value; |
759 |
|
760 |
status = VL53L1_WrByte( |
761 |
Dev, |
762 |
VL53L1_FIRMWARE__ENABLE, |
763 |
pdev->sys_ctrl.firmware__enable); |
764 |
|
765 |
return status;
|
766 |
} |
767 |
|
768 |
VL53L1_Error VL53L1_enable_firmware( |
769 |
VL53L1_DEV Dev) |
770 |
{ |
771 |
/*
|
772 |
* Enable firmware
|
773 |
*/
|
774 |
|
775 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
776 |
|
777 |
LOG_FUNCTION_START("");
|
778 |
|
779 |
status = VL53L1_set_firmware_enable_register(Dev, 0x01);
|
780 |
|
781 |
LOG_FUNCTION_END(status); |
782 |
|
783 |
return status;
|
784 |
} |
785 |
|
786 |
|
787 |
VL53L1_Error VL53L1_disable_firmware( |
788 |
VL53L1_DEV Dev) |
789 |
{ |
790 |
/*
|
791 |
* Disable firmware
|
792 |
*/
|
793 |
|
794 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
795 |
|
796 |
LOG_FUNCTION_START("");
|
797 |
|
798 |
status = VL53L1_set_firmware_enable_register(Dev, 0x00);
|
799 |
|
800 |
LOG_FUNCTION_END(status); |
801 |
|
802 |
return status;
|
803 |
} |
804 |
|
805 |
|
806 |
VL53L1_Error VL53L1_set_powerforce_register( |
807 |
VL53L1_DEV Dev, |
808 |
uint8_t value) |
809 |
{ |
810 |
/*
|
811 |
* Set power force register
|
812 |
*/
|
813 |
|
814 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
815 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
816 |
|
817 |
pdev->sys_ctrl.power_management__go1_power_force = value; |
818 |
|
819 |
status = VL53L1_WrByte( |
820 |
Dev, |
821 |
VL53L1_POWER_MANAGEMENT__GO1_POWER_FORCE, |
822 |
pdev->sys_ctrl.power_management__go1_power_force); |
823 |
|
824 |
return status;
|
825 |
} |
826 |
|
827 |
|
828 |
VL53L1_Error VL53L1_enable_powerforce( |
829 |
VL53L1_DEV Dev) |
830 |
{ |
831 |
/*
|
832 |
* Enable power force
|
833 |
*/
|
834 |
|
835 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
836 |
|
837 |
LOG_FUNCTION_START("");
|
838 |
|
839 |
status = VL53L1_set_powerforce_register(Dev, 0x01);
|
840 |
|
841 |
LOG_FUNCTION_END(status); |
842 |
|
843 |
return status;
|
844 |
} |
845 |
|
846 |
|
847 |
VL53L1_Error VL53L1_disable_powerforce( |
848 |
VL53L1_DEV Dev) |
849 |
{ |
850 |
/*
|
851 |
* Disable power force
|
852 |
*/
|
853 |
|
854 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
855 |
|
856 |
LOG_FUNCTION_START("");
|
857 |
|
858 |
status = VL53L1_set_powerforce_register(Dev, 0x00);
|
859 |
|
860 |
LOG_FUNCTION_END(status); |
861 |
|
862 |
return status;
|
863 |
} |
864 |
|
865 |
|
866 |
VL53L1_Error VL53L1_clear_interrupt( |
867 |
VL53L1_DEV Dev) |
868 |
{ |
869 |
/*
|
870 |
* Clear Ranging interrupt by writing to
|
871 |
*/
|
872 |
|
873 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
874 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
875 |
|
876 |
LOG_FUNCTION_START("");
|
877 |
|
878 |
pdev->sys_ctrl.system__interrupt_clear = VL53L1_CLEAR_RANGE_INT; |
879 |
|
880 |
status = VL53L1_WrByte( |
881 |
Dev, |
882 |
VL53L1_SYSTEM__INTERRUPT_CLEAR, |
883 |
pdev->sys_ctrl.system__interrupt_clear); |
884 |
|
885 |
LOG_FUNCTION_END(status); |
886 |
|
887 |
return status;
|
888 |
} |
889 |
|
890 |
|
891 |
#ifdef VL53L1_DEBUG
|
892 |
VL53L1_Error VL53L1_force_shadow_stream_count_to_zero( |
893 |
VL53L1_DEV Dev) |
894 |
{ |
895 |
/*
|
896 |
* Forces shadow stream count to zero
|
897 |
*/
|
898 |
|
899 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
900 |
|
901 |
if (status == VL53L1_ERROR_NONE) { /*lint !e774 always true*/ |
902 |
status = VL53L1_disable_firmware(Dev); |
903 |
} |
904 |
|
905 |
if (status == VL53L1_ERROR_NONE) {
|
906 |
status = VL53L1_WrByte( |
907 |
Dev, |
908 |
VL53L1_SHADOW_RESULT__STREAM_COUNT, |
909 |
0x00);
|
910 |
} |
911 |
|
912 |
if (status == VL53L1_ERROR_NONE) {
|
913 |
status = VL53L1_enable_firmware(Dev); |
914 |
} |
915 |
|
916 |
return status;
|
917 |
} |
918 |
#endif
|
919 |
|
920 |
uint32_t VL53L1_calc_macro_period_us( |
921 |
uint16_t fast_osc_frequency, |
922 |
uint8_t vcsel_period) |
923 |
{ |
924 |
/* Calculates macro period in [us] from the input fast oscillator
|
925 |
* frequency and VCSEL period
|
926 |
*
|
927 |
* Macro period fixed point format = unsigned 12.12
|
928 |
* Maximum supported macro period = 4095.9999 us
|
929 |
*/
|
930 |
|
931 |
uint32_t pll_period_us = 0;
|
932 |
uint8_t vcsel_period_pclks = 0;
|
933 |
uint32_t macro_period_us = 0;
|
934 |
|
935 |
LOG_FUNCTION_START("");
|
936 |
|
937 |
/* Calculate PLL period in [us] from the fast_osc_frequency
|
938 |
* Fast osc frequency fixed point format = unsigned 4.12
|
939 |
*/
|
940 |
|
941 |
pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency); |
942 |
|
943 |
/* VCSEL period
|
944 |
* - the real VCSEL period in PLL clocks = 2*(VCSEL_PERIOD+1)
|
945 |
*/
|
946 |
|
947 |
vcsel_period_pclks = VL53L1_decode_vcsel_period(vcsel_period); |
948 |
|
949 |
/* Macro period
|
950 |
* - PLL period [us] = 0.24 format
|
951 |
* - for 1.0 MHz fast oscillator freq
|
952 |
* - max PLL period = 1/64 (6-bits)
|
953 |
* - i.e only the lower 18-bits of PLL Period value are used
|
954 |
* - Macro period [vclks] = 2304 (12-bits)
|
955 |
*
|
956 |
* Max bits (24 - 6) + 12 = 30-bits usage
|
957 |
*
|
958 |
* Downshift by 6 before multiplying by the VCSEL Period
|
959 |
*/
|
960 |
|
961 |
macro_period_us = |
962 |
(uint32_t)VL53L1_MACRO_PERIOD_VCSEL_PERIODS * |
963 |
pll_period_us; |
964 |
macro_period_us = macro_period_us >> 6;
|
965 |
|
966 |
macro_period_us = macro_period_us * (uint32_t)vcsel_period_pclks; |
967 |
macro_period_us = macro_period_us >> 6;
|
968 |
|
969 |
#ifdef VL53L1_LOGGING
|
970 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
971 |
" %-48s : %10u\n", "pll_period_us", |
972 |
pll_period_us); |
973 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
974 |
" %-48s : %10u\n", "vcsel_period_pclks", |
975 |
vcsel_period_pclks); |
976 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
977 |
" %-48s : %10u\n", "macro_period_us", |
978 |
macro_period_us); |
979 |
#endif
|
980 |
|
981 |
LOG_FUNCTION_END(0);
|
982 |
|
983 |
return macro_period_us;
|
984 |
} |
985 |
|
986 |
|
987 |
uint16_t VL53L1_calc_range_ignore_threshold( |
988 |
uint32_t central_rate, |
989 |
int16_t x_gradient, |
990 |
int16_t y_gradient, |
991 |
uint8_t rate_mult) |
992 |
{ |
993 |
/* Calculates Range Ignore Threshold rate per spad
|
994 |
* in Mcps - 3.13 format
|
995 |
*
|
996 |
* Calculates worst case xtalk rate per spad in array corner
|
997 |
* based on input central xtalk and x and y gradients
|
998 |
*
|
999 |
* Worst case rate = central rate + (8*(magnitude(xgrad)) +
|
1000 |
* (8*(magnitude(ygrad)))
|
1001 |
*
|
1002 |
* Range ignore threshold rate is then multiplied by user input
|
1003 |
* rate_mult (in 3.5 fractional format)
|
1004 |
*
|
1005 |
*/
|
1006 |
|
1007 |
int32_t range_ignore_thresh_int = 0;
|
1008 |
uint16_t range_ignore_thresh_kcps = 0;
|
1009 |
int32_t central_rate_int = 0;
|
1010 |
int16_t x_gradient_int = 0;
|
1011 |
int16_t y_gradient_int = 0;
|
1012 |
|
1013 |
LOG_FUNCTION_START("");
|
1014 |
|
1015 |
/* Shift central_rate to .13 fractional for simple addition */
|
1016 |
|
1017 |
central_rate_int = ((int32_t)central_rate * (1 << 4)) / (1000); |
1018 |
|
1019 |
if (x_gradient < 0) { |
1020 |
x_gradient_int = x_gradient * -1;
|
1021 |
} |
1022 |
|
1023 |
if (y_gradient < 0) { |
1024 |
y_gradient_int = y_gradient * -1;
|
1025 |
} |
1026 |
|
1027 |
/* Calculate full rate per spad - worst case from measured xtalk */
|
1028 |
/* Generated here from .11 fractional kcps */
|
1029 |
/* Additional factor of 4 applied to bring fractional precision to .13 */
|
1030 |
|
1031 |
range_ignore_thresh_int = (8 * x_gradient_int * 4) + (8 * y_gradient_int * 4); |
1032 |
|
1033 |
/* Convert Kcps to Mcps */
|
1034 |
|
1035 |
range_ignore_thresh_int = range_ignore_thresh_int / 1000;
|
1036 |
|
1037 |
/* Combine with Central Rate - Mcps .13 format*/
|
1038 |
|
1039 |
range_ignore_thresh_int = range_ignore_thresh_int + central_rate_int; |
1040 |
|
1041 |
/* Mult by user input */
|
1042 |
|
1043 |
range_ignore_thresh_int = (int32_t)rate_mult * range_ignore_thresh_int; |
1044 |
|
1045 |
range_ignore_thresh_int = (range_ignore_thresh_int + (1<<4)) / (1<<5); |
1046 |
|
1047 |
/* Finally clip and output in correct format */
|
1048 |
|
1049 |
if (range_ignore_thresh_int > 0xFFFF) { |
1050 |
range_ignore_thresh_kcps = 0xFFFF;
|
1051 |
} else {
|
1052 |
range_ignore_thresh_kcps = (uint16_t)range_ignore_thresh_int; |
1053 |
} |
1054 |
|
1055 |
#ifdef VL53L1_LOGGING
|
1056 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
1057 |
" %-48s : %10u\n", "range_ignore_thresh_kcps", |
1058 |
range_ignore_thresh_kcps); |
1059 |
#endif
|
1060 |
|
1061 |
LOG_FUNCTION_END(0);
|
1062 |
|
1063 |
return range_ignore_thresh_kcps;
|
1064 |
} |
1065 |
|
1066 |
|
1067 |
uint32_t VL53L1_calc_timeout_mclks( |
1068 |
uint32_t timeout_us, |
1069 |
uint32_t macro_period_us) |
1070 |
{ |
1071 |
/* Calculates the timeout value in macro periods based on the input
|
1072 |
* timeout period in milliseconds and the macro period in [us]
|
1073 |
*
|
1074 |
* Max timeout supported is 1000000 us (1 sec) -> 20-bits
|
1075 |
* Max timeout in 20.12 format = 32-bits
|
1076 |
*
|
1077 |
* Macro period [us] = 12.12 format
|
1078 |
*/
|
1079 |
|
1080 |
uint32_t timeout_mclks = 0;
|
1081 |
|
1082 |
LOG_FUNCTION_START("");
|
1083 |
|
1084 |
timeout_mclks = |
1085 |
((timeout_us << 12) + (macro_period_us>>1)) / |
1086 |
macro_period_us; |
1087 |
|
1088 |
LOG_FUNCTION_END(0);
|
1089 |
|
1090 |
return timeout_mclks;
|
1091 |
} |
1092 |
|
1093 |
|
1094 |
uint16_t VL53L1_calc_encoded_timeout( |
1095 |
uint32_t timeout_us, |
1096 |
uint32_t macro_period_us) |
1097 |
{ |
1098 |
/* Calculates the encoded timeout register value based on the input
|
1099 |
* timeout period in milliseconds and the macro period in [us]
|
1100 |
*
|
1101 |
* Max timeout supported is 1000000 us (1 sec) -> 20-bits
|
1102 |
* Max timeout in 20.12 format = 32-bits
|
1103 |
*
|
1104 |
* Macro period [us] = 12.12 format
|
1105 |
*/
|
1106 |
|
1107 |
uint32_t timeout_mclks = 0;
|
1108 |
uint16_t timeout_encoded = 0;
|
1109 |
|
1110 |
LOG_FUNCTION_START("");
|
1111 |
|
1112 |
timeout_mclks = |
1113 |
VL53L1_calc_timeout_mclks(timeout_us, macro_period_us); |
1114 |
|
1115 |
timeout_encoded = |
1116 |
VL53L1_encode_timeout(timeout_mclks); |
1117 |
|
1118 |
#ifdef VL53L1_LOGGING
|
1119 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
1120 |
" %-48s : %10u (0x%04X)\n", "timeout_mclks", |
1121 |
timeout_mclks, timeout_mclks); |
1122 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
1123 |
" %-48s : %10u (0x%04X)\n", "timeout_encoded", |
1124 |
timeout_encoded, timeout_encoded); |
1125 |
#endif
|
1126 |
|
1127 |
LOG_FUNCTION_END(0);
|
1128 |
|
1129 |
return timeout_encoded;
|
1130 |
} |
1131 |
|
1132 |
|
1133 |
uint32_t VL53L1_calc_timeout_us( |
1134 |
uint32_t timeout_mclks, |
1135 |
uint32_t macro_period_us) |
1136 |
{ |
1137 |
/* Calculates the timeout in [us] based on the input
|
1138 |
* encoded timeout and the macro period in [us]
|
1139 |
*
|
1140 |
* Max timeout supported is 1000000 us (1 sec) -> 20-bits
|
1141 |
* Max timeout in 20.12 format = 32-bits
|
1142 |
*
|
1143 |
* Macro period [us] = 12.12 format
|
1144 |
*/
|
1145 |
|
1146 |
uint32_t timeout_us = 0;
|
1147 |
uint64_t tmp = 0;
|
1148 |
|
1149 |
LOG_FUNCTION_START("");
|
1150 |
|
1151 |
tmp = (uint64_t)timeout_mclks * (uint64_t)macro_period_us; |
1152 |
tmp += 0x00800;
|
1153 |
tmp = tmp >> 12;
|
1154 |
|
1155 |
timeout_us = (uint32_t)tmp; |
1156 |
|
1157 |
#ifdef VL53L1_LOGGING
|
1158 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
1159 |
" %-48s : %10u (0x%04X)\n", "timeout_mclks", |
1160 |
timeout_mclks, timeout_mclks); |
1161 |
|
1162 |
trace_print(VL53L1_TRACE_LEVEL_DEBUG, |
1163 |
" %-48s : %10u us\n", "timeout_us", |
1164 |
timeout_us, timeout_us); |
1165 |
#endif
|
1166 |
|
1167 |
LOG_FUNCTION_END(0);
|
1168 |
|
1169 |
return timeout_us;
|
1170 |
} |
1171 |
|
1172 |
uint32_t VL53L1_calc_crosstalk_plane_offset_with_margin( |
1173 |
uint32_t plane_offset_kcps, |
1174 |
int16_t margin_offset_kcps) |
1175 |
{ |
1176 |
uint32_t plane_offset_with_margin = 0;
|
1177 |
int32_t plane_offset_kcps_temp = 0;
|
1178 |
|
1179 |
LOG_FUNCTION_START("");
|
1180 |
|
1181 |
plane_offset_kcps_temp = |
1182 |
(int32_t)plane_offset_kcps + |
1183 |
(int32_t)margin_offset_kcps; |
1184 |
|
1185 |
if (plane_offset_kcps_temp < 0) { |
1186 |
plane_offset_kcps_temp = 0;
|
1187 |
} else {
|
1188 |
if (plane_offset_kcps_temp > 0x3FFFF) { |
1189 |
plane_offset_kcps_temp = 0x3FFFF;
|
1190 |
} |
1191 |
} |
1192 |
|
1193 |
plane_offset_with_margin = (uint32_t) plane_offset_kcps_temp; |
1194 |
|
1195 |
LOG_FUNCTION_END(0);
|
1196 |
|
1197 |
return plane_offset_with_margin;
|
1198 |
|
1199 |
} |
1200 |
|
1201 |
uint32_t VL53L1_calc_decoded_timeout_us( |
1202 |
uint16_t timeout_encoded, |
1203 |
uint32_t macro_period_us) |
1204 |
{ |
1205 |
/* Calculates the timeout in [us] based on the input
|
1206 |
* encoded timeout and the macro period in [us]
|
1207 |
*
|
1208 |
* Max timeout supported is 1000000 us (1 sec) -> 20-bits
|
1209 |
* Max timeout in 20.12 format = 32-bits
|
1210 |
*
|
1211 |
* Macro period [us] = 12.12 format
|
1212 |
*/
|
1213 |
|
1214 |
uint32_t timeout_mclks = 0;
|
1215 |
uint32_t timeout_us = 0;
|
1216 |
|
1217 |
LOG_FUNCTION_START("");
|
1218 |
|
1219 |
timeout_mclks = |
1220 |
VL53L1_decode_timeout(timeout_encoded); |
1221 |
|
1222 |
timeout_us = |
1223 |
VL53L1_calc_timeout_us(timeout_mclks, macro_period_us); |
1224 |
|
1225 |
LOG_FUNCTION_END(0);
|
1226 |
|
1227 |
return timeout_us;
|
1228 |
} |
1229 |
|
1230 |
|
1231 |
uint16_t VL53L1_encode_timeout(uint32_t timeout_mclks) |
1232 |
{ |
1233 |
/*
|
1234 |
* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
|
1235 |
*/
|
1236 |
|
1237 |
uint16_t encoded_timeout = 0;
|
1238 |
uint32_t ls_byte = 0;
|
1239 |
uint16_t ms_byte = 0;
|
1240 |
|
1241 |
if (timeout_mclks > 0) { |
1242 |
ls_byte = timeout_mclks - 1;
|
1243 |
|
1244 |
while ((ls_byte & 0xFFFFFF00) > 0) { |
1245 |
ls_byte = ls_byte >> 1;
|
1246 |
ms_byte++; |
1247 |
} |
1248 |
|
1249 |
encoded_timeout = (ms_byte << 8)
|
1250 |
+ (uint16_t) (ls_byte & 0x000000FF);
|
1251 |
} |
1252 |
|
1253 |
return encoded_timeout;
|
1254 |
} |
1255 |
|
1256 |
|
1257 |
uint32_t VL53L1_decode_timeout(uint16_t encoded_timeout) |
1258 |
{ |
1259 |
/*
|
1260 |
* Decode 16-bit timeout register value
|
1261 |
* format (LSByte * 2^MSByte) + 1
|
1262 |
*/
|
1263 |
|
1264 |
uint32_t timeout_macro_clks = 0;
|
1265 |
|
1266 |
timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
|
1267 |
<< (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1; |
1268 |
|
1269 |
return timeout_macro_clks;
|
1270 |
} |
1271 |
|
1272 |
|
1273 |
VL53L1_Error VL53L1_calc_timeout_register_values( |
1274 |
uint32_t phasecal_config_timeout_us, |
1275 |
uint32_t mm_config_timeout_us, |
1276 |
uint32_t range_config_timeout_us, |
1277 |
uint16_t fast_osc_frequency, |
1278 |
VL53L1_general_config_t *pgeneral, |
1279 |
VL53L1_timing_config_t *ptiming) |
1280 |
{ |
1281 |
/*
|
1282 |
* Converts the input MM and range timeouts in [us]
|
1283 |
* into the appropriate register values
|
1284 |
*
|
1285 |
* Must also be run after the VCSEL period settings are changed
|
1286 |
*/
|
1287 |
|
1288 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
1289 |
|
1290 |
uint32_t macro_period_us = 0;
|
1291 |
uint32_t timeout_mclks = 0;
|
1292 |
uint16_t timeout_encoded = 0;
|
1293 |
|
1294 |
LOG_FUNCTION_START("");
|
1295 |
|
1296 |
if (fast_osc_frequency == 0) { |
1297 |
status = VL53L1_ERROR_DIVISION_BY_ZERO; |
1298 |
} else {
|
1299 |
/* Update Macro Period for Range A VCSEL Period */
|
1300 |
macro_period_us = |
1301 |
VL53L1_calc_macro_period_us( |
1302 |
fast_osc_frequency, |
1303 |
ptiming->range_config__vcsel_period_a); |
1304 |
|
1305 |
/* Update Phase timeout - uses Timing A */
|
1306 |
timeout_mclks = |
1307 |
VL53L1_calc_timeout_mclks( |
1308 |
phasecal_config_timeout_us, |
1309 |
macro_period_us); |
1310 |
|
1311 |
/* clip as the phase cal timeout register is only 8-bits */
|
1312 |
if (timeout_mclks > 0xFF) |
1313 |
timeout_mclks = 0xFF;
|
1314 |
|
1315 |
pgeneral->phasecal_config__timeout_macrop = |
1316 |
(uint8_t)timeout_mclks; |
1317 |
|
1318 |
/* Update MM Timing A timeout */
|
1319 |
timeout_encoded = |
1320 |
VL53L1_calc_encoded_timeout( |
1321 |
mm_config_timeout_us, |
1322 |
macro_period_us); |
1323 |
|
1324 |
ptiming->mm_config__timeout_macrop_a_hi = |
1325 |
(uint8_t)((timeout_encoded & 0xFF00) >> 8); |
1326 |
ptiming->mm_config__timeout_macrop_a_lo = |
1327 |
(uint8_t) (timeout_encoded & 0x00FF);
|
1328 |
|
1329 |
/* Update Range Timing A timeout */
|
1330 |
timeout_encoded = |
1331 |
VL53L1_calc_encoded_timeout( |
1332 |
range_config_timeout_us, |
1333 |
macro_period_us); |
1334 |
|
1335 |
ptiming->range_config__timeout_macrop_a_hi = |
1336 |
(uint8_t)((timeout_encoded & 0xFF00) >> 8); |
1337 |
ptiming->range_config__timeout_macrop_a_lo = |
1338 |
(uint8_t) (timeout_encoded & 0x00FF);
|
1339 |
|
1340 |
/* Update Macro Period for Range B VCSEL Period */
|
1341 |
macro_period_us = |
1342 |
VL53L1_calc_macro_period_us( |
1343 |
fast_osc_frequency, |
1344 |
ptiming->range_config__vcsel_period_b); |
1345 |
|
1346 |
/* Update MM Timing B timeout */
|
1347 |
timeout_encoded = |
1348 |
VL53L1_calc_encoded_timeout( |
1349 |
mm_config_timeout_us, |
1350 |
macro_period_us); |
1351 |
|
1352 |
ptiming->mm_config__timeout_macrop_b_hi = |
1353 |
(uint8_t)((timeout_encoded & 0xFF00) >> 8); |
1354 |
ptiming->mm_config__timeout_macrop_b_lo = |
1355 |
(uint8_t) (timeout_encoded & 0x00FF);
|
1356 |
|
1357 |
/* Update Range Timing B timeout */
|
1358 |
timeout_encoded = VL53L1_calc_encoded_timeout( |
1359 |
range_config_timeout_us, |
1360 |
macro_period_us); |
1361 |
|
1362 |
ptiming->range_config__timeout_macrop_b_hi = |
1363 |
(uint8_t)((timeout_encoded & 0xFF00) >> 8); |
1364 |
ptiming->range_config__timeout_macrop_b_lo = |
1365 |
(uint8_t) (timeout_encoded & 0x00FF);
|
1366 |
} |
1367 |
|
1368 |
LOG_FUNCTION_END(0);
|
1369 |
|
1370 |
return status;
|
1371 |
|
1372 |
} |
1373 |
|
1374 |
|
1375 |
uint8_t VL53L1_encode_vcsel_period(uint8_t vcsel_period_pclks) |
1376 |
{ |
1377 |
/*
|
1378 |
* Converts the encoded VCSEL period register value into
|
1379 |
* the real period in PLL clocks
|
1380 |
*/
|
1381 |
|
1382 |
uint8_t vcsel_period_reg = 0;
|
1383 |
|
1384 |
vcsel_period_reg = (vcsel_period_pclks >> 1) - 1; |
1385 |
|
1386 |
return vcsel_period_reg;
|
1387 |
} |
1388 |
|
1389 |
|
1390 |
uint32_t VL53L1_decode_unsigned_integer( |
1391 |
uint8_t *pbuffer, |
1392 |
uint8_t no_of_bytes) |
1393 |
{ |
1394 |
/*
|
1395 |
* Decodes a integer number from the buffer
|
1396 |
*/
|
1397 |
|
1398 |
uint8_t i = 0;
|
1399 |
uint32_t decoded_value = 0;
|
1400 |
|
1401 |
for (i = 0 ; i < no_of_bytes ; i++) { |
1402 |
decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i];
|
1403 |
} |
1404 |
|
1405 |
return decoded_value;
|
1406 |
} |
1407 |
|
1408 |
|
1409 |
void VL53L1_encode_unsigned_integer(
|
1410 |
uint32_t ip_value, |
1411 |
uint8_t no_of_bytes, |
1412 |
uint8_t *pbuffer) |
1413 |
{ |
1414 |
/*
|
1415 |
* Encodes an integer number into the buffer
|
1416 |
*/
|
1417 |
|
1418 |
uint8_t i = 0;
|
1419 |
uint32_t data = 0;
|
1420 |
|
1421 |
data = ip_value; |
1422 |
for (i = 0; i < no_of_bytes ; i++) { |
1423 |
pbuffer[no_of_bytes-i-1] = data & 0x00FF; |
1424 |
data = data >> 8;
|
1425 |
} |
1426 |
} |
1427 |
|
1428 |
|
1429 |
void VL53L1_spad_number_to_byte_bit_index(
|
1430 |
uint8_t spad_number, |
1431 |
uint8_t *pbyte_index, |
1432 |
uint8_t *pbit_index, |
1433 |
uint8_t *pbit_mask) |
1434 |
{ |
1435 |
|
1436 |
/**
|
1437 |
* Converts the input SPAD number into the SPAD Enable byte index, bit index and bit mask
|
1438 |
*
|
1439 |
* byte_index = (spad_no >> 3)
|
1440 |
* bit_index = spad_no & 0x07
|
1441 |
* bit_mask = 0x01 << bit_index
|
1442 |
*/
|
1443 |
|
1444 |
*pbyte_index = spad_number >> 3;
|
1445 |
*pbit_index = spad_number & 0x07;
|
1446 |
*pbit_mask = 0x01 << *pbit_index;
|
1447 |
|
1448 |
} |
1449 |
|
1450 |
|
1451 |
void VL53L1_encode_row_col(
|
1452 |
uint8_t row, |
1453 |
uint8_t col, |
1454 |
uint8_t *pspad_number) |
1455 |
{ |
1456 |
/**
|
1457 |
* Encodes the input array(row,col) location as SPAD number.
|
1458 |
*/
|
1459 |
|
1460 |
if (row > 7) { |
1461 |
*pspad_number = 128 + (col << 3) + (15-row); |
1462 |
} else {
|
1463 |
*pspad_number = ((15-col) << 3) + row; |
1464 |
} |
1465 |
} |
1466 |
|
1467 |
|
1468 |
void VL53L1_decode_zone_size(
|
1469 |
uint8_t encoded_xy_size, |
1470 |
uint8_t *pwidth, |
1471 |
uint8_t *pheight) |
1472 |
{ |
1473 |
|
1474 |
/* extract x and y sizes
|
1475 |
*
|
1476 |
* Important: the sense of the device width and height is swapped
|
1477 |
* versus the API sense
|
1478 |
*
|
1479 |
* MS Nibble = height
|
1480 |
* LS Nibble = width
|
1481 |
*/
|
1482 |
|
1483 |
*pheight = encoded_xy_size >> 4;
|
1484 |
*pwidth = encoded_xy_size & 0x0F;
|
1485 |
|
1486 |
} |
1487 |
|
1488 |
|
1489 |
void VL53L1_encode_zone_size(
|
1490 |
uint8_t width, |
1491 |
uint8_t height, |
1492 |
uint8_t *pencoded_xy_size) |
1493 |
{ |
1494 |
/* merge x and y sizes
|
1495 |
*
|
1496 |
* Important: the sense of the device width and height is swapped
|
1497 |
* versus the API sense
|
1498 |
*
|
1499 |
* MS Nibble = height
|
1500 |
* LS Nibble = width
|
1501 |
*/
|
1502 |
|
1503 |
*pencoded_xy_size = (height << 4) + width;
|
1504 |
|
1505 |
} |
1506 |
|
1507 |
|
1508 |
void VL53L1_decode_zone_limits(
|
1509 |
uint8_t encoded_xy_centre, |
1510 |
uint8_t encoded_xy_size, |
1511 |
int16_t *px_ll, |
1512 |
int16_t *py_ll, |
1513 |
int16_t *px_ur, |
1514 |
int16_t *py_ur) |
1515 |
{ |
1516 |
|
1517 |
/*
|
1518 |
* compute zone lower left and upper right limits
|
1519 |
*
|
1520 |
* centre (8,8) width = 16, height = 16 -> (0,0) -> (15,15)
|
1521 |
* centre (8,8) width = 14, height = 16 -> (1,0) -> (14,15)
|
1522 |
*/
|
1523 |
|
1524 |
uint8_t x_centre = 0;
|
1525 |
uint8_t y_centre = 0;
|
1526 |
uint8_t width = 0;
|
1527 |
uint8_t height = 0;
|
1528 |
|
1529 |
/* decode zone centre and size information */
|
1530 |
|
1531 |
VL53L1_decode_row_col( |
1532 |
encoded_xy_centre, |
1533 |
&y_centre, |
1534 |
&x_centre); |
1535 |
|
1536 |
VL53L1_decode_zone_size( |
1537 |
encoded_xy_size, |
1538 |
&width, |
1539 |
&height); |
1540 |
|
1541 |
/* compute bounds and clip */
|
1542 |
|
1543 |
*px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2; |
1544 |
if (*px_ll < 0) |
1545 |
*px_ll = 0;
|
1546 |
|
1547 |
*px_ur = *px_ll + (int16_t)width; |
1548 |
if (*px_ur > (VL53L1_SPAD_ARRAY_WIDTH-1)) |
1549 |
*px_ur = VL53L1_SPAD_ARRAY_WIDTH-1;
|
1550 |
|
1551 |
*py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2; |
1552 |
if (*py_ll < 0) |
1553 |
*py_ll = 0;
|
1554 |
|
1555 |
*py_ur = *py_ll + (int16_t)height; |
1556 |
if (*py_ur > (VL53L1_SPAD_ARRAY_HEIGHT-1)) |
1557 |
*py_ur = VL53L1_SPAD_ARRAY_HEIGHT-1;
|
1558 |
} |
1559 |
|
1560 |
|
1561 |
uint8_t VL53L1_is_aperture_location( |
1562 |
uint8_t row, |
1563 |
uint8_t col) |
1564 |
{ |
1565 |
/*
|
1566 |
* Returns > 0 if input (row,col) location is an aperture
|
1567 |
*/
|
1568 |
|
1569 |
uint8_t is_aperture = 0;
|
1570 |
uint8_t mod_row = row % 4;
|
1571 |
uint8_t mod_col = col % 4;
|
1572 |
|
1573 |
if (mod_row == 0 && mod_col == 2) |
1574 |
is_aperture = 1;
|
1575 |
|
1576 |
if (mod_row == 2 && mod_col == 0) |
1577 |
is_aperture = 1;
|
1578 |
|
1579 |
return is_aperture;
|
1580 |
} |
1581 |
|
1582 |
|
1583 |
void VL53L1_calc_mm_effective_spads(
|
1584 |
uint8_t encoded_mm_roi_centre, |
1585 |
uint8_t encoded_mm_roi_size, |
1586 |
uint8_t encoded_zone_centre, |
1587 |
uint8_t encoded_zone_size, |
1588 |
uint8_t *pgood_spads, |
1589 |
uint16_t aperture_attenuation, |
1590 |
uint16_t *pmm_inner_effective_spads, |
1591 |
uint16_t *pmm_outer_effective_spads) |
1592 |
{ |
1593 |
|
1594 |
/* Calculates the effective SPAD counts for the MM inner and outer
|
1595 |
* regions based on the input MM ROI, Zone info and return good
|
1596 |
* SPAD map
|
1597 |
*/
|
1598 |
|
1599 |
int16_t x = 0;
|
1600 |
int16_t y = 0;
|
1601 |
|
1602 |
int16_t mm_x_ll = 0;
|
1603 |
int16_t mm_y_ll = 0;
|
1604 |
int16_t mm_x_ur = 0;
|
1605 |
int16_t mm_y_ur = 0;
|
1606 |
|
1607 |
int16_t zone_x_ll = 0;
|
1608 |
int16_t zone_y_ll = 0;
|
1609 |
int16_t zone_x_ur = 0;
|
1610 |
int16_t zone_y_ur = 0;
|
1611 |
|
1612 |
uint8_t spad_number = 0;
|
1613 |
uint8_t byte_index = 0;
|
1614 |
uint8_t bit_index = 0;
|
1615 |
uint8_t bit_mask = 0;
|
1616 |
|
1617 |
uint8_t is_aperture = 0;
|
1618 |
uint16_t spad_attenuation = 0;
|
1619 |
|
1620 |
/* decode the MM ROI and Zone limits */
|
1621 |
|
1622 |
VL53L1_decode_zone_limits( |
1623 |
encoded_mm_roi_centre, |
1624 |
encoded_mm_roi_size, |
1625 |
&mm_x_ll, |
1626 |
&mm_y_ll, |
1627 |
&mm_x_ur, |
1628 |
&mm_y_ur); |
1629 |
|
1630 |
VL53L1_decode_zone_limits( |
1631 |
encoded_zone_centre, |
1632 |
encoded_zone_size, |
1633 |
&zone_x_ll, |
1634 |
&zone_y_ll, |
1635 |
&zone_x_ur, |
1636 |
&zone_y_ur); |
1637 |
|
1638 |
/*
|
1639 |
* Loop though all SPAD within the zone. Check if it is
|
1640 |
* a good SPAD then add the transmission value to either
|
1641 |
* the inner or outer effective SPAD count dependent if
|
1642 |
* the SPAD lies within the MM ROI.
|
1643 |
*/
|
1644 |
|
1645 |
*pmm_inner_effective_spads = 0;
|
1646 |
*pmm_outer_effective_spads = 0;
|
1647 |
|
1648 |
for (y = zone_y_ll ; y <= zone_y_ur ; y++) {
|
1649 |
for (x = zone_x_ll ; x <= zone_x_ur ; x++) {
|
1650 |
|
1651 |
/* Convert location into SPAD number */
|
1652 |
|
1653 |
VL53L1_encode_row_col( |
1654 |
(uint8_t)y, |
1655 |
(uint8_t)x, |
1656 |
&spad_number); |
1657 |
|
1658 |
/* Convert spad number into byte and bit index
|
1659 |
* this is required to look up the appropriate
|
1660 |
* SPAD enable bit with the 32-byte good SPAD
|
1661 |
* enable buffer
|
1662 |
*/
|
1663 |
|
1664 |
VL53L1_spad_number_to_byte_bit_index( |
1665 |
spad_number, |
1666 |
&byte_index, |
1667 |
&bit_index, |
1668 |
&bit_mask); |
1669 |
|
1670 |
/* If spad is good then add it */
|
1671 |
|
1672 |
if ((pgood_spads[byte_index] & bit_mask) > 0) { |
1673 |
/* determine if apertured SPAD or not */
|
1674 |
|
1675 |
is_aperture = VL53L1_is_aperture_location( |
1676 |
(uint8_t)y, |
1677 |
(uint8_t)x); |
1678 |
|
1679 |
if (is_aperture > 0) |
1680 |
spad_attenuation = aperture_attenuation; |
1681 |
else
|
1682 |
spad_attenuation = 0x0100;
|
1683 |
|
1684 |
/*
|
1685 |
* if inside MM roi add to inner effective SPAD count
|
1686 |
* otherwise add to outer effective SPAD Count
|
1687 |
*/
|
1688 |
|
1689 |
if (x >= mm_x_ll && x <= mm_x_ur &&
|
1690 |
y >= mm_y_ll && y <= mm_y_ur) |
1691 |
*pmm_inner_effective_spads += |
1692 |
spad_attenuation; |
1693 |
else
|
1694 |
*pmm_outer_effective_spads += |
1695 |
spad_attenuation; |
1696 |
} |
1697 |
} |
1698 |
} |
1699 |
} |
1700 |
|
1701 |
|
1702 |
/*
|
1703 |
* Encodes VL53L1_GPIO_interrupt_config_t structure to FW register format
|
1704 |
*/
|
1705 |
|
1706 |
uint8_t VL53L1_encode_GPIO_interrupt_config( |
1707 |
VL53L1_GPIO_interrupt_config_t *pintconf) |
1708 |
{ |
1709 |
uint8_t system__interrupt_config; |
1710 |
|
1711 |
system__interrupt_config = pintconf->intr_mode_distance; |
1712 |
system__interrupt_config |= ((pintconf->intr_mode_rate) << 2);
|
1713 |
system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5);
|
1714 |
system__interrupt_config |= ((pintconf->intr_no_target) << 6);
|
1715 |
system__interrupt_config |= ((pintconf->intr_combined_mode) << 7);
|
1716 |
|
1717 |
return system__interrupt_config;
|
1718 |
} |
1719 |
|
1720 |
/*
|
1721 |
* Decodes FW register to VL53L1_GPIO_interrupt_config_t structure
|
1722 |
*/
|
1723 |
|
1724 |
VL53L1_GPIO_interrupt_config_t VL53L1_decode_GPIO_interrupt_config( |
1725 |
uint8_t system__interrupt_config) |
1726 |
{ |
1727 |
VL53L1_GPIO_interrupt_config_t intconf; |
1728 |
|
1729 |
intconf.intr_mode_distance = system__interrupt_config & 0x03;
|
1730 |
intconf.intr_mode_rate = (system__interrupt_config >> 2) & 0x03; |
1731 |
intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01; |
1732 |
intconf.intr_no_target = (system__interrupt_config >> 6) & 0x01; |
1733 |
intconf.intr_combined_mode = (system__interrupt_config >> 7) & 0x01; |
1734 |
|
1735 |
/* set some default values */
|
1736 |
intconf.threshold_rate_low = 0;
|
1737 |
intconf.threshold_rate_high = 0;
|
1738 |
intconf.threshold_distance_low = 0;
|
1739 |
intconf.threshold_distance_high = 0;
|
1740 |
|
1741 |
return intconf;
|
1742 |
} |
1743 |
|
1744 |
/*
|
1745 |
* Set GPIO distance threshold
|
1746 |
*/
|
1747 |
|
1748 |
VL53L1_Error VL53L1_set_GPIO_distance_threshold( |
1749 |
VL53L1_DEV Dev, |
1750 |
uint16_t threshold_high, |
1751 |
uint16_t threshold_low) |
1752 |
{ |
1753 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
1754 |
|
1755 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
1756 |
|
1757 |
LOG_FUNCTION_START("");
|
1758 |
|
1759 |
pdev->dyn_cfg.system__thresh_high = threshold_high; |
1760 |
pdev->dyn_cfg.system__thresh_low = threshold_low; |
1761 |
|
1762 |
LOG_FUNCTION_END(status); |
1763 |
return status;
|
1764 |
} |
1765 |
|
1766 |
/*
|
1767 |
* Set GPIO rate threshold
|
1768 |
*/
|
1769 |
|
1770 |
VL53L1_Error VL53L1_set_GPIO_rate_threshold( |
1771 |
VL53L1_DEV Dev, |
1772 |
uint16_t threshold_high, |
1773 |
uint16_t threshold_low) |
1774 |
{ |
1775 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
1776 |
|
1777 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
1778 |
|
1779 |
LOG_FUNCTION_START("");
|
1780 |
|
1781 |
pdev->gen_cfg.system__thresh_rate_high = threshold_high; |
1782 |
pdev->gen_cfg.system__thresh_rate_low = threshold_low; |
1783 |
|
1784 |
LOG_FUNCTION_END(status); |
1785 |
return status;
|
1786 |
} |
1787 |
|
1788 |
/*
|
1789 |
* Set GPIO thresholds from structure
|
1790 |
*/
|
1791 |
|
1792 |
VL53L1_Error VL53L1_set_GPIO_thresholds_from_struct( |
1793 |
VL53L1_DEV Dev, |
1794 |
VL53L1_GPIO_interrupt_config_t *pintconf) |
1795 |
{ |
1796 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
1797 |
|
1798 |
LOG_FUNCTION_START("");
|
1799 |
|
1800 |
status = VL53L1_set_GPIO_distance_threshold( |
1801 |
Dev, |
1802 |
pintconf->threshold_distance_high, |
1803 |
pintconf->threshold_distance_low); |
1804 |
|
1805 |
if (status == VL53L1_ERROR_NONE) {
|
1806 |
status = |
1807 |
VL53L1_set_GPIO_rate_threshold( |
1808 |
Dev, |
1809 |
pintconf->threshold_rate_high, |
1810 |
pintconf->threshold_rate_low); |
1811 |
} |
1812 |
|
1813 |
LOG_FUNCTION_END(status); |
1814 |
return status;
|
1815 |
} |
1816 |
|
1817 |
|
1818 |
#ifndef VL53L1_NOCALIB
|
1819 |
VL53L1_Error VL53L1_set_ref_spad_char_config( |
1820 |
VL53L1_DEV Dev, |
1821 |
uint8_t vcsel_period_a, |
1822 |
uint32_t phasecal_timeout_us, |
1823 |
uint16_t total_rate_target_mcps, |
1824 |
uint16_t max_count_rate_rtn_limit_mcps, |
1825 |
uint16_t min_count_rate_rtn_limit_mcps, |
1826 |
uint16_t fast_osc_frequency) |
1827 |
{ |
1828 |
/*
|
1829 |
* Initialises the VCSEL period A and phasecal timeout registers
|
1830 |
* for the Reference SPAD Characterisation test
|
1831 |
*/
|
1832 |
|
1833 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
1834 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
1835 |
|
1836 |
uint8_t buffer[2];
|
1837 |
|
1838 |
uint32_t macro_period_us = 0;
|
1839 |
uint32_t timeout_mclks = 0;
|
1840 |
|
1841 |
LOG_FUNCTION_START("");
|
1842 |
|
1843 |
/*
|
1844 |
* Update Macro Period for Range A VCSEL Period
|
1845 |
*/
|
1846 |
macro_period_us = |
1847 |
VL53L1_calc_macro_period_us( |
1848 |
fast_osc_frequency, |
1849 |
vcsel_period_a); |
1850 |
|
1851 |
/*
|
1852 |
* Calculate PhaseCal timeout and clip to max of 255 macro periods
|
1853 |
*/
|
1854 |
|
1855 |
timeout_mclks = phasecal_timeout_us << 12;
|
1856 |
timeout_mclks = timeout_mclks + (macro_period_us>>1);
|
1857 |
timeout_mclks = timeout_mclks / macro_period_us; |
1858 |
|
1859 |
if (timeout_mclks > 0xFF) |
1860 |
pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF;
|
1861 |
else
|
1862 |
pdev->gen_cfg.phasecal_config__timeout_macrop = |
1863 |
(uint8_t)timeout_mclks; |
1864 |
|
1865 |
pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a; |
1866 |
|
1867 |
/*
|
1868 |
* Update device settings
|
1869 |
*/
|
1870 |
|
1871 |
if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/ |
1872 |
status = |
1873 |
VL53L1_WrByte( |
1874 |
Dev, |
1875 |
VL53L1_PHASECAL_CONFIG__TIMEOUT_MACROP, |
1876 |
pdev->gen_cfg.phasecal_config__timeout_macrop); |
1877 |
|
1878 |
if (status == VL53L1_ERROR_NONE)
|
1879 |
status = |
1880 |
VL53L1_WrByte( |
1881 |
Dev, |
1882 |
VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A, |
1883 |
pdev->tim_cfg.range_config__vcsel_period_a); |
1884 |
|
1885 |
/*
|
1886 |
* Copy vcsel register value to the WOI registers to ensure that
|
1887 |
* it is correctly set for the specified VCSEL period
|
1888 |
*/
|
1889 |
|
1890 |
buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a;
|
1891 |
buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a;
|
1892 |
|
1893 |
if (status == VL53L1_ERROR_NONE)
|
1894 |
status = |
1895 |
VL53L1_WriteMulti( |
1896 |
Dev, |
1897 |
VL53L1_SD_CONFIG__WOI_SD0, |
1898 |
buffer, |
1899 |
2); /* It should be be replaced with a define */ |
1900 |
|
1901 |
/*
|
1902 |
* Set min, target and max rate limits
|
1903 |
*/
|
1904 |
|
1905 |
pdev->customer.ref_spad_char__total_rate_target_mcps = |
1906 |
total_rate_target_mcps; |
1907 |
|
1908 |
if (status == VL53L1_ERROR_NONE)
|
1909 |
status = |
1910 |
VL53L1_WrWord( |
1911 |
Dev, |
1912 |
VL53L1_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS, |
1913 |
total_rate_target_mcps); /* 9.7 format */
|
1914 |
|
1915 |
if (status == VL53L1_ERROR_NONE)
|
1916 |
status = |
1917 |
VL53L1_WrWord( |
1918 |
Dev, |
1919 |
VL53L1_RANGE_CONFIG__SIGMA_THRESH, |
1920 |
max_count_rate_rtn_limit_mcps); |
1921 |
|
1922 |
if (status == VL53L1_ERROR_NONE)
|
1923 |
status = |
1924 |
VL53L1_WrWord( |
1925 |
Dev, |
1926 |
VL53L1_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, |
1927 |
min_count_rate_rtn_limit_mcps); |
1928 |
|
1929 |
LOG_FUNCTION_END(status); |
1930 |
|
1931 |
return status;
|
1932 |
} |
1933 |
|
1934 |
|
1935 |
VL53L1_Error VL53L1_set_ssc_config( |
1936 |
VL53L1_DEV Dev, |
1937 |
VL53L1_ssc_config_t *pssc_cfg, |
1938 |
uint16_t fast_osc_frequency) |
1939 |
{ |
1940 |
/**
|
1941 |
* Builds and sends a single I2C multiple byte transaction to
|
1942 |
* initialize the device for SSC.
|
1943 |
*
|
1944 |
* The function also sets the WOI registers based on the input
|
1945 |
* vcsel period register value.
|
1946 |
*/
|
1947 |
|
1948 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
1949 |
uint8_t buffer[5];
|
1950 |
|
1951 |
uint32_t macro_period_us = 0;
|
1952 |
uint16_t timeout_encoded = 0;
|
1953 |
|
1954 |
LOG_FUNCTION_START("");
|
1955 |
|
1956 |
/*
|
1957 |
* Update Macro Period for Range A VCSEL Period
|
1958 |
*/
|
1959 |
macro_period_us = |
1960 |
VL53L1_calc_macro_period_us( |
1961 |
fast_osc_frequency, |
1962 |
pssc_cfg->vcsel_period); |
1963 |
|
1964 |
/*
|
1965 |
* Update MM Timing A timeout
|
1966 |
*/
|
1967 |
timeout_encoded = |
1968 |
VL53L1_calc_encoded_timeout( |
1969 |
pssc_cfg->timeout_us, |
1970 |
macro_period_us); |
1971 |
|
1972 |
/* update VCSEL timings */
|
1973 |
|
1974 |
if (status == VL53L1_ERROR_NONE)
|
1975 |
status = |
1976 |
VL53L1_WrByte( |
1977 |
Dev, |
1978 |
VL53L1_CAL_CONFIG__VCSEL_START, |
1979 |
pssc_cfg->vcsel_start); |
1980 |
|
1981 |
if (status == VL53L1_ERROR_NONE)
|
1982 |
status = |
1983 |
VL53L1_WrByte( |
1984 |
Dev, |
1985 |
VL53L1_GLOBAL_CONFIG__VCSEL_WIDTH, |
1986 |
pssc_cfg->vcsel_width); |
1987 |
|
1988 |
/* build buffer for timeouts, period and rate limit */
|
1989 |
|
1990 |
buffer[0] = (uint8_t)((timeout_encoded & 0x0000FF00) >> 8); |
1991 |
buffer[1] = (uint8_t) (timeout_encoded & 0x000000FF); |
1992 |
buffer[2] = pssc_cfg->vcsel_period;
|
1993 |
buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8); |
1994 |
buffer[4] = (uint8_t) (pssc_cfg->rate_limit_mcps & 0x000000FF); |
1995 |
|
1996 |
if (status == VL53L1_ERROR_NONE)
|
1997 |
status = |
1998 |
VL53L1_WriteMulti( |
1999 |
Dev, |
2000 |
VL53L1_RANGE_CONFIG__TIMEOUT_MACROP_B_HI, |
2001 |
buffer, |
2002 |
5);
|
2003 |
|
2004 |
/*
|
2005 |
* Copy vcsel register value to the WOI registers to ensure that
|
2006 |
* it is correctly set for the specified VCSEL period
|
2007 |
*/
|
2008 |
|
2009 |
buffer[0] = pssc_cfg->vcsel_period;
|
2010 |
buffer[1] = pssc_cfg->vcsel_period;
|
2011 |
|
2012 |
if (status == VL53L1_ERROR_NONE)
|
2013 |
status = |
2014 |
VL53L1_WriteMulti( |
2015 |
Dev, |
2016 |
VL53L1_SD_CONFIG__WOI_SD0, |
2017 |
buffer, |
2018 |
2);
|
2019 |
|
2020 |
/*
|
2021 |
* Write zero to NVM_BIST_CTRL to send RTN CountRate to Patch RAM
|
2022 |
* or 1 to write REF CountRate to Patch RAM
|
2023 |
*/
|
2024 |
if (status == VL53L1_ERROR_NONE)
|
2025 |
status = |
2026 |
VL53L1_WrByte( |
2027 |
Dev, |
2028 |
VL53L1_NVM_BIST__CTRL, |
2029 |
pssc_cfg->array_select); |
2030 |
|
2031 |
LOG_FUNCTION_END(status); |
2032 |
|
2033 |
return status;
|
2034 |
} |
2035 |
#endif
|
2036 |
|
2037 |
|
2038 |
#ifndef VL53L1_NOCALIB
|
2039 |
VL53L1_Error VL53L1_get_spad_rate_data( |
2040 |
VL53L1_DEV Dev, |
2041 |
VL53L1_spad_rate_data_t *pspad_rates) |
2042 |
{ |
2043 |
|
2044 |
/**
|
2045 |
* Gets the SSC rate map output
|
2046 |
*/
|
2047 |
|
2048 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
2049 |
int i = 0; |
2050 |
|
2051 |
uint8_t data[512];
|
2052 |
uint8_t *pdata = &data[0];
|
2053 |
|
2054 |
LOG_FUNCTION_START("");
|
2055 |
|
2056 |
/* Disable Firmware to Read Patch Ram */
|
2057 |
|
2058 |
if (status == VL53L1_ERROR_NONE)
|
2059 |
status = VL53L1_disable_firmware(Dev); |
2060 |
|
2061 |
/*
|
2062 |
* Read Return SPADs Rates from patch RAM.
|
2063 |
* Note : platform layer splits the I2C comms into smaller chunks
|
2064 |
*/
|
2065 |
|
2066 |
if (status == VL53L1_ERROR_NONE)
|
2067 |
status = |
2068 |
VL53L1_ReadMulti( |
2069 |
Dev, |
2070 |
VL53L1_PRIVATE__PATCH_BASE_ADDR_RSLV, |
2071 |
pdata, |
2072 |
512);
|
2073 |
|
2074 |
/* now convert into 16-bit number */
|
2075 |
pdata = &data[0];
|
2076 |
for (i = 0 ; i < VL53L1_NO_OF_SPAD_ENABLES ; i++) { |
2077 |
pspad_rates->rate_data[i] = |
2078 |
(uint16_t)VL53L1_decode_unsigned_integer(pdata, 2);
|
2079 |
pdata += 2;
|
2080 |
} |
2081 |
|
2082 |
/* Initialise structure info */
|
2083 |
|
2084 |
pspad_rates->buffer_size = VL53L1_NO_OF_SPAD_ENABLES; |
2085 |
pspad_rates->no_of_values = VL53L1_NO_OF_SPAD_ENABLES; |
2086 |
pspad_rates->fractional_bits = 15;
|
2087 |
|
2088 |
/* Re-enable Firmware */
|
2089 |
|
2090 |
if (status == VL53L1_ERROR_NONE)
|
2091 |
status = VL53L1_enable_firmware(Dev); |
2092 |
|
2093 |
LOG_FUNCTION_END(status); |
2094 |
|
2095 |
return status;
|
2096 |
} |
2097 |
#endif
|
2098 |
|
2099 |
/* Start Patch_LowPowerAutoMode */
|
2100 |
|
2101 |
VL53L1_Error VL53L1_low_power_auto_data_init( |
2102 |
VL53L1_DEV Dev |
2103 |
) |
2104 |
{ |
2105 |
|
2106 |
/*
|
2107 |
* Initializes internal data structures for low power auto mode
|
2108 |
*/
|
2109 |
|
2110 |
/* don't really use this here */
|
2111 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
2112 |
|
2113 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
2114 |
|
2115 |
LOG_FUNCTION_START("");
|
2116 |
|
2117 |
pdev->low_power_auto_data.vhv_loop_bound = |
2118 |
VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT; |
2119 |
pdev->low_power_auto_data.is_low_power_auto_mode = 0;
|
2120 |
pdev->low_power_auto_data.low_power_auto_range_count = 0;
|
2121 |
pdev->low_power_auto_data.saved_interrupt_config = 0;
|
2122 |
pdev->low_power_auto_data.saved_vhv_init = 0;
|
2123 |
pdev->low_power_auto_data.saved_vhv_timeout = 0;
|
2124 |
pdev->low_power_auto_data.first_run_phasecal_result = 0;
|
2125 |
pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
|
2126 |
pdev->low_power_auto_data.dss__required_spads = 0;
|
2127 |
|
2128 |
LOG_FUNCTION_END(status); |
2129 |
|
2130 |
return status;
|
2131 |
} |
2132 |
|
2133 |
VL53L1_Error VL53L1_low_power_auto_data_stop_range( |
2134 |
VL53L1_DEV Dev |
2135 |
) |
2136 |
{ |
2137 |
|
2138 |
/*
|
2139 |
* Range has been paused but may continue later
|
2140 |
*/
|
2141 |
|
2142 |
/* don't really use this here */
|
2143 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
2144 |
|
2145 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
2146 |
|
2147 |
LOG_FUNCTION_START("");
|
2148 |
|
2149 |
/* doing this ensures stop_range followed by a get_device_results does
|
2150 |
* not mess up the counters */
|
2151 |
|
2152 |
pdev->low_power_auto_data.low_power_auto_range_count = 0xFF;
|
2153 |
|
2154 |
pdev->low_power_auto_data.first_run_phasecal_result = 0;
|
2155 |
pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
|
2156 |
pdev->low_power_auto_data.dss__required_spads = 0;
|
2157 |
|
2158 |
/* restore vhv configs */
|
2159 |
if (pdev->low_power_auto_data.saved_vhv_init != 0) |
2160 |
pdev->stat_nvm.vhv_config__init = |
2161 |
pdev->low_power_auto_data.saved_vhv_init; |
2162 |
if (pdev->low_power_auto_data.saved_vhv_timeout != 0) |
2163 |
pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = |
2164 |
pdev->low_power_auto_data.saved_vhv_timeout; |
2165 |
|
2166 |
/* remove phasecal override */
|
2167 |
pdev->gen_cfg.phasecal_config__override = 0x00;
|
2168 |
|
2169 |
LOG_FUNCTION_END(status); |
2170 |
|
2171 |
return status;
|
2172 |
} |
2173 |
|
2174 |
VL53L1_Error VL53L1_config_low_power_auto_mode( |
2175 |
VL53L1_general_config_t *pgeneral, |
2176 |
VL53L1_dynamic_config_t *pdynamic, |
2177 |
VL53L1_low_power_auto_data_t *plpadata |
2178 |
) |
2179 |
{ |
2180 |
|
2181 |
/*
|
2182 |
* Initializes configs for when low power auto presets are selected
|
2183 |
*/
|
2184 |
|
2185 |
/* don't really use this here */
|
2186 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
2187 |
|
2188 |
LOG_FUNCTION_START("");
|
2189 |
|
2190 |
/* set low power auto mode */
|
2191 |
plpadata->is_low_power_auto_mode = 1;
|
2192 |
|
2193 |
/* set low power range count to 0 */
|
2194 |
plpadata->low_power_auto_range_count = 0;
|
2195 |
|
2196 |
/* Turn off MM1/MM2 and DSS2 */
|
2197 |
pdynamic->system__sequence_config = \ |
2198 |
VL53L1_SEQUENCE_VHV_EN | \ |
2199 |
VL53L1_SEQUENCE_PHASECAL_EN | \ |
2200 |
VL53L1_SEQUENCE_DSS1_EN | \ |
2201 |
/* VL53L1_SEQUENCE_DSS2_EN | \*/
|
2202 |
/* VL53L1_SEQUENCE_MM1_EN | \*/
|
2203 |
/* VL53L1_SEQUENCE_MM2_EN | \*/
|
2204 |
VL53L1_SEQUENCE_RANGE_EN; |
2205 |
|
2206 |
/* Set DSS to manual/expected SPADs */
|
2207 |
pgeneral->dss_config__manual_effective_spads_select = 200 << 8; |
2208 |
pgeneral->dss_config__roi_mode_control = |
2209 |
VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; |
2210 |
|
2211 |
LOG_FUNCTION_END(status); |
2212 |
|
2213 |
return status;
|
2214 |
} |
2215 |
|
2216 |
VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration( |
2217 |
VL53L1_DEV Dev) |
2218 |
{ |
2219 |
|
2220 |
/*
|
2221 |
* Setup ranges after the first one in low power auto mode by turning
|
2222 |
* off FW calibration steps and programming static values
|
2223 |
*/
|
2224 |
|
2225 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
2226 |
|
2227 |
/* don't really use this here */
|
2228 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
2229 |
|
2230 |
LOG_FUNCTION_START("");
|
2231 |
|
2232 |
/* save original vhv configs */
|
2233 |
pdev->low_power_auto_data.saved_vhv_init = |
2234 |
pdev->stat_nvm.vhv_config__init; |
2235 |
pdev->low_power_auto_data.saved_vhv_timeout = |
2236 |
pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound; |
2237 |
|
2238 |
/* disable VHV init */
|
2239 |
pdev->stat_nvm.vhv_config__init &= 0x7F;
|
2240 |
/* set loop bound to tuning param */
|
2241 |
pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound = |
2242 |
(pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) +
|
2243 |
(pdev->low_power_auto_data.vhv_loop_bound << 2);
|
2244 |
/* override phasecal */
|
2245 |
pdev->gen_cfg.phasecal_config__override = 0x01;
|
2246 |
pdev->low_power_auto_data.first_run_phasecal_result = |
2247 |
pdev->dbg_results.phasecal_result__vcsel_start; |
2248 |
pdev->gen_cfg.cal_config__vcsel_start = |
2249 |
pdev->low_power_auto_data.first_run_phasecal_result; |
2250 |
|
2251 |
LOG_FUNCTION_END(status); |
2252 |
|
2253 |
return status;
|
2254 |
} |
2255 |
|
2256 |
VL53L1_Error VL53L1_low_power_auto_update_DSS( |
2257 |
VL53L1_DEV Dev) |
2258 |
{ |
2259 |
|
2260 |
/*
|
2261 |
* Do a DSS calculation and update manual config
|
2262 |
*/
|
2263 |
|
2264 |
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
2265 |
|
2266 |
/* don't really use this here */
|
2267 |
VL53L1_Error status = VL53L1_ERROR_NONE; |
2268 |
|
2269 |
uint32_t utemp32a; |
2270 |
|
2271 |
LOG_FUNCTION_START("");
|
2272 |
|
2273 |
/* Calc total rate per spad */
|
2274 |
|
2275 |
/* 9.7 format */
|
2276 |
utemp32a = pdev->sys_results.result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 + |
2277 |
pdev->sys_results.result__ambient_count_rate_mcps_sd0; |
2278 |
|
2279 |
/* clip to 16 bits */
|
2280 |
if (utemp32a > 0xFFFF) |
2281 |
utemp32a = 0xFFFF;
|
2282 |
|
2283 |
/* shift up to take advantage of 32 bits */
|
2284 |
/* 9.23 format */
|
2285 |
utemp32a = utemp32a << 16;
|
2286 |
|
2287 |
/* check SPAD count */
|
2288 |
if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0) |
2289 |
status = VL53L1_ERROR_DIVISION_BY_ZERO; |
2290 |
else {
|
2291 |
/* format 17.15 */
|
2292 |
utemp32a = utemp32a / |
2293 |
pdev->sys_results.result__dss_actual_effective_spads_sd0; |
2294 |
/* save intermediate result */
|
2295 |
pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = |
2296 |
utemp32a; |
2297 |
|
2298 |
/* get the target rate and shift up by 16
|
2299 |
* format 9.23 */
|
2300 |
utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps << |
2301 |
16;
|
2302 |
|
2303 |
/* check for divide by zero */
|
2304 |
if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps == 0) |
2305 |
status = VL53L1_ERROR_DIVISION_BY_ZERO; |
2306 |
else {
|
2307 |
/* divide by rate per spad
|
2308 |
* format 24.8 */
|
2309 |
utemp32a = utemp32a / |
2310 |
pdev->low_power_auto_data.dss__total_rate_per_spad_mcps; |
2311 |
|
2312 |
/* clip to 16 bit */
|
2313 |
if (utemp32a > 0xFFFF) |
2314 |
utemp32a = 0xFFFF;
|
2315 |
|
2316 |
/* save result in low power auto data */
|
2317 |
pdev->low_power_auto_data.dss__required_spads = |
2318 |
(uint16_t)utemp32a; |
2319 |
|
2320 |
/* override DSS config */
|
2321 |
pdev->gen_cfg.dss_config__manual_effective_spads_select = |
2322 |
pdev->low_power_auto_data.dss__required_spads; |
2323 |
pdev->gen_cfg.dss_config__roi_mode_control = |
2324 |
VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; |
2325 |
} |
2326 |
|
2327 |
} |
2328 |
|
2329 |
if (status == VL53L1_ERROR_DIVISION_BY_ZERO) {
|
2330 |
/* We want to gracefully set a spad target, not just exit with
|
2331 |
* an error */
|
2332 |
|
2333 |
/* set target to mid point */
|
2334 |
pdev->low_power_auto_data.dss__required_spads = 0x8000;
|
2335 |
|
2336 |
/* override DSS config */
|
2337 |
pdev->gen_cfg.dss_config__manual_effective_spads_select = |
2338 |
pdev->low_power_auto_data.dss__required_spads; |
2339 |
pdev->gen_cfg.dss_config__roi_mode_control = |
2340 |
VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS; |
2341 |
|
2342 |
/* reset error */
|
2343 |
status = VL53L1_ERROR_NONE; |
2344 |
} |
2345 |
|
2346 |
LOG_FUNCTION_END(status); |
2347 |
|
2348 |
return status;
|
2349 |
} |
2350 |
|
2351 |
|
2352 |
/* End Patch_LowPowerAutoMode */
|