205 lines
4.1 KiB
C
205 lines
4.1 KiB
C
/**
|
|
******************************************************************************
|
|
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
|
|
|
|
This file is part of VL53LX Protected and is licensed
|
|
'STMicroelectronics Proprietary license'
|
|
|
|
License terms: STMicroelectronics Proprietary in accordance with licensing
|
|
terms at www.st.com/sla0081
|
|
|
|
******************************************************************************
|
|
*/
|
|
|
|
|
|
|
|
|
|
#include <vl53lx_platform_log.h>
|
|
#include <vl53lx_types.h>
|
|
#include "vl53lx_core_support.h"
|
|
#include "vl53lx_error_codes.h"
|
|
|
|
#include "vl53lx_sigma_estimate.h"
|
|
|
|
|
|
#define LOG_FUNCTION_START(fmt, ...) \
|
|
_LOG_FUNCTION_START(VL53LX_TRACE_MODULE_PROTECTED, fmt, ##__VA_ARGS__)
|
|
#define LOG_FUNCTION_END(status, ...) \
|
|
_LOG_FUNCTION_END(VL53LX_TRACE_MODULE_PROTECTED, status, ##__VA_ARGS__)
|
|
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
|
_LOG_FUNCTION_END_FMT(VL53LX_TRACE_MODULE_PROTECTED, \
|
|
status, fmt, ##__VA_ARGS__)
|
|
|
|
#define trace_print(level, ...) \
|
|
_LOG_TRACE_PRINT(VL53LX_TRACE_MODULE_PROTECTED, \
|
|
level, VL53LX_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
|
|
|
|
|
|
|
|
VL53LX_Error VL53LX_f_023(
|
|
uint8_t sigma_estimator__sigma_ref_mm,
|
|
uint32_t VL53LX_p_007,
|
|
uint32_t VL53LX_p_032,
|
|
uint32_t VL53LX_p_001,
|
|
uint32_t a_zp,
|
|
uint32_t c_zp,
|
|
uint32_t bx,
|
|
uint32_t ax_zp,
|
|
uint32_t cx_zp,
|
|
uint32_t VL53LX_p_028,
|
|
uint16_t fast_osc_frequency,
|
|
uint16_t *psigma_est)
|
|
{
|
|
|
|
|
|
VL53LX_Error status = VL53LX_ERROR_DIVISION_BY_ZERO;
|
|
uint32_t sigma_int = VL53LX_D_002;
|
|
|
|
uint32_t pll_period_mm = 0;
|
|
|
|
uint64_t tmp0 = 0;
|
|
uint64_t tmp1 = 0;
|
|
uint64_t b_minus_amb = 0;
|
|
uint64_t VL53LX_p_055 = 0;
|
|
|
|
*psigma_est = VL53LX_D_002;
|
|
|
|
|
|
|
|
if (fast_osc_frequency != 0) {
|
|
|
|
|
|
|
|
pll_period_mm = VL53LX_calc_pll_period_mm(fast_osc_frequency);
|
|
|
|
|
|
|
|
if (VL53LX_p_028 > VL53LX_p_032)
|
|
b_minus_amb = (uint64_t)VL53LX_p_028 -
|
|
(uint64_t)VL53LX_p_032;
|
|
else
|
|
b_minus_amb = (uint64_t)VL53LX_p_032 -
|
|
(uint64_t)VL53LX_p_028;
|
|
|
|
|
|
|
|
if (VL53LX_p_007 > VL53LX_p_001)
|
|
VL53LX_p_055 = (uint64_t)VL53LX_p_007 -
|
|
(uint64_t)VL53LX_p_001;
|
|
else
|
|
VL53LX_p_055 = (uint64_t)VL53LX_p_001 -
|
|
(uint64_t)VL53LX_p_007;
|
|
|
|
|
|
|
|
if (b_minus_amb != 0) {
|
|
|
|
|
|
|
|
|
|
tmp0 = (uint64_t)VL53LX_p_032 + (uint64_t)bx +
|
|
(uint64_t)VL53LX_p_028;
|
|
if (tmp0 > VL53LX_D_003)
|
|
tmp0 = VL53LX_D_003;
|
|
|
|
|
|
|
|
tmp1 = (uint64_t)VL53LX_p_055 * (uint64_t)VL53LX_p_055;
|
|
tmp1 = tmp1 << 8;
|
|
|
|
|
|
if (tmp1 > VL53LX_D_004)
|
|
tmp1 = VL53LX_D_004;
|
|
|
|
|
|
tmp1 = do_division_u(tmp1, b_minus_amb);
|
|
tmp1 = do_division_u(tmp1, b_minus_amb);
|
|
|
|
|
|
if (tmp1 > (uint64_t)VL53LX_D_005)
|
|
tmp1 = (uint64_t)VL53LX_D_005;
|
|
|
|
|
|
tmp0 = tmp1 * tmp0;
|
|
|
|
|
|
tmp1 = (uint64_t)c_zp + (uint64_t)cx_zp +
|
|
(uint64_t)a_zp + (uint64_t)ax_zp;
|
|
|
|
|
|
if (tmp1 > (uint64_t)VL53LX_D_003)
|
|
tmp1 = (uint64_t)VL53LX_D_003;
|
|
|
|
tmp1 = tmp1 << 8;
|
|
|
|
|
|
tmp0 = tmp1 + tmp0;
|
|
if (tmp0 > (uint64_t)VL53LX_D_006)
|
|
tmp0 = (uint64_t)VL53LX_D_006;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (tmp0 > (uint64_t)VL53LX_D_007) {
|
|
tmp0 = do_division_u(tmp0, b_minus_amb);
|
|
tmp0 = tmp0 * pll_period_mm;
|
|
} else {
|
|
tmp0 = tmp0 * pll_period_mm;
|
|
tmp0 = do_division_u(tmp0, b_minus_amb);
|
|
}
|
|
|
|
|
|
if (tmp0 > (uint64_t)VL53LX_D_006)
|
|
tmp0 = (uint64_t)VL53LX_D_006;
|
|
|
|
|
|
|
|
if (tmp0 > (uint64_t)VL53LX_D_007) {
|
|
tmp0 = do_division_u(tmp0, b_minus_amb);
|
|
tmp0 = do_division_u(tmp0, 4);
|
|
tmp0 = tmp0 * pll_period_mm;
|
|
} else {
|
|
tmp0 = tmp0 * pll_period_mm;
|
|
tmp0 = do_division_u(tmp0, b_minus_amb);
|
|
tmp0 = do_division_u(tmp0, 4);
|
|
}
|
|
|
|
|
|
if (tmp0 > (uint64_t)VL53LX_D_006)
|
|
tmp0 = (uint64_t)VL53LX_D_006;
|
|
|
|
|
|
tmp0 = tmp0 >> 2;
|
|
|
|
|
|
if (tmp0 > (uint64_t)VL53LX_D_007)
|
|
tmp0 = (uint64_t)VL53LX_D_007;
|
|
|
|
|
|
tmp1 = (uint64_t)sigma_estimator__sigma_ref_mm << 7;
|
|
tmp1 = tmp1 * tmp1;
|
|
tmp0 = tmp0 + tmp1;
|
|
|
|
|
|
if (tmp0 > (uint64_t)VL53LX_D_007)
|
|
tmp0 = (uint64_t)VL53LX_D_007;
|
|
|
|
|
|
sigma_int = VL53LX_isqrt((uint32_t)tmp0);
|
|
|
|
*psigma_est = (uint16_t)sigma_int;
|
|
|
|
status = VL53LX_ERROR_NONE;
|
|
}
|
|
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
|
|
|
|
|