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@0:3ac96e360672, 2020-11-06 (annotated)

Committer:
charlesmn
Date:
Fri Nov 06 10:06:37 2020 +0000
Revision:
0:3ac96e360672
Child:
7:1add29d51e72
Library for ST Vl53L1A1 time of flight sensor.

Who changed what in which revision?

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