in boards/STM32L475_Discovery/BSP/Components/vl53l0x/vl53l0x_api_core.c [2043:2275]
VL53L0X_Error VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
uint8_t DeviceRangeStatus,
FixPoint1616_t SignalRate,
uint16_t EffectiveSpadRtnCount,
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
uint8_t *pPalRangeStatus)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
uint8_t NoneFlag;
uint8_t SigmaLimitflag = 0;
uint8_t SignalRefClipflag = 0;
uint8_t RangeIgnoreThresholdflag = 0;
uint8_t SigmaLimitCheckEnable = 0;
uint8_t SignalRateFinalRangeLimitCheckEnable = 0;
uint8_t SignalRefClipLimitCheckEnable = 0;
uint8_t RangeIgnoreThresholdLimitCheckEnable = 0;
FixPoint1616_t SigmaEstimate;
FixPoint1616_t SigmaLimitValue;
FixPoint1616_t SignalRefClipValue;
FixPoint1616_t RangeIgnoreThresholdValue;
FixPoint1616_t SignalRatePerSpad;
uint8_t DeviceRangeStatusInternal = 0;
uint16_t tmpWord = 0;
uint8_t Temp8;
uint32_t Dmax_mm = 0;
FixPoint1616_t LastSignalRefMcps;
LOG_FUNCTION_START("");
/*
* VL53L0X has a good ranging when the value of the
* DeviceRangeStatus = 11. This function will replace the value 0 with
* the value 11 in the DeviceRangeStatus.
* In addition, the SigmaEstimator is not included in the VL53L0X
* DeviceRangeStatus, this will be added in the PalRangeStatus.
*/
DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3);
if (DeviceRangeStatusInternal == 0 ||
DeviceRangeStatusInternal == 5 ||
DeviceRangeStatusInternal == 7 ||
DeviceRangeStatusInternal == 12 ||
DeviceRangeStatusInternal == 13 ||
DeviceRangeStatusInternal == 14 ||
DeviceRangeStatusInternal == 15
) {
NoneFlag = 1;
} else {
NoneFlag = 0;
}
/* LastSignalRefMcps */
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x01);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_RdWord(Dev,
VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
&tmpWord);
LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord);
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_WrByte(Dev, 0xFF, 0x00);
PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps);
/*
* Check if Sigma limit is enabled, if yes then do comparison with limit
* value and put the result back into pPalRangeStatus.
*/
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
&SigmaLimitCheckEnable);
if ((SigmaLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) {
/*
* compute the Sigma and check with limit
*/
Status = VL53L0X_calc_sigma_estimate(
Dev,
pRangingMeasurementData,
&SigmaEstimate,
&Dmax_mm);
if (Status == VL53L0X_ERROR_NONE)
pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm;
if (Status == VL53L0X_ERROR_NONE) {
Status = VL53L0X_GetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
&SigmaLimitValue);
if ((SigmaLimitValue > 0) &&
(SigmaEstimate > SigmaLimitValue))
/* Limit Fail */
SigmaLimitflag = 1;
}
}
/*
* Check if Signal ref clip limit is enabled, if yes then do comparison
* with limit value and put the result back into pPalRangeStatus.
*/
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
&SignalRefClipLimitCheckEnable);
if ((SignalRefClipLimitCheckEnable != 0) &&
(Status == VL53L0X_ERROR_NONE)) {
Status = VL53L0X_GetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
&SignalRefClipValue);
if ((SignalRefClipValue > 0) &&
(LastSignalRefMcps > SignalRefClipValue)) {
/* Limit Fail */
SignalRefClipflag = 1;
}
}
/*
* Check if Signal ref clip limit is enabled, if yes then do comparison
* with limit value and put the result back into pPalRangeStatus.
* EffectiveSpadRtnCount has a format 8.8
* If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
*/
if (Status == VL53L0X_ERROR_NONE)
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
&RangeIgnoreThresholdLimitCheckEnable);
if ((RangeIgnoreThresholdLimitCheckEnable != 0) &&
(Status == VL53L0X_ERROR_NONE)) {
/* Compute the signal rate per spad */
if (EffectiveSpadRtnCount == 0) {
SignalRatePerSpad = 0;
} else {
SignalRatePerSpad = (FixPoint1616_t)((256 * SignalRate)
/ EffectiveSpadRtnCount);
}
Status = VL53L0X_GetLimitCheckValue(Dev,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
&RangeIgnoreThresholdValue);
if ((RangeIgnoreThresholdValue > 0) &&
(SignalRatePerSpad < RangeIgnoreThresholdValue)) {
/* Limit Fail add 2^6 to range status */
RangeIgnoreThresholdflag = 1;
}
}
if (Status == VL53L0X_ERROR_NONE) {
if (NoneFlag == 1) {
*pPalRangeStatus = 255; /* NONE */
} else if (DeviceRangeStatusInternal == 1 ||
DeviceRangeStatusInternal == 2 ||
DeviceRangeStatusInternal == 3) {
*pPalRangeStatus = 5; /* HW fail */
} else if (DeviceRangeStatusInternal == 6 ||
DeviceRangeStatusInternal == 9) {
*pPalRangeStatus = 4; /* Phase fail */
} else if (DeviceRangeStatusInternal == 8 ||
DeviceRangeStatusInternal == 10 ||
SignalRefClipflag == 1) {
*pPalRangeStatus = 3; /* Min range */
} else if (DeviceRangeStatusInternal == 4 ||
RangeIgnoreThresholdflag == 1) {
*pPalRangeStatus = 2; /* Signal Fail */
} else if (SigmaLimitflag == 1) {
*pPalRangeStatus = 1; /* Sigma Fail */
} else {
*pPalRangeStatus = 0; /* Range Valid */
}
}
/* DMAX only relevant during range error */
if (*pPalRangeStatus == 0)
pRangingMeasurementData->RangeDMaxMilliMeter = 0;
/* fill the Limit Check Status */
Status = VL53L0X_GetLimitCheckEnable(Dev,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
&SignalRateFinalRangeLimitCheckEnable);
if (Status == VL53L0X_ERROR_NONE) {
if ((SigmaLimitCheckEnable == 0) || (SigmaLimitflag == 1))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8);
if ((DeviceRangeStatusInternal == 4) ||
(SignalRateFinalRangeLimitCheckEnable == 0))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
Temp8);
if ((SignalRefClipLimitCheckEnable == 0) ||
(SignalRefClipflag == 1))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8);
if ((RangeIgnoreThresholdLimitCheckEnable == 0) ||
(RangeIgnoreThresholdflag == 1))
Temp8 = 1;
else
Temp8 = 0;
VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus,
VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
Temp8);
}
LOG_FUNCTION_END(Status);
return Status;
}