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_xtalk.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_xtalk.h"
Charles MacNeill 5:89031b2f5316 31 #include "vl53lx_hist_core.h"
Charles MacNeill 5:89031b2f5316 32
Charles MacNeill 5:89031b2f5316 33
Charles MacNeill 5:89031b2f5316 34
Charles MacNeill 5:89031b2f5316 35
Charles MacNeill 5:89031b2f5316 36 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 37 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 38 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 39 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 40 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 41 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 42 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 43
Charles MacNeill 5:89031b2f5316 44 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 45 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 46 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 47
Charles MacNeill 5:89031b2f5316 48
Charles MacNeill 5:89031b2f5316 49 VL53LX_Error VL53LX_xtalk_calibration_process_data(
Charles MacNeill 5:89031b2f5316 50 VL53LX_xtalk_range_results_t *pxtalk_results,
Charles MacNeill 5:89031b2f5316 51 VL53LX_xtalk_histogram_data_t *pxtalk_shape,
Charles MacNeill 5:89031b2f5316 52 VL53LX_xtalk_calibration_results_t *pxtalk_cal)
Charles MacNeill 5:89031b2f5316 53 {
Charles MacNeill 5:89031b2f5316 54
Charles MacNeill 5:89031b2f5316 55
Charles MacNeill 5:89031b2f5316 56 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 57
Charles MacNeill 5:89031b2f5316 58 VL53LX_xtalk_algo_data_t xtalk_debug;
Charles MacNeill 5:89031b2f5316 59 VL53LX_xtalk_algo_data_t *pdebug = &xtalk_debug;
Charles MacNeill 5:89031b2f5316 60 VL53LX_xtalk_range_data_t *pxtalk_data = NULL;
Charles MacNeill 5:89031b2f5316 61
Charles MacNeill 5:89031b2f5316 62 VL53LX_histogram_bin_data_t avg_bins;
Charles MacNeill 5:89031b2f5316 63 VL53LX_histogram_bin_data_t *pavg_bins = &avg_bins;
Charles MacNeill 5:89031b2f5316 64
Charles MacNeill 5:89031b2f5316 65 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 66
Charles MacNeill 5:89031b2f5316 67
Charles MacNeill 5:89031b2f5316 68
Charles MacNeill 5:89031b2f5316 69 memcpy(pavg_bins, &(pxtalk_results->central_histogram_avg),
Charles MacNeill 5:89031b2f5316 70 sizeof(VL53LX_histogram_bin_data_t));
Charles MacNeill 5:89031b2f5316 71
Charles MacNeill 5:89031b2f5316 72
Charles MacNeill 5:89031b2f5316 73
Charles MacNeill 5:89031b2f5316 74 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 75 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 76 0, 0, &(pdebug->VL53LX_p_056));
Charles MacNeill 5:89031b2f5316 77
Charles MacNeill 5:89031b2f5316 78 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 79 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 80 0, 0, &(pdebug->VL53LX_p_057));
Charles MacNeill 5:89031b2f5316 81
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83
Charles MacNeill 5:89031b2f5316 84 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 85 status = VL53LX_f_039(
Charles MacNeill 5:89031b2f5316 86 pxtalk_results,
Charles MacNeill 5:89031b2f5316 87 pdebug,
Charles MacNeill 5:89031b2f5316 88 &(pxtalk_cal->algo__crosstalk_compensation_x_plane_gradient_kcps
Charles MacNeill 5:89031b2f5316 89 ),
Charles MacNeill 5:89031b2f5316 90 &(pxtalk_cal->algo__crosstalk_compensation_y_plane_gradient_kcps
Charles MacNeill 5:89031b2f5316 91 ));
Charles MacNeill 5:89031b2f5316 92
Charles MacNeill 5:89031b2f5316 93
Charles MacNeill 5:89031b2f5316 94
Charles MacNeill 5:89031b2f5316 95
Charles MacNeill 5:89031b2f5316 96
Charles MacNeill 5:89031b2f5316 97 if (status != VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 98 goto ENDFUNC;
Charles MacNeill 5:89031b2f5316 99
Charles MacNeill 5:89031b2f5316 100 pxtalk_data = &(pxtalk_results->VL53LX_p_003[4]);
Charles MacNeill 5:89031b2f5316 101
Charles MacNeill 5:89031b2f5316 102 if (pxtalk_data->no_of_samples > 0) {
Charles MacNeill 5:89031b2f5316 103
Charles MacNeill 5:89031b2f5316 104
Charles MacNeill 5:89031b2f5316 105
Charles MacNeill 5:89031b2f5316 106 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 107 memcpy(&(pdebug->VL53LX_p_056),
Charles MacNeill 5:89031b2f5316 108 pavg_bins,
Charles MacNeill 5:89031b2f5316 109 sizeof(VL53LX_histogram_bin_data_t));
Charles MacNeill 5:89031b2f5316 110 }
Charles MacNeill 5:89031b2f5316 111
Charles MacNeill 5:89031b2f5316 112
Charles MacNeill 5:89031b2f5316 113
Charles MacNeill 5:89031b2f5316 114 status = VL53LX_f_040(
Charles MacNeill 5:89031b2f5316 115 pxtalk_data,
Charles MacNeill 5:89031b2f5316 116 pdebug,
Charles MacNeill 5:89031b2f5316 117 &(pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps));
Charles MacNeill 5:89031b2f5316 118
Charles MacNeill 5:89031b2f5316 119
Charles MacNeill 5:89031b2f5316 120
Charles MacNeill 5:89031b2f5316 121 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 122 status = VL53LX_f_041(
Charles MacNeill 5:89031b2f5316 123 pavg_bins,
Charles MacNeill 5:89031b2f5316 124 pdebug,
Charles MacNeill 5:89031b2f5316 125 pxtalk_data,
Charles MacNeill 5:89031b2f5316 126 pxtalk_results->central_histogram__window_start,
Charles MacNeill 5:89031b2f5316 127 pxtalk_results->central_histogram__window_end,
Charles MacNeill 5:89031b2f5316 128 &(pxtalk_shape->xtalk_shape));
Charles MacNeill 5:89031b2f5316 129
Charles MacNeill 5:89031b2f5316 130 } else {
Charles MacNeill 5:89031b2f5316 131
Charles MacNeill 5:89031b2f5316 132
Charles MacNeill 5:89031b2f5316 133
Charles MacNeill 5:89031b2f5316 134 pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps = 0;
Charles MacNeill 5:89031b2f5316 135
Charles MacNeill 5:89031b2f5316 136
Charles MacNeill 5:89031b2f5316 137
Charles MacNeill 5:89031b2f5316 138 pdebug->VL53LX_p_058 = 0;
Charles MacNeill 5:89031b2f5316 139
Charles MacNeill 5:89031b2f5316 140
Charles MacNeill 5:89031b2f5316 141 }
Charles MacNeill 5:89031b2f5316 142
Charles MacNeill 5:89031b2f5316 143
Charles MacNeill 5:89031b2f5316 144 ENDFUNC:
Charles MacNeill 5:89031b2f5316 145
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147
Charles MacNeill 5:89031b2f5316 148 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 149
Charles MacNeill 5:89031b2f5316 150 return status;
Charles MacNeill 5:89031b2f5316 151 }
Charles MacNeill 5:89031b2f5316 152
Charles MacNeill 5:89031b2f5316 153
Charles MacNeill 5:89031b2f5316 154 VL53LX_Error VL53LX_f_041(
Charles MacNeill 5:89031b2f5316 155 VL53LX_histogram_bin_data_t *pavg_bins,
Charles MacNeill 5:89031b2f5316 156 VL53LX_xtalk_algo_data_t *pdebug,
Charles MacNeill 5:89031b2f5316 157 VL53LX_xtalk_range_data_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 158 uint8_t histogram__window_start,
Charles MacNeill 5:89031b2f5316 159 uint8_t histogram__window_end,
Charles MacNeill 5:89031b2f5316 160 VL53LX_xtalk_histogram_shape_t *pxtalk_shape)
Charles MacNeill 5:89031b2f5316 161 {
Charles MacNeill 5:89031b2f5316 162
Charles MacNeill 5:89031b2f5316 163 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 164
Charles MacNeill 5:89031b2f5316 165
Charles MacNeill 5:89031b2f5316 166
Charles MacNeill 5:89031b2f5316 167 uint32_t ambient_thresh = 0;
Charles MacNeill 5:89031b2f5316 168
Charles MacNeill 5:89031b2f5316 169
Charles MacNeill 5:89031b2f5316 170
Charles MacNeill 5:89031b2f5316 171 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 172 VL53LX_f_029(
Charles MacNeill 5:89031b2f5316 173 pavg_bins,
Charles MacNeill 5:89031b2f5316 174 pavg_bins->VL53LX_p_028);
Charles MacNeill 5:89031b2f5316 175
Charles MacNeill 5:89031b2f5316 176
Charles MacNeill 5:89031b2f5316 177
Charles MacNeill 5:89031b2f5316 178 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 179 VL53LX_f_043(
Charles MacNeill 5:89031b2f5316 180 6,
Charles MacNeill 5:89031b2f5316 181 pavg_bins->VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 182 &ambient_thresh);
Charles MacNeill 5:89031b2f5316 183
Charles MacNeill 5:89031b2f5316 184
Charles MacNeill 5:89031b2f5316 185
Charles MacNeill 5:89031b2f5316 186 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 187 status = VL53LX_f_044(
Charles MacNeill 5:89031b2f5316 188 pavg_bins,
Charles MacNeill 5:89031b2f5316 189 ambient_thresh,
Charles MacNeill 5:89031b2f5316 190 histogram__window_start,
Charles MacNeill 5:89031b2f5316 191 histogram__window_end);
Charles MacNeill 5:89031b2f5316 192
Charles MacNeill 5:89031b2f5316 193
Charles MacNeill 5:89031b2f5316 194
Charles MacNeill 5:89031b2f5316 195 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 196 status = VL53LX_f_045(
Charles MacNeill 5:89031b2f5316 197 pavg_bins,
Charles MacNeill 5:89031b2f5316 198 pxtalk_data,
Charles MacNeill 5:89031b2f5316 199 pdebug,
Charles MacNeill 5:89031b2f5316 200 pxtalk_shape);
Charles MacNeill 5:89031b2f5316 201
Charles MacNeill 5:89031b2f5316 202
Charles MacNeill 5:89031b2f5316 203 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205 return status;
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207 }
Charles MacNeill 5:89031b2f5316 208
Charles MacNeill 5:89031b2f5316 209
Charles MacNeill 5:89031b2f5316 210 VL53LX_Error VL53LX_f_039(
Charles MacNeill 5:89031b2f5316 211 VL53LX_xtalk_range_results_t *pxtalk_results,
Charles MacNeill 5:89031b2f5316 212 VL53LX_xtalk_algo_data_t *pdebug,
Charles MacNeill 5:89031b2f5316 213 int16_t *xgradient,
Charles MacNeill 5:89031b2f5316 214 int16_t *ygradient
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
Charles MacNeill 5:89031b2f5316 220 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 221
Charles MacNeill 5:89031b2f5316 222 VL53LX_xtalk_range_data_t *presults_int = NULL;
Charles MacNeill 5:89031b2f5316 223
Charles MacNeill 5:89031b2f5316 224 int i = 0;
Charles MacNeill 5:89031b2f5316 225
Charles MacNeill 5:89031b2f5316 226 uint32_t xtalk_per_spad[4];
Charles MacNeill 5:89031b2f5316 227 int32_t VL53LX_p_059 = 0;
Charles MacNeill 5:89031b2f5316 228 int32_t VL53LX_p_060 = 0;
Charles MacNeill 5:89031b2f5316 229
Charles MacNeill 5:89031b2f5316 230 uint8_t result_invalid = 0;
Charles MacNeill 5:89031b2f5316 231
Charles MacNeill 5:89031b2f5316 232
Charles MacNeill 5:89031b2f5316 233 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 234
Charles MacNeill 5:89031b2f5316 235
Charles MacNeill 5:89031b2f5316 236
Charles MacNeill 5:89031b2f5316 237 *xgradient = 0;
Charles MacNeill 5:89031b2f5316 238 *ygradient = 0;
Charles MacNeill 5:89031b2f5316 239
Charles MacNeill 5:89031b2f5316 240
Charles MacNeill 5:89031b2f5316 241
Charles MacNeill 5:89031b2f5316 242
Charles MacNeill 5:89031b2f5316 243 for (i = 0; i < 4; i++)
Charles MacNeill 5:89031b2f5316 244 xtalk_per_spad[i] = 0;
Charles MacNeill 5:89031b2f5316 245
Charles MacNeill 5:89031b2f5316 246
Charles MacNeill 5:89031b2f5316 247
Charles MacNeill 5:89031b2f5316 248 for (i = 0; i < 4; i++) {
Charles MacNeill 5:89031b2f5316 249
Charles MacNeill 5:89031b2f5316 250 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 251
Charles MacNeill 5:89031b2f5316 252 presults_int = &(pxtalk_results->VL53LX_p_003[i]);
Charles MacNeill 5:89031b2f5316 253
Charles MacNeill 5:89031b2f5316 254
Charles MacNeill 5:89031b2f5316 255
Charles MacNeill 5:89031b2f5316 256
Charles MacNeill 5:89031b2f5316 257 if (presults_int->no_of_samples == 0) {
Charles MacNeill 5:89031b2f5316 258
Charles MacNeill 5:89031b2f5316 259
Charles MacNeill 5:89031b2f5316 260 result_invalid = 1;
Charles MacNeill 5:89031b2f5316 261 pdebug->VL53LX_p_061[i] = 0;
Charles MacNeill 5:89031b2f5316 262
Charles MacNeill 5:89031b2f5316 263
Charles MacNeill 5:89031b2f5316 264 } else {
Charles MacNeill 5:89031b2f5316 265
Charles MacNeill 5:89031b2f5316 266 xtalk_per_spad[i] =
Charles MacNeill 5:89031b2f5316 267 presults_int->rate_per_spad_kcps_avg;
Charles MacNeill 5:89031b2f5316 268
Charles MacNeill 5:89031b2f5316 269
Charles MacNeill 5:89031b2f5316 270
Charles MacNeill 5:89031b2f5316 271 pdebug->VL53LX_p_061[i] =
Charles MacNeill 5:89031b2f5316 272 (uint32_t)xtalk_per_spad[i];
Charles MacNeill 5:89031b2f5316 273
Charles MacNeill 5:89031b2f5316 274 }
Charles MacNeill 5:89031b2f5316 275 }
Charles MacNeill 5:89031b2f5316 276
Charles MacNeill 5:89031b2f5316 277 }
Charles MacNeill 5:89031b2f5316 278
Charles MacNeill 5:89031b2f5316 279
Charles MacNeill 5:89031b2f5316 280
Charles MacNeill 5:89031b2f5316 281 if ((status == VL53LX_ERROR_NONE) && (result_invalid == 0)) {
Charles MacNeill 5:89031b2f5316 282
Charles MacNeill 5:89031b2f5316 283
Charles MacNeill 5:89031b2f5316 284
Charles MacNeill 5:89031b2f5316 285 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 286 VL53LX_p_059 = ((int32_t)xtalk_per_spad[1]
Charles MacNeill 5:89031b2f5316 287 - (int32_t)xtalk_per_spad[0]) / (8);
Charles MacNeill 5:89031b2f5316 288 VL53LX_p_060 = ((int32_t)xtalk_per_spad[3]
Charles MacNeill 5:89031b2f5316 289 - (int32_t)xtalk_per_spad[2]) / (8);
Charles MacNeill 5:89031b2f5316 290 }
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 (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 296 if (VL53LX_p_059 < -32767) {
Charles MacNeill 5:89031b2f5316 297 VL53LX_p_059 = -32767;
Charles MacNeill 5:89031b2f5316 298 } else {
Charles MacNeill 5:89031b2f5316 299 if (VL53LX_p_059 > 32767)
Charles MacNeill 5:89031b2f5316 300 VL53LX_p_059 = 32767;
Charles MacNeill 5:89031b2f5316 301 }
Charles MacNeill 5:89031b2f5316 302
Charles MacNeill 5:89031b2f5316 303 if (VL53LX_p_060 < -32767) {
Charles MacNeill 5:89031b2f5316 304 VL53LX_p_060 = -32767;
Charles MacNeill 5:89031b2f5316 305 } else {
Charles MacNeill 5:89031b2f5316 306 if (VL53LX_p_060 > 32767)
Charles MacNeill 5:89031b2f5316 307 VL53LX_p_060 = 32767;
Charles MacNeill 5:89031b2f5316 308 }
Charles MacNeill 5:89031b2f5316 309
Charles MacNeill 5:89031b2f5316 310
Charles MacNeill 5:89031b2f5316 311
Charles MacNeill 5:89031b2f5316 312 pdebug->VL53LX_p_059 = (int16_t)VL53LX_p_059;
Charles MacNeill 5:89031b2f5316 313 pdebug->VL53LX_p_060 = (int16_t)VL53LX_p_060;
Charles MacNeill 5:89031b2f5316 314 }
Charles MacNeill 5:89031b2f5316 315
Charles MacNeill 5:89031b2f5316 316 } else {
Charles MacNeill 5:89031b2f5316 317
Charles MacNeill 5:89031b2f5316 318
Charles MacNeill 5:89031b2f5316 319
Charles MacNeill 5:89031b2f5316 320 VL53LX_p_059 = 0;
Charles MacNeill 5:89031b2f5316 321 VL53LX_p_060 = 0;
Charles MacNeill 5:89031b2f5316 322
Charles MacNeill 5:89031b2f5316 323 pdebug->VL53LX_p_059 = 0;
Charles MacNeill 5:89031b2f5316 324 pdebug->VL53LX_p_060 = 0;
Charles MacNeill 5:89031b2f5316 325 }
Charles MacNeill 5:89031b2f5316 326
Charles MacNeill 5:89031b2f5316 327
Charles MacNeill 5:89031b2f5316 328
Charles MacNeill 5:89031b2f5316 329 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 330 *xgradient = (int16_t)VL53LX_p_059;
Charles MacNeill 5:89031b2f5316 331 *ygradient = (int16_t)VL53LX_p_060;
Charles MacNeill 5:89031b2f5316 332 }
Charles MacNeill 5:89031b2f5316 333
Charles MacNeill 5:89031b2f5316 334 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 335
Charles MacNeill 5:89031b2f5316 336 return status;
Charles MacNeill 5:89031b2f5316 337 }
Charles MacNeill 5:89031b2f5316 338
Charles MacNeill 5:89031b2f5316 339
Charles MacNeill 5:89031b2f5316 340 VL53LX_Error VL53LX_f_040(
Charles MacNeill 5:89031b2f5316 341 VL53LX_xtalk_range_data_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 342 VL53LX_xtalk_algo_data_t *pdebug,
Charles MacNeill 5:89031b2f5316 343 uint32_t *xtalk_mean_offset_kcps
Charles MacNeill 5:89031b2f5316 344 )
Charles MacNeill 5:89031b2f5316 345 {
Charles MacNeill 5:89031b2f5316 346
Charles MacNeill 5:89031b2f5316 347
Charles MacNeill 5:89031b2f5316 348 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 349
Charles MacNeill 5:89031b2f5316 350 uint32_t xtalk_per_spad = 0;
Charles MacNeill 5:89031b2f5316 351 uint8_t result_invalid = 0;
Charles MacNeill 5:89031b2f5316 352
Charles MacNeill 5:89031b2f5316 353 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 354
Charles MacNeill 5:89031b2f5316 355 *xtalk_mean_offset_kcps = 0;
Charles MacNeill 5:89031b2f5316 356
Charles MacNeill 5:89031b2f5316 357
Charles MacNeill 5:89031b2f5316 358 if (pxtalk_data->no_of_samples == 0) {
Charles MacNeill 5:89031b2f5316 359
Charles MacNeill 5:89031b2f5316 360
Charles MacNeill 5:89031b2f5316 361
Charles MacNeill 5:89031b2f5316 362 result_invalid = 1;
Charles MacNeill 5:89031b2f5316 363
Charles MacNeill 5:89031b2f5316 364
Charles MacNeill 5:89031b2f5316 365
Charles MacNeill 5:89031b2f5316 366 pdebug->VL53LX_p_058 = 0;
Charles MacNeill 5:89031b2f5316 367
Charles MacNeill 5:89031b2f5316 368 }
Charles MacNeill 5:89031b2f5316 369
Charles MacNeill 5:89031b2f5316 370
Charles MacNeill 5:89031b2f5316 371
Charles MacNeill 5:89031b2f5316 372
Charles MacNeill 5:89031b2f5316 373 if ((status == VL53LX_ERROR_NONE) && (result_invalid == 0)) {
Charles MacNeill 5:89031b2f5316 374
Charles MacNeill 5:89031b2f5316 375
Charles MacNeill 5:89031b2f5316 376
Charles MacNeill 5:89031b2f5316 377 xtalk_per_spad = pxtalk_data->rate_per_spad_kcps_avg >> 2;
Charles MacNeill 5:89031b2f5316 378
Charles MacNeill 5:89031b2f5316 379
Charles MacNeill 5:89031b2f5316 380
Charles MacNeill 5:89031b2f5316 381 pdebug->VL53LX_p_058 = xtalk_per_spad;
Charles MacNeill 5:89031b2f5316 382
Charles MacNeill 5:89031b2f5316 383
Charles MacNeill 5:89031b2f5316 384 if (xtalk_per_spad < 0x3FFFF)
Charles MacNeill 5:89031b2f5316 385 *xtalk_mean_offset_kcps = (uint32_t)xtalk_per_spad;
Charles MacNeill 5:89031b2f5316 386 else
Charles MacNeill 5:89031b2f5316 387 *xtalk_mean_offset_kcps = 0x3FFFF;
Charles MacNeill 5:89031b2f5316 388
Charles MacNeill 5:89031b2f5316 389 } else {
Charles MacNeill 5:89031b2f5316 390
Charles MacNeill 5:89031b2f5316 391
Charles MacNeill 5:89031b2f5316 392
Charles MacNeill 5:89031b2f5316 393 *xtalk_mean_offset_kcps = 0;
Charles MacNeill 5:89031b2f5316 394 }
Charles MacNeill 5:89031b2f5316 395
Charles MacNeill 5:89031b2f5316 396 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 397
Charles MacNeill 5:89031b2f5316 398 return status;
Charles MacNeill 5:89031b2f5316 399
Charles MacNeill 5:89031b2f5316 400 }
Charles MacNeill 5:89031b2f5316 401
Charles MacNeill 5:89031b2f5316 402
Charles MacNeill 5:89031b2f5316 403
Charles MacNeill 5:89031b2f5316 404
Charles MacNeill 5:89031b2f5316 405 VL53LX_Error VL53LX_f_045(
Charles MacNeill 5:89031b2f5316 406 VL53LX_histogram_bin_data_t *phist_data,
Charles MacNeill 5:89031b2f5316 407 VL53LX_xtalk_range_data_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 408 VL53LX_xtalk_algo_data_t *pdebug,
Charles MacNeill 5:89031b2f5316 409 VL53LX_xtalk_histogram_shape_t *pxtalk_histo
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
Charles MacNeill 5:89031b2f5316 416 uint8_t idx;
Charles MacNeill 5:89031b2f5316 417 int32_t tmpi32;
Charles MacNeill 5:89031b2f5316 418 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 419 uint64_t bin_data[VL53LX_XTALK_HISTO_BINS];
Charles MacNeill 5:89031b2f5316 420
Charles MacNeill 5:89031b2f5316 421 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 422
Charles MacNeill 5:89031b2f5316 423
Charles MacNeill 5:89031b2f5316 424
Charles MacNeill 5:89031b2f5316 425
Charles MacNeill 5:89031b2f5316 426
Charles MacNeill 5:89031b2f5316 427
Charles MacNeill 5:89031b2f5316 428 pxtalk_histo->VL53LX_p_020 =
Charles MacNeill 5:89031b2f5316 429 phist_data->VL53LX_p_020;
Charles MacNeill 5:89031b2f5316 430 pxtalk_histo->cal_config__vcsel_start =
Charles MacNeill 5:89031b2f5316 431 phist_data->cal_config__vcsel_start;
Charles MacNeill 5:89031b2f5316 432 pxtalk_histo->VL53LX_p_015 =
Charles MacNeill 5:89031b2f5316 433 phist_data->VL53LX_p_015;
Charles MacNeill 5:89031b2f5316 434 pxtalk_histo->VL53LX_p_019 =
Charles MacNeill 5:89031b2f5316 435 phist_data->VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 436 pxtalk_histo->time_stamp =
Charles MacNeill 5:89031b2f5316 437 phist_data->time_stamp;
Charles MacNeill 5:89031b2f5316 438 pxtalk_histo->vcsel_width =
Charles MacNeill 5:89031b2f5316 439 phist_data->vcsel_width;
Charles MacNeill 5:89031b2f5316 440 pxtalk_histo->zero_distance_phase =
Charles MacNeill 5:89031b2f5316 441 phist_data->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 442 pxtalk_histo->zone_id =
Charles MacNeill 5:89031b2f5316 443 phist_data->zone_id;
Charles MacNeill 5:89031b2f5316 444 pxtalk_histo->VL53LX_p_021 =
Charles MacNeill 5:89031b2f5316 445 VL53LX_XTALK_HISTO_BINS;
Charles MacNeill 5:89031b2f5316 446 pxtalk_histo->phasecal_result__reference_phase =
Charles MacNeill 5:89031b2f5316 447 phist_data->phasecal_result__reference_phase;
Charles MacNeill 5:89031b2f5316 448 pxtalk_histo->phasecal_result__vcsel_start =
Charles MacNeill 5:89031b2f5316 449 phist_data->phasecal_result__vcsel_start;
Charles MacNeill 5:89031b2f5316 450
Charles MacNeill 5:89031b2f5316 451
Charles MacNeill 5:89031b2f5316 452 memcpy(&(pdebug->VL53LX_p_057),
Charles MacNeill 5:89031b2f5316 453 phist_data, sizeof(VL53LX_histogram_bin_data_t));
Charles MacNeill 5:89031b2f5316 454
Charles MacNeill 5:89031b2f5316 455
Charles MacNeill 5:89031b2f5316 456
Charles MacNeill 5:89031b2f5316 457
Charles MacNeill 5:89031b2f5316 458
Charles MacNeill 5:89031b2f5316 459 if (pxtalk_data->signal_total_events_avg == 0) {
Charles MacNeill 5:89031b2f5316 460 for (i = 0; i < pxtalk_histo->VL53LX_p_021; i++)
Charles MacNeill 5:89031b2f5316 461 bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 462 goto FAIL;
Charles MacNeill 5:89031b2f5316 463 }
Charles MacNeill 5:89031b2f5316 464
Charles MacNeill 5:89031b2f5316 465 for (i = 0; i < pxtalk_histo->VL53LX_p_021; i++) {
Charles MacNeill 5:89031b2f5316 466 idx = i + phist_data->number_of_ambient_bins;
Charles MacNeill 5:89031b2f5316 467 if (phist_data->bin_data[idx] > 0) {
Charles MacNeill 5:89031b2f5316 468 bin_data[i] =
Charles MacNeill 5:89031b2f5316 469 ((uint64_t)phist_data->bin_data[idx] << 10);
Charles MacNeill 5:89031b2f5316 470 tmpi32 = pxtalk_data->signal_total_events_avg / 2;
Charles MacNeill 5:89031b2f5316 471 bin_data[i] = bin_data[i] + (uint64_t)tmpi32;
Charles MacNeill 5:89031b2f5316 472 bin_data[i] = do_division_u(bin_data[i],
Charles MacNeill 5:89031b2f5316 473 (uint64_t)pxtalk_data->signal_total_events_avg);
Charles MacNeill 5:89031b2f5316 474 } else {
Charles MacNeill 5:89031b2f5316 475 bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 476 }
Charles MacNeill 5:89031b2f5316 477 }
Charles MacNeill 5:89031b2f5316 478
Charles MacNeill 5:89031b2f5316 479 FAIL:
Charles MacNeill 5:89031b2f5316 480
Charles MacNeill 5:89031b2f5316 481
Charles MacNeill 5:89031b2f5316 482 for (i = 0; i < VL53LX_XTALK_HISTO_BINS; i++)
Charles MacNeill 5:89031b2f5316 483 pxtalk_histo->bin_data[i] = (uint32_t)bin_data[i];
Charles MacNeill 5:89031b2f5316 484
Charles MacNeill 5:89031b2f5316 485
Charles MacNeill 5:89031b2f5316 486
Charles MacNeill 5:89031b2f5316 487 for (i = 0; i < pxtalk_histo->VL53LX_p_021; i++)
Charles MacNeill 5:89031b2f5316 488 pdebug->VL53LX_p_062[i] = pxtalk_histo->bin_data[i];
Charles MacNeill 5:89031b2f5316 489
Charles MacNeill 5:89031b2f5316 490
Charles MacNeill 5:89031b2f5316 491 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 492
Charles MacNeill 5:89031b2f5316 493 return status;
Charles MacNeill 5:89031b2f5316 494
Charles MacNeill 5:89031b2f5316 495 }
Charles MacNeill 5:89031b2f5316 496
Charles MacNeill 5:89031b2f5316 497
Charles MacNeill 5:89031b2f5316 498 VL53LX_Error VL53LX_f_046(
Charles MacNeill 5:89031b2f5316 499 VL53LX_customer_nvm_managed_t *pcustomer,
Charles MacNeill 5:89031b2f5316 500 VL53LX_dynamic_config_t *pdyn_cfg,
Charles MacNeill 5:89031b2f5316 501 VL53LX_xtalk_histogram_data_t *pxtalk_shape,
Charles MacNeill 5:89031b2f5316 502 VL53LX_histogram_bin_data_t *pip_hist_data,
Charles MacNeill 5:89031b2f5316 503 VL53LX_histogram_bin_data_t *pop_hist_data,
Charles MacNeill 5:89031b2f5316 504 VL53LX_histogram_bin_data_t *pxtalk_count_data)
Charles MacNeill 5:89031b2f5316 505 {
Charles MacNeill 5:89031b2f5316 506
Charles MacNeill 5:89031b2f5316 507
Charles MacNeill 5:89031b2f5316 508
Charles MacNeill 5:89031b2f5316 509 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 510
Charles MacNeill 5:89031b2f5316 511
Charles MacNeill 5:89031b2f5316 512
Charles MacNeill 5:89031b2f5316 513 uint32_t xtalk_rate_kcps = 0;
Charles MacNeill 5:89031b2f5316 514
Charles MacNeill 5:89031b2f5316 515 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 516
Charles MacNeill 5:89031b2f5316 517
Charles MacNeill 5:89031b2f5316 518
Charles MacNeill 5:89031b2f5316 519 memcpy(pop_hist_data, pip_hist_data,
Charles MacNeill 5:89031b2f5316 520 sizeof(VL53LX_histogram_bin_data_t));
Charles MacNeill 5:89031b2f5316 521
Charles MacNeill 5:89031b2f5316 522
Charles MacNeill 5:89031b2f5316 523
Charles MacNeill 5:89031b2f5316 524 status =
Charles MacNeill 5:89031b2f5316 525 VL53LX_f_032(
Charles MacNeill 5:89031b2f5316 526 pcustomer->algo__crosstalk_compensation_plane_offset_kcps,
Charles MacNeill 5:89031b2f5316 527 pcustomer->algo__crosstalk_compensation_x_plane_gradient_kcps,
Charles MacNeill 5:89031b2f5316 528 pcustomer->algo__crosstalk_compensation_y_plane_gradient_kcps,
Charles MacNeill 5:89031b2f5316 529 0,
Charles MacNeill 5:89031b2f5316 530 0,
Charles MacNeill 5:89031b2f5316 531 pip_hist_data->result__dss_actual_effective_spads,
Charles MacNeill 5:89031b2f5316 532
Charles MacNeill 5:89031b2f5316 533 pdyn_cfg->roi_config__user_roi_centre_spad,
Charles MacNeill 5:89031b2f5316 534 pdyn_cfg->roi_config__user_roi_requested_global_xy_size,
Charles MacNeill 5:89031b2f5316 535 &(xtalk_rate_kcps));
Charles MacNeill 5:89031b2f5316 536
Charles MacNeill 5:89031b2f5316 537
Charles MacNeill 5:89031b2f5316 538
Charles MacNeill 5:89031b2f5316 539 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 540 status =
Charles MacNeill 5:89031b2f5316 541 VL53LX_f_033(
Charles MacNeill 5:89031b2f5316 542 pip_hist_data,
Charles MacNeill 5:89031b2f5316 543 &(pxtalk_shape->xtalk_shape),
Charles MacNeill 5:89031b2f5316 544 xtalk_rate_kcps,
Charles MacNeill 5:89031b2f5316 545 pxtalk_count_data);
Charles MacNeill 5:89031b2f5316 546
Charles MacNeill 5:89031b2f5316 547
Charles MacNeill 5:89031b2f5316 548
Charles MacNeill 5:89031b2f5316 549 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 550 status =
Charles MacNeill 5:89031b2f5316 551 VL53LX_f_047(
Charles MacNeill 5:89031b2f5316 552 pop_hist_data,
Charles MacNeill 5:89031b2f5316 553 pxtalk_count_data,
Charles MacNeill 5:89031b2f5316 554 pip_hist_data->number_of_ambient_bins);
Charles MacNeill 5:89031b2f5316 555
Charles MacNeill 5:89031b2f5316 556 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 557
Charles MacNeill 5:89031b2f5316 558 return status;
Charles MacNeill 5:89031b2f5316 559 }
Charles MacNeill 5:89031b2f5316 560
Charles MacNeill 5:89031b2f5316 561
Charles MacNeill 5:89031b2f5316 562 VL53LX_Error VL53LX_f_032(
Charles MacNeill 5:89031b2f5316 563 uint32_t mean_offset,
Charles MacNeill 5:89031b2f5316 564 int16_t xgradient,
Charles MacNeill 5:89031b2f5316 565 int16_t ygradient,
Charles MacNeill 5:89031b2f5316 566 int8_t centre_offset_x,
Charles MacNeill 5:89031b2f5316 567 int8_t centre_offset_y,
Charles MacNeill 5:89031b2f5316 568 uint16_t roi_effective_spads,
Charles MacNeill 5:89031b2f5316 569 uint8_t roi_centre_spad,
Charles MacNeill 5:89031b2f5316 570 uint8_t roi_xy_size,
Charles MacNeill 5:89031b2f5316 571 uint32_t *xtalk_rate_kcps
Charles MacNeill 5:89031b2f5316 572 )
Charles MacNeill 5:89031b2f5316 573 {
Charles MacNeill 5:89031b2f5316 574
Charles MacNeill 5:89031b2f5316 575
Charles MacNeill 5:89031b2f5316 576
Charles MacNeill 5:89031b2f5316 577 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 578
Charles MacNeill 5:89031b2f5316 579 uint8_t row = 0;
Charles MacNeill 5:89031b2f5316 580 uint8_t col = 0;
Charles MacNeill 5:89031b2f5316 581
Charles MacNeill 5:89031b2f5316 582
Charles MacNeill 5:89031b2f5316 583
Charles MacNeill 5:89031b2f5316 584 int16_t bound_l_x = 0;
Charles MacNeill 5:89031b2f5316 585 int16_t bound_r_x = 0;
Charles MacNeill 5:89031b2f5316 586 int16_t bound_u_y = 0;
Charles MacNeill 5:89031b2f5316 587 int16_t bound_d_y = 0;
Charles MacNeill 5:89031b2f5316 588
Charles MacNeill 5:89031b2f5316 589 int64_t xtalk_rate_ll = 0;
Charles MacNeill 5:89031b2f5316 590 int64_t xtalk_rate_ur = 0;
Charles MacNeill 5:89031b2f5316 591
Charles MacNeill 5:89031b2f5316 592 int64_t xtalk_avg = 0;
Charles MacNeill 5:89031b2f5316 593
Charles MacNeill 5:89031b2f5316 594 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 595
Charles MacNeill 5:89031b2f5316 596 SUPPRESS_UNUSED_WARNING(roi_effective_spads);
Charles MacNeill 5:89031b2f5316 597
Charles MacNeill 5:89031b2f5316 598
Charles MacNeill 5:89031b2f5316 599
Charles MacNeill 5:89031b2f5316 600
Charles MacNeill 5:89031b2f5316 601
Charles MacNeill 5:89031b2f5316 602
Charles MacNeill 5:89031b2f5316 603 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 604 VL53LX_decode_row_col(
Charles MacNeill 5:89031b2f5316 605 roi_centre_spad,
Charles MacNeill 5:89031b2f5316 606 &row,
Charles MacNeill 5:89031b2f5316 607 &col);
Charles MacNeill 5:89031b2f5316 608 }
Charles MacNeill 5:89031b2f5316 609
Charles MacNeill 5:89031b2f5316 610 trace_print(
Charles MacNeill 5:89031b2f5316 611 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 612 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 613 "Row", row);
Charles MacNeill 5:89031b2f5316 614
Charles MacNeill 5:89031b2f5316 615 trace_print(
Charles MacNeill 5:89031b2f5316 616 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 617 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 618 "Col", col);
Charles MacNeill 5:89031b2f5316 619
Charles MacNeill 5:89031b2f5316 620
Charles MacNeill 5:89031b2f5316 621
Charles MacNeill 5:89031b2f5316 622 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 623 if ((((int16_t)roi_xy_size / 16) & 0x01) == 1)
Charles MacNeill 5:89031b2f5316 624 bound_l_x = (int16_t) col -
Charles MacNeill 5:89031b2f5316 625 (((int16_t)roi_xy_size / 32) + 1);
Charles MacNeill 5:89031b2f5316 626 else
Charles MacNeill 5:89031b2f5316 627 bound_l_x = (int16_t) col -
Charles MacNeill 5:89031b2f5316 628 ((int16_t)roi_xy_size / 32);
Charles MacNeill 5:89031b2f5316 629
Charles MacNeill 5:89031b2f5316 630 bound_r_x = (int16_t) col + ((int16_t)roi_xy_size / 32);
Charles MacNeill 5:89031b2f5316 631
Charles MacNeill 5:89031b2f5316 632 if ((((int16_t)roi_xy_size) & 0x01) == 1)
Charles MacNeill 5:89031b2f5316 633 bound_d_y = (int16_t) row -
Charles MacNeill 5:89031b2f5316 634 ((((int16_t)roi_xy_size & 0x0f) / 2) + 1);
Charles MacNeill 5:89031b2f5316 635 else
Charles MacNeill 5:89031b2f5316 636 bound_d_y = (int16_t) row -
Charles MacNeill 5:89031b2f5316 637 (((int16_t)roi_xy_size & 0x0f) / 2);
Charles MacNeill 5:89031b2f5316 638
Charles MacNeill 5:89031b2f5316 639 bound_u_y = (int16_t) row +
Charles MacNeill 5:89031b2f5316 640 (((int16_t)roi_xy_size & 0xf) / 2);
Charles MacNeill 5:89031b2f5316 641 }
Charles MacNeill 5:89031b2f5316 642
Charles MacNeill 5:89031b2f5316 643 trace_print(
Charles MacNeill 5:89031b2f5316 644 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 645 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 646 "Bound_l_x", bound_l_x);
Charles MacNeill 5:89031b2f5316 647 trace_print(
Charles MacNeill 5:89031b2f5316 648 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 649 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 650 "Bound_r_x", bound_r_x);
Charles MacNeill 5:89031b2f5316 651 trace_print(
Charles MacNeill 5:89031b2f5316 652 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 653 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 654 "Bound_u_y", bound_u_y);
Charles MacNeill 5:89031b2f5316 655 trace_print(
Charles MacNeill 5:89031b2f5316 656 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 657 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 658 "Bound_d_y", bound_d_y);
Charles MacNeill 5:89031b2f5316 659
Charles MacNeill 5:89031b2f5316 660
Charles MacNeill 5:89031b2f5316 661
Charles MacNeill 5:89031b2f5316 662
Charles MacNeill 5:89031b2f5316 663 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 664 bound_l_x = (2 * bound_l_x) - 15 +
Charles MacNeill 5:89031b2f5316 665 (2 * (int16_t)centre_offset_x);
Charles MacNeill 5:89031b2f5316 666 bound_r_x = (2 * bound_r_x) - 15 +
Charles MacNeill 5:89031b2f5316 667 (2 * (int16_t)centre_offset_x);
Charles MacNeill 5:89031b2f5316 668 bound_u_y = (2 * bound_u_y) - 15 +
Charles MacNeill 5:89031b2f5316 669 (2 * (int16_t)centre_offset_y);
Charles MacNeill 5:89031b2f5316 670 bound_d_y = (2 * bound_d_y) - 15 +
Charles MacNeill 5:89031b2f5316 671 (2 * (int16_t)centre_offset_y);
Charles MacNeill 5:89031b2f5316 672 }
Charles MacNeill 5:89031b2f5316 673
Charles MacNeill 5:89031b2f5316 674 trace_print(
Charles MacNeill 5:89031b2f5316 675 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 676 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 677 "Bound_l_x", bound_l_x);
Charles MacNeill 5:89031b2f5316 678
Charles MacNeill 5:89031b2f5316 679 trace_print(
Charles MacNeill 5:89031b2f5316 680 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 681 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 682 "Bound_r_x", bound_r_x);
Charles MacNeill 5:89031b2f5316 683
Charles MacNeill 5:89031b2f5316 684 trace_print(
Charles MacNeill 5:89031b2f5316 685 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 686 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 687 "Bound_u_y", bound_u_y);
Charles MacNeill 5:89031b2f5316 688
Charles MacNeill 5:89031b2f5316 689 trace_print(
Charles MacNeill 5:89031b2f5316 690 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 691 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 692 "Bound_d_y", bound_d_y);
Charles MacNeill 5:89031b2f5316 693
Charles MacNeill 5:89031b2f5316 694
Charles MacNeill 5:89031b2f5316 695
Charles MacNeill 5:89031b2f5316 696
Charles MacNeill 5:89031b2f5316 697 if (status == VL53LX_ERROR_NONE) {
Charles MacNeill 5:89031b2f5316 698 xtalk_rate_ll = ((int64_t)bound_l_x *
Charles MacNeill 5:89031b2f5316 699 ((int64_t)xgradient)) + ((int64_t)bound_d_y *
Charles MacNeill 5:89031b2f5316 700 ((int64_t)ygradient));
Charles MacNeill 5:89031b2f5316 701 xtalk_rate_ll = do_division_s((xtalk_rate_ll + 1), 2);
Charles MacNeill 5:89031b2f5316 702 xtalk_rate_ll += ((int64_t)mean_offset * 4);
Charles MacNeill 5:89031b2f5316 703
Charles MacNeill 5:89031b2f5316 704 xtalk_rate_ur = ((int64_t)bound_r_x *
Charles MacNeill 5:89031b2f5316 705 ((int64_t)xgradient)) + ((int64_t)bound_u_y *
Charles MacNeill 5:89031b2f5316 706 ((int64_t)ygradient));
Charles MacNeill 5:89031b2f5316 707 xtalk_rate_ur = do_division_s((xtalk_rate_ur + 1), 2);
Charles MacNeill 5:89031b2f5316 708 xtalk_rate_ur += ((int64_t)mean_offset * 4);
Charles MacNeill 5:89031b2f5316 709 }
Charles MacNeill 5:89031b2f5316 710
Charles MacNeill 5:89031b2f5316 711 trace_print(
Charles MacNeill 5:89031b2f5316 712 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 713 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 714 "xtalk_rate_ll", xtalk_rate_ll);
Charles MacNeill 5:89031b2f5316 715
Charles MacNeill 5:89031b2f5316 716 trace_print(
Charles MacNeill 5:89031b2f5316 717 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 718 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 719 "xtalk_rate_ur", xtalk_rate_ur);
Charles MacNeill 5:89031b2f5316 720
Charles MacNeill 5:89031b2f5316 721
Charles MacNeill 5:89031b2f5316 722
Charles MacNeill 5:89031b2f5316 723 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 724 xtalk_avg = do_division_s(
Charles MacNeill 5:89031b2f5316 725 ((xtalk_rate_ll + xtalk_rate_ur) + 1), 2);
Charles MacNeill 5:89031b2f5316 726
Charles MacNeill 5:89031b2f5316 727 trace_print(
Charles MacNeill 5:89031b2f5316 728 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 729 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 730 "xtalk_avg", xtalk_avg);
Charles MacNeill 5:89031b2f5316 731
Charles MacNeill 5:89031b2f5316 732
Charles MacNeill 5:89031b2f5316 733
Charles MacNeill 5:89031b2f5316 734 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 735 if (xtalk_avg < 0)
Charles MacNeill 5:89031b2f5316 736 xtalk_avg = 0;
Charles MacNeill 5:89031b2f5316 737
Charles MacNeill 5:89031b2f5316 738
Charles MacNeill 5:89031b2f5316 739
Charles MacNeill 5:89031b2f5316 740
Charles MacNeill 5:89031b2f5316 741
Charles MacNeill 5:89031b2f5316 742 *xtalk_rate_kcps = (uint32_t) xtalk_avg;
Charles MacNeill 5:89031b2f5316 743
Charles MacNeill 5:89031b2f5316 744 trace_print(
Charles MacNeill 5:89031b2f5316 745 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 746 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 747 "xtalk_rate_kcps", xtalk_avg);
Charles MacNeill 5:89031b2f5316 748
Charles MacNeill 5:89031b2f5316 749 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 750
Charles MacNeill 5:89031b2f5316 751 return status;
Charles MacNeill 5:89031b2f5316 752 }
Charles MacNeill 5:89031b2f5316 753
Charles MacNeill 5:89031b2f5316 754
Charles MacNeill 5:89031b2f5316 755
Charles MacNeill 5:89031b2f5316 756 VL53LX_Error VL53LX_f_033(
Charles MacNeill 5:89031b2f5316 757 VL53LX_histogram_bin_data_t *phist_data,
Charles MacNeill 5:89031b2f5316 758 VL53LX_xtalk_histogram_shape_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 759 uint32_t xtalk_rate_kcps,
Charles MacNeill 5:89031b2f5316 760 VL53LX_histogram_bin_data_t *pxtalkcount_data
Charles MacNeill 5:89031b2f5316 761 )
Charles MacNeill 5:89031b2f5316 762 {
Charles MacNeill 5:89031b2f5316 763
Charles MacNeill 5:89031b2f5316 764
Charles MacNeill 5:89031b2f5316 765 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 766
Charles MacNeill 5:89031b2f5316 767 uint64_t xtalk_events_per_spad = 0;
Charles MacNeill 5:89031b2f5316 768 uint64_t xtalk_total_events = 0;
Charles MacNeill 5:89031b2f5316 769 uint64_t xtalk_temp_bin = 0;
Charles MacNeill 5:89031b2f5316 770
Charles MacNeill 5:89031b2f5316 771 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 772
Charles MacNeill 5:89031b2f5316 773 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 774
Charles MacNeill 5:89031b2f5316 775 trace_print(
Charles MacNeill 5:89031b2f5316 776 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 777 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 778 "pk_duration_internal", phist_data->peak_duration_us);
Charles MacNeill 5:89031b2f5316 779
Charles MacNeill 5:89031b2f5316 780
Charles MacNeill 5:89031b2f5316 781
Charles MacNeill 5:89031b2f5316 782 xtalk_events_per_spad = do_division_u((((uint64_t)xtalk_rate_kcps *
Charles MacNeill 5:89031b2f5316 783 (uint64_t)phist_data->peak_duration_us) + 500), 1000);
Charles MacNeill 5:89031b2f5316 784
Charles MacNeill 5:89031b2f5316 785
Charles MacNeill 5:89031b2f5316 786 trace_print(
Charles MacNeill 5:89031b2f5316 787 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 788 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 789 "xtalk_events_per_spad", xtalk_events_per_spad);
Charles MacNeill 5:89031b2f5316 790
Charles MacNeill 5:89031b2f5316 791
Charles MacNeill 5:89031b2f5316 792
Charles MacNeill 5:89031b2f5316 793
Charles MacNeill 5:89031b2f5316 794 xtalk_total_events = xtalk_events_per_spad *
Charles MacNeill 5:89031b2f5316 795 (uint64_t)phist_data->result__dss_actual_effective_spads;
Charles MacNeill 5:89031b2f5316 796
Charles MacNeill 5:89031b2f5316 797 xtalk_total_events = do_division_u((xtalk_total_events), 256);
Charles MacNeill 5:89031b2f5316 798
Charles MacNeill 5:89031b2f5316 799 xtalk_total_events = do_division_u((xtalk_total_events + 1024), 2048);
Charles MacNeill 5:89031b2f5316 800
Charles MacNeill 5:89031b2f5316 801 if (xtalk_total_events > 0xFFFFFFFF)
Charles MacNeill 5:89031b2f5316 802 xtalk_total_events = 0xFFFFFFFF;
Charles MacNeill 5:89031b2f5316 803
Charles MacNeill 5:89031b2f5316 804 trace_print(
Charles MacNeill 5:89031b2f5316 805 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 806 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 807 "xtalk_total_events", xtalk_total_events);
Charles MacNeill 5:89031b2f5316 808
Charles MacNeill 5:89031b2f5316 809
Charles MacNeill 5:89031b2f5316 810
Charles MacNeill 5:89031b2f5316 811
Charles MacNeill 5:89031b2f5316 812
Charles MacNeill 5:89031b2f5316 813
Charles MacNeill 5:89031b2f5316 814 for (i = 0; i < pxtalk_data->VL53LX_p_021; i++) {
Charles MacNeill 5:89031b2f5316 815 xtalk_temp_bin = (uint64_t)pxtalk_data->bin_data[i] *
Charles MacNeill 5:89031b2f5316 816 (uint64_t)xtalk_total_events;
Charles MacNeill 5:89031b2f5316 817 xtalk_temp_bin = do_division_u((xtalk_temp_bin + 512), 1024);
Charles MacNeill 5:89031b2f5316 818
Charles MacNeill 5:89031b2f5316 819 pxtalkcount_data->bin_data[i] = (uint32_t)xtalk_temp_bin;
Charles MacNeill 5:89031b2f5316 820
Charles MacNeill 5:89031b2f5316 821 trace_print(
Charles MacNeill 5:89031b2f5316 822 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 823 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 824 "bin_data", pxtalkcount_data->bin_data[i]);
Charles MacNeill 5:89031b2f5316 825 }
Charles MacNeill 5:89031b2f5316 826
Charles MacNeill 5:89031b2f5316 827 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 828
Charles MacNeill 5:89031b2f5316 829 return status;
Charles MacNeill 5:89031b2f5316 830 }
Charles MacNeill 5:89031b2f5316 831
Charles MacNeill 5:89031b2f5316 832
Charles MacNeill 5:89031b2f5316 833 VL53LX_Error VL53LX_f_047(
Charles MacNeill 5:89031b2f5316 834 VL53LX_histogram_bin_data_t *phist_data,
Charles MacNeill 5:89031b2f5316 835 VL53LX_histogram_bin_data_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 836 uint8_t xtalk_bin_offset)
Charles MacNeill 5:89031b2f5316 837 {
Charles MacNeill 5:89031b2f5316 838
Charles MacNeill 5:89031b2f5316 839
Charles MacNeill 5:89031b2f5316 840 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 841
Charles MacNeill 5:89031b2f5316 842 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 843
Charles MacNeill 5:89031b2f5316 844 int32_t temp_bin;
Charles MacNeill 5:89031b2f5316 845
Charles MacNeill 5:89031b2f5316 846 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 847
Charles MacNeill 5:89031b2f5316 848
Charles MacNeill 5:89031b2f5316 849
Charles MacNeill 5:89031b2f5316 850
Charles MacNeill 5:89031b2f5316 851 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 852 for (i = xtalk_bin_offset;
Charles MacNeill 5:89031b2f5316 853 i < pxtalk_data->VL53LX_p_021; i++) {
Charles MacNeill 5:89031b2f5316 854
Charles MacNeill 5:89031b2f5316 855 temp_bin = (int32_t)phist_data->bin_data[i] -
Charles MacNeill 5:89031b2f5316 856 (int32_t)pxtalk_data->bin_data[i - xtalk_bin_offset];
Charles MacNeill 5:89031b2f5316 857
Charles MacNeill 5:89031b2f5316 858 if (temp_bin < 0)
Charles MacNeill 5:89031b2f5316 859 temp_bin = 0;
Charles MacNeill 5:89031b2f5316 860
Charles MacNeill 5:89031b2f5316 861 phist_data->bin_data[i] = (uint32_t)temp_bin;
Charles MacNeill 5:89031b2f5316 862 }
Charles MacNeill 5:89031b2f5316 863
Charles MacNeill 5:89031b2f5316 864
Charles MacNeill 5:89031b2f5316 865 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 866
Charles MacNeill 5:89031b2f5316 867 return status;
Charles MacNeill 5:89031b2f5316 868 }
Charles MacNeill 5:89031b2f5316 869
Charles MacNeill 5:89031b2f5316 870
Charles MacNeill 5:89031b2f5316 871 VL53LX_Error VL53LX_f_044(
Charles MacNeill 5:89031b2f5316 872 VL53LX_histogram_bin_data_t *pxtalk_data,
Charles MacNeill 5:89031b2f5316 873 uint32_t amb_threshold,
Charles MacNeill 5:89031b2f5316 874 uint8_t VL53LX_p_019,
Charles MacNeill 5:89031b2f5316 875 uint8_t VL53LX_p_024)
Charles MacNeill 5:89031b2f5316 876 {
Charles MacNeill 5:89031b2f5316 877
Charles MacNeill 5:89031b2f5316 878
Charles MacNeill 5:89031b2f5316 879 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 880
Charles MacNeill 5:89031b2f5316 881 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 882 uint8_t first_bin_int = 0;
Charles MacNeill 5:89031b2f5316 883 uint8_t first_bin_inc = 0;
Charles MacNeill 5:89031b2f5316 884 uint8_t last_bin_int = 0;
Charles MacNeill 5:89031b2f5316 885 uint8_t realign_bin = 0;
Charles MacNeill 5:89031b2f5316 886 uint8_t realign_index = 0;
Charles MacNeill 5:89031b2f5316 887 int32_t realign_bin_data[VL53LX_HISTOGRAM_BUFFER_SIZE];
Charles MacNeill 5:89031b2f5316 888
Charles MacNeill 5:89031b2f5316 889 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 890
Charles MacNeill 5:89031b2f5316 891
Charles MacNeill 5:89031b2f5316 892
Charles MacNeill 5:89031b2f5316 893 for (i = 0 ; i < VL53LX_HISTOGRAM_BUFFER_SIZE ; i++)
Charles MacNeill 5:89031b2f5316 894 realign_bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 895
Charles MacNeill 5:89031b2f5316 896 first_bin_int = VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 897 last_bin_int = VL53LX_p_024;
Charles MacNeill 5:89031b2f5316 898
Charles MacNeill 5:89031b2f5316 899
Charles MacNeill 5:89031b2f5316 900
Charles MacNeill 5:89031b2f5316 901
Charles MacNeill 5:89031b2f5316 902
Charles MacNeill 5:89031b2f5316 903 VL53LX_hist_remove_ambient_bins(pxtalk_data);
Charles MacNeill 5:89031b2f5316 904
Charles MacNeill 5:89031b2f5316 905
Charles MacNeill 5:89031b2f5316 906
Charles MacNeill 5:89031b2f5316 907 first_bin_int = (first_bin_int) %
Charles MacNeill 5:89031b2f5316 908 pxtalk_data->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 909
Charles MacNeill 5:89031b2f5316 910 last_bin_int = (last_bin_int) %
Charles MacNeill 5:89031b2f5316 911 pxtalk_data->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 912
Charles MacNeill 5:89031b2f5316 913 first_bin_inc = (first_bin_int + 1) % pxtalk_data->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 914
Charles MacNeill 5:89031b2f5316 915
Charles MacNeill 5:89031b2f5316 916
Charles MacNeill 5:89031b2f5316 917 if (first_bin_inc > last_bin_int) {
Charles MacNeill 5:89031b2f5316 918
Charles MacNeill 5:89031b2f5316 919
Charles MacNeill 5:89031b2f5316 920 realign_bin = pxtalk_data->VL53LX_p_021 - first_bin_inc;
Charles MacNeill 5:89031b2f5316 921
Charles MacNeill 5:89031b2f5316 922
Charles MacNeill 5:89031b2f5316 923 first_bin_int = (first_bin_int + realign_bin) %
Charles MacNeill 5:89031b2f5316 924 pxtalk_data->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 925 last_bin_int = (last_bin_int + realign_bin) %
Charles MacNeill 5:89031b2f5316 926 pxtalk_data->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 927
Charles MacNeill 5:89031b2f5316 928
Charles MacNeill 5:89031b2f5316 929 pxtalk_data->zero_distance_phase =
Charles MacNeill 5:89031b2f5316 930 pxtalk_data->zero_distance_phase +
Charles MacNeill 5:89031b2f5316 931 ((uint16_t)realign_bin * 2048);
Charles MacNeill 5:89031b2f5316 932 }
Charles MacNeill 5:89031b2f5316 933
Charles MacNeill 5:89031b2f5316 934 if (realign_bin > 0) {
Charles MacNeill 5:89031b2f5316 935
Charles MacNeill 5:89031b2f5316 936 for (i = 0; i < pxtalk_data->VL53LX_p_021; i++)
Charles MacNeill 5:89031b2f5316 937 realign_bin_data[i] = pxtalk_data->bin_data[i];
Charles MacNeill 5:89031b2f5316 938
Charles MacNeill 5:89031b2f5316 939
Charles MacNeill 5:89031b2f5316 940 for (i = 0; i < pxtalk_data->VL53LX_p_021; i++) {
Charles MacNeill 5:89031b2f5316 941 realign_index = (pxtalk_data->VL53LX_p_021 -
Charles MacNeill 5:89031b2f5316 942 realign_bin + i)
Charles MacNeill 5:89031b2f5316 943 % pxtalk_data->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 944
Charles MacNeill 5:89031b2f5316 945 pxtalk_data->bin_data[i] =
Charles MacNeill 5:89031b2f5316 946 realign_bin_data[realign_index];
Charles MacNeill 5:89031b2f5316 947 }
Charles MacNeill 5:89031b2f5316 948 }
Charles MacNeill 5:89031b2f5316 949
Charles MacNeill 5:89031b2f5316 950
Charles MacNeill 5:89031b2f5316 951
Charles MacNeill 5:89031b2f5316 952
Charles MacNeill 5:89031b2f5316 953 trace_print(
Charles MacNeill 5:89031b2f5316 954 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 955 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 956 "first bin int", first_bin_int);
Charles MacNeill 5:89031b2f5316 957
Charles MacNeill 5:89031b2f5316 958 trace_print(
Charles MacNeill 5:89031b2f5316 959 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 960 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 961 "last bin int", last_bin_int);
Charles MacNeill 5:89031b2f5316 962
Charles MacNeill 5:89031b2f5316 963 trace_print(
Charles MacNeill 5:89031b2f5316 964 VL53LX_TRACE_LEVEL_DEBUG,
Charles MacNeill 5:89031b2f5316 965 " %-48s : %10d\n",
Charles MacNeill 5:89031b2f5316 966 "amb thresh", amb_threshold);
Charles MacNeill 5:89031b2f5316 967
Charles MacNeill 5:89031b2f5316 968
Charles MacNeill 5:89031b2f5316 969
Charles MacNeill 5:89031b2f5316 970
Charles MacNeill 5:89031b2f5316 971
Charles MacNeill 5:89031b2f5316 972 for (i = 0; i < pxtalk_data->VL53LX_p_021; i++) {
Charles MacNeill 5:89031b2f5316 973
Charles MacNeill 5:89031b2f5316 974 if (first_bin_int <= last_bin_int) {
Charles MacNeill 5:89031b2f5316 975 if ((i >= first_bin_int) && (i <= last_bin_int)) {
Charles MacNeill 5:89031b2f5316 976 if (pxtalk_data->bin_data[i] <
Charles MacNeill 5:89031b2f5316 977 (int32_t)amb_threshold)
Charles MacNeill 5:89031b2f5316 978 pxtalk_data->bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 979 } else {
Charles MacNeill 5:89031b2f5316 980 pxtalk_data->bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 981 }
Charles MacNeill 5:89031b2f5316 982 } else {
Charles MacNeill 5:89031b2f5316 983 if ((i >= first_bin_int) || (i <= last_bin_int)) {
Charles MacNeill 5:89031b2f5316 984 if (pxtalk_data->bin_data[i] <
Charles MacNeill 5:89031b2f5316 985 (int32_t)amb_threshold) {
Charles MacNeill 5:89031b2f5316 986 pxtalk_data->bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 987 }
Charles MacNeill 5:89031b2f5316 988 } else {
Charles MacNeill 5:89031b2f5316 989 pxtalk_data->bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 990 }
Charles MacNeill 5:89031b2f5316 991 }
Charles MacNeill 5:89031b2f5316 992 }
Charles MacNeill 5:89031b2f5316 993
Charles MacNeill 5:89031b2f5316 994
Charles MacNeill 5:89031b2f5316 995
Charles MacNeill 5:89031b2f5316 996
Charles MacNeill 5:89031b2f5316 997
Charles MacNeill 5:89031b2f5316 998 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 999
Charles MacNeill 5:89031b2f5316 1000 return status;
Charles MacNeill 5:89031b2f5316 1001 }
Charles MacNeill 5:89031b2f5316 1002
Charles MacNeill 5:89031b2f5316 1003
Charles MacNeill 5:89031b2f5316 1004 VL53LX_Error VL53LX_f_043(
Charles MacNeill 5:89031b2f5316 1005 uint8_t sigma_mult,
Charles MacNeill 5:89031b2f5316 1006 int32_t VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 1007 uint32_t *ambient_noise)
Charles MacNeill 5:89031b2f5316 1008 {
Charles MacNeill 5:89031b2f5316 1009
Charles MacNeill 5:89031b2f5316 1010
Charles MacNeill 5:89031b2f5316 1011
Charles MacNeill 5:89031b2f5316 1012 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 1013
Charles MacNeill 5:89031b2f5316 1014 uint32_t ambient_events_per_bin_int = 0;
Charles MacNeill 5:89031b2f5316 1015
Charles MacNeill 5:89031b2f5316 1016 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 1017
Charles MacNeill 5:89031b2f5316 1018 if (VL53LX_p_028 <= 0)
Charles MacNeill 5:89031b2f5316 1019 ambient_events_per_bin_int = 1;
Charles MacNeill 5:89031b2f5316 1020 else
Charles MacNeill 5:89031b2f5316 1021 ambient_events_per_bin_int = (uint32_t)VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 1022
Charles MacNeill 5:89031b2f5316 1023 *ambient_noise = VL53LX_isqrt(ambient_events_per_bin_int);
Charles MacNeill 5:89031b2f5316 1024
Charles MacNeill 5:89031b2f5316 1025 *ambient_noise = *ambient_noise * (uint32_t)sigma_mult;
Charles MacNeill 5:89031b2f5316 1026
Charles MacNeill 5:89031b2f5316 1027 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 1028
Charles MacNeill 5:89031b2f5316 1029 return status;
Charles MacNeill 5:89031b2f5316 1030 }
Charles MacNeill 5:89031b2f5316 1031
Charles MacNeill 5:89031b2f5316 1032
Charles MacNeill 5:89031b2f5316 1033
Charles MacNeill 5:89031b2f5316 1034
Charles MacNeill 5:89031b2f5316 1035
Charles MacNeill 5:89031b2f5316 1036
Charles MacNeill 5:89031b2f5316 1037
Charles MacNeill 5:89031b2f5316 1038
Charles MacNeill 5:89031b2f5316 1039
Charles MacNeill 5:89031b2f5316 1040
Charles MacNeill 5:89031b2f5316 1041
Charles MacNeill 5:89031b2f5316 1042
Charles MacNeill 5:89031b2f5316 1043
Charles MacNeill 5:89031b2f5316 1044
Charles MacNeill 5:89031b2f5316 1045
Charles MacNeill 5:89031b2f5316 1046
Charles MacNeill 5:89031b2f5316 1047
Charles MacNeill 5:89031b2f5316 1048
Charles MacNeill 5:89031b2f5316 1049
Charles MacNeill 5:89031b2f5316 1050