ST Expansion SW Team / VL53L1CB

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

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

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

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
Charles MacNeill 7:1add29d51e72 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 7:1add29d51e72 3 /******************************************************************************
Charles MacNeill 7:1add29d51e72 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 5
Charles MacNeill 7:1add29d51e72 6 This file is part of VL53L1 Protected and is dual licensed,
Charles MacNeill 7:1add29d51e72 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 7:1add29d51e72 9
Charles MacNeill 7:1add29d51e72 10 ******************************************************************************
Charles MacNeill 7:1add29d51e72 11
Charles MacNeill 7:1add29d51e72 12 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 13
Charles MacNeill 7:1add29d51e72 14 ******************************************************************************
charlesmn 0:3ac96e360672 15
charlesmn 0:3ac96e360672 16 License terms: STMicroelectronics Proprietary in accordance with licensing
charlesmn 0:3ac96e360672 17 terms at www.st.com/sla0081
charlesmn 0:3ac96e360672 18
Charles MacNeill 7:1add29d51e72 19 ******************************************************************************
Charles MacNeill 7:1add29d51e72 20 */
charlesmn 0:3ac96e360672 21
charlesmn 0:3ac96e360672 22
charlesmn 0:3ac96e360672 23
charlesmn 0:3ac96e360672 24
charlesmn 0:3ac96e360672 25 #include "vl53l1_types.h"
charlesmn 0:3ac96e360672 26 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 27
charlesmn 0:3ac96e360672 28 #include "vl53l1_core_support.h"
charlesmn 0:3ac96e360672 29 #include "vl53l1_error_codes.h"
charlesmn 0:3ac96e360672 30
charlesmn 0:3ac96e360672 31 #include "vl53l1_xtalk.h"
charlesmn 0:3ac96e360672 32 #include "vl53l1_hist_core.h"
charlesmn 0:3ac96e360672 33
charlesmn 0:3ac96e360672 34
charlesmn 0:3ac96e360672 35
charlesmn 0:3ac96e360672 36
charlesmn 0:3ac96e360672 37 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 38 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 39 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 40 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 41 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 42 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 43 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 44
charlesmn 0:3ac96e360672 45 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 46 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 47 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 48
charlesmn 0:3ac96e360672 49
charlesmn 0:3ac96e360672 50 VL53L1_Error VL53L1_xtalk_calibration_process_data(
charlesmn 0:3ac96e360672 51 VL53L1_xtalk_range_results_t *pxtalk_results,
charlesmn 0:3ac96e360672 52 VL53L1_xtalk_histogram_data_t *pxtalk_shape,
charlesmn 0:3ac96e360672 53 VL53L1_xtalk_calibration_results_t *pxtalk_cal)
charlesmn 0:3ac96e360672 54 {
charlesmn 0:3ac96e360672 55
charlesmn 0:3ac96e360672 56
charlesmn 0:3ac96e360672 57 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 58
charlesmn 0:3ac96e360672 59 VL53L1_xtalk_algo_data_t xtalk_debug;
charlesmn 0:3ac96e360672 60 VL53L1_xtalk_algo_data_t *pdebug = &xtalk_debug;
charlesmn 0:3ac96e360672 61 VL53L1_xtalk_range_data_t *pxtalk_data = NULL;
charlesmn 0:3ac96e360672 62
charlesmn 0:3ac96e360672 63 VL53L1_histogram_bin_data_t avg_bins;
charlesmn 0:3ac96e360672 64 VL53L1_histogram_bin_data_t *pavg_bins = &avg_bins;
charlesmn 0:3ac96e360672 65
charlesmn 0:3ac96e360672 66 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 67
charlesmn 0:3ac96e360672 68
charlesmn 0:3ac96e360672 69
charlesmn 0:3ac96e360672 70 memcpy(pavg_bins, &(pxtalk_results->central_histogram_avg),
charlesmn 0:3ac96e360672 71 sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 72
charlesmn 0:3ac96e360672 73
charlesmn 0:3ac96e360672 74
charlesmn 0:3ac96e360672 75 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 76 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 77 0, 0, &(pdebug->VL53L1_p_057));
charlesmn 0:3ac96e360672 78
charlesmn 0:3ac96e360672 79 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 80 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 81 0, 0, &(pdebug->VL53L1_p_058));
charlesmn 0:3ac96e360672 82
charlesmn 0:3ac96e360672 83
charlesmn 0:3ac96e360672 84
charlesmn 0:3ac96e360672 85 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 86 status = VL53L1_f_047(
charlesmn 0:3ac96e360672 87 pxtalk_results,
charlesmn 0:3ac96e360672 88 pdebug,
charlesmn 0:3ac96e360672 89 &(pxtalk_cal->algo__crosstalk_compensation_x_plane_gradient_kcps
charlesmn 0:3ac96e360672 90 ),
charlesmn 0:3ac96e360672 91 &(pxtalk_cal->algo__crosstalk_compensation_y_plane_gradient_kcps
charlesmn 0:3ac96e360672 92 ));
charlesmn 0:3ac96e360672 93
charlesmn 0:3ac96e360672 94
charlesmn 0:3ac96e360672 95
charlesmn 0:3ac96e360672 96
charlesmn 0:3ac96e360672 97
charlesmn 0:3ac96e360672 98 if (status != VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 99 goto ENDFUNC;
charlesmn 0:3ac96e360672 100
charlesmn 0:3ac96e360672 101 pxtalk_data = &(pxtalk_results->VL53L1_p_002[4]);
charlesmn 0:3ac96e360672 102
charlesmn 0:3ac96e360672 103 if (pxtalk_data->no_of_samples > 0) {
charlesmn 0:3ac96e360672 104
charlesmn 0:3ac96e360672 105
charlesmn 0:3ac96e360672 106
charlesmn 0:3ac96e360672 107 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 108 memcpy(&(pdebug->VL53L1_p_057),
charlesmn 0:3ac96e360672 109 pavg_bins,
charlesmn 0:3ac96e360672 110 sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 111 }
charlesmn 0:3ac96e360672 112
charlesmn 0:3ac96e360672 113
charlesmn 0:3ac96e360672 114
charlesmn 0:3ac96e360672 115 status = VL53L1_f_048(
charlesmn 0:3ac96e360672 116 pxtalk_data,
charlesmn 0:3ac96e360672 117 pdebug,
charlesmn 0:3ac96e360672 118 &(pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps));
charlesmn 0:3ac96e360672 119
charlesmn 0:3ac96e360672 120
charlesmn 0:3ac96e360672 121
charlesmn 0:3ac96e360672 122 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 123 status = VL53L1_f_049(
charlesmn 0:3ac96e360672 124 pavg_bins,
charlesmn 0:3ac96e360672 125 pdebug,
charlesmn 0:3ac96e360672 126 pxtalk_data,
charlesmn 0:3ac96e360672 127 pxtalk_results->central_histogram__window_start,
charlesmn 0:3ac96e360672 128 pxtalk_results->central_histogram__window_end,
charlesmn 0:3ac96e360672 129 &(pxtalk_shape->xtalk_shape));
charlesmn 0:3ac96e360672 130
charlesmn 0:3ac96e360672 131 } else {
charlesmn 0:3ac96e360672 132
charlesmn 0:3ac96e360672 133
charlesmn 0:3ac96e360672 134
charlesmn 0:3ac96e360672 135 pxtalk_cal->algo__crosstalk_compensation_plane_offset_kcps = 0;
charlesmn 0:3ac96e360672 136
charlesmn 0:3ac96e360672 137
charlesmn 0:3ac96e360672 138
charlesmn 0:3ac96e360672 139 pdebug->VL53L1_p_059 = 0;
charlesmn 0:3ac96e360672 140
charlesmn 0:3ac96e360672 141
charlesmn 0:3ac96e360672 142 }
charlesmn 0:3ac96e360672 143
charlesmn 0:3ac96e360672 144
charlesmn 0:3ac96e360672 145 ENDFUNC:
charlesmn 0:3ac96e360672 146
charlesmn 0:3ac96e360672 147
charlesmn 0:3ac96e360672 148
charlesmn 0:3ac96e360672 149 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 150
charlesmn 0:3ac96e360672 151 return status;
charlesmn 0:3ac96e360672 152 }
charlesmn 0:3ac96e360672 153
charlesmn 0:3ac96e360672 154
charlesmn 0:3ac96e360672 155 VL53L1_Error VL53L1_generate_dual_reflectance_xtalk_samples(
charlesmn 0:3ac96e360672 156 VL53L1_xtalk_range_results_t *pxtalk_results,
charlesmn 0:3ac96e360672 157 uint16_t expected_target_distance_mm,
charlesmn 0:3ac96e360672 158 uint8_t higher_reflectance,
charlesmn 0:3ac96e360672 159 VL53L1_histogram_bin_data_t *pxtalk_avg_samples
charlesmn 0:3ac96e360672 160 )
charlesmn 0:3ac96e360672 161 {
charlesmn 0:3ac96e360672 162
charlesmn 0:3ac96e360672 163
charlesmn 0:3ac96e360672 164 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 165
charlesmn 0:3ac96e360672 166 VL53L1_histogram_bin_data_t *pzone_avg_1 =
charlesmn 0:3ac96e360672 167 &(pxtalk_results->histogram_avg_1[0]);
charlesmn 0:3ac96e360672 168 VL53L1_histogram_bin_data_t *pzone_avg_2 =
charlesmn 0:3ac96e360672 169 &(pxtalk_results->histogram_avg_2[0]);
charlesmn 0:3ac96e360672 170
charlesmn 0:3ac96e360672 171 VL53L1_histogram_bin_data_t *pxtalk_output = pxtalk_avg_samples;
charlesmn 0:3ac96e360672 172
charlesmn 0:3ac96e360672 173
charlesmn 0:3ac96e360672 174
charlesmn 0:3ac96e360672 175 int i = 0;
charlesmn 0:3ac96e360672 176
charlesmn 0:3ac96e360672 177
charlesmn 0:3ac96e360672 178
charlesmn 0:3ac96e360672 179 for (i = 0 ; i < 5 ; i++) {
charlesmn 0:3ac96e360672 180
charlesmn 0:3ac96e360672 181 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 182 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 183 0, 0, pzone_avg_1);
charlesmn 0:3ac96e360672 184
charlesmn 0:3ac96e360672 185 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 186 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 187 0, 0, pzone_avg_2);
charlesmn 0:3ac96e360672 188
charlesmn 0:3ac96e360672 189 pzone_avg_1++;
charlesmn 0:3ac96e360672 190 pzone_avg_2++;
charlesmn 0:3ac96e360672 191 }
charlesmn 0:3ac96e360672 192
charlesmn 0:3ac96e360672 193
charlesmn 0:3ac96e360672 194
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196 pzone_avg_1 = &(pxtalk_results->histogram_avg_1[0]);
charlesmn 0:3ac96e360672 197 pzone_avg_2 = &(pxtalk_results->histogram_avg_2[0]);
charlesmn 0:3ac96e360672 198
charlesmn 0:3ac96e360672 199 for (i = 0 ; i < 5 ; i++) {
charlesmn 0:3ac96e360672 200
charlesmn 0:3ac96e360672 201 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 202
charlesmn 0:3ac96e360672 203 status = VL53L1_f_050(
charlesmn 0:3ac96e360672 204 pzone_avg_1,
charlesmn 0:3ac96e360672 205 pzone_avg_2,
charlesmn 0:3ac96e360672 206 expected_target_distance_mm,
charlesmn 0:3ac96e360672 207 0x01,
charlesmn 0:3ac96e360672 208 higher_reflectance,
charlesmn 0:3ac96e360672 209 pxtalk_output
charlesmn 0:3ac96e360672 210 );
charlesmn 0:3ac96e360672 211
charlesmn 0:3ac96e360672 212
charlesmn 0:3ac96e360672 213
charlesmn 0:3ac96e360672 214 pzone_avg_1++;
charlesmn 0:3ac96e360672 215 pzone_avg_2++;
charlesmn 0:3ac96e360672 216 pxtalk_output++;
charlesmn 0:3ac96e360672 217
charlesmn 0:3ac96e360672 218 }
charlesmn 0:3ac96e360672 219 }
charlesmn 0:3ac96e360672 220
charlesmn 0:3ac96e360672 221 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 222
charlesmn 0:3ac96e360672 223 return status;
charlesmn 0:3ac96e360672 224 }
charlesmn 0:3ac96e360672 225
charlesmn 0:3ac96e360672 226
charlesmn 0:3ac96e360672 227 VL53L1_Error VL53L1_f_050(
charlesmn 0:3ac96e360672 228 VL53L1_histogram_bin_data_t *pzone_avg_1,
charlesmn 0:3ac96e360672 229 VL53L1_histogram_bin_data_t *pzone_avg_2,
charlesmn 0:3ac96e360672 230 uint16_t expected_target_distance,
charlesmn 0:3ac96e360672 231 uint8_t subtract_amb,
charlesmn 0:3ac96e360672 232 uint8_t higher_reflectance,
charlesmn 0:3ac96e360672 233 VL53L1_histogram_bin_data_t *pxtalk_output
charlesmn 0:3ac96e360672 234 )
charlesmn 0:3ac96e360672 235 {
charlesmn 0:3ac96e360672 236
charlesmn 0:3ac96e360672 237
charlesmn 0:3ac96e360672 238 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 239
charlesmn 0:3ac96e360672 240 VL53L1_histogram_bin_data_t zone_avg_realigned;
charlesmn 0:3ac96e360672 241
charlesmn 0:3ac96e360672 242
charlesmn 0:3ac96e360672 243 SUPPRESS_UNUSED_WARNING(pxtalk_output);
charlesmn 0:3ac96e360672 244 SUPPRESS_UNUSED_WARNING(expected_target_distance);
charlesmn 0:3ac96e360672 245
charlesmn 0:3ac96e360672 246
charlesmn 0:3ac96e360672 247
charlesmn 0:3ac96e360672 248 if ((status == VL53L1_ERROR_NONE) && (subtract_amb == 0x01)) {
charlesmn 0:3ac96e360672 249 VL53L1_f_037(
charlesmn 0:3ac96e360672 250 pzone_avg_1,
charlesmn 0:3ac96e360672 251 pzone_avg_1->VL53L1_p_004);
charlesmn 0:3ac96e360672 252
charlesmn 0:3ac96e360672 253
charlesmn 0:3ac96e360672 254 pzone_avg_1->VL53L1_p_004 = 0x0;
charlesmn 0:3ac96e360672 255 }
charlesmn 0:3ac96e360672 256
charlesmn 0:3ac96e360672 257 if ((status == VL53L1_ERROR_NONE) && (subtract_amb == 0x01)) {
charlesmn 0:3ac96e360672 258 VL53L1_f_037(
charlesmn 0:3ac96e360672 259 pzone_avg_2,
charlesmn 0:3ac96e360672 260 pzone_avg_2->VL53L1_p_004);
charlesmn 0:3ac96e360672 261
charlesmn 0:3ac96e360672 262
charlesmn 0:3ac96e360672 263 pzone_avg_2->VL53L1_p_004 = 0x0;
charlesmn 0:3ac96e360672 264 }
charlesmn 0:3ac96e360672 265
charlesmn 0:3ac96e360672 266
charlesmn 0:3ac96e360672 267
charlesmn 0:3ac96e360672 268
charlesmn 0:3ac96e360672 269
charlesmn 0:3ac96e360672 270 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 271 if (higher_reflectance == 0x01) {
charlesmn 0:3ac96e360672 272 VL53L1_f_004(
charlesmn 0:3ac96e360672 273 pzone_avg_2,
charlesmn 0:3ac96e360672 274 pzone_avg_1,
charlesmn 0:3ac96e360672 275 &zone_avg_realigned);
charlesmn 0:3ac96e360672 276 } else {
charlesmn 0:3ac96e360672 277
charlesmn 0:3ac96e360672 278
charlesmn 0:3ac96e360672 279
charlesmn 0:3ac96e360672 280 VL53L1_f_004(
charlesmn 0:3ac96e360672 281 pzone_avg_1,
charlesmn 0:3ac96e360672 282 pzone_avg_2,
charlesmn 0:3ac96e360672 283 &zone_avg_realigned);
charlesmn 0:3ac96e360672 284
charlesmn 0:3ac96e360672 285
charlesmn 0:3ac96e360672 286
charlesmn 0:3ac96e360672 287 }
charlesmn 0:3ac96e360672 288 }
charlesmn 0:3ac96e360672 289
charlesmn 0:3ac96e360672 290
charlesmn 0:3ac96e360672 291
charlesmn 0:3ac96e360672 292
charlesmn 0:3ac96e360672 293
charlesmn 0:3ac96e360672 294
charlesmn 0:3ac96e360672 295
charlesmn 0:3ac96e360672 296
charlesmn 0:3ac96e360672 297 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 298
charlesmn 0:3ac96e360672 299 return status;
charlesmn 0:3ac96e360672 300 }
charlesmn 0:3ac96e360672 301
charlesmn 0:3ac96e360672 302 VL53L1_Error VL53L1_f_049(
charlesmn 0:3ac96e360672 303 VL53L1_histogram_bin_data_t *pavg_bins,
charlesmn 0:3ac96e360672 304 VL53L1_xtalk_algo_data_t *pdebug,
charlesmn 0:3ac96e360672 305 VL53L1_xtalk_range_data_t *pxtalk_data,
charlesmn 0:3ac96e360672 306 uint8_t histogram__window_start,
charlesmn 0:3ac96e360672 307 uint8_t histogram__window_end,
charlesmn 0:3ac96e360672 308 VL53L1_xtalk_histogram_shape_t *pxtalk_shape)
charlesmn 0:3ac96e360672 309 {
charlesmn 0:3ac96e360672 310
charlesmn 0:3ac96e360672 311 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 312
charlesmn 0:3ac96e360672 313
charlesmn 0:3ac96e360672 314
charlesmn 0:3ac96e360672 315 uint32_t ambient_thresh = 0;
charlesmn 0:3ac96e360672 316
charlesmn 0:3ac96e360672 317
charlesmn 0:3ac96e360672 318
charlesmn 0:3ac96e360672 319 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 320 VL53L1_f_037(
charlesmn 0:3ac96e360672 321 pavg_bins,
charlesmn 0:3ac96e360672 322 pavg_bins->VL53L1_p_004);
charlesmn 0:3ac96e360672 323
charlesmn 0:3ac96e360672 324
charlesmn 0:3ac96e360672 325
charlesmn 0:3ac96e360672 326 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 327 VL53L1_f_051(
charlesmn 0:3ac96e360672 328 6,
charlesmn 0:3ac96e360672 329 pavg_bins->VL53L1_p_004,
charlesmn 0:3ac96e360672 330 &ambient_thresh);
charlesmn 0:3ac96e360672 331
charlesmn 0:3ac96e360672 332
charlesmn 0:3ac96e360672 333
charlesmn 0:3ac96e360672 334 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 335 status = VL53L1_f_052(
charlesmn 0:3ac96e360672 336 pavg_bins,
charlesmn 0:3ac96e360672 337 ambient_thresh,
charlesmn 0:3ac96e360672 338 histogram__window_start,
charlesmn 0:3ac96e360672 339 histogram__window_end);
charlesmn 0:3ac96e360672 340
charlesmn 0:3ac96e360672 341
charlesmn 0:3ac96e360672 342
charlesmn 0:3ac96e360672 343 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 344 status = VL53L1_f_053(
charlesmn 0:3ac96e360672 345 pavg_bins,
charlesmn 0:3ac96e360672 346 pxtalk_data,
charlesmn 0:3ac96e360672 347 pdebug,
charlesmn 0:3ac96e360672 348 pxtalk_shape);
charlesmn 0:3ac96e360672 349
charlesmn 0:3ac96e360672 350
charlesmn 0:3ac96e360672 351 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 352
charlesmn 0:3ac96e360672 353 return status;
charlesmn 0:3ac96e360672 354
charlesmn 0:3ac96e360672 355 }
charlesmn 0:3ac96e360672 356
charlesmn 0:3ac96e360672 357
charlesmn 0:3ac96e360672 358 VL53L1_Error VL53L1_f_047(
charlesmn 0:3ac96e360672 359 VL53L1_xtalk_range_results_t *pxtalk_results,
charlesmn 0:3ac96e360672 360 VL53L1_xtalk_algo_data_t *pdebug,
charlesmn 0:3ac96e360672 361 int16_t *xgradient,
charlesmn 0:3ac96e360672 362 int16_t *ygradient
charlesmn 0:3ac96e360672 363 )
charlesmn 0:3ac96e360672 364 {
charlesmn 0:3ac96e360672 365
charlesmn 0:3ac96e360672 366
charlesmn 0:3ac96e360672 367
charlesmn 0:3ac96e360672 368 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 369
charlesmn 0:3ac96e360672 370 VL53L1_xtalk_range_data_t *presults_int = NULL;
charlesmn 0:3ac96e360672 371
charlesmn 0:3ac96e360672 372 int i = 0;
charlesmn 0:3ac96e360672 373
charlesmn 0:3ac96e360672 374 uint32_t xtalk_per_spad[4];
charlesmn 0:3ac96e360672 375 int32_t VL53L1_p_060 = 0;
charlesmn 0:3ac96e360672 376 int32_t VL53L1_p_061 = 0;
charlesmn 0:3ac96e360672 377
charlesmn 0:3ac96e360672 378 uint8_t result_invalid = 0;
charlesmn 0:3ac96e360672 379
charlesmn 0:3ac96e360672 380
charlesmn 0:3ac96e360672 381 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 382
charlesmn 0:3ac96e360672 383
charlesmn 0:3ac96e360672 384
charlesmn 0:3ac96e360672 385 *xgradient = 0;
charlesmn 0:3ac96e360672 386 *ygradient = 0;
charlesmn 0:3ac96e360672 387
charlesmn 0:3ac96e360672 388
charlesmn 0:3ac96e360672 389
charlesmn 0:3ac96e360672 390
charlesmn 0:3ac96e360672 391 for (i = 0; i < 4; i++)
charlesmn 0:3ac96e360672 392 xtalk_per_spad[i] = 0;
charlesmn 0:3ac96e360672 393
charlesmn 0:3ac96e360672 394
charlesmn 0:3ac96e360672 395
charlesmn 0:3ac96e360672 396 for (i = 0; i < 4; i++) {
charlesmn 0:3ac96e360672 397
charlesmn 0:3ac96e360672 398 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 399
charlesmn 0:3ac96e360672 400 presults_int = &(pxtalk_results->VL53L1_p_002[i]);
charlesmn 0:3ac96e360672 401
charlesmn 0:3ac96e360672 402
charlesmn 0:3ac96e360672 403
charlesmn 0:3ac96e360672 404
charlesmn 0:3ac96e360672 405 if (presults_int->no_of_samples == 0) {
charlesmn 0:3ac96e360672 406
charlesmn 0:3ac96e360672 407
charlesmn 0:3ac96e360672 408 result_invalid = 1;
charlesmn 0:3ac96e360672 409 pdebug->VL53L1_p_062[i] = 0;
charlesmn 0:3ac96e360672 410
charlesmn 0:3ac96e360672 411
charlesmn 0:3ac96e360672 412 } else {
charlesmn 0:3ac96e360672 413
charlesmn 0:3ac96e360672 414 xtalk_per_spad[i] =
charlesmn 0:3ac96e360672 415 presults_int->rate_per_spad_kcps_avg;
charlesmn 0:3ac96e360672 416
charlesmn 0:3ac96e360672 417
charlesmn 0:3ac96e360672 418
charlesmn 0:3ac96e360672 419 pdebug->VL53L1_p_062[i] =
charlesmn 0:3ac96e360672 420 (uint32_t)xtalk_per_spad[i];
charlesmn 0:3ac96e360672 421
charlesmn 0:3ac96e360672 422 }
charlesmn 0:3ac96e360672 423 }
charlesmn 0:3ac96e360672 424
charlesmn 0:3ac96e360672 425 }
charlesmn 0:3ac96e360672 426
charlesmn 0:3ac96e360672 427
charlesmn 0:3ac96e360672 428
charlesmn 0:3ac96e360672 429 if ((status == VL53L1_ERROR_NONE) && (result_invalid == 0)) {
charlesmn 0:3ac96e360672 430
charlesmn 0:3ac96e360672 431
charlesmn 0:3ac96e360672 432
charlesmn 0:3ac96e360672 433 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 434 VL53L1_p_060 = ((int32_t)xtalk_per_spad[1]
charlesmn 0:3ac96e360672 435 - (int32_t)xtalk_per_spad[0]) / (8);
charlesmn 0:3ac96e360672 436 VL53L1_p_061 = ((int32_t)xtalk_per_spad[3]
charlesmn 0:3ac96e360672 437 - (int32_t)xtalk_per_spad[2]) / (8);
charlesmn 0:3ac96e360672 438 }
charlesmn 0:3ac96e360672 439
charlesmn 0:3ac96e360672 440
charlesmn 0:3ac96e360672 441
charlesmn 0:3ac96e360672 442
charlesmn 0:3ac96e360672 443 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 444 if (VL53L1_p_060 < -32767) {
charlesmn 0:3ac96e360672 445 VL53L1_p_060 = -32767;
charlesmn 0:3ac96e360672 446 } else {
charlesmn 0:3ac96e360672 447 if (VL53L1_p_060 > 32767)
charlesmn 0:3ac96e360672 448 VL53L1_p_060 = 32767;
charlesmn 0:3ac96e360672 449 }
charlesmn 0:3ac96e360672 450
charlesmn 0:3ac96e360672 451 if (VL53L1_p_061 < -32767) {
charlesmn 0:3ac96e360672 452 VL53L1_p_061 = -32767;
charlesmn 0:3ac96e360672 453 } else {
charlesmn 0:3ac96e360672 454 if (VL53L1_p_061 > 32767)
charlesmn 0:3ac96e360672 455 VL53L1_p_061 = 32767;
charlesmn 0:3ac96e360672 456 }
charlesmn 0:3ac96e360672 457
charlesmn 0:3ac96e360672 458
charlesmn 0:3ac96e360672 459
charlesmn 0:3ac96e360672 460 pdebug->VL53L1_p_060 = (int16_t)VL53L1_p_060;
charlesmn 0:3ac96e360672 461 pdebug->VL53L1_p_061 = (int16_t)VL53L1_p_061;
charlesmn 0:3ac96e360672 462 }
charlesmn 0:3ac96e360672 463
charlesmn 0:3ac96e360672 464 } else {
charlesmn 0:3ac96e360672 465
charlesmn 0:3ac96e360672 466
charlesmn 0:3ac96e360672 467
charlesmn 0:3ac96e360672 468 VL53L1_p_060 = 0;
charlesmn 0:3ac96e360672 469 VL53L1_p_061 = 0;
charlesmn 0:3ac96e360672 470
charlesmn 0:3ac96e360672 471 pdebug->VL53L1_p_060 = 0;
charlesmn 0:3ac96e360672 472 pdebug->VL53L1_p_061 = 0;
charlesmn 0:3ac96e360672 473 }
charlesmn 0:3ac96e360672 474
charlesmn 0:3ac96e360672 475
charlesmn 0:3ac96e360672 476
charlesmn 0:3ac96e360672 477 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 478 *xgradient = (int16_t)VL53L1_p_060;
charlesmn 0:3ac96e360672 479 *ygradient = (int16_t)VL53L1_p_061;
charlesmn 0:3ac96e360672 480 }
charlesmn 0:3ac96e360672 481
charlesmn 0:3ac96e360672 482 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 483
charlesmn 0:3ac96e360672 484 return status;
charlesmn 0:3ac96e360672 485 }
charlesmn 0:3ac96e360672 486
charlesmn 0:3ac96e360672 487
charlesmn 0:3ac96e360672 488 VL53L1_Error VL53L1_f_048(
charlesmn 0:3ac96e360672 489 VL53L1_xtalk_range_data_t *pxtalk_data,
charlesmn 0:3ac96e360672 490 VL53L1_xtalk_algo_data_t *pdebug,
charlesmn 0:3ac96e360672 491 uint32_t *xtalk_mean_offset_kcps
charlesmn 0:3ac96e360672 492 )
charlesmn 0:3ac96e360672 493 {
charlesmn 0:3ac96e360672 494
charlesmn 0:3ac96e360672 495
charlesmn 0:3ac96e360672 496 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 497
charlesmn 0:3ac96e360672 498 uint32_t xtalk_per_spad = 0;
charlesmn 0:3ac96e360672 499 uint8_t result_invalid = 0;
charlesmn 0:3ac96e360672 500
charlesmn 0:3ac96e360672 501 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 502
charlesmn 0:3ac96e360672 503 *xtalk_mean_offset_kcps = 0;
charlesmn 0:3ac96e360672 504
charlesmn 0:3ac96e360672 505
charlesmn 0:3ac96e360672 506 if (pxtalk_data->no_of_samples == 0) {
charlesmn 0:3ac96e360672 507
charlesmn 0:3ac96e360672 508
charlesmn 0:3ac96e360672 509
charlesmn 0:3ac96e360672 510 result_invalid = 1;
charlesmn 0:3ac96e360672 511
charlesmn 0:3ac96e360672 512
charlesmn 0:3ac96e360672 513
charlesmn 0:3ac96e360672 514 pdebug->VL53L1_p_059 = 0;
charlesmn 0:3ac96e360672 515
charlesmn 0:3ac96e360672 516 }
charlesmn 0:3ac96e360672 517
charlesmn 0:3ac96e360672 518
charlesmn 0:3ac96e360672 519
charlesmn 0:3ac96e360672 520
charlesmn 0:3ac96e360672 521 if ((status == VL53L1_ERROR_NONE) && (result_invalid == 0)) {
charlesmn 0:3ac96e360672 522
charlesmn 0:3ac96e360672 523
charlesmn 0:3ac96e360672 524
charlesmn 0:3ac96e360672 525 xtalk_per_spad = pxtalk_data->rate_per_spad_kcps_avg >> 2;
charlesmn 0:3ac96e360672 526
charlesmn 0:3ac96e360672 527
charlesmn 0:3ac96e360672 528
charlesmn 0:3ac96e360672 529 pdebug->VL53L1_p_059 = xtalk_per_spad;
charlesmn 0:3ac96e360672 530
charlesmn 0:3ac96e360672 531
charlesmn 0:3ac96e360672 532 if (xtalk_per_spad < 0x3FFFF)
charlesmn 0:3ac96e360672 533 *xtalk_mean_offset_kcps = (uint32_t)xtalk_per_spad;
charlesmn 0:3ac96e360672 534 else
charlesmn 0:3ac96e360672 535 *xtalk_mean_offset_kcps = 0x3FFFF;
charlesmn 0:3ac96e360672 536
charlesmn 0:3ac96e360672 537 } else {
charlesmn 0:3ac96e360672 538
charlesmn 0:3ac96e360672 539
charlesmn 0:3ac96e360672 540
charlesmn 0:3ac96e360672 541 *xtalk_mean_offset_kcps = 0;
charlesmn 0:3ac96e360672 542 }
charlesmn 0:3ac96e360672 543
charlesmn 0:3ac96e360672 544 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 545
charlesmn 0:3ac96e360672 546 return status;
charlesmn 0:3ac96e360672 547
charlesmn 0:3ac96e360672 548 }
charlesmn 0:3ac96e360672 549
charlesmn 0:3ac96e360672 550
charlesmn 0:3ac96e360672 551
charlesmn 0:3ac96e360672 552
charlesmn 0:3ac96e360672 553 VL53L1_Error VL53L1_f_053(
charlesmn 0:3ac96e360672 554 VL53L1_histogram_bin_data_t *phist_data,
charlesmn 0:3ac96e360672 555 VL53L1_xtalk_range_data_t *pxtalk_data,
charlesmn 0:3ac96e360672 556 VL53L1_xtalk_algo_data_t *pdebug,
charlesmn 0:3ac96e360672 557 VL53L1_xtalk_histogram_shape_t *pxtalk_histo
charlesmn 0:3ac96e360672 558 )
charlesmn 0:3ac96e360672 559 {
charlesmn 0:3ac96e360672 560
charlesmn 0:3ac96e360672 561
charlesmn 0:3ac96e360672 562 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 563 uint8_t idx;
charlesmn 0:3ac96e360672 564 int32_t tmpi32;
charlesmn 0:3ac96e360672 565 uint8_t i = 0;
charlesmn 0:3ac96e360672 566 uint64_t bin_data[VL53L1_XTALK_HISTO_BINS];
charlesmn 0:3ac96e360672 567
charlesmn 0:3ac96e360672 568 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 569
charlesmn 0:3ac96e360672 570
charlesmn 0:3ac96e360672 571
charlesmn 0:3ac96e360672 572
charlesmn 0:3ac96e360672 573
charlesmn 0:3ac96e360672 574
charlesmn 0:3ac96e360672 575 pxtalk_histo->VL53L1_p_023 =
charlesmn 0:3ac96e360672 576 phist_data->VL53L1_p_023;
charlesmn 0:3ac96e360672 577 pxtalk_histo->cal_config__vcsel_start =
charlesmn 0:3ac96e360672 578 phist_data->cal_config__vcsel_start;
charlesmn 0:3ac96e360672 579 pxtalk_histo->VL53L1_p_019 =
charlesmn 0:3ac96e360672 580 phist_data->VL53L1_p_019;
charlesmn 0:3ac96e360672 581 pxtalk_histo->VL53L1_p_022 =
charlesmn 0:3ac96e360672 582 phist_data->VL53L1_p_022;
charlesmn 0:3ac96e360672 583 pxtalk_histo->time_stamp =
charlesmn 0:3ac96e360672 584 phist_data->time_stamp;
charlesmn 0:3ac96e360672 585 pxtalk_histo->vcsel_width =
charlesmn 0:3ac96e360672 586 phist_data->vcsel_width;
charlesmn 0:3ac96e360672 587 pxtalk_histo->zero_distance_phase =
charlesmn 0:3ac96e360672 588 phist_data->zero_distance_phase;
charlesmn 0:3ac96e360672 589 pxtalk_histo->zone_id =
charlesmn 0:3ac96e360672 590 phist_data->zone_id;
charlesmn 0:3ac96e360672 591 pxtalk_histo->VL53L1_p_024 =
charlesmn 0:3ac96e360672 592 VL53L1_XTALK_HISTO_BINS;
charlesmn 0:3ac96e360672 593 pxtalk_histo->phasecal_result__reference_phase =
charlesmn 0:3ac96e360672 594 phist_data->phasecal_result__reference_phase;
charlesmn 0:3ac96e360672 595 pxtalk_histo->phasecal_result__vcsel_start =
charlesmn 0:3ac96e360672 596 phist_data->phasecal_result__vcsel_start;
charlesmn 0:3ac96e360672 597
charlesmn 0:3ac96e360672 598 memcpy(&(pdebug->VL53L1_p_058),
charlesmn 0:3ac96e360672 599 phist_data, sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 600
charlesmn 0:3ac96e360672 601
charlesmn 0:3ac96e360672 602
charlesmn 0:3ac96e360672 603
charlesmn 0:3ac96e360672 604
charlesmn 0:3ac96e360672 605 if (pxtalk_data->signal_total_events_avg == 0) {
charlesmn 0:3ac96e360672 606 for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++)
charlesmn 0:3ac96e360672 607 bin_data[i] = 0;
charlesmn 0:3ac96e360672 608 goto FAIL;
charlesmn 0:3ac96e360672 609 }
charlesmn 0:3ac96e360672 610
charlesmn 0:3ac96e360672 611 for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++) {
charlesmn 0:3ac96e360672 612 idx = i + phist_data->number_of_ambient_bins;
charlesmn 0:3ac96e360672 613 if (phist_data->bin_data[idx] > 0) {
charlesmn 0:3ac96e360672 614 bin_data[i] =
charlesmn 0:3ac96e360672 615 ((uint64_t)phist_data->bin_data[idx] << 10);
charlesmn 0:3ac96e360672 616 tmpi32 = pxtalk_data->signal_total_events_avg / 2;
charlesmn 0:3ac96e360672 617 bin_data[i] = bin_data[i] + (uint64_t)tmpi32;
charlesmn 0:3ac96e360672 618 bin_data[i] = do_division_u(bin_data[i],
charlesmn 0:3ac96e360672 619 (uint64_t)pxtalk_data->signal_total_events_avg);
charlesmn 0:3ac96e360672 620 } else {
charlesmn 0:3ac96e360672 621 bin_data[i] = 0;
charlesmn 0:3ac96e360672 622 }
charlesmn 0:3ac96e360672 623 }
charlesmn 0:3ac96e360672 624
charlesmn 0:3ac96e360672 625 FAIL:
charlesmn 0:3ac96e360672 626
charlesmn 0:3ac96e360672 627
charlesmn 0:3ac96e360672 628 for (i = 0; i < VL53L1_XTALK_HISTO_BINS; i++)
charlesmn 0:3ac96e360672 629 pxtalk_histo->bin_data[i] = (uint32_t)bin_data[i];
charlesmn 0:3ac96e360672 630
charlesmn 0:3ac96e360672 631
charlesmn 0:3ac96e360672 632
charlesmn 0:3ac96e360672 633 for (i = 0; i < pxtalk_histo->VL53L1_p_024; i++)
charlesmn 0:3ac96e360672 634 pdebug->VL53L1_p_063[i] = pxtalk_histo->bin_data[i];
charlesmn 0:3ac96e360672 635
charlesmn 0:3ac96e360672 636
charlesmn 0:3ac96e360672 637 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 638
charlesmn 0:3ac96e360672 639 return status;
charlesmn 0:3ac96e360672 640
charlesmn 0:3ac96e360672 641 }
charlesmn 0:3ac96e360672 642
charlesmn 0:3ac96e360672 643
charlesmn 0:3ac96e360672 644 VL53L1_Error VL53L1_f_054(
charlesmn 0:3ac96e360672 645 VL53L1_customer_nvm_managed_t *pcustomer,
charlesmn 0:3ac96e360672 646 VL53L1_dynamic_config_t *pdyn_cfg,
charlesmn 0:3ac96e360672 647 VL53L1_xtalk_histogram_data_t *pxtalk_shape,
charlesmn 0:3ac96e360672 648 VL53L1_histogram_bin_data_t *pip_hist_data,
charlesmn 0:3ac96e360672 649 VL53L1_histogram_bin_data_t *pop_hist_data,
charlesmn 0:3ac96e360672 650 VL53L1_histogram_bin_data_t *pxtalk_count_data)
charlesmn 0:3ac96e360672 651 {
charlesmn 0:3ac96e360672 652
charlesmn 0:3ac96e360672 653
charlesmn 0:3ac96e360672 654
charlesmn 0:3ac96e360672 655 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 656
charlesmn 0:3ac96e360672 657
charlesmn 0:3ac96e360672 658
charlesmn 0:3ac96e360672 659 uint32_t xtalk_rate_kcps = 0;
charlesmn 0:3ac96e360672 660
charlesmn 0:3ac96e360672 661 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 662
charlesmn 0:3ac96e360672 663
charlesmn 0:3ac96e360672 664
charlesmn 0:3ac96e360672 665 memcpy(pop_hist_data, pip_hist_data,
charlesmn 0:3ac96e360672 666 sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 667
charlesmn 0:3ac96e360672 668
charlesmn 0:3ac96e360672 669
charlesmn 0:3ac96e360672 670 status =
charlesmn 0:3ac96e360672 671 VL53L1_f_040(
charlesmn 0:3ac96e360672 672 pcustomer->algo__crosstalk_compensation_plane_offset_kcps,
charlesmn 0:3ac96e360672 673 pcustomer->algo__crosstalk_compensation_x_plane_gradient_kcps,
charlesmn 0:3ac96e360672 674 pcustomer->algo__crosstalk_compensation_y_plane_gradient_kcps,
charlesmn 0:3ac96e360672 675 0,
charlesmn 0:3ac96e360672 676 0,
charlesmn 0:3ac96e360672 677 pip_hist_data->result__dss_actual_effective_spads,
charlesmn 0:3ac96e360672 678
charlesmn 0:3ac96e360672 679 pdyn_cfg->roi_config__user_roi_centre_spad,
charlesmn 0:3ac96e360672 680 pdyn_cfg->roi_config__user_roi_requested_global_xy_size,
charlesmn 0:3ac96e360672 681 &(xtalk_rate_kcps));
charlesmn 0:3ac96e360672 682
charlesmn 0:3ac96e360672 683
charlesmn 0:3ac96e360672 684
charlesmn 0:3ac96e360672 685 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 686 status =
charlesmn 0:3ac96e360672 687 VL53L1_f_041(
charlesmn 0:3ac96e360672 688 pip_hist_data,
charlesmn 0:3ac96e360672 689 &(pxtalk_shape->xtalk_shape),
charlesmn 0:3ac96e360672 690 xtalk_rate_kcps,
charlesmn 0:3ac96e360672 691 pxtalk_count_data);
charlesmn 0:3ac96e360672 692
charlesmn 0:3ac96e360672 693
charlesmn 0:3ac96e360672 694
charlesmn 0:3ac96e360672 695 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 696 status =
charlesmn 0:3ac96e360672 697 VL53L1_f_055(
charlesmn 0:3ac96e360672 698 pop_hist_data,
charlesmn 0:3ac96e360672 699 pxtalk_count_data,
charlesmn 0:3ac96e360672 700 pip_hist_data->number_of_ambient_bins);
charlesmn 0:3ac96e360672 701
charlesmn 0:3ac96e360672 702 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 703
charlesmn 0:3ac96e360672 704 return status;
charlesmn 0:3ac96e360672 705 }
charlesmn 0:3ac96e360672 706
charlesmn 0:3ac96e360672 707
charlesmn 0:3ac96e360672 708 VL53L1_Error VL53L1_f_040(
charlesmn 0:3ac96e360672 709 uint32_t mean_offset,
charlesmn 0:3ac96e360672 710 int16_t xgradient,
charlesmn 0:3ac96e360672 711 int16_t ygradient,
charlesmn 0:3ac96e360672 712 int8_t centre_offset_x,
charlesmn 0:3ac96e360672 713 int8_t centre_offset_y,
charlesmn 0:3ac96e360672 714 uint16_t roi_effective_spads,
charlesmn 0:3ac96e360672 715 uint8_t roi_centre_spad,
charlesmn 0:3ac96e360672 716 uint8_t roi_xy_size,
charlesmn 0:3ac96e360672 717 uint32_t *xtalk_rate_kcps
charlesmn 0:3ac96e360672 718 )
charlesmn 0:3ac96e360672 719 {
charlesmn 0:3ac96e360672 720
charlesmn 0:3ac96e360672 721
charlesmn 0:3ac96e360672 722
charlesmn 0:3ac96e360672 723 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 724
charlesmn 0:3ac96e360672 725 uint8_t row = 0;
charlesmn 0:3ac96e360672 726 uint8_t col = 0;
charlesmn 0:3ac96e360672 727
charlesmn 0:3ac96e360672 728
charlesmn 0:3ac96e360672 729
charlesmn 0:3ac96e360672 730 int16_t bound_l_x = 0;
charlesmn 0:3ac96e360672 731 int16_t bound_r_x = 0;
charlesmn 0:3ac96e360672 732 int16_t bound_u_y = 0;
charlesmn 0:3ac96e360672 733 int16_t bound_d_y = 0;
charlesmn 0:3ac96e360672 734
charlesmn 0:3ac96e360672 735 int64_t xtalk_rate_ll = 0;
charlesmn 0:3ac96e360672 736 int64_t xtalk_rate_ur = 0;
charlesmn 0:3ac96e360672 737
charlesmn 0:3ac96e360672 738 int64_t xtalk_avg = 0;
charlesmn 0:3ac96e360672 739
charlesmn 0:3ac96e360672 740 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 741
charlesmn 0:3ac96e360672 742 SUPPRESS_UNUSED_WARNING(roi_effective_spads);
charlesmn 0:3ac96e360672 743
charlesmn 0:3ac96e360672 744
charlesmn 0:3ac96e360672 745
charlesmn 0:3ac96e360672 746
charlesmn 0:3ac96e360672 747
charlesmn 0:3ac96e360672 748
charlesmn 0:3ac96e360672 749 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 750 VL53L1_decode_row_col(
charlesmn 0:3ac96e360672 751 roi_centre_spad,
charlesmn 0:3ac96e360672 752 &row,
charlesmn 0:3ac96e360672 753 &col);
charlesmn 0:3ac96e360672 754 }
charlesmn 0:3ac96e360672 755
charlesmn 0:3ac96e360672 756 trace_print(
charlesmn 0:3ac96e360672 757 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 758 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 759 "Row", row);
charlesmn 0:3ac96e360672 760
charlesmn 0:3ac96e360672 761 trace_print(
charlesmn 0:3ac96e360672 762 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 763 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 764 "Col", col);
charlesmn 0:3ac96e360672 765
charlesmn 0:3ac96e360672 766
charlesmn 0:3ac96e360672 767
charlesmn 0:3ac96e360672 768 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 769 if ((((int16_t)roi_xy_size / 16) & 0x01) == 1)
charlesmn 0:3ac96e360672 770 bound_l_x = (int16_t) col -
charlesmn 0:3ac96e360672 771 (((int16_t)roi_xy_size / 32) + 1);
charlesmn 0:3ac96e360672 772 else
charlesmn 0:3ac96e360672 773 bound_l_x = (int16_t) col -
charlesmn 0:3ac96e360672 774 ((int16_t)roi_xy_size / 32);
charlesmn 0:3ac96e360672 775
charlesmn 0:3ac96e360672 776 bound_r_x = (int16_t) col + ((int16_t)roi_xy_size / 32);
charlesmn 0:3ac96e360672 777
charlesmn 0:3ac96e360672 778 if ((((int16_t)roi_xy_size) & 0x01) == 1)
charlesmn 0:3ac96e360672 779 bound_d_y = (int16_t) row -
charlesmn 0:3ac96e360672 780 ((((int16_t)roi_xy_size & 0x0f) / 2) + 1);
charlesmn 0:3ac96e360672 781 else
charlesmn 0:3ac96e360672 782 bound_d_y = (int16_t) row -
charlesmn 0:3ac96e360672 783 (((int16_t)roi_xy_size & 0x0f) / 2);
charlesmn 0:3ac96e360672 784
charlesmn 0:3ac96e360672 785 bound_u_y = (int16_t) row +
charlesmn 0:3ac96e360672 786 (((int16_t)roi_xy_size & 0xf) / 2);
charlesmn 0:3ac96e360672 787 }
charlesmn 0:3ac96e360672 788
charlesmn 0:3ac96e360672 789 trace_print(
charlesmn 0:3ac96e360672 790 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 791 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 792 "Bound_l_x", bound_l_x);
charlesmn 0:3ac96e360672 793 trace_print(
charlesmn 0:3ac96e360672 794 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 795 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 796 "Bound_r_x", bound_r_x);
charlesmn 0:3ac96e360672 797 trace_print(
charlesmn 0:3ac96e360672 798 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 799 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 800 "Bound_u_y", bound_u_y);
charlesmn 0:3ac96e360672 801 trace_print(
charlesmn 0:3ac96e360672 802 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 803 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 804 "Bound_d_y", bound_d_y);
charlesmn 0:3ac96e360672 805
charlesmn 0:3ac96e360672 806
charlesmn 0:3ac96e360672 807
charlesmn 0:3ac96e360672 808
charlesmn 0:3ac96e360672 809 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 810 bound_l_x = (2 * bound_l_x) - 15 +
charlesmn 0:3ac96e360672 811 (2 * (int16_t)centre_offset_x);
charlesmn 0:3ac96e360672 812 bound_r_x = (2 * bound_r_x) - 15 +
charlesmn 0:3ac96e360672 813 (2 * (int16_t)centre_offset_x);
charlesmn 0:3ac96e360672 814 bound_u_y = (2 * bound_u_y) - 15 +
charlesmn 0:3ac96e360672 815 (2 * (int16_t)centre_offset_y);
charlesmn 0:3ac96e360672 816 bound_d_y = (2 * bound_d_y) - 15 +
charlesmn 0:3ac96e360672 817 (2 * (int16_t)centre_offset_y);
charlesmn 0:3ac96e360672 818 }
charlesmn 0:3ac96e360672 819
charlesmn 0:3ac96e360672 820 trace_print(
charlesmn 0:3ac96e360672 821 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 822 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 823 "Bound_l_x", bound_l_x);
charlesmn 0:3ac96e360672 824
charlesmn 0:3ac96e360672 825 trace_print(
charlesmn 0:3ac96e360672 826 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 827 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 828 "Bound_r_x", bound_r_x);
charlesmn 0:3ac96e360672 829
charlesmn 0:3ac96e360672 830 trace_print(
charlesmn 0:3ac96e360672 831 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 832 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 833 "Bound_u_y", bound_u_y);
charlesmn 0:3ac96e360672 834
charlesmn 0:3ac96e360672 835 trace_print(
charlesmn 0:3ac96e360672 836 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 837 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 838 "Bound_d_y", bound_d_y);
charlesmn 0:3ac96e360672 839
charlesmn 0:3ac96e360672 840
charlesmn 0:3ac96e360672 841
charlesmn 0:3ac96e360672 842
charlesmn 0:3ac96e360672 843 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 844 xtalk_rate_ll = ((int64_t)bound_l_x *
charlesmn 0:3ac96e360672 845 ((int64_t)xgradient)) + ((int64_t)bound_d_y *
charlesmn 0:3ac96e360672 846 ((int64_t)ygradient));
charlesmn 0:3ac96e360672 847 xtalk_rate_ll = do_division_s((xtalk_rate_ll + 1), 2);
charlesmn 0:3ac96e360672 848 xtalk_rate_ll += ((int64_t)mean_offset * 4);
charlesmn 0:3ac96e360672 849
charlesmn 0:3ac96e360672 850 xtalk_rate_ur = ((int64_t)bound_r_x *
charlesmn 0:3ac96e360672 851 ((int64_t)xgradient)) + ((int64_t)bound_u_y *
charlesmn 0:3ac96e360672 852 ((int64_t)ygradient));
charlesmn 0:3ac96e360672 853 xtalk_rate_ur = do_division_s((xtalk_rate_ur + 1), 2);
charlesmn 0:3ac96e360672 854 xtalk_rate_ur += ((int64_t)mean_offset * 4);
charlesmn 0:3ac96e360672 855 }
charlesmn 0:3ac96e360672 856
charlesmn 0:3ac96e360672 857 trace_print(
charlesmn 0:3ac96e360672 858 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 859 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 860 "xtalk_rate_ll", xtalk_rate_ll);
charlesmn 0:3ac96e360672 861
charlesmn 0:3ac96e360672 862 trace_print(
charlesmn 0:3ac96e360672 863 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 864 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 865 "xtalk_rate_ur", xtalk_rate_ur);
charlesmn 0:3ac96e360672 866
charlesmn 0:3ac96e360672 867
charlesmn 0:3ac96e360672 868
charlesmn 0:3ac96e360672 869 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 870 xtalk_avg = do_division_s(
charlesmn 0:3ac96e360672 871 ((xtalk_rate_ll + xtalk_rate_ur) + 1), 2);
charlesmn 0:3ac96e360672 872
charlesmn 0:3ac96e360672 873 trace_print(
charlesmn 0:3ac96e360672 874 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 875 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 876 "xtalk_avg", xtalk_avg);
charlesmn 0:3ac96e360672 877
charlesmn 0:3ac96e360672 878
charlesmn 0:3ac96e360672 879
charlesmn 0:3ac96e360672 880 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 881 if (xtalk_avg < 0)
charlesmn 0:3ac96e360672 882 xtalk_avg = 0;
charlesmn 0:3ac96e360672 883
charlesmn 0:3ac96e360672 884
charlesmn 0:3ac96e360672 885
charlesmn 0:3ac96e360672 886
charlesmn 0:3ac96e360672 887
charlesmn 0:3ac96e360672 888 *xtalk_rate_kcps = (uint32_t) xtalk_avg;
charlesmn 0:3ac96e360672 889
charlesmn 0:3ac96e360672 890 trace_print(
charlesmn 0:3ac96e360672 891 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 892 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 893 "xtalk_rate_kcps", xtalk_avg);
charlesmn 0:3ac96e360672 894
charlesmn 0:3ac96e360672 895 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 896
charlesmn 0:3ac96e360672 897 return status;
charlesmn 0:3ac96e360672 898 }
charlesmn 0:3ac96e360672 899
charlesmn 0:3ac96e360672 900
charlesmn 0:3ac96e360672 901
charlesmn 0:3ac96e360672 902 VL53L1_Error VL53L1_f_041(
charlesmn 0:3ac96e360672 903 VL53L1_histogram_bin_data_t *phist_data,
charlesmn 0:3ac96e360672 904 VL53L1_xtalk_histogram_shape_t *pxtalk_data,
charlesmn 0:3ac96e360672 905 uint32_t xtalk_rate_kcps,
charlesmn 0:3ac96e360672 906 VL53L1_histogram_bin_data_t *pxtalkcount_data
charlesmn 0:3ac96e360672 907 )
charlesmn 0:3ac96e360672 908 {
charlesmn 0:3ac96e360672 909
charlesmn 0:3ac96e360672 910
charlesmn 0:3ac96e360672 911 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 912
charlesmn 0:3ac96e360672 913 uint64_t xtalk_events_per_spad = 0;
charlesmn 0:3ac96e360672 914 uint64_t xtalk_total_events = 0;
charlesmn 0:3ac96e360672 915 uint64_t xtalk_temp_bin = 0;
charlesmn 0:3ac96e360672 916
charlesmn 0:3ac96e360672 917 uint8_t i = 0;
charlesmn 0:3ac96e360672 918
charlesmn 0:3ac96e360672 919 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 920
charlesmn 0:3ac96e360672 921 trace_print(
charlesmn 0:3ac96e360672 922 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 923 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 924 "pk_duration_internal", phist_data->peak_duration_us);
charlesmn 0:3ac96e360672 925
charlesmn 0:3ac96e360672 926
charlesmn 0:3ac96e360672 927
charlesmn 0:3ac96e360672 928 xtalk_events_per_spad = do_division_u((((uint64_t)xtalk_rate_kcps *
charlesmn 0:3ac96e360672 929 (uint64_t)phist_data->peak_duration_us) + 500), 1000);
charlesmn 0:3ac96e360672 930
charlesmn 0:3ac96e360672 931
charlesmn 0:3ac96e360672 932 trace_print(
charlesmn 0:3ac96e360672 933 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 934 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 935 "xtalk_events_per_spad", xtalk_events_per_spad);
charlesmn 0:3ac96e360672 936
charlesmn 0:3ac96e360672 937
charlesmn 0:3ac96e360672 938
charlesmn 0:3ac96e360672 939
charlesmn 0:3ac96e360672 940 xtalk_total_events = xtalk_events_per_spad *
charlesmn 0:3ac96e360672 941 (uint64_t)phist_data->result__dss_actual_effective_spads;
charlesmn 0:3ac96e360672 942
charlesmn 0:3ac96e360672 943 xtalk_total_events = do_division_u((xtalk_total_events), 256);
charlesmn 0:3ac96e360672 944
charlesmn 0:3ac96e360672 945 xtalk_total_events = do_division_u((xtalk_total_events + 1024), 2048);
charlesmn 0:3ac96e360672 946
charlesmn 0:3ac96e360672 947 if (xtalk_total_events > 0xFFFFFFFF)
charlesmn 0:3ac96e360672 948 xtalk_total_events = 0xFFFFFFFF;
charlesmn 0:3ac96e360672 949
charlesmn 0:3ac96e360672 950 trace_print(
charlesmn 0:3ac96e360672 951 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 952 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 953 "xtalk_total_events", xtalk_total_events);
charlesmn 0:3ac96e360672 954
charlesmn 0:3ac96e360672 955
charlesmn 0:3ac96e360672 956
charlesmn 0:3ac96e360672 957
charlesmn 0:3ac96e360672 958
charlesmn 0:3ac96e360672 959
charlesmn 0:3ac96e360672 960 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) {
charlesmn 0:3ac96e360672 961 xtalk_temp_bin = (uint64_t)pxtalk_data->bin_data[i] *
charlesmn 0:3ac96e360672 962 (uint64_t)xtalk_total_events;
charlesmn 0:3ac96e360672 963 xtalk_temp_bin = do_division_u((xtalk_temp_bin + 512), 1024);
charlesmn 0:3ac96e360672 964 pxtalkcount_data->bin_data[i] = (uint32_t)xtalk_temp_bin;
charlesmn 0:3ac96e360672 965
charlesmn 0:3ac96e360672 966 trace_print(
charlesmn 0:3ac96e360672 967 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 968 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 969 "bin_data", pxtalkcount_data->bin_data[i]);
charlesmn 0:3ac96e360672 970 }
charlesmn 0:3ac96e360672 971
charlesmn 0:3ac96e360672 972 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 973
charlesmn 0:3ac96e360672 974 return status;
charlesmn 0:3ac96e360672 975 }
charlesmn 0:3ac96e360672 976
charlesmn 0:3ac96e360672 977
charlesmn 0:3ac96e360672 978 VL53L1_Error VL53L1_f_055(
charlesmn 0:3ac96e360672 979 VL53L1_histogram_bin_data_t *phist_data,
charlesmn 0:3ac96e360672 980 VL53L1_histogram_bin_data_t *pxtalk_data,
charlesmn 0:3ac96e360672 981 uint8_t xtalk_bin_offset)
charlesmn 0:3ac96e360672 982 {
charlesmn 0:3ac96e360672 983
charlesmn 0:3ac96e360672 984
charlesmn 0:3ac96e360672 985 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 986
charlesmn 0:3ac96e360672 987 uint8_t i = 0;
charlesmn 0:3ac96e360672 988
charlesmn 0:3ac96e360672 989 int32_t temp_bin;
charlesmn 0:3ac96e360672 990
charlesmn 0:3ac96e360672 991 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 992
charlesmn 0:3ac96e360672 993
charlesmn 0:3ac96e360672 994
charlesmn 0:3ac96e360672 995
charlesmn 0:3ac96e360672 996 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 997 for (i = xtalk_bin_offset;
charlesmn 0:3ac96e360672 998 i < pxtalk_data->VL53L1_p_024; i++) {
charlesmn 0:3ac96e360672 999
charlesmn 0:3ac96e360672 1000 temp_bin = (int32_t)phist_data->bin_data[i] -
charlesmn 0:3ac96e360672 1001 (int32_t)pxtalk_data->bin_data[i - xtalk_bin_offset];
charlesmn 0:3ac96e360672 1002
charlesmn 0:3ac96e360672 1003 if (temp_bin < 0)
charlesmn 0:3ac96e360672 1004 temp_bin = 0;
charlesmn 0:3ac96e360672 1005
charlesmn 0:3ac96e360672 1006 phist_data->bin_data[i] = (uint32_t)temp_bin;
charlesmn 0:3ac96e360672 1007 }
charlesmn 0:3ac96e360672 1008
charlesmn 0:3ac96e360672 1009
charlesmn 0:3ac96e360672 1010 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 1011
charlesmn 0:3ac96e360672 1012 return status;
charlesmn 0:3ac96e360672 1013 }
charlesmn 0:3ac96e360672 1014
charlesmn 0:3ac96e360672 1015
charlesmn 0:3ac96e360672 1016 VL53L1_Error VL53L1_f_052(
charlesmn 0:3ac96e360672 1017 VL53L1_histogram_bin_data_t *pxtalk_data,
charlesmn 0:3ac96e360672 1018 uint32_t amb_threshold,
charlesmn 0:3ac96e360672 1019 uint8_t VL53L1_p_022,
charlesmn 0:3ac96e360672 1020 uint8_t VL53L1_p_026)
charlesmn 0:3ac96e360672 1021 {
charlesmn 0:3ac96e360672 1022
charlesmn 0:3ac96e360672 1023
charlesmn 0:3ac96e360672 1024 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 1025
charlesmn 0:3ac96e360672 1026 uint8_t i = 0;
charlesmn 0:3ac96e360672 1027 uint8_t first_bin_int = 0;
charlesmn 0:3ac96e360672 1028 uint8_t first_bin_inc = 0;
charlesmn 0:3ac96e360672 1029 uint8_t last_bin_int = 0;
charlesmn 0:3ac96e360672 1030 uint8_t realign_bin = 0;
charlesmn 0:3ac96e360672 1031 uint8_t realign_index = 0;
charlesmn 0:3ac96e360672 1032 int32_t realign_bin_data[VL53L1_HISTOGRAM_BUFFER_SIZE];
charlesmn 0:3ac96e360672 1033
charlesmn 0:3ac96e360672 1034 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 1035
charlesmn 0:3ac96e360672 1036
charlesmn 0:3ac96e360672 1037
charlesmn 0:3ac96e360672 1038 for (i = 0 ; i < VL53L1_HISTOGRAM_BUFFER_SIZE ; i++)
charlesmn 0:3ac96e360672 1039 realign_bin_data[i] = 0;
charlesmn 0:3ac96e360672 1040
charlesmn 0:3ac96e360672 1041 first_bin_int = VL53L1_p_022;
charlesmn 0:3ac96e360672 1042 last_bin_int = VL53L1_p_026;
charlesmn 0:3ac96e360672 1043
charlesmn 0:3ac96e360672 1044
charlesmn 0:3ac96e360672 1045
charlesmn 0:3ac96e360672 1046
charlesmn 0:3ac96e360672 1047
charlesmn 0:3ac96e360672 1048 VL53L1_hist_remove_ambient_bins(pxtalk_data);
charlesmn 0:3ac96e360672 1049
charlesmn 0:3ac96e360672 1050
charlesmn 0:3ac96e360672 1051
charlesmn 0:3ac96e360672 1052 first_bin_int = (first_bin_int) %
charlesmn 0:3ac96e360672 1053 pxtalk_data->VL53L1_p_024;
charlesmn 0:3ac96e360672 1054
charlesmn 0:3ac96e360672 1055 last_bin_int = (last_bin_int) %
charlesmn 0:3ac96e360672 1056 pxtalk_data->VL53L1_p_024;
charlesmn 0:3ac96e360672 1057
charlesmn 0:3ac96e360672 1058 first_bin_inc = (first_bin_int + 1) % pxtalk_data->VL53L1_p_024;
charlesmn 0:3ac96e360672 1059
charlesmn 0:3ac96e360672 1060
charlesmn 0:3ac96e360672 1061
charlesmn 0:3ac96e360672 1062 if (first_bin_inc > last_bin_int) {
charlesmn 0:3ac96e360672 1063
charlesmn 0:3ac96e360672 1064
charlesmn 0:3ac96e360672 1065 realign_bin = pxtalk_data->VL53L1_p_024 - first_bin_inc;
charlesmn 0:3ac96e360672 1066
charlesmn 0:3ac96e360672 1067
charlesmn 0:3ac96e360672 1068 first_bin_int = (first_bin_int + realign_bin) %
charlesmn 0:3ac96e360672 1069 pxtalk_data->VL53L1_p_024;
charlesmn 0:3ac96e360672 1070 last_bin_int = (last_bin_int + realign_bin) %
charlesmn 0:3ac96e360672 1071 pxtalk_data->VL53L1_p_024;
charlesmn 0:3ac96e360672 1072
charlesmn 0:3ac96e360672 1073
charlesmn 0:3ac96e360672 1074 pxtalk_data->zero_distance_phase =
charlesmn 0:3ac96e360672 1075 pxtalk_data->zero_distance_phase +
charlesmn 0:3ac96e360672 1076 ((uint16_t)realign_bin * 2048);
charlesmn 0:3ac96e360672 1077 }
charlesmn 0:3ac96e360672 1078
charlesmn 0:3ac96e360672 1079 if (realign_bin > 0) {
charlesmn 0:3ac96e360672 1080
charlesmn 0:3ac96e360672 1081 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++)
charlesmn 0:3ac96e360672 1082 realign_bin_data[i] = pxtalk_data->bin_data[i];
charlesmn 0:3ac96e360672 1083
charlesmn 0:3ac96e360672 1084
charlesmn 0:3ac96e360672 1085 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) {
charlesmn 0:3ac96e360672 1086 realign_index = (pxtalk_data->VL53L1_p_024 -
charlesmn 0:3ac96e360672 1087 realign_bin + i)
charlesmn 0:3ac96e360672 1088 % pxtalk_data->VL53L1_p_024;
charlesmn 0:3ac96e360672 1089
charlesmn 0:3ac96e360672 1090 pxtalk_data->bin_data[i] =
charlesmn 0:3ac96e360672 1091 realign_bin_data[realign_index];
charlesmn 0:3ac96e360672 1092 }
charlesmn 0:3ac96e360672 1093 }
charlesmn 0:3ac96e360672 1094
charlesmn 0:3ac96e360672 1095
charlesmn 0:3ac96e360672 1096
charlesmn 0:3ac96e360672 1097
charlesmn 0:3ac96e360672 1098 trace_print(
charlesmn 0:3ac96e360672 1099 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 1100 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 1101 "first bin int", first_bin_int);
charlesmn 0:3ac96e360672 1102
charlesmn 0:3ac96e360672 1103 trace_print(
charlesmn 0:3ac96e360672 1104 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 1105 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 1106 "last bin int", last_bin_int);
charlesmn 0:3ac96e360672 1107
charlesmn 0:3ac96e360672 1108 trace_print(
charlesmn 0:3ac96e360672 1109 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 1110 " %-48s : %10d\n",
charlesmn 0:3ac96e360672 1111 "amb thresh", amb_threshold);
charlesmn 0:3ac96e360672 1112
charlesmn 0:3ac96e360672 1113
charlesmn 0:3ac96e360672 1114
charlesmn 0:3ac96e360672 1115
charlesmn 0:3ac96e360672 1116
charlesmn 0:3ac96e360672 1117 for (i = 0; i < pxtalk_data->VL53L1_p_024; i++) {
charlesmn 0:3ac96e360672 1118
charlesmn 0:3ac96e360672 1119 if (first_bin_int <= last_bin_int) {
charlesmn 0:3ac96e360672 1120 if ((i >= first_bin_int) && (i <= last_bin_int)) {
charlesmn 0:3ac96e360672 1121 if (pxtalk_data->bin_data[i] <
charlesmn 0:3ac96e360672 1122 (int32_t)amb_threshold)
charlesmn 0:3ac96e360672 1123 pxtalk_data->bin_data[i] = 0;
charlesmn 0:3ac96e360672 1124 } else {
charlesmn 0:3ac96e360672 1125 pxtalk_data->bin_data[i] = 0;
charlesmn 0:3ac96e360672 1126 }
charlesmn 0:3ac96e360672 1127 } else {
charlesmn 0:3ac96e360672 1128 if ((i >= first_bin_int) || (i <= last_bin_int)) {
charlesmn 0:3ac96e360672 1129 if (pxtalk_data->bin_data[i] <
charlesmn 0:3ac96e360672 1130 (int32_t)amb_threshold) {
charlesmn 0:3ac96e360672 1131 pxtalk_data->bin_data[i] = 0;
charlesmn 0:3ac96e360672 1132 }
charlesmn 0:3ac96e360672 1133 } else {
charlesmn 0:3ac96e360672 1134 pxtalk_data->bin_data[i] = 0;
charlesmn 0:3ac96e360672 1135 }
charlesmn 0:3ac96e360672 1136 }
charlesmn 0:3ac96e360672 1137 }
charlesmn 0:3ac96e360672 1138
charlesmn 0:3ac96e360672 1139
charlesmn 0:3ac96e360672 1140
charlesmn 0:3ac96e360672 1141
charlesmn 0:3ac96e360672 1142
charlesmn 0:3ac96e360672 1143 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 1144
charlesmn 0:3ac96e360672 1145 return status;
charlesmn 0:3ac96e360672 1146 }
charlesmn 0:3ac96e360672 1147
charlesmn 0:3ac96e360672 1148
charlesmn 0:3ac96e360672 1149 VL53L1_Error VL53L1_f_051(
charlesmn 0:3ac96e360672 1150 uint8_t sigma_mult,
charlesmn 0:3ac96e360672 1151 int32_t VL53L1_p_004,
charlesmn 0:3ac96e360672 1152 uint32_t *ambient_noise)
charlesmn 0:3ac96e360672 1153 {
charlesmn 0:3ac96e360672 1154
charlesmn 0:3ac96e360672 1155
charlesmn 0:3ac96e360672 1156
charlesmn 0:3ac96e360672 1157 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 1158
charlesmn 0:3ac96e360672 1159 uint32_t ambient_events_per_bin_int = 0;
charlesmn 0:3ac96e360672 1160
charlesmn 0:3ac96e360672 1161 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 1162
charlesmn 0:3ac96e360672 1163 if (VL53L1_p_004 <= 0)
charlesmn 0:3ac96e360672 1164 ambient_events_per_bin_int = 1;
charlesmn 0:3ac96e360672 1165 else
charlesmn 0:3ac96e360672 1166 ambient_events_per_bin_int = (uint32_t)VL53L1_p_004;
charlesmn 0:3ac96e360672 1167
charlesmn 0:3ac96e360672 1168 *ambient_noise = VL53L1_isqrt(ambient_events_per_bin_int);
charlesmn 0:3ac96e360672 1169
charlesmn 0:3ac96e360672 1170 *ambient_noise = *ambient_noise * (uint32_t)sigma_mult;
charlesmn 0:3ac96e360672 1171
charlesmn 0:3ac96e360672 1172 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 1173
charlesmn 0:3ac96e360672 1174 return status;
charlesmn 0:3ac96e360672 1175 }
charlesmn 0:3ac96e360672 1176
charlesmn 0:3ac96e360672 1177
charlesmn 0:3ac96e360672 1178
charlesmn 0:3ac96e360672 1179
charlesmn 0:3ac96e360672 1180
charlesmn 0:3ac96e360672 1181
charlesmn 0:3ac96e360672 1182
charlesmn 0:3ac96e360672 1183
charlesmn 0:3ac96e360672 1184
charlesmn 0:3ac96e360672 1185
charlesmn 0:3ac96e360672 1186
charlesmn 0:3ac96e360672 1187
charlesmn 0:3ac96e360672 1188
charlesmn 0:3ac96e360672 1189
charlesmn 0:3ac96e360672 1190
charlesmn 0:3ac96e360672 1191
charlesmn 0:3ac96e360672 1192
charlesmn 0:3ac96e360672 1193
charlesmn 0:3ac96e360672 1194
charlesmn 0:3ac96e360672 1195