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_core_support.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: GPL-2.0+ OR 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 and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either GPL-2.0+
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
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14
Charles MacNeill 5:89031b2f5316 15 #include <vl53lx_platform_log.h>
Charles MacNeill 5:89031b2f5316 16 #include <vl53lx_platform_user_data.h>
Charles MacNeill 5:89031b2f5316 17 #include <vl53lx_platform_user_defines.h>
Charles MacNeill 5:89031b2f5316 18 #include "vl53lx_ll_def.h"
Charles MacNeill 5:89031b2f5316 19 #include "vl53lx_ll_device.h"
Charles MacNeill 5:89031b2f5316 20 #include "vl53lx_core_support.h"
Charles MacNeill 5:89031b2f5316 21
Charles MacNeill 5:89031b2f5316 22
Charles MacNeill 5:89031b2f5316 23
Charles MacNeill 5:89031b2f5316 24 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 25 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 26 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 27 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 28 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 29 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_CORE, \
Charles MacNeill 5:89031b2f5316 30 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 31
Charles MacNeill 5:89031b2f5316 32 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 33 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_CORE, \
Charles MacNeill 5:89031b2f5316 34 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 35
Charles MacNeill 5:89031b2f5316 36
Charles MacNeill 5:89031b2f5316 37 uint32_t VL53LX_calc_pll_period_us(
Charles MacNeill 5:89031b2f5316 38 uint16_t fast_osc_frequency)
Charles MacNeill 5:89031b2f5316 39 {
Charles MacNeill 5:89031b2f5316 40
Charles MacNeill 5:89031b2f5316 41
Charles MacNeill 5:89031b2f5316 42 uint32_t pll_period_us = 0;
Charles MacNeill 5:89031b2f5316 43
Charles MacNeill 5:89031b2f5316 44 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 45
Charles MacNeill 5:89031b2f5316 46 if (fast_osc_frequency > 0)
Charles MacNeill 5:89031b2f5316 47 pll_period_us = (0x01 << 30) / fast_osc_frequency;
Charles MacNeill 5:89031b2f5316 48
Charles MacNeill 5:89031b2f5316 49
Charles MacNeill 5:89031b2f5316 50
Charles MacNeill 5:89031b2f5316 51 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 52
Charles MacNeill 5:89031b2f5316 53 return pll_period_us;
Charles MacNeill 5:89031b2f5316 54 }
Charles MacNeill 5:89031b2f5316 55
Charles MacNeill 5:89031b2f5316 56
Charles MacNeill 5:89031b2f5316 57 uint32_t VL53LX_duration_maths(
Charles MacNeill 5:89031b2f5316 58 uint32_t pll_period_us,
Charles MacNeill 5:89031b2f5316 59 uint32_t vcsel_parm_pclks,
Charles MacNeill 5:89031b2f5316 60 uint32_t window_vclks,
Charles MacNeill 5:89031b2f5316 61 uint32_t elapsed_mclks)
Charles MacNeill 5:89031b2f5316 62 {
Charles MacNeill 5:89031b2f5316 63
Charles MacNeill 5:89031b2f5316 64
Charles MacNeill 5:89031b2f5316 65 uint64_t tmp_long_int = 0;
Charles MacNeill 5:89031b2f5316 66 uint32_t duration_us = 0;
Charles MacNeill 5:89031b2f5316 67
Charles MacNeill 5:89031b2f5316 68
Charles MacNeill 5:89031b2f5316 69 duration_us = window_vclks * pll_period_us;
Charles MacNeill 5:89031b2f5316 70
Charles MacNeill 5:89031b2f5316 71
Charles MacNeill 5:89031b2f5316 72 duration_us = duration_us >> 12;
Charles MacNeill 5:89031b2f5316 73
Charles MacNeill 5:89031b2f5316 74
Charles MacNeill 5:89031b2f5316 75 tmp_long_int = (uint64_t)duration_us;
Charles MacNeill 5:89031b2f5316 76
Charles MacNeill 5:89031b2f5316 77
Charles MacNeill 5:89031b2f5316 78 duration_us = elapsed_mclks * vcsel_parm_pclks;
Charles MacNeill 5:89031b2f5316 79
Charles MacNeill 5:89031b2f5316 80
Charles MacNeill 5:89031b2f5316 81 duration_us = duration_us >> 4;
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83
Charles MacNeill 5:89031b2f5316 84 tmp_long_int = tmp_long_int * (uint64_t)duration_us;
Charles MacNeill 5:89031b2f5316 85
Charles MacNeill 5:89031b2f5316 86
Charles MacNeill 5:89031b2f5316 87 tmp_long_int = tmp_long_int >> 12;
Charles MacNeill 5:89031b2f5316 88
Charles MacNeill 5:89031b2f5316 89
Charles MacNeill 5:89031b2f5316 90 if (tmp_long_int > 0xFFFFFFFF)
Charles MacNeill 5:89031b2f5316 91 tmp_long_int = 0xFFFFFFFF;
Charles MacNeill 5:89031b2f5316 92
Charles MacNeill 5:89031b2f5316 93 duration_us = (uint32_t)tmp_long_int;
Charles MacNeill 5:89031b2f5316 94
Charles MacNeill 5:89031b2f5316 95 return duration_us;
Charles MacNeill 5:89031b2f5316 96 }
Charles MacNeill 5:89031b2f5316 97
Charles MacNeill 5:89031b2f5316 98
Charles MacNeill 5:89031b2f5316 99 uint32_t VL53LX_events_per_spad_maths(
Charles MacNeill 5:89031b2f5316 100 int32_t VL53LX_p_010,
Charles MacNeill 5:89031b2f5316 101 uint16_t num_spads,
Charles MacNeill 5:89031b2f5316 102 uint32_t duration)
Charles MacNeill 5:89031b2f5316 103 {
Charles MacNeill 5:89031b2f5316 104 uint64_t total_hist_counts = 0;
Charles MacNeill 5:89031b2f5316 105 uint64_t xtalk_per_spad = 0;
Charles MacNeill 5:89031b2f5316 106 uint32_t rate_per_spad_kcps = 0;
Charles MacNeill 5:89031b2f5316 107
Charles MacNeill 5:89031b2f5316 108
Charles MacNeill 5:89031b2f5316 109
Charles MacNeill 5:89031b2f5316 110
Charles MacNeill 5:89031b2f5316 111
Charles MacNeill 5:89031b2f5316 112
Charles MacNeill 5:89031b2f5316 113 uint64_t dividend = ((uint64_t)VL53LX_p_010
Charles MacNeill 5:89031b2f5316 114 * 1000 * 256);
Charles MacNeill 5:89031b2f5316 115
Charles MacNeill 5:89031b2f5316 116 if (num_spads != 0)
Charles MacNeill 5:89031b2f5316 117 total_hist_counts = do_division_u(
Charles MacNeill 5:89031b2f5316 118 dividend, (uint64_t)num_spads);
Charles MacNeill 5:89031b2f5316 119
Charles MacNeill 5:89031b2f5316 120
Charles MacNeill 5:89031b2f5316 121
Charles MacNeill 5:89031b2f5316 122 if (duration > 0) {
Charles MacNeill 5:89031b2f5316 123
Charles MacNeill 5:89031b2f5316 124
Charles MacNeill 5:89031b2f5316 125 uint64_t dividend = (((uint64_t)(total_hist_counts << 11))
Charles MacNeill 5:89031b2f5316 126 + ((uint64_t)duration / 2));
Charles MacNeill 5:89031b2f5316 127
Charles MacNeill 5:89031b2f5316 128 xtalk_per_spad = do_division_u(dividend, (uint64_t)duration);
Charles MacNeill 5:89031b2f5316 129 } else {
Charles MacNeill 5:89031b2f5316 130 xtalk_per_spad = (uint64_t)(total_hist_counts << 11);
Charles MacNeill 5:89031b2f5316 131 }
Charles MacNeill 5:89031b2f5316 132
Charles MacNeill 5:89031b2f5316 133 rate_per_spad_kcps = (uint32_t)xtalk_per_spad;
Charles MacNeill 5:89031b2f5316 134
Charles MacNeill 5:89031b2f5316 135 return rate_per_spad_kcps;
Charles MacNeill 5:89031b2f5316 136 }
Charles MacNeill 5:89031b2f5316 137
Charles MacNeill 5:89031b2f5316 138
Charles MacNeill 5:89031b2f5316 139 uint32_t VL53LX_isqrt(uint32_t num)
Charles MacNeill 5:89031b2f5316 140 {
Charles MacNeill 5:89031b2f5316 141
Charles MacNeill 5:89031b2f5316 142
Charles MacNeill 5:89031b2f5316 143
Charles MacNeill 5:89031b2f5316 144 uint32_t res = 0;
Charles MacNeill 5:89031b2f5316 145 uint32_t bit = 1 << 30;
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147
Charles MacNeill 5:89031b2f5316 148 while (bit > num)
Charles MacNeill 5:89031b2f5316 149 bit >>= 2;
Charles MacNeill 5:89031b2f5316 150
Charles MacNeill 5:89031b2f5316 151 while (bit != 0) {
Charles MacNeill 5:89031b2f5316 152 if (num >= res + bit) {
Charles MacNeill 5:89031b2f5316 153 num -= res + bit;
Charles MacNeill 5:89031b2f5316 154 res = (res >> 1) + bit;
Charles MacNeill 5:89031b2f5316 155 } else {
Charles MacNeill 5:89031b2f5316 156 res >>= 1;
Charles MacNeill 5:89031b2f5316 157 }
Charles MacNeill 5:89031b2f5316 158 bit >>= 2;
Charles MacNeill 5:89031b2f5316 159 }
Charles MacNeill 5:89031b2f5316 160
Charles MacNeill 5:89031b2f5316 161 return res;
Charles MacNeill 5:89031b2f5316 162 }
Charles MacNeill 5:89031b2f5316 163
Charles MacNeill 5:89031b2f5316 164
Charles MacNeill 5:89031b2f5316 165 void VL53LX_hist_calc_zero_distance_phase(
Charles MacNeill 5:89031b2f5316 166 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 167 {
Charles MacNeill 5:89031b2f5316 168
Charles MacNeill 5:89031b2f5316 169
Charles MacNeill 5:89031b2f5316 170 uint32_t period = 0;
Charles MacNeill 5:89031b2f5316 171 uint32_t VL53LX_p_014 = 0;
Charles MacNeill 5:89031b2f5316 172
Charles MacNeill 5:89031b2f5316 173 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 174
Charles MacNeill 5:89031b2f5316 175 period = 2048 *
Charles MacNeill 5:89031b2f5316 176 (uint32_t)VL53LX_decode_vcsel_period(pdata->VL53LX_p_005);
Charles MacNeill 5:89031b2f5316 177
Charles MacNeill 5:89031b2f5316 178 VL53LX_p_014 = period;
Charles MacNeill 5:89031b2f5316 179 VL53LX_p_014 += (uint32_t)pdata->phasecal_result__reference_phase;
Charles MacNeill 5:89031b2f5316 180 VL53LX_p_014 += (2048 * (uint32_t)pdata->phasecal_result__vcsel_start);
Charles MacNeill 5:89031b2f5316 181 VL53LX_p_014 -= (2048 * (uint32_t)pdata->cal_config__vcsel_start);
Charles MacNeill 5:89031b2f5316 182
Charles MacNeill 5:89031b2f5316 183 VL53LX_p_014 = VL53LX_p_014 % period;
Charles MacNeill 5:89031b2f5316 184
Charles MacNeill 5:89031b2f5316 185 pdata->zero_distance_phase = (uint16_t)VL53LX_p_014;
Charles MacNeill 5:89031b2f5316 186
Charles MacNeill 5:89031b2f5316 187 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 188 }
Charles MacNeill 5:89031b2f5316 189
Charles MacNeill 5:89031b2f5316 190
Charles MacNeill 5:89031b2f5316 191 void VL53LX_hist_estimate_ambient_from_thresholded_bins(
Charles MacNeill 5:89031b2f5316 192 int32_t ambient_threshold_sigma,
Charles MacNeill 5:89031b2f5316 193 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 194 {
Charles MacNeill 5:89031b2f5316 195
Charles MacNeill 5:89031b2f5316 196
Charles MacNeill 5:89031b2f5316 197 uint8_t bin = 0;
Charles MacNeill 5:89031b2f5316 198 int32_t VL53LX_p_031 = 0;
Charles MacNeill 5:89031b2f5316 199
Charles MacNeill 5:89031b2f5316 200 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 201
Charles MacNeill 5:89031b2f5316 202
Charles MacNeill 5:89031b2f5316 203
Charles MacNeill 5:89031b2f5316 204 VL53LX_hist_find_min_max_bin_values(pdata);
Charles MacNeill 5:89031b2f5316 205
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207
Charles MacNeill 5:89031b2f5316 208 VL53LX_p_031 =
Charles MacNeill 5:89031b2f5316 209 (int32_t)VL53LX_isqrt((uint32_t)pdata->min_bin_value);
Charles MacNeill 5:89031b2f5316 210 VL53LX_p_031 *= ambient_threshold_sigma;
Charles MacNeill 5:89031b2f5316 211 VL53LX_p_031 += 0x07;
Charles MacNeill 5:89031b2f5316 212 VL53LX_p_031 = VL53LX_p_031 >> 4;
Charles MacNeill 5:89031b2f5316 213 VL53LX_p_031 += pdata->min_bin_value;
Charles MacNeill 5:89031b2f5316 214
Charles MacNeill 5:89031b2f5316 215
Charles MacNeill 5:89031b2f5316 216
Charles MacNeill 5:89031b2f5316 217 pdata->number_of_ambient_samples = 0;
Charles MacNeill 5:89031b2f5316 218 pdata->ambient_events_sum = 0;
Charles MacNeill 5:89031b2f5316 219
Charles MacNeill 5:89031b2f5316 220 for (bin = 0; bin < pdata->VL53LX_p_021; bin++)
Charles MacNeill 5:89031b2f5316 221 if (pdata->bin_data[bin] < VL53LX_p_031) {
Charles MacNeill 5:89031b2f5316 222 pdata->ambient_events_sum += pdata->bin_data[bin];
Charles MacNeill 5:89031b2f5316 223 pdata->number_of_ambient_samples++;
Charles MacNeill 5:89031b2f5316 224 }
Charles MacNeill 5:89031b2f5316 225
Charles MacNeill 5:89031b2f5316 226
Charles MacNeill 5:89031b2f5316 227
Charles MacNeill 5:89031b2f5316 228 if (pdata->number_of_ambient_samples > 0) {
Charles MacNeill 5:89031b2f5316 229 pdata->VL53LX_p_028 =
Charles MacNeill 5:89031b2f5316 230 pdata->ambient_events_sum;
Charles MacNeill 5:89031b2f5316 231 pdata->VL53LX_p_028 +=
Charles MacNeill 5:89031b2f5316 232 ((int32_t)pdata->number_of_ambient_samples/2);
Charles MacNeill 5:89031b2f5316 233 pdata->VL53LX_p_028 /=
Charles MacNeill 5:89031b2f5316 234 (int32_t)pdata->number_of_ambient_samples;
Charles MacNeill 5:89031b2f5316 235 }
Charles MacNeill 5:89031b2f5316 236
Charles MacNeill 5:89031b2f5316 237 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 238 }
Charles MacNeill 5:89031b2f5316 239
Charles MacNeill 5:89031b2f5316 240
Charles MacNeill 5:89031b2f5316 241 void VL53LX_hist_remove_ambient_bins(
Charles MacNeill 5:89031b2f5316 242 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 243 {
Charles MacNeill 5:89031b2f5316 244
Charles MacNeill 5:89031b2f5316 245
Charles MacNeill 5:89031b2f5316 246
Charles MacNeill 5:89031b2f5316 247 uint8_t bin = 0;
Charles MacNeill 5:89031b2f5316 248 uint8_t lc = 0;
Charles MacNeill 5:89031b2f5316 249 uint8_t i = 0;
Charles MacNeill 5:89031b2f5316 250
Charles MacNeill 5:89031b2f5316 251
Charles MacNeill 5:89031b2f5316 252
Charles MacNeill 5:89031b2f5316 253 if ((pdata->bin_seq[0] & 0x07) == 0x07) {
Charles MacNeill 5:89031b2f5316 254
Charles MacNeill 5:89031b2f5316 255 i = 0;
Charles MacNeill 5:89031b2f5316 256 for (lc = 0; lc < VL53LX_MAX_BIN_SEQUENCE_LENGTH; lc++) {
Charles MacNeill 5:89031b2f5316 257 if ((pdata->bin_seq[lc] & 0x07) != 0x07) {
Charles MacNeill 5:89031b2f5316 258 pdata->bin_seq[i] = pdata->bin_seq[lc];
Charles MacNeill 5:89031b2f5316 259 pdata->bin_rep[i] = pdata->bin_rep[lc];
Charles MacNeill 5:89031b2f5316 260 i++;
Charles MacNeill 5:89031b2f5316 261 }
Charles MacNeill 5:89031b2f5316 262 }
Charles MacNeill 5:89031b2f5316 263
Charles MacNeill 5:89031b2f5316 264
Charles MacNeill 5:89031b2f5316 265
Charles MacNeill 5:89031b2f5316 266 for (lc = i; lc < VL53LX_MAX_BIN_SEQUENCE_LENGTH; lc++) {
Charles MacNeill 5:89031b2f5316 267 pdata->bin_seq[lc] = VL53LX_MAX_BIN_SEQUENCE_CODE + 1;
Charles MacNeill 5:89031b2f5316 268 pdata->bin_rep[lc] = 0;
Charles MacNeill 5:89031b2f5316 269 }
Charles MacNeill 5:89031b2f5316 270 }
Charles MacNeill 5:89031b2f5316 271
Charles MacNeill 5:89031b2f5316 272 if (pdata->number_of_ambient_bins > 0) {
Charles MacNeill 5:89031b2f5316 273
Charles MacNeill 5:89031b2f5316 274
Charles MacNeill 5:89031b2f5316 275 for (bin = pdata->number_of_ambient_bins;
Charles MacNeill 5:89031b2f5316 276 bin < pdata->VL53LX_p_020; bin++) {
Charles MacNeill 5:89031b2f5316 277 pdata->bin_data[bin-pdata->number_of_ambient_bins] =
Charles MacNeill 5:89031b2f5316 278 pdata->bin_data[bin];
Charles MacNeill 5:89031b2f5316 279 }
Charles MacNeill 5:89031b2f5316 280
Charles MacNeill 5:89031b2f5316 281
Charles MacNeill 5:89031b2f5316 282 pdata->VL53LX_p_021 =
Charles MacNeill 5:89031b2f5316 283 pdata->VL53LX_p_021 -
Charles MacNeill 5:89031b2f5316 284 pdata->number_of_ambient_bins;
Charles MacNeill 5:89031b2f5316 285 pdata->number_of_ambient_bins = 0;
Charles MacNeill 5:89031b2f5316 286 }
Charles MacNeill 5:89031b2f5316 287 }
Charles MacNeill 5:89031b2f5316 288
Charles MacNeill 5:89031b2f5316 289
Charles MacNeill 5:89031b2f5316 290 uint32_t VL53LX_calc_pll_period_mm(
Charles MacNeill 5:89031b2f5316 291 uint16_t fast_osc_frequency)
Charles MacNeill 5:89031b2f5316 292 {
Charles MacNeill 5:89031b2f5316 293
Charles MacNeill 5:89031b2f5316 294
Charles MacNeill 5:89031b2f5316 295 uint32_t pll_period_us = 0;
Charles MacNeill 5:89031b2f5316 296 uint32_t pll_period_mm = 0;
Charles MacNeill 5:89031b2f5316 297
Charles MacNeill 5:89031b2f5316 298 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 299
Charles MacNeill 5:89031b2f5316 300
Charles MacNeill 5:89031b2f5316 301
Charles MacNeill 5:89031b2f5316 302 pll_period_us = VL53LX_calc_pll_period_us(fast_osc_frequency);
Charles MacNeill 5:89031b2f5316 303
Charles MacNeill 5:89031b2f5316 304
Charles MacNeill 5:89031b2f5316 305
Charles MacNeill 5:89031b2f5316 306
Charles MacNeill 5:89031b2f5316 307 pll_period_mm =
Charles MacNeill 5:89031b2f5316 308 VL53LX_SPEED_OF_LIGHT_IN_AIR_DIV_8 *
Charles MacNeill 5:89031b2f5316 309 (pll_period_us >> 2);
Charles MacNeill 5:89031b2f5316 310
Charles MacNeill 5:89031b2f5316 311
Charles MacNeill 5:89031b2f5316 312 pll_period_mm = (pll_period_mm + (0x01<<15)) >> 16;
Charles MacNeill 5:89031b2f5316 313
Charles MacNeill 5:89031b2f5316 314 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 315
Charles MacNeill 5:89031b2f5316 316 return pll_period_mm;
Charles MacNeill 5:89031b2f5316 317 }
Charles MacNeill 5:89031b2f5316 318
Charles MacNeill 5:89031b2f5316 319
Charles MacNeill 5:89031b2f5316 320 uint16_t VL53LX_rate_maths(
Charles MacNeill 5:89031b2f5316 321 int32_t VL53LX_p_018,
Charles MacNeill 5:89031b2f5316 322 uint32_t time_us)
Charles MacNeill 5:89031b2f5316 323 {
Charles MacNeill 5:89031b2f5316 324
Charles MacNeill 5:89031b2f5316 325
Charles MacNeill 5:89031b2f5316 326 uint32_t tmp_int = 0;
Charles MacNeill 5:89031b2f5316 327 uint32_t frac_bits = 7;
Charles MacNeill 5:89031b2f5316 328 uint16_t rate_mcps = 0;
Charles MacNeill 5:89031b2f5316 329
Charles MacNeill 5:89031b2f5316 330
Charles MacNeill 5:89031b2f5316 331
Charles MacNeill 5:89031b2f5316 332 if (VL53LX_p_018 > VL53LX_SPAD_TOTAL_COUNT_MAX)
Charles MacNeill 5:89031b2f5316 333 tmp_int = VL53LX_SPAD_TOTAL_COUNT_MAX;
Charles MacNeill 5:89031b2f5316 334 else if (VL53LX_p_018 > 0)
Charles MacNeill 5:89031b2f5316 335 tmp_int = (uint32_t)VL53LX_p_018;
Charles MacNeill 5:89031b2f5316 336
Charles MacNeill 5:89031b2f5316 337
Charles MacNeill 5:89031b2f5316 338
Charles MacNeill 5:89031b2f5316 339
Charles MacNeill 5:89031b2f5316 340 if (VL53LX_p_018 > VL53LX_SPAD_TOTAL_COUNT_RES_THRES)
Charles MacNeill 5:89031b2f5316 341 frac_bits = 3;
Charles MacNeill 5:89031b2f5316 342 else
Charles MacNeill 5:89031b2f5316 343 frac_bits = 7;
Charles MacNeill 5:89031b2f5316 344
Charles MacNeill 5:89031b2f5316 345
Charles MacNeill 5:89031b2f5316 346 if (time_us > 0)
Charles MacNeill 5:89031b2f5316 347 tmp_int = ((tmp_int << frac_bits) + (time_us / 2)) / time_us;
Charles MacNeill 5:89031b2f5316 348
Charles MacNeill 5:89031b2f5316 349
Charles MacNeill 5:89031b2f5316 350 if (VL53LX_p_018 > VL53LX_SPAD_TOTAL_COUNT_RES_THRES)
Charles MacNeill 5:89031b2f5316 351 tmp_int = tmp_int << 4;
Charles MacNeill 5:89031b2f5316 352
Charles MacNeill 5:89031b2f5316 353
Charles MacNeill 5:89031b2f5316 354
Charles MacNeill 5:89031b2f5316 355 if (tmp_int > 0xFFFF)
Charles MacNeill 5:89031b2f5316 356 tmp_int = 0xFFFF;
Charles MacNeill 5:89031b2f5316 357
Charles MacNeill 5:89031b2f5316 358 rate_mcps = (uint16_t)tmp_int;
Charles MacNeill 5:89031b2f5316 359
Charles MacNeill 5:89031b2f5316 360 return rate_mcps;
Charles MacNeill 5:89031b2f5316 361 }
Charles MacNeill 5:89031b2f5316 362
Charles MacNeill 5:89031b2f5316 363
Charles MacNeill 5:89031b2f5316 364 uint16_t VL53LX_rate_per_spad_maths(
Charles MacNeill 5:89031b2f5316 365 uint32_t frac_bits,
Charles MacNeill 5:89031b2f5316 366 uint32_t peak_count_rate,
Charles MacNeill 5:89031b2f5316 367 uint16_t num_spads,
Charles MacNeill 5:89031b2f5316 368 uint32_t max_output_value)
Charles MacNeill 5:89031b2f5316 369 {
Charles MacNeill 5:89031b2f5316 370
Charles MacNeill 5:89031b2f5316 371 uint32_t tmp_int = 0;
Charles MacNeill 5:89031b2f5316 372
Charles MacNeill 5:89031b2f5316 373
Charles MacNeill 5:89031b2f5316 374 uint16_t rate_per_spad = 0;
Charles MacNeill 5:89031b2f5316 375
Charles MacNeill 5:89031b2f5316 376
Charles MacNeill 5:89031b2f5316 377
Charles MacNeill 5:89031b2f5316 378
Charles MacNeill 5:89031b2f5316 379
Charles MacNeill 5:89031b2f5316 380 if (num_spads > 0) {
Charles MacNeill 5:89031b2f5316 381 tmp_int = (peak_count_rate << 8) << frac_bits;
Charles MacNeill 5:89031b2f5316 382 tmp_int = (tmp_int +
Charles MacNeill 5:89031b2f5316 383 ((uint32_t)num_spads / 2)) /
Charles MacNeill 5:89031b2f5316 384 (uint32_t)num_spads;
Charles MacNeill 5:89031b2f5316 385 } else {
Charles MacNeill 5:89031b2f5316 386 tmp_int = ((peak_count_rate) << frac_bits);
Charles MacNeill 5:89031b2f5316 387 }
Charles MacNeill 5:89031b2f5316 388
Charles MacNeill 5:89031b2f5316 389
Charles MacNeill 5:89031b2f5316 390
Charles MacNeill 5:89031b2f5316 391 if (tmp_int > max_output_value)
Charles MacNeill 5:89031b2f5316 392 tmp_int = max_output_value;
Charles MacNeill 5:89031b2f5316 393
Charles MacNeill 5:89031b2f5316 394 rate_per_spad = (uint16_t)tmp_int;
Charles MacNeill 5:89031b2f5316 395
Charles MacNeill 5:89031b2f5316 396 return rate_per_spad;
Charles MacNeill 5:89031b2f5316 397 }
Charles MacNeill 5:89031b2f5316 398
Charles MacNeill 5:89031b2f5316 399
Charles MacNeill 5:89031b2f5316 400 int32_t VL53LX_range_maths(
Charles MacNeill 5:89031b2f5316 401 uint16_t fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 402 uint16_t VL53LX_p_014,
Charles MacNeill 5:89031b2f5316 403 uint16_t zero_distance_phase,
Charles MacNeill 5:89031b2f5316 404 uint8_t fractional_bits,
Charles MacNeill 5:89031b2f5316 405 int32_t gain_factor,
Charles MacNeill 5:89031b2f5316 406 int32_t range_offset_mm)
Charles MacNeill 5:89031b2f5316 407 {
Charles MacNeill 5:89031b2f5316 408
Charles MacNeill 5:89031b2f5316 409
Charles MacNeill 5:89031b2f5316 410 uint32_t pll_period_us = 0;
Charles MacNeill 5:89031b2f5316 411 int64_t tmp_long_int = 0;
Charles MacNeill 5:89031b2f5316 412 int32_t range_mm = 0;
Charles MacNeill 5:89031b2f5316 413 int32_t range_mm_10 = 0;
Charles MacNeill 5:89031b2f5316 414
Charles MacNeill 5:89031b2f5316 415
Charles MacNeill 5:89031b2f5316 416
Charles MacNeill 5:89031b2f5316 417 pll_period_us = VL53LX_calc_pll_period_us(fast_osc_frequency);
Charles MacNeill 5:89031b2f5316 418
Charles MacNeill 5:89031b2f5316 419
Charles MacNeill 5:89031b2f5316 420
Charles MacNeill 5:89031b2f5316 421 tmp_long_int = (int64_t)VL53LX_p_014 - (int64_t)zero_distance_phase;
Charles MacNeill 5:89031b2f5316 422
Charles MacNeill 5:89031b2f5316 423
Charles MacNeill 5:89031b2f5316 424
Charles MacNeill 5:89031b2f5316 425 tmp_long_int = tmp_long_int * (int64_t)pll_period_us;
Charles MacNeill 5:89031b2f5316 426
Charles MacNeill 5:89031b2f5316 427
Charles MacNeill 5:89031b2f5316 428
Charles MacNeill 5:89031b2f5316 429 tmp_long_int = tmp_long_int / (0x01 << 9);
Charles MacNeill 5:89031b2f5316 430
Charles MacNeill 5:89031b2f5316 431
Charles MacNeill 5:89031b2f5316 432
Charles MacNeill 5:89031b2f5316 433 tmp_long_int = tmp_long_int * VL53LX_SPEED_OF_LIGHT_IN_AIR_DIV_8;
Charles MacNeill 5:89031b2f5316 434
Charles MacNeill 5:89031b2f5316 435
Charles MacNeill 5:89031b2f5316 436
Charles MacNeill 5:89031b2f5316 437 tmp_long_int = tmp_long_int / (0x01 << 22);
Charles MacNeill 5:89031b2f5316 438
Charles MacNeill 5:89031b2f5316 439
Charles MacNeill 5:89031b2f5316 440 range_mm = (int32_t)tmp_long_int + range_offset_mm;
Charles MacNeill 5:89031b2f5316 441
Charles MacNeill 5:89031b2f5316 442
Charles MacNeill 5:89031b2f5316 443 range_mm *= gain_factor;
Charles MacNeill 5:89031b2f5316 444 range_mm += 0x0400;
Charles MacNeill 5:89031b2f5316 445 range_mm /= 0x0800;
Charles MacNeill 5:89031b2f5316 446
Charles MacNeill 5:89031b2f5316 447
Charles MacNeill 5:89031b2f5316 448 if (fractional_bits == 0) {
Charles MacNeill 5:89031b2f5316 449 range_mm_10 = range_mm * 10;
Charles MacNeill 5:89031b2f5316 450 range_mm_10 = range_mm_10 / (0x01 << 2);
Charles MacNeill 5:89031b2f5316 451 if ((range_mm_10 % 10) < 5)
Charles MacNeill 5:89031b2f5316 452 range_mm = (int16_t)(range_mm_10 / 10);
Charles MacNeill 5:89031b2f5316 453 else
Charles MacNeill 5:89031b2f5316 454 range_mm = (int16_t)(range_mm_10 / 10 + 1);
Charles MacNeill 5:89031b2f5316 455 } else if (fractional_bits == 1)
Charles MacNeill 5:89031b2f5316 456 range_mm = range_mm / (0x01 << 1);
Charles MacNeill 5:89031b2f5316 457
Charles MacNeill 5:89031b2f5316 458 return range_mm;
Charles MacNeill 5:89031b2f5316 459 }
Charles MacNeill 5:89031b2f5316 460
Charles MacNeill 5:89031b2f5316 461
Charles MacNeill 5:89031b2f5316 462 uint8_t VL53LX_decode_vcsel_period(uint8_t vcsel_period_reg)
Charles MacNeill 5:89031b2f5316 463 {
Charles MacNeill 5:89031b2f5316 464
Charles MacNeill 5:89031b2f5316 465
Charles MacNeill 5:89031b2f5316 466 uint8_t VL53LX_p_030 = 0;
Charles MacNeill 5:89031b2f5316 467
Charles MacNeill 5:89031b2f5316 468 VL53LX_p_030 = (vcsel_period_reg + 1) << 1;
Charles MacNeill 5:89031b2f5316 469
Charles MacNeill 5:89031b2f5316 470 return VL53LX_p_030;
Charles MacNeill 5:89031b2f5316 471 }
Charles MacNeill 5:89031b2f5316 472
Charles MacNeill 5:89031b2f5316 473
Charles MacNeill 5:89031b2f5316 474 void VL53LX_copy_xtalk_bin_data_to_histogram_data_struct(
Charles MacNeill 5:89031b2f5316 475 VL53LX_xtalk_histogram_shape_t *pxtalk,
Charles MacNeill 5:89031b2f5316 476 VL53LX_histogram_bin_data_t *phist)
Charles MacNeill 5:89031b2f5316 477 {
Charles MacNeill 5:89031b2f5316 478
Charles MacNeill 5:89031b2f5316 479
Charles MacNeill 5:89031b2f5316 480 phist->cal_config__vcsel_start =
Charles MacNeill 5:89031b2f5316 481 pxtalk->cal_config__vcsel_start;
Charles MacNeill 5:89031b2f5316 482 phist->VL53LX_p_015 =
Charles MacNeill 5:89031b2f5316 483 pxtalk->VL53LX_p_015;
Charles MacNeill 5:89031b2f5316 484 phist->VL53LX_p_019 =
Charles MacNeill 5:89031b2f5316 485 pxtalk->VL53LX_p_019;
Charles MacNeill 5:89031b2f5316 486
Charles MacNeill 5:89031b2f5316 487 phist->phasecal_result__reference_phase =
Charles MacNeill 5:89031b2f5316 488 pxtalk->phasecal_result__reference_phase;
Charles MacNeill 5:89031b2f5316 489 phist->phasecal_result__vcsel_start =
Charles MacNeill 5:89031b2f5316 490 pxtalk->phasecal_result__vcsel_start;
Charles MacNeill 5:89031b2f5316 491
Charles MacNeill 5:89031b2f5316 492 phist->vcsel_width =
Charles MacNeill 5:89031b2f5316 493 pxtalk->vcsel_width;
Charles MacNeill 5:89031b2f5316 494 phist->zero_distance_phase =
Charles MacNeill 5:89031b2f5316 495 pxtalk->zero_distance_phase;
Charles MacNeill 5:89031b2f5316 496
Charles MacNeill 5:89031b2f5316 497 phist->zone_id = pxtalk->zone_id;
Charles MacNeill 5:89031b2f5316 498 phist->VL53LX_p_020 = pxtalk->VL53LX_p_020;
Charles MacNeill 5:89031b2f5316 499 phist->time_stamp = pxtalk->time_stamp;
Charles MacNeill 5:89031b2f5316 500 }
Charles MacNeill 5:89031b2f5316 501
Charles MacNeill 5:89031b2f5316 502
Charles MacNeill 5:89031b2f5316 503 void VL53LX_init_histogram_bin_data_struct(
Charles MacNeill 5:89031b2f5316 504 int32_t bin_value,
Charles MacNeill 5:89031b2f5316 505 uint16_t VL53LX_p_021,
Charles MacNeill 5:89031b2f5316 506 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 507 {
Charles MacNeill 5:89031b2f5316 508
Charles MacNeill 5:89031b2f5316 509
Charles MacNeill 5:89031b2f5316 510
Charles MacNeill 5:89031b2f5316 511 uint16_t i = 0;
Charles MacNeill 5:89031b2f5316 512
Charles MacNeill 5:89031b2f5316 513 pdata->cfg_device_state = VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 514 pdata->rd_device_state = VL53LX_DEVICESTATE_SW_STANDBY;
Charles MacNeill 5:89031b2f5316 515
Charles MacNeill 5:89031b2f5316 516 pdata->zone_id = 0;
Charles MacNeill 5:89031b2f5316 517 pdata->time_stamp = 0;
Charles MacNeill 5:89031b2f5316 518
Charles MacNeill 5:89031b2f5316 519 pdata->VL53LX_p_019 = 0;
Charles MacNeill 5:89031b2f5316 520 pdata->VL53LX_p_020 = VL53LX_HISTOGRAM_BUFFER_SIZE;
Charles MacNeill 5:89031b2f5316 521 pdata->VL53LX_p_021 = (uint8_t)VL53LX_p_021;
Charles MacNeill 5:89031b2f5316 522 pdata->number_of_ambient_bins = 0;
Charles MacNeill 5:89031b2f5316 523
Charles MacNeill 5:89031b2f5316 524 pdata->result__interrupt_status = 0;
Charles MacNeill 5:89031b2f5316 525 pdata->result__range_status = 0;
Charles MacNeill 5:89031b2f5316 526 pdata->result__report_status = 0;
Charles MacNeill 5:89031b2f5316 527 pdata->result__stream_count = 0;
Charles MacNeill 5:89031b2f5316 528
Charles MacNeill 5:89031b2f5316 529 pdata->result__dss_actual_effective_spads = 0;
Charles MacNeill 5:89031b2f5316 530 pdata->phasecal_result__reference_phase = 0;
Charles MacNeill 5:89031b2f5316 531 pdata->phasecal_result__vcsel_start = 0;
Charles MacNeill 5:89031b2f5316 532 pdata->cal_config__vcsel_start = 0;
Charles MacNeill 5:89031b2f5316 533
Charles MacNeill 5:89031b2f5316 534 pdata->vcsel_width = 0;
Charles MacNeill 5:89031b2f5316 535 pdata->VL53LX_p_005 = 0;
Charles MacNeill 5:89031b2f5316 536 pdata->VL53LX_p_015 = 0;
Charles MacNeill 5:89031b2f5316 537 pdata->total_periods_elapsed = 0;
Charles MacNeill 5:89031b2f5316 538
Charles MacNeill 5:89031b2f5316 539 pdata->min_bin_value = 0;
Charles MacNeill 5:89031b2f5316 540 pdata->max_bin_value = 0;
Charles MacNeill 5:89031b2f5316 541
Charles MacNeill 5:89031b2f5316 542 pdata->zero_distance_phase = 0;
Charles MacNeill 5:89031b2f5316 543 pdata->number_of_ambient_samples = 0;
Charles MacNeill 5:89031b2f5316 544 pdata->ambient_events_sum = 0;
Charles MacNeill 5:89031b2f5316 545 pdata->VL53LX_p_028 = 0;
Charles MacNeill 5:89031b2f5316 546
Charles MacNeill 5:89031b2f5316 547 for (i = 0; i < VL53LX_MAX_BIN_SEQUENCE_LENGTH; i++)
Charles MacNeill 5:89031b2f5316 548 pdata->bin_seq[i] = (uint8_t)i;
Charles MacNeill 5:89031b2f5316 549
Charles MacNeill 5:89031b2f5316 550 for (i = 0; i < VL53LX_MAX_BIN_SEQUENCE_LENGTH; i++)
Charles MacNeill 5:89031b2f5316 551 pdata->bin_rep[i] = 1;
Charles MacNeill 5:89031b2f5316 552
Charles MacNeill 5:89031b2f5316 553
Charles MacNeill 5:89031b2f5316 554 for (i = 0; i < VL53LX_HISTOGRAM_BUFFER_SIZE; i++)
Charles MacNeill 5:89031b2f5316 555 if (i < VL53LX_p_021)
Charles MacNeill 5:89031b2f5316 556 pdata->bin_data[i] = bin_value;
Charles MacNeill 5:89031b2f5316 557 else
Charles MacNeill 5:89031b2f5316 558 pdata->bin_data[i] = 0;
Charles MacNeill 5:89031b2f5316 559
Charles MacNeill 5:89031b2f5316 560
Charles MacNeill 5:89031b2f5316 561 }
Charles MacNeill 5:89031b2f5316 562
Charles MacNeill 5:89031b2f5316 563
Charles MacNeill 5:89031b2f5316 564 void VL53LX_decode_row_col(
Charles MacNeill 5:89031b2f5316 565 uint8_t spad_number,
Charles MacNeill 5:89031b2f5316 566 uint8_t *prow,
Charles MacNeill 5:89031b2f5316 567 uint8_t *pcol)
Charles MacNeill 5:89031b2f5316 568 {
Charles MacNeill 5:89031b2f5316 569
Charles MacNeill 5:89031b2f5316 570
Charles MacNeill 5:89031b2f5316 571
Charles MacNeill 5:89031b2f5316 572 if (spad_number > 127) {
Charles MacNeill 5:89031b2f5316 573 *prow = 8 + ((255-spad_number) & 0x07);
Charles MacNeill 5:89031b2f5316 574 *pcol = (spad_number-128) >> 3;
Charles MacNeill 5:89031b2f5316 575 } else {
Charles MacNeill 5:89031b2f5316 576 *prow = spad_number & 0x07;
Charles MacNeill 5:89031b2f5316 577 *pcol = (127-spad_number) >> 3;
Charles MacNeill 5:89031b2f5316 578 }
Charles MacNeill 5:89031b2f5316 579 }
Charles MacNeill 5:89031b2f5316 580
Charles MacNeill 5:89031b2f5316 581
Charles MacNeill 5:89031b2f5316 582 void VL53LX_hist_find_min_max_bin_values(
Charles MacNeill 5:89031b2f5316 583 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 584 {
Charles MacNeill 5:89031b2f5316 585
Charles MacNeill 5:89031b2f5316 586
Charles MacNeill 5:89031b2f5316 587 uint8_t bin = 0;
Charles MacNeill 5:89031b2f5316 588
Charles MacNeill 5:89031b2f5316 589 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 590
Charles MacNeill 5:89031b2f5316 591 for (bin = 0; bin < pdata->VL53LX_p_021; bin++) {
Charles MacNeill 5:89031b2f5316 592
Charles MacNeill 5:89031b2f5316 593 if (bin == 0 || pdata->min_bin_value >= pdata->bin_data[bin])
Charles MacNeill 5:89031b2f5316 594 pdata->min_bin_value = pdata->bin_data[bin];
Charles MacNeill 5:89031b2f5316 595
Charles MacNeill 5:89031b2f5316 596 if (bin == 0 || pdata->max_bin_value <= pdata->bin_data[bin])
Charles MacNeill 5:89031b2f5316 597 pdata->max_bin_value = pdata->bin_data[bin];
Charles MacNeill 5:89031b2f5316 598
Charles MacNeill 5:89031b2f5316 599 }
Charles MacNeill 5:89031b2f5316 600
Charles MacNeill 5:89031b2f5316 601 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 602
Charles MacNeill 5:89031b2f5316 603 }
Charles MacNeill 5:89031b2f5316 604
Charles MacNeill 5:89031b2f5316 605
Charles MacNeill 5:89031b2f5316 606 void VL53LX_hist_estimate_ambient_from_ambient_bins(
Charles MacNeill 5:89031b2f5316 607 VL53LX_histogram_bin_data_t *pdata)
Charles MacNeill 5:89031b2f5316 608 {
Charles MacNeill 5:89031b2f5316 609
Charles MacNeill 5:89031b2f5316 610
Charles MacNeill 5:89031b2f5316 611 uint8_t bin = 0;
Charles MacNeill 5:89031b2f5316 612
Charles MacNeill 5:89031b2f5316 613 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 614
Charles MacNeill 5:89031b2f5316 615 if (pdata->number_of_ambient_bins > 0) {
Charles MacNeill 5:89031b2f5316 616
Charles MacNeill 5:89031b2f5316 617 pdata->number_of_ambient_samples =
Charles MacNeill 5:89031b2f5316 618 pdata->number_of_ambient_bins;
Charles MacNeill 5:89031b2f5316 619
Charles MacNeill 5:89031b2f5316 620
Charles MacNeill 5:89031b2f5316 621
Charles MacNeill 5:89031b2f5316 622 pdata->ambient_events_sum = 0;
Charles MacNeill 5:89031b2f5316 623 for (bin = 0; bin < pdata->number_of_ambient_bins; bin++)
Charles MacNeill 5:89031b2f5316 624 pdata->ambient_events_sum += pdata->bin_data[bin];
Charles MacNeill 5:89031b2f5316 625
Charles MacNeill 5:89031b2f5316 626 pdata->VL53LX_p_028 = pdata->ambient_events_sum;
Charles MacNeill 5:89031b2f5316 627 pdata->VL53LX_p_028 +=
Charles MacNeill 5:89031b2f5316 628 ((int32_t)pdata->number_of_ambient_bins / 2);
Charles MacNeill 5:89031b2f5316 629 pdata->VL53LX_p_028 /=
Charles MacNeill 5:89031b2f5316 630 (int32_t)pdata->number_of_ambient_bins;
Charles MacNeill 5:89031b2f5316 631
Charles MacNeill 5:89031b2f5316 632 }
Charles MacNeill 5:89031b2f5316 633
Charles MacNeill 5:89031b2f5316 634 LOG_FUNCTION_END(0);
Charles MacNeill 5:89031b2f5316 635 }
Charles MacNeill 5:89031b2f5316 636
Charles MacNeill 5:89031b2f5316 637