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
VL53L1_init_version(VL53L1_DEV Dev)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
VL53L1_init_ll_driver_state(VL53L1_DEV Dev,VL53L1_DeviceState device_state)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
VL53L1_update_ll_driver_rd_state(VL53L1_DEV Dev)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
VL53L1_check_ll_driver_rd_state(VL53L1_DEV Dev)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
VL53L1_update_ll_driver_cfg_state(VL53L1_DEV Dev)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
VL53L1_copy_rtn_good_spads_to_buffer(VL53L1_nvm_copy_data_t * pdata,uint8_t * pbuffer)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
VL53L1_init_system_results(VL53L1_system_results_t * pdata)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
VL53L1_i2c_encode_uint16_t(uint16_t ip_value,uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_decode_uint16_t(uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_encode_int16_t(int16_t ip_value,uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_decode_int16_t(uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_encode_uint32_t(uint32_t ip_value,uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_decode_uint32_t(uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_decode_with_mask(uint16_t count,uint8_t * pbuffer,uint32_t bit_mask,uint32_t down_shift,uint32_t offset)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
VL53L1_i2c_encode_int32_t(int32_t ip_value,uint16_t count,uint8_t * pbuffer)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
VL53L1_i2c_decode_int32_t(uint16_t count,uint8_t * pbuffer)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
VL53L1_start_test(VL53L1_DEV Dev,uint8_t test_mode__ctrl)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
VL53L1_set_firmware_enable_register(VL53L1_DEV Dev,uint8_t value)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
VL53L1_enable_firmware(VL53L1_DEV Dev)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
VL53L1_disable_firmware(VL53L1_DEV Dev)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
VL53L1_set_powerforce_register(VL53L1_DEV Dev,uint8_t value)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
VL53L1_enable_powerforce(VL53L1_DEV Dev)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
VL53L1_disable_powerforce(VL53L1_DEV Dev)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
VL53L1_clear_interrupt(VL53L1_DEV Dev)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
VL53L1_force_shadow_stream_count_to_zero(VL53L1_DEV Dev)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
VL53L1_calc_macro_period_us(uint16_t fast_osc_frequency,uint8_t vcsel_period)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
VL53L1_calc_range_ignore_threshold(uint32_t central_rate,int16_t x_gradient,int16_t y_gradient,uint8_t rate_mult)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
VL53L1_calc_timeout_mclks(uint32_t timeout_us,uint32_t macro_period_us)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
VL53L1_calc_encoded_timeout(uint32_t timeout_us,uint32_t macro_period_us)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
VL53L1_calc_timeout_us(uint32_t timeout_mclks,uint32_t macro_period_us)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
VL53L1_calc_crosstalk_plane_offset_with_margin(uint32_t plane_offset_kcps,int16_t margin_offset_kcps)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
VL53L1_calc_decoded_timeout_us(uint16_t timeout_encoded,uint32_t macro_period_us)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
VL53L1_encode_timeout(uint32_t timeout_mclks)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
VL53L1_decode_timeout(uint16_t encoded_timeout)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
VL53L1_calc_timeout_register_values(uint32_t phasecal_config_timeout_us,uint32_t mm_config_timeout_us,uint32_t range_config_timeout_us,uint16_t fast_osc_frequency,VL53L1_general_config_t * pgeneral,VL53L1_timing_config_t * ptiming)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
VL53L1_encode_vcsel_period(uint8_t vcsel_period_pclks)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
VL53L1_decode_unsigned_integer(uint8_t * pbuffer,uint8_t no_of_bytes)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
VL53L1_encode_unsigned_integer(uint32_t ip_value,uint8_t no_of_bytes,uint8_t * pbuffer)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
VL53L1_spad_number_to_byte_bit_index(uint8_t spad_number,uint8_t * pbyte_index,uint8_t * pbit_index,uint8_t * pbit_mask)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
VL53L1_encode_row_col(uint8_t row,uint8_t col,uint8_t * pspad_number)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
VL53L1_decode_zone_size(uint8_t encoded_xy_size,uint8_t * pwidth,uint8_t * pheight)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
VL53L1_encode_zone_size(uint8_t width,uint8_t height,uint8_t * pencoded_xy_size)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
VL53L1_decode_zone_limits(uint8_t encoded_xy_centre,uint8_t encoded_xy_size,int16_t * px_ll,int16_t * py_ll,int16_t * px_ur,int16_t * py_ur)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
VL53L1_is_aperture_location(uint8_t row,uint8_t col)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
VL53L1_calc_mm_effective_spads(uint8_t encoded_mm_roi_centre,uint8_t encoded_mm_roi_size,uint8_t encoded_zone_centre,uint8_t encoded_zone_size,uint8_t * pgood_spads,uint16_t aperture_attenuation,uint16_t * pmm_inner_effective_spads,uint16_t * pmm_outer_effective_spads)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
VL53L1_encode_GPIO_interrupt_config(VL53L1_GPIO_interrupt_config_t * pintconf)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
VL53L1_decode_GPIO_interrupt_config(uint8_t system__interrupt_config)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
VL53L1_set_GPIO_distance_threshold(VL53L1_DEV Dev,uint16_t threshold_high,uint16_t threshold_low)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
VL53L1_set_GPIO_rate_threshold(VL53L1_DEV Dev,uint16_t threshold_high,uint16_t threshold_low)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
VL53L1_set_GPIO_thresholds_from_struct(VL53L1_DEV Dev,VL53L1_GPIO_interrupt_config_t * pintconf)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
VL53L1_set_ref_spad_char_config(VL53L1_DEV Dev,uint8_t vcsel_period_a,uint32_t phasecal_timeout_us,uint16_t total_rate_target_mcps,uint16_t max_count_rate_rtn_limit_mcps,uint16_t min_count_rate_rtn_limit_mcps,uint16_t fast_osc_frequency)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
VL53L1_set_ssc_config(VL53L1_DEV Dev,VL53L1_ssc_config_t * pssc_cfg,uint16_t fast_osc_frequency)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
VL53L1_get_spad_rate_data(VL53L1_DEV Dev,VL53L1_spad_rate_data_t * pspad_rates)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
VL53L1_low_power_auto_data_init(VL53L1_DEV Dev)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
VL53L1_low_power_auto_data_stop_range(VL53L1_DEV Dev)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
VL53L1_config_low_power_auto_mode(VL53L1_general_config_t * pgeneral,VL53L1_dynamic_config_t * pdynamic,VL53L1_low_power_auto_data_t * plpadata)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 SUPPRESS_UNUSED_WARNING(pgeneral);
2189
2190 LOG_FUNCTION_START("");
2191
2192 /* set low power auto mode */
2193 plpadata->is_low_power_auto_mode = 1;
2194
2195 /* set low power range count to 0 */
2196 plpadata->low_power_auto_range_count = 0;
2197
2198 /* Turn off MM1/MM2 and DSS2 */
2199 pdynamic->system__sequence_config = \
2200 VL53L1_SEQUENCE_VHV_EN | \
2201 VL53L1_SEQUENCE_PHASECAL_EN | \
2202 VL53L1_SEQUENCE_DSS1_EN | \
2203 VL53L1_SEQUENCE_DSS2_EN | \
2204 /* VL53L1_SEQUENCE_MM1_EN | \*/
2205 /* VL53L1_SEQUENCE_MM2_EN | \*/
2206 VL53L1_SEQUENCE_RANGE_EN;
2207
2208 LOG_FUNCTION_END(status);
2209
2210 return status;
2211 }
2212
VL53L1_low_power_auto_setup_manual_calibration(VL53L1_DEV Dev)2213 VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration(
2214 VL53L1_DEV Dev)
2215 {
2216
2217 /*
2218 * Setup ranges after the first one in low power auto mode by turning
2219 * off FW calibration steps and programming static values
2220 */
2221
2222 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
2223
2224 /* don't really use this here */
2225 VL53L1_Error status = VL53L1_ERROR_NONE;
2226
2227 LOG_FUNCTION_START("");
2228
2229 /* save original vhv configs */
2230 pdev->low_power_auto_data.saved_vhv_init =
2231 pdev->stat_nvm.vhv_config__init;
2232 pdev->low_power_auto_data.saved_vhv_timeout =
2233 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound;
2234
2235 /* disable VHV init */
2236 pdev->stat_nvm.vhv_config__init &= 0x7F;
2237 /* set loop bound to tuning param */
2238 pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
2239 (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) +
2240 (pdev->low_power_auto_data.vhv_loop_bound << 2);
2241 /* override phasecal */
2242 pdev->gen_cfg.phasecal_config__override = 0x01;
2243 pdev->low_power_auto_data.first_run_phasecal_result =
2244 pdev->dbg_results.phasecal_result__vcsel_start;
2245 pdev->gen_cfg.cal_config__vcsel_start =
2246 pdev->low_power_auto_data.first_run_phasecal_result;
2247
2248 LOG_FUNCTION_END(status);
2249
2250 return status;
2251 }
2252
VL53L1_low_power_auto_update_DSS(VL53L1_DEV Dev)2253 VL53L1_Error VL53L1_low_power_auto_update_DSS(
2254 VL53L1_DEV Dev)
2255 {
2256
2257 /*
2258 * Do a DSS calculation and update manual config
2259 */
2260
2261 VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
2262
2263 /* don't really use this here */
2264 VL53L1_Error status = VL53L1_ERROR_NONE;
2265
2266 uint32_t utemp32a;
2267
2268 LOG_FUNCTION_START("");
2269
2270 /* Calc total rate per spad */
2271
2272 /* 9.7 format */
2273 utemp32a = pdev->sys_results.result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 +
2274 pdev->sys_results.result__ambient_count_rate_mcps_sd0;
2275
2276 /* clip to 16 bits */
2277 if (utemp32a > 0xFFFF)
2278 utemp32a = 0xFFFF;
2279
2280 /* shift up to take advantage of 32 bits */
2281 /* 9.23 format */
2282 utemp32a = utemp32a << 16;
2283
2284 /* check SPAD count */
2285 if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0)
2286 status = VL53L1_ERROR_DIVISION_BY_ZERO;
2287 else {
2288 /* format 17.15 */
2289 utemp32a = utemp32a /
2290 pdev->sys_results.result__dss_actual_effective_spads_sd0;
2291 /* save intermediate result */
2292 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps =
2293 utemp32a;
2294
2295 /* get the target rate and shift up by 16
2296 * format 9.23 */
2297 utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps;
2298 utemp32a = utemp32a << 16;
2299
2300 /* check for divide by zero */
2301 if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps == 0)
2302 status = VL53L1_ERROR_DIVISION_BY_ZERO;
2303 else {
2304 /* divide by rate per spad
2305 * format 24.8 */
2306 utemp32a = utemp32a /
2307 pdev->low_power_auto_data.dss__total_rate_per_spad_mcps;
2308
2309 /* clip to 16 bit */
2310 if (utemp32a > 0xFFFF)
2311 utemp32a = 0xFFFF;
2312
2313 /* save result in low power auto data */
2314 pdev->low_power_auto_data.dss__required_spads =
2315 (uint16_t)utemp32a;
2316
2317 /* override DSS config */
2318 pdev->gen_cfg.dss_config__manual_effective_spads_select =
2319 pdev->low_power_auto_data.dss__required_spads;
2320 pdev->gen_cfg.dss_config__roi_mode_control =
2321 VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
2322 }
2323
2324 }
2325
2326 if (status == VL53L1_ERROR_DIVISION_BY_ZERO) {
2327 /* We want to gracefully set a spad target, not just exit with
2328 * an error */
2329
2330 /* set target to mid point */
2331 pdev->low_power_auto_data.dss__required_spads = 0x8000;
2332
2333 /* override DSS config */
2334 pdev->gen_cfg.dss_config__manual_effective_spads_select =
2335 pdev->low_power_auto_data.dss__required_spads;
2336 pdev->gen_cfg.dss_config__roi_mode_control =
2337 VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
2338
2339 /* reset error */
2340 status = VL53L1_ERROR_NONE;
2341 }
2342
2343 LOG_FUNCTION_END(status);
2344
2345 return status;
2346 }
2347
2348
2349 /* End Patch_LowPowerAutoMode */
2350
