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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charles MacNeill 5:89031b2f5316 1
Charles MacNeill 5:89031b2f5316 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 5:89031b2f5316 3 /******************************************************************************
Charles MacNeill 5:89031b2f5316 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
Charles MacNeill 5:89031b2f5316 5
Charles MacNeill 5:89031b2f5316 6 This file is part of VL53LX Protected and is dual licensed,
Charles MacNeill 5:89031b2f5316 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 5:89031b2f5316 9
Charles MacNeill 5:89031b2f5316 10 ******************************************************************************
Charles MacNeill 5:89031b2f5316 11
Charles MacNeill 5:89031b2f5316 12 'STMicroelectronics Proprietary license'
Charles MacNeill 5:89031b2f5316 13
Charles MacNeill 5:89031b2f5316 14 ******************************************************************************
Charles MacNeill 5:89031b2f5316 15
Charles MacNeill 5:89031b2f5316 16 License terms: STMicroelectronics Proprietary in accordance with licensing
Charles MacNeill 5:89031b2f5316 17 terms at www.st.com/sla0081
Charles MacNeill 5:89031b2f5316 18
Charles MacNeill 5:89031b2f5316 19 ******************************************************************************
Charles MacNeill 5:89031b2f5316 20 */
Charles MacNeill 5:89031b2f5316 21
Charles MacNeill 5:89031b2f5316 22
Charles MacNeill 5:89031b2f5316 23
Charles MacNeill 5:89031b2f5316 24
Charles MacNeill 5:89031b2f5316 25 #include <vl53lx_platform_log.h>
Charles MacNeill 5:89031b2f5316 26 #include <vl53lx_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