in boards/STM32L475_Discovery/BSP/Components/vl53l0x/vl53l0x_api_core.c [1705:2041]
VL53L0X_Error VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
FixPoint1616_t *pSigmaEstimate,
uint32_t *pDmax_mm)
{
/* Expressed in 100ths of a ns, i.e. centi-ns */
const uint32_t cPulseEffectiveWidth_centi_ns = 800;
/* Expressed in 100ths of a ns, i.e. centi-ns */
const uint32_t cAmbientEffectiveWidth_centi_ns = 600;
const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */
const uint32_t cVcselPulseWidth_ps = 4700; /* pico secs */
const FixPoint1616_t cSigmaEstMax = 0x028F87AE;
const FixPoint1616_t cSigmaEstRtnMax = 0xF000;
const FixPoint1616_t cAmbToSignalRatioMax = 0xF0000000/
cAmbientEffectiveWidth_centi_ns;
/* Time Of Flight per mm (6.6 pico secs) */
const FixPoint1616_t cTOF_per_mm_ps = 0x0006999A;
const uint32_t c16BitRoundingParam = 0x00008000;
const FixPoint1616_t cMaxXTalk_kcps = 0x00320000;
const uint32_t cPllPeriod_ps = 1655;
uint32_t vcselTotalEventsRtn;
uint32_t finalRangeTimeoutMicroSecs;
uint32_t preRangeTimeoutMicroSecs;
FixPoint1616_t sigmaEstimateP1;
FixPoint1616_t sigmaEstimateP2;
FixPoint1616_t sigmaEstimateP3;
FixPoint1616_t deltaT_ps;
FixPoint1616_t pwMult;
FixPoint1616_t sigmaEstRtn;
FixPoint1616_t sigmaEstimate;
FixPoint1616_t xTalkCorrection;
FixPoint1616_t ambientRate_kcps;
FixPoint1616_t peakSignalRate_kcps;
FixPoint1616_t xTalkCompRate_mcps;
uint32_t xTalkCompRate_kcps;
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
FixPoint1616_t diff1_mcps;
FixPoint1616_t diff2_mcps;
FixPoint1616_t sqr1;
FixPoint1616_t sqr2;
FixPoint1616_t sqrSum;
FixPoint1616_t sqrtResult_centi_ns;
FixPoint1616_t sqrtResult;
FixPoint1616_t totalSignalRate_mcps;
FixPoint1616_t correctedSignalRate_mcps;
uint32_t vcselWidth;
uint32_t finalRangeMacroPCLKS;
uint32_t preRangeMacroPCLKS;
uint32_t peakVcselDuration_us;
uint8_t finalRangeVcselPCLKS;
uint8_t preRangeVcselPCLKS;
/*! \addtogroup calc_sigma_estimate
* @{
*
* Estimates the range sigma based on the
*
* - vcsel_rate_kcps
* - ambient_rate_kcps
* - signal_total_events
* - xtalk_rate
*
* and the following parameters
*
* - SigmaEstRefArray
* - SigmaEstEffPulseWidth
* - SigmaEstEffAmbWidth
*/
LOG_FUNCTION_START("");
VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps,
xTalkCompRate_mcps);
/*
* We work in kcps rather than mcps as this helps keep within the
* confines of the 32 Fix1616 type.
*/
ambientRate_kcps =
(pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16;
correctedSignalRate_mcps =
pRangingMeasurementData->SignalRateRtnMegaCps;
Status = VL53L0X_get_total_signal_rate(
Dev, pRangingMeasurementData, &totalSignalRate_mcps);
Status = VL53L0X_get_total_xtalk_rate(
Dev, pRangingMeasurementData, &xTalkCompRate_mcps);
/* Signal rate measurement provided by device is the
* peak signal rate, not average.
*/
peakSignalRate_kcps = (totalSignalRate_mcps * 1000);
peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16;
xTalkCompRate_kcps = xTalkCompRate_mcps * 1000;
if (xTalkCompRate_kcps > cMaxXTalk_kcps)
xTalkCompRate_kcps = cMaxXTalk_kcps;
if (Status == VL53L0X_ERROR_NONE) {
/* Calculate final range macro periods */
finalRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, FinalRangeTimeoutMicroSecs);
finalRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, FinalRangeVcselPulsePeriod);
finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS);
/* Calculate pre-range macro periods */
preRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, PreRangeTimeoutMicroSecs);
preRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER(
Dev, PreRangeVcselPulsePeriod);
preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks(
Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS);
vcselWidth = 3;
if (finalRangeVcselPCLKS == 8)
vcselWidth = 2;
peakVcselDuration_us = vcselWidth * 2048 *
(preRangeMacroPCLKS + finalRangeMacroPCLKS);
peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
peakVcselDuration_us *= cPllPeriod_ps;
peakVcselDuration_us = (peakVcselDuration_us + 500)/1000;
/* Fix1616 >> 8 = Fix2408 */
totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8;
/* Fix2408 * uint32 = Fix2408 */
vcselTotalEventsRtn = totalSignalRate_mcps *
peakVcselDuration_us;
/* Fix2408 >> 8 = uint32 */
vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8;
/* Fix2408 << 8 = Fix1616 = */
totalSignalRate_mcps <<= 8;
}
if (Status != VL53L0X_ERROR_NONE) {
LOG_FUNCTION_END(Status);
return Status;
}
if (peakSignalRate_kcps == 0) {
*pSigmaEstimate = cSigmaEstMax;
PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax);
*pDmax_mm = 0;
} else {
if (vcselTotalEventsRtn < 1)
vcselTotalEventsRtn = 1;
/*
* Calculate individual components of the main equation -
* replicating the equation implemented in the script
* OpenAll_Ewok_ranging_data.jsl.
*
* sigmaEstimateP1 represents the effective pulse width, which
* is a tuning parameter, rather than a real value.
*
* sigmaEstimateP2 represents the ambient/signal rate ratio
* expressed as a multiple of the effective ambient width
* (tuning parameter).
*
* sigmaEstimateP3 provides the signal event component, with the
* knowledge that
* - Noise of a square pulse is 1/sqrt(12) of the pulse
* width.
* - at 0Lux, sigma is proportional to
* effectiveVcselPulseWidth/sqrt(12 * signalTotalEvents)
*
* deltaT_ps represents the time of flight in pico secs for the
* current range measurement, using the "TOF per mm" constant
* (in ps).
*/
sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns;
/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sigmaEstimateP2 = (ambientRate_kcps << 16)/peakSignalRate_kcps;
if (sigmaEstimateP2 > cAmbToSignalRatioMax) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigmaEstimateP2 = cAmbToSignalRatioMax;
}
sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns;
sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12);
/* uint32 * FixPoint1616 = FixPoint1616 */
deltaT_ps = pRangingMeasurementData->RangeMilliMeter *
cTOF_per_mm_ps;
/*
* vcselRate - xtalkCompRate
* (uint32 << 16) - FixPoint1616 = FixPoint1616.
* Divide result by 1000 to convert to mcps.
* 500 is added to ensure rounding when integer division
* truncates.
*/
diff1_mcps = (((peakSignalRate_kcps << 16) -
xTalkCompRate_kcps) + 500)/1000;
/* vcselRate + xtalkCompRate */
diff2_mcps = (((peakSignalRate_kcps << 16) +
xTalkCompRate_kcps) + 500)/1000;
/* Shift by 8 bits to increase resolution prior to the
* division */
diff1_mcps <<= 8;
/* FixPoint0824/FixPoint1616 = FixPoint2408 */
xTalkCorrection = abs(diff1_mcps/diff2_mcps);
/* FixPoint2408 << 8 = FixPoint1616 */
xTalkCorrection <<= 8;
/* FixPoint1616/uint32 = FixPoint1616 */
pwMult = deltaT_ps/cVcselPulseWidth_ps; /* smaller than 1.0f */
/*
* FixPoint1616 * FixPoint1616 = FixPoint3232, however both
* values are small enough such that32 bits will not be
* exceeded.
*/
pwMult *= ((1 << 16) - xTalkCorrection);
/* (FixPoint3232 >> 16) = FixPoint1616 */
pwMult = (pwMult + c16BitRoundingParam) >> 16;
/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
pwMult += (1 << 16);
/*
* At this point the value will be 1.xx, therefore if we square
* the value this will exceed 32 bits. To address this perform
* a single shift to the right before the multiplication.
*/
pwMult >>= 1;
/* FixPoint1715 * FixPoint1715 = FixPoint3430 */
pwMult = pwMult * pwMult;
/* (FixPoint3430 >> 14) = Fix1616 */
pwMult >>= 14;
/* FixPoint1616 * uint32 = FixPoint1616 */
sqr1 = pwMult * sigmaEstimateP1;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr1 = (sqr1 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr1 *= sqr1;
sqr2 = sigmaEstimateP2;
/* (FixPoint1616 >> 16) = FixPoint3200 */
sqr2 = (sqr2 + 0x8000) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr2 *= sqr2;
/* FixPoint64000 + FixPoint6400 = FixPoint6400 */
sqrSum = sqr1 + sqr2;
/* SQRT(FixPoin6400) = FixPoint3200 */
sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum);
/* (FixPoint3200 << 16) = FixPoint1616 */
sqrtResult_centi_ns <<= 16;
/*
* Note that the Speed Of Light is expressed in um per 1E-10
* seconds (2997) Therefore to get mm/ns we have to divide by
* 10000
*/
sigmaEstRtn = (((sqrtResult_centi_ns+50)/100) /
sigmaEstimateP3);
sigmaEstRtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
/* Add 5000 before dividing by 10000 to ensure rounding. */
sigmaEstRtn += 5000;
sigmaEstRtn /= 10000;
if (sigmaEstRtn > cSigmaEstRtnMax) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigmaEstRtn = cSigmaEstRtnMax;
}
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr1 = sigmaEstRtn * sigmaEstRtn;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr2 = cSigmaEstRef * cSigmaEstRef;
/* sqrt(FixPoint3232) = FixPoint1616 */
sqrtResult = VL53L0X_isqrt((sqr1 + sqr2));
/*
* Note that the Shift by 4 bits increases resolution prior to
* the sqrt, therefore the result must be shifted by 2 bits to
* the right to revert back to the FixPoint1616 format.
*/
sigmaEstimate = 1000 * sqrtResult;
if ((peakSignalRate_kcps < 1) || (vcselTotalEventsRtn < 1) ||
(sigmaEstimate > cSigmaEstMax)) {
sigmaEstimate = cSigmaEstMax;
}
*pSigmaEstimate = (uint32_t)(sigmaEstimate);
PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate);
Status = VL53L0X_calc_dmax(
Dev,
totalSignalRate_mcps,
correctedSignalRate_mcps,
pwMult,
sigmaEstimateP1,
sigmaEstimateP2,
peakVcselDuration_us,
pDmax_mm);
}
LOG_FUNCTION_END(Status);
return Status;
}