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_dmax.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_types.h>
Charles MacNeill 5:89031b2f5316 27 #include "vl53lx_platform_user_defines.h"
Charles MacNeill 5:89031b2f5316 28 #include "vl53lx_core_support.h"
Charles MacNeill 5:89031b2f5316 29 #include "vl53lx_error_codes.h"
Charles MacNeill 5:89031b2f5316 30
Charles MacNeill 5:89031b2f5316 31 #include "vl53lx_dmax.h"
Charles MacNeill 5:89031b2f5316 32
Charles MacNeill 5:89031b2f5316 33
Charles MacNeill 5:89031b2f5316 34 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 35 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 36 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 37 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 38 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 39 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_PROTECTED, \
Charles MacNeill 5:89031b2f5316 40 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 41
Charles MacNeill 5:89031b2f5316 42 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 43 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_PROTECTED, \
Charles MacNeill 5:89031b2f5316 44 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 45
Charles MacNeill 5:89031b2f5316 46
Charles MacNeill 5:89031b2f5316 47 VL53LX_Error VL53LX_f_001(
Charles MacNeill 5:89031b2f5316 48 uint16_t target_reflectance,
Charles MacNeill 5:89031b2f5316 49 VL53LX_dmax_calibration_data_t *pcal,
Charles MacNeill 5:89031b2f5316 50 VL53LX_hist_gen3_dmax_config_t *pcfg,
Charles MacNeill 5:89031b2f5316 51 VL53LX_histogram_bin_data_t *pbins,
Charles MacNeill 5:89031b2f5316 52 VL53LX_hist_gen3_dmax_private_data_t *pdata,
Charles MacNeill 5:89031b2f5316 53 int16_t *pambient_dmax_mm)
Charles MacNeill 5:89031b2f5316 54 {
Charles MacNeill 5:89031b2f5316 55
Charles MacNeill 5:89031b2f5316 56
Charles MacNeill 5:89031b2f5316 57
Charles MacNeill 5:89031b2f5316 58 VL53LX_Error status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 59
Charles MacNeill 5:89031b2f5316 60 uint32_t pll_period_us = 0;
Charles MacNeill 5:89031b2f5316 61 uint32_t periods_elapsed = 0;
Charles MacNeill 5:89031b2f5316 62
Charles MacNeill 5:89031b2f5316 63 uint32_t tmp32 = 0;
Charles MacNeill 5:89031b2f5316 64 uint64_t tmp64 = 0;
Charles MacNeill 5:89031b2f5316 65
Charles MacNeill 5:89031b2f5316 66 uint32_t amb_thres_delta = 0;
Charles MacNeill 5:89031b2f5316 67
Charles MacNeill 5:89031b2f5316 68 LOG_FUNCTION_START("");
Charles MacNeill 5:89031b2f5316 69
Charles MacNeill 5:89031b2f5316 70
Charles MacNeill 5:89031b2f5316 71
Charles MacNeill 5:89031b2f5316 72 pdata->VL53LX_p_004 = 0x0000;
Charles MacNeill 5:89031b2f5316 73 pdata->VL53LX_p_033 = 0x0000;
Charles MacNeill 5:89031b2f5316 74 pdata->VL53LX_p_034 = 0x0000;
Charles MacNeill 5:89031b2f5316 75 pdata->VL53LX_p_009 = 0x0000;
Charles MacNeill 5:89031b2f5316 76 pdata->VL53LX_p_028 = 0x0000;
Charles MacNeill 5:89031b2f5316 77 pdata->VL53LX_p_035 = 0x0000;
Charles MacNeill 5:89031b2f5316 78 pdata->VL53LX_p_036 = 0;
Charles MacNeill 5:89031b2f5316 79 pdata->VL53LX_p_022 = 0;
Charles MacNeill 5:89031b2f5316 80
Charles MacNeill 5:89031b2f5316 81 *pambient_dmax_mm = 0;
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83
Charles MacNeill 5:89031b2f5316 84 if ((pbins->VL53LX_p_015 != 0) &&
Charles MacNeill 5:89031b2f5316 85 (pbins->total_periods_elapsed != 0)) {
Charles MacNeill 5:89031b2f5316 86
Charles MacNeill 5:89031b2f5316 87
Charles MacNeill 5:89031b2f5316 88
Charles MacNeill 5:89031b2f5316 89 pll_period_us =
Charles MacNeill 5:89031b2f5316 90 VL53LX_calc_pll_period_us(pbins->VL53LX_p_015);
Charles MacNeill 5:89031b2f5316 91
Charles MacNeill 5:89031b2f5316 92
Charles MacNeill 5:89031b2f5316 93
Charles MacNeill 5:89031b2f5316 94 periods_elapsed = pbins->total_periods_elapsed + 1;
Charles MacNeill 5:89031b2f5316 95
Charles MacNeill 5:89031b2f5316 96
Charles MacNeill 5:89031b2f5316 97
Charles MacNeill 5:89031b2f5316 98 pdata->VL53LX_p_037 =
Charles MacNeill 5:89031b2f5316 99 VL53LX_duration_maths(
Charles MacNeill 5:89031b2f5316 100 pll_period_us,
Charles MacNeill 5:89031b2f5316 101 1<<4,
Charles MacNeill 5:89031b2f5316 102 VL53LX_RANGING_WINDOW_VCSEL_PERIODS,
Charles MacNeill 5:89031b2f5316 103 periods_elapsed);
Charles MacNeill 5:89031b2f5316 104
Charles MacNeill 5:89031b2f5316 105
Charles MacNeill 5:89031b2f5316 106 pdata->VL53LX_p_034 =
Charles MacNeill 5:89031b2f5316 107 VL53LX_rate_maths(
Charles MacNeill 5:89031b2f5316 108 pbins->VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 109 pdata->VL53LX_p_037);
Charles MacNeill 5:89031b2f5316 110
Charles MacNeill 5:89031b2f5316 111
Charles MacNeill 5:89031b2f5316 112
Charles MacNeill 5:89031b2f5316 113 pdata->VL53LX_p_033 =
Charles MacNeill 5:89031b2f5316 114 VL53LX_events_per_spad_maths(
Charles MacNeill 5:89031b2f5316 115 pbins->VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 116 pbins->result__dss_actual_effective_spads,
Charles MacNeill 5:89031b2f5316 117 pdata->VL53LX_p_037);
Charles MacNeill 5:89031b2f5316 118
Charles MacNeill 5:89031b2f5316 119
Charles MacNeill 5:89031b2f5316 120
Charles MacNeill 5:89031b2f5316 121 pdata->VL53LX_p_038 = pcfg->max_effective_spads;
Charles MacNeill 5:89031b2f5316 122 pdata->VL53LX_p_004 = pcfg->max_effective_spads;
Charles MacNeill 5:89031b2f5316 123
Charles MacNeill 5:89031b2f5316 124 if (pdata->VL53LX_p_033 > 0) {
Charles MacNeill 5:89031b2f5316 125 tmp64 =
Charles MacNeill 5:89031b2f5316 126 (uint64_t)pcfg->dss_config__target_total_rate_mcps;
Charles MacNeill 5:89031b2f5316 127 tmp64 *= 1000;
Charles MacNeill 5:89031b2f5316 128 tmp64 <<= (11+1);
Charles MacNeill 5:89031b2f5316 129 tmp32 = pdata->VL53LX_p_033/2;
Charles MacNeill 5:89031b2f5316 130 tmp64 += (uint64_t)tmp32;
Charles MacNeill 5:89031b2f5316 131 tmp64 = do_division_u(tmp64,
Charles MacNeill 5:89031b2f5316 132 (uint64_t)pdata->VL53LX_p_033);
Charles MacNeill 5:89031b2f5316 133
Charles MacNeill 5:89031b2f5316 134 if (tmp64 < (uint64_t)pcfg->max_effective_spads)
Charles MacNeill 5:89031b2f5316 135 pdata->VL53LX_p_004 = (uint16_t)tmp64;
Charles MacNeill 5:89031b2f5316 136 }
Charles MacNeill 5:89031b2f5316 137 }
Charles MacNeill 5:89031b2f5316 138
Charles MacNeill 5:89031b2f5316 139
Charles MacNeill 5:89031b2f5316 140
Charles MacNeill 5:89031b2f5316 141 if ((pcal->ref__actual_effective_spads != 0) &&
Charles MacNeill 5:89031b2f5316 142 (pbins->VL53LX_p_015 != 0) &&
Charles MacNeill 5:89031b2f5316 143 (pcal->ref_reflectance_pc != 0) &&
Charles MacNeill 5:89031b2f5316 144 (pbins->total_periods_elapsed != 0)) {
Charles MacNeill 5:89031b2f5316 145
Charles MacNeill 5:89031b2f5316 146
Charles MacNeill 5:89031b2f5316 147
Charles MacNeill 5:89031b2f5316 148 tmp64 = (uint64_t)pcal->ref__peak_signal_count_rate_mcps;
Charles MacNeill 5:89031b2f5316 149 tmp64 *= (1000 * 256);
Charles MacNeill 5:89031b2f5316 150 tmp32 = pcal->ref__actual_effective_spads/2;
Charles MacNeill 5:89031b2f5316 151 tmp64 += (uint64_t)tmp32;
Charles MacNeill 5:89031b2f5316 152 tmp64 = do_division_u(tmp64,
Charles MacNeill 5:89031b2f5316 153 (uint64_t)pcal->ref__actual_effective_spads);
Charles MacNeill 5:89031b2f5316 154
Charles MacNeill 5:89031b2f5316 155 pdata->VL53LX_p_009 = (uint32_t)tmp64;
Charles MacNeill 5:89031b2f5316 156 pdata->VL53LX_p_009 <<= 4;
Charles MacNeill 5:89031b2f5316 157
Charles MacNeill 5:89031b2f5316 158
Charles MacNeill 5:89031b2f5316 159
Charles MacNeill 5:89031b2f5316 160 tmp64 = (uint64_t)pdata->VL53LX_p_037;
Charles MacNeill 5:89031b2f5316 161 tmp64 *= (uint64_t)pdata->VL53LX_p_033;
Charles MacNeill 5:89031b2f5316 162 tmp64 *= (uint64_t)pdata->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 163 tmp64 += (1<<(11+7));
Charles MacNeill 5:89031b2f5316 164 tmp64 >>= (11+8);
Charles MacNeill 5:89031b2f5316 165 tmp64 += 500;
Charles MacNeill 5:89031b2f5316 166 tmp64 = do_division_u(tmp64, 1000);
Charles MacNeill 5:89031b2f5316 167
Charles MacNeill 5:89031b2f5316 168
Charles MacNeill 5:89031b2f5316 169 if (tmp64 > 0x00FFFFFF)
Charles MacNeill 5:89031b2f5316 170 tmp64 = 0x00FFFFFF;
Charles MacNeill 5:89031b2f5316 171
Charles MacNeill 5:89031b2f5316 172 pdata->VL53LX_p_028 = (uint32_t)tmp64;
Charles MacNeill 5:89031b2f5316 173
Charles MacNeill 5:89031b2f5316 174
Charles MacNeill 5:89031b2f5316 175
Charles MacNeill 5:89031b2f5316 176 tmp64 = (uint64_t)pdata->VL53LX_p_037;
Charles MacNeill 5:89031b2f5316 177 tmp64 *= (uint64_t)pdata->VL53LX_p_009;
Charles MacNeill 5:89031b2f5316 178 tmp64 *= (uint64_t)pdata->VL53LX_p_004;
Charles MacNeill 5:89031b2f5316 179 tmp64 += (1<<(11+7));
Charles MacNeill 5:89031b2f5316 180 tmp64 >>= (11+8);
Charles MacNeill 5:89031b2f5316 181
Charles MacNeill 5:89031b2f5316 182
Charles MacNeill 5:89031b2f5316 183
Charles MacNeill 5:89031b2f5316 184 tmp64 *= ((uint64_t)target_reflectance *
Charles MacNeill 5:89031b2f5316 185 (uint64_t)pcal->coverglass_transmission);
Charles MacNeill 5:89031b2f5316 186
Charles MacNeill 5:89031b2f5316 187 tmp64 += ((uint64_t)pcal->ref_reflectance_pc * 128);
Charles MacNeill 5:89031b2f5316 188 tmp64 = do_division_u(tmp64,
Charles MacNeill 5:89031b2f5316 189 ((uint64_t)pcal->ref_reflectance_pc * 256));
Charles MacNeill 5:89031b2f5316 190
Charles MacNeill 5:89031b2f5316 191 tmp64 += 500;
Charles MacNeill 5:89031b2f5316 192 tmp64 = do_division_u(tmp64, 1000);
Charles MacNeill 5:89031b2f5316 193
Charles MacNeill 5:89031b2f5316 194
Charles MacNeill 5:89031b2f5316 195 if (tmp64 > 0x00FFFFFF)
Charles MacNeill 5:89031b2f5316 196 tmp64 = 0x00FFFFFF;
Charles MacNeill 5:89031b2f5316 197
Charles MacNeill 5:89031b2f5316 198 pdata->VL53LX_p_035 = (uint32_t)tmp64;
Charles MacNeill 5:89031b2f5316 199
Charles MacNeill 5:89031b2f5316 200
Charles MacNeill 5:89031b2f5316 201
Charles MacNeill 5:89031b2f5316 202 tmp32 = VL53LX_isqrt(pdata->VL53LX_p_028 << 8);
Charles MacNeill 5:89031b2f5316 203 tmp32 *= (uint32_t)pcfg->ambient_thresh_sigma;
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207 if (pdata->VL53LX_p_028 <
Charles MacNeill 5:89031b2f5316 208 (uint32_t)pcfg->min_ambient_thresh_events) {
Charles MacNeill 5:89031b2f5316 209
Charles MacNeill 5:89031b2f5316 210 amb_thres_delta =
Charles MacNeill 5:89031b2f5316 211 pcfg->min_ambient_thresh_events -
Charles MacNeill 5:89031b2f5316 212 (uint32_t)pdata->VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 213
Charles MacNeill 5:89031b2f5316 214
Charles MacNeill 5:89031b2f5316 215 amb_thres_delta <<= 8;
Charles MacNeill 5:89031b2f5316 216
Charles MacNeill 5:89031b2f5316 217 if (tmp32 < amb_thres_delta)
Charles MacNeill 5:89031b2f5316 218 tmp32 = amb_thres_delta;
Charles MacNeill 5:89031b2f5316 219 }
Charles MacNeill 5:89031b2f5316 220
Charles MacNeill 5:89031b2f5316 221
Charles MacNeill 5:89031b2f5316 222
Charles MacNeill 5:89031b2f5316 223 pdata->VL53LX_p_022 =
Charles MacNeill 5:89031b2f5316 224 (int16_t)VL53LX_f_002(
Charles MacNeill 5:89031b2f5316 225 tmp32,
Charles MacNeill 5:89031b2f5316 226 pdata->VL53LX_p_035,
Charles MacNeill 5:89031b2f5316 227 (uint32_t)pcal->ref__distance_mm,
Charles MacNeill 5:89031b2f5316 228 (uint32_t)pcfg->signal_thresh_sigma);
Charles MacNeill 5:89031b2f5316 229
Charles MacNeill 5:89031b2f5316 230
Charles MacNeill 5:89031b2f5316 231
Charles MacNeill 5:89031b2f5316 232 tmp32 = (uint32_t)pdata->VL53LX_p_035;
Charles MacNeill 5:89031b2f5316 233 tmp32 *= (uint32_t)pbins->vcsel_width;
Charles MacNeill 5:89031b2f5316 234 tmp32 += (1 << 3);
Charles MacNeill 5:89031b2f5316 235 tmp32 /= (1 << 4);
Charles MacNeill 5:89031b2f5316 236
Charles MacNeill 5:89031b2f5316 237 pdata->VL53LX_p_036 =
Charles MacNeill 5:89031b2f5316 238 (int16_t)VL53LX_f_002(
Charles MacNeill 5:89031b2f5316 239 256 * (uint32_t)pcfg->signal_total_events_limit,
Charles MacNeill 5:89031b2f5316 240 tmp32,
Charles MacNeill 5:89031b2f5316 241 (uint32_t)pcal->ref__distance_mm,
Charles MacNeill 5:89031b2f5316 242 (uint32_t)pcfg->signal_thresh_sigma);
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 if (pdata->VL53LX_p_036 < pdata->VL53LX_p_022)
Charles MacNeill 5:89031b2f5316 248 *pambient_dmax_mm = pdata->VL53LX_p_036;
Charles MacNeill 5:89031b2f5316 249 else
Charles MacNeill 5:89031b2f5316 250 *pambient_dmax_mm = pdata->VL53LX_p_022;
Charles MacNeill 5:89031b2f5316 251
Charles MacNeill 5:89031b2f5316 252 }
Charles MacNeill 5:89031b2f5316 253
Charles MacNeill 5:89031b2f5316 254 LOG_FUNCTION_END(status);
Charles MacNeill 5:89031b2f5316 255
Charles MacNeill 5:89031b2f5316 256 return status;
Charles MacNeill 5:89031b2f5316 257
Charles MacNeill 5:89031b2f5316 258 }
Charles MacNeill 5:89031b2f5316 259
Charles MacNeill 5:89031b2f5316 260
Charles MacNeill 5:89031b2f5316 261 uint32_t VL53LX_f_002(
Charles MacNeill 5:89031b2f5316 262 uint32_t events_threshold,
Charles MacNeill 5:89031b2f5316 263 uint32_t ref_signal_events,
Charles MacNeill 5:89031b2f5316 264 uint32_t ref_distance_mm,
Charles MacNeill 5:89031b2f5316 265 uint32_t signal_thresh_sigma)
Charles MacNeill 5:89031b2f5316 266 {
Charles MacNeill 5:89031b2f5316 267
Charles MacNeill 5:89031b2f5316 268
Charles MacNeill 5:89031b2f5316 269
Charles MacNeill 5:89031b2f5316 270 uint32_t tmp32 = 0;
Charles MacNeill 5:89031b2f5316 271 uint32_t range_mm = 0;
Charles MacNeill 5:89031b2f5316 272
Charles MacNeill 5:89031b2f5316 273 tmp32 = 4 * events_threshold;
Charles MacNeill 5:89031b2f5316 274
Charles MacNeill 5:89031b2f5316 275
Charles MacNeill 5:89031b2f5316 276
Charles MacNeill 5:89031b2f5316 277 tmp32 += ((uint32_t)signal_thresh_sigma *
Charles MacNeill 5:89031b2f5316 278 (uint32_t)signal_thresh_sigma);
Charles MacNeill 5:89031b2f5316 279
Charles MacNeill 5:89031b2f5316 280
Charles MacNeill 5:89031b2f5316 281
Charles MacNeill 5:89031b2f5316 282 tmp32 = VL53LX_isqrt(tmp32);
Charles MacNeill 5:89031b2f5316 283 tmp32 += (uint32_t)signal_thresh_sigma;
Charles MacNeill 5:89031b2f5316 284
Charles MacNeill 5:89031b2f5316 285
Charles MacNeill 5:89031b2f5316 286
Charles MacNeill 5:89031b2f5316 287 range_mm =
Charles MacNeill 5:89031b2f5316 288 (uint32_t)VL53LX_isqrt(ref_signal_events << 4);
Charles MacNeill 5:89031b2f5316 289 range_mm *= ref_distance_mm;
Charles MacNeill 5:89031b2f5316 290 if (tmp32 != 0) {
Charles MacNeill 5:89031b2f5316 291 range_mm += (tmp32);
Charles MacNeill 5:89031b2f5316 292 range_mm /= (2*tmp32);
Charles MacNeill 5:89031b2f5316 293 }
Charles MacNeill 5:89031b2f5316 294
Charles MacNeill 5:89031b2f5316 295 return range_mm;
Charles MacNeill 5:89031b2f5316 296
Charles MacNeill 5:89031b2f5316 297 }
Charles MacNeill 5:89031b2f5316 298