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