ST Expansion SW Team / VL53L1CB

The VL53L1CB proximity sensor, based on ST’s FlightSense™, Time-of-Flight technology.

Dependencies:   X_NUCLEO_COMMON ST_INTERFACES

Dependents:   VL53L1CB_noshield_1sensor_polls_auton VL53L1CB_noshield_1sensor_interrupt_auton X_NUCLEO_53L1A2

Based on VL53L1 library, this is a library for the VL53L1CB ToF chip.

src/vl53l1_dmax.c@0:3ac96e360672, 2020-11-06 (annotated)

Committer:
charlesmn
Date:
Fri Nov 06 10:06:37 2020 +0000
Revision:
0:3ac96e360672
Child:
7:1add29d51e72
Library for ST Vl53L1A1 time of flight sensor.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
charlesmn 0:3ac96e360672 2 /*******************************************************************************
charlesmn 0:3ac96e360672 3 This file is part of VL53L1 Protected
charlesmn 0:3ac96e360672 4
charlesmn 0:3ac96e360672 5 Copyright (c) 2020, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 6
charlesmn 0:3ac96e360672 7 License terms: STMicroelectronics Proprietary in accordance with licensing
charlesmn 0:3ac96e360672 8 terms at www.st.com/sla0081
charlesmn 0:3ac96e360672 9
charlesmn 0:3ac96e360672 10 STMicroelectronics confidential
charlesmn 0:3ac96e360672 11 Reproduction and Communication of this document is strictly prohibited unless
charlesmn 0:3ac96e360672 12 specifically authorized in writing by STMicroelectronics.
charlesmn 0:3ac96e360672 13
charlesmn 0:3ac96e360672 14 */
charlesmn 0:3ac96e360672 15
charlesmn 0:3ac96e360672 16
charlesmn 0:3ac96e360672 17
charlesmn 0:3ac96e360672 18
charlesmn 0:3ac96e360672 19 #include "vl53l1_types.h"
charlesmn 0:3ac96e360672 20 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 21 #include "vl53l1_platform_user_defines.h"
charlesmn 0:3ac96e360672 22 #include "vl53l1_core_support.h"
charlesmn 0:3ac96e360672 23 #include "vl53l1_error_codes.h"
charlesmn 0:3ac96e360672 24
charlesmn 0:3ac96e360672 25 #include "vl53l1_dmax.h"
charlesmn 0:3ac96e360672 26
charlesmn 0:3ac96e360672 27
charlesmn 0:3ac96e360672 28 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 29 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 30 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 31 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 32 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 33 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_PROTECTED, \
charlesmn 0:3ac96e360672 34 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 35
charlesmn 0:3ac96e360672 36 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 37 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_PROTECTED, \
charlesmn 0:3ac96e360672 38 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 39
charlesmn 0:3ac96e360672 40
charlesmn 0:3ac96e360672 41 VL53L1_Error VL53L1_f_001(
charlesmn 0:3ac96e360672 42 uint16_t target_reflectance,
charlesmn 0:3ac96e360672 43 VL53L1_dmax_calibration_data_t *pcal,
charlesmn 0:3ac96e360672 44 VL53L1_hist_gen3_dmax_config_t *pcfg,
charlesmn 0:3ac96e360672 45 VL53L1_histogram_bin_data_t *pbins,
charlesmn 0:3ac96e360672 46 VL53L1_hist_gen3_dmax_private_data_t *pdata,
charlesmn 0:3ac96e360672 47 int16_t *pambient_dmax_mm)
charlesmn 0:3ac96e360672 48 {
charlesmn 0:3ac96e360672 49
charlesmn 0:3ac96e360672 50
charlesmn 0:3ac96e360672 51
charlesmn 0:3ac96e360672 52 VL53L1_Error status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 53
charlesmn 0:3ac96e360672 54 uint32_t pll_period_us = 0;
charlesmn 0:3ac96e360672 55 uint32_t periods_elapsed = 0;
charlesmn 0:3ac96e360672 56
charlesmn 0:3ac96e360672 57 uint32_t tmp32 = 0;
charlesmn 0:3ac96e360672 58 uint64_t tmp64 = 0;
charlesmn 0:3ac96e360672 59
charlesmn 0:3ac96e360672 60 uint32_t amb_thres_delta = 0;
charlesmn 0:3ac96e360672 61
charlesmn 0:3ac96e360672 62 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 63
charlesmn 0:3ac96e360672 64
charlesmn 0:3ac96e360672 65
charlesmn 0:3ac96e360672 66 pdata->VL53L1_p_006 = 0x0000;
charlesmn 0:3ac96e360672 67 pdata->VL53L1_p_033 = 0x0000;
charlesmn 0:3ac96e360672 68 pdata->VL53L1_p_001 = 0x0000;
charlesmn 0:3ac96e360672 69 pdata->VL53L1_p_012 = 0x0000;
charlesmn 0:3ac96e360672 70 pdata->VL53L1_p_004 = 0x0000;
charlesmn 0:3ac96e360672 71 pdata->VL53L1_p_034 = 0x0000;
charlesmn 0:3ac96e360672 72 pdata->VL53L1_p_035 = 0;
charlesmn 0:3ac96e360672 73 pdata->VL53L1_p_007 = 0;
charlesmn 0:3ac96e360672 74
charlesmn 0:3ac96e360672 75 *pambient_dmax_mm = 0;
charlesmn 0:3ac96e360672 76
charlesmn 0:3ac96e360672 77
charlesmn 0:3ac96e360672 78 if ((pbins->VL53L1_p_019 != 0) &&
charlesmn 0:3ac96e360672 79 (pbins->total_periods_elapsed != 0)) {
charlesmn 0:3ac96e360672 80
charlesmn 0:3ac96e360672 81
charlesmn 0:3ac96e360672 82
charlesmn 0:3ac96e360672 83 pll_period_us =
charlesmn 0:3ac96e360672 84 VL53L1_calc_pll_period_us(pbins->VL53L1_p_019);
charlesmn 0:3ac96e360672 85
charlesmn 0:3ac96e360672 86
charlesmn 0:3ac96e360672 87
charlesmn 0:3ac96e360672 88 periods_elapsed = pbins->total_periods_elapsed + 1;
charlesmn 0:3ac96e360672 89
charlesmn 0:3ac96e360672 90
charlesmn 0:3ac96e360672 91
charlesmn 0:3ac96e360672 92 pdata->VL53L1_p_036 =
charlesmn 0:3ac96e360672 93 VL53L1_duration_maths(
charlesmn 0:3ac96e360672 94 pll_period_us,
charlesmn 0:3ac96e360672 95 1<<4,
charlesmn 0:3ac96e360672 96 VL53L1_RANGING_WINDOW_VCSEL_PERIODS,
charlesmn 0:3ac96e360672 97 periods_elapsed);
charlesmn 0:3ac96e360672 98
charlesmn 0:3ac96e360672 99
charlesmn 0:3ac96e360672 100 pdata->VL53L1_p_001 =
charlesmn 0:3ac96e360672 101 VL53L1_rate_maths(
charlesmn 0:3ac96e360672 102 pbins->VL53L1_p_004,
charlesmn 0:3ac96e360672 103 pdata->VL53L1_p_036);
charlesmn 0:3ac96e360672 104
charlesmn 0:3ac96e360672 105
charlesmn 0:3ac96e360672 106
charlesmn 0:3ac96e360672 107 pdata->VL53L1_p_033 =
charlesmn 0:3ac96e360672 108 VL53L1_events_per_spad_maths(
charlesmn 0:3ac96e360672 109 pbins->VL53L1_p_004,
charlesmn 0:3ac96e360672 110 pbins->result__dss_actual_effective_spads,
charlesmn 0:3ac96e360672 111 pdata->VL53L1_p_036);
charlesmn 0:3ac96e360672 112
charlesmn 0:3ac96e360672 113
charlesmn 0:3ac96e360672 114
charlesmn 0:3ac96e360672 115 pdata->VL53L1_p_037 = pcfg->max_effective_spads;
charlesmn 0:3ac96e360672 116 pdata->VL53L1_p_006 = pcfg->max_effective_spads;
charlesmn 0:3ac96e360672 117
charlesmn 0:3ac96e360672 118 if (pdata->VL53L1_p_033 > 0) {
charlesmn 0:3ac96e360672 119 tmp64 =
charlesmn 0:3ac96e360672 120 (uint64_t)pcfg->dss_config__target_total_rate_mcps;
charlesmn 0:3ac96e360672 121 tmp64 *= 1000;
charlesmn 0:3ac96e360672 122 tmp64 <<= (11+1);
charlesmn 0:3ac96e360672 123 tmp32 = pdata->VL53L1_p_033/2;
charlesmn 0:3ac96e360672 124 tmp64 += (uint64_t)tmp32;
charlesmn 0:3ac96e360672 125 tmp64 = do_division_u(tmp64,
charlesmn 0:3ac96e360672 126 (uint64_t)pdata->VL53L1_p_033);
charlesmn 0:3ac96e360672 127
charlesmn 0:3ac96e360672 128 if (tmp64 < (uint64_t)pcfg->max_effective_spads)
charlesmn 0:3ac96e360672 129 pdata->VL53L1_p_006 = (uint16_t)tmp64;
charlesmn 0:3ac96e360672 130 }
charlesmn 0:3ac96e360672 131 }
charlesmn 0:3ac96e360672 132
charlesmn 0:3ac96e360672 133
charlesmn 0:3ac96e360672 134
charlesmn 0:3ac96e360672 135 if ((pcal->ref__actual_effective_spads != 0) &&
charlesmn 0:3ac96e360672 136 (pbins->VL53L1_p_019 != 0) &&
charlesmn 0:3ac96e360672 137 (pcal->ref_reflectance_pc != 0) &&
charlesmn 0:3ac96e360672 138 (pbins->total_periods_elapsed != 0)) {
charlesmn 0:3ac96e360672 139
charlesmn 0:3ac96e360672 140
charlesmn 0:3ac96e360672 141
charlesmn 0:3ac96e360672 142 tmp64 = (uint64_t)pcal->ref__peak_signal_count_rate_mcps;
charlesmn 0:3ac96e360672 143 tmp64 *= (1000 * 256);
charlesmn 0:3ac96e360672 144 tmp32 = pcal->ref__actual_effective_spads/2;
charlesmn 0:3ac96e360672 145 tmp64 += (uint64_t)tmp32;
charlesmn 0:3ac96e360672 146 tmp64 = do_division_u(tmp64,
charlesmn 0:3ac96e360672 147 (uint64_t)pcal->ref__actual_effective_spads);
charlesmn 0:3ac96e360672 148
charlesmn 0:3ac96e360672 149 pdata->VL53L1_p_012 = (uint32_t)tmp64;
charlesmn 0:3ac96e360672 150 pdata->VL53L1_p_012 <<= 4;
charlesmn 0:3ac96e360672 151
charlesmn 0:3ac96e360672 152
charlesmn 0:3ac96e360672 153
charlesmn 0:3ac96e360672 154 tmp64 = (uint64_t)pdata->VL53L1_p_036;
charlesmn 0:3ac96e360672 155 tmp64 *= (uint64_t)pdata->VL53L1_p_033;
charlesmn 0:3ac96e360672 156 tmp64 *= (uint64_t)pdata->VL53L1_p_006;
charlesmn 0:3ac96e360672 157 tmp64 += (1<<(11+7));
charlesmn 0:3ac96e360672 158 tmp64 >>= (11+8);
charlesmn 0:3ac96e360672 159 tmp64 += 500;
charlesmn 0:3ac96e360672 160 tmp64 = do_division_u(tmp64, 1000);
charlesmn 0:3ac96e360672 161
charlesmn 0:3ac96e360672 162
charlesmn 0:3ac96e360672 163 if (tmp64 > 0x00FFFFFF)
charlesmn 0:3ac96e360672 164 tmp64 = 0x00FFFFFF;
charlesmn 0:3ac96e360672 165
charlesmn 0:3ac96e360672 166 pdata->VL53L1_p_004 = (uint32_t)tmp64;
charlesmn 0:3ac96e360672 167
charlesmn 0:3ac96e360672 168
charlesmn 0:3ac96e360672 169
charlesmn 0:3ac96e360672 170 tmp64 = (uint64_t)pdata->VL53L1_p_036;
charlesmn 0:3ac96e360672 171 tmp64 *= (uint64_t)pdata->VL53L1_p_012;
charlesmn 0:3ac96e360672 172 tmp64 *= (uint64_t)pdata->VL53L1_p_006;
charlesmn 0:3ac96e360672 173 tmp64 += (1<<(11+7));
charlesmn 0:3ac96e360672 174 tmp64 >>= (11+8);
charlesmn 0:3ac96e360672 175
charlesmn 0:3ac96e360672 176
charlesmn 0:3ac96e360672 177
charlesmn 0:3ac96e360672 178 tmp64 *= ((uint64_t)target_reflectance *
charlesmn 0:3ac96e360672 179 (uint64_t)pcal->coverglass_transmission);
charlesmn 0:3ac96e360672 180
charlesmn 0:3ac96e360672 181 tmp64 += ((uint64_t)pcal->ref_reflectance_pc * 128);
charlesmn 0:3ac96e360672 182 tmp64 = do_division_u(tmp64,
charlesmn 0:3ac96e360672 183 ((uint64_t)pcal->ref_reflectance_pc * 256));
charlesmn 0:3ac96e360672 184
charlesmn 0:3ac96e360672 185 tmp64 += 500;
charlesmn 0:3ac96e360672 186 tmp64 = do_division_u(tmp64, 1000);
charlesmn 0:3ac96e360672 187
charlesmn 0:3ac96e360672 188
charlesmn 0:3ac96e360672 189 if (tmp64 > 0x00FFFFFF)
charlesmn 0:3ac96e360672 190 tmp64 = 0x00FFFFFF;
charlesmn 0:3ac96e360672 191
charlesmn 0:3ac96e360672 192 pdata->VL53L1_p_034 = (uint32_t)tmp64;
charlesmn 0:3ac96e360672 193
charlesmn 0:3ac96e360672 194
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196 tmp32 = VL53L1_isqrt(pdata->VL53L1_p_004 << 8);
charlesmn 0:3ac96e360672 197 tmp32 *= (uint32_t)pcfg->ambient_thresh_sigma;
charlesmn 0:3ac96e360672 198
charlesmn 0:3ac96e360672 199
charlesmn 0:3ac96e360672 200
charlesmn 0:3ac96e360672 201 if (pdata->VL53L1_p_004 <
charlesmn 0:3ac96e360672 202 (uint32_t)pcfg->min_ambient_thresh_events) {
charlesmn 0:3ac96e360672 203
charlesmn 0:3ac96e360672 204 amb_thres_delta =
charlesmn 0:3ac96e360672 205 pcfg->min_ambient_thresh_events -
charlesmn 0:3ac96e360672 206 (uint32_t)pdata->VL53L1_p_004;
charlesmn 0:3ac96e360672 207
charlesmn 0:3ac96e360672 208
charlesmn 0:3ac96e360672 209 amb_thres_delta <<= 8;
charlesmn 0:3ac96e360672 210
charlesmn 0:3ac96e360672 211 if (tmp32 < amb_thres_delta)
charlesmn 0:3ac96e360672 212 tmp32 = amb_thres_delta;
charlesmn 0:3ac96e360672 213 }
charlesmn 0:3ac96e360672 214
charlesmn 0:3ac96e360672 215
charlesmn 0:3ac96e360672 216
charlesmn 0:3ac96e360672 217 pdata->VL53L1_p_007 =
charlesmn 0:3ac96e360672 218 (int16_t)VL53L1_f_002(
charlesmn 0:3ac96e360672 219 tmp32,
charlesmn 0:3ac96e360672 220 pdata->VL53L1_p_034,
charlesmn 0:3ac96e360672 221 (uint32_t)pcal->ref__distance_mm,
charlesmn 0:3ac96e360672 222 (uint32_t)pcfg->signal_thresh_sigma);
charlesmn 0:3ac96e360672 223
charlesmn 0:3ac96e360672 224
charlesmn 0:3ac96e360672 225
charlesmn 0:3ac96e360672 226 tmp32 = (uint32_t)pdata->VL53L1_p_034;
charlesmn 0:3ac96e360672 227 tmp32 *= (uint32_t)pbins->vcsel_width;
charlesmn 0:3ac96e360672 228 tmp32 += (1 << 3);
charlesmn 0:3ac96e360672 229 tmp32 /= (1 << 4);
charlesmn 0:3ac96e360672 230
charlesmn 0:3ac96e360672 231 pdata->VL53L1_p_035 =
charlesmn 0:3ac96e360672 232 (int16_t)VL53L1_f_002(
charlesmn 0:3ac96e360672 233 256 * (uint32_t)pcfg->signal_total_events_limit,
charlesmn 0:3ac96e360672 234 tmp32,
charlesmn 0:3ac96e360672 235 (uint32_t)pcal->ref__distance_mm,
charlesmn 0:3ac96e360672 236 (uint32_t)pcfg->signal_thresh_sigma);
charlesmn 0:3ac96e360672 237
charlesmn 0:3ac96e360672 238
charlesmn 0:3ac96e360672 239
charlesmn 0:3ac96e360672 240
charlesmn 0:3ac96e360672 241 if (pdata->VL53L1_p_035 < pdata->VL53L1_p_007)
charlesmn 0:3ac96e360672 242 *pambient_dmax_mm = pdata->VL53L1_p_035;
charlesmn 0:3ac96e360672 243 else
charlesmn 0:3ac96e360672 244 *pambient_dmax_mm = pdata->VL53L1_p_007;
charlesmn 0:3ac96e360672 245
charlesmn 0:3ac96e360672 246 }
charlesmn 0:3ac96e360672 247
charlesmn 0:3ac96e360672 248 LOG_FUNCTION_END(status);
charlesmn 0:3ac96e360672 249
charlesmn 0:3ac96e360672 250 return status;
charlesmn 0:3ac96e360672 251
charlesmn 0:3ac96e360672 252 }
charlesmn 0:3ac96e360672 253
charlesmn 0:3ac96e360672 254
charlesmn 0:3ac96e360672 255 uint32_t VL53L1_f_002(
charlesmn 0:3ac96e360672 256 uint32_t events_threshold,
charlesmn 0:3ac96e360672 257 uint32_t ref_signal_events,
charlesmn 0:3ac96e360672 258 uint32_t ref_distance_mm,
charlesmn 0:3ac96e360672 259 uint32_t signal_thresh_sigma)
charlesmn 0:3ac96e360672 260 {
charlesmn 0:3ac96e360672 261
charlesmn 0:3ac96e360672 262
charlesmn 0:3ac96e360672 263
charlesmn 0:3ac96e360672 264 uint32_t tmp32 = 0;
charlesmn 0:3ac96e360672 265 uint32_t range_mm = 0;
charlesmn 0:3ac96e360672 266
charlesmn 0:3ac96e360672 267 tmp32 = 4 * events_threshold;
charlesmn 0:3ac96e360672 268
charlesmn 0:3ac96e360672 269
charlesmn 0:3ac96e360672 270
charlesmn 0:3ac96e360672 271 tmp32 += ((uint32_t)signal_thresh_sigma *
charlesmn 0:3ac96e360672 272 (uint32_t)signal_thresh_sigma);
charlesmn 0:3ac96e360672 273
charlesmn 0:3ac96e360672 274
charlesmn 0:3ac96e360672 275
charlesmn 0:3ac96e360672 276 tmp32 = VL53L1_isqrt(tmp32);
charlesmn 0:3ac96e360672 277 tmp32 += (uint32_t)signal_thresh_sigma;
charlesmn 0:3ac96e360672 278
charlesmn 0:3ac96e360672 279
charlesmn 0:3ac96e360672 280
charlesmn 0:3ac96e360672 281 range_mm =
charlesmn 0:3ac96e360672 282 (uint32_t)VL53L1_isqrt(ref_signal_events << 4);
charlesmn 0:3ac96e360672 283 range_mm *= ref_distance_mm;
charlesmn 0:3ac96e360672 284
charlesmn 0:3ac96e360672 285 if (tmp32 > 0) {
charlesmn 0:3ac96e360672 286 range_mm += (tmp32);
charlesmn 0:3ac96e360672 287 range_mm /= (2*tmp32);
charlesmn 0:3ac96e360672 288 }
charlesmn 0:3ac96e360672 289
charlesmn 0:3ac96e360672 290 return range_mm;
charlesmn 0:3ac96e360672 291
charlesmn 0:3ac96e360672 292 }
charlesmn 0:3ac96e360672 293
charlesmn 0:3ac96e360672 294