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_core.c@5:89031b2f5316, 2021-07-14 (annotated)

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

Who changed what in which revision?

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