diff --git a/STM32CubeIDE/.settings/language.settings.xml b/STM32CubeIDE/.settings/language.settings.xml
index 2d055d2..6d8b782 100644
--- a/STM32CubeIDE/.settings/language.settings.xml
+++ b/STM32CubeIDE/.settings/language.settings.xml
@@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
- <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="686453894977928153" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
+ <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="900329934260214400" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
@@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
- <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="686453894977928153" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
+ <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="900329934260214400" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD "${INPUTS}"" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
diff --git a/TOF/App/X-WL55_WLE5_53L0X.h b/TOF/App/X-WL55_WLE5_53L0X.h
index 54efe94..196f4cc 100644
--- a/TOF/App/X-WL55_WLE5_53L0X.h
+++ b/TOF/App/X-WL55_WLE5_53L0X.h
@@ -20,6 +20,7 @@
/** @ingroup VL53L0X_config
* @{*/
+#define MAX_TOF_COUNT 3
#ifndef VL53L0X_HAVE_UART
/**
@@ -185,9 +186,9 @@ extern void XNUCLEO53L1A1_USART2_UART_Init(void);
* @note Most functions are using a device selector as input. ASCII 'c', 'l' or 'r' are also accepted.
*/
enum XNUCLEO53L1A1_dev_e{
- XNUCLEO53L1A1_DEV_LEFT = 0, //!< left satellite device P21 header : 'l'
- XNUCLEO53L1A1_DEV_CENTER = 1, //!< center (built-in) vl053 device : 'c"
- XNUCLEO53L1A1_DEV_RIGHT= 2 //!< Right satellite device P22 header : 'r'
+ XNUCLEO53L0X_LEFT = 0, //!< left satellite device P21 header : 'l'
+ XNUCLEO53L0X_CENTER = 1, //!< center (built-in) vl053 device : 'c"
+ XNUCLEO53L0X_RIGHT= 2 //!< Right satellite device P22 header : 'r'
};
/**
@@ -251,5 +252,6 @@ int XNUCLEO53L1A1_ResetId(int DevNo, int state );
* >0 interrupt configured but with potential sharing on EXTI groups see @ref VL53L0X_GPIO1_MAP
*/
int XNUCLEO53L1A1_SetIntrStateId(int EnableIntr, int DevNo);
+int sts_tof_vl53l0x_DetectSensors(void);
#endif /* _X_WL55_WLE5_53L0X_H_ */
diff --git a/TOF/App/app_tof.h b/TOF/App/app_tof.h
index 9898dac..ee0dc66 100644
--- a/TOF/App/app_tof.h
+++ b/TOF/App/app_tof.h
@@ -30,6 +30,10 @@
#include "yunhorn_sts_prd_conf.h"
#include "yunhorn_sts_sensors.h"
+#include "vl53l0x_def.h"
+
+
+
/* Exported defines ----------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
@@ -45,11 +49,10 @@ void sts_vl53lx_ranging(vl53lx_model, range_mode, i_distance_threshold_mm, i_int
void STS_R0_SENSOR_Read(STS_R0_SensorDataTypeDef *r0_data);
-int sts_tof_vl53l0x_DetectSensors(void);
-#ifdef STS_USE_TOF_VL53L0X
+
void STS_TOF_VL53L0X_Range_Process(void);
void STS_R0_SENSOR_Read(STS_R0_SensorDataTypeDef *r0_data);
-#endif
+
diff --git a/TOF/App/app_tof_vl53l0x_range.c b/TOF/App/app_tof_vl53l0x_range.c
index 8d231df..708297e 100644
--- a/TOF/App/app_tof_vl53l0x_range.c
+++ b/TOF/App/app_tof_vl53l0x_range.c
@@ -75,22 +75,29 @@ VL53L0X_RangingMeasurementData_t RangingMeasurementData;
* r(n) = averaged_r(n-1)*leaky +r(n)(1-leaky)
*
* */
-int LeakyFactorFix8 = (int)( 0.6 *256);
+int LeakyFactorFix8 = (int)( 0.6 *256); // 0.6 for ranging, 0.0 for gesture detect
/** How many device detect set by @a DetectSensors()*/
int nDevPresent=0;
+/** bit is index in VL53L0XDevs that is not necessary the dev id of the BSP */
+int nDevMask;
-volatile int sts_tof_distance_data;
+int nSensorPresent;
+
+volatile int sts_tof_distance_data[MAX_TOF_COUNT];
extern volatile uint8_t sensor_data_ready;
-VL53L0X_Dev_t VL53L0XDevs={
- .I2cHandle=&X_WL55_WLE5_53L0X_hi2c,
- .I2cDevAddr=0x52
+VL53L0X_Dev_t VL53L0XDevs[]={
+ {.Id=XNUCLEO53L0X_LEFT, .DevLetter='l',.I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
+ {.Id=XNUCLEO53L0X_CENTER,.DevLetter='c', .I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
+ {.Id=XNUCLEO53L0X_RIGHT, .DevLetter='r',.I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
};
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
//void ResetAndDetectSensor(int SetDisplay);
+void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange);
+void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig);
/* USER CODE END PFP */
/* USER CODE BEGIN 0 */
@@ -117,26 +124,33 @@ void HandleError(int err){
*/
int sts_tof_vl53l0x_DetectSensors(void)
{
-
+ int i;
uint16_t Id;
int status;
int FinalAddress;
+ nDevPresent = 0;
/* detect all sensors (even on-board)*/
+ for (i=0; i < MAX_TOF_COUNT; i++)
+ {
VL53L0X_Dev_t *pDev;
- pDev = &VL53L0XDevs;
+ pDev = &VL53L0XDevs[i];
pDev->I2cDevAddr = 0x52;
pDev->Present = 0;
//status = XNUCLEO53L0A1_ResetId(pDev->Id, 1);
//HAL_Delay(2);
- FinalAddress=0x52;
+ FinalAddress=0x52 +(i+1)*2;
+
+ APP_LOG(TS_OFF, VLEVEL_L, "\r\n Detect TOF sensors #%u with I2CDevAddr=0x%02x \r\n", i, pDev->I2cDevAddr);
+
do {
/* Set I2C standard mode (400 KHz) before doing the first register access */
//if (status == VL53L0X_ERROR_NONE)
- status = VL53L0X_WrByte(pDev, 0x88, 0x00);
+ //status = VL53L0X_WrByte(pDev, 0x88, 0x00);
/* Try to read one register using default 0x52 address */
status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
+ APP_LOG(TS_OFF, VLEVEL_L, "#%u read id = %04x \r\n",i, Id);
if (status) {
APP_LOG(TS_OFF, VLEVEL_L, "# Read id fail\n");
break;
@@ -159,30 +173,27 @@ int sts_tof_vl53l0x_DetectSensors(void)
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_DataInit %d fail\n");
break;
}
- //APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X Present and initiated to final 0x%x\n", pDev->I2cDevAddr);
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X %d Present and initiated to final 0x%x\n", pDev->Id, pDev->I2cDevAddr);
+ nDevPresent++;
+ nDevMask |= 1 << i;
pDev->Present = 1;
}
else {
APP_LOG(TS_OFF, VLEVEL_L, "# unknown ID %x\n", Id);
status = 1;
}
-
- /* if fail r can't use for any reason then put the device back to reset */
- //if (status) {
- //XNUCLEO53L0A1_ResetId(i, 0);
- //}
} while(0);
/* Display detected sensor(s) */
+ }
-
- return Id;
+ return nDevPresent;
}
/**
* Setup all detected sensors for single shot mode and setup ranging configuration
*/
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
-{
+{ int i;
int status=VL53L0X_ERROR_NONE;
uint8_t VhvSettings;
uint8_t PhaseCal;
@@ -193,100 +204,106 @@ void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
uint32_t timingBudget = 33000;
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
- // uart_printf("\r\n######### start setup for single shot \r\n");
+ // uart_printf("\r\n######### start setup for single shot \r\n");
+ for( i=0; i<3; i++){
+ if( VL53L0XDevs[i].Present){
- status=VL53L0X_StaticInit(&VL53L0XDevs);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n");
- }
+ status=VL53L0X_StaticInit(&VL53L0XDevs[i]);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n");
+ }
- status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
- }
+ status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
+ }
- status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs, &refSpadCount, &isApertureSpads);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefSpadManagement failed\n");
- }
+ status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs[i], &refSpadCount, &isApertureSpads);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefSpadManagement failed\n");
+ }
- status = VL53L0X_SetDeviceMode(&VL53L0XDevs, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetDeviceMode failed\n");
- }
- status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
- }
- status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
- }
- /* Ranging configuration */
- switch(rangingConfig) {
- case LONG_RANGE:
- signalLimit = (FixPoint1616_t)(0.1*65536);
- sigmaLimit = (FixPoint1616_t)(60*65536);
- timingBudget = 33000;
- preRangeVcselPeriod = 18;
- finalRangeVcselPeriod = 14;
- break;
- case HIGH_ACCURACY:
- signalLimit = (FixPoint1616_t)(0.25*65536);
- sigmaLimit = (FixPoint1616_t)(18*65536);
- timingBudget = 200000;
- preRangeVcselPeriod = 14;
- finalRangeVcselPeriod = 10;
- break;
- case HIGH_SPEED:
- signalLimit = (FixPoint1616_t)(0.25*65536);
- sigmaLimit = (FixPoint1616_t)(32*65536);
- timingBudget = 20000;
- preRangeVcselPeriod = 14;
- finalRangeVcselPeriod = 10;
- break;
- default:
- APP_LOG(TS_OFF, VLEVEL_L, "Not Supported");
- break;
- }
+ status = VL53L0X_SetDeviceMode(&VL53L0XDevs[i], VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetDeviceMode failed\n");
+ }
+ status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
+ }
+ status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
+ }
+ /* Ranging configuration */
+ switch(rangingConfig) {
+ case LONG_RANGE:
+ signalLimit = (FixPoint1616_t)(0.1*65536);
+ sigmaLimit = (FixPoint1616_t)(60*65536);
+ timingBudget = 33000;
+ preRangeVcselPeriod = 18;
+ finalRangeVcselPeriod = 14;
+ break;
+ case HIGH_ACCURACY:
+ signalLimit = (FixPoint1616_t)(0.25*65536);
+ sigmaLimit = (FixPoint1616_t)(18*65536);
+ timingBudget = 200000;
+ preRangeVcselPeriod = 14;
+ finalRangeVcselPeriod = 10;
+ break;
+ case HIGH_SPEED:
+ signalLimit = (FixPoint1616_t)(0.25*65536);
+ sigmaLimit = (FixPoint1616_t)(32*65536);
+ timingBudget = 20000;
+ preRangeVcselPeriod = 14;
+ finalRangeVcselPeriod = 10;
+ break;
+ default:
+ APP_LOG(TS_OFF, VLEVEL_L, "Not Supported");
+ break;
+ }
- status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
- }
+ status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
+ }
- status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
- }
+ status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
+ }
- status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs, timingBudget);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
- }
+ status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs[i], timingBudget);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
+ }
- status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs, VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
- }
+ status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i], VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
+ }
- status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
- }
+ status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i], VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
+ }
- status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
- if( status ){
- APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
- }
+ status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);
+ if( status ){
+ APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
+ }
- VL53L0XDevs.LeakyFirst =1;
+ VL53L0XDevs[i].LeakyFirst =1;
+
+ }
+ }
}
/* Store new ranging data into the device structure, apply leaky integrator if needed */
-void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange){
+void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange)
+{
if( pRange->RangeStatus == 0 ){
if( pDev->LeakyFirst ){
pDev->LeakyFirst = 0;
@@ -301,56 +318,61 @@ void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeas
}
}
-
-VL53L0X_Error sts_tof_vl53l0x_range(VL53L0X_Dev_t *dev,VL53L0X_RangingMeasurementData_t *pdata, RangingConfig_e rangingConfig)
-{
- //int over=0;
- VL53L0X_Error status = VL53L0X_ERROR_NONE;
-
- /* Setup all sensors in Single Shot mode */
- sts_tof_vl53l0x_SetupSingleShot(rangingConfig);
- /* Call All-In-One blocking API function */
- status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs, &RangingMeasurementData);
- if( status ==0 ){
- sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs,&RangingMeasurementData);
- /* Display distance in mm */
- if( RangingMeasurementData.RangeStatus == 0 ){
- sts_tof_distance_data = (int)VL53L0XDevs.LeakyRange;
- //uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
- }
- }
- else{
- HandleError(ERR_DEMO_RANGE_ONE);
- }
- return status;
-}
-
/* USER CODE END 0 */
void STS_TOF_VL53L0X_Range_Process(void)
{
- int Status=0;
+ int status=0, i;
RangingConfig_e RangingConfig = HIGH_ACCURACY; //HIGH_ACCURACY; //LONG_RANGE;
XWL55_WLE5_53L0X_Init();
- Status = sts_tof_vl53l0x_DetectSensors(); // confirm sensor online
- //if (Status ==1 )
- //APP_LOG(TS_OFF, VLEVEL_M, "sensor online \r\n");
- // RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
-
- Status = sts_tof_vl53l0x_range(&VL53L0XDevs,&RangingMeasurementData, RangingConfig);
- // ########## two conditions
- // ########## 1) return status ==0,
- // ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
- if ((Status == 0) && (RangingMeasurementData.RangeStatus == 0 ))
- {
- //sts_tof_distance_data = (int) VL53L0XDevs.LeakyRange;
- APP_LOG(TS_OFF, VLEVEL_M, "## Measured Range = %4d mm\r\n", (int)sts_tof_distance_data);
- sensor_data_ready = 1;
- }
+ nSensorPresent = sts_tof_vl53l0x_DetectSensors(); // confirm sensor online
+ APP_LOG(TS_OFF, VLEVEL_M, "sensor online \r\n");
+ nDevMask = 0;
+
+ if (nSensorPresent > 0)
+ {
+ // RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
+ sts_tof_vl53l0x_SetupSingleShot(RangingConfig);
+
+ for (i=0; i< MAX_TOF_COUNT; i++)
+ {
+ if (VL53L0XDevs[i].Present ==1)
+ {
+ status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
+ if( status ==0 )
+ {
+ sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
+ /* Display distance in mm */
+ if( RangingMeasurementData.RangeStatus == 0 )
+ {
+ sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
+ //uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
+ }
+ } else
+ {
+ HandleError(ERR_DEMO_RANGE_ONE);
+ }
+ // ########## two conditions
+ // ########## 1) return status ==0,
+ // ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
+
+ if ((status == 0) && (RangingMeasurementData.RangeStatus == 0 ))
+ {
+ //sts_tof_distance_data = (int) VL53L0XDevs.LeakyRange;
+ APP_LOG(TS_OFF, VLEVEL_M, "## Measured Range TOF #0 = %4d mm, TOF #1 = %4d mm\r\n",
+ (int)sts_tof_distance_data[0],(int)sts_tof_distance_data[1]);
+ nDevMask |= (1 << i);
+ sensor_data_ready |= 1;
+ }
+ }
+
+ } // for i < MAX_TOF_COUNT
+ } // nSensorPresent >0
}
+
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */