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@7:1add29d51e72, 2021-06-08 (annotated)

Committer:
Charles MacNeill
Date:
Tue Jun 08 10:34:47 2021 +0100
Revision:
7:1add29d51e72
Parent:
0:3ac96e360672
Child:
18:0696efe39d08
Update to v6.6.5 of bare_driver

Who changed what in which revision?

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