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@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?
User | Revision | Line number | New 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 |