charles macneill
Initial release. Mbed library for VL53L1CB
Embed:
(wiki syntax)
Show/hide line numbers
vl53l1_hist_algos_gen3.c
00001 00002 /******************************************************************************* 00003 This file is part of VL53L1 Protected 00004 00005 Copyright (c) 2020, STMicroelectronics - All Rights Reserved 00006 00007 License terms: STMicroelectronics Proprietary in accordance with licensing 00008 terms at www.st.com/sla0081 00009 00010 STMicroelectronics confidential 00011 Reproduction and Communication of this document is strictly prohibited unless 00012 specifically authorized in writing by STMicroelectronics. 00013 00014 */ 00015 00016 00017 00018 00019 #include "vl53l1_types.h" 00020 #include "vl53l1_platform_log.h" 00021 00022 #include "vl53l1_core_support.h" 00023 #include "vl53l1_error_codes.h" 00024 00025 #include "vl53l1_hist_core.h" 00026 #include "vl53l1_hist_algos_gen3.h" 00027 #include "vl53l1_sigma_estimate.h" 00028 #include "vl53l1_dmax.h" 00029 00030 00031 00032 #define LOG_FUNCTION_START(fmt, ...) \ 00033 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__) 00034 #define LOG_FUNCTION_END(status, ...) \ 00035 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__) 00036 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \ 00037 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM,\ 00038 status, fmt, ##__VA_ARGS__) 00039 00040 #define trace_print(level, ...) \ 00041 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \ 00042 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__) 00043 00044 00045 void VL53L1_f_016( 00046 VL53L1_hist_gen3_algo_private_data_t *palgo) 00047 { 00048 00049 00050 uint8_t lb = 0; 00051 00052 palgo->VL53L1_p_023 = VL53L1_HISTOGRAM_BUFFER_SIZE; 00053 palgo->VL53L1_p_022 = 0; 00054 palgo->VL53L1_p_024 = 0; 00055 palgo->VL53L1_p_045 = 0; 00056 palgo->VL53L1_p_004 = 0; 00057 palgo->VL53L1_p_032 = 0; 00058 00059 for (lb = palgo->VL53L1_p_022; lb < palgo->VL53L1_p_023; lb++) { 00060 palgo->VL53L1_p_043[lb] = 0; 00061 palgo->VL53L1_p_046[lb] = 0; 00062 palgo->VL53L1_p_047[lb] = 0; 00063 palgo->VL53L1_p_048[lb] = 0; 00064 palgo->VL53L1_p_008[lb] = 0; 00065 } 00066 00067 palgo->VL53L1_p_049 = 0; 00068 palgo->VL53L1_p_050 = VL53L1_D_001; 00069 palgo->VL53L1_p_051 = 0; 00070 00071 00072 00073 VL53L1_init_histogram_bin_data_struct( 00074 0, 00075 VL53L1_HISTOGRAM_BUFFER_SIZE, 00076 &(palgo->VL53L1_p_010)); 00077 VL53L1_init_histogram_bin_data_struct( 00078 0, 00079 VL53L1_HISTOGRAM_BUFFER_SIZE, 00080 &(palgo->VL53L1_p_038)); 00081 VL53L1_init_histogram_bin_data_struct( 00082 0, 00083 VL53L1_HISTOGRAM_BUFFER_SIZE, 00084 &(palgo->VL53L1_p_052)); 00085 VL53L1_init_histogram_bin_data_struct( 00086 0, 00087 VL53L1_HISTOGRAM_BUFFER_SIZE, 00088 &(palgo->VL53L1_p_053)); 00089 VL53L1_init_histogram_bin_data_struct( 00090 0, 00091 VL53L1_HISTOGRAM_BUFFER_SIZE, 00092 &(palgo->VL53L1_p_054)); 00093 } 00094 00095 00096 00097 VL53L1_Error VL53L1_f_018( 00098 uint16_t ambient_threshold_events_scaler, 00099 int32_t ambient_threshold_sigma, 00100 int32_t min_ambient_threshold_events, 00101 uint8_t algo__crosstalk_compensation_enable, 00102 VL53L1_histogram_bin_data_t *pbins, 00103 VL53L1_histogram_bin_data_t *pxtalk, 00104 VL53L1_hist_gen3_algo_private_data_t *palgo) 00105 { 00106 00107 00108 00109 VL53L1_Error status = VL53L1_ERROR_NONE; 00110 uint8_t lb = 0; 00111 uint8_t VL53L1_p_001 = 0; 00112 int64_t tmp = 0; 00113 int32_t amb_events = 0; 00114 int32_t VL53L1_p_008 = 0; 00115 int32_t samples = 0; 00116 00117 LOG_FUNCTION_START(""); 00118 00119 00120 palgo->VL53L1_p_023 = pbins->VL53L1_p_023; 00121 palgo->VL53L1_p_022 = pbins->VL53L1_p_022; 00122 palgo->VL53L1_p_024 = pbins->VL53L1_p_024; 00123 palgo->VL53L1_p_004 = pbins->VL53L1_p_004; 00124 00125 00126 00127 palgo->VL53L1_p_031 = 00128 VL53L1_decode_vcsel_period(pbins->VL53L1_p_009); 00129 00130 00131 00132 tmp = (int64_t)pbins->VL53L1_p_004; 00133 tmp *= (int64_t)ambient_threshold_events_scaler; 00134 tmp += 2048; 00135 tmp = do_division_s(tmp, 4096); 00136 amb_events = (int32_t)tmp; 00137 00138 00139 00140 for (lb = 0; lb < pbins->VL53L1_p_024; lb++) { 00141 00142 VL53L1_p_001 = lb >> 2; 00143 samples = (int32_t)pbins->bin_rep[VL53L1_p_001]; 00144 00145 if (samples > 0) { 00146 00147 if (lb < pxtalk->VL53L1_p_024 && 00148 algo__crosstalk_compensation_enable > 0) 00149 VL53L1_p_008 = samples * (amb_events + 00150 pxtalk->bin_data[lb]); 00151 else 00152 VL53L1_p_008 = samples * amb_events; 00153 00154 VL53L1_p_008 = VL53L1_isqrt(VL53L1_p_008); 00155 00156 VL53L1_p_008 += (samples/2); 00157 VL53L1_p_008 /= samples; 00158 VL53L1_p_008 *= ambient_threshold_sigma; 00159 VL53L1_p_008 += 8; 00160 VL53L1_p_008 /= 16; 00161 VL53L1_p_008 += amb_events; 00162 00163 if (VL53L1_p_008 < min_ambient_threshold_events) 00164 VL53L1_p_008 = min_ambient_threshold_events; 00165 00166 palgo->VL53L1_p_056[lb] = VL53L1_p_008; 00167 palgo->VL53L1_p_032 = VL53L1_p_008; 00168 } 00169 00170 00171 00172 } 00173 00174 00175 00176 palgo->VL53L1_p_045 = 0; 00177 00178 for (lb = pbins->VL53L1_p_022; lb < pbins->VL53L1_p_024; lb++) { 00179 00180 if (pbins->bin_data[lb] > palgo->VL53L1_p_056[lb]) { 00181 palgo->VL53L1_p_043[lb] = 1; 00182 palgo->VL53L1_p_046[lb] = 1; 00183 palgo->VL53L1_p_045++; 00184 } else { 00185 palgo->VL53L1_p_043[lb] = 0; 00186 palgo->VL53L1_p_046[lb] = 0; 00187 } 00188 } 00189 00190 LOG_FUNCTION_END(status); 00191 00192 return status; 00193 00194 } 00195 00196 00197 00198 00199 VL53L1_Error VL53L1_f_019( 00200 VL53L1_hist_gen3_algo_private_data_t *palgo) 00201 { 00202 00203 00204 00205 VL53L1_Error status = VL53L1_ERROR_NONE; 00206 00207 uint8_t i = 0; 00208 uint8_t j = 0; 00209 uint8_t found = 0; 00210 00211 LOG_FUNCTION_START(""); 00212 00213 palgo->VL53L1_p_049 = 0; 00214 00215 for (i = 0; i < palgo->VL53L1_p_031; i++) { 00216 00217 j = (i + 1) % palgo->VL53L1_p_031; 00218 00219 00220 00221 if (i < palgo->VL53L1_p_024 && j < palgo->VL53L1_p_024) { 00222 if (palgo->VL53L1_p_046[i] == 0 && 00223 palgo->VL53L1_p_046[j] == 1 && 00224 found == 0) { 00225 palgo->VL53L1_p_049 = i; 00226 found = 1; 00227 } 00228 } 00229 } 00230 00231 LOG_FUNCTION_END(status); 00232 00233 return status; 00234 } 00235 00236 00237 VL53L1_Error VL53L1_f_020( 00238 VL53L1_hist_gen3_algo_private_data_t *palgo) 00239 { 00240 00241 00242 00243 VL53L1_Error status = VL53L1_ERROR_NONE; 00244 uint8_t i = 0; 00245 uint8_t j = 0; 00246 uint8_t lb = 0; 00247 00248 LOG_FUNCTION_START(""); 00249 00250 for (lb = palgo->VL53L1_p_049; 00251 lb < (palgo->VL53L1_p_049 + 00252 palgo->VL53L1_p_031); 00253 lb++) { 00254 00255 00256 00257 i = lb % palgo->VL53L1_p_031; 00258 j = (lb + 1) % palgo->VL53L1_p_031; 00259 00260 00261 00262 if (i < palgo->VL53L1_p_024 && j < palgo->VL53L1_p_024) { 00263 00264 if (palgo->VL53L1_p_046[i] == 0 && 00265 palgo->VL53L1_p_046[j] == 1) 00266 palgo->VL53L1_p_051++; 00267 00268 if (palgo->VL53L1_p_046[i] > 0) 00269 palgo->VL53L1_p_047[i] = palgo->VL53L1_p_051; 00270 else 00271 palgo->VL53L1_p_047[i] = 0; 00272 } 00273 00274 } 00275 00276 00277 if (palgo->VL53L1_p_051 > palgo->VL53L1_p_050) 00278 palgo->VL53L1_p_051 = palgo->VL53L1_p_050; 00279 00280 LOG_FUNCTION_END(status); 00281 00282 return status; 00283 00284 } 00285 00286 00287 VL53L1_Error VL53L1_f_021( 00288 VL53L1_hist_gen3_algo_private_data_t *palgo) 00289 { 00290 00291 00292 00293 VL53L1_Error status = VL53L1_ERROR_NONE; 00294 00295 uint8_t i = 0; 00296 uint8_t j = 0; 00297 uint8_t blb = 0; 00298 uint8_t pulse_no = 0; 00299 00300 uint8_t max_filter_half_width = 0; 00301 00302 VL53L1_hist_pulse_data_t *pdata; 00303 00304 LOG_FUNCTION_START(""); 00305 00306 00307 00308 max_filter_half_width = palgo->VL53L1_p_031 - 1; 00309 max_filter_half_width = max_filter_half_width >> 1; 00310 00311 for (blb = palgo->VL53L1_p_049; 00312 blb < (palgo->VL53L1_p_049 + 00313 palgo->VL53L1_p_031); 00314 blb++) { 00315 00316 00317 00318 i = blb % palgo->VL53L1_p_031; 00319 j = (blb + 1) % palgo->VL53L1_p_031; 00320 00321 00322 00323 if (i < palgo->VL53L1_p_024 && 00324 j < palgo->VL53L1_p_024) { 00325 00326 00327 00328 if (palgo->VL53L1_p_047[i] == 0 && 00329 palgo->VL53L1_p_047[j] > 0) { 00330 00331 pulse_no = palgo->VL53L1_p_047[j] - 1; 00332 pdata = &(palgo->VL53L1_p_002[pulse_no]); 00333 00334 if (pulse_no < palgo->VL53L1_p_050) { 00335 pdata->VL53L1_p_015 = blb; 00336 pdata->VL53L1_p_022 = blb + 1; 00337 pdata->VL53L1_p_025 = 0xFF; 00338 pdata->VL53L1_p_026 = 0; 00339 pdata->VL53L1_p_016 = 0; 00340 } 00341 } 00342 00343 00344 00345 if (palgo->VL53L1_p_047[i] > 0 00346 && palgo->VL53L1_p_047[j] == 0) { 00347 00348 pulse_no = palgo->VL53L1_p_047[i] - 1; 00349 pdata = &(palgo->VL53L1_p_002[pulse_no]); 00350 00351 if (pulse_no < palgo->VL53L1_p_050) { 00352 00353 pdata->VL53L1_p_026 = blb; 00354 pdata->VL53L1_p_016 = blb + 1; 00355 00356 pdata->VL53L1_p_027 = 00357 (pdata->VL53L1_p_026 + 1) - 00358 pdata->VL53L1_p_022; 00359 pdata->VL53L1_p_055 = 00360 (pdata->VL53L1_p_016 + 1) - 00361 pdata->VL53L1_p_015; 00362 00363 if (pdata->VL53L1_p_055 > 00364 max_filter_half_width) 00365 pdata->VL53L1_p_055 = 00366 max_filter_half_width; 00367 } 00368 00369 } 00370 } 00371 } 00372 00373 LOG_FUNCTION_END(status); 00374 00375 return status; 00376 00377 } 00378 00379 00380 VL53L1_Error VL53L1_f_028( 00381 VL53L1_HistTargetOrder target_order, 00382 VL53L1_hist_gen3_algo_private_data_t *palgo) 00383 { 00384 00385 00386 00387 VL53L1_Error status = VL53L1_ERROR_NONE; 00388 00389 VL53L1_hist_pulse_data_t tmp; 00390 VL53L1_hist_pulse_data_t *ptmp = &tmp; 00391 VL53L1_hist_pulse_data_t *p0; 00392 VL53L1_hist_pulse_data_t *p1; 00393 00394 uint8_t i = 0; 00395 uint8_t swapped = 1; 00396 00397 LOG_FUNCTION_START(""); 00398 00399 if (!(palgo->VL53L1_p_051 > 1)) 00400 goto ENDFUNC; 00401 00402 while (swapped > 0) { 00403 00404 swapped = 0; 00405 00406 for (i = 1; i < palgo->VL53L1_p_051; i++) { 00407 00408 p0 = &(palgo->VL53L1_p_002[i-1]); 00409 p1 = &(palgo->VL53L1_p_002[i]); 00410 00411 00412 00413 if (target_order 00414 == VL53L1_HIST_TARGET_ORDER__STRONGEST_FIRST) { 00415 00416 if (p0->VL53L1_p_013 < 00417 p1->VL53L1_p_013) { 00418 00419 00420 00421 memcpy(ptmp, 00422 p1, sizeof(VL53L1_hist_pulse_data_t)); 00423 memcpy(p1, 00424 p0, sizeof(VL53L1_hist_pulse_data_t)); 00425 memcpy(p0, 00426 ptmp, sizeof(VL53L1_hist_pulse_data_t)); 00427 00428 swapped = 1; 00429 } 00430 00431 } else { 00432 00433 if (p0->VL53L1_p_014 > p1->VL53L1_p_014) { 00434 00435 00436 00437 memcpy(ptmp, 00438 p1, sizeof(VL53L1_hist_pulse_data_t)); 00439 memcpy(p1, 00440 p0, sizeof(VL53L1_hist_pulse_data_t)); 00441 memcpy(p0, 00442 ptmp, sizeof(VL53L1_hist_pulse_data_t)); 00443 00444 swapped = 1; 00445 } 00446 00447 } 00448 } 00449 } 00450 00451 ENDFUNC: 00452 LOG_FUNCTION_END(status); 00453 00454 return status; 00455 00456 } 00457 00458 00459 VL53L1_Error VL53L1_f_022( 00460 uint8_t pulse_no, 00461 VL53L1_histogram_bin_data_t *pbins, 00462 VL53L1_hist_gen3_algo_private_data_t *palgo) 00463 { 00464 00465 00466 VL53L1_Error status = VL53L1_ERROR_NONE; 00467 00468 uint8_t i = 0; 00469 uint8_t lb = 0; 00470 00471 VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]); 00472 00473 LOG_FUNCTION_START(""); 00474 00475 00476 00477 pdata->VL53L1_p_021 = 0; 00478 pdata->VL53L1_p_020 = 0; 00479 00480 for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) { 00481 i = lb % palgo->VL53L1_p_031; 00482 pdata->VL53L1_p_021 += pbins->bin_data[i]; 00483 pdata->VL53L1_p_020 += palgo->VL53L1_p_004; 00484 } 00485 00486 00487 00488 pdata->VL53L1_p_013 = 00489 pdata->VL53L1_p_021 - pdata->VL53L1_p_020; 00490 00491 LOG_FUNCTION_END(status); 00492 00493 return status; 00494 } 00495 00496 00497 VL53L1_Error VL53L1_f_027( 00498 uint8_t pulse_no, 00499 uint8_t clip_events, 00500 VL53L1_histogram_bin_data_t *pbins, 00501 VL53L1_hist_gen3_algo_private_data_t *palgo) 00502 { 00503 00504 00505 VL53L1_Error status = VL53L1_ERROR_NONE; 00506 00507 uint8_t i = 0; 00508 int16_t VL53L1_p_015 = 0; 00509 int16_t VL53L1_p_016 = 0; 00510 int16_t window_width = 0; 00511 uint32_t tmp_phase = 0; 00512 00513 VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]); 00514 00515 LOG_FUNCTION_START(""); 00516 00517 00518 00519 i = pdata->VL53L1_p_025 % palgo->VL53L1_p_031; 00520 00521 VL53L1_p_015 = (int16_t)i; 00522 VL53L1_p_015 += (int16_t)pdata->VL53L1_p_015; 00523 VL53L1_p_015 -= (int16_t)pdata->VL53L1_p_025; 00524 00525 VL53L1_p_016 = (int16_t)i; 00526 VL53L1_p_016 += (int16_t)pdata->VL53L1_p_016; 00527 VL53L1_p_016 -= (int16_t)pdata->VL53L1_p_025; 00528 00529 00530 window_width = VL53L1_p_016 - VL53L1_p_015; 00531 if (window_width > 3) 00532 window_width = 3; 00533 00534 status = 00535 VL53L1_f_030( 00536 VL53L1_p_015, 00537 VL53L1_p_015 + window_width, 00538 palgo->VL53L1_p_031, 00539 clip_events, 00540 pbins, 00541 &(pdata->VL53L1_p_028)); 00542 00543 00544 if (status == VL53L1_ERROR_NONE) 00545 status = 00546 VL53L1_f_030( 00547 VL53L1_p_016 - window_width, 00548 VL53L1_p_016, 00549 palgo->VL53L1_p_031, 00550 clip_events, 00551 pbins, 00552 &(pdata->VL53L1_p_029)); 00553 00554 00555 if (pdata->VL53L1_p_028 > pdata->VL53L1_p_029) { 00556 tmp_phase = pdata->VL53L1_p_028; 00557 pdata->VL53L1_p_028 = pdata->VL53L1_p_029; 00558 pdata->VL53L1_p_029 = tmp_phase; 00559 } 00560 00561 00562 if (pdata->VL53L1_p_014 < pdata->VL53L1_p_028) 00563 pdata->VL53L1_p_028 = pdata->VL53L1_p_014; 00564 00565 00566 if (pdata->VL53L1_p_014 > pdata->VL53L1_p_029) 00567 pdata->VL53L1_p_029 = pdata->VL53L1_p_014; 00568 00569 LOG_FUNCTION_END(status); 00570 00571 return status; 00572 } 00573 00574 00575 VL53L1_Error VL53L1_f_030( 00576 int16_t VL53L1_p_022, 00577 int16_t VL53L1_p_026, 00578 uint8_t VL53L1_p_031, 00579 uint8_t clip_events, 00580 VL53L1_histogram_bin_data_t *pbins, 00581 uint32_t *pphase) 00582 { 00583 00584 00585 VL53L1_Error status = VL53L1_ERROR_NONE; 00586 00587 int16_t i = 0; 00588 int16_t lb = 0; 00589 00590 int64_t VL53L1_p_008 = 0; 00591 int64_t event_sum = 0; 00592 int64_t weighted_sum = 0; 00593 00594 LOG_FUNCTION_START(""); 00595 00596 *pphase = VL53L1_MAX_ALLOWED_PHASE; 00597 00598 for (lb = VL53L1_p_022; lb <= VL53L1_p_026; lb++) { 00599 00600 00601 if (lb < 0) 00602 i = lb + (int16_t)VL53L1_p_031; 00603 else 00604 i = lb % (int16_t)VL53L1_p_031; 00605 00606 VL53L1_p_008 = 00607 (int64_t)pbins->bin_data[i] - 00608 (int64_t)pbins->VL53L1_p_004; 00609 00610 00611 if (clip_events > 0 && VL53L1_p_008 < 0) 00612 VL53L1_p_008 = 0; 00613 00614 event_sum += VL53L1_p_008; 00615 00616 weighted_sum += 00617 (VL53L1_p_008 * (1024 + (2048*(int64_t)lb))); 00618 00619 trace_print( 00620 VL53L1_TRACE_LEVEL_INFO, 00621 "\tb = %5d : i = %5d : VL53L1_p_008 = %8d,", 00622 lb, i, VL53L1_p_008); 00623 00624 trace_print( 00625 VL53L1_TRACE_LEVEL_INFO, 00626 " event_sum = %8d, weighted_sum = %8d\n", 00627 event_sum, weighted_sum); 00628 } 00629 00630 if (event_sum > 0) { 00631 00632 weighted_sum += do_division_s(event_sum, 2); 00633 weighted_sum = do_division_s(weighted_sum, event_sum); 00634 00635 if (weighted_sum < 0) 00636 weighted_sum = 0; 00637 00638 *pphase = (uint32_t)weighted_sum; 00639 } 00640 00641 LOG_FUNCTION_END(status); 00642 00643 return status; 00644 } 00645 00646 00647 VL53L1_Error VL53L1_f_023( 00648 uint8_t pulse_no, 00649 VL53L1_histogram_bin_data_t *pbins, 00650 VL53L1_hist_gen3_algo_private_data_t *palgo, 00651 int32_t pad_value, 00652 VL53L1_histogram_bin_data_t *ppulse) 00653 { 00654 00655 00656 00657 VL53L1_Error status = VL53L1_ERROR_NONE; 00658 00659 uint8_t i = 0; 00660 uint8_t lb = 0; 00661 00662 VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]); 00663 00664 LOG_FUNCTION_START(""); 00665 00666 00667 00668 memcpy(ppulse, pbins, sizeof(VL53L1_histogram_bin_data_t)); 00669 00670 00671 00672 for (lb = palgo->VL53L1_p_049; 00673 lb < (palgo->VL53L1_p_049 + 00674 palgo->VL53L1_p_031); 00675 lb++) { 00676 00677 if (lb < pdata->VL53L1_p_015 || lb > pdata->VL53L1_p_016) { 00678 i = lb % palgo->VL53L1_p_031; 00679 if (i < ppulse->VL53L1_p_024) 00680 ppulse->bin_data[i] = pad_value; 00681 } 00682 } 00683 00684 LOG_FUNCTION_END(status); 00685 00686 return status; 00687 } 00688 00689 00690 VL53L1_Error VL53L1_f_026( 00691 uint8_t bin, 00692 uint8_t sigma_estimator__sigma_ref_mm, 00693 uint8_t VL53L1_p_031, 00694 uint8_t VL53L1_p_055, 00695 uint8_t crosstalk_compensation_enable, 00696 VL53L1_histogram_bin_data_t *phist_data_ap, 00697 VL53L1_histogram_bin_data_t *phist_data_zp, 00698 VL53L1_histogram_bin_data_t *pxtalk_hist, 00699 uint16_t *psigma_est) 00700 { 00701 00702 00703 VL53L1_Error status = VL53L1_ERROR_NONE; 00704 VL53L1_Error func_status = VL53L1_ERROR_NONE; 00705 00706 uint8_t i = 0; 00707 int32_t VL53L1_p_003 = 0; 00708 int32_t VL53L1_p_018 = 0; 00709 int32_t VL53L1_p_001 = 0; 00710 int32_t a_zp = 0; 00711 int32_t c_zp = 0; 00712 int32_t ax = 0; 00713 int32_t bx = 0; 00714 int32_t cx = 0; 00715 00716 00717 00718 i = bin % VL53L1_p_031; 00719 00720 00721 00722 VL53L1_f_013( 00723 i, 00724 VL53L1_p_055, 00725 phist_data_zp, 00726 &a_zp, 00727 &VL53L1_p_018, 00728 &c_zp); 00729 00730 00731 00732 VL53L1_f_013( 00733 i, 00734 VL53L1_p_055, 00735 phist_data_ap, 00736 &VL53L1_p_003, 00737 &VL53L1_p_018, 00738 &VL53L1_p_001); 00739 00740 if (crosstalk_compensation_enable > 0) 00741 VL53L1_f_013( 00742 i, 00743 VL53L1_p_055, 00744 pxtalk_hist, 00745 &ax, 00746 &bx, 00747 &cx); 00748 00749 00750 00751 00752 00753 00754 00755 func_status = 00756 VL53L1_f_014( 00757 sigma_estimator__sigma_ref_mm, 00758 (uint32_t)VL53L1_p_003, 00759 (uint32_t)VL53L1_p_018, 00760 (uint32_t)VL53L1_p_001, 00761 (uint32_t)a_zp, 00762 (uint32_t)c_zp, 00763 (uint32_t)bx, 00764 (uint32_t)ax, 00765 (uint32_t)cx, 00766 (uint32_t)phist_data_ap->VL53L1_p_004, 00767 phist_data_ap->VL53L1_p_019, 00768 psigma_est); 00769 00770 00771 00772 00773 if (func_status == VL53L1_ERROR_DIVISION_BY_ZERO) 00774 *psigma_est = 0xFFFF; 00775 00776 00777 return status; 00778 } 00779 00780 00781 void VL53L1_f_029( 00782 uint8_t range_id, 00783 uint8_t valid_phase_low, 00784 uint8_t valid_phase_high, 00785 uint16_t sigma_thres, 00786 VL53L1_histogram_bin_data_t *pbins, 00787 VL53L1_hist_pulse_data_t *ppulse, 00788 VL53L1_range_data_t *pdata) 00789 { 00790 00791 uint16_t lower_phase_limit = 0; 00792 uint16_t upper_phase_limit = 0; 00793 00794 00795 00796 pdata->range_id = range_id; 00797 pdata->time_stamp = 0; 00798 00799 pdata->VL53L1_p_015 = ppulse->VL53L1_p_015; 00800 pdata->VL53L1_p_022 = ppulse->VL53L1_p_022; 00801 pdata->VL53L1_p_025 = ppulse->VL53L1_p_025; 00802 pdata->VL53L1_p_026 = ppulse->VL53L1_p_026; 00803 pdata->VL53L1_p_016 = ppulse->VL53L1_p_016; 00804 pdata->VL53L1_p_027 = ppulse->VL53L1_p_027; 00805 00806 00807 00808 pdata->VL53L1_p_030 = 00809 (ppulse->VL53L1_p_016 + 1) - ppulse->VL53L1_p_015; 00810 00811 00812 00813 pdata->zero_distance_phase = pbins->zero_distance_phase; 00814 pdata->VL53L1_p_005 = ppulse->VL53L1_p_005; 00815 pdata->VL53L1_p_028 = (uint16_t)ppulse->VL53L1_p_028; 00816 pdata->VL53L1_p_014 = (uint16_t)ppulse->VL53L1_p_014; 00817 pdata->VL53L1_p_029 = (uint16_t)ppulse->VL53L1_p_029; 00818 pdata->VL53L1_p_021 = (uint32_t)ppulse->VL53L1_p_021; 00819 pdata->VL53L1_p_013 = ppulse->VL53L1_p_013; 00820 pdata->VL53L1_p_020 = (uint32_t)ppulse->VL53L1_p_020; 00821 pdata->total_periods_elapsed = pbins->total_periods_elapsed; 00822 00823 00824 00825 pdata->range_status = VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK; 00826 00827 00828 if (sigma_thres > 0 && 00829 (uint32_t)ppulse->VL53L1_p_005 > ((uint32_t)sigma_thres << 5)) 00830 pdata->range_status = VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK; 00831 00832 00833 00834 lower_phase_limit = (uint8_t)valid_phase_low << 8; 00835 if (lower_phase_limit < pdata->zero_distance_phase) 00836 lower_phase_limit = 00837 pdata->zero_distance_phase - 00838 lower_phase_limit; 00839 else 00840 lower_phase_limit = 0; 00841 00842 upper_phase_limit = (uint8_t)valid_phase_high << 8; 00843 upper_phase_limit += pbins->zero_distance_phase; 00844 00845 if (pdata->VL53L1_p_014 < lower_phase_limit || 00846 pdata->VL53L1_p_014 > upper_phase_limit) 00847 pdata->range_status = VL53L1_DEVICEERROR_RANGEPHASECHECK; 00848 00849 } 00850 00851 00852
Generated on Tue Jul 12 2022 20:07:14 by
1.7.2