ST Expansion SW Team / VL53L1CB

The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.

src/vl53l1_hist_algos_gen4.c@7:1add29d51e72, 2021-06-08 (annotated)

Committer:
Charles MacNeill
Date:
Tue Jun 08 10:34:47 2021 +0100
Revision:
7:1add29d51e72
Parent:
0:3ac96e360672
Child:
18:0696efe39d08
Update to v6.6.5 of bare_driver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
Charles MacNeill 7:1add29d51e72 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 7:1add29d51e72 3 /******************************************************************************
Charles MacNeill 7:1add29d51e72 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 5
Charles MacNeill 7:1add29d51e72 6 This file is part of VL53L1 Protected and is dual licensed,
Charles MacNeill 7:1add29d51e72 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 7:1add29d51e72 9
Charles MacNeill 7:1add29d51e72 10 ******************************************************************************
Charles MacNeill 7:1add29d51e72 11
Charles MacNeill 7:1add29d51e72 12 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 13
Charles MacNeill 7:1add29d51e72 14 ******************************************************************************
charlesmn 0:3ac96e360672 15
charlesmn 0:3ac96e360672 16 License terms: STMicroelectronics Proprietary in accordance with licensing
charlesmn 0:3ac96e360672 17 terms at www.st.com/sla0081
charlesmn 0:3ac96e360672 18
Charles MacNeill 7:1add29d51e72 19 ******************************************************************************
Charles MacNeill 7:1add29d51e72 20 */
charlesmn 0:3ac96e360672 21
charlesmn 0:3ac96e360672 22
charlesmn 0:3ac96e360672 23
charlesmn 0:3ac96e360672 24
charlesmn 0:3ac96e360672 25 #include "vl53l1_types.h"
charlesmn 0:3ac96e360672 26 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 27
charlesmn 0:3ac96e360672 28 #include "vl53l1_core_support.h"
charlesmn 0:3ac96e360672 29 #include "vl53l1_error_codes.h"
charlesmn 0:3ac96e360672 30
charlesmn 0:3ac96e360672 31 #include "vl53l1_hist_core.h"
charlesmn 0:3ac96e360672 32 #include "vl53l1_hist_algos_gen3.h"
charlesmn 0:3ac96e360672 33 #include "vl53l1_hist_algos_gen4.h"
charlesmn 0:3ac96e360672 34 #include "vl53l1_sigma_estimate.h"
charlesmn 0:3ac96e360672 35 #include "vl53l1_dmax.h"
charlesmn 0:3ac96e360672 36 #ifdef __KERNEL__
charlesmn 0:3ac96e360672 37 #include <linux/math64.h>
charlesmn 0:3ac96e360672 38 #include <linux/kernel.h>
charlesmn 0:3ac96e360672 39 #endif
charlesmn 0:3ac96e360672 40
charlesmn 0:3ac96e360672 41
charlesmn 0:3ac96e360672 42
charlesmn 0:3ac96e360672 43 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 44 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 45 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 46 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 47 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 48 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 49 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 50
charlesmn 0:3ac96e360672 51 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 52 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 53 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 54
charlesmn 0:3ac96e360672 55
charlesmn 0:3ac96e360672 56 void VL53L1_f_032(
charlesmn 0:3ac96e360672 57 VL53L1_hist_gen4_algo_filtered_data_t *palgo)
charlesmn 0:3ac96e360672 58 {
charlesmn 0:3ac96e360672 59
charlesmn 0:3ac96e360672 60
charlesmn 0:3ac96e360672 61 uint8_t lb = 0;
charlesmn 0:3ac96e360672 62
charlesmn 0:3ac96e360672 63 palgo->VL53L1_p_023 = VL53L1_HISTOGRAM_BUFFER_SIZE;
charlesmn 0:3ac96e360672 64 palgo->VL53L1_p_022 = 0;
charlesmn 0:3ac96e360672 65 palgo->VL53L1_p_024 = 0;
charlesmn 0:3ac96e360672 66
charlesmn 0:3ac96e360672 67 for (lb = palgo->VL53L1_p_022; lb < palgo->VL53L1_p_023; lb++) {
charlesmn 0:3ac96e360672 68 palgo->VL53L1_p_003[lb] = 0;
charlesmn 0:3ac96e360672 69 palgo->VL53L1_p_018[lb] = 0;
charlesmn 0:3ac96e360672 70 palgo->VL53L1_p_001[lb] = 0;
charlesmn 0:3ac96e360672 71 palgo->VL53L1_p_039[lb] = 0;
charlesmn 0:3ac96e360672 72 palgo->VL53L1_p_040[lb] = 0;
charlesmn 0:3ac96e360672 73 palgo->VL53L1_p_043[lb] = 0;
charlesmn 0:3ac96e360672 74 }
charlesmn 0:3ac96e360672 75 }
charlesmn 0:3ac96e360672 76
charlesmn 0:3ac96e360672 77
charlesmn 0:3ac96e360672 78 VL53L1_Error VL53L1_f_033(
charlesmn 0:3ac96e360672 79 VL53L1_dmax_calibration_data_t *pdmax_cal,
charlesmn 0:3ac96e360672 80 VL53L1_hist_gen3_dmax_config_t *pdmax_cfg,
charlesmn 0:3ac96e360672 81 VL53L1_hist_post_process_config_t *ppost_cfg,
charlesmn 0:3ac96e360672 82 VL53L1_histogram_bin_data_t *pbins_input,
charlesmn 0:3ac96e360672 83 VL53L1_histogram_bin_data_t *pxtalk,
charlesmn 0:3ac96e360672 84 VL53L1_hist_gen3_algo_private_data_t *palgo3,
charlesmn 0:3ac96e360672 85 VL53L1_hist_gen4_algo_filtered_data_t *pfiltered,
charlesmn 0:3ac96e360672 86 VL53L1_hist_gen3_dmax_private_data_t *pdmax_algo,
charlesmn 0:3ac96e360672 87 VL53L1_range_results_t *presults,
charlesmn 0:3ac96e360672 88 uint8_t histo_merge_nb)
charlesmn 0:3ac96e360672 89 {
charlesmn 0:3ac96e360672 90
charlesmn 0:3ac96e360672 91
charlesmn 0:3ac96e360672 92 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 93
charlesmn 0:3ac96e360672 94 VL53L1_hist_pulse_data_t *ppulse_data;
charlesmn 0:3ac96e360672 95 VL53L1_range_data_t *prange_data;
charlesmn 0:3ac96e360672 96
charlesmn 0:3ac96e360672 97 uint8_t p = 0;
charlesmn 0:3ac96e360672 98 VL53L1_histogram_bin_data_t *pB = &(palgo3->VL53L1_p_010);
charlesmn 0:3ac96e360672 99
charlesmn 0:3ac96e360672 100 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 101
charlesmn 0:3ac96e360672 102
charlesmn 0:3ac96e360672 103
charlesmn 0:3ac96e360672 104
charlesmn 0:3ac96e360672 105
charlesmn 0:3ac96e360672 106 VL53L1_f_016(palgo3);
charlesmn 0:3ac96e360672 107
charlesmn 0:3ac96e360672 108
charlesmn 0:3ac96e360672 109
charlesmn 0:3ac96e360672 110 memcpy(
charlesmn 0:3ac96e360672 111 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 112 pbins_input,
charlesmn 0:3ac96e360672 113 sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 114
charlesmn 0:3ac96e360672 115
charlesmn 0:3ac96e360672 116
charlesmn 0:3ac96e360672 117 presults->cfg_device_state = pbins_input->cfg_device_state;
charlesmn 0:3ac96e360672 118 presults->rd_device_state = pbins_input->rd_device_state;
charlesmn 0:3ac96e360672 119 presults->zone_id = pbins_input->zone_id;
charlesmn 0:3ac96e360672 120 presults->stream_count = pbins_input->result__stream_count;
charlesmn 0:3ac96e360672 121 presults->wrap_dmax_mm = 0;
charlesmn 0:3ac96e360672 122 presults->max_results = VL53L1_MAX_RANGE_RESULTS;
charlesmn 0:3ac96e360672 123 presults->active_results = 0;
charlesmn 0:3ac96e360672 124
charlesmn 0:3ac96e360672 125 for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++)
charlesmn 0:3ac96e360672 126 presults->VL53L1_p_007[p] = 0;
charlesmn 0:3ac96e360672 127
charlesmn 0:3ac96e360672 128
charlesmn 0:3ac96e360672 129
charlesmn 0:3ac96e360672 130 VL53L1_hist_calc_zero_distance_phase(&(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 131
charlesmn 0:3ac96e360672 132
charlesmn 0:3ac96e360672 133
charlesmn 0:3ac96e360672 134 if (ppost_cfg->hist_amb_est_method ==
charlesmn 0:3ac96e360672 135 VL53L1_HIST_AMB_EST_METHOD__THRESHOLDED_BINS)
charlesmn 0:3ac96e360672 136 VL53L1_hist_estimate_ambient_from_thresholded_bins(
charlesmn 0:3ac96e360672 137 (int32_t)ppost_cfg->ambient_thresh_sigma0,
charlesmn 0:3ac96e360672 138 &(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 139 else
charlesmn 0:3ac96e360672 140 VL53L1_hist_estimate_ambient_from_ambient_bins(
charlesmn 0:3ac96e360672 141 &(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 142
charlesmn 0:3ac96e360672 143
charlesmn 0:3ac96e360672 144 VL53L1_hist_remove_ambient_bins(&(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 145
charlesmn 0:3ac96e360672 146
charlesmn 0:3ac96e360672 147 if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
charlesmn 0:3ac96e360672 148 VL53L1_f_004(
charlesmn 0:3ac96e360672 149 pxtalk,
charlesmn 0:3ac96e360672 150 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 151 &(palgo3->VL53L1_p_038));
charlesmn 0:3ac96e360672 152
charlesmn 0:3ac96e360672 153
charlesmn 0:3ac96e360672 154
charlesmn 0:3ac96e360672 155 pdmax_cfg->ambient_thresh_sigma =
charlesmn 0:3ac96e360672 156 ppost_cfg->ambient_thresh_sigma1;
charlesmn 0:3ac96e360672 157
charlesmn 0:3ac96e360672 158 for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++) {
charlesmn 0:3ac96e360672 159 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 160 status =
charlesmn 0:3ac96e360672 161 VL53L1_f_001(
charlesmn 0:3ac96e360672 162 pdmax_cfg->target_reflectance_for_dmax_calc[p],
charlesmn 0:3ac96e360672 163 pdmax_cal,
charlesmn 0:3ac96e360672 164 pdmax_cfg,
charlesmn 0:3ac96e360672 165 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 166 pdmax_algo,
charlesmn 0:3ac96e360672 167 &(presults->VL53L1_p_007[p]));
charlesmn 0:3ac96e360672 168 }
charlesmn 0:3ac96e360672 169 }
charlesmn 0:3ac96e360672 170
charlesmn 0:3ac96e360672 171
charlesmn 0:3ac96e360672 172
charlesmn 0:3ac96e360672 173
charlesmn 0:3ac96e360672 174
charlesmn 0:3ac96e360672 175 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 176 status =
charlesmn 0:3ac96e360672 177 VL53L1_f_018(
charlesmn 0:3ac96e360672 178 ppost_cfg->ambient_thresh_events_scaler,
charlesmn 0:3ac96e360672 179 (int32_t)ppost_cfg->ambient_thresh_sigma1,
charlesmn 0:3ac96e360672 180 (int32_t)ppost_cfg->min_ambient_thresh_events,
charlesmn 0:3ac96e360672 181 ppost_cfg->algo__crosstalk_compensation_enable,
charlesmn 0:3ac96e360672 182 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 183 &(palgo3->VL53L1_p_038),
charlesmn 0:3ac96e360672 184 palgo3);
charlesmn 0:3ac96e360672 185
charlesmn 0:3ac96e360672 186
charlesmn 0:3ac96e360672 187
charlesmn 0:3ac96e360672 188
charlesmn 0:3ac96e360672 189
charlesmn 0:3ac96e360672 190 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 191 status =
charlesmn 0:3ac96e360672 192 VL53L1_f_019(palgo3);
charlesmn 0:3ac96e360672 193
charlesmn 0:3ac96e360672 194
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 197 status =
charlesmn 0:3ac96e360672 198 VL53L1_f_020(palgo3);
charlesmn 0:3ac96e360672 199
charlesmn 0:3ac96e360672 200
charlesmn 0:3ac96e360672 201
charlesmn 0:3ac96e360672 202 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 203 status =
charlesmn 0:3ac96e360672 204 VL53L1_f_021(palgo3);
charlesmn 0:3ac96e360672 205
charlesmn 0:3ac96e360672 206
charlesmn 0:3ac96e360672 207
charlesmn 0:3ac96e360672 208 for (p = 0; p < palgo3->VL53L1_p_051; p++) {
charlesmn 0:3ac96e360672 209
charlesmn 0:3ac96e360672 210 ppulse_data = &(palgo3->VL53L1_p_002[p]);
charlesmn 0:3ac96e360672 211
charlesmn 0:3ac96e360672 212
charlesmn 0:3ac96e360672 213
charlesmn 0:3ac96e360672 214 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 215 status =
charlesmn 0:3ac96e360672 216 VL53L1_f_022(
charlesmn 0:3ac96e360672 217 p,
charlesmn 0:3ac96e360672 218 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 219 palgo3);
charlesmn 0:3ac96e360672 220
charlesmn 0:3ac96e360672 221
charlesmn 0:3ac96e360672 222
charlesmn 0:3ac96e360672 223 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 224 status =
charlesmn 0:3ac96e360672 225 VL53L1_f_023(
charlesmn 0:3ac96e360672 226 p,
charlesmn 0:3ac96e360672 227 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 228 palgo3,
charlesmn 0:3ac96e360672 229 pB->VL53L1_p_004,
charlesmn 0:3ac96e360672 230 &(palgo3->VL53L1_p_052));
charlesmn 0:3ac96e360672 231
charlesmn 0:3ac96e360672 232
charlesmn 0:3ac96e360672 233
charlesmn 0:3ac96e360672 234 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 235 status =
charlesmn 0:3ac96e360672 236 VL53L1_f_023(
charlesmn 0:3ac96e360672 237 p,
charlesmn 0:3ac96e360672 238 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 239 palgo3,
charlesmn 0:3ac96e360672 240 0,
charlesmn 0:3ac96e360672 241 &(palgo3->VL53L1_p_053));
charlesmn 0:3ac96e360672 242 }
charlesmn 0:3ac96e360672 243
charlesmn 0:3ac96e360672 244
charlesmn 0:3ac96e360672 245
charlesmn 0:3ac96e360672 246 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 247 status =
charlesmn 0:3ac96e360672 248 VL53L1_f_023(
charlesmn 0:3ac96e360672 249 p,
charlesmn 0:3ac96e360672 250 &(palgo3->VL53L1_p_038),
charlesmn 0:3ac96e360672 251 palgo3,
charlesmn 0:3ac96e360672 252 0,
charlesmn 0:3ac96e360672 253 &(palgo3->VL53L1_p_054));
charlesmn 0:3ac96e360672 254 }
charlesmn 0:3ac96e360672 255
charlesmn 0:3ac96e360672 256
charlesmn 0:3ac96e360672 257
charlesmn 0:3ac96e360672 258 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 259 status =
charlesmn 0:3ac96e360672 260 VL53L1_f_034(
charlesmn 0:3ac96e360672 261 p,
charlesmn 0:3ac96e360672 262 &(palgo3->VL53L1_p_052),
charlesmn 0:3ac96e360672 263 palgo3,
charlesmn 0:3ac96e360672 264 pfiltered);
charlesmn 0:3ac96e360672 265
charlesmn 0:3ac96e360672 266
charlesmn 0:3ac96e360672 267
charlesmn 0:3ac96e360672 268 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 269 status =
charlesmn 0:3ac96e360672 270 VL53L1_f_035(
charlesmn 0:3ac96e360672 271 p,
charlesmn 0:3ac96e360672 272 ppost_cfg->noise_threshold,
charlesmn 0:3ac96e360672 273 pfiltered,
charlesmn 0:3ac96e360672 274 palgo3);
charlesmn 0:3ac96e360672 275
charlesmn 0:3ac96e360672 276 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 277 status =
charlesmn 0:3ac96e360672 278 VL53L1_f_026(
charlesmn 0:3ac96e360672 279 ppulse_data->VL53L1_p_025,
charlesmn 0:3ac96e360672 280 ppost_cfg->sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 281 palgo3->VL53L1_p_031,
charlesmn 0:3ac96e360672 282 ppulse_data->VL53L1_p_055,
charlesmn 0:3ac96e360672 283 ppost_cfg->algo__crosstalk_compensation_enable,
charlesmn 0:3ac96e360672 284 &(palgo3->VL53L1_p_052),
charlesmn 0:3ac96e360672 285 &(palgo3->VL53L1_p_053),
charlesmn 0:3ac96e360672 286 &(palgo3->VL53L1_p_054),
charlesmn 0:3ac96e360672 287 &(ppulse_data->VL53L1_p_005));
charlesmn 0:3ac96e360672 288
charlesmn 0:3ac96e360672 289
charlesmn 0:3ac96e360672 290
charlesmn 0:3ac96e360672 291 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 292 status =
charlesmn 0:3ac96e360672 293 VL53L1_f_027(
charlesmn 0:3ac96e360672 294 p,
charlesmn 0:3ac96e360672 295 1,
charlesmn 0:3ac96e360672 296 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 297 palgo3);
charlesmn 0:3ac96e360672 298
charlesmn 0:3ac96e360672 299 }
charlesmn 0:3ac96e360672 300
charlesmn 0:3ac96e360672 301
charlesmn 0:3ac96e360672 302
charlesmn 0:3ac96e360672 303 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 304 status =
charlesmn 0:3ac96e360672 305 VL53L1_f_028(
charlesmn 0:3ac96e360672 306 ppost_cfg->hist_target_order,
charlesmn 0:3ac96e360672 307 palgo3);
charlesmn 0:3ac96e360672 308
charlesmn 0:3ac96e360672 309
charlesmn 0:3ac96e360672 310
charlesmn 0:3ac96e360672 311 for (p = 0; p < palgo3->VL53L1_p_051; p++) {
charlesmn 0:3ac96e360672 312
charlesmn 0:3ac96e360672 313 ppulse_data = &(palgo3->VL53L1_p_002[p]);
charlesmn 0:3ac96e360672 314
charlesmn 0:3ac96e360672 315
charlesmn 0:3ac96e360672 316 if (!(presults->active_results < presults->max_results))
charlesmn 0:3ac96e360672 317 continue;
charlesmn 0:3ac96e360672 318
charlesmn 0:3ac96e360672 319
charlesmn 0:3ac96e360672 320
charlesmn 0:3ac96e360672 321
charlesmn 0:3ac96e360672 322 if (ppulse_data->VL53L1_p_013 >
charlesmn 0:3ac96e360672 323 ppost_cfg->signal_total_events_limit &&
charlesmn 0:3ac96e360672 324 ppulse_data->VL53L1_p_025 < 0xFF) {
charlesmn 0:3ac96e360672 325
charlesmn 0:3ac96e360672 326 prange_data =
charlesmn 0:3ac96e360672 327 &(presults->VL53L1_p_002[presults->active_results]);
charlesmn 0:3ac96e360672 328
charlesmn 0:3ac96e360672 329 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 330 VL53L1_f_029(
charlesmn 0:3ac96e360672 331 presults->active_results,
charlesmn 0:3ac96e360672 332 ppost_cfg->valid_phase_low,
charlesmn 0:3ac96e360672 333 ppost_cfg->valid_phase_high,
charlesmn 0:3ac96e360672 334 ppost_cfg->sigma_thresh,
charlesmn 0:3ac96e360672 335 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 336 ppulse_data,
charlesmn 0:3ac96e360672 337 prange_data);
charlesmn 0:3ac96e360672 338
charlesmn 0:3ac96e360672 339 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 340 status =
charlesmn 0:3ac96e360672 341 VL53L1_f_011(
charlesmn 0:3ac96e360672 342 pB->vcsel_width,
charlesmn 0:3ac96e360672 343 pB->VL53L1_p_019,
charlesmn 0:3ac96e360672 344 pB->total_periods_elapsed,
charlesmn 0:3ac96e360672 345 pB->result__dss_actual_effective_spads,
charlesmn 0:3ac96e360672 346 prange_data,
charlesmn 0:3ac96e360672 347 histo_merge_nb);
charlesmn 0:3ac96e360672 348
charlesmn 0:3ac96e360672 349 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 350 VL53L1_f_012(
charlesmn 0:3ac96e360672 351 ppost_cfg->gain_factor,
charlesmn 0:3ac96e360672 352 ppost_cfg->range_offset_mm,
charlesmn 0:3ac96e360672 353 prange_data);
charlesmn 0:3ac96e360672 354
charlesmn 0:3ac96e360672 355 presults->active_results++;
charlesmn 0:3ac96e360672 356 }
charlesmn 0:3ac96e360672 357
charlesmn 0:3ac96e360672 358 }
charlesmn 0:3ac96e360672 359
charlesmn 0:3ac96e360672 360
charlesmn 0:3ac96e360672 361
charlesmn 0:3ac96e360672 362 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 363
charlesmn 0:3ac96e360672 364 return status;
charlesmn 0:3ac96e360672 365 }
charlesmn 0:3ac96e360672 366
charlesmn 0:3ac96e360672 367
charlesmn 0:3ac96e360672 368
charlesmn 0:3ac96e360672 369 VL53L1_Error VL53L1_f_034(
charlesmn 0:3ac96e360672 370 uint8_t pulse_no,
charlesmn 0:3ac96e360672 371 VL53L1_histogram_bin_data_t *ppulse,
charlesmn 0:3ac96e360672 372 VL53L1_hist_gen3_algo_private_data_t *palgo3,
charlesmn 0:3ac96e360672 373 VL53L1_hist_gen4_algo_filtered_data_t *pfiltered)
charlesmn 0:3ac96e360672 374 {
charlesmn 0:3ac96e360672 375
charlesmn 0:3ac96e360672 376
charlesmn 0:3ac96e360672 377
charlesmn 0:3ac96e360672 378
charlesmn 0:3ac96e360672 379 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 380
charlesmn 0:3ac96e360672 381 VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 382
charlesmn 0:3ac96e360672 383 uint8_t lb = 0;
charlesmn 0:3ac96e360672 384 uint8_t i = 0;
charlesmn 0:3ac96e360672 385 int32_t suma = 0;
charlesmn 0:3ac96e360672 386 int32_t sumb = 0;
charlesmn 0:3ac96e360672 387 int32_t sumc = 0;
charlesmn 0:3ac96e360672 388
charlesmn 0:3ac96e360672 389 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 390
charlesmn 0:3ac96e360672 391 pfiltered->VL53L1_p_023 = palgo3->VL53L1_p_023;
charlesmn 0:3ac96e360672 392 pfiltered->VL53L1_p_022 = palgo3->VL53L1_p_022;
charlesmn 0:3ac96e360672 393 pfiltered->VL53L1_p_024 = palgo3->VL53L1_p_024;
charlesmn 0:3ac96e360672 394
charlesmn 0:3ac96e360672 395
charlesmn 0:3ac96e360672 396
charlesmn 0:3ac96e360672 397 for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) {
charlesmn 0:3ac96e360672 398
charlesmn 0:3ac96e360672 399 i = lb % palgo3->VL53L1_p_031;
charlesmn 0:3ac96e360672 400
charlesmn 0:3ac96e360672 401
charlesmn 0:3ac96e360672 402 VL53L1_f_013(
charlesmn 0:3ac96e360672 403 i,
charlesmn 0:3ac96e360672 404 pdata->VL53L1_p_055,
charlesmn 0:3ac96e360672 405 ppulse,
charlesmn 0:3ac96e360672 406 &suma,
charlesmn 0:3ac96e360672 407 &sumb,
charlesmn 0:3ac96e360672 408 &sumc);
charlesmn 0:3ac96e360672 409
charlesmn 0:3ac96e360672 410
charlesmn 0:3ac96e360672 411 pfiltered->VL53L1_p_003[i] = suma;
charlesmn 0:3ac96e360672 412 pfiltered->VL53L1_p_018[i] = sumb;
charlesmn 0:3ac96e360672 413 pfiltered->VL53L1_p_001[i] = sumc;
charlesmn 0:3ac96e360672 414
charlesmn 0:3ac96e360672 415
charlesmn 0:3ac96e360672 416
charlesmn 0:3ac96e360672 417 pfiltered->VL53L1_p_039[i] =
charlesmn 0:3ac96e360672 418 (suma + sumb) -
charlesmn 0:3ac96e360672 419 (sumc + palgo3->VL53L1_p_004);
charlesmn 0:3ac96e360672 420
charlesmn 0:3ac96e360672 421
charlesmn 0:3ac96e360672 422
charlesmn 0:3ac96e360672 423 pfiltered->VL53L1_p_040[i] =
charlesmn 0:3ac96e360672 424 (sumb + sumc) -
charlesmn 0:3ac96e360672 425 (suma + palgo3->VL53L1_p_004);
charlesmn 0:3ac96e360672 426 }
charlesmn 0:3ac96e360672 427
charlesmn 0:3ac96e360672 428 return status;
charlesmn 0:3ac96e360672 429 }
charlesmn 0:3ac96e360672 430
charlesmn 0:3ac96e360672 431
charlesmn 0:3ac96e360672 432 VL53L1_Error VL53L1_f_035(
charlesmn 0:3ac96e360672 433 uint8_t pulse_no,
charlesmn 0:3ac96e360672 434 uint16_t noise_threshold,
charlesmn 0:3ac96e360672 435 VL53L1_hist_gen4_algo_filtered_data_t *pfiltered,
charlesmn 0:3ac96e360672 436 VL53L1_hist_gen3_algo_private_data_t *palgo3)
charlesmn 0:3ac96e360672 437 {
charlesmn 0:3ac96e360672 438
charlesmn 0:3ac96e360672 439
charlesmn 0:3ac96e360672 440
charlesmn 0:3ac96e360672 441 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 442 VL53L1_Error func_status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 443
charlesmn 0:3ac96e360672 444 VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 445
charlesmn 0:3ac96e360672 446 uint8_t lb = 0;
charlesmn 0:3ac96e360672 447 uint8_t i = 0;
charlesmn 0:3ac96e360672 448 uint8_t j = 0;
charlesmn 0:3ac96e360672 449
charlesmn 0:3ac96e360672 450 SUPPRESS_UNUSED_WARNING(noise_threshold);
charlesmn 0:3ac96e360672 451
charlesmn 0:3ac96e360672 452 for (lb = pdata->VL53L1_p_015; lb < pdata->VL53L1_p_016; lb++) {
charlesmn 0:3ac96e360672 453
charlesmn 0:3ac96e360672 454 i = lb % palgo3->VL53L1_p_031;
charlesmn 0:3ac96e360672 455 j = (lb+1) % palgo3->VL53L1_p_031;
charlesmn 0:3ac96e360672 456
charlesmn 0:3ac96e360672 457 if (i < palgo3->VL53L1_p_024 &&
charlesmn 0:3ac96e360672 458 j < palgo3->VL53L1_p_024) {
charlesmn 0:3ac96e360672 459
charlesmn 0:3ac96e360672 460 if (pfiltered->VL53L1_p_039[i] == 0 &&
charlesmn 0:3ac96e360672 461 pfiltered->VL53L1_p_040[i] == 0)
charlesmn 0:3ac96e360672 462
charlesmn 0:3ac96e360672 463 pfiltered->VL53L1_p_043[i] = 0;
charlesmn 0:3ac96e360672 464
charlesmn 0:3ac96e360672 465 else if (pfiltered->VL53L1_p_039[i] >= 0 &&
charlesmn 0:3ac96e360672 466 pfiltered->VL53L1_p_040[i] >= 0)
charlesmn 0:3ac96e360672 467 pfiltered->VL53L1_p_043[i] = 1;
charlesmn 0:3ac96e360672 468
charlesmn 0:3ac96e360672 469 else if (pfiltered->VL53L1_p_039[i] < 0 &&
charlesmn 0:3ac96e360672 470 pfiltered->VL53L1_p_040[i] >= 0 &&
charlesmn 0:3ac96e360672 471 pfiltered->VL53L1_p_039[j] >= 0 &&
charlesmn 0:3ac96e360672 472 pfiltered->VL53L1_p_040[j] < 0)
charlesmn 0:3ac96e360672 473 pfiltered->VL53L1_p_043[i] = 1;
charlesmn 0:3ac96e360672 474
charlesmn 0:3ac96e360672 475 else
charlesmn 0:3ac96e360672 476 pfiltered->VL53L1_p_043[i] = 0;
charlesmn 0:3ac96e360672 477
charlesmn 0:3ac96e360672 478
charlesmn 0:3ac96e360672 479 if (pfiltered->VL53L1_p_043[i] > 0) {
charlesmn 0:3ac96e360672 480
charlesmn 0:3ac96e360672 481 pdata->VL53L1_p_025 = lb;
charlesmn 0:3ac96e360672 482
charlesmn 0:3ac96e360672 483 func_status =
charlesmn 0:3ac96e360672 484 VL53L1_f_036(
charlesmn 0:3ac96e360672 485 lb,
charlesmn 0:3ac96e360672 486 pfiltered->VL53L1_p_003[i],
charlesmn 0:3ac96e360672 487 pfiltered->VL53L1_p_018[i],
charlesmn 0:3ac96e360672 488 pfiltered->VL53L1_p_001[i],
charlesmn 0:3ac96e360672 489 0,
charlesmn 0:3ac96e360672 490 0,
charlesmn 0:3ac96e360672 491 0,
charlesmn 0:3ac96e360672 492 palgo3->VL53L1_p_004,
charlesmn 0:3ac96e360672 493 palgo3->VL53L1_p_031,
charlesmn 0:3ac96e360672 494 &(pdata->VL53L1_p_014));
charlesmn 0:3ac96e360672 495
charlesmn 0:3ac96e360672 496 if (func_status ==
charlesmn 0:3ac96e360672 497 VL53L1_ERROR_DIVISION_BY_ZERO)
charlesmn 0:3ac96e360672 498 pfiltered->VL53L1_p_043[i] = 0;
charlesmn 0:3ac96e360672 499
charlesmn 0:3ac96e360672 500 }
charlesmn 0:3ac96e360672 501 }
charlesmn 0:3ac96e360672 502 }
charlesmn 0:3ac96e360672 503
charlesmn 0:3ac96e360672 504 return status;
charlesmn 0:3ac96e360672 505 }
charlesmn 0:3ac96e360672 506
charlesmn 0:3ac96e360672 507
charlesmn 0:3ac96e360672 508 VL53L1_Error VL53L1_f_036(
charlesmn 0:3ac96e360672 509 uint8_t bin,
charlesmn 0:3ac96e360672 510 int32_t VL53L1_p_003,
charlesmn 0:3ac96e360672 511 int32_t VL53L1_p_018,
charlesmn 0:3ac96e360672 512 int32_t VL53L1_p_001,
charlesmn 0:3ac96e360672 513 int32_t ax,
charlesmn 0:3ac96e360672 514 int32_t bx,
charlesmn 0:3ac96e360672 515 int32_t cx,
charlesmn 0:3ac96e360672 516 int32_t VL53L1_p_004,
charlesmn 0:3ac96e360672 517 uint8_t VL53L1_p_031,
charlesmn 0:3ac96e360672 518 uint32_t *pmean_phase)
charlesmn 0:3ac96e360672 519 {
charlesmn 0:3ac96e360672 520
charlesmn 0:3ac96e360672 521
charlesmn 0:3ac96e360672 522 VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
charlesmn 0:3ac96e360672 523
charlesmn 0:3ac96e360672 524 int64_t mean_phase = VL53L1_MAX_ALLOWED_PHASE;
charlesmn 0:3ac96e360672 525 int32_t mean_phase32;
charlesmn 0:3ac96e360672 526 int64_t VL53L1_p_041 = 0;
charlesmn 0:3ac96e360672 527 int64_t half_b_minus_amb = 0;
charlesmn 0:3ac96e360672 528
charlesmn 0:3ac96e360672 529
charlesmn 0:3ac96e360672 530 VL53L1_p_041 = 4096 * ((int64_t)VL53L1_p_001 -
charlesmn 0:3ac96e360672 531 (int64_t)cx - (int64_t)VL53L1_p_003 - (int64_t)ax);
charlesmn 0:3ac96e360672 532 half_b_minus_amb = 4096 * ((int64_t)VL53L1_p_018 -
charlesmn 0:3ac96e360672 533 (int64_t)bx - (int64_t)VL53L1_p_004);
charlesmn 0:3ac96e360672 534
charlesmn 0:3ac96e360672 535 if (half_b_minus_amb != 0) {
charlesmn 0:3ac96e360672 536 mean_phase = (4096 * VL53L1_p_041) + half_b_minus_amb;
charlesmn 0:3ac96e360672 537 mean_phase = do_division_s(mean_phase, (half_b_minus_amb * 2));
charlesmn 0:3ac96e360672 538 mean_phase += 2048;
charlesmn 0:3ac96e360672 539 mean_phase += (4096 * (int64_t)bin);
charlesmn 0:3ac96e360672 540
charlesmn 0:3ac96e360672 541 mean_phase = do_division_s((mean_phase + 1), 2);
charlesmn 0:3ac96e360672 542
charlesmn 0:3ac96e360672 543 if (mean_phase < 0)
charlesmn 0:3ac96e360672 544 mean_phase = 0;
charlesmn 0:3ac96e360672 545 if (mean_phase > VL53L1_MAX_ALLOWED_PHASE)
charlesmn 0:3ac96e360672 546 mean_phase = VL53L1_MAX_ALLOWED_PHASE;
charlesmn 0:3ac96e360672 547
charlesmn 0:3ac96e360672 548 mean_phase32 = (int32_t)mean_phase;
charlesmn 0:3ac96e360672 549 mean_phase32 = mean_phase32 %
charlesmn 0:3ac96e360672 550 ((int32_t)VL53L1_p_031 * 2048);
charlesmn 0:3ac96e360672 551 mean_phase = mean_phase32;
charlesmn 0:3ac96e360672 552
charlesmn 0:3ac96e360672 553 status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 554 }
charlesmn 0:3ac96e360672 555
charlesmn 0:3ac96e360672 556 *pmean_phase = (uint32_t)mean_phase;
charlesmn 0:3ac96e360672 557
charlesmn 0:3ac96e360672 558 return status;
charlesmn 0:3ac96e360672 559 }
charlesmn 0:3ac96e360672 560
charlesmn 0:3ac96e360672 561