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_sigma_estimate.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_core_support.h"
Charles MacNeill 5:89031b2f5316 28 #include "vl53lx_error_codes.h"
Charles MacNeill 5:89031b2f5316 29
Charles MacNeill 5:89031b2f5316 30 #include "vl53lx_sigma_estimate.h"
Charles MacNeill 5:89031b2f5316 31
Charles MacNeill 5:89031b2f5316 32
Charles MacNeill 5:89031b2f5316 33 #define LOG_FUNCTION_START(fmt, ...) \
Charles MacNeill 5:89031b2f5316 34 _LOG_FUNCTION_START(VL53LX_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 35 #define LOG_FUNCTION_END(status, ...) \
Charles MacNeill 5:89031b2f5316 36 _LOG_FUNCTION_END(VL53LX_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 37 #define LOG_FUNCTION_END_FMT(status, fmt, ...) \
Charles MacNeill 5:89031b2f5316 38 _LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_PROTECTED, \
Charles MacNeill 5:89031b2f5316 39 status, fmt, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 40
Charles MacNeill 5:89031b2f5316 41 #define trace_print(level, ...) \
Charles MacNeill 5:89031b2f5316 42 _LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_PROTECTED, \
Charles MacNeill 5:89031b2f5316 43 level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
Charles MacNeill 5:89031b2f5316 44
Charles MacNeill 5:89031b2f5316 45
Charles MacNeill 5:89031b2f5316 46
Charles MacNeill 5:89031b2f5316 47 VL53LX_Error VL53LX_f_023(
Charles MacNeill 5:89031b2f5316 48 uint8_t sigma_estimator__sigma_ref_mm,
Charles MacNeill 5:89031b2f5316 49 uint32_t VL53LX_p_007,
Charles MacNeill 5:89031b2f5316 50 uint32_t VL53LX_p_032,
Charles MacNeill 5:89031b2f5316 51 uint32_t VL53LX_p_001,
Charles MacNeill 5:89031b2f5316 52 uint32_t a_zp,
Charles MacNeill 5:89031b2f5316 53 uint32_t c_zp,
Charles MacNeill 5:89031b2f5316 54 uint32_t bx,
Charles MacNeill 5:89031b2f5316 55 uint32_t ax_zp,
Charles MacNeill 5:89031b2f5316 56 uint32_t cx_zp,
Charles MacNeill 5:89031b2f5316 57 uint32_t VL53LX_p_028,
Charles MacNeill 5:89031b2f5316 58 uint16_t fast_osc_frequency,
Charles MacNeill 5:89031b2f5316 59 uint16_t *psigma_est)
Charles MacNeill 5:89031b2f5316 60 {
Charles MacNeill 5:89031b2f5316 61
Charles MacNeill 5:89031b2f5316 62
Charles MacNeill 5:89031b2f5316 63 VL53LX_Error status = VL53LX_ERROR_DIVISION_BY_ZERO;
Charles MacNeill 5:89031b2f5316 64 uint32_t sigma_int = VL53LX_D_002;
Charles MacNeill 5:89031b2f5316 65
Charles MacNeill 5:89031b2f5316 66 uint32_t pll_period_mm = 0;
Charles MacNeill 5:89031b2f5316 67
Charles MacNeill 5:89031b2f5316 68 uint64_t tmp0 = 0;
Charles MacNeill 5:89031b2f5316 69 uint64_t tmp1 = 0;
Charles MacNeill 5:89031b2f5316 70 uint64_t b_minus_amb = 0;
Charles MacNeill 5:89031b2f5316 71 uint64_t VL53LX_p_055 = 0;
Charles MacNeill 5:89031b2f5316 72
Charles MacNeill 5:89031b2f5316 73 *psigma_est = VL53LX_D_002;
Charles MacNeill 5:89031b2f5316 74
Charles MacNeill 5:89031b2f5316 75
Charles MacNeill 5:89031b2f5316 76
Charles MacNeill 5:89031b2f5316 77 if (fast_osc_frequency != 0) {
Charles MacNeill 5:89031b2f5316 78
Charles MacNeill 5:89031b2f5316 79
Charles MacNeill 5:89031b2f5316 80
Charles MacNeill 5:89031b2f5316 81 pll_period_mm = VL53LX_calc_pll_period_mm(fast_osc_frequency);
Charles MacNeill 5:89031b2f5316 82
Charles MacNeill 5:89031b2f5316 83
Charles MacNeill 5:89031b2f5316 84
Charles MacNeill 5:89031b2f5316 85 if (VL53LX_p_028 > VL53LX_p_032)
Charles MacNeill 5:89031b2f5316 86 b_minus_amb = (uint64_t)VL53LX_p_028 -
Charles MacNeill 5:89031b2f5316 87 (uint64_t)VL53LX_p_032;
Charles MacNeill 5:89031b2f5316 88 else
Charles MacNeill 5:89031b2f5316 89 b_minus_amb = (uint64_t)VL53LX_p_032 -
Charles MacNeill 5:89031b2f5316 90 (uint64_t)VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 91
Charles MacNeill 5:89031b2f5316 92
Charles MacNeill 5:89031b2f5316 93
Charles MacNeill 5:89031b2f5316 94 if (VL53LX_p_007 > VL53LX_p_001)
Charles MacNeill 5:89031b2f5316 95 VL53LX_p_055 = (uint64_t)VL53LX_p_007 -
Charles MacNeill 5:89031b2f5316 96 (uint64_t)VL53LX_p_001;
Charles MacNeill 5:89031b2f5316 97 else
Charles MacNeill 5:89031b2f5316 98 VL53LX_p_055 = (uint64_t)VL53LX_p_001 -
Charles MacNeill 5:89031b2f5316 99 (uint64_t)VL53LX_p_007;
Charles MacNeill 5:89031b2f5316 100
Charles MacNeill 5:89031b2f5316 101
Charles MacNeill 5:89031b2f5316 102
Charles MacNeill 5:89031b2f5316 103 if (b_minus_amb != 0) {
Charles MacNeill 5:89031b2f5316 104
Charles MacNeill 5:89031b2f5316 105
Charles MacNeill 5:89031b2f5316 106
Charles MacNeill 5:89031b2f5316 107
Charles MacNeill 5:89031b2f5316 108 tmp0 = (uint64_t)VL53LX_p_032 + (uint64_t)bx +
Charles MacNeill 5:89031b2f5316 109 (uint64_t)VL53LX_p_028;
Charles MacNeill 5:89031b2f5316 110 if (tmp0 > VL53LX_D_003)
Charles MacNeill 5:89031b2f5316 111 tmp0 = VL53LX_D_003;
Charles MacNeill 5:89031b2f5316 112
Charles MacNeill 5:89031b2f5316 113
Charles MacNeill 5:89031b2f5316 114
Charles MacNeill 5:89031b2f5316 115 tmp1 = (uint64_t)VL53LX_p_055 * (uint64_t)VL53LX_p_055;
Charles MacNeill 5:89031b2f5316 116 tmp1 = tmp1 << 8;
Charles MacNeill 5:89031b2f5316 117
Charles MacNeill 5:89031b2f5316 118
Charles MacNeill 5:89031b2f5316 119 if (tmp1 > VL53LX_D_004)
Charles MacNeill 5:89031b2f5316 120 tmp1 = VL53LX_D_004;
Charles MacNeill 5:89031b2f5316 121
Charles MacNeill 5:89031b2f5316 122
Charles MacNeill 5:89031b2f5316 123 tmp1 = do_division_u(tmp1, b_minus_amb);
Charles MacNeill 5:89031b2f5316 124 tmp1 = do_division_u(tmp1, b_minus_amb);
Charles MacNeill 5:89031b2f5316 125
Charles MacNeill 5:89031b2f5316 126
Charles MacNeill 5:89031b2f5316 127 if (tmp1 > (uint64_t)VL53LX_D_005)
Charles MacNeill 5:89031b2f5316 128 tmp1 = (uint64_t)VL53LX_D_005;
Charles MacNeill 5:89031b2f5316 129
Charles MacNeill 5:89031b2f5316 130
Charles MacNeill 5:89031b2f5316 131 tmp0 = tmp1 * tmp0;
Charles MacNeill 5:89031b2f5316 132
Charles MacNeill 5:89031b2f5316 133
Charles MacNeill 5:89031b2f5316 134 tmp1 = (uint64_t)c_zp + (uint64_t)cx_zp +
Charles MacNeill 5:89031b2f5316 135 (uint64_t)a_zp + (uint64_t)ax_zp;
Charles MacNeill 5:89031b2f5316 136
Charles MacNeill 5:89031b2f5316 137
Charles MacNeill 5:89031b2f5316 138 if (tmp1 > (uint64_t)VL53LX_D_003)
Charles MacNeill 5:89031b2f5316 139 tmp1 = (uint64_t)VL53LX_D_003;
Charles MacNeill 5:89031b2f5316 140
Charles MacNeill 5:89031b2f5316 141 tmp1 = tmp1 << 8;
Charles MacNeill 5:89031b2f5316 142
Charles MacNeill 5:89031b2f5316 143
Charles MacNeill 5:89031b2f5316 144 tmp0 = tmp1 + tmp0;
Charles MacNeill 5:89031b2f5316 145 if (tmp0 > (uint64_t)VL53LX_D_006)
Charles MacNeill 5:89031b2f5316 146 tmp0 = (uint64_t)VL53LX_D_006;
Charles MacNeill 5:89031b2f5316 147
Charles MacNeill 5:89031b2f5316 148
Charles MacNeill 5:89031b2f5316 149
Charles MacNeill 5:89031b2f5316 150
Charles MacNeill 5:89031b2f5316 151
Charles MacNeill 5:89031b2f5316 152
Charles MacNeill 5:89031b2f5316 153 if (tmp0 > (uint64_t)VL53LX_D_007) {
Charles MacNeill 5:89031b2f5316 154 tmp0 = do_division_u(tmp0, b_minus_amb);
Charles MacNeill 5:89031b2f5316 155 tmp0 = tmp0 * pll_period_mm;
Charles MacNeill 5:89031b2f5316 156 } else {
Charles MacNeill 5:89031b2f5316 157 tmp0 = tmp0 * pll_period_mm;
Charles MacNeill 5:89031b2f5316 158 tmp0 = do_division_u(tmp0, b_minus_amb);
Charles MacNeill 5:89031b2f5316 159 }
Charles MacNeill 5:89031b2f5316 160
Charles MacNeill 5:89031b2f5316 161
Charles MacNeill 5:89031b2f5316 162 if (tmp0 > (uint64_t)VL53LX_D_006)
Charles MacNeill 5:89031b2f5316 163 tmp0 = (uint64_t)VL53LX_D_006;
Charles MacNeill 5:89031b2f5316 164
Charles MacNeill 5:89031b2f5316 165
Charles MacNeill 5:89031b2f5316 166
Charles MacNeill 5:89031b2f5316 167 if (tmp0 > (uint64_t)VL53LX_D_007) {
Charles MacNeill 5:89031b2f5316 168 tmp0 = do_division_u(tmp0, b_minus_amb);
Charles MacNeill 5:89031b2f5316 169 tmp0 = do_division_u(tmp0, 4);
Charles MacNeill 5:89031b2f5316 170 tmp0 = tmp0 * pll_period_mm;
Charles MacNeill 5:89031b2f5316 171 } else {
Charles MacNeill 5:89031b2f5316 172 tmp0 = tmp0 * pll_period_mm;
Charles MacNeill 5:89031b2f5316 173 tmp0 = do_division_u(tmp0, b_minus_amb);
Charles MacNeill 5:89031b2f5316 174 tmp0 = do_division_u(tmp0, 4);
Charles MacNeill 5:89031b2f5316 175 }
Charles MacNeill 5:89031b2f5316 176
Charles MacNeill 5:89031b2f5316 177
Charles MacNeill 5:89031b2f5316 178 if (tmp0 > (uint64_t)VL53LX_D_006)
Charles MacNeill 5:89031b2f5316 179 tmp0 = (uint64_t)VL53LX_D_006;
Charles MacNeill 5:89031b2f5316 180
Charles MacNeill 5:89031b2f5316 181
Charles MacNeill 5:89031b2f5316 182 tmp0 = tmp0 >> 2;
Charles MacNeill 5:89031b2f5316 183
Charles MacNeill 5:89031b2f5316 184
Charles MacNeill 5:89031b2f5316 185 if (tmp0 > (uint64_t)VL53LX_D_007)
Charles MacNeill 5:89031b2f5316 186 tmp0 = (uint64_t)VL53LX_D_007;
Charles MacNeill 5:89031b2f5316 187
Charles MacNeill 5:89031b2f5316 188
Charles MacNeill 5:89031b2f5316 189 tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 7;
Charles MacNeill 5:89031b2f5316 190 tmp1 = tmp1 * tmp1;
Charles MacNeill 5:89031b2f5316 191 tmp0 = tmp0 + tmp1;
Charles MacNeill 5:89031b2f5316 192
Charles MacNeill 5:89031b2f5316 193
Charles MacNeill 5:89031b2f5316 194 if (tmp0 > (uint64_t)VL53LX_D_007)
Charles MacNeill 5:89031b2f5316 195 tmp0 = (uint64_t)VL53LX_D_007;
Charles MacNeill 5:89031b2f5316 196
Charles MacNeill 5:89031b2f5316 197
Charles MacNeill 5:89031b2f5316 198 sigma_int = VL53LX_isqrt((uint32_t)tmp0);
Charles MacNeill 5:89031b2f5316 199
Charles MacNeill 5:89031b2f5316 200 *psigma_est = (uint16_t)sigma_int;
Charles MacNeill 5:89031b2f5316 201
Charles MacNeill 5:89031b2f5316 202 status = VL53LX_ERROR_NONE;
Charles MacNeill 5:89031b2f5316 203 }
Charles MacNeill 5:89031b2f5316 204
Charles MacNeill 5:89031b2f5316 205 }
Charles MacNeill 5:89031b2f5316 206
Charles MacNeill 5:89031b2f5316 207 return status;
Charles MacNeill 5:89031b2f5316 208 }
Charles MacNeill 5:89031b2f5316 209
Charles MacNeill 5:89031b2f5316 210
Charles MacNeill 5:89031b2f5316 211
Charles MacNeill 5:89031b2f5316 212