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_funcs.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
Charles MacNeill 5:89031b2f5316 26
Charles MacNeill 5:89031b2f5316 27
Charles MacNeill 5:89031b2f5316 28 #include <vl53lx_platform_log.h>
Charles MacNeill 5:89031b2f5316 29 #include <vl53lx_types.h>
Charles MacNeill 5:89031b2f5316 30 #include "vl53lx_core_support.h"
Charles MacNeill 5:89031b2f5316 31 #include "vl53lx_error_codes.h"
Charles MacNeill 5:89031b2f5316 32 #include "vl53lx_ll_def.h"
Charles MacNeill 5:89031b2f5316 33
Charles MacNeill 5:89031b2f5316 34 #include "vl53lx_hist_funcs.h"
Charles MacNeill 5:89031b2f5316 35 #include "vl53lx_hist_core.h"
Charles MacNeill 5:89031b2f5316 36 #include "vl53lx_hist_private_structs.h"
Charles MacNeill 5:89031b2f5316 37 #include "vl53lx_dmax_private_structs.h"
Charles MacNeill 5:89031b2f5316 38 #include "vl53lx_xtalk.h"
Charles MacNeill 5:89031b2f5316 39 #include "vl53lx_hist_algos_gen3.h"
Charles MacNeill 5:89031b2f5316 40 #include "vl53lx_hist_algos_gen4.h"
Charles MacNeill 5:89031b2f5316 41 #include "vl53lx_dmax.h"
Charles MacNeill 5:89031b2f5316 42
Charles MacNeill 5:89031b2f5316 43
Charles MacNeill 5:89031b2f5316 44
Charles MacNeill 5:89031b2f5316 45 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 46 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 47 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 48 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 49 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 50 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 51 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 52
Charles MacNeill 5:89031b2f5316 53 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 54 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 55 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 56
Charles MacNeill 5:89031b2f5316 57
Charles MacNeill 5:89031b2f5316 58 VL53LX_Error VL53LX_hist_process_data(
Charles MacNeill 5:89031b2f5316 59 VL53LX_dmax_calibration_data_t *pdmax_cal,
Charles MacNeill 5:89031b2f5316 60 VL53LX_hist_gen3_dmax_config_t *pdmax_cfg,
Charles MacNeill 5:89031b2f5316 61 VL53LX_hist_post_process_config_t *ppost_cfg,
Charles MacNeill 5:89031b2f5316 62 VL53LX_histogram_bin_data_t *pbins_input,
Charles MacNeill 5:89031b2f5316 63 VL53LX_xtalk_histogram_data_t *pxtalk_shape,
Charles MacNeill 5:89031b2f5316 64 uint8_t *pArea1,
Charles MacNeill 5:89031b2f5316 65 uint8_t *pArea2,
Charles MacNeill 5:89031b2f5316 66 VL53LX_range_results_t *presults,
Charles MacNeill 5:89031b2f5316 67 uint8_t *HistMergeNumber)
Charles MacNeill 5:89031b2f5316 68 {
Charles MacNeill 5:89031b2f5316 69
Charles MacNeill 5:89031b2f5316 70
Charles MacNeill 5:89031b2f5316 71
Charles MacNeill 5:89031b2f5316 72 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 73
Charles MacNeill 5:89031b2f5316 74 VL53LX_hist_gen3_algo_private_data_t *palgo_gen3 =
Charles MacNeill 5:89031b2f5316 75 (VL53LX_hist_gen3_algo_private_data_t *) pArea1;
Charles MacNeill 5:89031b2f5316 76 VL53LX_hist_gen4_algo_filtered_data_t *pfiltered4 =
Charles MacNeill 5:89031b2f5316 77 (VL53LX_hist_gen4_algo_filtered_data_t *) pArea2;
Charles MacNeill 5:89031b2f5316 78
Charles MacNeill 5:89031b2f5316 79 VL53LX_hist_gen3_dmax_private_data_t dmax_algo_gen3;
Charles MacNeill 5:89031b2f5316 80 VL53LX_hist_gen3_dmax_private_data_t *pdmax_algo_gen3 =
Charles MacNeill 5:89031b2f5316 81 &dmax_algo_gen3;
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83 VL53LX_histogram_bin_data_t bins_averaged;
Charles MacNeill 5:89031b2f5316 84 VL53LX_histogram_bin_data_t *pbins_averaged = &bins_averaged;
Charles MacNeill 5:89031b2f5316 85
Charles MacNeill 5:89031b2f5316 86 VL53LX_range_data_t *pdata;
Charles MacNeill 5:89031b2f5316 87
Charles MacNeill 5:89031b2f5316 88 uint32_t xtalk_rate_kcps = 0;
Charles MacNeill 5:89031b2f5316 89 uint32_t max_xtalk_rate_per_spad_kcps = 0;
Charles MacNeill 5:89031b2f5316 90 uint8_t xtalk_enable = 0;
Charles MacNeill 5:89031b2f5316 91 uint8_t r = 0;
Charles MacNeill 5:89031b2f5316 92 uint8_t t = 0;
Charles MacNeill 5:89031b2f5316 93 uint32_t XtalkDetectMaxSigma = 0;
Charles MacNeill 5:89031b2f5316 94
Charles MacNeill 5:89031b2f5316 95
Charles MacNeill 5:89031b2f5316 96 int16_t delta_mm = 0;
Charles MacNeill 5:89031b2f5316 97
Charles MacNeill 5:89031b2f5316 98
Charles MacNeill 5:89031b2f5316 99 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 100
Charles MacNeill 5:89031b2f5316 101
Charles MacNeill 5:89031b2f5316 102
Charles MacNeill 5:89031b2f5316 103 VL53LX_f_031(
Charles MacNeill 5:89031b2f5316 104 pbins_input,
Charles MacNeill 5:89031b2f5316 105 pbins_averaged);
Charles MacNeill 5:89031b2f5316 106
Charles MacNeill 5:89031b2f5316 107
Charles MacNeill 5:89031b2f5316 108
Charles MacNeill 5:89031b2f5316 109 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 110 0,
Charles MacNeill 5:89031b2f5316 111 pxtalk_shape->xtalk_shape.VL53LX_p_021,
Charles MacNeill 5:89031b2f5316 112 &(pxtalk_shape->xtalk_hist_removed));
Charles MacNeill 5:89031b2f5316 113
Charles MacNeill 5:89031b2f5316 114
Charles MacNeill 5:89031b2f5316 115
Charles MacNeill 5:89031b2f5316 116 VL53LX_copy_xtalk_bin_data_to_histogram_data_struct(
Charles MacNeill 5:89031b2f5316 117 &(pxtalk_shape->xtalk_shape),
Charles MacNeill 5:89031b2f5316 118 &(pxtalk_shape->xtalk_hist_removed));
Charles MacNeill 5:89031b2f5316 119
Charles MacNeill 5:89031b2f5316 120
Charles MacNeill 5:89031b2f5316 121
Charles MacNeill 5:89031b2f5316 122 if ((status == VL53LX_ERROR_NONE) &&
Charles MacNeill 5:89031b2f5316 123 (ppost_cfg->algo__crosstalk_compensation_enable > 0))
Charles MacNeill 5:89031b2f5316 124 status =
Charles MacNeill 5:89031b2f5316 125 VL53LX_f_032(
Charles MacNeill 5:89031b2f5316 126 ppost_cfg->algo__crosstalk_compensation_plane_offset_kcps,
Charles MacNeill 5:89031b2f5316 127 ppost_cfg->algo__crosstalk_compensation_x_plane_gradient_kcps,
Charles MacNeill 5:89031b2f5316 128 ppost_cfg->algo__crosstalk_compensation_y_plane_gradient_kcps,
Charles MacNeill 5:89031b2f5316 129 0,
Charles MacNeill 5:89031b2f5316 130 0,
Charles MacNeill 5:89031b2f5316 131 pbins_input->result__dss_actual_effective_spads,
Charles MacNeill 5:89031b2f5316 132 pbins_input->roi_config__user_roi_centre_spad,
Charles MacNeill 5:89031b2f5316 133 pbins_input->roi_config__user_roi_requested_global_xy_size,
Charles MacNeill 5:89031b2f5316 134 &(xtalk_rate_kcps));
Charles MacNeill 5:89031b2f5316 135
Charles MacNeill 5:89031b2f5316 136
Charles MacNeill 5:89031b2f5316 137
Charles MacNeill 5:89031b2f5316 138 if ((status == VL53LX_ERROR_NONE) &&
Charles MacNeill 5:89031b2f5316 139 (ppost_cfg->algo__crosstalk_compensation_enable > 0))
Charles MacNeill 5:89031b2f5316 140 status =
Charles MacNeill 5:89031b2f5316 141 VL53LX_f_033(
Charles MacNeill 5:89031b2f5316 142 pbins_averaged,
Charles MacNeill 5:89031b2f5316 143 &(pxtalk_shape->xtalk_shape),
Charles MacNeill 5:89031b2f5316 144 xtalk_rate_kcps,
Charles MacNeill 5:89031b2f5316 145 &(pxtalk_shape->xtalk_hist_removed));
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147
Charles MacNeill 5:89031b2f5316 148
Charles MacNeill 5:89031b2f5316 149
Charles MacNeill 5:89031b2f5316 150
Charles MacNeill 5:89031b2f5316 151 presults->xmonitor.total_periods_elapsed =
Charles MacNeill 5:89031b2f5316 152 pbins_averaged->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 153 presults->xmonitor.VL53LX_p_004 =
Charles MacNeill 5:89031b2f5316 154 pbins_averaged->result__dss_actual_effective_spads;
Charles MacNeill 5:89031b2f5316 155
Charles MacNeill 5:89031b2f5316 156 presults->xmonitor.peak_signal_count_rate_mcps = 0;
Charles MacNeill 5:89031b2f5316 157 presults->xmonitor.VL53LX_p_009 = 0;
Charles MacNeill 5:89031b2f5316 158
Charles MacNeill 5:89031b2f5316 159 presults->xmonitor.range_id = 0;
Charles MacNeill 5:89031b2f5316 160 presults->xmonitor.range_status = VL53LX_DEVICEERROR_NOUPDATE;
Charles MacNeill 5:89031b2f5316 161
Charles MacNeill 5:89031b2f5316 162
Charles MacNeill 5:89031b2f5316 163
Charles MacNeill 5:89031b2f5316 164 xtalk_enable = 0;
Charles MacNeill 5:89031b2f5316 165 if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
Charles MacNeill 5:89031b2f5316 166 xtalk_enable = 1;
Charles MacNeill 5:89031b2f5316 167
Charles MacNeill 5:89031b2f5316 168
Charles MacNeill 5:89031b2f5316 169
Charles MacNeill 5:89031b2f5316 170 for (r = 0 ; r <= xtalk_enable ; r++) {
Charles MacNeill 5:89031b2f5316 171
Charles MacNeill 5:89031b2f5316 172
Charles MacNeill 5:89031b2f5316 173 ppost_cfg->algo__crosstalk_compensation_enable = r;
Charles MacNeill 5:89031b2f5316 174
Charles MacNeill 5:89031b2f5316 175
Charles MacNeill 5:89031b2f5316 176
Charles MacNeill 5:89031b2f5316 177 status =
Charles MacNeill 5:89031b2f5316 178 VL53LX_f_025(
Charles MacNeill 5:89031b2f5316 179 pdmax_cal,
Charles MacNeill 5:89031b2f5316 180 pdmax_cfg,
Charles MacNeill 5:89031b2f5316 181 ppost_cfg,
Charles MacNeill 5:89031b2f5316 182 pbins_averaged,
Charles MacNeill 5:89031b2f5316 183 &(pxtalk_shape->xtalk_hist_removed),
Charles MacNeill 5:89031b2f5316 184 palgo_gen3,
Charles MacNeill 5:89031b2f5316 185 pfiltered4,
Charles MacNeill 5:89031b2f5316 186 pdmax_algo_gen3,
Charles MacNeill 5:89031b2f5316 187 presults,
Charles MacNeill 5:89031b2f5316 188 *HistMergeNumber);
Charles MacNeill 5:89031b2f5316 189
Charles MacNeill 5:89031b2f5316 190
Charles MacNeill 5:89031b2f5316 191 if (!(status == VL53LX_ERROR_NONE && r == 0))
Charles MacNeill 5:89031b2f5316 192 continue;
Charles MacNeill 5:89031b2f5316 193
Charles MacNeill 5:89031b2f5316 194
Charles MacNeill 5:89031b2f5316 195
Charles MacNeill 5:89031b2f5316 196 if (presults->active_results == 0) {
Charles MacNeill 5:89031b2f5316 197 pdata = &(presults->VL53LX_p_003[0]);
Charles MacNeill 5:89031b2f5316 198 pdata->ambient_count_rate_mcps =
Charles MacNeill 5:89031b2f5316 199 pdmax_algo_gen3->VL53LX_p_034;
Charles MacNeill 5:89031b2f5316 200 pdata->VL53LX_p_004 =
Charles MacNeill 5:89031b2f5316 201 pdmax_algo_gen3->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 202 }
Charles MacNeill 5:89031b2f5316 203
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205
Charles MacNeill 5:89031b2f5316 206 max_xtalk_rate_per_spad_kcps = (uint32_t)(
Charles MacNeill 5:89031b2f5316 207 ppost_cfg->algo__crosstalk_detect_max_valid_rate_kcps);
Charles MacNeill 5:89031b2f5316 208 max_xtalk_rate_per_spad_kcps *= (uint32_t)(*HistMergeNumber);
Charles MacNeill 5:89031b2f5316 209 max_xtalk_rate_per_spad_kcps <<= 4;
Charles MacNeill 5:89031b2f5316 210
Charles MacNeill 5:89031b2f5316 211 for (t = 0 ; t < presults->active_results ; t++) {
Charles MacNeill 5:89031b2f5316 212
Charles MacNeill 5:89031b2f5316 213 pdata = &(presults->VL53LX_p_003[t]);
Charles MacNeill 5:89031b2f5316 214
Charles MacNeill 5:89031b2f5316 215
Charles MacNeill 5:89031b2f5316 216
Charles MacNeill 5:89031b2f5316 217 if (pdata->max_range_mm > pdata->min_range_mm)
Charles MacNeill 5:89031b2f5316 218 delta_mm =
Charles MacNeill 5:89031b2f5316 219 pdata->max_range_mm -
Charles MacNeill 5:89031b2f5316 220 pdata->min_range_mm;
Charles MacNeill 5:89031b2f5316 221 else
Charles MacNeill 5:89031b2f5316 222 delta_mm =
Charles MacNeill 5:89031b2f5316 223 pdata->min_range_mm -
Charles MacNeill 5:89031b2f5316 224 pdata->max_range_mm;
Charles MacNeill 5:89031b2f5316 225
Charles MacNeill 5:89031b2f5316 226 XtalkDetectMaxSigma =
Charles MacNeill 5:89031b2f5316 227 ppost_cfg->algo__crosstalk_detect_max_sigma_mm;
Charles MacNeill 5:89031b2f5316 228 XtalkDetectMaxSigma *= (uint32_t)(*HistMergeNumber);
Charles MacNeill 5:89031b2f5316 229 XtalkDetectMaxSigma <<= 5;
Charles MacNeill 5:89031b2f5316 230 if (pdata->median_range_mm >
Charles MacNeill 5:89031b2f5316 231 ppost_cfg->algo__crosstalk_detect_min_valid_range_mm &&
Charles MacNeill 5:89031b2f5316 232 pdata->median_range_mm <
Charles MacNeill 5:89031b2f5316 233 ppost_cfg->algo__crosstalk_detect_max_valid_range_mm &&
Charles MacNeill 5:89031b2f5316 234 pdata->VL53LX_p_009 <
Charles MacNeill 5:89031b2f5316 235 max_xtalk_rate_per_spad_kcps &&
Charles MacNeill 5:89031b2f5316 236 pdata->VL53LX_p_002 < XtalkDetectMaxSigma &&
Charles MacNeill 5:89031b2f5316 237 delta_mm <
Charles MacNeill 5:89031b2f5316 238 ppost_cfg->algo__crosstalk_detect_min_max_tolerance) {
Charles MacNeill 5:89031b2f5316 239
Charles MacNeill 5:89031b2f5316 240
Charles MacNeill 5:89031b2f5316 241
Charles MacNeill 5:89031b2f5316 242 memcpy(
Charles MacNeill 5:89031b2f5316 243 &(presults->xmonitor),
Charles MacNeill 5:89031b2f5316 244 pdata,
Charles MacNeill 5:89031b2f5316 245 sizeof(VL53LX_range_data_t));
Charles MacNeill 5:89031b2f5316 246
Charles MacNeill 5:89031b2f5316 247 }
Charles MacNeill 5:89031b2f5316 248 }
Charles MacNeill 5:89031b2f5316 249
Charles MacNeill 5:89031b2f5316 250 }
Charles MacNeill 5:89031b2f5316 251
Charles MacNeill 5:89031b2f5316 252
Charles MacNeill 5:89031b2f5316 253
Charles MacNeill 5:89031b2f5316 254 ppost_cfg->algo__crosstalk_compensation_enable = xtalk_enable;
Charles MacNeill 5:89031b2f5316 255
Charles MacNeill 5:89031b2f5316 256 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 257
Charles MacNeill 5:89031b2f5316 258 return status;
Charles MacNeill 5:89031b2f5316 259 }
Charles MacNeill 5:89031b2f5316 260
Charles MacNeill 5:89031b2f5316 261
Charles MacNeill 5:89031b2f5316 262 VL53LX_Error VL53LX_hist_ambient_dmax(
Charles MacNeill 5:89031b2f5316 263 uint16_t target_reflectance,
Charles MacNeill 5:89031b2f5316 264 VL53LX_dmax_calibration_data_t *pdmax_cal,
Charles MacNeill 5:89031b2f5316 265 VL53LX_hist_gen3_dmax_config_t *pdmax_cfg,
Charles MacNeill 5:89031b2f5316 266 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 267 int16_t *pambient_dmax_mm)
Charles MacNeill 5:89031b2f5316 268 {
Charles MacNeill 5:89031b2f5316 269
Charles MacNeill 5:89031b2f5316 270
Charles MacNeill 5:89031b2f5316 271
Charles MacNeill 5:89031b2f5316 272 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 273
Charles MacNeill 5:89031b2f5316 274 VL53LX_hist_gen3_dmax_private_data_t dmax_algo;
Charles MacNeill 5:89031b2f5316 275 VL53LX_hist_gen3_dmax_private_data_t *pdmax_algo = &dmax_algo;
Charles MacNeill 5:89031b2f5316 276
Charles MacNeill 5:89031b2f5316 277 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 278
Charles MacNeill 5:89031b2f5316 279 status =
Charles MacNeill 5:89031b2f5316 280 VL53LX_f_001(
Charles MacNeill 5:89031b2f5316 281 target_reflectance,
Charles MacNeill 5:89031b2f5316 282 pdmax_cal,
Charles MacNeill 5:89031b2f5316 283 pdmax_cfg,
Charles MacNeill 5:89031b2f5316 284 pbins,
Charles MacNeill 5:89031b2f5316 285 pdmax_algo,
Charles MacNeill 5:89031b2f5316 286 pambient_dmax_mm);
Charles MacNeill 5:89031b2f5316 287
Charles MacNeill 5:89031b2f5316 288 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 289
Charles MacNeill 5:89031b2f5316 290 return status;
Charles MacNeill 5:89031b2f5316 291 }
Charles MacNeill 5:89031b2f5316 292