amiro-lld / drivers / VL53L1X / v1 / api / core / vl53l1_api_calibration.c @ 3ed0cc4d
History | View | Annotate | Download (23.219 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_api_core.c
|
||
65 | *
|
||
66 | * @brief EwokPlus25 low level API function definition
|
||
67 | */
|
||
68 | |||
69 | |||
70 | #include "vl53l1_ll_def.h" |
||
71 | #include "vl53l1_ll_device.h" |
||
72 | #include "vl53l1_platform.h" |
||
73 | #include "vl53l1_register_map.h" |
||
74 | #include "vl53l1_register_funcs.h" |
||
75 | #include "vl53l1_register_settings.h" |
||
76 | #include "vl53l1_core.h" |
||
77 | #include "vl53l1_wait.h" |
||
78 | #include "vl53l1_api_preset_modes.h" |
||
79 | #include "vl53l1_silicon_core.h" |
||
80 | #include "vl53l1_api_core.h" |
||
81 | #include "vl53l1_api_calibration.h" |
||
82 | |||
83 | #ifdef VL53L1_LOG_ENABLE
|
||
84 | #include "vl53l1_api_debug.h" |
||
85 | #endif
|
||
86 | #ifdef VL53L1_LOGGING
|
||
87 | #include "vl53l1_debug.h" |
||
88 | #endif
|
||
89 | |||
90 | #define LOG_FUNCTION_START(fmt, ...) \
|
||
91 | _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__) |
||
92 | #define LOG_FUNCTION_END(status, ...) \
|
||
93 | _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__) |
||
94 | #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||
95 | _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, \ |
||
96 | fmt, ##__VA_ARGS__) |
||
97 | |||
98 | #define trace_print(level, ...) \
|
||
99 | _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \ |
||
100 | level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) |
||
101 | |||
102 | |||
103 | #ifndef VL53L1_NOCALIB
|
||
104 | VL53L1_Error VL53L1_run_ref_spad_char( |
||
105 | VL53L1_DEV Dev, |
||
106 | VL53L1_Error *pcal_status) |
||
107 | { |
||
108 | /*
|
||
109 | * Runs Reference SPAD Characterisation
|
||
110 | */
|
||
111 | |||
112 | VL53L1_Error status = VL53L1_ERROR_NONE; |
||
113 | VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
||
114 | |||
115 | uint8_t comms_buffer[6];
|
||
116 | |||
117 | VL53L1_refspadchar_config_t *prefspadchar = &(pdev->refspadchar); |
||
118 | |||
119 | LOG_FUNCTION_START("");
|
||
120 | |||
121 | /*
|
||
122 | * Ensure power force is enabled
|
||
123 | */
|
||
124 | |||
125 | if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/ |
||
126 | status = VL53L1_enable_powerforce(Dev); |
||
127 | |||
128 | /*
|
||
129 | * Configure device
|
||
130 | */
|
||
131 | |||
132 | if (status == VL53L1_ERROR_NONE)
|
||
133 | status = |
||
134 | VL53L1_set_ref_spad_char_config( |
||
135 | Dev, |
||
136 | prefspadchar->vcsel_period, |
||
137 | prefspadchar->timeout_us, |
||
138 | prefspadchar->target_count_rate_mcps, |
||
139 | prefspadchar->max_count_rate_limit_mcps, |
||
140 | prefspadchar->min_count_rate_limit_mcps, |
||
141 | pdev->stat_nvm.osc_measured__fast_osc__frequency); |
||
142 | |||
143 | /*
|
||
144 | * Run device test
|
||
145 | */
|
||
146 | |||
147 | if (status == VL53L1_ERROR_NONE)
|
||
148 | status = VL53L1_run_device_test( |
||
149 | Dev, |
||
150 | prefspadchar->device_test_mode); |
||
151 | |||
152 | /*
|
||
153 | * Read results
|
||
154 | */
|
||
155 | |||
156 | if (status == VL53L1_ERROR_NONE)
|
||
157 | status = |
||
158 | VL53L1_ReadMulti( |
||
159 | Dev, |
||
160 | VL53L1_REF_SPAD_CHAR_RESULT__NUM_ACTUAL_REF_SPADS, |
||
161 | comms_buffer, |
||
162 | 2);
|
||
163 | |||
164 | if (status == VL53L1_ERROR_NONE) {
|
||
165 | pdev->dbg_results.ref_spad_char_result__num_actual_ref_spads = |
||
166 | comms_buffer[0];
|
||
167 | pdev->dbg_results.ref_spad_char_result__ref_location = |
||
168 | comms_buffer[1];
|
||
169 | } |
||
170 | |||
171 | /*
|
||
172 | * copy results to customer nvm managed G02 registers
|
||
173 | */
|
||
174 | |||
175 | if (status == VL53L1_ERROR_NONE)
|
||
176 | status = |
||
177 | VL53L1_WriteMulti( |
||
178 | Dev, |
||
179 | VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS, |
||
180 | comms_buffer, |
||
181 | 2);
|
||
182 | |||
183 | if (status == VL53L1_ERROR_NONE) {
|
||
184 | pdev->customer.ref_spad_man__num_requested_ref_spads = |
||
185 | comms_buffer[0];
|
||
186 | pdev->customer.ref_spad_man__ref_location = |
||
187 | comms_buffer[1];
|
||
188 | } |
||
189 | |||
190 | /* After Ref Spad Char the final set of good SPAD enables
|
||
191 | * are stored in the NCY results registers below
|
||
192 | *
|
||
193 | * - RESULT__SPARE_0_SD_1
|
||
194 | * - RESULT__SPARE_1_SD_1
|
||
195 | * - RESULT__SPARE_2_SD_1
|
||
196 | */
|
||
197 | |||
198 | if (status == VL53L1_ERROR_NONE)
|
||
199 | status = |
||
200 | VL53L1_ReadMulti( |
||
201 | Dev, |
||
202 | VL53L1_RESULT__SPARE_0_SD1, |
||
203 | comms_buffer, |
||
204 | 6);
|
||
205 | |||
206 | /*
|
||
207 | * copy reference SPAD enables to customer nvm managed
|
||
208 | * G02 registers
|
||
209 | */
|
||
210 | |||
211 | if (status == VL53L1_ERROR_NONE)
|
||
212 | status = |
||
213 | VL53L1_WriteMulti( |
||
214 | Dev, |
||
215 | VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0, |
||
216 | comms_buffer, |
||
217 | 6);
|
||
218 | |||
219 | if (status == VL53L1_ERROR_NONE) {
|
||
220 | pdev->customer.global_config__spad_enables_ref_0 = |
||
221 | comms_buffer[0];
|
||
222 | pdev->customer.global_config__spad_enables_ref_1 = |
||
223 | comms_buffer[1];
|
||
224 | pdev->customer.global_config__spad_enables_ref_2 = |
||
225 | comms_buffer[2];
|
||
226 | pdev->customer.global_config__spad_enables_ref_3 = |
||
227 | comms_buffer[3];
|
||
228 | pdev->customer.global_config__spad_enables_ref_4 = |
||
229 | comms_buffer[4];
|
||
230 | pdev->customer.global_config__spad_enables_ref_5 = |
||
231 | comms_buffer[5];
|
||
232 | } |
||
233 | |||
234 | #ifdef VL53L1_LOG_ENABLE
|
||
235 | /* Print customer nvm managed data */
|
||
236 | if (status == VL53L1_ERROR_NONE)
|
||
237 | VL53L1_print_customer_nvm_managed( |
||
238 | &(pdev->customer), |
||
239 | "run_ref_spad_char():pdev->lldata.customer.",
|
||
240 | VL53L1_TRACE_MODULE_REF_SPAD_CHAR); |
||
241 | #endif
|
||
242 | |||
243 | if (status == VL53L1_ERROR_NONE) {
|
||
244 | |||
245 | switch (pdev->sys_results.result__range_status) {
|
||
246 | |||
247 | case VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS:
|
||
248 | status = VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS; |
||
249 | break;
|
||
250 | |||
251 | case VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET:
|
||
252 | status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH; |
||
253 | break;
|
||
254 | |||
255 | case VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET:
|
||
256 | status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW; |
||
257 | break;
|
||
258 | } |
||
259 | } |
||
260 | |||
261 | /*
|
||
262 | * Save unfiltered status
|
||
263 | */
|
||
264 | |||
265 | *pcal_status = status; |
||
266 | |||
267 | /* Status exception code */
|
||
268 | |||
269 | IGNORE_STATUS( |
||
270 | IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS, |
||
271 | VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS, |
||
272 | status); |
||
273 | |||
274 | IGNORE_STATUS( |
||
275 | IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH, |
||
276 | VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH, |
||
277 | status); |
||
278 | |||
279 | IGNORE_STATUS( |
||
280 | IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW, |
||
281 | VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW, |
||
282 | status); |
||
283 | |||
284 | |||
285 | LOG_FUNCTION_END(status); |
||
286 | |||
287 | return status;
|
||
288 | } |
||
289 | |||
290 | VL53L1_Error VL53L1_run_offset_calibration( |
||
291 | VL53L1_DEV Dev, |
||
292 | int16_t cal_distance_mm, |
||
293 | VL53L1_Error *pcal_status) |
||
294 | { |
||
295 | /*
|
||
296 | * Runs offset calibration
|
||
297 | *
|
||
298 | * Recommended tuning parm settings:
|
||
299 | *
|
||
300 | * - pre_num_of_samples = 32
|
||
301 | * - mm1_num_of_samples = 100
|
||
302 | * - mm2_num_of_samples = 64
|
||
303 | * - target_distance_mm = 140mm
|
||
304 | * - target reflectance = 5%
|
||
305 | *
|
||
306 | * Standard Ranging (sigma delta mode):
|
||
307 | * - dss_config__target_total_rate_mcps = 20.0 -40.0 Mcps
|
||
308 | * - phasecal_config_timeout_us = 1000
|
||
309 | * - range_config_timeout_us = 13000
|
||
310 | * - mm_config_timeout_us = 13000
|
||
311 | *
|
||
312 | *
|
||
313 | * Note: function parms simplified as part of
|
||
314 | * Patch_CalFunctionSimplification_11791
|
||
315 | *
|
||
316 | */
|
||
317 | |||
318 | VL53L1_Error status = VL53L1_ERROR_NONE; |
||
319 | VL53L1_LLDriverData_t *pdev = |
||
320 | VL53L1DevStructGetLLDriverHandle(Dev); |
||
321 | |||
322 | VL53L1_DevicePresetModes device_preset_modes[VL53L1_MAX_OFFSET_RANGE_RESULTS]; |
||
323 | |||
324 | VL53L1_range_results_t range_results; |
||
325 | VL53L1_range_results_t *prange_results = &range_results; |
||
326 | VL53L1_range_data_t *prange_data = NULL;
|
||
327 | VL53L1_offset_range_data_t *poffset = NULL;
|
||
328 | |||
329 | uint8_t i = 0;
|
||
330 | uint8_t m = 0;
|
||
331 | uint8_t measurement_mode = |
||
332 | VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK; |
||
333 | uint16_t manual_effective_spads = |
||
334 | pdev->gen_cfg.dss_config__manual_effective_spads_select; |
||
335 | |||
336 | uint8_t num_of_samples[VL53L1_MAX_OFFSET_RANGE_RESULTS]; |
||
337 | |||
338 | LOG_FUNCTION_START("");
|
||
339 | |||
340 | /* select requested offset calibration mode */
|
||
341 | |||
342 | switch (pdev->offset_calibration_mode) {
|
||
343 | |||
344 | default:
|
||
345 | device_preset_modes[0] =
|
||
346 | VL53L1_DEVICEPRESETMODE_STANDARD_RANGING; |
||
347 | device_preset_modes[1] =
|
||
348 | VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL; |
||
349 | device_preset_modes[2] =
|
||
350 | VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL; |
||
351 | break;
|
||
352 | } |
||
353 | |||
354 | /* initialise num_of_samples */
|
||
355 | /* Start Patch_CalFunctionSimplification_11791 */
|
||
356 | num_of_samples[0] = pdev->offsetcal_cfg.pre_num_of_samples;
|
||
357 | num_of_samples[1] = pdev->offsetcal_cfg.mm1_num_of_samples;
|
||
358 | num_of_samples[2] = pdev->offsetcal_cfg.mm2_num_of_samples;
|
||
359 | /* End Patch_CalFunctionSimplification_11791 */
|
||
360 | |||
361 | /* force all offsets to zero */
|
||
362 | |||
363 | switch (pdev->offset_calibration_mode) {
|
||
364 | |||
365 | case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
|
||
366 | /* only run pre range */
|
||
367 | pdev->offset_results.active_results = 1;
|
||
368 | |||
369 | break;
|
||
370 | |||
371 | default:
|
||
372 | |||
373 | pdev->customer.mm_config__inner_offset_mm = 0;
|
||
374 | pdev->customer.mm_config__outer_offset_mm = 0;
|
||
375 | pdev->offset_results.active_results = |
||
376 | VL53L1_MAX_OFFSET_RANGE_RESULTS; |
||
377 | |||
378 | break;
|
||
379 | } |
||
380 | |||
381 | pdev->customer.algo__part_to_part_range_offset_mm = 0;
|
||
382 | |||
383 | /* initialise offset range results */
|
||
384 | |||
385 | pdev->offset_results.max_results = VL53L1_MAX_OFFSET_RANGE_RESULTS; |
||
386 | pdev->offset_results.cal_distance_mm = cal_distance_mm; |
||
387 | |||
388 | for (m = 0 ; m < VL53L1_MAX_OFFSET_RANGE_RESULTS; m++) { |
||
389 | |||
390 | poffset = &(pdev->offset_results.data[m]); |
||
391 | poffset->preset_mode = 0;
|
||
392 | poffset->no_of_samples = 0;
|
||
393 | poffset->effective_spads = 0;
|
||
394 | poffset->peak_rate_mcps = 0;
|
||
395 | poffset->sigma_mm = 0;
|
||
396 | poffset->median_range_mm = 0;
|
||
397 | } |
||
398 | |||
399 | for (m = 0 ; m < pdev->offset_results.active_results ; m++) { |
||
400 | |||
401 | poffset = &(pdev->offset_results.data[m]); |
||
402 | |||
403 | poffset->preset_mode = device_preset_modes[m]; |
||
404 | |||
405 | /* Apply preset mode */
|
||
406 | |||
407 | if (status == VL53L1_ERROR_NONE)
|
||
408 | status = |
||
409 | VL53L1_set_preset_mode( |
||
410 | Dev, |
||
411 | device_preset_modes[m], |
||
412 | /* Start Patch_CalFunctionSimplification_11791 */
|
||
413 | pdev->offsetcal_cfg.dss_config__target_total_rate_mcps, |
||
414 | pdev->offsetcal_cfg.phasecal_config_timeout_us, |
||
415 | pdev->offsetcal_cfg.mm_config_timeout_us, |
||
416 | pdev->offsetcal_cfg.range_config_timeout_us, |
||
417 | /* End Patch_CalFunctionSimplification_11791 */
|
||
418 | 100);
|
||
419 | |||
420 | pdev->gen_cfg.dss_config__manual_effective_spads_select = |
||
421 | manual_effective_spads; |
||
422 | |||
423 | /* Initialise device and start range */
|
||
424 | |||
425 | if (status == VL53L1_ERROR_NONE)
|
||
426 | status = |
||
427 | VL53L1_init_and_start_range( |
||
428 | Dev, |
||
429 | measurement_mode, |
||
430 | VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS); |
||
431 | |||
432 | for (i = 0 ; i <= (num_of_samples[m]+2) ; i++) { |
||
433 | |||
434 | /* Wait for range completion */
|
||
435 | |||
436 | if (status == VL53L1_ERROR_NONE)
|
||
437 | status = |
||
438 | VL53L1_wait_for_range_completion(Dev); |
||
439 | |||
440 | /*
|
||
441 | * Get Device Results
|
||
442 | * - Checks the stream count is the expected one
|
||
443 | * - Read device system results
|
||
444 | */
|
||
445 | |||
446 | if (status == VL53L1_ERROR_NONE)
|
||
447 | status = |
||
448 | VL53L1_get_device_results( |
||
449 | Dev, |
||
450 | VL53L1_DEVICERESULTSLEVEL_FULL, |
||
451 | prange_results); |
||
452 | |||
453 | /*
|
||
454 | * Ignore 1st two ranges to give the sigma delta initial
|
||
455 | * phase time to settle
|
||
456 | *
|
||
457 | * accummulate range results if range is successful
|
||
458 | */
|
||
459 | |||
460 | prange_data = &(prange_results->data[0]);
|
||
461 | |||
462 | if (prange_results->stream_count > 1) { |
||
463 | |||
464 | if (prange_data->range_status ==
|
||
465 | VL53L1_DEVICEERROR_RANGECOMPLETE) { |
||
466 | |||
467 | poffset->no_of_samples++; |
||
468 | poffset->effective_spads += |
||
469 | (uint32_t)prange_data->actual_effective_spads; |
||
470 | poffset->peak_rate_mcps += |
||
471 | (uint32_t)prange_data->peak_signal_count_rate_mcps; |
||
472 | poffset->sigma_mm += |
||
473 | (uint32_t)prange_data->sigma_mm; |
||
474 | poffset->median_range_mm += |
||
475 | (int32_t)prange_data->median_range_mm; |
||
476 | |||
477 | poffset->dss_config__roi_mode_control = |
||
478 | pdev->gen_cfg.dss_config__roi_mode_control; |
||
479 | poffset->dss_config__manual_effective_spads_select = |
||
480 | pdev->gen_cfg.dss_config__manual_effective_spads_select; |
||
481 | } |
||
482 | } |
||
483 | |||
484 | /*
|
||
485 | * Conditional wait for firmware ready. Only waits for timed
|
||
486 | * and single shot modes. Mode check is performed inside the
|
||
487 | * wait function
|
||
488 | */
|
||
489 | |||
490 | if (status == VL53L1_ERROR_NONE)
|
||
491 | status = |
||
492 | VL53L1_wait_for_firmware_ready(Dev); |
||
493 | |||
494 | /*
|
||
495 | * Send ranging handshake
|
||
496 | *
|
||
497 | * - Update Zone management
|
||
498 | * - Update GPH registers
|
||
499 | * - Clear current interrupt
|
||
500 | * - Initialise SYSTEM__MODE_START for next range (if there is one!)
|
||
501 | */
|
||
502 | |||
503 | if (status == VL53L1_ERROR_NONE)
|
||
504 | status = |
||
505 | VL53L1_clear_interrupt_and_enable_next_range( |
||
506 | Dev, |
||
507 | measurement_mode); |
||
508 | } |
||
509 | |||
510 | /* Stop range */
|
||
511 | |||
512 | if (status == VL53L1_ERROR_NONE)
|
||
513 | status = VL53L1_stop_range(Dev); |
||
514 | |||
515 | /* Wait for Stop (abort) range to complete */
|
||
516 | |||
517 | if (status == VL53L1_ERROR_NONE)
|
||
518 | status = VL53L1_WaitUs(Dev, 1000);
|
||
519 | |||
520 | /* generate average values */
|
||
521 | if (poffset->no_of_samples > 0) { |
||
522 | |||
523 | poffset->effective_spads += (poffset->no_of_samples/2);
|
||
524 | poffset->effective_spads /= poffset->no_of_samples; |
||
525 | |||
526 | poffset->peak_rate_mcps += (poffset->no_of_samples/2);
|
||
527 | poffset->peak_rate_mcps /= poffset->no_of_samples; |
||
528 | |||
529 | poffset->sigma_mm += (poffset->no_of_samples/2);
|
||
530 | poffset->sigma_mm /= poffset->no_of_samples; |
||
531 | |||
532 | poffset->median_range_mm += (poffset->no_of_samples/2);
|
||
533 | poffset->median_range_mm /= poffset->no_of_samples; |
||
534 | |||
535 | poffset->range_mm_offset = (int32_t)cal_distance_mm; |
||
536 | poffset->range_mm_offset -= poffset->median_range_mm; |
||
537 | |||
538 | /* remember the number of SPADs for standard ranging */
|
||
539 | if (poffset->preset_mode ==
|
||
540 | VL53L1_DEVICEPRESETMODE_STANDARD_RANGING) |
||
541 | manual_effective_spads = |
||
542 | (uint16_t)poffset->effective_spads; |
||
543 | } |
||
544 | } |
||
545 | |||
546 | /* Calculate offsets */
|
||
547 | |||
548 | switch (pdev->offset_calibration_mode) {
|
||
549 | |||
550 | case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
|
||
551 | |||
552 | /* copy offsets to customer data structure */
|
||
553 | pdev->customer.mm_config__inner_offset_mm += |
||
554 | (int16_t)pdev->offset_results.data[0].range_mm_offset;
|
||
555 | pdev->customer.mm_config__outer_offset_mm += |
||
556 | (int16_t)pdev->offset_results.data[0].range_mm_offset;
|
||
557 | break;
|
||
558 | |||
559 | default:
|
||
560 | /* copy offsets to customer data structure */
|
||
561 | pdev->customer.mm_config__inner_offset_mm = |
||
562 | (int16_t)pdev->offset_results.data[1].range_mm_offset;
|
||
563 | pdev->customer.mm_config__outer_offset_mm = |
||
564 | (int16_t)pdev->offset_results.data[2].range_mm_offset;
|
||
565 | pdev->customer.algo__part_to_part_range_offset_mm = 0;
|
||
566 | |||
567 | /* copy average rate and effective SPAD count to
|
||
568 | additional offset calibration data structure */
|
||
569 | |||
570 | pdev->add_off_cal_data.result__mm_inner_actual_effective_spads = |
||
571 | (uint16_t)pdev->offset_results.data[1].effective_spads;
|
||
572 | pdev->add_off_cal_data.result__mm_outer_actual_effective_spads = |
||
573 | (uint16_t)pdev->offset_results.data[2].effective_spads;
|
||
574 | |||
575 | pdev->add_off_cal_data.result__mm_inner_peak_signal_count_rtn_mcps = |
||
576 | (uint16_t)pdev->offset_results.data[1].peak_rate_mcps;
|
||
577 | pdev->add_off_cal_data.result__mm_outer_peak_signal_count_rtn_mcps = |
||
578 | (uint16_t)pdev->offset_results.data[2].peak_rate_mcps;
|
||
579 | |||
580 | break;
|
||
581 | } |
||
582 | |||
583 | |||
584 | /* apply to device */
|
||
585 | |||
586 | if (status == VL53L1_ERROR_NONE)
|
||
587 | status = |
||
588 | VL53L1_set_customer_nvm_managed( |
||
589 | Dev, |
||
590 | &(pdev->customer)); |
||
591 | |||
592 | /*
|
||
593 | * Check the peak rates, sigma, min spads for each stage
|
||
594 | */
|
||
595 | |||
596 | for (m = 0 ; m < pdev->offset_results.active_results ; m++) { |
||
597 | |||
598 | poffset = &(pdev->offset_results.data[m]); |
||
599 | |||
600 | if (status == VL53L1_ERROR_NONE) {
|
||
601 | |||
602 | pdev->offset_results.cal_report = m; |
||
603 | |||
604 | if (poffset->no_of_samples < num_of_samples[m])
|
||
605 | status = VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES; |
||
606 | |||
607 | /* only check sigma for the pre-range as
|
||
608 | * the it is not calculated by the device
|
||
609 | * for the MM1 and MM2 stages
|
||
610 | */
|
||
611 | if (m == 0 && poffset->sigma_mm > |
||
612 | ((uint32_t)VL53L1_OFFSET_CAL_MAX_SIGMA_MM<<5))
|
||
613 | status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH; |
||
614 | |||
615 | if (poffset->peak_rate_mcps >
|
||
616 | VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS) |
||
617 | status = VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH; |
||
618 | |||
619 | if (poffset->dss_config__manual_effective_spads_select <
|
||
620 | VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS) |
||
621 | status = VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW; |
||
622 | |||
623 | if (poffset->dss_config__manual_effective_spads_select == 0) |
||
624 | status = VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL; |
||
625 | |||
626 | if (poffset->no_of_samples == 0) |
||
627 | status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL; |
||
628 | } |
||
629 | } |
||
630 | |||
631 | /*
|
||
632 | * Save unfiltered status
|
||
633 | */
|
||
634 | |||
635 | pdev->offset_results.cal_status = status; |
||
636 | *pcal_status = pdev->offset_results.cal_status; |
||
637 | |||
638 | /* Status exception codes */
|
||
639 | |||
640 | IGNORE_STATUS( |
||
641 | IGNORE_OFFSET_CAL_MISSING_SAMPLES, |
||
642 | VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES, |
||
643 | status); |
||
644 | |||
645 | IGNORE_STATUS( |
||
646 | IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH, |
||
647 | VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH, |
||
648 | status); |
||
649 | |||
650 | IGNORE_STATUS( |
||
651 | IGNORE_OFFSET_CAL_RATE_TOO_HIGH, |
||
652 | VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH, |
||
653 | status); |
||
654 | |||
655 | IGNORE_STATUS( |
||
656 | IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW, |
||
657 | VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW, |
||
658 | status); |
||
659 | |||
660 | #ifdef VL53L1_LOG_ENABLE
|
||
661 | |||
662 | /* Prints out the offset calibration data for debug */
|
||
663 | |||
664 | VL53L1_print_customer_nvm_managed( |
||
665 | &(pdev->customer), |
||
666 | "run_offset_calibration():pdev->lldata.customer.",
|
||
667 | VL53L1_TRACE_MODULE_OFFSET_DATA); |
||
668 | |||
669 | VL53L1_print_additional_offset_cal_data( |
||
670 | &(pdev->add_off_cal_data), |
||
671 | "run_offset_calibration():pdev->lldata.add_off_cal_data.",
|
||
672 | VL53L1_TRACE_MODULE_OFFSET_DATA); |
||
673 | |||
674 | VL53L1_print_offset_range_results( |
||
675 | &(pdev->offset_results), |
||
676 | "run_offset_calibration():pdev->lldata.offset_results.",
|
||
677 | VL53L1_TRACE_MODULE_OFFSET_DATA); |
||
678 | #endif
|
||
679 | |||
680 | LOG_FUNCTION_END(status); |
||
681 | |||
682 | return status;
|
||
683 | } |
||
684 | #endif
|
||
685 | |||
686 | #ifndef VL53L1_NOCALIB
|
||
687 | VL53L1_Error VL53L1_run_spad_rate_map( |
||
688 | VL53L1_DEV Dev, |
||
689 | VL53L1_DeviceTestMode device_test_mode, |
||
690 | VL53L1_DeviceSscArray array_select, |
||
691 | uint32_t ssc_config_timeout_us, |
||
692 | VL53L1_spad_rate_data_t *pspad_rate_data) |
||
693 | { |
||
694 | |||
695 | /**
|
||
696 | * Runs SPAD Rate Map
|
||
697 | */
|
||
698 | |||
699 | VL53L1_Error status = VL53L1_ERROR_NONE; |
||
700 | |||
701 | VL53L1_LLDriverData_t *pdev = |
||
702 | VL53L1DevStructGetLLDriverHandle(Dev); |
||
703 | |||
704 | LOG_FUNCTION_START("");
|
||
705 | |||
706 | /*
|
||
707 | * Ensure power force is enabled
|
||
708 | */
|
||
709 | if (status == VL53L1_ERROR_NONE)
|
||
710 | status = VL53L1_enable_powerforce(Dev); |
||
711 | |||
712 | /*
|
||
713 | * Configure the test
|
||
714 | */
|
||
715 | |||
716 | if (status == VL53L1_ERROR_NONE) {
|
||
717 | pdev->ssc_cfg.array_select = array_select; |
||
718 | pdev->ssc_cfg.timeout_us = ssc_config_timeout_us; |
||
719 | status = |
||
720 | VL53L1_set_ssc_config( |
||
721 | Dev, |
||
722 | &(pdev->ssc_cfg), |
||
723 | pdev->stat_nvm.osc_measured__fast_osc__frequency); |
||
724 | } |
||
725 | |||
726 | /*
|
||
727 | * Run device test
|
||
728 | */
|
||
729 | |||
730 | if (status == VL53L1_ERROR_NONE)
|
||
731 | status = |
||
732 | VL53L1_run_device_test( |
||
733 | Dev, |
||
734 | device_test_mode); |
||
735 | |||
736 | /*
|
||
737 | * Read Rate Data from Patch Ram
|
||
738 | */
|
||
739 | |||
740 | if (status == VL53L1_ERROR_NONE)
|
||
741 | status = |
||
742 | VL53L1_get_spad_rate_data( |
||
743 | Dev, |
||
744 | pspad_rate_data); |
||
745 | |||
746 | if (device_test_mode == VL53L1_DEVICETESTMODE_LCR_VCSEL_ON)
|
||
747 | pspad_rate_data->fractional_bits = 7;
|
||
748 | else
|
||
749 | pspad_rate_data->fractional_bits = 15;
|
||
750 | |||
751 | /* Ensure power force is disabled */
|
||
752 | |||
753 | if (status == VL53L1_ERROR_NONE)
|
||
754 | status = VL53L1_disable_powerforce(Dev); |
||
755 | |||
756 | #ifdef VL53L1_LOG_ENABLE
|
||
757 | /* Print return rate data and map */
|
||
758 | |||
759 | if (status == VL53L1_ERROR_NONE) {
|
||
760 | VL53L1_print_spad_rate_data( |
||
761 | pspad_rate_data, |
||
762 | "run_spad_rate_map():",
|
||
763 | VL53L1_TRACE_MODULE_SPAD_RATE_MAP); |
||
764 | VL53L1_print_spad_rate_map( |
||
765 | pspad_rate_data, |
||
766 | "run_spad_rate_map():",
|
||
767 | VL53L1_TRACE_MODULE_SPAD_RATE_MAP); |
||
768 | } |
||
769 | #endif
|
||
770 | |||
771 | LOG_FUNCTION_END(status); |
||
772 | |||
773 | return status;
|
||
774 | } |
||
775 | #endif
|
||
776 | |||
777 | |||
778 | #ifndef VL53L1_NOCALIB
|
||
779 | VL53L1_Error VL53L1_run_device_test( |
||
780 | VL53L1_DEV Dev, |
||
781 | VL53L1_DeviceTestMode device_test_mode) |
||
782 | { |
||
783 | /*
|
||
784 | * Runs the selected Device Test Mode
|
||
785 | */
|
||
786 | |||
787 | VL53L1_Error status = VL53L1_ERROR_NONE; |
||
788 | VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev); |
||
789 | |||
790 | uint8_t comms_buffer[2];
|
||
791 | uint8_t gpio_hv_mux__ctrl = 0;
|
||
792 | |||
793 | LOG_FUNCTION_START("");
|
||
794 | |||
795 | /*
|
||
796 | * Get current interrupt config
|
||
797 | */
|
||
798 | |||
799 | if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/ |
||
800 | status = |
||
801 | VL53L1_RdByte( |
||
802 | Dev, |
||
803 | VL53L1_GPIO_HV_MUX__CTRL, |
||
804 | &gpio_hv_mux__ctrl); |
||
805 | |||
806 | if (status == VL53L1_ERROR_NONE)
|
||
807 | pdev->stat_cfg.gpio_hv_mux__ctrl = gpio_hv_mux__ctrl; |
||
808 | |||
809 | /*
|
||
810 | * Trigger the test
|
||
811 | */
|
||
812 | if (status == VL53L1_ERROR_NONE)
|
||
813 | status = VL53L1_start_test( |
||
814 | Dev, |
||
815 | device_test_mode); |
||
816 | |||
817 | /*
|
||
818 | * Wait for test completion
|
||
819 | */
|
||
820 | if (status == VL53L1_ERROR_NONE)
|
||
821 | status = VL53L1_wait_for_test_completion(Dev); |
||
822 | |||
823 | /*
|
||
824 | * Read range and report status
|
||
825 | */
|
||
826 | if (status == VL53L1_ERROR_NONE)
|
||
827 | status = |
||
828 | VL53L1_ReadMulti( |
||
829 | Dev, |
||
830 | VL53L1_RESULT__RANGE_STATUS, |
||
831 | comms_buffer, |
||
832 | 2);
|
||
833 | |||
834 | if (status == VL53L1_ERROR_NONE) {
|
||
835 | pdev->sys_results.result__range_status = comms_buffer[0];
|
||
836 | pdev->sys_results.result__report_status = comms_buffer[1];
|
||
837 | } |
||
838 | |||
839 | /* mask range status bits */
|
||
840 | |||
841 | pdev->sys_results.result__range_status &= |
||
842 | VL53L1_RANGE_STATUS__RANGE_STATUS_MASK; |
||
843 | |||
844 | if (status == VL53L1_ERROR_NONE) {
|
||
845 | trace_print( |
||
846 | VL53L1_TRACE_LEVEL_INFO, |
||
847 | " Device Test Complete:\n\t%-32s = %3u\n\t%-32s = %3u\n",
|
||
848 | "result__range_status",
|
||
849 | pdev->sys_results.result__range_status, |
||
850 | "result__report_status",
|
||
851 | pdev->sys_results.result__report_status); |
||
852 | |||
853 | /*
|
||
854 | * Clear interrupt
|
||
855 | */
|
||
856 | if (status == VL53L1_ERROR_NONE)
|
||
857 | status = VL53L1_clear_interrupt(Dev); |
||
858 | } |
||
859 | |||
860 | /*
|
||
861 | * Clear test mode register
|
||
862 | * - required so that next test command will trigger
|
||
863 | * internal MCU interrupt
|
||
864 | */
|
||
865 | |||
866 | if (status == VL53L1_ERROR_NONE)
|
||
867 | status = |
||
868 | VL53L1_start_test( |
||
869 | Dev, |
||
870 | 0x00);
|
||
871 | |||
872 | LOG_FUNCTION_END(status); |
||
873 | |||
874 | return status;
|
||
875 | } |
||
876 | #endif
|