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@0:3ac96e360672, 2020-11-06 (annotated)

Committer:
charlesmn
Date:
Fri Nov 06 10:06:37 2020 +0000
Revision:
0:3ac96e360672
Child:
7:1add29d51e72
Library for ST Vl53L1A1 time of flight sensor.

Who changed what in which revision?

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