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@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?

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