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@18:0696efe39d08, 2021-07-21 (annotated)

Committer:
lugandc
Date:
Wed Jul 21 17:06:38 2021 +0200
Revision:
18:0696efe39d08
Parent:
7:1add29d51e72 
Cleanup i2c functions, removed all bad references to L1X

Cleanup VL53L1CB class:

 - i2c device object is passed in a consistent way in MyDevice structure

 - removed useless functions

Updated VL53L1CB component driver with bare driver release 6.6.7 content

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);
lugandc 18:0696efe39d08 99 uint16_t enabled_spads_ninety_percent;
lugandc 18:0696efe39d08 100 int32_t amb_threshold_sigma;
charlesmn 0:3ac96e360672 101
charlesmn 0:3ac96e360672 102 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 103
charlesmn 0:3ac96e360672 104
charlesmn 0:3ac96e360672 105
charlesmn 0:3ac96e360672 106
charlesmn 0:3ac96e360672 107
charlesmn 0:3ac96e360672 108 VL53L1_f_016(palgo3);
charlesmn 0:3ac96e360672 109
charlesmn 0:3ac96e360672 110
charlesmn 0:3ac96e360672 111
charlesmn 0:3ac96e360672 112 memcpy(
charlesmn 0:3ac96e360672 113 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 114 pbins_input,
charlesmn 0:3ac96e360672 115 sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 116
charlesmn 0:3ac96e360672 117
charlesmn 0:3ac96e360672 118
charlesmn 0:3ac96e360672 119 presults->cfg_device_state = pbins_input->cfg_device_state;
charlesmn 0:3ac96e360672 120 presults->rd_device_state = pbins_input->rd_device_state;
charlesmn 0:3ac96e360672 121 presults->zone_id = pbins_input->zone_id;
charlesmn 0:3ac96e360672 122 presults->stream_count = pbins_input->result__stream_count;
charlesmn 0:3ac96e360672 123 presults->wrap_dmax_mm = 0;
charlesmn 0:3ac96e360672 124 presults->max_results = VL53L1_MAX_RANGE_RESULTS;
charlesmn 0:3ac96e360672 125 presults->active_results = 0;
charlesmn 0:3ac96e360672 126
charlesmn 0:3ac96e360672 127 for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++)
charlesmn 0:3ac96e360672 128 presults->VL53L1_p_007[p] = 0;
charlesmn 0:3ac96e360672 129
charlesmn 0:3ac96e360672 130
charlesmn 0:3ac96e360672 131
charlesmn 0:3ac96e360672 132 VL53L1_hist_calc_zero_distance_phase(&(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 133
charlesmn 0:3ac96e360672 134
charlesmn 0:3ac96e360672 135
charlesmn 0:3ac96e360672 136 if (ppost_cfg->hist_amb_est_method ==
charlesmn 0:3ac96e360672 137 VL53L1_HIST_AMB_EST_METHOD__THRESHOLDED_BINS)
charlesmn 0:3ac96e360672 138 VL53L1_hist_estimate_ambient_from_thresholded_bins(
charlesmn 0:3ac96e360672 139 (int32_t)ppost_cfg->ambient_thresh_sigma0,
charlesmn 0:3ac96e360672 140 &(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 141 else
charlesmn 0:3ac96e360672 142 VL53L1_hist_estimate_ambient_from_ambient_bins(
charlesmn 0:3ac96e360672 143 &(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 144
charlesmn 0:3ac96e360672 145
charlesmn 0:3ac96e360672 146 VL53L1_hist_remove_ambient_bins(&(palgo3->VL53L1_p_010));
charlesmn 0:3ac96e360672 147
charlesmn 0:3ac96e360672 148
charlesmn 0:3ac96e360672 149 if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
charlesmn 0:3ac96e360672 150 VL53L1_f_004(
charlesmn 0:3ac96e360672 151 pxtalk,
charlesmn 0:3ac96e360672 152 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 153 &(palgo3->VL53L1_p_038));
charlesmn 0:3ac96e360672 154
charlesmn 0:3ac96e360672 155
lugandc 18:0696efe39d08 156 amb_threshold_sigma = ppost_cfg->ambient_thresh_sigma1;
lugandc 18:0696efe39d08 157 enabled_spads_ninety_percent =
lugandc 18:0696efe39d08 158 presults->fmt_total_enabled_spads * 230;
lugandc 18:0696efe39d08 159 if ((pbins_input->result__dss_actual_effective_spads <
lugandc 18:0696efe39d08 160 enabled_spads_ninety_percent) &&
lugandc 18:0696efe39d08 161 (presults->VL53L1_p_002[0].ambient_count_rate_mcps < (5 * 128))){
lugandc 18:0696efe39d08 162 amb_threshold_sigma *= histo_merge_nb;
lugandc 18:0696efe39d08 163 }
charlesmn 0:3ac96e360672 164
lugandc 18:0696efe39d08 165 pdmax_cfg->ambient_thresh_sigma = amb_threshold_sigma;
charlesmn 0:3ac96e360672 166
charlesmn 0:3ac96e360672 167 for (p = 0; p < VL53L1_MAX_AMBIENT_DMAX_VALUES; p++) {
charlesmn 0:3ac96e360672 168 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 169 status =
charlesmn 0:3ac96e360672 170 VL53L1_f_001(
charlesmn 0:3ac96e360672 171 pdmax_cfg->target_reflectance_for_dmax_calc[p],
charlesmn 0:3ac96e360672 172 pdmax_cal,
charlesmn 0:3ac96e360672 173 pdmax_cfg,
charlesmn 0:3ac96e360672 174 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 175 pdmax_algo,
charlesmn 0:3ac96e360672 176 &(presults->VL53L1_p_007[p]));
charlesmn 0:3ac96e360672 177 }
charlesmn 0:3ac96e360672 178 }
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_018(
charlesmn 0:3ac96e360672 187 ppost_cfg->ambient_thresh_events_scaler,
lugandc 18:0696efe39d08 188 (int32_t)amb_threshold_sigma,
charlesmn 0:3ac96e360672 189 (int32_t)ppost_cfg->min_ambient_thresh_events,
charlesmn 0:3ac96e360672 190 ppost_cfg->algo__crosstalk_compensation_enable,
charlesmn 0:3ac96e360672 191 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 192 &(palgo3->VL53L1_p_038),
charlesmn 0:3ac96e360672 193 palgo3);
charlesmn 0:3ac96e360672 194
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196
charlesmn 0:3ac96e360672 197
charlesmn 0:3ac96e360672 198
charlesmn 0:3ac96e360672 199 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 200 status =
charlesmn 0:3ac96e360672 201 VL53L1_f_019(palgo3);
charlesmn 0:3ac96e360672 202
charlesmn 0:3ac96e360672 203
charlesmn 0:3ac96e360672 204
charlesmn 0:3ac96e360672 205 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 206 status =
charlesmn 0:3ac96e360672 207 VL53L1_f_020(palgo3);
charlesmn 0:3ac96e360672 208
charlesmn 0:3ac96e360672 209
charlesmn 0:3ac96e360672 210
charlesmn 0:3ac96e360672 211 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 212 status =
charlesmn 0:3ac96e360672 213 VL53L1_f_021(palgo3);
charlesmn 0:3ac96e360672 214
charlesmn 0:3ac96e360672 215
charlesmn 0:3ac96e360672 216
charlesmn 0:3ac96e360672 217 for (p = 0; p < palgo3->VL53L1_p_051; p++) {
charlesmn 0:3ac96e360672 218
charlesmn 0:3ac96e360672 219 ppulse_data = &(palgo3->VL53L1_p_002[p]);
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_022(
charlesmn 0:3ac96e360672 226 p,
charlesmn 0:3ac96e360672 227 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 228 palgo3);
charlesmn 0:3ac96e360672 229
charlesmn 0:3ac96e360672 230
charlesmn 0:3ac96e360672 231
charlesmn 0:3ac96e360672 232 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 233 status =
charlesmn 0:3ac96e360672 234 VL53L1_f_023(
charlesmn 0:3ac96e360672 235 p,
charlesmn 0:3ac96e360672 236 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 237 palgo3,
charlesmn 0:3ac96e360672 238 pB->VL53L1_p_004,
charlesmn 0:3ac96e360672 239 &(palgo3->VL53L1_p_052));
charlesmn 0:3ac96e360672 240
charlesmn 0:3ac96e360672 241
charlesmn 0:3ac96e360672 242
charlesmn 0:3ac96e360672 243 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 244 status =
charlesmn 0:3ac96e360672 245 VL53L1_f_023(
charlesmn 0:3ac96e360672 246 p,
charlesmn 0:3ac96e360672 247 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 248 palgo3,
charlesmn 0:3ac96e360672 249 0,
charlesmn 0:3ac96e360672 250 &(palgo3->VL53L1_p_053));
charlesmn 0:3ac96e360672 251 }
charlesmn 0:3ac96e360672 252
charlesmn 0:3ac96e360672 253
charlesmn 0:3ac96e360672 254
charlesmn 0:3ac96e360672 255 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 256 status =
charlesmn 0:3ac96e360672 257 VL53L1_f_023(
charlesmn 0:3ac96e360672 258 p,
charlesmn 0:3ac96e360672 259 &(palgo3->VL53L1_p_038),
charlesmn 0:3ac96e360672 260 palgo3,
charlesmn 0:3ac96e360672 261 0,
charlesmn 0:3ac96e360672 262 &(palgo3->VL53L1_p_054));
charlesmn 0:3ac96e360672 263 }
charlesmn 0:3ac96e360672 264
charlesmn 0:3ac96e360672 265
charlesmn 0:3ac96e360672 266
charlesmn 0:3ac96e360672 267 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 268 status =
charlesmn 0:3ac96e360672 269 VL53L1_f_034(
charlesmn 0:3ac96e360672 270 p,
charlesmn 0:3ac96e360672 271 &(palgo3->VL53L1_p_052),
charlesmn 0:3ac96e360672 272 palgo3,
charlesmn 0:3ac96e360672 273 pfiltered);
charlesmn 0:3ac96e360672 274
charlesmn 0:3ac96e360672 275
charlesmn 0:3ac96e360672 276
charlesmn 0:3ac96e360672 277 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 278 status =
charlesmn 0:3ac96e360672 279 VL53L1_f_035(
charlesmn 0:3ac96e360672 280 p,
charlesmn 0:3ac96e360672 281 ppost_cfg->noise_threshold,
charlesmn 0:3ac96e360672 282 pfiltered,
charlesmn 0:3ac96e360672 283 palgo3);
charlesmn 0:3ac96e360672 284
charlesmn 0:3ac96e360672 285 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 286 status =
charlesmn 0:3ac96e360672 287 VL53L1_f_026(
charlesmn 0:3ac96e360672 288 ppulse_data->VL53L1_p_025,
charlesmn 0:3ac96e360672 289 ppost_cfg->sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 290 palgo3->VL53L1_p_031,
charlesmn 0:3ac96e360672 291 ppulse_data->VL53L1_p_055,
charlesmn 0:3ac96e360672 292 ppost_cfg->algo__crosstalk_compensation_enable,
charlesmn 0:3ac96e360672 293 &(palgo3->VL53L1_p_052),
charlesmn 0:3ac96e360672 294 &(palgo3->VL53L1_p_053),
charlesmn 0:3ac96e360672 295 &(palgo3->VL53L1_p_054),
charlesmn 0:3ac96e360672 296 &(ppulse_data->VL53L1_p_005));
charlesmn 0:3ac96e360672 297
charlesmn 0:3ac96e360672 298
charlesmn 0:3ac96e360672 299
charlesmn 0:3ac96e360672 300 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 301 status =
charlesmn 0:3ac96e360672 302 VL53L1_f_027(
charlesmn 0:3ac96e360672 303 p,
charlesmn 0:3ac96e360672 304 1,
charlesmn 0:3ac96e360672 305 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 306 palgo3);
charlesmn 0:3ac96e360672 307
charlesmn 0:3ac96e360672 308 }
charlesmn 0:3ac96e360672 309
charlesmn 0:3ac96e360672 310
charlesmn 0:3ac96e360672 311
charlesmn 0:3ac96e360672 312 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 313 status =
charlesmn 0:3ac96e360672 314 VL53L1_f_028(
charlesmn 0:3ac96e360672 315 ppost_cfg->hist_target_order,
charlesmn 0:3ac96e360672 316 palgo3);
charlesmn 0:3ac96e360672 317
charlesmn 0:3ac96e360672 318
charlesmn 0:3ac96e360672 319
charlesmn 0:3ac96e360672 320 for (p = 0; p < palgo3->VL53L1_p_051; p++) {
charlesmn 0:3ac96e360672 321
charlesmn 0:3ac96e360672 322 ppulse_data = &(palgo3->VL53L1_p_002[p]);
charlesmn 0:3ac96e360672 323
charlesmn 0:3ac96e360672 324
charlesmn 0:3ac96e360672 325 if (!(presults->active_results < presults->max_results))
charlesmn 0:3ac96e360672 326 continue;
charlesmn 0:3ac96e360672 327
charlesmn 0:3ac96e360672 328
charlesmn 0:3ac96e360672 329
charlesmn 0:3ac96e360672 330
charlesmn 0:3ac96e360672 331 if (ppulse_data->VL53L1_p_013 >
charlesmn 0:3ac96e360672 332 ppost_cfg->signal_total_events_limit &&
charlesmn 0:3ac96e360672 333 ppulse_data->VL53L1_p_025 < 0xFF) {
charlesmn 0:3ac96e360672 334
charlesmn 0:3ac96e360672 335 prange_data =
charlesmn 0:3ac96e360672 336 &(presults->VL53L1_p_002[presults->active_results]);
charlesmn 0:3ac96e360672 337
charlesmn 0:3ac96e360672 338 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 339 VL53L1_f_029(
charlesmn 0:3ac96e360672 340 presults->active_results,
charlesmn 0:3ac96e360672 341 ppost_cfg->valid_phase_low,
charlesmn 0:3ac96e360672 342 ppost_cfg->valid_phase_high,
charlesmn 0:3ac96e360672 343 ppost_cfg->sigma_thresh,
charlesmn 0:3ac96e360672 344 &(palgo3->VL53L1_p_010),
charlesmn 0:3ac96e360672 345 ppulse_data,
charlesmn 0:3ac96e360672 346 prange_data);
charlesmn 0:3ac96e360672 347
charlesmn 0:3ac96e360672 348 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 349 status =
charlesmn 0:3ac96e360672 350 VL53L1_f_011(
charlesmn 0:3ac96e360672 351 pB->vcsel_width,
charlesmn 0:3ac96e360672 352 pB->VL53L1_p_019,
charlesmn 0:3ac96e360672 353 pB->total_periods_elapsed,
charlesmn 0:3ac96e360672 354 pB->result__dss_actual_effective_spads,
charlesmn 0:3ac96e360672 355 prange_data,
charlesmn 0:3ac96e360672 356 histo_merge_nb);
charlesmn 0:3ac96e360672 357
charlesmn 0:3ac96e360672 358 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 359 VL53L1_f_012(
charlesmn 0:3ac96e360672 360 ppost_cfg->gain_factor,
charlesmn 0:3ac96e360672 361 ppost_cfg->range_offset_mm,
charlesmn 0:3ac96e360672 362 prange_data);
charlesmn 0:3ac96e360672 363
charlesmn 0:3ac96e360672 364 presults->active_results++;
charlesmn 0:3ac96e360672 365 }
charlesmn 0:3ac96e360672 366
charlesmn 0:3ac96e360672 367 }
charlesmn 0:3ac96e360672 368
charlesmn 0:3ac96e360672 369
charlesmn 0:3ac96e360672 370
charlesmn 0:3ac96e360672 371 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 372
charlesmn 0:3ac96e360672 373 return status;
charlesmn 0:3ac96e360672 374 }
charlesmn 0:3ac96e360672 375
charlesmn 0:3ac96e360672 376
charlesmn 0:3ac96e360672 377
charlesmn 0:3ac96e360672 378 VL53L1_Error VL53L1_f_034(
charlesmn 0:3ac96e360672 379 uint8_t pulse_no,
charlesmn 0:3ac96e360672 380 VL53L1_histogram_bin_data_t *ppulse,
charlesmn 0:3ac96e360672 381 VL53L1_hist_gen3_algo_private_data_t *palgo3,
charlesmn 0:3ac96e360672 382 VL53L1_hist_gen4_algo_filtered_data_t *pfiltered)
charlesmn 0:3ac96e360672 383 {
charlesmn 0:3ac96e360672 384
charlesmn 0:3ac96e360672 385
charlesmn 0:3ac96e360672 386
charlesmn 0:3ac96e360672 387
charlesmn 0:3ac96e360672 388 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 389
charlesmn 0:3ac96e360672 390 VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 391
charlesmn 0:3ac96e360672 392 uint8_t lb = 0;
charlesmn 0:3ac96e360672 393 uint8_t i = 0;
charlesmn 0:3ac96e360672 394 int32_t suma = 0;
charlesmn 0:3ac96e360672 395 int32_t sumb = 0;
charlesmn 0:3ac96e360672 396 int32_t sumc = 0;
charlesmn 0:3ac96e360672 397
charlesmn 0:3ac96e360672 398 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 399
charlesmn 0:3ac96e360672 400 pfiltered->VL53L1_p_023 = palgo3->VL53L1_p_023;
charlesmn 0:3ac96e360672 401 pfiltered->VL53L1_p_022 = palgo3->VL53L1_p_022;
charlesmn 0:3ac96e360672 402 pfiltered->VL53L1_p_024 = palgo3->VL53L1_p_024;
charlesmn 0:3ac96e360672 403
charlesmn 0:3ac96e360672 404
charlesmn 0:3ac96e360672 405
charlesmn 0:3ac96e360672 406 for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) {
charlesmn 0:3ac96e360672 407
charlesmn 0:3ac96e360672 408 i = lb % palgo3->VL53L1_p_031;
charlesmn 0:3ac96e360672 409
charlesmn 0:3ac96e360672 410
charlesmn 0:3ac96e360672 411 VL53L1_f_013(
charlesmn 0:3ac96e360672 412 i,
charlesmn 0:3ac96e360672 413 pdata->VL53L1_p_055,
charlesmn 0:3ac96e360672 414 ppulse,
charlesmn 0:3ac96e360672 415 &suma,
charlesmn 0:3ac96e360672 416 &sumb,
charlesmn 0:3ac96e360672 417 &sumc);
charlesmn 0:3ac96e360672 418
charlesmn 0:3ac96e360672 419
charlesmn 0:3ac96e360672 420 pfiltered->VL53L1_p_003[i] = suma;
charlesmn 0:3ac96e360672 421 pfiltered->VL53L1_p_018[i] = sumb;
charlesmn 0:3ac96e360672 422 pfiltered->VL53L1_p_001[i] = sumc;
charlesmn 0:3ac96e360672 423
charlesmn 0:3ac96e360672 424
charlesmn 0:3ac96e360672 425
charlesmn 0:3ac96e360672 426 pfiltered->VL53L1_p_039[i] =
charlesmn 0:3ac96e360672 427 (suma + sumb) -
charlesmn 0:3ac96e360672 428 (sumc + palgo3->VL53L1_p_004);
charlesmn 0:3ac96e360672 429
charlesmn 0:3ac96e360672 430
charlesmn 0:3ac96e360672 431
charlesmn 0:3ac96e360672 432 pfiltered->VL53L1_p_040[i] =
charlesmn 0:3ac96e360672 433 (sumb + sumc) -
charlesmn 0:3ac96e360672 434 (suma + palgo3->VL53L1_p_004);
charlesmn 0:3ac96e360672 435 }
charlesmn 0:3ac96e360672 436
charlesmn 0:3ac96e360672 437 return status;
charlesmn 0:3ac96e360672 438 }
charlesmn 0:3ac96e360672 439
charlesmn 0:3ac96e360672 440
charlesmn 0:3ac96e360672 441 VL53L1_Error VL53L1_f_035(
charlesmn 0:3ac96e360672 442 uint8_t pulse_no,
charlesmn 0:3ac96e360672 443 uint16_t noise_threshold,
charlesmn 0:3ac96e360672 444 VL53L1_hist_gen4_algo_filtered_data_t *pfiltered,
charlesmn 0:3ac96e360672 445 VL53L1_hist_gen3_algo_private_data_t *palgo3)
charlesmn 0:3ac96e360672 446 {
charlesmn 0:3ac96e360672 447
charlesmn 0:3ac96e360672 448
charlesmn 0:3ac96e360672 449
charlesmn 0:3ac96e360672 450 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 451 VL53L1_Error func_status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 452
charlesmn 0:3ac96e360672 453 VL53L1_hist_pulse_data_t *pdata = &(palgo3->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 454
charlesmn 0:3ac96e360672 455 uint8_t lb = 0;
charlesmn 0:3ac96e360672 456 uint8_t i = 0;
charlesmn 0:3ac96e360672 457 uint8_t j = 0;
charlesmn 0:3ac96e360672 458
charlesmn 0:3ac96e360672 459 SUPPRESS_UNUSED_WARNING(noise_threshold);
charlesmn 0:3ac96e360672 460
charlesmn 0:3ac96e360672 461 for (lb = pdata->VL53L1_p_015; lb < pdata->VL53L1_p_016; lb++) {
charlesmn 0:3ac96e360672 462
charlesmn 0:3ac96e360672 463 i = lb % palgo3->VL53L1_p_031;
charlesmn 0:3ac96e360672 464 j = (lb+1) % palgo3->VL53L1_p_031;
charlesmn 0:3ac96e360672 465
charlesmn 0:3ac96e360672 466 if (i < palgo3->VL53L1_p_024 &&
charlesmn 0:3ac96e360672 467 j < palgo3->VL53L1_p_024) {
charlesmn 0:3ac96e360672 468
charlesmn 0:3ac96e360672 469 if (pfiltered->VL53L1_p_039[i] == 0 &&
charlesmn 0:3ac96e360672 470 pfiltered->VL53L1_p_040[i] == 0)
charlesmn 0:3ac96e360672 471
charlesmn 0:3ac96e360672 472 pfiltered->VL53L1_p_043[i] = 0;
charlesmn 0:3ac96e360672 473
charlesmn 0:3ac96e360672 474 else if (pfiltered->VL53L1_p_039[i] >= 0 &&
charlesmn 0:3ac96e360672 475 pfiltered->VL53L1_p_040[i] >= 0)
charlesmn 0:3ac96e360672 476 pfiltered->VL53L1_p_043[i] = 1;
charlesmn 0:3ac96e360672 477
charlesmn 0:3ac96e360672 478 else if (pfiltered->VL53L1_p_039[i] < 0 &&
charlesmn 0:3ac96e360672 479 pfiltered->VL53L1_p_040[i] >= 0 &&
charlesmn 0:3ac96e360672 480 pfiltered->VL53L1_p_039[j] >= 0 &&
charlesmn 0:3ac96e360672 481 pfiltered->VL53L1_p_040[j] < 0)
charlesmn 0:3ac96e360672 482 pfiltered->VL53L1_p_043[i] = 1;
charlesmn 0:3ac96e360672 483
charlesmn 0:3ac96e360672 484 else
charlesmn 0:3ac96e360672 485 pfiltered->VL53L1_p_043[i] = 0;
charlesmn 0:3ac96e360672 486
charlesmn 0:3ac96e360672 487
charlesmn 0:3ac96e360672 488 if (pfiltered->VL53L1_p_043[i] > 0) {
charlesmn 0:3ac96e360672 489
charlesmn 0:3ac96e360672 490 pdata->VL53L1_p_025 = lb;
charlesmn 0:3ac96e360672 491
charlesmn 0:3ac96e360672 492 func_status =
charlesmn 0:3ac96e360672 493 VL53L1_f_036(
charlesmn 0:3ac96e360672 494 lb,
charlesmn 0:3ac96e360672 495 pfiltered->VL53L1_p_003[i],
charlesmn 0:3ac96e360672 496 pfiltered->VL53L1_p_018[i],
charlesmn 0:3ac96e360672 497 pfiltered->VL53L1_p_001[i],
charlesmn 0:3ac96e360672 498 0,
charlesmn 0:3ac96e360672 499 0,
charlesmn 0:3ac96e360672 500 0,
charlesmn 0:3ac96e360672 501 palgo3->VL53L1_p_004,
charlesmn 0:3ac96e360672 502 palgo3->VL53L1_p_031,
charlesmn 0:3ac96e360672 503 &(pdata->VL53L1_p_014));
charlesmn 0:3ac96e360672 504
charlesmn 0:3ac96e360672 505 if (func_status ==
charlesmn 0:3ac96e360672 506 VL53L1_ERROR_DIVISION_BY_ZERO)
charlesmn 0:3ac96e360672 507 pfiltered->VL53L1_p_043[i] = 0;
charlesmn 0:3ac96e360672 508
charlesmn 0:3ac96e360672 509 }
charlesmn 0:3ac96e360672 510 }
charlesmn 0:3ac96e360672 511 }
charlesmn 0:3ac96e360672 512
charlesmn 0:3ac96e360672 513 return status;
charlesmn 0:3ac96e360672 514 }
charlesmn 0:3ac96e360672 515
charlesmn 0:3ac96e360672 516
charlesmn 0:3ac96e360672 517 VL53L1_Error VL53L1_f_036(
charlesmn 0:3ac96e360672 518 uint8_t bin,
charlesmn 0:3ac96e360672 519 int32_t VL53L1_p_003,
charlesmn 0:3ac96e360672 520 int32_t VL53L1_p_018,
charlesmn 0:3ac96e360672 521 int32_t VL53L1_p_001,
charlesmn 0:3ac96e360672 522 int32_t ax,
charlesmn 0:3ac96e360672 523 int32_t bx,
charlesmn 0:3ac96e360672 524 int32_t cx,
charlesmn 0:3ac96e360672 525 int32_t VL53L1_p_004,
charlesmn 0:3ac96e360672 526 uint8_t VL53L1_p_031,
charlesmn 0:3ac96e360672 527 uint32_t *pmean_phase)
charlesmn 0:3ac96e360672 528 {
charlesmn 0:3ac96e360672 529
charlesmn 0:3ac96e360672 530
charlesmn 0:3ac96e360672 531 VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
charlesmn 0:3ac96e360672 532
charlesmn 0:3ac96e360672 533 int64_t mean_phase = VL53L1_MAX_ALLOWED_PHASE;
charlesmn 0:3ac96e360672 534 int32_t mean_phase32;
charlesmn 0:3ac96e360672 535 int64_t VL53L1_p_041 = 0;
charlesmn 0:3ac96e360672 536 int64_t half_b_minus_amb = 0;
charlesmn 0:3ac96e360672 537
charlesmn 0:3ac96e360672 538
charlesmn 0:3ac96e360672 539 VL53L1_p_041 = 4096 * ((int64_t)VL53L1_p_001 -
charlesmn 0:3ac96e360672 540 (int64_t)cx - (int64_t)VL53L1_p_003 - (int64_t)ax);
charlesmn 0:3ac96e360672 541 half_b_minus_amb = 4096 * ((int64_t)VL53L1_p_018 -
charlesmn 0:3ac96e360672 542 (int64_t)bx - (int64_t)VL53L1_p_004);
charlesmn 0:3ac96e360672 543
charlesmn 0:3ac96e360672 544 if (half_b_minus_amb != 0) {
charlesmn 0:3ac96e360672 545 mean_phase = (4096 * VL53L1_p_041) + half_b_minus_amb;
charlesmn 0:3ac96e360672 546 mean_phase = do_division_s(mean_phase, (half_b_minus_amb * 2));
charlesmn 0:3ac96e360672 547 mean_phase += 2048;
charlesmn 0:3ac96e360672 548 mean_phase += (4096 * (int64_t)bin);
charlesmn 0:3ac96e360672 549
charlesmn 0:3ac96e360672 550 mean_phase = do_division_s((mean_phase + 1), 2);
charlesmn 0:3ac96e360672 551
charlesmn 0:3ac96e360672 552 if (mean_phase < 0)
charlesmn 0:3ac96e360672 553 mean_phase = 0;
charlesmn 0:3ac96e360672 554 if (mean_phase > VL53L1_MAX_ALLOWED_PHASE)
charlesmn 0:3ac96e360672 555 mean_phase = VL53L1_MAX_ALLOWED_PHASE;
charlesmn 0:3ac96e360672 556
charlesmn 0:3ac96e360672 557 mean_phase32 = (int32_t)mean_phase;
charlesmn 0:3ac96e360672 558 mean_phase32 = mean_phase32 %
charlesmn 0:3ac96e360672 559 ((int32_t)VL53L1_p_031 * 2048);
charlesmn 0:3ac96e360672 560 mean_phase = mean_phase32;
charlesmn 0:3ac96e360672 561
charlesmn 0:3ac96e360672 562 status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 563 }
charlesmn 0:3ac96e360672 564
charlesmn 0:3ac96e360672 565 *pmean_phase = (uint32_t)mean_phase;
charlesmn 0:3ac96e360672 566
charlesmn 0:3ac96e360672 567 return status;
charlesmn 0:3ac96e360672 568 }
charlesmn 0:3ac96e360672 569
charlesmn 0:3ac96e360672 570