ST Expansion SW Team / VL53L3CX_mbed

Rename library

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L3CX_NoShield_1Sensor_poll_Mb06x VL53L3_NoShield_1Sensor_polling_Mb63 X_NUCLEO_53L3A2 53L3A2_Ranging

modules/vl53lx_hist_algos_gen3.c@5:89031b2f5316, 2021-07-14 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 5:89031b2f5316 1
Charles MacNeill 5:89031b2f5316 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 5:89031b2f5316 3 /******************************************************************************
Charles MacNeill 5:89031b2f5316 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 5:89031b2f5316 5
Charles MacNeill 5:89031b2f5316 6 This file is part of VL53LX Protected and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 5:89031b2f5316 9
Charles MacNeill 5:89031b2f5316 10 ******************************************************************************
Charles MacNeill 5:89031b2f5316 11
Charles MacNeill 5:89031b2f5316 12 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14 ******************************************************************************
Charles MacNeill 5:89031b2f5316 15
Charles MacNeill 5:89031b2f5316 16 License terms: STMicroelectronics Proprietary in accordance with licensing
Charles MacNeill 5:89031b2f5316 17 terms at www.st.com/sla0081
Charles MacNeill 5:89031b2f5316 18
Charles MacNeill 5:89031b2f5316 19 ******************************************************************************
Charles MacNeill 5:89031b2f5316 20 */
Charles MacNeill 5:89031b2f5316 21
Charles MacNeill 5:89031b2f5316 22
Charles MacNeill 5:89031b2f5316 23
Charles MacNeill 5:89031b2f5316 24
Charles MacNeill 5:89031b2f5316 25 #include <vl53lx_platform_log.h>
Charles MacNeill 5:89031b2f5316 26 #include <vl53lx_types.h>
Charles MacNeill 5:89031b2f5316 27 #include "vl53lx_core_support.h"
Charles MacNeill 5:89031b2f5316 28 #include "vl53lx_error_codes.h"
Charles MacNeill 5:89031b2f5316 29
Charles MacNeill 5:89031b2f5316 30 #include "vl53lx_hist_core.h"
Charles MacNeill 5:89031b2f5316 31 #include "vl53lx_hist_algos_gen3.h"
Charles MacNeill 5:89031b2f5316 32 #include "vl53lx_sigma_estimate.h"
Charles MacNeill 5:89031b2f5316 33 #include "vl53lx_dmax.h"
Charles MacNeill 5:89031b2f5316 34
Charles MacNeill 5:89031b2f5316 35
Charles MacNeill 5:89031b2f5316 36
Charles MacNeill 5:89031b2f5316 37 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 38 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 39 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 40 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 41 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 42 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_HISTOGRAM,\
Charles MacNeill 5:89031b2f5316 43 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 44
Charles MacNeill 5:89031b2f5316 45 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 46 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_HISTOGRAM, \
Charles MacNeill 5:89031b2f5316 47 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 48
Charles MacNeill 5:89031b2f5316 49
Charles MacNeill 5:89031b2f5316 50 void VL53LX_f_003(
Charles MacNeill 5:89031b2f5316 51 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 52 {
Charles MacNeill 5:89031b2f5316 53
Charles MacNeill 5:89031b2f5316 54
Charles MacNeill 5:89031b2f5316 55 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 56
Charles MacNeill 5:89031b2f5316 57 palgo->VL53LX_p_020 = VL53LX_HISTOGRAM_BUFFER_SIZE;
Charles MacNeill 5:89031b2f5316 58 palgo->VL53LX_p_019 = 0;
Charles MacNeill 5:89031b2f5316 59 palgo->VL53LX_p_021 = 0;
Charles MacNeill 5:89031b2f5316 60 palgo->VL53LX_p_039 = 0;
Charles MacNeill 5:89031b2f5316 61 palgo->VL53LX_p_028 = 0;
Charles MacNeill 5:89031b2f5316 62 palgo->VL53LX_p_031 = 0;
Charles MacNeill 5:89031b2f5316 63
Charles MacNeill 5:89031b2f5316 64 for (lb = palgo->VL53LX_p_019; lb < palgo->VL53LX_p_020; lb++) {
Charles MacNeill 5:89031b2f5316 65 palgo->VL53LX_p_040[lb] = 0;
Charles MacNeill 5:89031b2f5316 66 palgo->VL53LX_p_041[lb] = 0;
Charles MacNeill 5:89031b2f5316 67 palgo->VL53LX_p_042[lb] = 0;
Charles MacNeill 5:89031b2f5316 68 palgo->VL53LX_p_043[lb] = 0;
Charles MacNeill 5:89031b2f5316 69 palgo->VL53LX_p_018[lb] = 0;
Charles MacNeill 5:89031b2f5316 70 }
Charles MacNeill 5:89031b2f5316 71
Charles MacNeill 5:89031b2f5316 72 palgo->VL53LX_p_044 = 0;
Charles MacNeill 5:89031b2f5316 73 palgo->VL53LX_p_045 = VL53LX_D_001;
Charles MacNeill 5:89031b2f5316 74 palgo->VL53LX_p_046 = 0;
Charles MacNeill 5:89031b2f5316 75
Charles MacNeill 5:89031b2f5316 76
Charles MacNeill 5:89031b2f5316 77
Charles MacNeill 5:89031b2f5316 78 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 79 0,
Charles MacNeill 5:89031b2f5316 80 VL53LX_HISTOGRAM_BUFFER_SIZE,
Charles MacNeill 5:89031b2f5316 81 &(palgo->VL53LX_p_006));
Charles MacNeill 5:89031b2f5316 82 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 83 0,
Charles MacNeill 5:89031b2f5316 84 VL53LX_HISTOGRAM_BUFFER_SIZE,
Charles MacNeill 5:89031b2f5316 85 &(palgo->VL53LX_p_047));
Charles MacNeill 5:89031b2f5316 86 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 87 0,
Charles MacNeill 5:89031b2f5316 88 VL53LX_HISTOGRAM_BUFFER_SIZE,
Charles MacNeill 5:89031b2f5316 89 &(palgo->VL53LX_p_048));
Charles MacNeill 5:89031b2f5316 90 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 91 0,
Charles MacNeill 5:89031b2f5316 92 VL53LX_HISTOGRAM_BUFFER_SIZE,
Charles MacNeill 5:89031b2f5316 93 &(palgo->VL53LX_p_049));
Charles MacNeill 5:89031b2f5316 94 VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 95 0,
Charles MacNeill 5:89031b2f5316 96 VL53LX_HISTOGRAM_BUFFER_SIZE,
Charles MacNeill 5:89031b2f5316 97 &(palgo->VL53LX_p_050));
Charles MacNeill 5:89031b2f5316 98 }
Charles MacNeill 5:89031b2f5316 99
Charles MacNeill 5:89031b2f5316 100
Charles MacNeill 5:89031b2f5316 101
Charles MacNeill 5:89031b2f5316 102 VL53LX_Error VL53LX_f_006(
Charles MacNeill 5:89031b2f5316 103 uint16_t ambient_threshold_events_scaler,
Charles MacNeill 5:89031b2f5316 104 int32_t ambient_threshold_sigma,
Charles MacNeill 5:89031b2f5316 105 int32_t min_ambient_threshold_events,
Charles MacNeill 5:89031b2f5316 106 uint8_t algo__crosstalk_compensation_enable,
Charles MacNeill 5:89031b2f5316 107 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 108 VL53LX_histogram_bin_data_t *pxtalk,
Charles MacNeill 5:89031b2f5316 109 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 110 {
Charles MacNeill 5:89031b2f5316 111
Charles MacNeill 5:89031b2f5316 112
Charles MacNeill 5:89031b2f5316 113
Charles MacNeill 5:89031b2f5316 114 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 115 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 116 uint8_t VL53LX_p_001 = 0;
Charles MacNeill 5:89031b2f5316 117 int64_t tmp = 0;
Charles MacNeill 5:89031b2f5316 118 int32_t amb_events = 0;
Charles MacNeill 5:89031b2f5316 119 int32_t VL53LX_p_018 = 0;
Charles MacNeill 5:89031b2f5316 120 int32_t samples = 0;
Charles MacNeill 5:89031b2f5316 121
Charles MacNeill 5:89031b2f5316 122 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 123
Charles MacNeill 5:89031b2f5316 124
Charles MacNeill 5:89031b2f5316 125 palgo->VL53LX_p_020 = pbins->VL53LX_p_020;
Charles MacNeill 5:89031b2f5316 126 palgo->VL53LX_p_019 = pbins->VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 127 palgo->VL53LX_p_021 = pbins->VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 128 palgo->VL53LX_p_028 = pbins->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 129
Charles MacNeill 5:89031b2f5316 130
Charles MacNeill 5:89031b2f5316 131
Charles MacNeill 5:89031b2f5316 132 palgo->VL53LX_p_030 =
Charles MacNeill 5:89031b2f5316 133 VL53LX_decode_vcsel_period(pbins->VL53LX_p_005);
Charles MacNeill 5:89031b2f5316 134
Charles MacNeill 5:89031b2f5316 135
Charles MacNeill 5:89031b2f5316 136
Charles MacNeill 5:89031b2f5316 137 tmp = (int64_t)pbins->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 138 tmp *= (int64_t)ambient_threshold_events_scaler;
Charles MacNeill 5:89031b2f5316 139 tmp += 2048;
Charles MacNeill 5:89031b2f5316 140 tmp = do_division_s(tmp, 4096);
Charles MacNeill 5:89031b2f5316 141 amb_events = (int32_t)tmp;
Charles MacNeill 5:89031b2f5316 142
Charles MacNeill 5:89031b2f5316 143
Charles MacNeill 5:89031b2f5316 144
Charles MacNeill 5:89031b2f5316 145 for (lb = 0; lb < pbins->VL53LX_p_021; lb++) {
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147 VL53LX_p_001 = lb >> 2;
Charles MacNeill 5:89031b2f5316 148 samples = (int32_t)pbins->bin_rep[VL53LX_p_001];
Charles MacNeill 5:89031b2f5316 149
Charles MacNeill 5:89031b2f5316 150 if (samples > 0) {
Charles MacNeill 5:89031b2f5316 151
Charles MacNeill 5:89031b2f5316 152 if (lb < pxtalk->VL53LX_p_021 &&
Charles MacNeill 5:89031b2f5316 153 algo__crosstalk_compensation_enable > 0)
Charles MacNeill 5:89031b2f5316 154 VL53LX_p_018 = samples * (amb_events +
Charles MacNeill 5:89031b2f5316 155 pxtalk->bin_data[lb]);
Charles MacNeill 5:89031b2f5316 156 else
Charles MacNeill 5:89031b2f5316 157 VL53LX_p_018 = samples * amb_events;
Charles MacNeill 5:89031b2f5316 158
Charles MacNeill 5:89031b2f5316 159 VL53LX_p_018 = VL53LX_isqrt(VL53LX_p_018);
Charles MacNeill 5:89031b2f5316 160
Charles MacNeill 5:89031b2f5316 161 VL53LX_p_018 += (samples/2);
Charles MacNeill 5:89031b2f5316 162 VL53LX_p_018 /= samples;
Charles MacNeill 5:89031b2f5316 163 VL53LX_p_018 *= ambient_threshold_sigma;
Charles MacNeill 5:89031b2f5316 164 VL53LX_p_018 += 8;
Charles MacNeill 5:89031b2f5316 165 VL53LX_p_018 /= 16;
Charles MacNeill 5:89031b2f5316 166 VL53LX_p_018 += amb_events;
Charles MacNeill 5:89031b2f5316 167
Charles MacNeill 5:89031b2f5316 168 if (VL53LX_p_018 < min_ambient_threshold_events)
Charles MacNeill 5:89031b2f5316 169 VL53LX_p_018 = min_ambient_threshold_events;
Charles MacNeill 5:89031b2f5316 170
Charles MacNeill 5:89031b2f5316 171 palgo->VL53LX_p_052[lb] = VL53LX_p_018;
Charles MacNeill 5:89031b2f5316 172 palgo->VL53LX_p_031 = VL53LX_p_018;
Charles MacNeill 5:89031b2f5316 173 }
Charles MacNeill 5:89031b2f5316 174
Charles MacNeill 5:89031b2f5316 175
Charles MacNeill 5:89031b2f5316 176
Charles MacNeill 5:89031b2f5316 177 }
Charles MacNeill 5:89031b2f5316 178
Charles MacNeill 5:89031b2f5316 179
Charles MacNeill 5:89031b2f5316 180
Charles MacNeill 5:89031b2f5316 181 palgo->VL53LX_p_039 = 0;
Charles MacNeill 5:89031b2f5316 182
Charles MacNeill 5:89031b2f5316 183 for (lb = pbins->VL53LX_p_019; lb < pbins->VL53LX_p_021; lb++) {
Charles MacNeill 5:89031b2f5316 184
Charles MacNeill 5:89031b2f5316 185 if (pbins->bin_data[lb] > palgo->VL53LX_p_052[lb]) {
Charles MacNeill 5:89031b2f5316 186 palgo->VL53LX_p_040[lb] = 1;
Charles MacNeill 5:89031b2f5316 187 palgo->VL53LX_p_041[lb] = 1;
Charles MacNeill 5:89031b2f5316 188 palgo->VL53LX_p_039++;
Charles MacNeill 5:89031b2f5316 189 } else {
Charles MacNeill 5:89031b2f5316 190 palgo->VL53LX_p_040[lb] = 0;
Charles MacNeill 5:89031b2f5316 191 palgo->VL53LX_p_041[lb] = 0;
Charles MacNeill 5:89031b2f5316 192 }
Charles MacNeill 5:89031b2f5316 193 }
Charles MacNeill 5:89031b2f5316 194
Charles MacNeill 5:89031b2f5316 195 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 196
Charles MacNeill 5:89031b2f5316 197 return status;
Charles MacNeill 5:89031b2f5316 198
Charles MacNeill 5:89031b2f5316 199 }
Charles MacNeill 5:89031b2f5316 200
Charles MacNeill 5:89031b2f5316 201
Charles MacNeill 5:89031b2f5316 202
Charles MacNeill 5:89031b2f5316 203
Charles MacNeill 5:89031b2f5316 204 VL53LX_Error VL53LX_f_007(
Charles MacNeill 5:89031b2f5316 205 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 206 {
Charles MacNeill 5:89031b2f5316 207
Charles MacNeill 5:89031b2f5316 208
Charles MacNeill 5:89031b2f5316 209
Charles MacNeill 5:89031b2f5316 210 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 211
Charles MacNeill 5:89031b2f5316 212 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 213 uint8_t j = 0;
Charles MacNeill 5:89031b2f5316 214 uint8_t found = 0;
Charles MacNeill 5:89031b2f5316 215
Charles MacNeill 5:89031b2f5316 216 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 217
Charles MacNeill 5:89031b2f5316 218 palgo->VL53LX_p_044 = 0;
Charles MacNeill 5:89031b2f5316 219
Charles MacNeill 5:89031b2f5316 220 for (i = 0; i < palgo->VL53LX_p_030; i++) {
Charles MacNeill 5:89031b2f5316 221
Charles MacNeill 5:89031b2f5316 222 j = (i + 1) % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 223
Charles MacNeill 5:89031b2f5316 224
Charles MacNeill 5:89031b2f5316 225
Charles MacNeill 5:89031b2f5316 226 if (i < palgo->VL53LX_p_021 && j < palgo->VL53LX_p_021) {
Charles MacNeill 5:89031b2f5316 227 if (palgo->VL53LX_p_041[i] == 0 &&
Charles MacNeill 5:89031b2f5316 228 palgo->VL53LX_p_041[j] == 1 &&
Charles MacNeill 5:89031b2f5316 229 found == 0) {
Charles MacNeill 5:89031b2f5316 230 palgo->VL53LX_p_044 = i;
Charles MacNeill 5:89031b2f5316 231 found = 1;
Charles MacNeill 5:89031b2f5316 232 }
Charles MacNeill 5:89031b2f5316 233 }
Charles MacNeill 5:89031b2f5316 234 }
Charles MacNeill 5:89031b2f5316 235
Charles MacNeill 5:89031b2f5316 236 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 237
Charles MacNeill 5:89031b2f5316 238 return status;
Charles MacNeill 5:89031b2f5316 239 }
Charles MacNeill 5:89031b2f5316 240
Charles MacNeill 5:89031b2f5316 241
Charles MacNeill 5:89031b2f5316 242 VL53LX_Error VL53LX_f_008(
Charles MacNeill 5:89031b2f5316 243 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 244 {
Charles MacNeill 5:89031b2f5316 245
Charles MacNeill 5:89031b2f5316 246
Charles MacNeill 5:89031b2f5316 247
Charles MacNeill 5:89031b2f5316 248 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 249 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 250 uint8_t j = 0;
Charles MacNeill 5:89031b2f5316 251 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 252
Charles MacNeill 5:89031b2f5316 253 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 254
Charles MacNeill 5:89031b2f5316 255 for (lb = palgo->VL53LX_p_044;
Charles MacNeill 5:89031b2f5316 256 lb < (palgo->VL53LX_p_044 +
Charles MacNeill 5:89031b2f5316 257 palgo->VL53LX_p_030);
Charles MacNeill 5:89031b2f5316 258 lb++) {
Charles MacNeill 5:89031b2f5316 259
Charles MacNeill 5:89031b2f5316 260
Charles MacNeill 5:89031b2f5316 261
Charles MacNeill 5:89031b2f5316 262 i = lb % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 263 j = (lb + 1) % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 264
Charles MacNeill 5:89031b2f5316 265
Charles MacNeill 5:89031b2f5316 266
Charles MacNeill 5:89031b2f5316 267 if (i < palgo->VL53LX_p_021 && j < palgo->VL53LX_p_021) {
Charles MacNeill 5:89031b2f5316 268
Charles MacNeill 5:89031b2f5316 269 if (palgo->VL53LX_p_041[i] == 0 &&
Charles MacNeill 5:89031b2f5316 270 palgo->VL53LX_p_041[j] == 1)
Charles MacNeill 5:89031b2f5316 271 palgo->VL53LX_p_046++;
Charles MacNeill 5:89031b2f5316 272
Charles MacNeill 5:89031b2f5316 273 if (palgo->VL53LX_p_041[i] > 0)
Charles MacNeill 5:89031b2f5316 274 palgo->VL53LX_p_042[i] = palgo->VL53LX_p_046;
Charles MacNeill 5:89031b2f5316 275 else
Charles MacNeill 5:89031b2f5316 276 palgo->VL53LX_p_042[i] = 0;
Charles MacNeill 5:89031b2f5316 277 }
Charles MacNeill 5:89031b2f5316 278
Charles MacNeill 5:89031b2f5316 279 }
Charles MacNeill 5:89031b2f5316 280
Charles MacNeill 5:89031b2f5316 281
Charles MacNeill 5:89031b2f5316 282 if (palgo->VL53LX_p_046 > palgo->VL53LX_p_045)
Charles MacNeill 5:89031b2f5316 283 palgo->VL53LX_p_046 = palgo->VL53LX_p_045;
Charles MacNeill 5:89031b2f5316 284
Charles MacNeill 5:89031b2f5316 285 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 286
Charles MacNeill 5:89031b2f5316 287 return status;
Charles MacNeill 5:89031b2f5316 288
Charles MacNeill 5:89031b2f5316 289 }
Charles MacNeill 5:89031b2f5316 290
Charles MacNeill 5:89031b2f5316 291
Charles MacNeill 5:89031b2f5316 292 VL53LX_Error VL53LX_f_009(
Charles MacNeill 5:89031b2f5316 293 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 294 {
Charles MacNeill 5:89031b2f5316 295
Charles MacNeill 5:89031b2f5316 296
Charles MacNeill 5:89031b2f5316 297
Charles MacNeill 5:89031b2f5316 298 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 299
Charles MacNeill 5:89031b2f5316 300 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 301 uint8_t j = 0;
Charles MacNeill 5:89031b2f5316 302 uint8_t blb = 0;
Charles MacNeill 5:89031b2f5316 303 uint8_t pulse_no = 0;
Charles MacNeill 5:89031b2f5316 304
Charles MacNeill 5:89031b2f5316 305 uint8_t max_filter_half_width = 0;
Charles MacNeill 5:89031b2f5316 306
Charles MacNeill 5:89031b2f5316 307 VL53LX_hist_pulse_data_t *pdata;
Charles MacNeill 5:89031b2f5316 308
Charles MacNeill 5:89031b2f5316 309 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 310
Charles MacNeill 5:89031b2f5316 311
Charles MacNeill 5:89031b2f5316 312
Charles MacNeill 5:89031b2f5316 313 max_filter_half_width = palgo->VL53LX_p_030 - 1;
Charles MacNeill 5:89031b2f5316 314 max_filter_half_width = max_filter_half_width >> 1;
Charles MacNeill 5:89031b2f5316 315
Charles MacNeill 5:89031b2f5316 316 for (blb = palgo->VL53LX_p_044;
Charles MacNeill 5:89031b2f5316 317 blb < (palgo->VL53LX_p_044 +
Charles MacNeill 5:89031b2f5316 318 palgo->VL53LX_p_030);
Charles MacNeill 5:89031b2f5316 319 blb++) {
Charles MacNeill 5:89031b2f5316 320
Charles MacNeill 5:89031b2f5316 321
Charles MacNeill 5:89031b2f5316 322
Charles MacNeill 5:89031b2f5316 323 i = blb % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 324 j = (blb + 1) % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 325
Charles MacNeill 5:89031b2f5316 326
Charles MacNeill 5:89031b2f5316 327
Charles MacNeill 5:89031b2f5316 328 if (i < palgo->VL53LX_p_021 &&
Charles MacNeill 5:89031b2f5316 329 j < palgo->VL53LX_p_021) {
Charles MacNeill 5:89031b2f5316 330
Charles MacNeill 5:89031b2f5316 331
Charles MacNeill 5:89031b2f5316 332
Charles MacNeill 5:89031b2f5316 333 if (palgo->VL53LX_p_042[i] == 0 &&
Charles MacNeill 5:89031b2f5316 334 palgo->VL53LX_p_042[j] > 0) {
Charles MacNeill 5:89031b2f5316 335
Charles MacNeill 5:89031b2f5316 336 pulse_no = palgo->VL53LX_p_042[j] - 1;
Charles MacNeill 5:89031b2f5316 337 pdata = &(palgo->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 338
Charles MacNeill 5:89031b2f5316 339 if (pulse_no < palgo->VL53LX_p_045) {
Charles MacNeill 5:89031b2f5316 340 pdata->VL53LX_p_012 = blb;
Charles MacNeill 5:89031b2f5316 341 pdata->VL53LX_p_019 = blb + 1;
Charles MacNeill 5:89031b2f5316 342 pdata->VL53LX_p_023 = 0xFF;
Charles MacNeill 5:89031b2f5316 343 pdata->VL53LX_p_024 = 0;
Charles MacNeill 5:89031b2f5316 344 pdata->VL53LX_p_013 = 0;
Charles MacNeill 5:89031b2f5316 345 }
Charles MacNeill 5:89031b2f5316 346 }
Charles MacNeill 5:89031b2f5316 347
Charles MacNeill 5:89031b2f5316 348
Charles MacNeill 5:89031b2f5316 349
Charles MacNeill 5:89031b2f5316 350 if (palgo->VL53LX_p_042[i] > 0
Charles MacNeill 5:89031b2f5316 351 && palgo->VL53LX_p_042[j] == 0) {
Charles MacNeill 5:89031b2f5316 352
Charles MacNeill 5:89031b2f5316 353 pulse_no = palgo->VL53LX_p_042[i] - 1;
Charles MacNeill 5:89031b2f5316 354 pdata = &(palgo->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 355
Charles MacNeill 5:89031b2f5316 356 if (pulse_no < palgo->VL53LX_p_045) {
Charles MacNeill 5:89031b2f5316 357
Charles MacNeill 5:89031b2f5316 358 pdata->VL53LX_p_024 = blb;
Charles MacNeill 5:89031b2f5316 359 pdata->VL53LX_p_013 = blb + 1;
Charles MacNeill 5:89031b2f5316 360
Charles MacNeill 5:89031b2f5316 361 pdata->VL53LX_p_025 =
Charles MacNeill 5:89031b2f5316 362 (pdata->VL53LX_p_024 + 1) -
Charles MacNeill 5:89031b2f5316 363 pdata->VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 364 pdata->VL53LX_p_051 =
Charles MacNeill 5:89031b2f5316 365 (pdata->VL53LX_p_013 + 1) -
Charles MacNeill 5:89031b2f5316 366 pdata->VL53LX_p_012;
Charles MacNeill 5:89031b2f5316 367
Charles MacNeill 5:89031b2f5316 368 if (pdata->VL53LX_p_051 >
Charles MacNeill 5:89031b2f5316 369 max_filter_half_width)
Charles MacNeill 5:89031b2f5316 370 pdata->VL53LX_p_051 =
Charles MacNeill 5:89031b2f5316 371 max_filter_half_width;
Charles MacNeill 5:89031b2f5316 372 }
Charles MacNeill 5:89031b2f5316 373
Charles MacNeill 5:89031b2f5316 374 }
Charles MacNeill 5:89031b2f5316 375 }
Charles MacNeill 5:89031b2f5316 376 }
Charles MacNeill 5:89031b2f5316 377
Charles MacNeill 5:89031b2f5316 378 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 379
Charles MacNeill 5:89031b2f5316 380 return status;
Charles MacNeill 5:89031b2f5316 381
Charles MacNeill 5:89031b2f5316 382 }
Charles MacNeill 5:89031b2f5316 383
Charles MacNeill 5:89031b2f5316 384
Charles MacNeill 5:89031b2f5316 385 VL53LX_Error VL53LX_f_016(
Charles MacNeill 5:89031b2f5316 386 VL53LX_HistTargetOrder target_order,
Charles MacNeill 5:89031b2f5316 387 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 388 {
Charles MacNeill 5:89031b2f5316 389
Charles MacNeill 5:89031b2f5316 390
Charles MacNeill 5:89031b2f5316 391
Charles MacNeill 5:89031b2f5316 392 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 393
Charles MacNeill 5:89031b2f5316 394 VL53LX_hist_pulse_data_t tmp;
Charles MacNeill 5:89031b2f5316 395 VL53LX_hist_pulse_data_t *ptmp = &tmp;
Charles MacNeill 5:89031b2f5316 396 VL53LX_hist_pulse_data_t *p0;
Charles MacNeill 5:89031b2f5316 397 VL53LX_hist_pulse_data_t *p1;
Charles MacNeill 5:89031b2f5316 398
Charles MacNeill 5:89031b2f5316 399 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 400 uint8_t swapped = 1;
Charles MacNeill 5:89031b2f5316 401
Charles MacNeill 5:89031b2f5316 402 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 403
Charles MacNeill 5:89031b2f5316 404 if (!(palgo->VL53LX_p_046 > 1))
Charles MacNeill 5:89031b2f5316 405 goto ENDFUNC;
Charles MacNeill 5:89031b2f5316 406
Charles MacNeill 5:89031b2f5316 407 while (swapped > 0) {
Charles MacNeill 5:89031b2f5316 408
Charles MacNeill 5:89031b2f5316 409 swapped = 0;
Charles MacNeill 5:89031b2f5316 410
Charles MacNeill 5:89031b2f5316 411 for (i = 1; i < palgo->VL53LX_p_046; i++) {
Charles MacNeill 5:89031b2f5316 412
Charles MacNeill 5:89031b2f5316 413 p0 = &(palgo->VL53LX_p_003[i-1]);
Charles MacNeill 5:89031b2f5316 414 p1 = &(palgo->VL53LX_p_003[i]);
Charles MacNeill 5:89031b2f5316 415
Charles MacNeill 5:89031b2f5316 416
Charles MacNeill 5:89031b2f5316 417
Charles MacNeill 5:89031b2f5316 418 if (target_order
Charles MacNeill 5:89031b2f5316 419 == VL53LX_HIST_TARGET_ORDER__STRONGEST_FIRST) {
Charles MacNeill 5:89031b2f5316 420
Charles MacNeill 5:89031b2f5316 421 if (p0->VL53LX_p_010 <
Charles MacNeill 5:89031b2f5316 422 p1->VL53LX_p_010) {
Charles MacNeill 5:89031b2f5316 423
Charles MacNeill 5:89031b2f5316 424
Charles MacNeill 5:89031b2f5316 425
Charles MacNeill 5:89031b2f5316 426 memcpy(ptmp,
Charles MacNeill 5:89031b2f5316 427 p1, sizeof(VL53LX_hist_pulse_data_t));
Charles MacNeill 5:89031b2f5316 428 memcpy(p1,
Charles MacNeill 5:89031b2f5316 429 p0, sizeof(VL53LX_hist_pulse_data_t));
Charles MacNeill 5:89031b2f5316 430 memcpy(p0,
Charles MacNeill 5:89031b2f5316 431 ptmp, sizeof(VL53LX_hist_pulse_data_t));
Charles MacNeill 5:89031b2f5316 432
Charles MacNeill 5:89031b2f5316 433 swapped = 1;
Charles MacNeill 5:89031b2f5316 434 }
Charles MacNeill 5:89031b2f5316 435
Charles MacNeill 5:89031b2f5316 436 } else {
Charles MacNeill 5:89031b2f5316 437
Charles MacNeill 5:89031b2f5316 438 if (p0->VL53LX_p_011 > p1->VL53LX_p_011) {
Charles MacNeill 5:89031b2f5316 439
Charles MacNeill 5:89031b2f5316 440
Charles MacNeill 5:89031b2f5316 441
Charles MacNeill 5:89031b2f5316 442 memcpy(ptmp,
Charles MacNeill 5:89031b2f5316 443 p1, sizeof(VL53LX_hist_pulse_data_t));
Charles MacNeill 5:89031b2f5316 444 memcpy(p1,
Charles MacNeill 5:89031b2f5316 445 p0, sizeof(VL53LX_hist_pulse_data_t));
Charles MacNeill 5:89031b2f5316 446 memcpy(p0,
Charles MacNeill 5:89031b2f5316 447 ptmp, sizeof(VL53LX_hist_pulse_data_t));
Charles MacNeill 5:89031b2f5316 448
Charles MacNeill 5:89031b2f5316 449 swapped = 1;
Charles MacNeill 5:89031b2f5316 450 }
Charles MacNeill 5:89031b2f5316 451
Charles MacNeill 5:89031b2f5316 452 }
Charles MacNeill 5:89031b2f5316 453 }
Charles MacNeill 5:89031b2f5316 454 }
Charles MacNeill 5:89031b2f5316 455
Charles MacNeill 5:89031b2f5316 456 ENDFUNC:
Charles MacNeill 5:89031b2f5316 457 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 458
Charles MacNeill 5:89031b2f5316 459 return status;
Charles MacNeill 5:89031b2f5316 460
Charles MacNeill 5:89031b2f5316 461 }
Charles MacNeill 5:89031b2f5316 462
Charles MacNeill 5:89031b2f5316 463
Charles MacNeill 5:89031b2f5316 464 VL53LX_Error VL53LX_f_010(
Charles MacNeill 5:89031b2f5316 465 uint8_t pulse_no,
Charles MacNeill 5:89031b2f5316 466 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 467 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 468 {
Charles MacNeill 5:89031b2f5316 469
Charles MacNeill 5:89031b2f5316 470
Charles MacNeill 5:89031b2f5316 471 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 472
Charles MacNeill 5:89031b2f5316 473 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 474 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 475
Charles MacNeill 5:89031b2f5316 476 VL53LX_hist_pulse_data_t *pdata = &(palgo->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 477
Charles MacNeill 5:89031b2f5316 478 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 479
Charles MacNeill 5:89031b2f5316 480
Charles MacNeill 5:89031b2f5316 481
Charles MacNeill 5:89031b2f5316 482 pdata->VL53LX_p_017 = 0;
Charles MacNeill 5:89031b2f5316 483 pdata->VL53LX_p_016 = 0;
Charles MacNeill 5:89031b2f5316 484
Charles MacNeill 5:89031b2f5316 485 for (lb = pdata->VL53LX_p_012; lb <= pdata->VL53LX_p_013; lb++) {
Charles MacNeill 5:89031b2f5316 486 i = lb % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 487 pdata->VL53LX_p_017 += pbins->bin_data[i];
Charles MacNeill 5:89031b2f5316 488 pdata->VL53LX_p_016 += palgo->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 489 }
Charles MacNeill 5:89031b2f5316 490
Charles MacNeill 5:89031b2f5316 491
Charles MacNeill 5:89031b2f5316 492
Charles MacNeill 5:89031b2f5316 493 pdata->VL53LX_p_010 =
Charles MacNeill 5:89031b2f5316 494 pdata->VL53LX_p_017 - pdata->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 495
Charles MacNeill 5:89031b2f5316 496 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 497
Charles MacNeill 5:89031b2f5316 498 return status;
Charles MacNeill 5:89031b2f5316 499 }
Charles MacNeill 5:89031b2f5316 500
Charles MacNeill 5:89031b2f5316 501
Charles MacNeill 5:89031b2f5316 502 VL53LX_Error VL53LX_f_015(
Charles MacNeill 5:89031b2f5316 503 uint8_t pulse_no,
Charles MacNeill 5:89031b2f5316 504 uint8_t clip_events,
Charles MacNeill 5:89031b2f5316 505 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 506 VL53LX_hist_gen3_algo_private_data_t *palgo)
Charles MacNeill 5:89031b2f5316 507 {
Charles MacNeill 5:89031b2f5316 508
Charles MacNeill 5:89031b2f5316 509
Charles MacNeill 5:89031b2f5316 510 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 511
Charles MacNeill 5:89031b2f5316 512 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 513 int16_t VL53LX_p_012 = 0;
Charles MacNeill 5:89031b2f5316 514 int16_t VL53LX_p_013 = 0;
Charles MacNeill 5:89031b2f5316 515 int16_t window_width = 0;
Charles MacNeill 5:89031b2f5316 516 uint32_t tmp_phase = 0;
Charles MacNeill 5:89031b2f5316 517
Charles MacNeill 5:89031b2f5316 518 VL53LX_hist_pulse_data_t *pdata = &(palgo->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 519
Charles MacNeill 5:89031b2f5316 520 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 521
Charles MacNeill 5:89031b2f5316 522
Charles MacNeill 5:89031b2f5316 523
Charles MacNeill 5:89031b2f5316 524 i = pdata->VL53LX_p_023 % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 525
Charles MacNeill 5:89031b2f5316 526 VL53LX_p_012 = (int16_t)i;
Charles MacNeill 5:89031b2f5316 527 VL53LX_p_012 += (int16_t)pdata->VL53LX_p_012;
Charles MacNeill 5:89031b2f5316 528 VL53LX_p_012 -= (int16_t)pdata->VL53LX_p_023;
Charles MacNeill 5:89031b2f5316 529
Charles MacNeill 5:89031b2f5316 530 VL53LX_p_013 = (int16_t)i;
Charles MacNeill 5:89031b2f5316 531 VL53LX_p_013 += (int16_t)pdata->VL53LX_p_013;
Charles MacNeill 5:89031b2f5316 532 VL53LX_p_013 -= (int16_t)pdata->VL53LX_p_023;
Charles MacNeill 5:89031b2f5316 533
Charles MacNeill 5:89031b2f5316 534
Charles MacNeill 5:89031b2f5316 535 window_width = VL53LX_p_013 - VL53LX_p_012;
Charles MacNeill 5:89031b2f5316 536 if (window_width > 3)
Charles MacNeill 5:89031b2f5316 537 window_width = 3;
Charles MacNeill 5:89031b2f5316 538
Charles MacNeill 5:89031b2f5316 539 status =
Charles MacNeill 5:89031b2f5316 540 VL53LX_f_020(
Charles MacNeill 5:89031b2f5316 541 VL53LX_p_012,
Charles MacNeill 5:89031b2f5316 542 VL53LX_p_012 + window_width,
Charles MacNeill 5:89031b2f5316 543 palgo->VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 544 clip_events,
Charles MacNeill 5:89031b2f5316 545 pbins,
Charles MacNeill 5:89031b2f5316 546 &(pdata->VL53LX_p_026));
Charles MacNeill 5:89031b2f5316 547
Charles MacNeill 5:89031b2f5316 548
Charles MacNeill 5:89031b2f5316 549 if (status == VL53LX_ERROR_NONE)
Charles MacNeill 5:89031b2f5316 550 status =
Charles MacNeill 5:89031b2f5316 551 VL53LX_f_020(
Charles MacNeill 5:89031b2f5316 552 VL53LX_p_013 - window_width,
Charles MacNeill 5:89031b2f5316 553 VL53LX_p_013,
Charles MacNeill 5:89031b2f5316 554 palgo->VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 555 clip_events,
Charles MacNeill 5:89031b2f5316 556 pbins,
Charles MacNeill 5:89031b2f5316 557 &(pdata->VL53LX_p_027));
Charles MacNeill 5:89031b2f5316 558
Charles MacNeill 5:89031b2f5316 559
Charles MacNeill 5:89031b2f5316 560 if (pdata->VL53LX_p_026 > pdata->VL53LX_p_027) {
Charles MacNeill 5:89031b2f5316 561 tmp_phase = pdata->VL53LX_p_026;
Charles MacNeill 5:89031b2f5316 562 pdata->VL53LX_p_026 = pdata->VL53LX_p_027;
Charles MacNeill 5:89031b2f5316 563 pdata->VL53LX_p_027 = tmp_phase;
Charles MacNeill 5:89031b2f5316 564 }
Charles MacNeill 5:89031b2f5316 565
Charles MacNeill 5:89031b2f5316 566
Charles MacNeill 5:89031b2f5316 567 if (pdata->VL53LX_p_011 < pdata->VL53LX_p_026)
Charles MacNeill 5:89031b2f5316 568 pdata->VL53LX_p_026 = pdata->VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 569
Charles MacNeill 5:89031b2f5316 570
Charles MacNeill 5:89031b2f5316 571 if (pdata->VL53LX_p_011 > pdata->VL53LX_p_027)
Charles MacNeill 5:89031b2f5316 572 pdata->VL53LX_p_027 = pdata->VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 573
Charles MacNeill 5:89031b2f5316 574 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 575
Charles MacNeill 5:89031b2f5316 576 return status;
Charles MacNeill 5:89031b2f5316 577 }
Charles MacNeill 5:89031b2f5316 578
Charles MacNeill 5:89031b2f5316 579
Charles MacNeill 5:89031b2f5316 580 VL53LX_Error VL53LX_f_020(
Charles MacNeill 5:89031b2f5316 581 int16_t VL53LX_p_019,
Charles MacNeill 5:89031b2f5316 582 int16_t VL53LX_p_024,
Charles MacNeill 5:89031b2f5316 583 uint8_t VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 584 uint8_t clip_events,
Charles MacNeill 5:89031b2f5316 585 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 586 uint32_t *pphase)
Charles MacNeill 5:89031b2f5316 587 {
Charles MacNeill 5:89031b2f5316 588
Charles MacNeill 5:89031b2f5316 589
Charles MacNeill 5:89031b2f5316 590 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 591
Charles MacNeill 5:89031b2f5316 592 int16_t i = 0;
Charles MacNeill 5:89031b2f5316 593 int16_t lb = 0;
Charles MacNeill 5:89031b2f5316 594
Charles MacNeill 5:89031b2f5316 595 int64_t VL53LX_p_018 = 0;
Charles MacNeill 5:89031b2f5316 596 int64_t event_sum = 0;
Charles MacNeill 5:89031b2f5316 597 int64_t weighted_sum = 0;
Charles MacNeill 5:89031b2f5316 598
Charles MacNeill 5:89031b2f5316 599 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 600
Charles MacNeill 5:89031b2f5316 601 *pphase = VL53LX_MAX_ALLOWED_PHASE;
Charles MacNeill 5:89031b2f5316 602
Charles MacNeill 5:89031b2f5316 603 for (lb = VL53LX_p_019; lb <= VL53LX_p_024; lb++) {
Charles MacNeill 5:89031b2f5316 604
Charles MacNeill 5:89031b2f5316 605 if (lb < 0)
Charles MacNeill 5:89031b2f5316 606 i = lb + (int16_t)VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 607 else
Charles MacNeill 5:89031b2f5316 608 i = lb % (int16_t)VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 609
Charles MacNeill 5:89031b2f5316 610 if ((i >= 0) && (i < VL53LX_HISTOGRAM_BUFFER_SIZE)) {
Charles MacNeill 5:89031b2f5316 611 VL53LX_p_018 =
Charles MacNeill 5:89031b2f5316 612 (int64_t)pbins->bin_data[i] -
Charles MacNeill 5:89031b2f5316 613 (int64_t)pbins->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 614
Charles MacNeill 5:89031b2f5316 615 if (clip_events > 0 && VL53LX_p_018 < 0)
Charles MacNeill 5:89031b2f5316 616 VL53LX_p_018 = 0;
Charles MacNeill 5:89031b2f5316 617 event_sum += VL53LX_p_018;
Charles MacNeill 5:89031b2f5316 618 weighted_sum +=
Charles MacNeill 5:89031b2f5316 619 (VL53LX_p_018 * (1024 + (2048*(int64_t)lb)));
Charles MacNeill 5:89031b2f5316 620 }
Charles MacNeill 5:89031b2f5316 621
Charles MacNeill 5:89031b2f5316 622 trace_print(
Charles MacNeill 5:89031b2f5316 623 VL53LX_TRACE_LEVEL_INFO,
Charles MacNeill 5:89031b2f5316 624 "\tb = %5d : i = %5d : VL53LX_p_018 = %8d,",
Charles MacNeill 5:89031b2f5316 625 lb, i, VL53LX_p_018);
Charles MacNeill 5:89031b2f5316 626
Charles MacNeill 5:89031b2f5316 627 trace_print(
Charles MacNeill 5:89031b2f5316 628 VL53LX_TRACE_LEVEL_INFO,
Charles MacNeill 5:89031b2f5316 629 " event_sum = %8d, weighted_sum = %8d\n",
Charles MacNeill 5:89031b2f5316 630 event_sum, weighted_sum);
Charles MacNeill 5:89031b2f5316 631 }
Charles MacNeill 5:89031b2f5316 632
Charles MacNeill 5:89031b2f5316 633 if (event_sum > 0) {
Charles MacNeill 5:89031b2f5316 634 weighted_sum += do_division_s(event_sum, 2);
Charles MacNeill 5:89031b2f5316 635 weighted_sum = do_division_s(weighted_sum, event_sum);
Charles MacNeill 5:89031b2f5316 636 if (weighted_sum < 0)
Charles MacNeill 5:89031b2f5316 637 weighted_sum = 0;
Charles MacNeill 5:89031b2f5316 638 *pphase = (uint32_t)weighted_sum;
Charles MacNeill 5:89031b2f5316 639 }
Charles MacNeill 5:89031b2f5316 640
Charles MacNeill 5:89031b2f5316 641 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 642
Charles MacNeill 5:89031b2f5316 643 return status;
Charles MacNeill 5:89031b2f5316 644 }
Charles MacNeill 5:89031b2f5316 645
Charles MacNeill 5:89031b2f5316 646
Charles MacNeill 5:89031b2f5316 647 VL53LX_Error VL53LX_f_011(
Charles MacNeill 5:89031b2f5316 648 uint8_t pulse_no,
Charles MacNeill 5:89031b2f5316 649 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 650 VL53LX_hist_gen3_algo_private_data_t *palgo,
Charles MacNeill 5:89031b2f5316 651 int32_t pad_value,
Charles MacNeill 5:89031b2f5316 652 VL53LX_histogram_bin_data_t *ppulse)
Charles MacNeill 5:89031b2f5316 653 {
Charles MacNeill 5:89031b2f5316 654
Charles MacNeill 5:89031b2f5316 655
Charles MacNeill 5:89031b2f5316 656
Charles MacNeill 5:89031b2f5316 657 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 658
Charles MacNeill 5:89031b2f5316 659 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 660 uint8_t lb = 0;
Charles MacNeill 5:89031b2f5316 661
Charles MacNeill 5:89031b2f5316 662 VL53LX_hist_pulse_data_t *pdata = &(palgo->VL53LX_p_003[pulse_no]);
Charles MacNeill 5:89031b2f5316 663
Charles MacNeill 5:89031b2f5316 664 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 665
Charles MacNeill 5:89031b2f5316 666
Charles MacNeill 5:89031b2f5316 667
Charles MacNeill 5:89031b2f5316 668 memcpy(ppulse, pbins, sizeof(VL53LX_histogram_bin_data_t));
Charles MacNeill 5:89031b2f5316 669
Charles MacNeill 5:89031b2f5316 670
Charles MacNeill 5:89031b2f5316 671
Charles MacNeill 5:89031b2f5316 672 for (lb = palgo->VL53LX_p_044;
Charles MacNeill 5:89031b2f5316 673 lb < (palgo->VL53LX_p_044 +
Charles MacNeill 5:89031b2f5316 674 palgo->VL53LX_p_030);
Charles MacNeill 5:89031b2f5316 675 lb++) {
Charles MacNeill 5:89031b2f5316 676
Charles MacNeill 5:89031b2f5316 677 if (lb < pdata->VL53LX_p_012 || lb > pdata->VL53LX_p_013) {
Charles MacNeill 5:89031b2f5316 678 i = lb % palgo->VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 679 if (i < ppulse->VL53LX_p_021)
Charles MacNeill 5:89031b2f5316 680 ppulse->bin_data[i] = pad_value;
Charles MacNeill 5:89031b2f5316 681 }
Charles MacNeill 5:89031b2f5316 682 }
Charles MacNeill 5:89031b2f5316 683
Charles MacNeill 5:89031b2f5316 684 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 685
Charles MacNeill 5:89031b2f5316 686 return status;
Charles MacNeill 5:89031b2f5316 687 }
Charles MacNeill 5:89031b2f5316 688
Charles MacNeill 5:89031b2f5316 689
Charles MacNeill 5:89031b2f5316 690 VL53LX_Error VL53LX_f_014(
Charles MacNeill 5:89031b2f5316 691 uint8_t bin,
Charles MacNeill 5:89031b2f5316 692 uint8_t sigma_estimator__sigma_ref_mm,
Charles MacNeill 5:89031b2f5316 693 uint8_t VL53LX_p_030,
Charles MacNeill 5:89031b2f5316 694 uint8_t VL53LX_p_051,
Charles MacNeill 5:89031b2f5316 695 uint8_t crosstalk_compensation_enable,
Charles MacNeill 5:89031b2f5316 696 VL53LX_histogram_bin_data_t *phist_data_ap,
Charles MacNeill 5:89031b2f5316 697 VL53LX_histogram_bin_data_t *phist_data_zp,
Charles MacNeill 5:89031b2f5316 698 VL53LX_histogram_bin_data_t *pxtalk_hist,
Charles MacNeill 5:89031b2f5316 699 uint16_t *psigma_est)
Charles MacNeill 5:89031b2f5316 700 {
Charles MacNeill 5:89031b2f5316 701
Charles MacNeill 5:89031b2f5316 702
Charles MacNeill 5:89031b2f5316 703 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 704 VL53LX_Error func_status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 705
Charles MacNeill 5:89031b2f5316 706 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 707 int32_t VL53LX_p_007 = 0;
Charles MacNeill 5:89031b2f5316 708 int32_t VL53LX_p_032 = 0;
Charles MacNeill 5:89031b2f5316 709 int32_t VL53LX_p_001 = 0;
Charles MacNeill 5:89031b2f5316 710 int32_t a_zp = 0;
Charles MacNeill 5:89031b2f5316 711 int32_t c_zp = 0;
Charles MacNeill 5:89031b2f5316 712 int32_t ax = 0;
Charles MacNeill 5:89031b2f5316 713 int32_t bx = 0;
Charles MacNeill 5:89031b2f5316 714 int32_t cx = 0;
Charles MacNeill 5:89031b2f5316 715
Charles MacNeill 5:89031b2f5316 716
Charles MacNeill 5:89031b2f5316 717
Charles MacNeill 5:89031b2f5316 718 i = bin % VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 719
Charles MacNeill 5:89031b2f5316 720
Charles MacNeill 5:89031b2f5316 721
Charles MacNeill 5:89031b2f5316 722 VL53LX_f_022(
Charles MacNeill 5:89031b2f5316 723 i,
Charles MacNeill 5:89031b2f5316 724 VL53LX_p_051,
Charles MacNeill 5:89031b2f5316 725 phist_data_zp,
Charles MacNeill 5:89031b2f5316 726 &a_zp,
Charles MacNeill 5:89031b2f5316 727 &VL53LX_p_032,
Charles MacNeill 5:89031b2f5316 728 &c_zp);
Charles MacNeill 5:89031b2f5316 729
Charles MacNeill 5:89031b2f5316 730
Charles MacNeill 5:89031b2f5316 731
Charles MacNeill 5:89031b2f5316 732 VL53LX_f_022(
Charles MacNeill 5:89031b2f5316 733 i,
Charles MacNeill 5:89031b2f5316 734 VL53LX_p_051,
Charles MacNeill 5:89031b2f5316 735 phist_data_ap,
Charles MacNeill 5:89031b2f5316 736 &VL53LX_p_007,
Charles MacNeill 5:89031b2f5316 737 &VL53LX_p_032,
Charles MacNeill 5:89031b2f5316 738 &VL53LX_p_001);
Charles MacNeill 5:89031b2f5316 739
Charles MacNeill 5:89031b2f5316 740 if (crosstalk_compensation_enable > 0)
Charles MacNeill 5:89031b2f5316 741 VL53LX_f_022(
Charles MacNeill 5:89031b2f5316 742 i,
Charles MacNeill 5:89031b2f5316 743 VL53LX_p_051,
Charles MacNeill 5:89031b2f5316 744 pxtalk_hist,
Charles MacNeill 5:89031b2f5316 745 &ax,
Charles MacNeill 5:89031b2f5316 746 &bx,
Charles MacNeill 5:89031b2f5316 747 &cx);
Charles MacNeill 5:89031b2f5316 748
Charles MacNeill 5:89031b2f5316 749
Charles MacNeill 5:89031b2f5316 750
Charles MacNeill 5:89031b2f5316 751
Charles MacNeill 5:89031b2f5316 752
Charles MacNeill 5:89031b2f5316 753
Charles MacNeill 5:89031b2f5316 754
Charles MacNeill 5:89031b2f5316 755 func_status =
Charles MacNeill 5:89031b2f5316 756 VL53LX_f_023(
Charles MacNeill 5:89031b2f5316 757 sigma_estimator__sigma_ref_mm,
Charles MacNeill 5:89031b2f5316 758 (uint32_t)VL53LX_p_007,
Charles MacNeill 5:89031b2f5316 759 (uint32_t)VL53LX_p_032,
Charles MacNeill 5:89031b2f5316 760 (uint32_t)VL53LX_p_001,
Charles MacNeill 5:89031b2f5316 761 (uint32_t)a_zp,
Charles MacNeill 5:89031b2f5316 762 (uint32_t)c_zp,
Charles MacNeill 5:89031b2f5316 763 (uint32_t)bx,
Charles MacNeill 5:89031b2f5316 764 (uint32_t)ax,
Charles MacNeill 5:89031b2f5316 765 (uint32_t)cx,
Charles MacNeill 5:89031b2f5316 766 (uint32_t)phist_data_ap->VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 767 phist_data_ap->VL53LX_p_015,
Charles MacNeill 5:89031b2f5316 768 psigma_est);
Charles MacNeill 5:89031b2f5316 769
Charles MacNeill 5:89031b2f5316 770
Charles MacNeill 5:89031b2f5316 771
Charles MacNeill 5:89031b2f5316 772
Charles MacNeill 5:89031b2f5316 773 if (func_status == VL53LX_ERROR_DIVISION_BY_ZERO)
Charles MacNeill 5:89031b2f5316 774 *psigma_est = 0xFFFF;
Charles MacNeill 5:89031b2f5316 775
Charles MacNeill 5:89031b2f5316 776
Charles MacNeill 5:89031b2f5316 777 return status;
Charles MacNeill 5:89031b2f5316 778 }
Charles MacNeill 5:89031b2f5316 779
Charles MacNeill 5:89031b2f5316 780
Charles MacNeill 5:89031b2f5316 781 void VL53LX_f_017(
Charles MacNeill 5:89031b2f5316 782 uint8_t range_id,
Charles MacNeill 5:89031b2f5316 783 uint8_t valid_phase_low,
Charles MacNeill 5:89031b2f5316 784 uint8_t valid_phase_high,
Charles MacNeill 5:89031b2f5316 785 uint16_t sigma_thres,
Charles MacNeill 5:89031b2f5316 786 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 787 VL53LX_hist_pulse_data_t *ppulse,
Charles MacNeill 5:89031b2f5316 788 VL53LX_range_data_t *pdata)
Charles MacNeill 5:89031b2f5316 789 {
Charles MacNeill 5:89031b2f5316 790
Charles MacNeill 5:89031b2f5316 791 uint16_t lower_phase_limit = 0;
Charles MacNeill 5:89031b2f5316 792 uint16_t upper_phase_limit = 0;
Charles MacNeill 5:89031b2f5316 793
Charles MacNeill 5:89031b2f5316 794
Charles MacNeill 5:89031b2f5316 795
Charles MacNeill 5:89031b2f5316 796 pdata->range_id = range_id;
Charles MacNeill 5:89031b2f5316 797 pdata->time_stamp = 0;
Charles MacNeill 5:89031b2f5316 798
Charles MacNeill 5:89031b2f5316 799 pdata->VL53LX_p_012 = ppulse->VL53LX_p_012;
Charles MacNeill 5:89031b2f5316 800 pdata->VL53LX_p_019 = ppulse->VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 801 pdata->VL53LX_p_023 = ppulse->VL53LX_p_023;
Charles MacNeill 5:89031b2f5316 802 pdata->VL53LX_p_024 = ppulse->VL53LX_p_024;
Charles MacNeill 5:89031b2f5316 803 pdata->VL53LX_p_013 = ppulse->VL53LX_p_013;
Charles MacNeill 5:89031b2f5316 804 pdata->VL53LX_p_025 = ppulse->VL53LX_p_025;
Charles MacNeill 5:89031b2f5316 805
Charles MacNeill 5:89031b2f5316 806
Charles MacNeill 5:89031b2f5316 807
Charles MacNeill 5:89031b2f5316 808 pdata->VL53LX_p_029 =
Charles MacNeill 5:89031b2f5316 809 (ppulse->VL53LX_p_013 + 1) - ppulse->VL53LX_p_012;
Charles MacNeill 5:89031b2f5316 810
Charles MacNeill 5:89031b2f5316 811
Charles MacNeill 5:89031b2f5316 812
Charles MacNeill 5:89031b2f5316 813 pdata->zero_distance_phase = pbins->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 814 pdata->VL53LX_p_002 = ppulse->VL53LX_p_002;
Charles MacNeill 5:89031b2f5316 815 pdata->VL53LX_p_026 = (uint16_t)ppulse->VL53LX_p_026;
Charles MacNeill 5:89031b2f5316 816 pdata->VL53LX_p_011 = (uint16_t)ppulse->VL53LX_p_011;
Charles MacNeill 5:89031b2f5316 817 pdata->VL53LX_p_027 = (uint16_t)ppulse->VL53LX_p_027;
Charles MacNeill 5:89031b2f5316 818 pdata->VL53LX_p_017 = (uint32_t)ppulse->VL53LX_p_017;
Charles MacNeill 5:89031b2f5316 819 pdata->VL53LX_p_010 = ppulse->VL53LX_p_010;
Charles MacNeill 5:89031b2f5316 820 pdata->VL53LX_p_016 = (uint32_t)ppulse->VL53LX_p_016;
Charles MacNeill 5:89031b2f5316 821 pdata->total_periods_elapsed = pbins->total_periods_elapsed;
Charles MacNeill 5:89031b2f5316 822
Charles MacNeill 5:89031b2f5316 823
Charles MacNeill 5:89031b2f5316 824
Charles MacNeill 5:89031b2f5316 825 pdata->range_status = VL53LX_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK;
Charles MacNeill 5:89031b2f5316 826
Charles MacNeill 5:89031b2f5316 827
Charles MacNeill 5:89031b2f5316 828 if (sigma_thres > 0 &&
Charles MacNeill 5:89031b2f5316 829 (uint32_t)ppulse->VL53LX_p_002 > ((uint32_t)sigma_thres << 5))
Charles MacNeill 5:89031b2f5316 830 pdata->range_status = VL53LX_DEVICEERROR_SIGMATHRESHOLDCHECK;
Charles MacNeill 5:89031b2f5316 831
Charles MacNeill 5:89031b2f5316 832
Charles MacNeill 5:89031b2f5316 833
Charles MacNeill 5:89031b2f5316 834 lower_phase_limit = (uint8_t)valid_phase_low << 8;
Charles MacNeill 5:89031b2f5316 835 if (lower_phase_limit < pdata->zero_distance_phase)
Charles MacNeill 5:89031b2f5316 836 lower_phase_limit =
Charles MacNeill 5:89031b2f5316 837 pdata->zero_distance_phase -
Charles MacNeill 5:89031b2f5316 838 lower_phase_limit;
Charles MacNeill 5:89031b2f5316 839 else
Charles MacNeill 5:89031b2f5316 840 lower_phase_limit = 0;
Charles MacNeill 5:89031b2f5316 841
Charles MacNeill 5:89031b2f5316 842 upper_phase_limit = (uint8_t)valid_phase_high << 8;
Charles MacNeill 5:89031b2f5316 843 upper_phase_limit += pbins->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 844
Charles MacNeill 5:89031b2f5316 845 if (pdata->VL53LX_p_011 < lower_phase_limit ||
Charles MacNeill 5:89031b2f5316 846 pdata->VL53LX_p_011 > upper_phase_limit)
Charles MacNeill 5:89031b2f5316 847 pdata->range_status = VL53LX_DEVICEERROR_RANGEPHASECHECK;
Charles MacNeill 5:89031b2f5316 848
Charles MacNeill 5:89031b2f5316 849 }
Charles MacNeill 5:89031b2f5316 850
Charles MacNeill 5:89031b2f5316 851