amiro-lld / drivers / VL53L1X / v1 / api / core / vl53l1_core_support.c @ 3ed0cc4d
History | View | Annotate | Download (11.105 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_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 | |||
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
|
||
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 | |||
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 | |||
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 | |||
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 | |||
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 | |||
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 | |||
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 |