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@5:89031b2f5316, 2021-07-14 (annotated)

Committer:
Charles MacNeill
Date:
Wed Jul 14 12:45:49 2021 +0100
Revision:
5:89031b2f5316
The class files now just are wrappers for the files in the "MODULES" directory.The porting directory includes the mbed interface

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