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_support.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_log.h"
72 #include "vl53l1_core_support.h"
73 #include "vl53l1_platform_user_data.h"
74 #include "vl53l1_platform_user_defines.h"
75
76 #ifdef VL53L1_LOGGING
77 #include "vl53l1_debug.h"
78 #include "vl53l1_register_debug.h"
79 #endif
80
81 #define LOG_FUNCTION_START(fmt, ...) \
82 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
83 #define LOG_FUNCTION_END(status, ...) \
84 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
85 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
86 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \
87 status, fmt, ##__VA_ARGS__)
88
89 #define trace_print(level, ...) \
90 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
91 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
92
93
VL53L1_calc_pll_period_us(uint16_t fast_osc_frequency)94 uint32_t VL53L1_calc_pll_period_us(
95 uint16_t fast_osc_frequency)
96 {
97 /* Calculates PLL frequency using NVM fast_osc_frequency
98 * Fast osc frequency fixed point format = unsigned 4.12
99 *
100 * PLL period fixed point format = unsigned 0.24
101 * Min input fast osc frequency = 1 MHz
102 * PLL Multiplier = 64 (fixed)
103 * Min PLL freq = 64.0MHz
104 * -> max PLL period = 1/ 64
105 * -> only the 18 LS bits are used
106 *
107 * 2^30 = (2^24) (1.0us) * 4096 (2^12) / 64 (PLL Multiplier)
108 */
109
110 uint32_t pll_period_us = 0;
111
112 LOG_FUNCTION_START("");
113
114 pll_period_us = (0x01 << 30) / fast_osc_frequency;
115
116 #ifdef VL53L1_LOGGING
117 trace_print(VL53L1_TRACE_LEVEL_DEBUG,
118 " %-48s : %10u\n", "pll_period_us",
119 pll_period_us);
120 #endif
121
122 LOG_FUNCTION_END(0);
123
124 return pll_period_us;
125 }
126
127
128 #ifdef PAL_EXTENDED
VL53L1_duration_maths(uint32_t pll_period_us,uint32_t vcsel_parm_pclks,uint32_t window_vclks,uint32_t elapsed_mclks)129 uint32_t VL53L1_duration_maths(
130 uint32_t pll_period_us,
131 uint32_t vcsel_parm_pclks,
132 uint32_t window_vclks,
133 uint32_t elapsed_mclks)
134 {
135 /*
136 * Generates the ranging duration in us
137 *
138 * duration_us = elapsed_mclks * vcsel_perm_pclks *
139 * window_vclks * pll_period_us
140 *
141 * returned value in [us] with no fraction bits
142 */
143
144 uint64_t tmp_long_int = 0;
145 uint32_t duration_us = 0;
146
147 /* PLL period us = 0.24 18 LS bits used
148 * window_vclks = 12.0 (2304 max)
149 * output 30b (6.24)
150 */
151 duration_us = window_vclks * pll_period_us;
152
153 /* down shift by 12
154 * output 18b (6.12)
155 */
156 duration_us = duration_us >> 12;
157
158 /* Save first part of the calc (#1) */
159 tmp_long_int = (uint64_t)duration_us;
160
161 /* Multiply elapsed macro periods (22-bit)
162 * by VCSEL parameter 6.4 (max 63.9999)
163 * output 32b (28.4)
164 */
165 duration_us = elapsed_mclks * vcsel_parm_pclks;
166
167 /* down shift by 4 to remove fractional bits (#2)
168 * output 28b (28.0)
169 */
170 duration_us = duration_us >> 4;
171
172 /* Multiply #1 18b (6.12) by #2 28b (28.0)
173 * output 46b (34.12)
174 */
175 tmp_long_int = tmp_long_int * (uint64_t)duration_us;
176
177 /* Remove fractional part
178 * output 34b (34.0)
179 */
180 tmp_long_int = tmp_long_int >> 12;
181
182 /* Clip to 32-bits */
183 if (tmp_long_int > 0xFFFFFFFF) {
184 tmp_long_int = 0xFFFFFFFF;
185 }
186
187 duration_us = (uint32_t)tmp_long_int;
188
189 return duration_us;
190 }
191
192
VL53L1_isqrt(uint32_t num)193 uint32_t VL53L1_isqrt(uint32_t num)
194 {
195
196 /*
197 * Implements an integer square root
198 *
199 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
200 */
201
202 uint32_t res = 0;
203 uint32_t bit = 1 << 30; /* The second-to-top bit is set: 1 << 14 for 16-bits, 1 << 30 for 32 bits */
204
205 /* "bit" starts at the highest power of four <= the argument. */
206 while (bit > num) {
207 bit >>= 2;
208 }
209
210 while (bit != 0) {
211 if (num >= res + bit) {
212 num -= res + bit;
213 res = (res >> 1) + bit;
214 } else {
215 res >>= 1;
216 }
217 bit >>= 2;
218 }
219
220 return res;
221 }
222
223
VL53L1_rate_maths(int32_t events,uint32_t time_us)224 uint16_t VL53L1_rate_maths(
225 int32_t events,
226 uint32_t time_us)
227 {
228 /*
229 * Converts events into count rate
230 *
231 * Max events = 512 Mcps * 1sec
232 * = 512,000,000 events
233 * = 29b
234 *
235 * If events > 2^24 use 3-bit fractional bits is used internally
236 * otherwise 7-bit fractional bits are used
237 */
238
239 uint32_t tmp_int = 0;
240 uint32_t frac_bits = 7;
241 uint16_t rate_mcps = 0; /* 9.7 format */
242
243 /*
244 * Clip input event range
245 */
246
247 if (events > VL53L1_SPAD_TOTAL_COUNT_MAX) {
248 tmp_int = VL53L1_SPAD_TOTAL_COUNT_MAX;
249 } else if (events > 0) {
250 tmp_int = (uint32_t)events;
251 }
252
253 /*
254 * if events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES use 3 rather
255 * than 7 fractional bits internal to function
256 */
257
258 if (events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES) {
259 frac_bits = 3;
260 } else {
261 frac_bits = 7;
262 }
263
264 /*
265 * Create 3 or 7 fractional bits
266 * output 32b (29.3 or 25.7)
267 * Divide by range duration in [us] - no fractional bits
268 */
269 if (time_us > 0) {
270 tmp_int = ((tmp_int << frac_bits) + (time_us / 2)) / time_us;
271 }
272
273 /*
274 * Re align if reduced resolution
275 */
276 if (events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES) {
277 tmp_int = tmp_int << 4;
278 }
279
280 /*
281 * Firmware internal count is 17.7 (24b) but it this
282 * case clip to 16-bit value for reporting
283 */
284
285 if (tmp_int > 0xFFFF) {
286 tmp_int = 0xFFFF;
287 }
288
289 rate_mcps = (uint16_t)tmp_int;
290
291 return rate_mcps;
292 }
293
VL53L1_rate_per_spad_maths(uint32_t frac_bits,uint32_t peak_count_rate,uint16_t num_spads,uint32_t max_output_value)294 uint16_t VL53L1_rate_per_spad_maths(
295 uint32_t frac_bits,
296 uint32_t peak_count_rate,
297 uint16_t num_spads,
298 uint32_t max_output_value)
299 {
300
301 uint32_t tmp_int = 0;
302
303 /* rate_per_spad Format varies with prog frac_bits */
304 uint16_t rate_per_spad = 0;
305
306 /* Calculate rate per spad with variable fractional bits */
307
308 /* Frac_bits should be programmed as final frac_bits - 7 as
309 * the pk_rate contains an inherent 7 bit resolution
310 */
311
312 if (num_spads > 0) {
313 tmp_int = (peak_count_rate << 8) << frac_bits;
314 tmp_int = (tmp_int + ((uint32_t)num_spads / 2)) / (uint32_t)num_spads;
315 } else {
316 tmp_int = ((peak_count_rate) << frac_bits);
317 }
318
319 /* Clip in case of overwrap - special code */
320
321 if (tmp_int > max_output_value) {
322 tmp_int = max_output_value;
323 }
324
325 rate_per_spad = (uint16_t)tmp_int;
326
327 return rate_per_spad;
328 }
329
VL53L1_range_maths(uint16_t fast_osc_frequency,uint16_t phase,uint16_t zero_distance_phase,uint8_t fractional_bits,int32_t gain_factor,int32_t range_offset_mm)330 int32_t VL53L1_range_maths(
331 uint16_t fast_osc_frequency,
332 uint16_t phase,
333 uint16_t zero_distance_phase,
334 uint8_t fractional_bits,
335 int32_t gain_factor,
336 int32_t range_offset_mm)
337 {
338 /*
339 * Converts phase information into distance in [mm]
340 */
341
342 uint32_t pll_period_us = 0; /* 0.24 format */
343 int64_t tmp_long_int = 0;
344 int32_t range_mm = 0;
345
346 /* Calculate PLL period in [ps] */
347
348 pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency);
349
350 /* Raw range in [mm]
351 *
352 * calculate the phase difference between return and reference phases
353 *
354 * phases 16b (5.11)
355 * output 17b including sign bit
356 */
357
358 tmp_long_int = (int64_t)phase - (int64_t)zero_distance_phase;
359
360 /*
361 * multiply by the PLL period
362 *
363 * PLL period 24bit (0.24) but only 18 LS bits used
364 *
365 * Output 35b (0.35) (17b + 18b)
366 */
367
368 tmp_long_int = tmp_long_int * (int64_t)pll_period_us;
369
370 /*
371 * Down shift by 9 - Output 26b (0.26)
372 */
373
374 tmp_long_int = tmp_long_int / (0x01 << 9);
375
376 /*
377 * multiply by speed of light in air divided by 8
378 * Factor of 8 includes 2 for the round trip and 4 scaling
379 *
380 * VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8 = 16b (16.2)
381 *
382 * Output 42b (18.24) (16b + 26b)
383 */
384
385 tmp_long_int = tmp_long_int * VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8;
386
387 /*
388 * Down shift by 22 - Output 20b (18.2)
389 */
390
391 tmp_long_int = tmp_long_int / (0x01 << 22);
392
393 /* Add range offset */
394 range_mm = (int32_t)tmp_long_int + range_offset_mm;
395
396 /* apply correction gain */
397 range_mm *= gain_factor;
398 range_mm += 0x0400;
399 range_mm /= 0x0800;
400
401 /* Remove fractional bits */
402 if (fractional_bits == 0)
403 range_mm = range_mm / (0x01 << 2);
404 else if (fractional_bits == 1)
405 range_mm = range_mm / (0x01 << 1);
406
407 return range_mm;
408 }
409 #endif
410
VL53L1_decode_vcsel_period(uint8_t vcsel_period_reg)411 uint8_t VL53L1_decode_vcsel_period(uint8_t vcsel_period_reg)
412 {
413 /*
414 * Converts the encoded VCSEL period register value into
415 * the real period in PLL clocks
416 */
417
418 uint8_t vcsel_period_pclks = 0;
419
420 vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
421
422 return vcsel_period_pclks;
423 }
424
425
VL53L1_decode_row_col(uint8_t spad_number,uint8_t * prow,uint8_t * pcol)426 void VL53L1_decode_row_col(
427 uint8_t spad_number,
428 uint8_t *prow,
429 uint8_t *pcol)
430 {
431
432 /**
433 * Decodes the array (row,col) location from
434 * the input SPAD number
435 */
436
437 if (spad_number > 127) {
438 *prow = 8 + ((255-spad_number) & 0x07);
439 *pcol = (spad_number-128) >> 3;
440 } else {
441 *prow = spad_number & 0x07;
442 *pcol = (127-spad_number) >> 3;
443 }
444 }
445
446
