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_sigma_estimate.c@7:1add29d51e72, 2021-06-08 (annotated)

Committer:
Charles MacNeill
Date:
Tue Jun 08 10:34:47 2021 +0100
Revision:
7:1add29d51e72
Parent:
0:3ac96e360672 
Update to v6.6.5 of bare_driver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charlesmn 0:3ac96e360672 1
Charles MacNeill 7:1add29d51e72 2 // SPDX-License-Identifier: BSD-3-Clause
Charles MacNeill 7:1add29d51e72 3 /******************************************************************************
Charles MacNeill 7:1add29d51e72 4 * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
charlesmn 0:3ac96e360672 5
Charles MacNeill 7:1add29d51e72 6 This file is part of VL53L1 Protected and is dual licensed,
Charles MacNeill 7:1add29d51e72 7 either 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 8 or 'BSD 3-clause "New" or "Revised" License' , at your option.
Charles MacNeill 7:1add29d51e72 9
Charles MacNeill 7:1add29d51e72 10 ******************************************************************************
Charles MacNeill 7:1add29d51e72 11
Charles MacNeill 7:1add29d51e72 12 'STMicroelectronics Proprietary license'
Charles MacNeill 7:1add29d51e72 13
Charles MacNeill 7:1add29d51e72 14 ******************************************************************************
charlesmn 0:3ac96e360672 15
charlesmn 0:3ac96e360672 16 License terms: STMicroelectronics Proprietary in accordance with licensing
charlesmn 0:3ac96e360672 17 terms at www.st.com/sla0081
charlesmn 0:3ac96e360672 18
Charles MacNeill 7:1add29d51e72 19 ******************************************************************************
Charles MacNeill 7:1add29d51e72 20 */
charlesmn 0:3ac96e360672 21
charlesmn 0:3ac96e360672 22
charlesmn 0:3ac96e360672 23
charlesmn 0:3ac96e360672 24
charlesmn 0:3ac96e360672 25 #include "vl53l1_types.h"
charlesmn 0:3ac96e360672 26 #include "vl53l1_platform_log.h"
charlesmn 0:3ac96e360672 27
charlesmn 0:3ac96e360672 28 #include "vl53l1_core_support.h"
charlesmn 0:3ac96e360672 29 #include "vl53l1_error_codes.h"
charlesmn 0:3ac96e360672 30
charlesmn 0:3ac96e360672 31 #include "vl53l1_sigma_estimate.h"
charlesmn 0:3ac96e360672 32
charlesmn 0:3ac96e360672 33
charlesmn 0:3ac96e360672 34 #define LOG_FUNCTION_START(fmt, ...) \
charlesmn 0:3ac96e360672 35 _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 36 #define LOG_FUNCTION_END(status, ...) \
charlesmn 0:3ac96e360672 37 _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 38 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
charlesmn 0:3ac96e360672 39 _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_PROTECTED, \
charlesmn 0:3ac96e360672 40 status, fmt, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 41
charlesmn 0:3ac96e360672 42 #define trace_print(level, ...) \
charlesmn 0:3ac96e360672 43 _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_PROTECTED, \
charlesmn 0:3ac96e360672 44 level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
charlesmn 0:3ac96e360672 45
charlesmn 0:3ac96e360672 46
charlesmn 0:3ac96e360672 47 uint16_t VL53L1_f_042(
charlesmn 0:3ac96e360672 48 uint8_t sigma_estimator__effective_pulse_width_ns,
charlesmn 0:3ac96e360672 49 uint8_t sigma_estimator__effective_ambient_width_ns,
charlesmn 0:3ac96e360672 50 uint8_t sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 51 VL53L1_range_data_t *pdata)
charlesmn 0:3ac96e360672 52 {
charlesmn 0:3ac96e360672 53
charlesmn 0:3ac96e360672 54
charlesmn 0:3ac96e360672 55
charlesmn 0:3ac96e360672 56 uint16_t sigma_est = VL53L1_D_002;
charlesmn 0:3ac96e360672 57
charlesmn 0:3ac96e360672 58 uint32_t tmp0 = 0;
charlesmn 0:3ac96e360672 59 uint32_t tmp1 = 0;
charlesmn 0:3ac96e360672 60 uint32_t tmp2 = 0;
charlesmn 0:3ac96e360672 61
charlesmn 0:3ac96e360672 62 uint32_t sigma_est__rtn_array = 0;
charlesmn 0:3ac96e360672 63 uint32_t sigma_est__ref_array = 0;
charlesmn 0:3ac96e360672 64
charlesmn 0:3ac96e360672 65 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 66
charlesmn 0:3ac96e360672 67 if (pdata->peak_signal_count_rate_mcps > 0 &&
charlesmn 0:3ac96e360672 68 pdata->VL53L1_p_013 > 0) {
charlesmn 0:3ac96e360672 69
charlesmn 0:3ac96e360672 70
charlesmn 0:3ac96e360672 71
charlesmn 0:3ac96e360672 72 tmp0 = 100 *
charlesmn 0:3ac96e360672 73 (uint32_t)sigma_estimator__effective_pulse_width_ns;
charlesmn 0:3ac96e360672 74
charlesmn 0:3ac96e360672 75
charlesmn 0:3ac96e360672 76
charlesmn 0:3ac96e360672 77 tmp1 = ((uint32_t)sigma_estimator__effective_pulse_width_ns *
charlesmn 0:3ac96e360672 78 100 *
charlesmn 0:3ac96e360672 79 (uint32_t)sigma_estimator__effective_ambient_width_ns);
charlesmn 0:3ac96e360672 80
charlesmn 0:3ac96e360672 81 tmp1 = (tmp1 +
charlesmn 0:3ac96e360672 82 (uint32_t)pdata->peak_signal_count_rate_mcps/2) /
charlesmn 0:3ac96e360672 83 (uint32_t)pdata->peak_signal_count_rate_mcps;
charlesmn 0:3ac96e360672 84
charlesmn 0:3ac96e360672 85
charlesmn 0:3ac96e360672 86
charlesmn 0:3ac96e360672 87 sigma_est__rtn_array =
charlesmn 0:3ac96e360672 88 VL53L1_f_043(tmp0, tmp1);
charlesmn 0:3ac96e360672 89
charlesmn 0:3ac96e360672 90
charlesmn 0:3ac96e360672 91
charlesmn 0:3ac96e360672 92 sigma_est__rtn_array =
charlesmn 0:3ac96e360672 93 ((VL53L1_SPEED_OF_LIGHT_IN_AIR + 1000) / 2000) *
charlesmn 0:3ac96e360672 94 sigma_est__rtn_array;
charlesmn 0:3ac96e360672 95
charlesmn 0:3ac96e360672 96
charlesmn 0:3ac96e360672 97
charlesmn 0:3ac96e360672 98 tmp2 =
charlesmn 0:3ac96e360672 99 VL53L1_isqrt(12 * (uint32_t)pdata->VL53L1_p_013);
charlesmn 0:3ac96e360672 100
charlesmn 0:3ac96e360672 101 if (tmp2 > 0) {
charlesmn 0:3ac96e360672 102
charlesmn 0:3ac96e360672 103 sigma_est__rtn_array =
charlesmn 0:3ac96e360672 104 (sigma_est__rtn_array + tmp2/2) / tmp2;
charlesmn 0:3ac96e360672 105
charlesmn 0:3ac96e360672 106
charlesmn 0:3ac96e360672 107
charlesmn 0:3ac96e360672 108 sigma_est__ref_array =
charlesmn 0:3ac96e360672 109 100 * (uint32_t)sigma_estimator__sigma_ref_mm;
charlesmn 0:3ac96e360672 110
charlesmn 0:3ac96e360672 111 sigma_est =
charlesmn 0:3ac96e360672 112 (uint16_t)VL53L1_f_043(
charlesmn 0:3ac96e360672 113 (uint32_t)sigma_est__ref_array,
charlesmn 0:3ac96e360672 114 sigma_est__rtn_array);
charlesmn 0:3ac96e360672 115
charlesmn 0:3ac96e360672 116 } else {
charlesmn 0:3ac96e360672 117 sigma_est = VL53L1_D_002;
charlesmn 0:3ac96e360672 118 }
charlesmn 0:3ac96e360672 119
charlesmn 0:3ac96e360672 120 }
charlesmn 0:3ac96e360672 121
charlesmn 0:3ac96e360672 122 pdata->VL53L1_p_005 = sigma_est;
charlesmn 0:3ac96e360672 123
charlesmn 0:3ac96e360672 124 LOG_FUNCTION_END(0);
charlesmn 0:3ac96e360672 125
charlesmn 0:3ac96e360672 126 return sigma_est;
charlesmn 0:3ac96e360672 127
charlesmn 0:3ac96e360672 128 }
charlesmn 0:3ac96e360672 129
charlesmn 0:3ac96e360672 130
charlesmn 0:3ac96e360672 131 uint16_t VL53L1_f_044(
charlesmn 0:3ac96e360672 132 uint8_t sigma_estimator__effective_pulse_width_ns,
charlesmn 0:3ac96e360672 133 uint8_t sigma_estimator__effective_ambient_width_ns,
charlesmn 0:3ac96e360672 134 uint8_t sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 135 VL53L1_range_data_t *pdata)
charlesmn 0:3ac96e360672 136 {
charlesmn 0:3ac96e360672 137
charlesmn 0:3ac96e360672 138
charlesmn 0:3ac96e360672 139 uint16_t sigma_est = VL53L1_D_002;
charlesmn 0:3ac96e360672 140
charlesmn 0:3ac96e360672 141 uint32_t eqn7 = 0;
charlesmn 0:3ac96e360672 142 uint32_t sigma_est__ref_sq = 0;
charlesmn 0:3ac96e360672 143 uint32_t sigma_est__rtn_sq = 0;
charlesmn 0:3ac96e360672 144
charlesmn 0:3ac96e360672 145 uint64_t tmp0 = 0;
charlesmn 0:3ac96e360672 146 uint64_t tmp1 = 0;
charlesmn 0:3ac96e360672 147
charlesmn 0:3ac96e360672 148 LOG_FUNCTION_START("");
charlesmn 0:3ac96e360672 149
charlesmn 0:3ac96e360672 150 if (pdata->peak_signal_count_rate_mcps > 0 &&
charlesmn 0:3ac96e360672 151 pdata->VL53L1_p_013 > 0) {
charlesmn 0:3ac96e360672 152
charlesmn 0:3ac96e360672 153
charlesmn 0:3ac96e360672 154
charlesmn 0:3ac96e360672 155 eqn7 = 4573 * 4573;
charlesmn 0:3ac96e360672 156 eqn7 = eqn7 / (3 * (uint32_t)pdata->VL53L1_p_013);
charlesmn 0:3ac96e360672 157
charlesmn 0:3ac96e360672 158
charlesmn 0:3ac96e360672 159
charlesmn 0:3ac96e360672 160 tmp0 = ((uint64_t)sigma_estimator__effective_pulse_width_ns)
charlesmn 0:3ac96e360672 161 << 8;
charlesmn 0:3ac96e360672 162
charlesmn 0:3ac96e360672 163
charlesmn 0:3ac96e360672 164
charlesmn 0:3ac96e360672 165 tmp1 = ((uint64_t)pdata->ambient_count_rate_mcps *
charlesmn 0:3ac96e360672 166 (uint64_t)sigma_estimator__effective_ambient_width_ns)
charlesmn 0:3ac96e360672 167 << 8;
charlesmn 0:3ac96e360672 168
charlesmn 0:3ac96e360672 169 tmp1 = do_division_u(tmp1,
charlesmn 0:3ac96e360672 170 (uint64_t)pdata->peak_signal_count_rate_mcps);
charlesmn 0:3ac96e360672 171
charlesmn 0:3ac96e360672 172
charlesmn 0:3ac96e360672 173
charlesmn 0:3ac96e360672 174 tmp1 = 16 * (uint64_t)eqn7 * (tmp0 * tmp0 + tmp1 * tmp1);
charlesmn 0:3ac96e360672 175 tmp1 = do_division_u(tmp1, (15625 * 15625));
charlesmn 0:3ac96e360672 176 sigma_est__rtn_sq = (uint32_t)tmp1;
charlesmn 0:3ac96e360672 177
charlesmn 0:3ac96e360672 178
charlesmn 0:3ac96e360672 179
charlesmn 0:3ac96e360672 180 sigma_est__ref_sq = ((uint32_t)sigma_estimator__sigma_ref_mm)
charlesmn 0:3ac96e360672 181 << 2;
charlesmn 0:3ac96e360672 182
charlesmn 0:3ac96e360672 183 sigma_est__ref_sq = sigma_est__ref_sq * sigma_est__ref_sq;
charlesmn 0:3ac96e360672 184
charlesmn 0:3ac96e360672 185
charlesmn 0:3ac96e360672 186
charlesmn 0:3ac96e360672 187 sigma_est = (uint16_t)VL53L1_isqrt(sigma_est__ref_sq +
charlesmn 0:3ac96e360672 188 sigma_est__rtn_sq);
charlesmn 0:3ac96e360672 189
charlesmn 0:3ac96e360672 190 }
charlesmn 0:3ac96e360672 191
charlesmn 0:3ac96e360672 192 pdata->VL53L1_p_005 = sigma_est;
charlesmn 0:3ac96e360672 193
charlesmn 0:3ac96e360672 194 LOG_FUNCTION_END(0);
charlesmn 0:3ac96e360672 195
charlesmn 0:3ac96e360672 196 return sigma_est;
charlesmn 0:3ac96e360672 197
charlesmn 0:3ac96e360672 198 }
charlesmn 0:3ac96e360672 199
charlesmn 0:3ac96e360672 200
charlesmn 0:3ac96e360672 201
charlesmn 0:3ac96e360672 202 VL53L1_Error VL53L1_f_045(
charlesmn 0:3ac96e360672 203 uint8_t sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 204 uint32_t VL53L1_p_003,
charlesmn 0:3ac96e360672 205 uint32_t VL53L1_p_018,
charlesmn 0:3ac96e360672 206 uint32_t VL53L1_p_001,
charlesmn 0:3ac96e360672 207 uint32_t a_zp,
charlesmn 0:3ac96e360672 208 uint32_t c_zp,
charlesmn 0:3ac96e360672 209 uint32_t bx,
charlesmn 0:3ac96e360672 210 uint32_t ax_zp,
charlesmn 0:3ac96e360672 211 uint32_t cx_zp,
charlesmn 0:3ac96e360672 212 uint32_t VL53L1_p_004,
charlesmn 0:3ac96e360672 213 uint16_t fast_osc_frequency,
charlesmn 0:3ac96e360672 214 uint16_t *psigma_est)
charlesmn 0:3ac96e360672 215 {
charlesmn 0:3ac96e360672 216
charlesmn 0:3ac96e360672 217
charlesmn 0:3ac96e360672 218 VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
charlesmn 0:3ac96e360672 219 uint32_t sigma_int = VL53L1_D_002;
charlesmn 0:3ac96e360672 220
charlesmn 0:3ac96e360672 221 uint32_t pll_period_mm = 0;
charlesmn 0:3ac96e360672 222
charlesmn 0:3ac96e360672 223 uint64_t tmp0 = 0;
charlesmn 0:3ac96e360672 224 uint64_t tmp1 = 0;
charlesmn 0:3ac96e360672 225 uint64_t b_minus_amb = 0;
charlesmn 0:3ac96e360672 226 uint64_t VL53L1_p_041 = 0;
charlesmn 0:3ac96e360672 227
charlesmn 0:3ac96e360672 228 *psigma_est = VL53L1_D_002;
charlesmn 0:3ac96e360672 229
charlesmn 0:3ac96e360672 230
charlesmn 0:3ac96e360672 231
charlesmn 0:3ac96e360672 232 if (fast_osc_frequency != 0) {
charlesmn 0:3ac96e360672 233
charlesmn 0:3ac96e360672 234
charlesmn 0:3ac96e360672 235
charlesmn 0:3ac96e360672 236 pll_period_mm = VL53L1_calc_pll_period_mm(fast_osc_frequency);
charlesmn 0:3ac96e360672 237
charlesmn 0:3ac96e360672 238
charlesmn 0:3ac96e360672 239 pll_period_mm = (pll_period_mm + 0x02) >> 2;
charlesmn 0:3ac96e360672 240
charlesmn 0:3ac96e360672 241
charlesmn 0:3ac96e360672 242
charlesmn 0:3ac96e360672 243 if (VL53L1_p_004 > VL53L1_p_018)
charlesmn 0:3ac96e360672 244 b_minus_amb = (uint64_t)VL53L1_p_004 -
charlesmn 0:3ac96e360672 245 (uint64_t)VL53L1_p_018;
charlesmn 0:3ac96e360672 246 else
charlesmn 0:3ac96e360672 247 b_minus_amb = (uint64_t)VL53L1_p_018 -
charlesmn 0:3ac96e360672 248 (uint64_t)VL53L1_p_004;
charlesmn 0:3ac96e360672 249
charlesmn 0:3ac96e360672 250
charlesmn 0:3ac96e360672 251
charlesmn 0:3ac96e360672 252 if (VL53L1_p_003 > VL53L1_p_001)
charlesmn 0:3ac96e360672 253 VL53L1_p_041 = (uint64_t)VL53L1_p_003 -
charlesmn 0:3ac96e360672 254 (uint64_t)VL53L1_p_001;
charlesmn 0:3ac96e360672 255 else
charlesmn 0:3ac96e360672 256 VL53L1_p_041 = (uint64_t)VL53L1_p_001 -
charlesmn 0:3ac96e360672 257 (uint64_t)VL53L1_p_003;
charlesmn 0:3ac96e360672 258
charlesmn 0:3ac96e360672 259
charlesmn 0:3ac96e360672 260
charlesmn 0:3ac96e360672 261
charlesmn 0:3ac96e360672 262
charlesmn 0:3ac96e360672 263 if (b_minus_amb != 0) {
charlesmn 0:3ac96e360672 264
charlesmn 0:3ac96e360672 265
charlesmn 0:3ac96e360672 266
charlesmn 0:3ac96e360672 267 tmp0 = (uint64_t)pll_period_mm *
charlesmn 0:3ac96e360672 268 (uint64_t)pll_period_mm;
charlesmn 0:3ac96e360672 269 tmp0 = tmp0 * ((uint64_t)c_zp +
charlesmn 0:3ac96e360672 270 (uint64_t)cx_zp + (uint64_t)a_zp +
charlesmn 0:3ac96e360672 271 (uint64_t)ax_zp);
charlesmn 0:3ac96e360672 272 tmp0 = do_division_u((tmp0 + (b_minus_amb >> 1)),
charlesmn 0:3ac96e360672 273 b_minus_amb);
charlesmn 0:3ac96e360672 274
charlesmn 0:3ac96e360672 275
charlesmn 0:3ac96e360672 276
charlesmn 0:3ac96e360672 277 tmp1 = (uint64_t)pll_period_mm *
charlesmn 0:3ac96e360672 278 (uint64_t)pll_period_mm * VL53L1_p_041;
charlesmn 0:3ac96e360672 279 tmp1 = do_division_u((tmp1 + (b_minus_amb >> 1)),
charlesmn 0:3ac96e360672 280 b_minus_amb);
charlesmn 0:3ac96e360672 281
charlesmn 0:3ac96e360672 282 tmp1 = tmp1 * VL53L1_p_041;
charlesmn 0:3ac96e360672 283 tmp1 = do_division_u((tmp1 + (b_minus_amb >> 1)),
charlesmn 0:3ac96e360672 284 b_minus_amb);
charlesmn 0:3ac96e360672 285
charlesmn 0:3ac96e360672 286 tmp1 = tmp1 * ((uint64_t)VL53L1_p_018 + (uint64_t)bx +
charlesmn 0:3ac96e360672 287 (uint64_t)VL53L1_p_004);
charlesmn 0:3ac96e360672 288 tmp1 = do_division_u((tmp1 + (b_minus_amb >> 1)),
charlesmn 0:3ac96e360672 289 b_minus_amb);
charlesmn 0:3ac96e360672 290
charlesmn 0:3ac96e360672 291
charlesmn 0:3ac96e360672 292
charlesmn 0:3ac96e360672 293 tmp0 = tmp0 + tmp1;
charlesmn 0:3ac96e360672 294 tmp0 = do_division_u((tmp0 + (b_minus_amb >> 1)),
charlesmn 0:3ac96e360672 295 b_minus_amb);
charlesmn 0:3ac96e360672 296 tmp0 = (tmp0 + 0x01) >> 2;
charlesmn 0:3ac96e360672 297
charlesmn 0:3ac96e360672 298
charlesmn 0:3ac96e360672 299
charlesmn 0:3ac96e360672 300 tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 2;
charlesmn 0:3ac96e360672 301 tmp1 = tmp1 * tmp1;
charlesmn 0:3ac96e360672 302 tmp0 = tmp0 + tmp1;
charlesmn 0:3ac96e360672 303
charlesmn 0:3ac96e360672 304
charlesmn 0:3ac96e360672 305
charlesmn 0:3ac96e360672 306 if (tmp0 > 0xFFFFFFFF)
charlesmn 0:3ac96e360672 307 tmp0 = 0xFFFFFFFF;
charlesmn 0:3ac96e360672 308
charlesmn 0:3ac96e360672 309 sigma_int = VL53L1_isqrt((uint32_t)tmp0);
charlesmn 0:3ac96e360672 310
charlesmn 0:3ac96e360672 311
charlesmn 0:3ac96e360672 312
charlesmn 0:3ac96e360672 313 if (sigma_int > VL53L1_D_002)
charlesmn 0:3ac96e360672 314 *psigma_est =
charlesmn 0:3ac96e360672 315 (uint16_t)VL53L1_D_002;
charlesmn 0:3ac96e360672 316 else
charlesmn 0:3ac96e360672 317 *psigma_est = (uint16_t)sigma_int;
charlesmn 0:3ac96e360672 318
charlesmn 0:3ac96e360672 319 status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 320 }
charlesmn 0:3ac96e360672 321
charlesmn 0:3ac96e360672 322 }
charlesmn 0:3ac96e360672 323
charlesmn 0:3ac96e360672 324 return status;
charlesmn 0:3ac96e360672 325 }
charlesmn 0:3ac96e360672 326
charlesmn 0:3ac96e360672 327
charlesmn 0:3ac96e360672 328
charlesmn 0:3ac96e360672 329 VL53L1_Error VL53L1_f_014(
charlesmn 0:3ac96e360672 330 uint8_t sigma_estimator__sigma_ref_mm,
charlesmn 0:3ac96e360672 331 uint32_t VL53L1_p_003,
charlesmn 0:3ac96e360672 332 uint32_t VL53L1_p_018,
charlesmn 0:3ac96e360672 333 uint32_t VL53L1_p_001,
charlesmn 0:3ac96e360672 334 uint32_t a_zp,
charlesmn 0:3ac96e360672 335 uint32_t c_zp,
charlesmn 0:3ac96e360672 336 uint32_t bx,
charlesmn 0:3ac96e360672 337 uint32_t ax_zp,
charlesmn 0:3ac96e360672 338 uint32_t cx_zp,
charlesmn 0:3ac96e360672 339 uint32_t VL53L1_p_004,
charlesmn 0:3ac96e360672 340 uint16_t fast_osc_frequency,
charlesmn 0:3ac96e360672 341 uint16_t *psigma_est)
charlesmn 0:3ac96e360672 342 {
charlesmn 0:3ac96e360672 343
charlesmn 0:3ac96e360672 344
charlesmn 0:3ac96e360672 345 VL53L1_Error status = VL53L1_ERROR_DIVISION_BY_ZERO;
charlesmn 0:3ac96e360672 346 uint32_t sigma_int = VL53L1_D_002;
charlesmn 0:3ac96e360672 347
charlesmn 0:3ac96e360672 348 uint32_t pll_period_mm = 0;
charlesmn 0:3ac96e360672 349
charlesmn 0:3ac96e360672 350 uint64_t tmp0 = 0;
charlesmn 0:3ac96e360672 351 uint64_t tmp1 = 0;
charlesmn 0:3ac96e360672 352 uint64_t b_minus_amb = 0;
charlesmn 0:3ac96e360672 353 uint64_t VL53L1_p_041 = 0;
charlesmn 0:3ac96e360672 354
charlesmn 0:3ac96e360672 355 *psigma_est = VL53L1_D_002;
charlesmn 0:3ac96e360672 356
charlesmn 0:3ac96e360672 357
charlesmn 0:3ac96e360672 358
charlesmn 0:3ac96e360672 359 if (fast_osc_frequency != 0) {
charlesmn 0:3ac96e360672 360
charlesmn 0:3ac96e360672 361
charlesmn 0:3ac96e360672 362
charlesmn 0:3ac96e360672 363 pll_period_mm = VL53L1_calc_pll_period_mm(fast_osc_frequency);
charlesmn 0:3ac96e360672 364
charlesmn 0:3ac96e360672 365
charlesmn 0:3ac96e360672 366
charlesmn 0:3ac96e360672 367 if (VL53L1_p_004 > VL53L1_p_018)
charlesmn 0:3ac96e360672 368 b_minus_amb = (uint64_t)VL53L1_p_004 -
charlesmn 0:3ac96e360672 369 (uint64_t)VL53L1_p_018;
charlesmn 0:3ac96e360672 370 else
charlesmn 0:3ac96e360672 371 b_minus_amb = (uint64_t)VL53L1_p_018 -
charlesmn 0:3ac96e360672 372 (uint64_t)VL53L1_p_004;
charlesmn 0:3ac96e360672 373
charlesmn 0:3ac96e360672 374
charlesmn 0:3ac96e360672 375
charlesmn 0:3ac96e360672 376 if (VL53L1_p_003 > VL53L1_p_001)
charlesmn 0:3ac96e360672 377 VL53L1_p_041 = (uint64_t)VL53L1_p_003 -
charlesmn 0:3ac96e360672 378 (uint64_t)VL53L1_p_001;
charlesmn 0:3ac96e360672 379 else
charlesmn 0:3ac96e360672 380 VL53L1_p_041 = (uint64_t)VL53L1_p_001 -
charlesmn 0:3ac96e360672 381 (uint64_t)VL53L1_p_003;
charlesmn 0:3ac96e360672 382
charlesmn 0:3ac96e360672 383
charlesmn 0:3ac96e360672 384
charlesmn 0:3ac96e360672 385 if (b_minus_amb != 0) {
charlesmn 0:3ac96e360672 386
charlesmn 0:3ac96e360672 387
charlesmn 0:3ac96e360672 388
charlesmn 0:3ac96e360672 389
charlesmn 0:3ac96e360672 390 tmp0 = (uint64_t)VL53L1_p_018 + (uint64_t)bx +
charlesmn 0:3ac96e360672 391 (uint64_t)VL53L1_p_004;
charlesmn 0:3ac96e360672 392 if (tmp0 > VL53L1_D_003)
charlesmn 0:3ac96e360672 393 tmp0 = VL53L1_D_003;
charlesmn 0:3ac96e360672 394
charlesmn 0:3ac96e360672 395
charlesmn 0:3ac96e360672 396
charlesmn 0:3ac96e360672 397 tmp1 = (uint64_t)VL53L1_p_041 * (uint64_t)VL53L1_p_041;
charlesmn 0:3ac96e360672 398 tmp1 = tmp1 << 8;
charlesmn 0:3ac96e360672 399
charlesmn 0:3ac96e360672 400
charlesmn 0:3ac96e360672 401 if (tmp1 > VL53L1_D_004)
charlesmn 0:3ac96e360672 402 tmp1 = VL53L1_D_004;
charlesmn 0:3ac96e360672 403
charlesmn 0:3ac96e360672 404
charlesmn 0:3ac96e360672 405 tmp1 = do_division_u(tmp1, b_minus_amb);
charlesmn 0:3ac96e360672 406 tmp1 = do_division_u(tmp1, b_minus_amb);
charlesmn 0:3ac96e360672 407
charlesmn 0:3ac96e360672 408
charlesmn 0:3ac96e360672 409 if (tmp1 > (uint64_t)VL53L1_D_005)
charlesmn 0:3ac96e360672 410 tmp1 = (uint64_t)VL53L1_D_005;
charlesmn 0:3ac96e360672 411
charlesmn 0:3ac96e360672 412
charlesmn 0:3ac96e360672 413 tmp0 = tmp1 * tmp0;
charlesmn 0:3ac96e360672 414
charlesmn 0:3ac96e360672 415
charlesmn 0:3ac96e360672 416 tmp1 = (uint64_t)c_zp + (uint64_t)cx_zp +
charlesmn 0:3ac96e360672 417 (uint64_t)a_zp + (uint64_t)ax_zp;
charlesmn 0:3ac96e360672 418
charlesmn 0:3ac96e360672 419
charlesmn 0:3ac96e360672 420 if (tmp1 > (uint64_t)VL53L1_D_003)
charlesmn 0:3ac96e360672 421 tmp1 = (uint64_t)VL53L1_D_003;
charlesmn 0:3ac96e360672 422
charlesmn 0:3ac96e360672 423 tmp1 = tmp1 << 8;
charlesmn 0:3ac96e360672 424
charlesmn 0:3ac96e360672 425
charlesmn 0:3ac96e360672 426 tmp0 = tmp1 + tmp0;
charlesmn 0:3ac96e360672 427 if (tmp0 > (uint64_t)VL53L1_D_006)
charlesmn 0:3ac96e360672 428 tmp0 = (uint64_t)VL53L1_D_006;
charlesmn 0:3ac96e360672 429
charlesmn 0:3ac96e360672 430
charlesmn 0:3ac96e360672 431
charlesmn 0:3ac96e360672 432
charlesmn 0:3ac96e360672 433
charlesmn 0:3ac96e360672 434
charlesmn 0:3ac96e360672 435 if (tmp0 > (uint64_t)VL53L1_D_007) {
charlesmn 0:3ac96e360672 436 tmp0 = do_division_u(tmp0, b_minus_amb);
charlesmn 0:3ac96e360672 437 tmp0 = tmp0 * pll_period_mm;
charlesmn 0:3ac96e360672 438 } else {
charlesmn 0:3ac96e360672 439 tmp0 = tmp0 * pll_period_mm;
charlesmn 0:3ac96e360672 440 tmp0 = do_division_u(tmp0, b_minus_amb);
charlesmn 0:3ac96e360672 441 }
charlesmn 0:3ac96e360672 442
charlesmn 0:3ac96e360672 443
charlesmn 0:3ac96e360672 444 if (tmp0 > (uint64_t)VL53L1_D_006)
charlesmn 0:3ac96e360672 445 tmp0 = (uint64_t)VL53L1_D_006;
charlesmn 0:3ac96e360672 446
charlesmn 0:3ac96e360672 447
charlesmn 0:3ac96e360672 448
charlesmn 0:3ac96e360672 449 if (tmp0 > (uint64_t)VL53L1_D_007) {
charlesmn 0:3ac96e360672 450 tmp0 = do_division_u(tmp0, b_minus_amb);
charlesmn 0:3ac96e360672 451 tmp0 = do_division_u(tmp0, 4);
charlesmn 0:3ac96e360672 452 tmp0 = tmp0 * pll_period_mm;
charlesmn 0:3ac96e360672 453 } else {
charlesmn 0:3ac96e360672 454 tmp0 = tmp0 * pll_period_mm;
charlesmn 0:3ac96e360672 455 tmp0 = do_division_u(tmp0, b_minus_amb);
charlesmn 0:3ac96e360672 456 tmp0 = do_division_u(tmp0, 4);
charlesmn 0:3ac96e360672 457 }
charlesmn 0:3ac96e360672 458
charlesmn 0:3ac96e360672 459
charlesmn 0:3ac96e360672 460 if (tmp0 > (uint64_t)VL53L1_D_006)
charlesmn 0:3ac96e360672 461 tmp0 = (uint64_t)VL53L1_D_006;
charlesmn 0:3ac96e360672 462
charlesmn 0:3ac96e360672 463
charlesmn 0:3ac96e360672 464 tmp0 = tmp0 >> 2;
charlesmn 0:3ac96e360672 465
charlesmn 0:3ac96e360672 466
charlesmn 0:3ac96e360672 467 if (tmp0 > (uint64_t)VL53L1_D_007)
charlesmn 0:3ac96e360672 468 tmp0 = (uint64_t)VL53L1_D_007;
charlesmn 0:3ac96e360672 469
charlesmn 0:3ac96e360672 470
charlesmn 0:3ac96e360672 471 tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 7;
charlesmn 0:3ac96e360672 472 tmp1 = tmp1 * tmp1;
charlesmn 0:3ac96e360672 473 tmp0 = tmp0 + tmp1;
charlesmn 0:3ac96e360672 474
charlesmn 0:3ac96e360672 475
charlesmn 0:3ac96e360672 476 if (tmp0 > (uint64_t)VL53L1_D_007)
charlesmn 0:3ac96e360672 477 tmp0 = (uint64_t)VL53L1_D_007;
charlesmn 0:3ac96e360672 478
charlesmn 0:3ac96e360672 479
charlesmn 0:3ac96e360672 480 sigma_int = VL53L1_isqrt((uint32_t)tmp0);
charlesmn 0:3ac96e360672 481
charlesmn 0:3ac96e360672 482 *psigma_est = (uint16_t)sigma_int;
charlesmn 0:3ac96e360672 483
charlesmn 0:3ac96e360672 484 status = VL53L1_ERROR_NONE;
charlesmn 0:3ac96e360672 485 }
charlesmn 0:3ac96e360672 486
charlesmn 0:3ac96e360672 487 }
charlesmn 0:3ac96e360672 488
charlesmn 0:3ac96e360672 489 return status;
charlesmn 0:3ac96e360672 490 }
charlesmn 0:3ac96e360672 491
charlesmn 0:3ac96e360672 492 uint32_t VL53L1_f_046(
charlesmn 0:3ac96e360672 493 uint64_t VL53L1_p_003,
charlesmn 0:3ac96e360672 494 uint32_t size
charlesmn 0:3ac96e360672 495 )
charlesmn 0:3ac96e360672 496 {
charlesmn 0:3ac96e360672 497
charlesmn 0:3ac96e360672 498
charlesmn 0:3ac96e360672 499 uint64_t next;
charlesmn 0:3ac96e360672 500 uint64_t upper;
charlesmn 0:3ac96e360672 501 uint64_t lower;
charlesmn 0:3ac96e360672 502 uint32_t stepsize;
charlesmn 0:3ac96e360672 503 uint32_t count;
charlesmn 0:3ac96e360672 504
charlesmn 0:3ac96e360672 505
charlesmn 0:3ac96e360672 506 next = VL53L1_p_003;
charlesmn 0:3ac96e360672 507 upper = 0;
charlesmn 0:3ac96e360672 508 lower = 0;
charlesmn 0:3ac96e360672 509 stepsize = size/2;
charlesmn 0:3ac96e360672 510 count = 0;
charlesmn 0:3ac96e360672 511
charlesmn 0:3ac96e360672 512 while (1) {
charlesmn 0:3ac96e360672 513 upper = next >> stepsize;
charlesmn 0:3ac96e360672 514 lower = next & ((1 << stepsize) - 1);
charlesmn 0:3ac96e360672 515
charlesmn 0:3ac96e360672 516 if (upper != 0) {
charlesmn 0:3ac96e360672 517 count += stepsize;
charlesmn 0:3ac96e360672 518 next = upper;
charlesmn 0:3ac96e360672 519 } else {
charlesmn 0:3ac96e360672 520 next = lower;
charlesmn 0:3ac96e360672 521 }
charlesmn 0:3ac96e360672 522
charlesmn 0:3ac96e360672 523 stepsize = stepsize / 2;
charlesmn 0:3ac96e360672 524 if (stepsize == 0)
charlesmn 0:3ac96e360672 525 break;
charlesmn 0:3ac96e360672 526 }
charlesmn 0:3ac96e360672 527
charlesmn 0:3ac96e360672 528 return count;
charlesmn 0:3ac96e360672 529 }
charlesmn 0:3ac96e360672 530
charlesmn 0:3ac96e360672 531
charlesmn 0:3ac96e360672 532
charlesmn 0:3ac96e360672 533 uint32_t VL53L1_f_043(
charlesmn 0:3ac96e360672 534 uint32_t VL53L1_p_003,
charlesmn 0:3ac96e360672 535 uint32_t VL53L1_p_018)
charlesmn 0:3ac96e360672 536 {
charlesmn 0:3ac96e360672 537
charlesmn 0:3ac96e360672 538
charlesmn 0:3ac96e360672 539 uint32_t res = 0;
charlesmn 0:3ac96e360672 540
charlesmn 0:3ac96e360672 541 if (VL53L1_p_003 > 65535 || VL53L1_p_018 > 65535)
charlesmn 0:3ac96e360672 542 res = 65535;
charlesmn 0:3ac96e360672 543 else
charlesmn 0:3ac96e360672 544 res = VL53L1_isqrt(VL53L1_p_003*VL53L1_p_003 +
charlesmn 0:3ac96e360672 545 VL53L1_p_018*VL53L1_p_018);
charlesmn 0:3ac96e360672 546
charlesmn 0:3ac96e360672 547 return res;
charlesmn 0:3ac96e360672 548 }
charlesmn 0:3ac96e360672 549
charlesmn 0:3ac96e360672 550