ST Expansion SW Team / VL53L3CX_mbed

Rename library

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging

modules/vl53lx_hist_algos_gen4.c@7:7f1bbf370283, 2021-07-21 (annotated)

Committer:
charlesmn
Date:
Wed Jul 21 14:07:59 2021 +0000
Revision:
7:7f1bbf370283
Parent:
5:89031b2f5316 
Moved vl53l3cx_class.cpp and .h to 53l3a2_RangingClass

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 5:89031b2f5316 1
Charles MacNeill 5:89031b2f5316 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 5:89031b2f5316 3 /******************************************************************************
Charles MacNeill 5:89031b2f5316 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 5:89031b2f5316 5
Charles MacNeill 5:89031b2f5316 6 This file is part of VL53LX Protected and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 5:89031b2f5316 9
Charles MacNeill 5:89031b2f5316 10 ******************************************************************************
Charles MacNeill 5:89031b2f5316 11
Charles MacNeill 5:89031b2f5316 12 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14 ******************************************************************************
Charles MacNeill 5:89031b2f5316 15
Charles MacNeill 5:89031b2f5316 16 License terms: STMicroelectronics Proprietary in accordance with licensing
Charles MacNeill 5:89031b2f5316 17 terms at www.st.com/sla0081
Charles MacNeill 5:89031b2f5316 18
Charles MacNeill 5:89031b2f5316 19 ******************************************************************************
Charles MacNeill 5:89031b2f5316 20 */
Charles MacNeill 5:89031b2f5316 21
Charles MacNeill 5:89031b2f5316 22
Charles MacNeill 5:89031b2f5316 23
Charles MacNeill 5:89031b2f5316 24
Charles MacNeill 5:89031b2f5316 25 #include <vl53lx_platform_log.h>
Charles MacNeill 5:89031b2f5316 26 #include <vl53lx_types.h>
Charles MacNeill 5:89031b2f5316 27 #include "vl53lx_core_support.h"
Charles MacNeill 5:89031b2f5316 28 #include "vl53lx_error_codes.h"
Charles MacNeill 5:89031b2f5316 29
Charles MacNeill 5:89031b2f5316 30 #include "vl53lx_hist_core.h"
Charles MacNeill 5:89031b2f5316 31 #include "vl53lx_hist_algos_gen3.h"
Charles MacNeill 5:89031b2f5316 32 #include "vl53lx_hist_algos_gen4.h"
Charles MacNeill 5:89031b2f5316 33 #include "vl53lx_sigma_estimate.h"
Charles MacNeill 5:89031b2f5316 34 #include "vl53lx_dmax.h"
Charles MacNeill 5:89031b2f5316 35
Charles MacNeill 5:89031b2f5316 36
Charles MacNeill 5:89031b2f5316 37
Charles MacNeill 5:89031b2f5316 38 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 39 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 40 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 41 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 42 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 43 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 44 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 45
Charles MacNeill 5:89031b2f5316 46 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 47 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 48 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 49
Charles MacNeill 5:89031b2f5316 50
Charles MacNeill 5:89031b2f5316 51 VL53LX_Error VL53LX_f_025(
Charles MacNeill 5:89031b2f5316 52 VL53LX_dmax_calibration_data_t *pdmax_cal,
Charles MacNeill 5:89031b2f5316 53 VL53LX_hist_gen3_dmax_config_t *pdmax_cfg,
Charles MacNeill 5:89031b2f5316 54 VL53LX_hist_post_process_config_t *ppost_cfg,
Charles MacNeill 5:89031b2f5316 55 VL53LX_histogram_bin_data_t *pbins_input,
Charles MacNeill 5:89031b2f5316 56 VL53LX_histogram_bin_data_t *pxtalk,
Charles MacNeill 5:89031b2f5316 57 VL53LX_hist_gen3_algo_private_data_t *palgo3,
Charles MacNeill 5:89031b2f5316 58 VL53LX_hist_gen4_algo_filtered_data_t *pfiltered,
Charles MacNeill 5:89031b2f5316 59 VL53LX_hist_gen3_dmax_private_data_t *pdmax_algo,
Charles MacNeill 5:89031b2f5316 60 VL53LX_range_results_t *presults,
Charles MacNeill 5:89031b2f5316 61 uint8_t histo_merge_nb)
Charles MacNeill 5:89031b2f5316 62 {
Charles MacNeill 5:89031b2f5316 63
Charles MacNeill 5:89031b2f5316 64
Charles MacNeill 5:89031b2f5316 65 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 66
Charles MacNeill 5:89031b2f5316 67 VL53LX_hist_pulse_data_t *ppulse_data;
Charles MacNeill 5:89031b2f5316 68 VL53LX_range_data_t *prange_data;
Charles MacNeill 5:89031b2f5316 69
Charles MacNeill 5:89031b2f5316 70 uint8_t p = 0;
Charles MacNeill 5:89031b2f5316 71 VL53LX_histogram_bin_data_t *pB = &(palgo3->VL53LX_p_006);
Charles MacNeill 5:89031b2f5316 72
Charles MacNeill 5:89031b2f5316 73 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 74
Charles MacNeill 5:89031b2f5316 75
Charles MacNeill 5:89031b2f5316 76
Charles MacNeill 5:89031b2f5316 77
Charles MacNeill 5:89031b2f5316 78
Charles MacNeill 5:89031b2f5316 79 VL53LX_f_003(palgo3);
Charles MacNeill 5:89031b2f5316 80
Charles MacNeill 5:89031b2f5316 81
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83 memcpy(
Charles MacNeill 5:89031b2f5316 84 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 85 pbins_input,
Charles MacNeill 5:89031b2f5316 86 sizeof(VL53LX_histogram_bin_data_t));
Charles MacNeill 5:89031b2f5316 87
Charles MacNeill 5:89031b2f5316 88
Charles MacNeill 5:89031b2f5316 89
Charles MacNeill 5:89031b2f5316 90 presults->cfg_device_state = pbins_input->cfg_device_state;
Charles MacNeill 5:89031b2f5316 91 presults->rd_device_state = pbins_input->rd_device_state;
Charles MacNeill 5:89031b2f5316 92 presults->zone_id = pbins_input->zone_id;
Charles MacNeill 5:89031b2f5316 93 presults->stream_count = pbins_input->result__stream_count;
Charles MacNeill 5:89031b2f5316 94 presults->wrap_dmax_mm = 0;
Charles MacNeill 5:89031b2f5316 95 presults->max_results = VL53LX_MAX_RANGE_RESULTS;
Charles MacNeill 5:89031b2f5316 96 presults->active_results = 0;
Charles MacNeill 5:89031b2f5316 97
Charles MacNeill 5:89031b2f5316 98 for (p = 0; p < VL53LX_MAX_AMBIENT_DMAX_VALUES; p++)
Charles MacNeill 5:89031b2f5316 99 presults->VL53LX_p_022[p] = 0;
Charles MacNeill 5:89031b2f5316 100
Charles MacNeill 5:89031b2f5316 101
Charles MacNeill 5:89031b2f5316 102
Charles MacNeill 5:89031b2f5316 103 VL53LX_hist_calc_zero_distance_phase(&(palgo3->VL53LX_p_006));
Charles MacNeill 5:89031b2f5316 104
Charles MacNeill 5:89031b2f5316 105
Charles MacNeill 5:89031b2f5316 106
Charles MacNeill 5:89031b2f5316 107 if (ppost_cfg->hist_amb_est_method ==
Charles MacNeill 5:89031b2f5316 108 VL53LX_HIST_AMB_EST_METHOD__THRESHOLDED_BINS)
Charles MacNeill 5:89031b2f5316 109 VL53LX_hist_estimate_ambient_from_thresholded_bins(
Charles MacNeill 5:89031b2f5316 110 (int32_t)ppost_cfg->ambient_thresh_sigma0,
Charles MacNeill 5:89031b2f5316 111 &(palgo3->VL53LX_p_006));
Charles MacNeill 5:89031b2f5316 112 else
Charles MacNeill 5:89031b2f5316 113 VL53LX_hist_estimate_ambient_from_ambient_bins(
Charles MacNeill 5:89031b2f5316 114 &(palgo3->VL53LX_p_006));
Charles MacNeill 5:89031b2f5316 115
Charles MacNeill 5:89031b2f5316 116
Charles MacNeill 5:89031b2f5316 117 VL53LX_hist_remove_ambient_bins(&(palgo3->VL53LX_p_006));
Charles MacNeill 5:89031b2f5316 118
Charles MacNeill 5:89031b2f5316 119
Charles MacNeill 5:89031b2f5316 120 if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
Charles MacNeill 5:89031b2f5316 121 VL53LX_f_005(
Charles MacNeill 5:89031b2f5316 122 pxtalk,
Charles MacNeill 5:89031b2f5316 123 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 124 &(palgo3->VL53LX_p_047));
Charles MacNeill 5:89031b2f5316 125
Charles MacNeill 5:89031b2f5316 126
Charles MacNeill 5:89031b2f5316 127
Charles MacNeill 5:89031b2f5316 128 pdmax_cfg->ambient_thresh_sigma =
Charles MacNeill 5:89031b2f5316 129 ppost_cfg->ambient_thresh_sigma1;
Charles MacNeill 5:89031b2f5316 130
Charles MacNeill 5:89031b2f5316 131 for (p = 0; p < VL53LX_MAX_AMBIENT_DMAX_VALUES; p++) {
Charles MacNeill 5:89031b2f5316 132 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 133 status =
Charles MacNeill 5:89031b2f5316 134 VL53LX_f_001(
Charles MacNeill 5:89031b2f5316 135 pdmax_cfg->target_reflectance_for_dmax_calc[p],
Charles MacNeill 5:89031b2f5316 136 pdmax_cal,
Charles MacNeill 5:89031b2f5316 137 pdmax_cfg,
Charles MacNeill 5:89031b2f5316 138 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 139 pdmax_algo,
Charles MacNeill 5:89031b2f5316 140 &(presults->VL53LX_p_022[p]));
Charles MacNeill 5:89031b2f5316 141 }
Charles MacNeill 5:89031b2f5316 142 }
Charles MacNeill 5:89031b2f5316 143
Charles MacNeill 5:89031b2f5316 144
Charles MacNeill 5:89031b2f5316 145
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147
Charles MacNeill 5:89031b2f5316 148 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 149 status =
Charles MacNeill 5:89031b2f5316 150 VL53LX_f_006(
Charles MacNeill 5:89031b2f5316 151 ppost_cfg->ambient_thresh_events_scaler,
Charles MacNeill 5:89031b2f5316 152 (int32_t)ppost_cfg->ambient_thresh_sigma1,
Charles MacNeill 5:89031b2f5316 153 (int32_t)ppost_cfg->min_ambient_thresh_events,
Charles MacNeill 5:89031b2f5316 154 ppost_cfg->algo__crosstalk_compensation_enable,
Charles MacNeill 5:89031b2f5316 155 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 156 &(palgo3->VL53LX_p_047),
Charles MacNeill 5:89031b2f5316 157 palgo3);
Charles MacNeill 5:89031b2f5316 158
Charles MacNeill 5:89031b2f5316 159
Charles MacNeill 5:89031b2f5316 160
Charles MacNeill 5:89031b2f5316 161
Charles MacNeill 5:89031b2f5316 162
Charles MacNeill 5:89031b2f5316 163 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 164 status =
Charles MacNeill 5:89031b2f5316 165 VL53LX_f_007(palgo3);
Charles MacNeill 5:89031b2f5316 166
Charles MacNeill 5:89031b2f5316 167
Charles MacNeill 5:89031b2f5316 168
Charles MacNeill 5:89031b2f5316 169 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 170 status =
Charles MacNeill 5:89031b2f5316 171 VL53LX_f_008(palgo3);
Charles MacNeill 5:89031b2f5316 172
Charles MacNeill 5:89031b2f5316 173
Charles MacNeill 5:89031b2f5316 174
Charles MacNeill 5:89031b2f5316 175 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 176 status =
Charles MacNeill 5:89031b2f5316 177 VL53LX_f_009(palgo3);
Charles MacNeill 5:89031b2f5316 178
Charles MacNeill 5:89031b2f5316 179
Charles MacNeill 5:89031b2f5316 180
Charles MacNeill 5:89031b2f5316 181 for (p = 0; p < palgo3->VL53LX_p_046; p++) {
Charles MacNeill 5:89031b2f5316 182
Charles MacNeill 5:89031b2f5316 183 ppulse_data = &(palgo3->VL53LX_p_003[p]);
Charles MacNeill 5:89031b2f5316 184
Charles MacNeill 5:89031b2f5316 185
Charles MacNeill 5:89031b2f5316 186
Charles MacNeill 5:89031b2f5316 187 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 188 status =
Charles MacNeill 5:89031b2f5316 189 VL53LX_f_010(
Charles MacNeill 5:89031b2f5316 190 p,
Charles MacNeill 5:89031b2f5316 191 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 192 palgo3);
Charles MacNeill 5:89031b2f5316 193
Charles MacNeill 5:89031b2f5316 194
Charles MacNeill 5:89031b2f5316 195
Charles MacNeill 5:89031b2f5316 196 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 197 status =
Charles MacNeill 5:89031b2f5316 198 VL53LX_f_011(
Charles MacNeill 5:89031b2f5316 199 p,
Charles MacNeill 5:89031b2f5316 200 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 201 palgo3,
Charles MacNeill 5:89031b2f5316 202 pB->VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 203 &(palgo3->VL53LX_p_048));
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 208 status =
Charles MacNeill 5:89031b2f5316 209 VL53LX_f_011(
Charles MacNeill 5:89031b2f5316 210 p,
Charles MacNeill 5:89031b2f5316 211 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 212 palgo3,
Charles MacNeill 5:89031b2f5316 213 0,
Charles MacNeill 5:89031b2f5316 214 &(palgo3->VL53LX_p_049));
Charles MacNeill 5:89031b2f5316 215 }
Charles MacNeill 5:89031b2f5316 216
Charles MacNeill 5:89031b2f5316 217
Charles MacNeill 5:89031b2f5316 218
Charles MacNeill 5:89031b2f5316 219 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 220 status =
Charles MacNeill 5:89031b2f5316 221 VL53LX_f_011(
Charles MacNeill 5:89031b2f5316 222 p,
Charles MacNeill 5:89031b2f5316 223 &(palgo3->VL53LX_p_047),
Charles MacNeill 5:89031b2f5316 224 palgo3,
Charles MacNeill 5:89031b2f5316 225 0,
Charles MacNeill 5:89031b2f5316 226 &(palgo3->VL53LX_p_050));
Charles MacNeill 5:89031b2f5316 227 }
Charles MacNeill 5:89031b2f5316 228
Charles MacNeill 5:89031b2f5316 229
Charles MacNeill 5:89031b2f5316 230
Charles MacNeill 5:89031b2f5316 231 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 232 status =
Charles MacNeill 5:89031b2f5316 233 VL53LX_f_026(
Charles MacNeill 5:89031b2f5316 234 p,
Charles MacNeill 5:89031b2f5316 235 &(palgo3->VL53LX_p_048),
Charles MacNeill 5:89031b2f5316 236 palgo3,
Charles MacNeill 5:89031b2f5316 237 pfiltered);
Charles MacNeill 5:89031b2f5316 238
Charles MacNeill 5:89031b2f5316 239
Charles MacNeill 5:89031b2f5316 240
Charles MacNeill 5:89031b2f5316 241 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 242 status =
Charles MacNeill 5:89031b2f5316 243 VL53LX_f_027(
Charles MacNeill 5:89031b2f5316 244 p,
Charles MacNeill 5:89031b2f5316 245 ppost_cfg->noise_threshold,
Charles MacNeill 5:89031b2f5316 246 pfiltered,
Charles MacNeill 5:89031b2f5316 247 palgo3);
Charles MacNeill 5:89031b2f5316 248
Charles MacNeill 5:89031b2f5316 249 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 250 status =
Charles MacNeill 5:89031b2f5316 251 VL53LX_f_014(
Charles MacNeill 5:89031b2f5316 252 ppulse_data->VL53LX_p_023,
Charles MacNeill 5:89031b2f5316 253 ppost_cfg->sigma_estimator__sigma_ref_mm,
Charles MacNeill 5:89031b2f5316 254 palgo3->VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 255 ppulse_data->VL53LX_p_051,
Charles MacNeill 5:89031b2f5316 256 ppost_cfg->algo__crosstalk_compensation_enable,
Charles MacNeill 5:89031b2f5316 257 &(palgo3->VL53LX_p_048),
Charles MacNeill 5:89031b2f5316 258 &(palgo3->VL53LX_p_049),
Charles MacNeill 5:89031b2f5316 259 &(palgo3->VL53LX_p_050),
Charles MacNeill 5:89031b2f5316 260 &(ppulse_data->VL53LX_p_002));
Charles MacNeill 5:89031b2f5316 261
Charles MacNeill 5:89031b2f5316 262
Charles MacNeill 5:89031b2f5316 263
Charles MacNeill 5:89031b2f5316 264 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 265 status =
Charles MacNeill 5:89031b2f5316 266 VL53LX_f_015(
Charles MacNeill 5:89031b2f5316 267 p,
Charles MacNeill 5:89031b2f5316 268 1,
Charles MacNeill 5:89031b2f5316 269 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 270 palgo3);
Charles MacNeill 5:89031b2f5316 271
Charles MacNeill 5:89031b2f5316 272 }
Charles MacNeill 5:89031b2f5316 273
Charles MacNeill 5:89031b2f5316 274
Charles MacNeill 5:89031b2f5316 275
Charles MacNeill 5:89031b2f5316 276 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 277 status =
Charles MacNeill 5:89031b2f5316 278 VL53LX_f_016(
Charles MacNeill 5:89031b2f5316 279 ppost_cfg->hist_target_order,
Charles MacNeill 5:89031b2f5316 280 palgo3);
Charles MacNeill 5:89031b2f5316 281
Charles MacNeill 5:89031b2f5316 282
Charles MacNeill 5:89031b2f5316 283
Charles MacNeill 5:89031b2f5316 284 for (p = 0; p < palgo3->VL53LX_p_046; p++) {
Charles MacNeill 5:89031b2f5316 285
Charles MacNeill 5:89031b2f5316 286 ppulse_data = &(palgo3->VL53LX_p_003[p]);
Charles MacNeill 5:89031b2f5316 287
Charles MacNeill 5:89031b2f5316 288
Charles MacNeill 5:89031b2f5316 289 if (!(presults->active_results < presults->max_results))
Charles MacNeill 5:89031b2f5316 290 continue;
Charles MacNeill 5:89031b2f5316 291
Charles MacNeill 5:89031b2f5316 292
Charles MacNeill 5:89031b2f5316 293
Charles MacNeill 5:89031b2f5316 294
Charles MacNeill 5:89031b2f5316 295 if (ppulse_data->VL53LX_p_010 >
Charles MacNeill 5:89031b2f5316 296 ppost_cfg->signal_total_events_limit &&
Charles MacNeill 5:89031b2f5316 297 ppulse_data->VL53LX_p_023 < 0xFF) {
Charles MacNeill 5:89031b2f5316 298
Charles MacNeill 5:89031b2f5316 299 prange_data =
Charles MacNeill 5:89031b2f5316 300 &(presults->VL53LX_p_003[presults->active_results]);
Charles MacNeill 5:89031b2f5316 301
Charles MacNeill 5:89031b2f5316 302 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 303 VL53LX_f_017(
Charles MacNeill 5:89031b2f5316 304 presults->active_results,
Charles MacNeill 5:89031b2f5316 305 ppost_cfg->valid_phase_low,
Charles MacNeill 5:89031b2f5316 306 ppost_cfg->valid_phase_high,
Charles MacNeill 5:89031b2f5316 307 ppost_cfg->sigma_thresh,
Charles MacNeill 5:89031b2f5316 308 &(palgo3->VL53LX_p_006),
Charles MacNeill 5:89031b2f5316 309 ppulse_data,
Charles MacNeill 5:89031b2f5316 310 prange_data);
Charles MacNeill 5:89031b2f5316 311
Charles MacNeill 5:89031b2f5316 312 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 313 status =
Charles MacNeill 5:89031b2f5316 314 VL53LX_f_018(
Charles MacNeill 5:89031b2f5316 315 pB->vcsel_width,
Charles MacNeill 5:89031b2f5316 316 pB->VL53LX_p_015,
Charles MacNeill 5:89031b2f5316 317 pB->total_periods_elapsed,
Charles MacNeill 5:89031b2f5316 318 pB->result__dss_actual_effective_spads,
Charles MacNeill 5:89031b2f5316 319 prange_data,
Charles MacNeill 5:89031b2f5316 320 histo_merge_nb);
Charles MacNeill 5:89031b2f5316 321
Charles MacNeill 5:89031b2f5316 322 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 323 VL53LX_f_019(
Charles MacNeill 5:89031b2f5316 324 ppost_cfg->gain_factor,
Charles MacNeill 5:89031b2f5316 325 ppost_cfg->range_offset_mm,
Charles MacNeill 5:89031b2f5316 326 prange_data);
Charles MacNeill 5:89031b2f5316 327
Charles MacNeill 5:89031b2f5316 328 presults->active_results++;
Charles MacNeill 5:89031b2f5316 329 }
Charles MacNeill 5:89031b2f5316 330
Charles MacNeill 5:89031b2f5316 331 }
Charles MacNeill 5:89031b2f5316 332
Charles MacNeill 5:89031b2f5316 333
Charles MacNeill 5:89031b2f5316 334
Charles MacNeill 5:89031b2f5316 335 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 336
Charles MacNeill 5:89031b2f5316 337 return status;
Charles MacNeill 5:89031b2f5316 338 }
Charles MacNeill 5:89031b2f5316 339
Charles MacNeill 5:89031b2f5316 340
Charles MacNeill 5:89031b2f5316 341
Charles MacNeill 5:89031b2f5316 342 VL53LX_Error VL53LX_f_026(
Charles MacNeill 5:89031b2f5316 343 uint8_t pulse_no,
Charles MacNeill 5:89031b2f5316 344 VL53LX_histogram_bin_data_t *ppulse,
Charles MacNeill 5:89031b2f5316 345 VL53LX_hist_gen3_algo_private_data_t *palgo3,
Charles MacNeill 5:89031b2f5316 346 VL53LX_hist_gen4_algo_filtered_data_t *pfiltered)
Charles MacNeill 5:89031b2f5316 347 {
Charles MacNeill 5:89031b2f5316 348
Charles MacNeill 5:89031b2f5316 349
Charles MacNeill 5:89031b2f5316 350
Charles MacNeill 5:89031b2f5316 351
Charles MacNeill 5:89031b2f5316 352 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 353
Charles MacNeill 5:89031b2f5316 354 VL53LX_hist_pulse_data_t *pdata = &(palgo3->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 355
Charles MacNeill 5:89031b2f5316 356 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 357 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 358 int32_t suma = 0;
Charles MacNeill 5:89031b2f5316 359 int32_t sumb = 0;
Charles MacNeill 5:89031b2f5316 360 int32_t sumc = 0;
Charles MacNeill 5:89031b2f5316 361
Charles MacNeill 5:89031b2f5316 362 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 363
Charles MacNeill 5:89031b2f5316 364 pfiltered->VL53LX_p_020 = palgo3->VL53LX_p_020;
Charles MacNeill 5:89031b2f5316 365 pfiltered->VL53LX_p_019 = palgo3->VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 366 pfiltered->VL53LX_p_021 = palgo3->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 367
Charles MacNeill 5:89031b2f5316 368
Charles MacNeill 5:89031b2f5316 369
Charles MacNeill 5:89031b2f5316 370 for (lb = pdata->VL53LX_p_012; lb <= pdata->VL53LX_p_013; lb++) {
Charles MacNeill 5:89031b2f5316 371
Charles MacNeill 5:89031b2f5316 372 i = lb % palgo3->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 373
Charles MacNeill 5:89031b2f5316 374
Charles MacNeill 5:89031b2f5316 375 VL53LX_f_022(
Charles MacNeill 5:89031b2f5316 376 i,
Charles MacNeill 5:89031b2f5316 377 pdata->VL53LX_p_051,
Charles MacNeill 5:89031b2f5316 378 ppulse,
Charles MacNeill 5:89031b2f5316 379 &suma,
Charles MacNeill 5:89031b2f5316 380 &sumb,
Charles MacNeill 5:89031b2f5316 381 &sumc);
Charles MacNeill 5:89031b2f5316 382
Charles MacNeill 5:89031b2f5316 383
Charles MacNeill 5:89031b2f5316 384 pfiltered->VL53LX_p_007[i] = suma;
Charles MacNeill 5:89031b2f5316 385 pfiltered->VL53LX_p_032[i] = sumb;
Charles MacNeill 5:89031b2f5316 386 pfiltered->VL53LX_p_001[i] = sumc;
Charles MacNeill 5:89031b2f5316 387
Charles MacNeill 5:89031b2f5316 388
Charles MacNeill 5:89031b2f5316 389
Charles MacNeill 5:89031b2f5316 390 pfiltered->VL53LX_p_053[i] =
Charles MacNeill 5:89031b2f5316 391 (suma + sumb) -
Charles MacNeill 5:89031b2f5316 392 (sumc + palgo3->VL53LX_p_028);
Charles MacNeill 5:89031b2f5316 393
Charles MacNeill 5:89031b2f5316 394
Charles MacNeill 5:89031b2f5316 395
Charles MacNeill 5:89031b2f5316 396 pfiltered->VL53LX_p_054[i] =
Charles MacNeill 5:89031b2f5316 397 (sumb + sumc) -
Charles MacNeill 5:89031b2f5316 398 (suma + palgo3->VL53LX_p_028);
Charles MacNeill 5:89031b2f5316 399 }
Charles MacNeill 5:89031b2f5316 400
Charles MacNeill 5:89031b2f5316 401 return status;
Charles MacNeill 5:89031b2f5316 402 }
Charles MacNeill 5:89031b2f5316 403
Charles MacNeill 5:89031b2f5316 404
Charles MacNeill 5:89031b2f5316 405 VL53LX_Error VL53LX_f_027(
Charles MacNeill 5:89031b2f5316 406 uint8_t pulse_no,
Charles MacNeill 5:89031b2f5316 407 uint16_t noise_threshold,
Charles MacNeill 5:89031b2f5316 408 VL53LX_hist_gen4_algo_filtered_data_t *pfiltered,
Charles MacNeill 5:89031b2f5316 409 VL53LX_hist_gen3_algo_private_data_t *palgo3)
Charles MacNeill 5:89031b2f5316 410 {
Charles MacNeill 5:89031b2f5316 411
Charles MacNeill 5:89031b2f5316 412
Charles MacNeill 5:89031b2f5316 413
Charles MacNeill 5:89031b2f5316 414 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 415 VL53LX_Error func_status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 416
Charles MacNeill 5:89031b2f5316 417 VL53LX_hist_pulse_data_t *pdata = &(palgo3->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 418
Charles MacNeill 5:89031b2f5316 419 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 420 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 421 uint8_t j = 0;
Charles MacNeill 5:89031b2f5316 422
Charles MacNeill 5:89031b2f5316 423 SUPPRESS_UNUSED_WARNING(noise_threshold);
Charles MacNeill 5:89031b2f5316 424
Charles MacNeill 5:89031b2f5316 425 for (lb = pdata->VL53LX_p_012; lb < pdata->VL53LX_p_013; lb++) {
Charles MacNeill 5:89031b2f5316 426
Charles MacNeill 5:89031b2f5316 427 i = lb % palgo3->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 428 j = (lb+1) % palgo3->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 429
Charles MacNeill 5:89031b2f5316 430 if (i < palgo3->VL53LX_p_021 &&
Charles MacNeill 5:89031b2f5316 431 j < palgo3->VL53LX_p_021) {
Charles MacNeill 5:89031b2f5316 432
Charles MacNeill 5:89031b2f5316 433 if (pfiltered->VL53LX_p_053[i] == 0 &&
Charles MacNeill 5:89031b2f5316 434 pfiltered->VL53LX_p_054[i] == 0)
Charles MacNeill 5:89031b2f5316 435
Charles MacNeill 5:89031b2f5316 436 pfiltered->VL53LX_p_040[i] = 0;
Charles MacNeill 5:89031b2f5316 437
Charles MacNeill 5:89031b2f5316 438 else if (pfiltered->VL53LX_p_053[i] >= 0 &&
Charles MacNeill 5:89031b2f5316 439 pfiltered->VL53LX_p_054[i] >= 0)
Charles MacNeill 5:89031b2f5316 440 pfiltered->VL53LX_p_040[i] = 1;
Charles MacNeill 5:89031b2f5316 441
Charles MacNeill 5:89031b2f5316 442 else if (pfiltered->VL53LX_p_053[i] < 0 &&
Charles MacNeill 5:89031b2f5316 443 pfiltered->VL53LX_p_054[i] >= 0 &&
Charles MacNeill 5:89031b2f5316 444 pfiltered->VL53LX_p_053[j] >= 0 &&
Charles MacNeill 5:89031b2f5316 445 pfiltered->VL53LX_p_054[j] < 0)
Charles MacNeill 5:89031b2f5316 446 pfiltered->VL53LX_p_040[i] = 1;
Charles MacNeill 5:89031b2f5316 447
Charles MacNeill 5:89031b2f5316 448 else
Charles MacNeill 5:89031b2f5316 449 pfiltered->VL53LX_p_040[i] = 0;
Charles MacNeill 5:89031b2f5316 450
Charles MacNeill 5:89031b2f5316 451
Charles MacNeill 5:89031b2f5316 452 if (pfiltered->VL53LX_p_040[i] > 0) {
Charles MacNeill 5:89031b2f5316 453
Charles MacNeill 5:89031b2f5316 454 pdata->VL53LX_p_023 = lb;
Charles MacNeill 5:89031b2f5316 455
Charles MacNeill 5:89031b2f5316 456 func_status =
Charles MacNeill 5:89031b2f5316 457 VL53LX_f_028(
Charles MacNeill 5:89031b2f5316 458 lb,
Charles MacNeill 5:89031b2f5316 459 pfiltered->VL53LX_p_007[i],
Charles MacNeill 5:89031b2f5316 460 pfiltered->VL53LX_p_032[i],
Charles MacNeill 5:89031b2f5316 461 pfiltered->VL53LX_p_001[i],
Charles MacNeill 5:89031b2f5316 462 0,
Charles MacNeill 5:89031b2f5316 463 0,
Charles MacNeill 5:89031b2f5316 464 0,
Charles MacNeill 5:89031b2f5316 465 palgo3->VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 466 palgo3->VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 467 &(pdata->VL53LX_p_011));
Charles MacNeill 5:89031b2f5316 468
Charles MacNeill 5:89031b2f5316 469 if (func_status ==
Charles MacNeill 5:89031b2f5316 470 VL53LX_ERROR_DIVISION_BY_ZERO)
Charles MacNeill 5:89031b2f5316 471 pfiltered->VL53LX_p_040[i] = 0;
Charles MacNeill 5:89031b2f5316 472
Charles MacNeill 5:89031b2f5316 473 }
Charles MacNeill 5:89031b2f5316 474 }
Charles MacNeill 5:89031b2f5316 475 }
Charles MacNeill 5:89031b2f5316 476
Charles MacNeill 5:89031b2f5316 477 return status;
Charles MacNeill 5:89031b2f5316 478 }
Charles MacNeill 5:89031b2f5316 479
Charles MacNeill 5:89031b2f5316 480
Charles MacNeill 5:89031b2f5316 481 VL53LX_Error VL53LX_f_028(
Charles MacNeill 5:89031b2f5316 482 uint8_t bin,
Charles MacNeill 5:89031b2f5316 483 int32_t VL53LX_p_007,
Charles MacNeill 5:89031b2f5316 484 int32_t VL53LX_p_032,
Charles MacNeill 5:89031b2f5316 485 int32_t VL53LX_p_001,
Charles MacNeill 5:89031b2f5316 486 int32_t ax,
Charles MacNeill 5:89031b2f5316 487 int32_t bx,
Charles MacNeill 5:89031b2f5316 488 int32_t cx,
Charles MacNeill 5:89031b2f5316 489 int32_t VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 490 uint8_t VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 491 uint32_t *pmean_phase)
Charles MacNeill 5:89031b2f5316 492 {
Charles MacNeill 5:89031b2f5316 493
Charles MacNeill 5:89031b2f5316 494
Charles MacNeill 5:89031b2f5316 495 VL53LX_Error status = VL53LX_ERROR_DIVISION_BY_ZERO;
Charles MacNeill 5:89031b2f5316 496
Charles MacNeill 5:89031b2f5316 497 int64_t mean_phase = VL53LX_MAX_ALLOWED_PHASE;
Charles MacNeill 5:89031b2f5316 498 int32_t mean_phase32;
Charles MacNeill 5:89031b2f5316 499 int64_t VL53LX_p_055 = 0;
Charles MacNeill 5:89031b2f5316 500 int64_t half_b_minus_amb = 0;
Charles MacNeill 5:89031b2f5316 501
Charles MacNeill 5:89031b2f5316 502
Charles MacNeill 5:89031b2f5316 503 VL53LX_p_055 = 4096 * ((int64_t)VL53LX_p_001 -
Charles MacNeill 5:89031b2f5316 504 (int64_t)cx - (int64_t)VL53LX_p_007 - (int64_t)ax);
Charles MacNeill 5:89031b2f5316 505 half_b_minus_amb = 4096 * ((int64_t)VL53LX_p_032 -
Charles MacNeill 5:89031b2f5316 506 (int64_t)bx - (int64_t)VL53LX_p_028);
Charles MacNeill 5:89031b2f5316 507
Charles MacNeill 5:89031b2f5316 508 if (half_b_minus_amb != 0) {
Charles MacNeill 5:89031b2f5316 509
Charles MacNeill 5:89031b2f5316 510 mean_phase = (4096 * VL53LX_p_055) + half_b_minus_amb;
Charles MacNeill 5:89031b2f5316 511 mean_phase = do_division_s(mean_phase, (half_b_minus_amb * 2));
Charles MacNeill 5:89031b2f5316 512
Charles MacNeill 5:89031b2f5316 513 mean_phase += 2048;
Charles MacNeill 5:89031b2f5316 514 mean_phase += (4096 * (int64_t)bin);
Charles MacNeill 5:89031b2f5316 515
Charles MacNeill 5:89031b2f5316 516
Charles MacNeill 5:89031b2f5316 517 mean_phase = do_division_s((mean_phase + 1), 2);
Charles MacNeill 5:89031b2f5316 518
Charles MacNeill 5:89031b2f5316 519
Charles MacNeill 5:89031b2f5316 520 if (mean_phase < 0)
Charles MacNeill 5:89031b2f5316 521 mean_phase = 0;
Charles MacNeill 5:89031b2f5316 522 if (mean_phase > VL53LX_MAX_ALLOWED_PHASE)
Charles MacNeill 5:89031b2f5316 523 mean_phase = VL53LX_MAX_ALLOWED_PHASE;
Charles MacNeill 5:89031b2f5316 524
Charles MacNeill 5:89031b2f5316 525
Charles MacNeill 5:89031b2f5316 526 mean_phase32 = (int32_t)mean_phase;
Charles MacNeill 5:89031b2f5316 527 mean_phase32 = mean_phase32 %
Charles MacNeill 5:89031b2f5316 528 ((int32_t)VL53LX_p_030 * 2048);
Charles MacNeill 5:89031b2f5316 529 mean_phase = mean_phase32;
Charles MacNeill 5:89031b2f5316 530
Charles MacNeill 5:89031b2f5316 531 status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 532
Charles MacNeill 5:89031b2f5316 533 }
Charles MacNeill 5:89031b2f5316 534
Charles MacNeill 5:89031b2f5316 535 *pmean_phase = (uint32_t)mean_phase;
Charles MacNeill 5:89031b2f5316 536
Charles MacNeill 5:89031b2f5316 537 return status;
Charles MacNeill 5:89031b2f5316 538 }
Charles MacNeill 5:89031b2f5316 539
Charles MacNeill 5:89031b2f5316 540