ST Expansion SW Team / VL53L1CB

The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.

src/vl53l1_hist_funcs.c@7:1add29d51e72, 2021-06-08 (annotated)

Committer:
Charles MacNeill
Date:
Tue Jun 08 10:34:47 2021 +0100
Revision:
7:1add29d51e72
Parent:
0:3ac96e360672 
Update to v6.6.5 of bare_driver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
Charles MacNeill 7:1add29d51e72 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 7:1add29d51e72 3 /******************************************************************************
Charles MacNeill 7:1add29d51e72 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 5
Charles MacNeill 7:1add29d51e72 6 This file is part of VL53L1 Protected and is dual licensed,
Charles MacNeill 7:1add29d51e72 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 7:1add29d51e72 9
Charles MacNeill 7:1add29d51e72 10 ******************************************************************************
Charles MacNeill 7:1add29d51e72 11
Charles MacNeill 7:1add29d51e72 12 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 13
Charles MacNeill 7:1add29d51e72 14 ******************************************************************************
charlesmn 0:3ac96e360672 15
charlesmn 0:3ac96e360672 16 License terms: STMicroelectronics Proprietary in accordance with licensing
charlesmn 0:3ac96e360672 17 terms at www.st.com/sla0081
charlesmn 0:3ac96e360672 18
Charles MacNeill 7:1add29d51e72 19 ******************************************************************************
Charles MacNeill 7:1add29d51e72 20 */
charlesmn 0:3ac96e360672 21
charlesmn 0:3ac96e360672 22
charlesmn 0:3ac96e360672 23
charlesmn 0:3ac96e360672 24
charlesmn 0:3ac96e360672 25
charlesmn 0:3ac96e360672 26
charlesmn 0:3ac96e360672 27
charlesmn 0:3ac96e360672 28 #include "vl53l1_types.h"
charlesmn 0:3ac96e360672 29 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 30
charlesmn 0:3ac96e360672 31 #include "vl53l1_core_support.h"
charlesmn 0:3ac96e360672 32 #include "vl53l1_error_codes.h"
charlesmn 0:3ac96e360672 33 #include "vl53l1_ll_def.h"
charlesmn 0:3ac96e360672 34
charlesmn 0:3ac96e360672 35 #include "vl53l1_hist_funcs.h"
charlesmn 0:3ac96e360672 36 #include "vl53l1_hist_core.h"
charlesmn 0:3ac96e360672 37 #include "vl53l1_hist_private_structs.h"
charlesmn 0:3ac96e360672 38 #include "vl53l1_dmax_private_structs.h"
charlesmn 0:3ac96e360672 39 #include "vl53l1_xtalk.h"
charlesmn 0:3ac96e360672 40 #include "vl53l1_hist_algos_gen3.h"
charlesmn 0:3ac96e360672 41 #include "vl53l1_hist_algos_gen4.h"
charlesmn 0:3ac96e360672 42 #include "vl53l1_dmax.h"
charlesmn 0:3ac96e360672 43
charlesmn 0:3ac96e360672 44
charlesmn 0:3ac96e360672 45
charlesmn 0:3ac96e360672 46 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 47 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 48 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 49 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 50 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 51 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 52 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 53
charlesmn 0:3ac96e360672 54 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 55 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 56 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 57
charlesmn 0:3ac96e360672 58
charlesmn 0:3ac96e360672 59 VL53L1_Error VL53L1_hist_process_data(
charlesmn 0:3ac96e360672 60 VL53L1_dmax_calibration_data_t *pdmax_cal,
charlesmn 0:3ac96e360672 61 VL53L1_hist_gen3_dmax_config_t *pdmax_cfg,
charlesmn 0:3ac96e360672 62 VL53L1_hist_post_process_config_t *ppost_cfg,
charlesmn 0:3ac96e360672 63 VL53L1_histogram_bin_data_t *pbins_input,
charlesmn 0:3ac96e360672 64 VL53L1_xtalk_histogram_data_t *pxtalk_shape,
charlesmn 0:3ac96e360672 65 uint8_t *pArea1,
charlesmn 0:3ac96e360672 66 uint8_t *pArea2,
charlesmn 0:3ac96e360672 67 VL53L1_range_results_t *presults,
charlesmn 0:3ac96e360672 68 uint8_t *HistMergeNumber)
charlesmn 0:3ac96e360672 69 {
charlesmn 0:3ac96e360672 70
charlesmn 0:3ac96e360672 71
charlesmn 0:3ac96e360672 72
charlesmn 0:3ac96e360672 73 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 74
charlesmn 0:3ac96e360672 75 VL53L1_hist_gen3_algo_private_data_t *palgo_gen3 =
charlesmn 0:3ac96e360672 76 (VL53L1_hist_gen3_algo_private_data_t *) pArea1;
charlesmn 0:3ac96e360672 77 VL53L1_hist_gen4_algo_filtered_data_t *pfiltered4 =
charlesmn 0:3ac96e360672 78 (VL53L1_hist_gen4_algo_filtered_data_t *) pArea2;
charlesmn 0:3ac96e360672 79
charlesmn 0:3ac96e360672 80 VL53L1_hist_gen3_dmax_private_data_t dmax_algo_gen3;
charlesmn 0:3ac96e360672 81 VL53L1_hist_gen3_dmax_private_data_t *pdmax_algo_gen3 =
charlesmn 0:3ac96e360672 82 &dmax_algo_gen3;
charlesmn 0:3ac96e360672 83
charlesmn 0:3ac96e360672 84 VL53L1_histogram_bin_data_t bins_averaged;
charlesmn 0:3ac96e360672 85 VL53L1_histogram_bin_data_t *pbins_averaged = &bins_averaged;
charlesmn 0:3ac96e360672 86
charlesmn 0:3ac96e360672 87 VL53L1_range_data_t *pdata;
charlesmn 0:3ac96e360672 88
charlesmn 0:3ac96e360672 89 uint32_t xtalk_rate_kcps = 0;
charlesmn 0:3ac96e360672 90 uint32_t max_xtalk_rate_per_spad_kcps = 0;
charlesmn 0:3ac96e360672 91 uint8_t xtalk_enable = 0;
charlesmn 0:3ac96e360672 92 uint8_t r = 0;
charlesmn 0:3ac96e360672 93 uint8_t t = 0;
charlesmn 0:3ac96e360672 94 uint32_t XtalkDetectMaxSigma = 0;
charlesmn 0:3ac96e360672 95
charlesmn 0:3ac96e360672 96
charlesmn 0:3ac96e360672 97 int16_t delta_mm = 0;
charlesmn 0:3ac96e360672 98
charlesmn 0:3ac96e360672 99
charlesmn 0:3ac96e360672 100 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 101
charlesmn 0:3ac96e360672 102
charlesmn 0:3ac96e360672 103
charlesmn 0:3ac96e360672 104 VL53L1_f_039(
charlesmn 0:3ac96e360672 105 pbins_input,
charlesmn 0:3ac96e360672 106 pbins_averaged);
charlesmn 0:3ac96e360672 107
charlesmn 0:3ac96e360672 108
charlesmn 0:3ac96e360672 109
charlesmn 0:3ac96e360672 110 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 111 0,
charlesmn 0:3ac96e360672 112 pxtalk_shape->xtalk_shape.VL53L1_p_024,
charlesmn 0:3ac96e360672 113 &(pxtalk_shape->xtalk_hist_removed));
charlesmn 0:3ac96e360672 114
charlesmn 0:3ac96e360672 115
charlesmn 0:3ac96e360672 116
charlesmn 0:3ac96e360672 117 VL53L1_copy_xtalk_bin_data_to_histogram_data_struct(
charlesmn 0:3ac96e360672 118 &(pxtalk_shape->xtalk_shape),
charlesmn 0:3ac96e360672 119 &(pxtalk_shape->xtalk_hist_removed));
charlesmn 0:3ac96e360672 120
charlesmn 0:3ac96e360672 121
charlesmn 0:3ac96e360672 122
charlesmn 0:3ac96e360672 123 if ((status == VL53L1_ERROR_NONE) &&
charlesmn 0:3ac96e360672 124 (ppost_cfg->algo__crosstalk_compensation_enable > 0))
charlesmn 0:3ac96e360672 125 status =
charlesmn 0:3ac96e360672 126 VL53L1_f_040(
charlesmn 0:3ac96e360672 127 ppost_cfg->algo__crosstalk_compensation_plane_offset_kcps,
charlesmn 0:3ac96e360672 128 ppost_cfg->algo__crosstalk_compensation_x_plane_gradient_kcps,
charlesmn 0:3ac96e360672 129 ppost_cfg->algo__crosstalk_compensation_y_plane_gradient_kcps,
charlesmn 0:3ac96e360672 130 0,
charlesmn 0:3ac96e360672 131 0,
charlesmn 0:3ac96e360672 132 pbins_input->result__dss_actual_effective_spads,
charlesmn 0:3ac96e360672 133 pbins_input->roi_config__user_roi_centre_spad,
charlesmn 0:3ac96e360672 134 pbins_input->roi_config__user_roi_requested_global_xy_size,
charlesmn 0:3ac96e360672 135 &(xtalk_rate_kcps));
charlesmn 0:3ac96e360672 136
charlesmn 0:3ac96e360672 137
charlesmn 0:3ac96e360672 138
charlesmn 0:3ac96e360672 139 if ((status == VL53L1_ERROR_NONE) &&
charlesmn 0:3ac96e360672 140 (ppost_cfg->algo__crosstalk_compensation_enable > 0))
charlesmn 0:3ac96e360672 141 status =
charlesmn 0:3ac96e360672 142 VL53L1_f_041(
charlesmn 0:3ac96e360672 143 pbins_averaged,
charlesmn 0:3ac96e360672 144 &(pxtalk_shape->xtalk_shape),
charlesmn 0:3ac96e360672 145 xtalk_rate_kcps,
charlesmn 0:3ac96e360672 146 &(pxtalk_shape->xtalk_hist_removed));
charlesmn 0:3ac96e360672 147
charlesmn 0:3ac96e360672 148
charlesmn 0:3ac96e360672 149
charlesmn 0:3ac96e360672 150
charlesmn 0:3ac96e360672 151
charlesmn 0:3ac96e360672 152 presults->xmonitor.total_periods_elapsed =
charlesmn 0:3ac96e360672 153 pbins_averaged->total_periods_elapsed;
charlesmn 0:3ac96e360672 154 presults->xmonitor.VL53L1_p_006 =
charlesmn 0:3ac96e360672 155 pbins_averaged->result__dss_actual_effective_spads;
charlesmn 0:3ac96e360672 156
charlesmn 0:3ac96e360672 157 presults->xmonitor.peak_signal_count_rate_mcps = 0;
charlesmn 0:3ac96e360672 158 presults->xmonitor.VL53L1_p_012 = 0;
charlesmn 0:3ac96e360672 159
charlesmn 0:3ac96e360672 160 presults->xmonitor.range_id = 0;
charlesmn 0:3ac96e360672 161 presults->xmonitor.range_status = VL53L1_DEVICEERROR_NOUPDATE;
charlesmn 0:3ac96e360672 162
charlesmn 0:3ac96e360672 163
charlesmn 0:3ac96e360672 164
charlesmn 0:3ac96e360672 165 xtalk_enable = 0;
charlesmn 0:3ac96e360672 166 if (ppost_cfg->algo__crosstalk_compensation_enable > 0)
charlesmn 0:3ac96e360672 167 xtalk_enable = 1;
charlesmn 0:3ac96e360672 168
charlesmn 0:3ac96e360672 169
charlesmn 0:3ac96e360672 170
charlesmn 0:3ac96e360672 171 for (r = 0 ; r <= xtalk_enable ; r++) {
charlesmn 0:3ac96e360672 172
charlesmn 0:3ac96e360672 173
charlesmn 0:3ac96e360672 174 ppost_cfg->algo__crosstalk_compensation_enable = r;
charlesmn 0:3ac96e360672 175
charlesmn 0:3ac96e360672 176
charlesmn 0:3ac96e360672 177
charlesmn 0:3ac96e360672 178 status =
charlesmn 0:3ac96e360672 179 VL53L1_f_033(
charlesmn 0:3ac96e360672 180 pdmax_cal,
charlesmn 0:3ac96e360672 181 pdmax_cfg,
charlesmn 0:3ac96e360672 182 ppost_cfg,
charlesmn 0:3ac96e360672 183 pbins_averaged,
charlesmn 0:3ac96e360672 184 &(pxtalk_shape->xtalk_hist_removed),
charlesmn 0:3ac96e360672 185 palgo_gen3,
charlesmn 0:3ac96e360672 186 pfiltered4,
charlesmn 0:3ac96e360672 187 pdmax_algo_gen3,
charlesmn 0:3ac96e360672 188 presults,
charlesmn 0:3ac96e360672 189 *HistMergeNumber);
charlesmn 0:3ac96e360672 190
charlesmn 0:3ac96e360672 191
charlesmn 0:3ac96e360672 192 if (!(status == VL53L1_ERROR_NONE && r == 0))
charlesmn 0:3ac96e360672 193 continue;
charlesmn 0:3ac96e360672 194
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196
charlesmn 0:3ac96e360672 197 if (presults->active_results == 0) {
charlesmn 0:3ac96e360672 198 pdata = &(presults->VL53L1_p_002[0]);
charlesmn 0:3ac96e360672 199 pdata->ambient_count_rate_mcps =
charlesmn 0:3ac96e360672 200 pdmax_algo_gen3->VL53L1_p_001;
charlesmn 0:3ac96e360672 201 pdata->VL53L1_p_006 =
charlesmn 0:3ac96e360672 202 pdmax_algo_gen3->VL53L1_p_006;
charlesmn 0:3ac96e360672 203 }
charlesmn 0:3ac96e360672 204
charlesmn 0:3ac96e360672 205
charlesmn 0:3ac96e360672 206
charlesmn 0:3ac96e360672 207 max_xtalk_rate_per_spad_kcps = (uint32_t)(
charlesmn 0:3ac96e360672 208 ppost_cfg->algo__crosstalk_detect_max_valid_rate_kcps);
charlesmn 0:3ac96e360672 209 max_xtalk_rate_per_spad_kcps *= (uint32_t)(*HistMergeNumber);
charlesmn 0:3ac96e360672 210 max_xtalk_rate_per_spad_kcps <<= 4;
charlesmn 0:3ac96e360672 211
charlesmn 0:3ac96e360672 212 for (t = 0 ; t < presults->active_results ; t++) {
charlesmn 0:3ac96e360672 213
charlesmn 0:3ac96e360672 214 pdata = &(presults->VL53L1_p_002[t]);
charlesmn 0:3ac96e360672 215
charlesmn 0:3ac96e360672 216
charlesmn 0:3ac96e360672 217
charlesmn 0:3ac96e360672 218 if (pdata->max_range_mm > pdata->min_range_mm)
charlesmn 0:3ac96e360672 219 delta_mm =
charlesmn 0:3ac96e360672 220 pdata->max_range_mm -
charlesmn 0:3ac96e360672 221 pdata->min_range_mm;
charlesmn 0:3ac96e360672 222 else
charlesmn 0:3ac96e360672 223 delta_mm =
charlesmn 0:3ac96e360672 224 pdata->min_range_mm -
charlesmn 0:3ac96e360672 225 pdata->max_range_mm;
charlesmn 0:3ac96e360672 226
charlesmn 0:3ac96e360672 227 XtalkDetectMaxSigma =
charlesmn 0:3ac96e360672 228 ppost_cfg->algo__crosstalk_detect_max_sigma_mm;
charlesmn 0:3ac96e360672 229 XtalkDetectMaxSigma *= (uint32_t)(*HistMergeNumber);
charlesmn 0:3ac96e360672 230 XtalkDetectMaxSigma <<= 5;
charlesmn 0:3ac96e360672 231 if (pdata->median_range_mm >
charlesmn 0:3ac96e360672 232 ppost_cfg->algo__crosstalk_detect_min_valid_range_mm &&
charlesmn 0:3ac96e360672 233 pdata->median_range_mm <
charlesmn 0:3ac96e360672 234 ppost_cfg->algo__crosstalk_detect_max_valid_range_mm &&
charlesmn 0:3ac96e360672 235 pdata->VL53L1_p_012 <
charlesmn 0:3ac96e360672 236 max_xtalk_rate_per_spad_kcps &&
charlesmn 0:3ac96e360672 237 pdata->VL53L1_p_005 < XtalkDetectMaxSigma &&
charlesmn 0:3ac96e360672 238 delta_mm <
charlesmn 0:3ac96e360672 239 ppost_cfg->algo__crosstalk_detect_min_max_tolerance) {
charlesmn 0:3ac96e360672 240
charlesmn 0:3ac96e360672 241
charlesmn 0:3ac96e360672 242
charlesmn 0:3ac96e360672 243 memcpy(
charlesmn 0:3ac96e360672 244 &(presults->xmonitor),
charlesmn 0:3ac96e360672 245 pdata,
charlesmn 0:3ac96e360672 246 sizeof(VL53L1_range_data_t));
charlesmn 0:3ac96e360672 247
charlesmn 0:3ac96e360672 248 }
charlesmn 0:3ac96e360672 249 }
charlesmn 0:3ac96e360672 250
charlesmn 0:3ac96e360672 251 }
charlesmn 0:3ac96e360672 252
charlesmn 0:3ac96e360672 253
charlesmn 0:3ac96e360672 254
charlesmn 0:3ac96e360672 255 ppost_cfg->algo__crosstalk_compensation_enable = xtalk_enable;
charlesmn 0:3ac96e360672 256
charlesmn 0:3ac96e360672 257 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 258
charlesmn 0:3ac96e360672 259 return status;
charlesmn 0:3ac96e360672 260 }
charlesmn 0:3ac96e360672 261
charlesmn 0:3ac96e360672 262
charlesmn 0:3ac96e360672 263 VL53L1_Error VL53L1_hist_ambient_dmax(
charlesmn 0:3ac96e360672 264 uint16_t target_reflectance,
charlesmn 0:3ac96e360672 265 VL53L1_dmax_calibration_data_t *pdmax_cal,
charlesmn 0:3ac96e360672 266 VL53L1_hist_gen3_dmax_config_t *pdmax_cfg,
charlesmn 0:3ac96e360672 267 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 268 int16_t *pambient_dmax_mm)
charlesmn 0:3ac96e360672 269 {
charlesmn 0:3ac96e360672 270
charlesmn 0:3ac96e360672 271
charlesmn 0:3ac96e360672 272
charlesmn 0:3ac96e360672 273 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 274
charlesmn 0:3ac96e360672 275 VL53L1_hist_gen3_dmax_private_data_t dmax_algo;
charlesmn 0:3ac96e360672 276 VL53L1_hist_gen3_dmax_private_data_t *pdmax_algo = &dmax_algo;
charlesmn 0:3ac96e360672 277
charlesmn 0:3ac96e360672 278 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 279
charlesmn 0:3ac96e360672 280 status =
charlesmn 0:3ac96e360672 281 VL53L1_f_001(
charlesmn 0:3ac96e360672 282 target_reflectance,
charlesmn 0:3ac96e360672 283 pdmax_cal,
charlesmn 0:3ac96e360672 284 pdmax_cfg,
charlesmn 0:3ac96e360672 285 pbins,
charlesmn 0:3ac96e360672 286 pdmax_algo,
charlesmn 0:3ac96e360672 287 pambient_dmax_mm);
charlesmn 0:3ac96e360672 288
charlesmn 0:3ac96e360672 289 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 290
charlesmn 0:3ac96e360672 291 return status;
charlesmn 0:3ac96e360672 292 }
charlesmn 0:3ac96e360672 293