ST Expansion SW Team / VL53L1CB

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

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

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

src/vl53l1_hist_algos_gen3.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
Child:
18:0696efe39d08
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_hist_core.h"
charlesmn 0:3ac96e360672 32 #include "vl53l1_hist_algos_gen3.h"
charlesmn 0:3ac96e360672 33 #include "vl53l1_sigma_estimate.h"
charlesmn 0:3ac96e360672 34 #include "vl53l1_dmax.h"
charlesmn 0:3ac96e360672 35
charlesmn 0:3ac96e360672 36
charlesmn 0:3ac96e360672 37
charlesmn 0:3ac96e360672 38 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 39 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 40 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 41 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 42 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 43 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM,\
charlesmn 0:3ac96e360672 44 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 45
charlesmn 0:3ac96e360672 46 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 47 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 48 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 49
charlesmn 0:3ac96e360672 50
charlesmn 0:3ac96e360672 51 void VL53L1_f_016(
charlesmn 0:3ac96e360672 52 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 53 {
charlesmn 0:3ac96e360672 54
charlesmn 0:3ac96e360672 55
charlesmn 0:3ac96e360672 56 uint8_t lb = 0;
charlesmn 0:3ac96e360672 57
charlesmn 0:3ac96e360672 58 palgo->VL53L1_p_023 = VL53L1_HISTOGRAM_BUFFER_SIZE;
charlesmn 0:3ac96e360672 59 palgo->VL53L1_p_022 = 0;
charlesmn 0:3ac96e360672 60 palgo->VL53L1_p_024 = 0;
charlesmn 0:3ac96e360672 61 palgo->VL53L1_p_045 = 0;
charlesmn 0:3ac96e360672 62 palgo->VL53L1_p_004 = 0;
charlesmn 0:3ac96e360672 63 palgo->VL53L1_p_032 = 0;
charlesmn 0:3ac96e360672 64
charlesmn 0:3ac96e360672 65 for (lb = palgo->VL53L1_p_022; lb < palgo->VL53L1_p_023; lb++) {
charlesmn 0:3ac96e360672 66 palgo->VL53L1_p_043[lb] = 0;
charlesmn 0:3ac96e360672 67 palgo->VL53L1_p_046[lb] = 0;
charlesmn 0:3ac96e360672 68 palgo->VL53L1_p_047[lb] = 0;
charlesmn 0:3ac96e360672 69 palgo->VL53L1_p_048[lb] = 0;
charlesmn 0:3ac96e360672 70 palgo->VL53L1_p_008[lb] = 0;
charlesmn 0:3ac96e360672 71 }
charlesmn 0:3ac96e360672 72
charlesmn 0:3ac96e360672 73 palgo->VL53L1_p_049 = 0;
charlesmn 0:3ac96e360672 74 palgo->VL53L1_p_050 = VL53L1_D_001;
charlesmn 0:3ac96e360672 75 palgo->VL53L1_p_051 = 0;
charlesmn 0:3ac96e360672 76
charlesmn 0:3ac96e360672 77
charlesmn 0:3ac96e360672 78
charlesmn 0:3ac96e360672 79 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 80 0,
charlesmn 0:3ac96e360672 81 VL53L1_HISTOGRAM_BUFFER_SIZE,
charlesmn 0:3ac96e360672 82 &(palgo->VL53L1_p_010));
charlesmn 0:3ac96e360672 83 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 84 0,
charlesmn 0:3ac96e360672 85 VL53L1_HISTOGRAM_BUFFER_SIZE,
charlesmn 0:3ac96e360672 86 &(palgo->VL53L1_p_038));
charlesmn 0:3ac96e360672 87 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 88 0,
charlesmn 0:3ac96e360672 89 VL53L1_HISTOGRAM_BUFFER_SIZE,
charlesmn 0:3ac96e360672 90 &(palgo->VL53L1_p_052));
charlesmn 0:3ac96e360672 91 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 92 0,
charlesmn 0:3ac96e360672 93 VL53L1_HISTOGRAM_BUFFER_SIZE,
charlesmn 0:3ac96e360672 94 &(palgo->VL53L1_p_053));
charlesmn 0:3ac96e360672 95 VL53L1_init_histogram_bin_data_struct(
charlesmn 0:3ac96e360672 96 0,
charlesmn 0:3ac96e360672 97 VL53L1_HISTOGRAM_BUFFER_SIZE,
charlesmn 0:3ac96e360672 98 &(palgo->VL53L1_p_054));
charlesmn 0:3ac96e360672 99 }
charlesmn 0:3ac96e360672 100
charlesmn 0:3ac96e360672 101
charlesmn 0:3ac96e360672 102
charlesmn 0:3ac96e360672 103 VL53L1_Error VL53L1_f_018(
charlesmn 0:3ac96e360672 104 uint16_t ambient_threshold_events_scaler,
charlesmn 0:3ac96e360672 105 int32_t ambient_threshold_sigma,
charlesmn 0:3ac96e360672 106 int32_t min_ambient_threshold_events,
charlesmn 0:3ac96e360672 107 uint8_t algo__crosstalk_compensation_enable,
charlesmn 0:3ac96e360672 108 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 109 VL53L1_histogram_bin_data_t *pxtalk,
charlesmn 0:3ac96e360672 110 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 111 {
charlesmn 0:3ac96e360672 112
charlesmn 0:3ac96e360672 113
charlesmn 0:3ac96e360672 114
charlesmn 0:3ac96e360672 115 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 116 uint8_t lb = 0;
charlesmn 0:3ac96e360672 117 uint8_t VL53L1_p_001 = 0;
charlesmn 0:3ac96e360672 118 int64_t tmp = 0;
charlesmn 0:3ac96e360672 119 int32_t amb_events = 0;
charlesmn 0:3ac96e360672 120 int32_t VL53L1_p_008 = 0;
charlesmn 0:3ac96e360672 121 int32_t samples = 0;
charlesmn 0:3ac96e360672 122
charlesmn 0:3ac96e360672 123 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 124
charlesmn 0:3ac96e360672 125
charlesmn 0:3ac96e360672 126 palgo->VL53L1_p_023 = pbins->VL53L1_p_023;
charlesmn 0:3ac96e360672 127 palgo->VL53L1_p_022 = pbins->VL53L1_p_022;
charlesmn 0:3ac96e360672 128 palgo->VL53L1_p_024 = pbins->VL53L1_p_024;
charlesmn 0:3ac96e360672 129 palgo->VL53L1_p_004 = pbins->VL53L1_p_004;
charlesmn 0:3ac96e360672 130
charlesmn 0:3ac96e360672 131
charlesmn 0:3ac96e360672 132
charlesmn 0:3ac96e360672 133 palgo->VL53L1_p_031 =
charlesmn 0:3ac96e360672 134 VL53L1_decode_vcsel_period(pbins->VL53L1_p_009);
charlesmn 0:3ac96e360672 135
charlesmn 0:3ac96e360672 136
charlesmn 0:3ac96e360672 137
charlesmn 0:3ac96e360672 138 tmp = (int64_t)pbins->VL53L1_p_004;
charlesmn 0:3ac96e360672 139 tmp *= (int64_t)ambient_threshold_events_scaler;
charlesmn 0:3ac96e360672 140 tmp += 2048;
charlesmn 0:3ac96e360672 141 tmp = do_division_s(tmp, 4096);
charlesmn 0:3ac96e360672 142 amb_events = (int32_t)tmp;
charlesmn 0:3ac96e360672 143
charlesmn 0:3ac96e360672 144
charlesmn 0:3ac96e360672 145
charlesmn 0:3ac96e360672 146 for (lb = 0; lb < pbins->VL53L1_p_024; lb++) {
charlesmn 0:3ac96e360672 147
charlesmn 0:3ac96e360672 148 VL53L1_p_001 = lb >> 2;
charlesmn 0:3ac96e360672 149 samples = (int32_t)pbins->bin_rep[VL53L1_p_001];
charlesmn 0:3ac96e360672 150
charlesmn 0:3ac96e360672 151 if (samples > 0) {
charlesmn 0:3ac96e360672 152
charlesmn 0:3ac96e360672 153 if (lb < pxtalk->VL53L1_p_024 &&
charlesmn 0:3ac96e360672 154 algo__crosstalk_compensation_enable > 0)
charlesmn 0:3ac96e360672 155 VL53L1_p_008 = samples * (amb_events +
charlesmn 0:3ac96e360672 156 pxtalk->bin_data[lb]);
charlesmn 0:3ac96e360672 157 else
charlesmn 0:3ac96e360672 158 VL53L1_p_008 = samples * amb_events;
charlesmn 0:3ac96e360672 159
charlesmn 0:3ac96e360672 160 VL53L1_p_008 = VL53L1_isqrt(VL53L1_p_008);
charlesmn 0:3ac96e360672 161
charlesmn 0:3ac96e360672 162 VL53L1_p_008 += (samples/2);
charlesmn 0:3ac96e360672 163 VL53L1_p_008 /= samples;
charlesmn 0:3ac96e360672 164 VL53L1_p_008 *= ambient_threshold_sigma;
charlesmn 0:3ac96e360672 165 VL53L1_p_008 += 8;
charlesmn 0:3ac96e360672 166 VL53L1_p_008 /= 16;
charlesmn 0:3ac96e360672 167 VL53L1_p_008 += amb_events;
charlesmn 0:3ac96e360672 168
charlesmn 0:3ac96e360672 169 if (VL53L1_p_008 < min_ambient_threshold_events)
charlesmn 0:3ac96e360672 170 VL53L1_p_008 = min_ambient_threshold_events;
charlesmn 0:3ac96e360672 171
charlesmn 0:3ac96e360672 172 palgo->VL53L1_p_056[lb] = VL53L1_p_008;
charlesmn 0:3ac96e360672 173 palgo->VL53L1_p_032 = VL53L1_p_008;
charlesmn 0:3ac96e360672 174 }
charlesmn 0:3ac96e360672 175
charlesmn 0:3ac96e360672 176
charlesmn 0:3ac96e360672 177
charlesmn 0:3ac96e360672 178 }
charlesmn 0:3ac96e360672 179
charlesmn 0:3ac96e360672 180
charlesmn 0:3ac96e360672 181
charlesmn 0:3ac96e360672 182 palgo->VL53L1_p_045 = 0;
charlesmn 0:3ac96e360672 183
charlesmn 0:3ac96e360672 184 for (lb = pbins->VL53L1_p_022; lb < pbins->VL53L1_p_024; lb++) {
charlesmn 0:3ac96e360672 185
charlesmn 0:3ac96e360672 186 if (pbins->bin_data[lb] > palgo->VL53L1_p_056[lb]) {
charlesmn 0:3ac96e360672 187 palgo->VL53L1_p_043[lb] = 1;
charlesmn 0:3ac96e360672 188 palgo->VL53L1_p_046[lb] = 1;
charlesmn 0:3ac96e360672 189 palgo->VL53L1_p_045++;
charlesmn 0:3ac96e360672 190 } else {
charlesmn 0:3ac96e360672 191 palgo->VL53L1_p_043[lb] = 0;
charlesmn 0:3ac96e360672 192 palgo->VL53L1_p_046[lb] = 0;
charlesmn 0:3ac96e360672 193 }
charlesmn 0:3ac96e360672 194 }
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 197
charlesmn 0:3ac96e360672 198 return status;
charlesmn 0:3ac96e360672 199
charlesmn 0:3ac96e360672 200 }
charlesmn 0:3ac96e360672 201
charlesmn 0:3ac96e360672 202
charlesmn 0:3ac96e360672 203
charlesmn 0:3ac96e360672 204
charlesmn 0:3ac96e360672 205 VL53L1_Error VL53L1_f_019(
charlesmn 0:3ac96e360672 206 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 207 {
charlesmn 0:3ac96e360672 208
charlesmn 0:3ac96e360672 209
charlesmn 0:3ac96e360672 210
charlesmn 0:3ac96e360672 211 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 212
charlesmn 0:3ac96e360672 213 uint8_t i = 0;
charlesmn 0:3ac96e360672 214 uint8_t j = 0;
charlesmn 0:3ac96e360672 215 uint8_t found = 0;
charlesmn 0:3ac96e360672 216
charlesmn 0:3ac96e360672 217 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 218
charlesmn 0:3ac96e360672 219 palgo->VL53L1_p_049 = 0;
charlesmn 0:3ac96e360672 220
charlesmn 0:3ac96e360672 221 for (i = 0; i < palgo->VL53L1_p_031; i++) {
charlesmn 0:3ac96e360672 222
charlesmn 0:3ac96e360672 223 j = (i + 1) % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 224
charlesmn 0:3ac96e360672 225
charlesmn 0:3ac96e360672 226
charlesmn 0:3ac96e360672 227 if (i < palgo->VL53L1_p_024 && j < palgo->VL53L1_p_024) {
charlesmn 0:3ac96e360672 228 if (palgo->VL53L1_p_046[i] == 0 &&
charlesmn 0:3ac96e360672 229 palgo->VL53L1_p_046[j] == 1 &&
charlesmn 0:3ac96e360672 230 found == 0) {
charlesmn 0:3ac96e360672 231 palgo->VL53L1_p_049 = i;
charlesmn 0:3ac96e360672 232 found = 1;
charlesmn 0:3ac96e360672 233 }
charlesmn 0:3ac96e360672 234 }
charlesmn 0:3ac96e360672 235 }
charlesmn 0:3ac96e360672 236
charlesmn 0:3ac96e360672 237 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 238
charlesmn 0:3ac96e360672 239 return status;
charlesmn 0:3ac96e360672 240 }
charlesmn 0:3ac96e360672 241
charlesmn 0:3ac96e360672 242
charlesmn 0:3ac96e360672 243 VL53L1_Error VL53L1_f_020(
charlesmn 0:3ac96e360672 244 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 245 {
charlesmn 0:3ac96e360672 246
charlesmn 0:3ac96e360672 247
charlesmn 0:3ac96e360672 248
charlesmn 0:3ac96e360672 249 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 250 uint8_t i = 0;
charlesmn 0:3ac96e360672 251 uint8_t j = 0;
charlesmn 0:3ac96e360672 252 uint8_t lb = 0;
charlesmn 0:3ac96e360672 253
charlesmn 0:3ac96e360672 254 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 255
charlesmn 0:3ac96e360672 256 for (lb = palgo->VL53L1_p_049;
charlesmn 0:3ac96e360672 257 lb < (palgo->VL53L1_p_049 +
charlesmn 0:3ac96e360672 258 palgo->VL53L1_p_031);
charlesmn 0:3ac96e360672 259 lb++) {
charlesmn 0:3ac96e360672 260
charlesmn 0:3ac96e360672 261
charlesmn 0:3ac96e360672 262
charlesmn 0:3ac96e360672 263 i = lb % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 264 j = (lb + 1) % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 265
charlesmn 0:3ac96e360672 266
charlesmn 0:3ac96e360672 267
charlesmn 0:3ac96e360672 268 if (i < palgo->VL53L1_p_024 && j < palgo->VL53L1_p_024) {
charlesmn 0:3ac96e360672 269
charlesmn 0:3ac96e360672 270 if (palgo->VL53L1_p_046[i] == 0 &&
charlesmn 0:3ac96e360672 271 palgo->VL53L1_p_046[j] == 1)
charlesmn 0:3ac96e360672 272 palgo->VL53L1_p_051++;
charlesmn 0:3ac96e360672 273
charlesmn 0:3ac96e360672 274 if (palgo->VL53L1_p_046[i] > 0)
charlesmn 0:3ac96e360672 275 palgo->VL53L1_p_047[i] = palgo->VL53L1_p_051;
charlesmn 0:3ac96e360672 276 else
charlesmn 0:3ac96e360672 277 palgo->VL53L1_p_047[i] = 0;
charlesmn 0:3ac96e360672 278 }
charlesmn 0:3ac96e360672 279
charlesmn 0:3ac96e360672 280 }
charlesmn 0:3ac96e360672 281
charlesmn 0:3ac96e360672 282
charlesmn 0:3ac96e360672 283 if (palgo->VL53L1_p_051 > palgo->VL53L1_p_050)
charlesmn 0:3ac96e360672 284 palgo->VL53L1_p_051 = palgo->VL53L1_p_050;
charlesmn 0:3ac96e360672 285
charlesmn 0:3ac96e360672 286 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 287
charlesmn 0:3ac96e360672 288 return status;
charlesmn 0:3ac96e360672 289
charlesmn 0:3ac96e360672 290 }
charlesmn 0:3ac96e360672 291
charlesmn 0:3ac96e360672 292
charlesmn 0:3ac96e360672 293 VL53L1_Error VL53L1_f_021(
charlesmn 0:3ac96e360672 294 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 295 {
charlesmn 0:3ac96e360672 296
charlesmn 0:3ac96e360672 297
charlesmn 0:3ac96e360672 298
charlesmn 0:3ac96e360672 299 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 300
charlesmn 0:3ac96e360672 301 uint8_t i = 0;
charlesmn 0:3ac96e360672 302 uint8_t j = 0;
charlesmn 0:3ac96e360672 303 uint8_t blb = 0;
charlesmn 0:3ac96e360672 304 uint8_t pulse_no = 0;
charlesmn 0:3ac96e360672 305
charlesmn 0:3ac96e360672 306 uint8_t max_filter_half_width = 0;
charlesmn 0:3ac96e360672 307
charlesmn 0:3ac96e360672 308 VL53L1_hist_pulse_data_t *pdata;
charlesmn 0:3ac96e360672 309
charlesmn 0:3ac96e360672 310 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 311
charlesmn 0:3ac96e360672 312
charlesmn 0:3ac96e360672 313
charlesmn 0:3ac96e360672 314 max_filter_half_width = palgo->VL53L1_p_031 - 1;
charlesmn 0:3ac96e360672 315 max_filter_half_width = max_filter_half_width >> 1;
charlesmn 0:3ac96e360672 316
charlesmn 0:3ac96e360672 317 for (blb = palgo->VL53L1_p_049;
charlesmn 0:3ac96e360672 318 blb < (palgo->VL53L1_p_049 +
charlesmn 0:3ac96e360672 319 palgo->VL53L1_p_031);
charlesmn 0:3ac96e360672 320 blb++) {
charlesmn 0:3ac96e360672 321
charlesmn 0:3ac96e360672 322
charlesmn 0:3ac96e360672 323
charlesmn 0:3ac96e360672 324 i = blb % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 325 j = (blb + 1) % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 326
charlesmn 0:3ac96e360672 327
charlesmn 0:3ac96e360672 328
charlesmn 0:3ac96e360672 329 if (i < palgo->VL53L1_p_024 &&
charlesmn 0:3ac96e360672 330 j < palgo->VL53L1_p_024) {
charlesmn 0:3ac96e360672 331
charlesmn 0:3ac96e360672 332
charlesmn 0:3ac96e360672 333
charlesmn 0:3ac96e360672 334 if (palgo->VL53L1_p_047[i] == 0 &&
charlesmn 0:3ac96e360672 335 palgo->VL53L1_p_047[j] > 0) {
charlesmn 0:3ac96e360672 336
charlesmn 0:3ac96e360672 337 pulse_no = palgo->VL53L1_p_047[j] - 1;
charlesmn 0:3ac96e360672 338 pdata = &(palgo->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 339
charlesmn 0:3ac96e360672 340 if (pulse_no < palgo->VL53L1_p_050) {
charlesmn 0:3ac96e360672 341 pdata->VL53L1_p_015 = blb;
charlesmn 0:3ac96e360672 342 pdata->VL53L1_p_022 = blb + 1;
charlesmn 0:3ac96e360672 343 pdata->VL53L1_p_025 = 0xFF;
charlesmn 0:3ac96e360672 344 pdata->VL53L1_p_026 = 0;
charlesmn 0:3ac96e360672 345 pdata->VL53L1_p_016 = 0;
charlesmn 0:3ac96e360672 346 }
charlesmn 0:3ac96e360672 347 }
charlesmn 0:3ac96e360672 348
charlesmn 0:3ac96e360672 349
charlesmn 0:3ac96e360672 350
charlesmn 0:3ac96e360672 351 if (palgo->VL53L1_p_047[i] > 0
charlesmn 0:3ac96e360672 352 && palgo->VL53L1_p_047[j] == 0) {
charlesmn 0:3ac96e360672 353
charlesmn 0:3ac96e360672 354 pulse_no = palgo->VL53L1_p_047[i] - 1;
charlesmn 0:3ac96e360672 355 pdata = &(palgo->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 356
charlesmn 0:3ac96e360672 357 if (pulse_no < palgo->VL53L1_p_050) {
charlesmn 0:3ac96e360672 358
charlesmn 0:3ac96e360672 359 pdata->VL53L1_p_026 = blb;
charlesmn 0:3ac96e360672 360 pdata->VL53L1_p_016 = blb + 1;
charlesmn 0:3ac96e360672 361
charlesmn 0:3ac96e360672 362 pdata->VL53L1_p_027 =
charlesmn 0:3ac96e360672 363 (pdata->VL53L1_p_026 + 1) -
charlesmn 0:3ac96e360672 364 pdata->VL53L1_p_022;
charlesmn 0:3ac96e360672 365 pdata->VL53L1_p_055 =
charlesmn 0:3ac96e360672 366 (pdata->VL53L1_p_016 + 1) -
charlesmn 0:3ac96e360672 367 pdata->VL53L1_p_015;
charlesmn 0:3ac96e360672 368
charlesmn 0:3ac96e360672 369 if (pdata->VL53L1_p_055 >
charlesmn 0:3ac96e360672 370 max_filter_half_width)
charlesmn 0:3ac96e360672 371 pdata->VL53L1_p_055 =
charlesmn 0:3ac96e360672 372 max_filter_half_width;
charlesmn 0:3ac96e360672 373 }
charlesmn 0:3ac96e360672 374
charlesmn 0:3ac96e360672 375 }
charlesmn 0:3ac96e360672 376 }
charlesmn 0:3ac96e360672 377 }
charlesmn 0:3ac96e360672 378
charlesmn 0:3ac96e360672 379 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 380
charlesmn 0:3ac96e360672 381 return status;
charlesmn 0:3ac96e360672 382
charlesmn 0:3ac96e360672 383 }
charlesmn 0:3ac96e360672 384
charlesmn 0:3ac96e360672 385
charlesmn 0:3ac96e360672 386 VL53L1_Error VL53L1_f_028(
charlesmn 0:3ac96e360672 387 VL53L1_HistTargetOrder target_order,
charlesmn 0:3ac96e360672 388 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 389 {
charlesmn 0:3ac96e360672 390
charlesmn 0:3ac96e360672 391
charlesmn 0:3ac96e360672 392
charlesmn 0:3ac96e360672 393 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 394
charlesmn 0:3ac96e360672 395 VL53L1_hist_pulse_data_t tmp;
charlesmn 0:3ac96e360672 396 VL53L1_hist_pulse_data_t *ptmp = &tmp;
charlesmn 0:3ac96e360672 397 VL53L1_hist_pulse_data_t *p0;
charlesmn 0:3ac96e360672 398 VL53L1_hist_pulse_data_t *p1;
charlesmn 0:3ac96e360672 399
charlesmn 0:3ac96e360672 400 uint8_t i = 0;
charlesmn 0:3ac96e360672 401 uint8_t swapped = 1;
charlesmn 0:3ac96e360672 402
charlesmn 0:3ac96e360672 403 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 404
charlesmn 0:3ac96e360672 405 if (!(palgo->VL53L1_p_051 > 1))
charlesmn 0:3ac96e360672 406 goto ENDFUNC;
charlesmn 0:3ac96e360672 407
charlesmn 0:3ac96e360672 408 while (swapped > 0) {
charlesmn 0:3ac96e360672 409
charlesmn 0:3ac96e360672 410 swapped = 0;
charlesmn 0:3ac96e360672 411
charlesmn 0:3ac96e360672 412 for (i = 1; i < palgo->VL53L1_p_051; i++) {
charlesmn 0:3ac96e360672 413
charlesmn 0:3ac96e360672 414 p0 = &(palgo->VL53L1_p_002[i-1]);
charlesmn 0:3ac96e360672 415 p1 = &(palgo->VL53L1_p_002[i]);
charlesmn 0:3ac96e360672 416
charlesmn 0:3ac96e360672 417
charlesmn 0:3ac96e360672 418
charlesmn 0:3ac96e360672 419 if (target_order
charlesmn 0:3ac96e360672 420 == VL53L1_HIST_TARGET_ORDER__STRONGEST_FIRST) {
charlesmn 0:3ac96e360672 421
charlesmn 0:3ac96e360672 422 if (p0->VL53L1_p_013 <
charlesmn 0:3ac96e360672 423 p1->VL53L1_p_013) {
charlesmn 0:3ac96e360672 424
charlesmn 0:3ac96e360672 425
charlesmn 0:3ac96e360672 426
charlesmn 0:3ac96e360672 427 memcpy(ptmp,
charlesmn 0:3ac96e360672 428 p1, sizeof(VL53L1_hist_pulse_data_t));
charlesmn 0:3ac96e360672 429 memcpy(p1,
charlesmn 0:3ac96e360672 430 p0, sizeof(VL53L1_hist_pulse_data_t));
charlesmn 0:3ac96e360672 431 memcpy(p0,
charlesmn 0:3ac96e360672 432 ptmp, sizeof(VL53L1_hist_pulse_data_t));
charlesmn 0:3ac96e360672 433
charlesmn 0:3ac96e360672 434 swapped = 1;
charlesmn 0:3ac96e360672 435 }
charlesmn 0:3ac96e360672 436
charlesmn 0:3ac96e360672 437 } else {
charlesmn 0:3ac96e360672 438
charlesmn 0:3ac96e360672 439 if (p0->VL53L1_p_014 > p1->VL53L1_p_014) {
charlesmn 0:3ac96e360672 440
charlesmn 0:3ac96e360672 441
charlesmn 0:3ac96e360672 442
charlesmn 0:3ac96e360672 443 memcpy(ptmp,
charlesmn 0:3ac96e360672 444 p1, sizeof(VL53L1_hist_pulse_data_t));
charlesmn 0:3ac96e360672 445 memcpy(p1,
charlesmn 0:3ac96e360672 446 p0, sizeof(VL53L1_hist_pulse_data_t));
charlesmn 0:3ac96e360672 447 memcpy(p0,
charlesmn 0:3ac96e360672 448 ptmp, sizeof(VL53L1_hist_pulse_data_t));
charlesmn 0:3ac96e360672 449
charlesmn 0:3ac96e360672 450 swapped = 1;
charlesmn 0:3ac96e360672 451 }
charlesmn 0:3ac96e360672 452
charlesmn 0:3ac96e360672 453 }
charlesmn 0:3ac96e360672 454 }
charlesmn 0:3ac96e360672 455 }
charlesmn 0:3ac96e360672 456
charlesmn 0:3ac96e360672 457 ENDFUNC:
charlesmn 0:3ac96e360672 458 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 459
charlesmn 0:3ac96e360672 460 return status;
charlesmn 0:3ac96e360672 461
charlesmn 0:3ac96e360672 462 }
charlesmn 0:3ac96e360672 463
charlesmn 0:3ac96e360672 464
charlesmn 0:3ac96e360672 465 VL53L1_Error VL53L1_f_022(
charlesmn 0:3ac96e360672 466 uint8_t pulse_no,
charlesmn 0:3ac96e360672 467 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 468 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 469 {
charlesmn 0:3ac96e360672 470
charlesmn 0:3ac96e360672 471
charlesmn 0:3ac96e360672 472 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 473
charlesmn 0:3ac96e360672 474 uint8_t i = 0;
charlesmn 0:3ac96e360672 475 uint8_t lb = 0;
charlesmn 0:3ac96e360672 476
charlesmn 0:3ac96e360672 477 VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 478
charlesmn 0:3ac96e360672 479 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 480
charlesmn 0:3ac96e360672 481
charlesmn 0:3ac96e360672 482
charlesmn 0:3ac96e360672 483 pdata->VL53L1_p_021 = 0;
charlesmn 0:3ac96e360672 484 pdata->VL53L1_p_020 = 0;
charlesmn 0:3ac96e360672 485
charlesmn 0:3ac96e360672 486 for (lb = pdata->VL53L1_p_015; lb <= pdata->VL53L1_p_016; lb++) {
charlesmn 0:3ac96e360672 487 i = lb % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 488 pdata->VL53L1_p_021 += pbins->bin_data[i];
charlesmn 0:3ac96e360672 489 pdata->VL53L1_p_020 += palgo->VL53L1_p_004;
charlesmn 0:3ac96e360672 490 }
charlesmn 0:3ac96e360672 491
charlesmn 0:3ac96e360672 492
charlesmn 0:3ac96e360672 493
charlesmn 0:3ac96e360672 494 pdata->VL53L1_p_013 =
charlesmn 0:3ac96e360672 495 pdata->VL53L1_p_021 - pdata->VL53L1_p_020;
charlesmn 0:3ac96e360672 496
charlesmn 0:3ac96e360672 497 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 498
charlesmn 0:3ac96e360672 499 return status;
charlesmn 0:3ac96e360672 500 }
charlesmn 0:3ac96e360672 501
charlesmn 0:3ac96e360672 502
charlesmn 0:3ac96e360672 503 VL53L1_Error VL53L1_f_027(
charlesmn 0:3ac96e360672 504 uint8_t pulse_no,
charlesmn 0:3ac96e360672 505 uint8_t clip_events,
charlesmn 0:3ac96e360672 506 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 507 VL53L1_hist_gen3_algo_private_data_t *palgo)
charlesmn 0:3ac96e360672 508 {
charlesmn 0:3ac96e360672 509
charlesmn 0:3ac96e360672 510
charlesmn 0:3ac96e360672 511 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 512
charlesmn 0:3ac96e360672 513 uint8_t i = 0;
charlesmn 0:3ac96e360672 514 int16_t VL53L1_p_015 = 0;
charlesmn 0:3ac96e360672 515 int16_t VL53L1_p_016 = 0;
charlesmn 0:3ac96e360672 516 int16_t window_width = 0;
charlesmn 0:3ac96e360672 517 uint32_t tmp_phase = 0;
charlesmn 0:3ac96e360672 518
charlesmn 0:3ac96e360672 519 VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 520
charlesmn 0:3ac96e360672 521 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 522
charlesmn 0:3ac96e360672 523
charlesmn 0:3ac96e360672 524
charlesmn 0:3ac96e360672 525 i = pdata->VL53L1_p_025 % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 526
charlesmn 0:3ac96e360672 527 VL53L1_p_015 = (int16_t)i;
charlesmn 0:3ac96e360672 528 VL53L1_p_015 += (int16_t)pdata->VL53L1_p_015;
charlesmn 0:3ac96e360672 529 VL53L1_p_015 -= (int16_t)pdata->VL53L1_p_025;
charlesmn 0:3ac96e360672 530
charlesmn 0:3ac96e360672 531 VL53L1_p_016 = (int16_t)i;
charlesmn 0:3ac96e360672 532 VL53L1_p_016 += (int16_t)pdata->VL53L1_p_016;
charlesmn 0:3ac96e360672 533 VL53L1_p_016 -= (int16_t)pdata->VL53L1_p_025;
charlesmn 0:3ac96e360672 534
charlesmn 0:3ac96e360672 535
charlesmn 0:3ac96e360672 536 window_width = VL53L1_p_016 - VL53L1_p_015;
charlesmn 0:3ac96e360672 537 if (window_width > 3)
charlesmn 0:3ac96e360672 538 window_width = 3;
charlesmn 0:3ac96e360672 539
charlesmn 0:3ac96e360672 540 status =
charlesmn 0:3ac96e360672 541 VL53L1_f_030(
charlesmn 0:3ac96e360672 542 VL53L1_p_015,
charlesmn 0:3ac96e360672 543 VL53L1_p_015 + window_width,
charlesmn 0:3ac96e360672 544 palgo->VL53L1_p_031,
charlesmn 0:3ac96e360672 545 clip_events,
charlesmn 0:3ac96e360672 546 pbins,
charlesmn 0:3ac96e360672 547 &(pdata->VL53L1_p_028));
charlesmn 0:3ac96e360672 548
charlesmn 0:3ac96e360672 549
charlesmn 0:3ac96e360672 550 if (status == VL53L1_ERROR_NONE)
charlesmn 0:3ac96e360672 551 status =
charlesmn 0:3ac96e360672 552 VL53L1_f_030(
charlesmn 0:3ac96e360672 553 VL53L1_p_016 - window_width,
charlesmn 0:3ac96e360672 554 VL53L1_p_016,
charlesmn 0:3ac96e360672 555 palgo->VL53L1_p_031,
charlesmn 0:3ac96e360672 556 clip_events,
charlesmn 0:3ac96e360672 557 pbins,
charlesmn 0:3ac96e360672 558 &(pdata->VL53L1_p_029));
charlesmn 0:3ac96e360672 559
charlesmn 0:3ac96e360672 560
charlesmn 0:3ac96e360672 561 if (pdata->VL53L1_p_028 > pdata->VL53L1_p_029) {
charlesmn 0:3ac96e360672 562 tmp_phase = pdata->VL53L1_p_028;
charlesmn 0:3ac96e360672 563 pdata->VL53L1_p_028 = pdata->VL53L1_p_029;
charlesmn 0:3ac96e360672 564 pdata->VL53L1_p_029 = tmp_phase;
charlesmn 0:3ac96e360672 565 }
charlesmn 0:3ac96e360672 566
charlesmn 0:3ac96e360672 567
charlesmn 0:3ac96e360672 568 if (pdata->VL53L1_p_014 < pdata->VL53L1_p_028)
charlesmn 0:3ac96e360672 569 pdata->VL53L1_p_028 = pdata->VL53L1_p_014;
charlesmn 0:3ac96e360672 570
charlesmn 0:3ac96e360672 571
charlesmn 0:3ac96e360672 572 if (pdata->VL53L1_p_014 > pdata->VL53L1_p_029)
charlesmn 0:3ac96e360672 573 pdata->VL53L1_p_029 = pdata->VL53L1_p_014;
charlesmn 0:3ac96e360672 574
charlesmn 0:3ac96e360672 575 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 576
charlesmn 0:3ac96e360672 577 return status;
charlesmn 0:3ac96e360672 578 }
charlesmn 0:3ac96e360672 579
charlesmn 0:3ac96e360672 580
charlesmn 0:3ac96e360672 581 VL53L1_Error VL53L1_f_030(
charlesmn 0:3ac96e360672 582 int16_t VL53L1_p_022,
charlesmn 0:3ac96e360672 583 int16_t VL53L1_p_026,
charlesmn 0:3ac96e360672 584 uint8_t VL53L1_p_031,
charlesmn 0:3ac96e360672 585 uint8_t clip_events,
charlesmn 0:3ac96e360672 586 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 587 uint32_t *pphase)
charlesmn 0:3ac96e360672 588 {
charlesmn 0:3ac96e360672 589
charlesmn 0:3ac96e360672 590
charlesmn 0:3ac96e360672 591 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 592
charlesmn 0:3ac96e360672 593 int16_t i = 0;
charlesmn 0:3ac96e360672 594 int16_t lb = 0;
charlesmn 0:3ac96e360672 595
charlesmn 0:3ac96e360672 596 int64_t VL53L1_p_008 = 0;
charlesmn 0:3ac96e360672 597 int64_t event_sum = 0;
charlesmn 0:3ac96e360672 598 int64_t weighted_sum = 0;
charlesmn 0:3ac96e360672 599
charlesmn 0:3ac96e360672 600 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 601
charlesmn 0:3ac96e360672 602 *pphase = VL53L1_MAX_ALLOWED_PHASE;
charlesmn 0:3ac96e360672 603
charlesmn 0:3ac96e360672 604 for (lb = VL53L1_p_022; lb <= VL53L1_p_026; lb++) {
charlesmn 0:3ac96e360672 605
charlesmn 0:3ac96e360672 606 if (lb < 0)
Charles MacNeill 7:1add29d51e72 607 i = lb + (int16_t)VL53L1_p_031;
charlesmn 0:3ac96e360672 608 else
Charles MacNeill 7:1add29d51e72 609 i = lb % (int16_t)VL53L1_p_031;
charlesmn 0:3ac96e360672 610
Charles MacNeill 7:1add29d51e72 611 if ((i >= 0) && (i < VL53L1_HISTOGRAM_BUFFER_SIZE)) {
Charles MacNeill 7:1add29d51e72 612 VL53L1_p_008 =
Charles MacNeill 7:1add29d51e72 613 (int64_t)pbins->bin_data[i] -
Charles MacNeill 7:1add29d51e72 614 (int64_t)pbins->VL53L1_p_004;
charlesmn 0:3ac96e360672 615
Charles MacNeill 7:1add29d51e72 616 if (clip_events > 0 && VL53L1_p_008 < 0)
Charles MacNeill 7:1add29d51e72 617 VL53L1_p_008 = 0;
Charles MacNeill 7:1add29d51e72 618 event_sum += VL53L1_p_008;
Charles MacNeill 7:1add29d51e72 619 weighted_sum +=
Charles MacNeill 7:1add29d51e72 620 (VL53L1_p_008 * (1024 + (2048*(int64_t)lb)));
Charles MacNeill 7:1add29d51e72 621 }
charlesmn 0:3ac96e360672 622
charlesmn 0:3ac96e360672 623 trace_print(
charlesmn 0:3ac96e360672 624 VL53L1_TRACE_LEVEL_INFO,
charlesmn 0:3ac96e360672 625 "\tb = %5d : i = %5d : VL53L1_p_008 = %8d,",
charlesmn 0:3ac96e360672 626 lb, i, VL53L1_p_008);
charlesmn 0:3ac96e360672 627
charlesmn 0:3ac96e360672 628 trace_print(
charlesmn 0:3ac96e360672 629 VL53L1_TRACE_LEVEL_INFO,
charlesmn 0:3ac96e360672 630 " event_sum = %8d, weighted_sum = %8d\n",
charlesmn 0:3ac96e360672 631 event_sum, weighted_sum);
charlesmn 0:3ac96e360672 632 }
charlesmn 0:3ac96e360672 633
charlesmn 0:3ac96e360672 634 if (event_sum > 0) {
charlesmn 0:3ac96e360672 635 weighted_sum += do_division_s(event_sum, 2);
charlesmn 0:3ac96e360672 636 weighted_sum = do_division_s(weighted_sum, event_sum);
charlesmn 0:3ac96e360672 637 if (weighted_sum < 0)
charlesmn 0:3ac96e360672 638 weighted_sum = 0;
charlesmn 0:3ac96e360672 639 *pphase = (uint32_t)weighted_sum;
charlesmn 0:3ac96e360672 640 }
charlesmn 0:3ac96e360672 641
charlesmn 0:3ac96e360672 642 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 643
charlesmn 0:3ac96e360672 644 return status;
charlesmn 0:3ac96e360672 645 }
charlesmn 0:3ac96e360672 646
charlesmn 0:3ac96e360672 647
charlesmn 0:3ac96e360672 648 VL53L1_Error VL53L1_f_023(
charlesmn 0:3ac96e360672 649 uint8_t pulse_no,
charlesmn 0:3ac96e360672 650 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 651 VL53L1_hist_gen3_algo_private_data_t *palgo,
charlesmn 0:3ac96e360672 652 int32_t pad_value,
charlesmn 0:3ac96e360672 653 VL53L1_histogram_bin_data_t *ppulse)
charlesmn 0:3ac96e360672 654 {
charlesmn 0:3ac96e360672 655
charlesmn 0:3ac96e360672 656
charlesmn 0:3ac96e360672 657
charlesmn 0:3ac96e360672 658 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 659
charlesmn 0:3ac96e360672 660 uint8_t i = 0;
charlesmn 0:3ac96e360672 661 uint8_t lb = 0;
charlesmn 0:3ac96e360672 662
charlesmn 0:3ac96e360672 663 VL53L1_hist_pulse_data_t *pdata = &(palgo->VL53L1_p_002[pulse_no]);
charlesmn 0:3ac96e360672 664
charlesmn 0:3ac96e360672 665 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 666
charlesmn 0:3ac96e360672 667
charlesmn 0:3ac96e360672 668
charlesmn 0:3ac96e360672 669 memcpy(ppulse, pbins, sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 670
charlesmn 0:3ac96e360672 671
charlesmn 0:3ac96e360672 672
charlesmn 0:3ac96e360672 673 for (lb = palgo->VL53L1_p_049;
charlesmn 0:3ac96e360672 674 lb < (palgo->VL53L1_p_049 +
charlesmn 0:3ac96e360672 675 palgo->VL53L1_p_031);
charlesmn 0:3ac96e360672 676 lb++) {
charlesmn 0:3ac96e360672 677
charlesmn 0:3ac96e360672 678 if (lb < pdata->VL53L1_p_015 || lb > pdata->VL53L1_p_016) {
charlesmn 0:3ac96e360672 679 i = lb % palgo->VL53L1_p_031;
charlesmn 0:3ac96e360672 680 if (i < ppulse->VL53L1_p_024)
charlesmn 0:3ac96e360672 681 ppulse->bin_data[i] = pad_value;
charlesmn 0:3ac96e360672 682 }
charlesmn 0:3ac96e360672 683 }
charlesmn 0:3ac96e360672 684
charlesmn 0:3ac96e360672 685 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 686
charlesmn 0:3ac96e360672 687 return status;
charlesmn 0:3ac96e360672 688 }
charlesmn 0:3ac96e360672 689
charlesmn 0:3ac96e360672 690
charlesmn 0:3ac96e360672 691 VL53L1_Error VL53L1_f_026(
charlesmn 0:3ac96e360672 692 uint8_t bin,
charlesmn 0:3ac96e360672 693 uint8_t sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 694 uint8_t VL53L1_p_031,
charlesmn 0:3ac96e360672 695 uint8_t VL53L1_p_055,
charlesmn 0:3ac96e360672 696 uint8_t crosstalk_compensation_enable,
charlesmn 0:3ac96e360672 697 VL53L1_histogram_bin_data_t *phist_data_ap,
charlesmn 0:3ac96e360672 698 VL53L1_histogram_bin_data_t *phist_data_zp,
charlesmn 0:3ac96e360672 699 VL53L1_histogram_bin_data_t *pxtalk_hist,
charlesmn 0:3ac96e360672 700 uint16_t *psigma_est)
charlesmn 0:3ac96e360672 701 {
charlesmn 0:3ac96e360672 702
charlesmn 0:3ac96e360672 703
charlesmn 0:3ac96e360672 704 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 705 VL53L1_Error func_status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 706
charlesmn 0:3ac96e360672 707 uint8_t i = 0;
charlesmn 0:3ac96e360672 708 int32_t VL53L1_p_003 = 0;
charlesmn 0:3ac96e360672 709 int32_t VL53L1_p_018 = 0;
charlesmn 0:3ac96e360672 710 int32_t VL53L1_p_001 = 0;
charlesmn 0:3ac96e360672 711 int32_t a_zp = 0;
charlesmn 0:3ac96e360672 712 int32_t c_zp = 0;
charlesmn 0:3ac96e360672 713 int32_t ax = 0;
charlesmn 0:3ac96e360672 714 int32_t bx = 0;
charlesmn 0:3ac96e360672 715 int32_t cx = 0;
charlesmn 0:3ac96e360672 716
charlesmn 0:3ac96e360672 717
charlesmn 0:3ac96e360672 718
charlesmn 0:3ac96e360672 719 i = bin % VL53L1_p_031;
charlesmn 0:3ac96e360672 720
charlesmn 0:3ac96e360672 721
charlesmn 0:3ac96e360672 722
charlesmn 0:3ac96e360672 723 VL53L1_f_013(
charlesmn 0:3ac96e360672 724 i,
charlesmn 0:3ac96e360672 725 VL53L1_p_055,
charlesmn 0:3ac96e360672 726 phist_data_zp,
charlesmn 0:3ac96e360672 727 &a_zp,
charlesmn 0:3ac96e360672 728 &VL53L1_p_018,
charlesmn 0:3ac96e360672 729 &c_zp);
charlesmn 0:3ac96e360672 730
charlesmn 0:3ac96e360672 731
charlesmn 0:3ac96e360672 732
charlesmn 0:3ac96e360672 733 VL53L1_f_013(
charlesmn 0:3ac96e360672 734 i,
charlesmn 0:3ac96e360672 735 VL53L1_p_055,
charlesmn 0:3ac96e360672 736 phist_data_ap,
charlesmn 0:3ac96e360672 737 &VL53L1_p_003,
charlesmn 0:3ac96e360672 738 &VL53L1_p_018,
charlesmn 0:3ac96e360672 739 &VL53L1_p_001);
charlesmn 0:3ac96e360672 740
charlesmn 0:3ac96e360672 741 if (crosstalk_compensation_enable > 0)
charlesmn 0:3ac96e360672 742 VL53L1_f_013(
charlesmn 0:3ac96e360672 743 i,
charlesmn 0:3ac96e360672 744 VL53L1_p_055,
charlesmn 0:3ac96e360672 745 pxtalk_hist,
charlesmn 0:3ac96e360672 746 &ax,
charlesmn 0:3ac96e360672 747 &bx,
charlesmn 0:3ac96e360672 748 &cx);
charlesmn 0:3ac96e360672 749
charlesmn 0:3ac96e360672 750
charlesmn 0:3ac96e360672 751
charlesmn 0:3ac96e360672 752
charlesmn 0:3ac96e360672 753
charlesmn 0:3ac96e360672 754
charlesmn 0:3ac96e360672 755
charlesmn 0:3ac96e360672 756 func_status =
charlesmn 0:3ac96e360672 757 VL53L1_f_014(
charlesmn 0:3ac96e360672 758 sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 759 (uint32_t)VL53L1_p_003,
charlesmn 0:3ac96e360672 760 (uint32_t)VL53L1_p_018,
charlesmn 0:3ac96e360672 761 (uint32_t)VL53L1_p_001,
charlesmn 0:3ac96e360672 762 (uint32_t)a_zp,
charlesmn 0:3ac96e360672 763 (uint32_t)c_zp,
charlesmn 0:3ac96e360672 764 (uint32_t)bx,
charlesmn 0:3ac96e360672 765 (uint32_t)ax,
charlesmn 0:3ac96e360672 766 (uint32_t)cx,
charlesmn 0:3ac96e360672 767 (uint32_t)phist_data_ap->VL53L1_p_004,
charlesmn 0:3ac96e360672 768 phist_data_ap->VL53L1_p_019,
charlesmn 0:3ac96e360672 769 psigma_est);
charlesmn 0:3ac96e360672 770
charlesmn 0:3ac96e360672 771
charlesmn 0:3ac96e360672 772
charlesmn 0:3ac96e360672 773
charlesmn 0:3ac96e360672 774 if (func_status == VL53L1_ERROR_DIVISION_BY_ZERO)
charlesmn 0:3ac96e360672 775 *psigma_est = 0xFFFF;
charlesmn 0:3ac96e360672 776
charlesmn 0:3ac96e360672 777
charlesmn 0:3ac96e360672 778 return status;
charlesmn 0:3ac96e360672 779 }
charlesmn 0:3ac96e360672 780
charlesmn 0:3ac96e360672 781
charlesmn 0:3ac96e360672 782 void VL53L1_f_029(
charlesmn 0:3ac96e360672 783 uint8_t range_id,
charlesmn 0:3ac96e360672 784 uint8_t valid_phase_low,
charlesmn 0:3ac96e360672 785 uint8_t valid_phase_high,
charlesmn 0:3ac96e360672 786 uint16_t sigma_thres,
charlesmn 0:3ac96e360672 787 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 788 VL53L1_hist_pulse_data_t *ppulse,
charlesmn 0:3ac96e360672 789 VL53L1_range_data_t *pdata)
charlesmn 0:3ac96e360672 790 {
charlesmn 0:3ac96e360672 791
charlesmn 0:3ac96e360672 792 uint16_t lower_phase_limit = 0;
charlesmn 0:3ac96e360672 793 uint16_t upper_phase_limit = 0;
charlesmn 0:3ac96e360672 794
charlesmn 0:3ac96e360672 795
charlesmn 0:3ac96e360672 796
charlesmn 0:3ac96e360672 797 pdata->range_id = range_id;
charlesmn 0:3ac96e360672 798 pdata->time_stamp = 0;
charlesmn 0:3ac96e360672 799
charlesmn 0:3ac96e360672 800 pdata->VL53L1_p_015 = ppulse->VL53L1_p_015;
charlesmn 0:3ac96e360672 801 pdata->VL53L1_p_022 = ppulse->VL53L1_p_022;
charlesmn 0:3ac96e360672 802 pdata->VL53L1_p_025 = ppulse->VL53L1_p_025;
charlesmn 0:3ac96e360672 803 pdata->VL53L1_p_026 = ppulse->VL53L1_p_026;
charlesmn 0:3ac96e360672 804 pdata->VL53L1_p_016 = ppulse->VL53L1_p_016;
charlesmn 0:3ac96e360672 805 pdata->VL53L1_p_027 = ppulse->VL53L1_p_027;
charlesmn 0:3ac96e360672 806
charlesmn 0:3ac96e360672 807
charlesmn 0:3ac96e360672 808
charlesmn 0:3ac96e360672 809 pdata->VL53L1_p_030 =
charlesmn 0:3ac96e360672 810 (ppulse->VL53L1_p_016 + 1) - ppulse->VL53L1_p_015;
charlesmn 0:3ac96e360672 811
charlesmn 0:3ac96e360672 812
charlesmn 0:3ac96e360672 813
charlesmn 0:3ac96e360672 814 pdata->zero_distance_phase = pbins->zero_distance_phase;
charlesmn 0:3ac96e360672 815 pdata->VL53L1_p_005 = ppulse->VL53L1_p_005;
charlesmn 0:3ac96e360672 816 pdata->VL53L1_p_028 = (uint16_t)ppulse->VL53L1_p_028;
charlesmn 0:3ac96e360672 817 pdata->VL53L1_p_014 = (uint16_t)ppulse->VL53L1_p_014;
charlesmn 0:3ac96e360672 818 pdata->VL53L1_p_029 = (uint16_t)ppulse->VL53L1_p_029;
charlesmn 0:3ac96e360672 819 pdata->VL53L1_p_021 = (uint32_t)ppulse->VL53L1_p_021;
charlesmn 0:3ac96e360672 820 pdata->VL53L1_p_013 = ppulse->VL53L1_p_013;
charlesmn 0:3ac96e360672 821 pdata->VL53L1_p_020 = (uint32_t)ppulse->VL53L1_p_020;
charlesmn 0:3ac96e360672 822 pdata->total_periods_elapsed = pbins->total_periods_elapsed;
charlesmn 0:3ac96e360672 823
charlesmn 0:3ac96e360672 824
charlesmn 0:3ac96e360672 825
charlesmn 0:3ac96e360672 826 pdata->range_status = VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK;
charlesmn 0:3ac96e360672 827
charlesmn 0:3ac96e360672 828
charlesmn 0:3ac96e360672 829 if (sigma_thres > 0 &&
charlesmn 0:3ac96e360672 830 (uint32_t)ppulse->VL53L1_p_005 > ((uint32_t)sigma_thres << 5))
charlesmn 0:3ac96e360672 831 pdata->range_status = VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK;
charlesmn 0:3ac96e360672 832
charlesmn 0:3ac96e360672 833
charlesmn 0:3ac96e360672 834
charlesmn 0:3ac96e360672 835 lower_phase_limit = (uint8_t)valid_phase_low << 8;
charlesmn 0:3ac96e360672 836 if (lower_phase_limit < pdata->zero_distance_phase)
charlesmn 0:3ac96e360672 837 lower_phase_limit =
charlesmn 0:3ac96e360672 838 pdata->zero_distance_phase -
charlesmn 0:3ac96e360672 839 lower_phase_limit;
charlesmn 0:3ac96e360672 840 else
charlesmn 0:3ac96e360672 841 lower_phase_limit = 0;
charlesmn 0:3ac96e360672 842
charlesmn 0:3ac96e360672 843 upper_phase_limit = (uint8_t)valid_phase_high << 8;
charlesmn 0:3ac96e360672 844 upper_phase_limit += pbins->zero_distance_phase;
charlesmn 0:3ac96e360672 845
charlesmn 0:3ac96e360672 846 if (pdata->VL53L1_p_014 < lower_phase_limit ||
charlesmn 0:3ac96e360672 847 pdata->VL53L1_p_014 > upper_phase_limit)
charlesmn 0:3ac96e360672 848 pdata->range_status = VL53L1_DEVICEERROR_RANGEPHASECHECK;
charlesmn 0:3ac96e360672 849
charlesmn 0:3ac96e360672 850 }
charlesmn 0:3ac96e360672 851
charlesmn 0:3ac96e360672 852