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