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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 5:89031b2f5316 1
Charles MacNeill 5:89031b2f5316 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 5:89031b2f5316 3 /******************************************************************************
Charles MacNeill 5:89031b2f5316 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 5:89031b2f5316 5
Charles MacNeill 5:89031b2f5316 6 This file is part of VL53LX Protected and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 5:89031b2f5316 9
Charles MacNeill 5:89031b2f5316 10 ******************************************************************************
Charles MacNeill 5:89031b2f5316 11
Charles MacNeill 5:89031b2f5316 12 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14 ******************************************************************************
Charles MacNeill 5:89031b2f5316 15
Charles MacNeill 5:89031b2f5316 16 License terms: STMicroelectronics Proprietary in accordance with licensing
Charles MacNeill 5:89031b2f5316 17 terms at www.st.com/sla0081
Charles MacNeill 5:89031b2f5316 18
Charles MacNeill 5:89031b2f5316 19 ******************************************************************************
Charles MacNeill 5:89031b2f5316 20 */
Charles MacNeill 5:89031b2f5316 21
Charles MacNeill 5:89031b2f5316 22
Charles MacNeill 5:89031b2f5316 23
Charles MacNeill 5:89031b2f5316 24
Charles MacNeill 5:89031b2f5316 25 #include <vl53lx_platform_log.h>
Charles MacNeill 5:89031b2f5316 26 #include <vl53lx_types.h>
Charles MacNeill 5:89031b2f5316 27 #include "vl53lx_core_support.h"
Charles MacNeill 5:89031b2f5316 28 #include "vl53lx_error_codes.h"
Charles MacNeill 5:89031b2f5316 29
Charles MacNeill 5:89031b2f5316 30 #include "vl53lx_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