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@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
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