amiro-lld / drivers / VL53L1X / v1 / api / core / vl53l1_api_calibration.c @ 4dba9195
History | View | Annotate | Download (23.219 KB)
| 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_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
|