From 7e90cb476c820150fec1405d7d218b01f340f442 Mon Sep 17 00:00:00 2001 From: YunHorn Technology Date: Fri, 7 Jul 2023 11:35:00 +0800 Subject: [PATCH] revise tof sensor detection for max of 3 sensors instead of single one. --- STM32CubeIDE/.settings/language.settings.xml | 4 +- TOF/App/X-WL55_WLE5_53L0X.h | 8 +- TOF/App/app_tof.h | 9 +- TOF/App/app_tof_vl53l0x_range.c | 294 ++++++++++--------- 4 files changed, 171 insertions(+), 144 deletions(-) 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 @@ - + @@ -16,7 +16,7 @@ - + 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 */