ST Expansion SW Team / VL53L1CB

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

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

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

src/vl53l1_hist_core.c@0:3ac96e360672, 2020-11-06 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
charlesmn 0:3ac96e360672 2 /*******************************************************************************
charlesmn 0:3ac96e360672 3 This file is part of VL53L1 Protected
charlesmn 0:3ac96e360672 4
charlesmn 0:3ac96e360672 5 Copyright (c) 2020, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 6
charlesmn 0:3ac96e360672 7 License terms: STMicroelectronics Proprietary in accordance with licensing
charlesmn 0:3ac96e360672 8 terms at www.st.com/sla0081
charlesmn 0:3ac96e360672 9
charlesmn 0:3ac96e360672 10 STMicroelectronics confidential
charlesmn 0:3ac96e360672 11 Reproduction and Communication of this document is strictly prohibited unless
charlesmn 0:3ac96e360672 12 specifically authorized in writing by STMicroelectronics.
charlesmn 0:3ac96e360672 13
charlesmn 0:3ac96e360672 14 */
charlesmn 0:3ac96e360672 15
charlesmn 0:3ac96e360672 16
charlesmn 0:3ac96e360672 17
charlesmn 0:3ac96e360672 18
charlesmn 0:3ac96e360672 19 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 20
charlesmn 0:3ac96e360672 21 #include "vl53l1_core_support.h"
charlesmn 0:3ac96e360672 22 #include "vl53l1_hist_structs.h"
charlesmn 0:3ac96e360672 23
charlesmn 0:3ac96e360672 24 #include "vl53l1_xtalk.h"
charlesmn 0:3ac96e360672 25 #include "vl53l1_sigma_estimate.h"
charlesmn 0:3ac96e360672 26
charlesmn 0:3ac96e360672 27 #include "vl53l1_hist_core.h"
charlesmn 0:3ac96e360672 28
charlesmn 0:3ac96e360672 29
charlesmn 0:3ac96e360672 30
charlesmn 0:3ac96e360672 31 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 32 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_HISTOGRAM, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 33 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 34 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_HISTOGRAM, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 35 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 36 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 37 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 38
charlesmn 0:3ac96e360672 39 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 40 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_HISTOGRAM, \
charlesmn 0:3ac96e360672 41 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 42
charlesmn 0:3ac96e360672 43
charlesmn 0:3ac96e360672 44 void VL53L1_f_013(
charlesmn 0:3ac96e360672 45 uint8_t VL53L1_p_018,
charlesmn 0:3ac96e360672 46 uint8_t filter_woi,
charlesmn 0:3ac96e360672 47 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 48 int32_t *pa,
charlesmn 0:3ac96e360672 49 int32_t *pb,
charlesmn 0:3ac96e360672 50 int32_t *pc)
charlesmn 0:3ac96e360672 51 {
charlesmn 0:3ac96e360672 52
charlesmn 0:3ac96e360672 53
charlesmn 0:3ac96e360672 54 uint8_t w = 0;
charlesmn 0:3ac96e360672 55 uint8_t j = 0;
charlesmn 0:3ac96e360672 56
charlesmn 0:3ac96e360672 57 *pa = 0;
charlesmn 0:3ac96e360672 58 *pb = pbins->bin_data[VL53L1_p_018];
charlesmn 0:3ac96e360672 59 *pc = 0;
charlesmn 0:3ac96e360672 60
charlesmn 0:3ac96e360672 61 for (w = 0 ; w < ((filter_woi << 1)+1) ; w++) {
charlesmn 0:3ac96e360672 62
charlesmn 0:3ac96e360672 63
charlesmn 0:3ac96e360672 64 j = ((VL53L1_p_018 + w + pbins->VL53L1_p_024) -
charlesmn 0:3ac96e360672 65 filter_woi) % pbins->VL53L1_p_024;
charlesmn 0:3ac96e360672 66
charlesmn 0:3ac96e360672 67
charlesmn 0:3ac96e360672 68 if (w < filter_woi)
charlesmn 0:3ac96e360672 69 *pa += pbins->bin_data[j];
charlesmn 0:3ac96e360672 70 else if (w > filter_woi)
charlesmn 0:3ac96e360672 71 *pc += pbins->bin_data[j];
charlesmn 0:3ac96e360672 72 }
charlesmn 0:3ac96e360672 73 }
charlesmn 0:3ac96e360672 74
charlesmn 0:3ac96e360672 75
charlesmn 0:3ac96e360672 76 VL53L1_Error VL53L1_f_011(
charlesmn 0:3ac96e360672 77 uint16_t vcsel_width,
charlesmn 0:3ac96e360672 78 uint16_t fast_osc_frequency,
charlesmn 0:3ac96e360672 79 uint32_t total_periods_elapsed,
charlesmn 0:3ac96e360672 80 uint16_t VL53L1_p_006,
charlesmn 0:3ac96e360672 81 VL53L1_range_data_t *pdata,
charlesmn 0:3ac96e360672 82 uint8_t histo_merge_nb)
charlesmn 0:3ac96e360672 83 {
charlesmn 0:3ac96e360672 84 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 85
charlesmn 0:3ac96e360672 86 uint32_t pll_period_us = 0;
charlesmn 0:3ac96e360672 87 uint32_t periods_elapsed = 0;
charlesmn 0:3ac96e360672 88 uint32_t count_rate_total = 0;
charlesmn 0:3ac96e360672 89
charlesmn 0:3ac96e360672 90 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 91
charlesmn 0:3ac96e360672 92
charlesmn 0:3ac96e360672 93
charlesmn 0:3ac96e360672 94 pdata->width = vcsel_width;
charlesmn 0:3ac96e360672 95 pdata->fast_osc_frequency = fast_osc_frequency;
charlesmn 0:3ac96e360672 96 pdata->total_periods_elapsed = total_periods_elapsed;
charlesmn 0:3ac96e360672 97 pdata->VL53L1_p_006 = VL53L1_p_006;
charlesmn 0:3ac96e360672 98
charlesmn 0:3ac96e360672 99
charlesmn 0:3ac96e360672 100
charlesmn 0:3ac96e360672 101 if (pdata->fast_osc_frequency == 0)
charlesmn 0:3ac96e360672 102 status = VL53L1_ERROR_DIVISION_BY_ZERO;
charlesmn 0:3ac96e360672 103
charlesmn 0:3ac96e360672 104 if (pdata->total_periods_elapsed == 0)
charlesmn 0:3ac96e360672 105 status = VL53L1_ERROR_DIVISION_BY_ZERO;
charlesmn 0:3ac96e360672 106
charlesmn 0:3ac96e360672 107 if (status == VL53L1_ERROR_NONE) {
charlesmn 0:3ac96e360672 108
charlesmn 0:3ac96e360672 109
charlesmn 0:3ac96e360672 110
charlesmn 0:3ac96e360672 111 pll_period_us =
charlesmn 0:3ac96e360672 112 VL53L1_calc_pll_period_us(pdata->fast_osc_frequency);
charlesmn 0:3ac96e360672 113
charlesmn 0:3ac96e360672 114
charlesmn 0:3ac96e360672 115
charlesmn 0:3ac96e360672 116 periods_elapsed = pdata->total_periods_elapsed + 1;
charlesmn 0:3ac96e360672 117
charlesmn 0:3ac96e360672 118
charlesmn 0:3ac96e360672 119
charlesmn 0:3ac96e360672 120 pdata->peak_duration_us = VL53L1_duration_maths(
charlesmn 0:3ac96e360672 121 pll_period_us,
charlesmn 0:3ac96e360672 122 (uint32_t)pdata->width,
charlesmn 0:3ac96e360672 123 VL53L1_RANGING_WINDOW_VCSEL_PERIODS,
charlesmn 0:3ac96e360672 124 periods_elapsed);
charlesmn 0:3ac96e360672 125
charlesmn 0:3ac96e360672 126 pdata->woi_duration_us = VL53L1_duration_maths(
charlesmn 0:3ac96e360672 127 pll_period_us,
charlesmn 0:3ac96e360672 128 ((uint32_t)pdata->VL53L1_p_030) << 4,
charlesmn 0:3ac96e360672 129 VL53L1_RANGING_WINDOW_VCSEL_PERIODS,
charlesmn 0:3ac96e360672 130 periods_elapsed);
charlesmn 0:3ac96e360672 131
charlesmn 0:3ac96e360672 132
charlesmn 0:3ac96e360672 133
charlesmn 0:3ac96e360672 134 pdata->peak_signal_count_rate_mcps = VL53L1_rate_maths(
charlesmn 0:3ac96e360672 135 (int32_t)pdata->VL53L1_p_013,
charlesmn 0:3ac96e360672 136 pdata->peak_duration_us);
charlesmn 0:3ac96e360672 137
charlesmn 0:3ac96e360672 138 pdata->avg_signal_count_rate_mcps = VL53L1_rate_maths(
charlesmn 0:3ac96e360672 139 (int32_t)pdata->VL53L1_p_013,
charlesmn 0:3ac96e360672 140 pdata->woi_duration_us);
charlesmn 0:3ac96e360672 141
charlesmn 0:3ac96e360672 142 pdata->ambient_count_rate_mcps = VL53L1_rate_maths(
charlesmn 0:3ac96e360672 143 (int32_t)pdata->VL53L1_p_020,
charlesmn 0:3ac96e360672 144 pdata->woi_duration_us);
charlesmn 0:3ac96e360672 145
charlesmn 0:3ac96e360672 146
charlesmn 0:3ac96e360672 147
charlesmn 0:3ac96e360672 148 count_rate_total =
charlesmn 0:3ac96e360672 149 (uint32_t)pdata->peak_signal_count_rate_mcps +
charlesmn 0:3ac96e360672 150 (uint32_t)pdata->ambient_count_rate_mcps;
charlesmn 0:3ac96e360672 151
charlesmn 0:3ac96e360672 152 if (histo_merge_nb > 1)
charlesmn 0:3ac96e360672 153 count_rate_total /= histo_merge_nb;
charlesmn 0:3ac96e360672 154
charlesmn 0:3ac96e360672 155 pdata->total_rate_per_spad_mcps =
charlesmn 0:3ac96e360672 156 VL53L1_rate_per_spad_maths(
charlesmn 0:3ac96e360672 157 0x06,
charlesmn 0:3ac96e360672 158 count_rate_total,
charlesmn 0:3ac96e360672 159 pdata->VL53L1_p_006,
charlesmn 0:3ac96e360672 160 0xFFFF);
charlesmn 0:3ac96e360672 161
charlesmn 0:3ac96e360672 162
charlesmn 0:3ac96e360672 163
charlesmn 0:3ac96e360672 164 pdata->VL53L1_p_012 =
charlesmn 0:3ac96e360672 165 VL53L1_events_per_spad_maths(
charlesmn 0:3ac96e360672 166 pdata->VL53L1_p_013,
charlesmn 0:3ac96e360672 167 pdata->VL53L1_p_006,
charlesmn 0:3ac96e360672 168 pdata->peak_duration_us);
charlesmn 0:3ac96e360672 169
charlesmn 0:3ac96e360672 170
charlesmn 0:3ac96e360672 171
charlesmn 0:3ac96e360672 172
charlesmn 0:3ac96e360672 173 trace_print(
charlesmn 0:3ac96e360672 174 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 175 " %d:%-46s : %10d\n",
charlesmn 0:3ac96e360672 176 pdata->range_id, "peak_duration_us",
charlesmn 0:3ac96e360672 177 pdata->peak_duration_us);
charlesmn 0:3ac96e360672 178 trace_print(
charlesmn 0:3ac96e360672 179 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 180 " %d:%-46s : %10d\n",
charlesmn 0:3ac96e360672 181 pdata->range_id, "woi_duration_us",
charlesmn 0:3ac96e360672 182 pdata->woi_duration_us);
charlesmn 0:3ac96e360672 183 trace_print(
charlesmn 0:3ac96e360672 184 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 185 " %d:%-46s : %10u\n",
charlesmn 0:3ac96e360672 186 pdata->range_id, "peak_signal_count_rate_mcps",
charlesmn 0:3ac96e360672 187 pdata->peak_signal_count_rate_mcps);
charlesmn 0:3ac96e360672 188 trace_print(
charlesmn 0:3ac96e360672 189 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 190 " %d:%-46s : %10u\n",
charlesmn 0:3ac96e360672 191 pdata->range_id, "ambient_count_rate_mcps",
charlesmn 0:3ac96e360672 192 pdata->ambient_count_rate_mcps);
charlesmn 0:3ac96e360672 193 trace_print(
charlesmn 0:3ac96e360672 194 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 195 " %d:%-46s : %10u\n",
charlesmn 0:3ac96e360672 196 pdata->range_id, "total_rate_per_spad_mcps",
charlesmn 0:3ac96e360672 197 pdata->total_rate_per_spad_mcps);
charlesmn 0:3ac96e360672 198 trace_print(
charlesmn 0:3ac96e360672 199 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 200 " %d:%-46s : %10u\n",
charlesmn 0:3ac96e360672 201 pdata->range_id, "VL53L1_p_012",
charlesmn 0:3ac96e360672 202 pdata->VL53L1_p_012);
charlesmn 0:3ac96e360672 203
charlesmn 0:3ac96e360672 204 }
charlesmn 0:3ac96e360672 205
charlesmn 0:3ac96e360672 206 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 207
charlesmn 0:3ac96e360672 208 return status;
charlesmn 0:3ac96e360672 209 }
charlesmn 0:3ac96e360672 210
charlesmn 0:3ac96e360672 211
charlesmn 0:3ac96e360672 212 void VL53L1_f_012(
charlesmn 0:3ac96e360672 213 uint16_t gain_factor,
charlesmn 0:3ac96e360672 214 int16_t range_offset_mm,
charlesmn 0:3ac96e360672 215 VL53L1_range_data_t *pdata)
charlesmn 0:3ac96e360672 216 {
charlesmn 0:3ac96e360672 217
charlesmn 0:3ac96e360672 218
charlesmn 0:3ac96e360672 219 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 220
charlesmn 0:3ac96e360672 221
charlesmn 0:3ac96e360672 222
charlesmn 0:3ac96e360672 223 pdata->min_range_mm =
charlesmn 0:3ac96e360672 224 (int16_t)VL53L1_range_maths(
charlesmn 0:3ac96e360672 225 pdata->fast_osc_frequency,
charlesmn 0:3ac96e360672 226 pdata->VL53L1_p_028,
charlesmn 0:3ac96e360672 227 pdata->zero_distance_phase,
charlesmn 0:3ac96e360672 228 0,
charlesmn 0:3ac96e360672 229 (int32_t)gain_factor,
charlesmn 0:3ac96e360672 230 (int32_t)range_offset_mm);
charlesmn 0:3ac96e360672 231
charlesmn 0:3ac96e360672 232 pdata->median_range_mm =
charlesmn 0:3ac96e360672 233 (int16_t)VL53L1_range_maths(
charlesmn 0:3ac96e360672 234 pdata->fast_osc_frequency,
charlesmn 0:3ac96e360672 235 pdata->VL53L1_p_014,
charlesmn 0:3ac96e360672 236 pdata->zero_distance_phase,
charlesmn 0:3ac96e360672 237 0,
charlesmn 0:3ac96e360672 238 (int32_t)gain_factor,
charlesmn 0:3ac96e360672 239 (int32_t)range_offset_mm);
charlesmn 0:3ac96e360672 240
charlesmn 0:3ac96e360672 241 pdata->max_range_mm =
charlesmn 0:3ac96e360672 242 (int16_t)VL53L1_range_maths(
charlesmn 0:3ac96e360672 243 pdata->fast_osc_frequency,
charlesmn 0:3ac96e360672 244 pdata->VL53L1_p_029,
charlesmn 0:3ac96e360672 245 pdata->zero_distance_phase,
charlesmn 0:3ac96e360672 246 0,
charlesmn 0:3ac96e360672 247 (int32_t)gain_factor,
charlesmn 0:3ac96e360672 248 (int32_t)range_offset_mm);
charlesmn 0:3ac96e360672 249
charlesmn 0:3ac96e360672 250
charlesmn 0:3ac96e360672 251
charlesmn 0:3ac96e360672 252
charlesmn 0:3ac96e360672 253
charlesmn 0:3ac96e360672 254 LOG_FUNCTION_END(0);
charlesmn 0:3ac96e360672 255 }
charlesmn 0:3ac96e360672 256
charlesmn 0:3ac96e360672 257
charlesmn 0:3ac96e360672 258 void VL53L1_f_037(
charlesmn 0:3ac96e360672 259 VL53L1_histogram_bin_data_t *pdata,
charlesmn 0:3ac96e360672 260 int32_t ambient_estimate_counts_per_bin)
charlesmn 0:3ac96e360672 261 {
charlesmn 0:3ac96e360672 262
charlesmn 0:3ac96e360672 263
charlesmn 0:3ac96e360672 264 uint8_t i = 0;
charlesmn 0:3ac96e360672 265
charlesmn 0:3ac96e360672 266 for (i = 0 ; i < pdata->VL53L1_p_024 ; i++)
charlesmn 0:3ac96e360672 267 pdata->bin_data[i] = pdata->bin_data[i] -
charlesmn 0:3ac96e360672 268 ambient_estimate_counts_per_bin;
charlesmn 0:3ac96e360672 269 }
charlesmn 0:3ac96e360672 270
charlesmn 0:3ac96e360672 271
charlesmn 0:3ac96e360672 272 void VL53L1_f_004(
charlesmn 0:3ac96e360672 273 VL53L1_histogram_bin_data_t *pxtalk,
charlesmn 0:3ac96e360672 274 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 275 VL53L1_histogram_bin_data_t *pxtalk_realigned)
charlesmn 0:3ac96e360672 276 {
charlesmn 0:3ac96e360672 277
charlesmn 0:3ac96e360672 278
charlesmn 0:3ac96e360672 279 uint8_t i = 0;
charlesmn 0:3ac96e360672 280 uint8_t min_bins = 0;
charlesmn 0:3ac96e360672 281 int8_t bin_offset = 0;
charlesmn 0:3ac96e360672 282 int8_t bin_access = 0;
charlesmn 0:3ac96e360672 283
charlesmn 0:3ac96e360672 284 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 285
charlesmn 0:3ac96e360672 286
charlesmn 0:3ac96e360672 287
charlesmn 0:3ac96e360672 288
charlesmn 0:3ac96e360672 289
charlesmn 0:3ac96e360672 290 memcpy(
charlesmn 0:3ac96e360672 291 pxtalk_realigned,
charlesmn 0:3ac96e360672 292 pbins,
charlesmn 0:3ac96e360672 293 sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 294
charlesmn 0:3ac96e360672 295 for (i = 0 ; i < pxtalk_realigned->VL53L1_p_023 ; i++)
charlesmn 0:3ac96e360672 296 pxtalk_realigned->bin_data[i] = 0;
charlesmn 0:3ac96e360672 297
charlesmn 0:3ac96e360672 298
charlesmn 0:3ac96e360672 299
charlesmn 0:3ac96e360672 300 bin_offset = VL53L1_f_038(
charlesmn 0:3ac96e360672 301 pbins,
charlesmn 0:3ac96e360672 302 pxtalk);
charlesmn 0:3ac96e360672 303
charlesmn 0:3ac96e360672 304
charlesmn 0:3ac96e360672 305
charlesmn 0:3ac96e360672 306 if (pxtalk->VL53L1_p_024 < pbins->VL53L1_p_024)
charlesmn 0:3ac96e360672 307 min_bins = pxtalk->VL53L1_p_024;
charlesmn 0:3ac96e360672 308 else
charlesmn 0:3ac96e360672 309 min_bins = pbins->VL53L1_p_024;
charlesmn 0:3ac96e360672 310
charlesmn 0:3ac96e360672 311
charlesmn 0:3ac96e360672 312 for (i = 0 ; i < min_bins ; i++) {
charlesmn 0:3ac96e360672 313
charlesmn 0:3ac96e360672 314
charlesmn 0:3ac96e360672 315
charlesmn 0:3ac96e360672 316 if (bin_offset >= 0)
charlesmn 0:3ac96e360672 317 bin_access = ((int8_t)i + (int8_t)bin_offset)
charlesmn 0:3ac96e360672 318 % (int8_t)pbins->VL53L1_p_024;
charlesmn 0:3ac96e360672 319 else
charlesmn 0:3ac96e360672 320 bin_access = ((int8_t)pbins->VL53L1_p_024 +
charlesmn 0:3ac96e360672 321 ((int8_t)i + (int8_t)bin_offset))
charlesmn 0:3ac96e360672 322 % (int8_t)pbins->VL53L1_p_024;
charlesmn 0:3ac96e360672 323
charlesmn 0:3ac96e360672 324 trace_print(
charlesmn 0:3ac96e360672 325 VL53L1_TRACE_LEVEL_DEBUG,
charlesmn 0:3ac96e360672 326 "Subtract: %8d : %8d : %8d : %8d : %8d : %8d\n",
charlesmn 0:3ac96e360672 327 i, bin_access, bin_offset, pbins->VL53L1_p_024,
charlesmn 0:3ac96e360672 328 pbins->bin_data[(uint8_t)bin_access],
charlesmn 0:3ac96e360672 329 pxtalk->bin_data[i]);
charlesmn 0:3ac96e360672 330
charlesmn 0:3ac96e360672 331
charlesmn 0:3ac96e360672 332
charlesmn 0:3ac96e360672 333 if (pbins->bin_data[(uint8_t)bin_access] >
charlesmn 0:3ac96e360672 334 pxtalk->bin_data[i]) {
charlesmn 0:3ac96e360672 335
charlesmn 0:3ac96e360672 336 pbins->bin_data[(uint8_t)bin_access] =
charlesmn 0:3ac96e360672 337 pbins->bin_data[(uint8_t)bin_access]
charlesmn 0:3ac96e360672 338 - pxtalk->bin_data[i];
charlesmn 0:3ac96e360672 339
charlesmn 0:3ac96e360672 340 } else {
charlesmn 0:3ac96e360672 341 pbins->bin_data[(uint8_t)bin_access] = 0;
charlesmn 0:3ac96e360672 342 }
charlesmn 0:3ac96e360672 343
charlesmn 0:3ac96e360672 344
charlesmn 0:3ac96e360672 345
charlesmn 0:3ac96e360672 346
charlesmn 0:3ac96e360672 347 pxtalk_realigned->bin_data[(uint8_t)bin_access] =
charlesmn 0:3ac96e360672 348 pxtalk->bin_data[i];
charlesmn 0:3ac96e360672 349
charlesmn 0:3ac96e360672 350
charlesmn 0:3ac96e360672 351
charlesmn 0:3ac96e360672 352 }
charlesmn 0:3ac96e360672 353
charlesmn 0:3ac96e360672 354
charlesmn 0:3ac96e360672 355
charlesmn 0:3ac96e360672 356 LOG_FUNCTION_END(0);
charlesmn 0:3ac96e360672 357 }
charlesmn 0:3ac96e360672 358
charlesmn 0:3ac96e360672 359
charlesmn 0:3ac96e360672 360 int8_t VL53L1_f_038(
charlesmn 0:3ac96e360672 361 VL53L1_histogram_bin_data_t *pdata1,
charlesmn 0:3ac96e360672 362 VL53L1_histogram_bin_data_t *pdata2)
charlesmn 0:3ac96e360672 363 {
charlesmn 0:3ac96e360672 364
charlesmn 0:3ac96e360672 365
charlesmn 0:3ac96e360672 366 int32_t phase_delta = 0;
charlesmn 0:3ac96e360672 367 int8_t bin_offset = 0;
charlesmn 0:3ac96e360672 368 uint32_t period = 0;
charlesmn 0:3ac96e360672 369 uint32_t remapped_phase = 0;
charlesmn 0:3ac96e360672 370
charlesmn 0:3ac96e360672 371 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 372
charlesmn 0:3ac96e360672 373
charlesmn 0:3ac96e360672 374
charlesmn 0:3ac96e360672 375 period = 2048 *
charlesmn 0:3ac96e360672 376 (uint32_t)VL53L1_decode_vcsel_period(pdata1->VL53L1_p_009);
charlesmn 0:3ac96e360672 377
charlesmn 0:3ac96e360672 378 remapped_phase = (uint32_t)pdata2->zero_distance_phase % period;
charlesmn 0:3ac96e360672 379
charlesmn 0:3ac96e360672 380
charlesmn 0:3ac96e360672 381 phase_delta = (int32_t)pdata1->zero_distance_phase
charlesmn 0:3ac96e360672 382 - (int32_t)remapped_phase;
charlesmn 0:3ac96e360672 383
charlesmn 0:3ac96e360672 384
charlesmn 0:3ac96e360672 385
charlesmn 0:3ac96e360672 386 if (phase_delta > 0)
charlesmn 0:3ac96e360672 387 bin_offset = (int8_t)((phase_delta + 1024) / 2048);
charlesmn 0:3ac96e360672 388 else
charlesmn 0:3ac96e360672 389 bin_offset = (int8_t)((phase_delta - 1024) / 2048);
charlesmn 0:3ac96e360672 390
charlesmn 0:3ac96e360672 391 LOG_FUNCTION_END(0);
charlesmn 0:3ac96e360672 392
charlesmn 0:3ac96e360672 393 return bin_offset;
charlesmn 0:3ac96e360672 394 }
charlesmn 0:3ac96e360672 395
charlesmn 0:3ac96e360672 396
charlesmn 0:3ac96e360672 397 VL53L1_Error VL53L1_f_039(
charlesmn 0:3ac96e360672 398 VL53L1_histogram_bin_data_t *pidata,
charlesmn 0:3ac96e360672 399 VL53L1_histogram_bin_data_t *podata)
charlesmn 0:3ac96e360672 400 {
charlesmn 0:3ac96e360672 401
charlesmn 0:3ac96e360672 402
charlesmn 0:3ac96e360672 403 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 404
charlesmn 0:3ac96e360672 405 uint8_t bin_initial_index[VL53L1_MAX_BIN_SEQUENCE_CODE+1];
charlesmn 0:3ac96e360672 406 uint8_t bin_repeat_count[VL53L1_MAX_BIN_SEQUENCE_CODE+1];
charlesmn 0:3ac96e360672 407
charlesmn 0:3ac96e360672 408 uint8_t bin_cfg = 0;
charlesmn 0:3ac96e360672 409 uint8_t bin_seq_length = 0;
charlesmn 0:3ac96e360672 410 int32_t repeat_count = 0;
charlesmn 0:3ac96e360672 411
charlesmn 0:3ac96e360672 412 uint8_t VL53L1_p_018 = 0;
charlesmn 0:3ac96e360672 413 uint8_t lc = 0;
charlesmn 0:3ac96e360672 414 uint8_t i = 0;
charlesmn 0:3ac96e360672 415
charlesmn 0:3ac96e360672 416 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 417
charlesmn 0:3ac96e360672 418
charlesmn 0:3ac96e360672 419
charlesmn 0:3ac96e360672 420 memcpy(podata, pidata, sizeof(VL53L1_histogram_bin_data_t));
charlesmn 0:3ac96e360672 421
charlesmn 0:3ac96e360672 422
charlesmn 0:3ac96e360672 423 podata->VL53L1_p_024 = 0;
charlesmn 0:3ac96e360672 424
charlesmn 0:3ac96e360672 425 for (lc = 0 ; lc < VL53L1_MAX_BIN_SEQUENCE_LENGTH ; lc++)
charlesmn 0:3ac96e360672 426 podata->bin_seq[lc] = VL53L1_MAX_BIN_SEQUENCE_CODE+1;
charlesmn 0:3ac96e360672 427
charlesmn 0:3ac96e360672 428 for (lc = 0 ; lc < podata->VL53L1_p_023 ; lc++)
charlesmn 0:3ac96e360672 429 podata->bin_data[lc] = 0;
charlesmn 0:3ac96e360672 430
charlesmn 0:3ac96e360672 431
charlesmn 0:3ac96e360672 432
charlesmn 0:3ac96e360672 433 for (lc = 0 ; lc <= VL53L1_MAX_BIN_SEQUENCE_CODE ; lc++) {
charlesmn 0:3ac96e360672 434 bin_initial_index[lc] = 0x00;
charlesmn 0:3ac96e360672 435 bin_repeat_count[lc] = 0x00;
charlesmn 0:3ac96e360672 436 }
charlesmn 0:3ac96e360672 437
charlesmn 0:3ac96e360672 438
charlesmn 0:3ac96e360672 439
charlesmn 0:3ac96e360672 440
charlesmn 0:3ac96e360672 441 bin_seq_length = 0x00;
charlesmn 0:3ac96e360672 442
charlesmn 0:3ac96e360672 443 for (lc = 0 ; lc < VL53L1_MAX_BIN_SEQUENCE_LENGTH ; lc++) {
charlesmn 0:3ac96e360672 444
charlesmn 0:3ac96e360672 445 bin_cfg = pidata->bin_seq[lc];
charlesmn 0:3ac96e360672 446
charlesmn 0:3ac96e360672 447
charlesmn 0:3ac96e360672 448
charlesmn 0:3ac96e360672 449 if (bin_repeat_count[bin_cfg] == 0) {
charlesmn 0:3ac96e360672 450 bin_initial_index[bin_cfg] = bin_seq_length * 4;
charlesmn 0:3ac96e360672 451 podata->bin_seq[bin_seq_length] = bin_cfg;
charlesmn 0:3ac96e360672 452 bin_seq_length++;
charlesmn 0:3ac96e360672 453 }
charlesmn 0:3ac96e360672 454
charlesmn 0:3ac96e360672 455 bin_repeat_count[bin_cfg]++;
charlesmn 0:3ac96e360672 456
charlesmn 0:3ac96e360672 457
charlesmn 0:3ac96e360672 458
charlesmn 0:3ac96e360672 459 VL53L1_p_018 = bin_initial_index[bin_cfg];
charlesmn 0:3ac96e360672 460
charlesmn 0:3ac96e360672 461 for (i = 0 ; i < 4 ; i++)
charlesmn 0:3ac96e360672 462 podata->bin_data[VL53L1_p_018+i] +=
charlesmn 0:3ac96e360672 463 pidata->bin_data[lc*4+i];
charlesmn 0:3ac96e360672 464
charlesmn 0:3ac96e360672 465 }
charlesmn 0:3ac96e360672 466
charlesmn 0:3ac96e360672 467
charlesmn 0:3ac96e360672 468
charlesmn 0:3ac96e360672 469 for (lc = 0 ; lc < VL53L1_MAX_BIN_SEQUENCE_LENGTH ; lc++) {
charlesmn 0:3ac96e360672 470
charlesmn 0:3ac96e360672 471 bin_cfg = podata->bin_seq[lc];
charlesmn 0:3ac96e360672 472
charlesmn 0:3ac96e360672 473 if (bin_cfg <= VL53L1_MAX_BIN_SEQUENCE_CODE)
charlesmn 0:3ac96e360672 474 podata->bin_rep[lc] =
charlesmn 0:3ac96e360672 475 bin_repeat_count[bin_cfg];
charlesmn 0:3ac96e360672 476 else
charlesmn 0:3ac96e360672 477 podata->bin_rep[lc] = 0;
charlesmn 0:3ac96e360672 478 }
charlesmn 0:3ac96e360672 479
charlesmn 0:3ac96e360672 480 podata->VL53L1_p_024 = bin_seq_length * 4;
charlesmn 0:3ac96e360672 481
charlesmn 0:3ac96e360672 482
charlesmn 0:3ac96e360672 483
charlesmn 0:3ac96e360672 484
charlesmn 0:3ac96e360672 485
charlesmn 0:3ac96e360672 486 for (lc = 0 ; lc <= VL53L1_MAX_BIN_SEQUENCE_CODE ; lc++) {
charlesmn 0:3ac96e360672 487
charlesmn 0:3ac96e360672 488 repeat_count = (int32_t)bin_repeat_count[lc];
charlesmn 0:3ac96e360672 489
charlesmn 0:3ac96e360672 490 if (repeat_count > 0) {
charlesmn 0:3ac96e360672 491
charlesmn 0:3ac96e360672 492 VL53L1_p_018 = bin_initial_index[lc];
charlesmn 0:3ac96e360672 493
charlesmn 0:3ac96e360672 494 for (i = 0 ; i < 4 ; i++) {
charlesmn 0:3ac96e360672 495 podata->bin_data[VL53L1_p_018+i] +=
charlesmn 0:3ac96e360672 496 (repeat_count/2);
charlesmn 0:3ac96e360672 497 podata->bin_data[VL53L1_p_018+i] /=
charlesmn 0:3ac96e360672 498 repeat_count;
charlesmn 0:3ac96e360672 499 }
charlesmn 0:3ac96e360672 500 }
charlesmn 0:3ac96e360672 501 }
charlesmn 0:3ac96e360672 502
charlesmn 0:3ac96e360672 503
charlesmn 0:3ac96e360672 504
charlesmn 0:3ac96e360672 505 podata->number_of_ambient_bins = 0;
charlesmn 0:3ac96e360672 506 if ((bin_repeat_count[7] > 0) ||
charlesmn 0:3ac96e360672 507 (bin_repeat_count[15] > 0))
charlesmn 0:3ac96e360672 508 podata->number_of_ambient_bins = 4;
charlesmn 0:3ac96e360672 509
charlesmn 0:3ac96e360672 510 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 511
charlesmn 0:3ac96e360672 512 return status;
charlesmn 0:3ac96e360672 513 }
charlesmn 0:3ac96e360672 514
charlesmn 0:3ac96e360672 515