amiro-lld / drivers / VL53L1X / v1 / api / core / vl53l1_core.c @ 3ed0cc4d
History | View | Annotate | Download (57.025 KB)
1 | 4dba9195 | galberding | /*
|
---|---|---|---|
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 */
|