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@7:7f1bbf370283, 2021-07-21 (annotated)

Committer:
charlesmn
Date:
Wed Jul 21 14:07:59 2021 +0000
Revision:
7:7f1bbf370283
Parent:
5:89031b2f5316 
Moved vl53l3cx_class.cpp and .h to 53l3a2_RangingClass

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