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@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_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